AP_NavEKF3: increase innovation variance instead of clipping innovations

ArduPilot · Jun 25, 2023 · 8e120d8 · 8e120d8
1 parent 219fd3e
commit 8e120d8
Showing 1 changed file with 9 additions and 10 deletions.
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -771,13 +771,13 @@ void NavEKF3_core::FuseVelPosNED()
                 posCheckPassed = true;
                 lastPosPassTime_ms = imuSampleTime_ms;
             } 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
-                // to indicate that large velocity and position innovations should be clipped instead of rejected.
+                // Handle the special case where the glitch radius parameter has been set to a non-positive number.
+                // The innovation variance is increased to limit the state update to an amount corresponding
+                // to a test ratio of 1.
                 posCheckPassed = true;
                 lastPosPassTime_ms = imuSampleTime_ms;
-                const ftype scaleFactor = 1.0F / sqrtF(posTestRatio);
-                innovVelPos[3] *= scaleFactor;
-                innovVelPos[4] *= scaleFactor;
+                varInnovVelPos[3] *= posTestRatio;
+                varInnovVelPos[4] *= posTestRatio;
                 posCheckPassed = true;
                 lastPosPassTime_ms = imuSampleTime_ms;
             }
@@ -846,15 +846,14 @@ void NavEKF3_core::FuseVelPosNED()
                 velCheckPassed = true;
                 lastVelPassTime_ms = imuSampleTime_ms;
             } else if (frontend->_gpsGlitchRadiusMax <= 0) {
-                // handle 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.
+                // Handle the special case where the glitch radius parameter has been set to a non-positive number.
+                // The innovation variance is increased to limit the state update to an amount corresponding
+                // to a test ratio of 1.
                 posCheckPassed = true;
                 lastPosPassTime_ms = imuSampleTime_ms;
-                const ftype scaleFactor = 1.0F / sqrtF(velTestRatio);
                 for (uint8_t i = 0; i<=imax; i++) {
-                    innovVelPos[i] *= scaleFactor;
+                    varInnovVelPos[i] *= velTestRatio;
                 }
-                innovVelPos[4] *= scaleFactor;
                 velCheckPassed = true;
                 lastVelPassTime_ms = imuSampleTime_ms;
             }