Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: increase innovation variance instead of clipping innovations

This commit is contained in:
Paul Riseborough 2023-06-25 18:06:11 +10:00 committed by Randy Mackay
parent 7a4acadc9f
commit 31d510042f
1 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -735,13 +735,13 @@ void NavEKF3_core::FuseVelPosNED()
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms;
} else if ((frontend->_gpsGlitchRadiusMax <= 0) && (PV_AidingMode != AID_NONE)) { } else if ((frontend->_gpsGlitchRadiusMax <= 0) && (PV_AidingMode != AID_NONE)) {
// handle special case where the glitch radius parameter has been set to a non-positive number // Handle the special case where the glitch radius parameter has been set to a non-positive number.
// to indicate that large velocity and position innovations should be clipped instead of rejected. // The innovation variance is increased to limit the state update to an amount corresponding
// to a test ratio of 1.
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms;
const ftype scaleFactor = 1.0F / sqrtF(posTestRatio); varInnovVelPos[3] *= posTestRatio;
innovVelPos[3] *= scaleFactor; varInnovVelPos[4] *= posTestRatio;
innovVelPos[4] *= scaleFactor;
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms;
} }
@ -810,15 +810,14 @@ void NavEKF3_core::FuseVelPosNED()
velCheckPassed = true; velCheckPassed = true;
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
} else if (frontend->_gpsGlitchRadiusMax <= 0) { } else if (frontend->_gpsGlitchRadiusMax <= 0) {
// handle special case where the glitch radius parameter has been set to a non-positive number // Handle the special case where the glitch radius parameter has been set to a non-positive number.
// to indicate that large velocity and position innovations should be clipped instead of rejected. // The innovation variance is increased to limit the state update to an amount corresponding
// to a test ratio of 1.
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms;
const ftype scaleFactor = 1.0F / sqrtF(velTestRatio);
for (uint8_t i = 0; i<=imax; i++) { for (uint8_t i = 0; i<=imax; i++) {
innovVelPos[i] *= scaleFactor; varInnovVelPos[i] *= velTestRatio;
} }
innovVelPos[4] *= scaleFactor;
velCheckPassed = true; velCheckPassed = true;
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
} }
@ -853,17 +852,18 @@ void NavEKF3_core::FuseVelPosNED()
// When on ground we accept a larger test ratio to allow the filter to handle large switch on IMU // When on ground we accept a larger test ratio to allow the filter to handle large switch on IMU
// bias errors without rejecting the height sensor. // bias errors without rejecting the height sensor.
const float maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0f : 1.0f; const bool onGroundNotNavigating = (PV_AidingMode == AID_NONE) && onGround;
const float maxTestRatio = onGroundNotNavigating ? 3.0f : 1.0f;
if (hgtTestRatio < maxTestRatio) { if (hgtTestRatio < maxTestRatio) {
hgtCheckPassed = true; hgtCheckPassed = true;
lastHgtPassTime_ms = imuSampleTime_ms; lastHgtPassTime_ms = imuSampleTime_ms;
} else if ((frontend->_gpsGlitchRadiusMax <= 0) && (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS)) { } else if ((frontend->_gpsGlitchRadiusMax <= 0) && !onGroundNotNavigating && (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS)) {
// handle special case where the glitch radius parameter has been set to a non-positive number // Handle the special case where the glitch radius parameter has been set to a non-positive number.
// to indicate that large GPS velocity and position innovations should be clipped instead of rejected. // The innovation variance is increased to limit the state update to an amount corresponding
// to a test ratio of 1.
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms;
const ftype scaleFactor = 1.0F / sqrtF(hgtTestRatio); varInnovVelPos[5] *= hgtTestRatio;
innovVelPos[5] *= scaleFactor;
hgtCheckPassed = true; hgtCheckPassed = true;
lastHgtPassTime_ms = imuSampleTime_ms; lastHgtPassTime_ms = imuSampleTime_ms;
} }