AP_NavEKF3: Provide option to clip velocity and position innovations

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -165,7 +165,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
 
     // @Param: VEL_I_GATE
     // @DisplayName: GPS velocity innovation gate size
-    // @Description: This sets the percentage number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
+    // @Description: This sets the percentage number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. If EK3_GLITCH_RAD set to 0 the velocity innovations will be clipped instead of rejected if they exceed the gate size and a smaller value of EK3_VEL_I_GATE not exceeding 300 is recommended to limit the effect of GPS transient errors.
     // @Range: 100 1000
     // @Increment: 25
     // @User: Advanced
@@ -182,7 +182,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
 
     // @Param: POS_I_GATE
     // @DisplayName: GPS position measurement gate size
-    // @Description: This sets the percentage number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
+    // @Description: This sets the percentage number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. If EK3_GLITCH_RAD has been set to 0 the horizontal position innovations will be clipped instead of rejected if they exceed the gate size so a smaller value of EK3_POS_I_GATE not exceeding 300 is recommended to limit the effect of GPS transient errors.
     // @Range: 100 1000
     // @Increment: 25
     // @User: Advanced
@@ -190,7 +190,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
 
     // @Param: GLITCH_RAD
     // @DisplayName: GPS glitch radius gate size (m)
-    // @Description: This controls the maximum radial uncertainty in position between the value predicted by the filter and the value measured by the GPS before the filter position and velocity states are reset to the GPS. Making this value larger allows the filter to ignore larger GPS glitches but also means that non-GPS errors such as IMU and compass can create a larger error in position before the filter is forced back to the GPS position.
+    // @Description: This controls the maximum radial uncertainty in position between the value predicted by the filter and the value measured by the GPS before the filter position and velocity states are reset to the GPS. Making this value larger allows the filter to ignore larger GPS glitches but also means that non-GPS errors such as IMU and compass can create a larger error in position before the filter is forced back to the GPS position. If EK3_GLITCH_RAD set to 0 the GPS innovations will be clipped instead of rejected if they exceed the gate size set by EK3_VEL_I_GATE and EK3_POS_I_GATE which can be useful if poor quality sensor data is causing GPS rejection and loss of navigation but does make the EKF more susceptible to GPS glitches.
     // @Range: 10 100
     // @Increment: 5
     // @User: Advanced
@@ -216,7 +216,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
 
     // @Param: HGT_I_GATE
     // @DisplayName: Height measurement gate size
-    // @Description: This sets the percentage number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
+    // @Description: This sets the percentage number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.  If EK3_GLITCH_RAD set to 0 the vertical position innovations will be clipped instead of rejected if they exceed the gate size and a smaller value of EK3_HGT_I_GATE not exceeding 300 is recommended to limit the effect of height sensor transient errors.
     // @Range: 100 1000
     // @Increment: 25
     // @User: Advanced

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -734,6 +734,16 @@ void NavEKF3_core::FuseVelPosNED()
             if (posTestRatio < 1.0f || (PV_AidingMode == AID_NONE)) {
                 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.
+                posCheckPassed = true;
+                lastPosPassTime_ms = imuSampleTime_ms;
+                const ftype scaleFactor = 1.0F / sqrtF(posTestRatio);
+                innovVelPos[3] *= scaleFactor;
+                innovVelPos[4] *= scaleFactor;
+                posCheckPassed = true;
+                lastPosPassTime_ms = imuSampleTime_ms;
             }
 
             // Use position data if healthy or timed out or bad IMU data
@@ -741,7 +751,9 @@ void NavEKF3_core::FuseVelPosNED()
             // from the measurement un-opposed if test threshold is exceeded.
             if (posCheckPassed || posTimeout || badIMUdata) {
                 // if timed out or outside the specified uncertainty radius, reset to the external sensor
-                if (posTimeout || ((P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax)))) {
+                // if velocity drift is being constrained, dont reset until gps passes quality checks
+                const bool posVarianceIsTooLarge = (frontend->_gpsGlitchRadiusMax > 0) && (P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax));
+                if (posTimeout || posVarianceIsTooLarge) {
                     // reset the position to the current external sensor position
                     ResetPosition(resetDataSource::DEFAULT);
 
@@ -797,6 +809,18 @@ void NavEKF3_core::FuseVelPosNED()
             if (velTestRatio < 1.0) {
                 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.
+                posCheckPassed = true;
+                lastPosPassTime_ms = imuSampleTime_ms;
+                const ftype scaleFactor = 1.0F / sqrtF(velTestRatio);
+                for (uint8_t i = 0; i<=imax; i++) {
+                    innovVelPos[i] *= scaleFactor;
+                }
+                innovVelPos[4] *= scaleFactor;
+                velCheckPassed = true;
+                lastVelPassTime_ms = imuSampleTime_ms;
             }
 
             // Use velocity data if healthy, timed out or when IMU fault has been detected
@@ -833,6 +857,15 @@ void NavEKF3_core::FuseVelPosNED()
             if (hgtTestRatio < maxTestRatio) {
                 hgtCheckPassed = true;
                 lastHgtPassTime_ms = imuSampleTime_ms;
+            } else if ((frontend->_gpsGlitchRadiusMax <= 0) && (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS)) {
+                // handle 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.
+                posCheckPassed = true;
+                lastPosPassTime_ms = imuSampleTime_ms;
+                const ftype scaleFactor = 1.0F / sqrtF(hgtTestRatio);
+                innovVelPos[5] *= scaleFactor;
+                hgtCheckPassed = true;
+                lastHgtPassTime_ms = imuSampleTime_ms;
             }
 
             // Use height data if innovation check passed or timed out or if bad IMU data