AP_NavEKF2: Increase resolution of innovation consistency gate parameters

Also adds protection against setting the gate to a number that would casue numerical errors.

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -24,15 +24,15 @@
 #define GBIAS_PNOISE_DEFAULT    7.0E-05f
 #define ABIAS_PNOISE_DEFAULT    1.0E-04f
 #define MAG_PNOISE_DEFAULT      2.5E-02f
-#define VEL_GATE_DEFAULT        2
-#define POS_GATE_DEFAULT        3
-#define HGT_GATE_DEFAULT        3
-#define MAG_GATE_DEFAULT        3
+#define VEL_GATE_DEFAULT        200
+#define POS_GATE_DEFAULT        300
+#define HGT_GATE_DEFAULT        300
+#define MAG_GATE_DEFAULT        300
 #define MAG_CAL_DEFAULT         3
 #define GLITCH_RADIUS_DEFAULT   25
 #define FLOW_MEAS_DELAY         10
 #define FLOW_NOISE_DEFAULT      0.25f
-#define FLOW_GATE_DEFAULT       3
+#define FLOW_GATE_DEFAULT       300
 #define GSCALE_PNOISE_DEFAULT   3.0E-03f
 #define CHECK_SCALER_DEFAULT    100
 
@@ -48,15 +48,15 @@
 #define GBIAS_PNOISE_DEFAULT    7.0E-05f
 #define ABIAS_PNOISE_DEFAULT    1.0E-04f
 #define MAG_PNOISE_DEFAULT      2.5E-02f
-#define VEL_GATE_DEFAULT        2
-#define POS_GATE_DEFAULT        3
-#define HGT_GATE_DEFAULT        3
-#define MAG_GATE_DEFAULT        3
+#define VEL_GATE_DEFAULT        200
+#define POS_GATE_DEFAULT        300
+#define HGT_GATE_DEFAULT        300
+#define MAG_GATE_DEFAULT        300
 #define MAG_CAL_DEFAULT         2
 #define GLITCH_RADIUS_DEFAULT   25
 #define FLOW_MEAS_DELAY         10
 #define FLOW_NOISE_DEFAULT      0.25f
-#define FLOW_GATE_DEFAULT       3
+#define FLOW_GATE_DEFAULT       300
 #define GSCALE_PNOISE_DEFAULT   3.0E-03f
 #define CHECK_SCALER_DEFAULT    100
 
@@ -72,15 +72,15 @@
 #define GBIAS_PNOISE_DEFAULT    7.0E-05f
 #define ABIAS_PNOISE_DEFAULT    1.0E-04f
 #define MAG_PNOISE_DEFAULT      2.5E-02f
-#define VEL_GATE_DEFAULT        2
-#define POS_GATE_DEFAULT        3
-#define HGT_GATE_DEFAULT        4
-#define MAG_GATE_DEFAULT        2
+#define VEL_GATE_DEFAULT        200
+#define POS_GATE_DEFAULT        300
+#define HGT_GATE_DEFAULT        400
+#define MAG_GATE_DEFAULT        200
 #define MAG_CAL_DEFAULT         0
 #define GLITCH_RADIUS_DEFAULT   25
 #define FLOW_MEAS_DELAY         10
 #define FLOW_NOISE_DEFAULT      0.25f
-#define FLOW_GATE_DEFAULT       3
+#define FLOW_GATE_DEFAULT       300
 #define GSCALE_PNOISE_DEFAULT   3.0E-03f
 #define CHECK_SCALER_DEFAULT    150
 
@@ -96,15 +96,15 @@
 #define GBIAS_PNOISE_DEFAULT    7.0E-05f
 #define ABIAS_PNOISE_DEFAULT    1.0E-04f
 #define MAG_PNOISE_DEFAULT      2.5E-02f
-#define VEL_GATE_DEFAULT        2
-#define POS_GATE_DEFAULT        3
-#define HGT_GATE_DEFAULT        3
-#define MAG_GATE_DEFAULT        3
+#define VEL_GATE_DEFAULT        200
+#define POS_GATE_DEFAULT        300
+#define HGT_GATE_DEFAULT        300
+#define MAG_GATE_DEFAULT        300
 #define MAG_CAL_DEFAULT         3
 #define GLITCH_RADIUS_DEFAULT   25
 #define FLOW_MEAS_DELAY         10
 #define FLOW_NOISE_DEFAULT      0.25f
-#define FLOW_GATE_DEFAULT       3
+#define FLOW_GATE_DEFAULT       300
 #define GSCALE_PNOISE_DEFAULT   3.0E-03f
 #define CHECK_SCALER_DEFAULT    100
 
@@ -151,9 +151,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
     // @Param: VEL_GATE
     // @DisplayName: GPS velocity innovation gate size
-    // @Description: This sets the number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
-    // @Range: 1 100
-    // @Increment: 1
+    // @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 willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
+    // @Range: 100 1000
+    // @Increment: 25
     // @User: Advanced
     AP_GROUPINFO("VEL_GATE", 4, NavEKF2, _gpsVelInnovGate, VEL_GATE_DEFAULT),
 
@@ -168,9 +168,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
     // @Param: POS_GATE
     // @DisplayName: GPS position measurement gate size
-    // @Description: This sets the 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.
-    // @Range: 1 100
-    // @Increment: 1
+    // @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.
+    // @Range: 100 1000
+    // @Increment: 25
     // @User: Advanced
     AP_GROUPINFO("POS_GATE", 6, NavEKF2, _gpsPosInnovGate, POS_GATE_DEFAULT),
 
@@ -212,9 +212,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
     // @Param: HGT_GATE
     // @DisplayName: Height measurement gate size
-    // @Description: This sets the 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.
-    // @Range: 1 100
-    // @Increment: 1
+    // @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.
+    // @Range: 100 1000
+    // @Increment: 25
     // @User: Advanced
     AP_GROUPINFO("HGT_GATE", 11, NavEKF2, _hgtInnovGate, HGT_GATE_DEFAULT),
 
@@ -247,9 +247,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
     // @Param: MAG_GATE
     // @DisplayName: Magnetometer measurement gate size
-    // @Description: This parameter sets the number of standard deviations applied to the magnetometer 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.
-    // @Range: 1 100
-    // @Increment: 1
+    // @Description: This sets the percentage number of standard deviations applied to the magnetometer 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.
+    // @Range: 100 1000
+    // @Increment: 25
     // @User: Advanced
     AP_GROUPINFO("MAG_GATE", 15, NavEKF2, _magInnovGate, MAG_GATE_DEFAULT),
 
@@ -266,11 +266,11 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
     // @Param: EAS_GATE
     // @DisplayName: Airspeed measurement gate size
-    // @Description: This is the RMS value of noise in equivalent airspeed measurements used by planes. Increasing it reduces the weighting of airspeed measurements and will make wind speed estimates less noisy and slower to converge. Increasing also increases navigation errors when dead-reckoning without GPS measurements.
-    // @Range: 1 100
-    // @Increment: 1
+    // @Description: This sets the percentage number of standard deviations applied to the airspeed 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.
+    // @Range: 100 1000
+    // @Increment: 25
     // @User: Advanced
-    AP_GROUPINFO("EAS_GATE", 17, NavEKF2, _tasInnovGate, 10),
+    AP_GROUPINFO("EAS_GATE", 17, NavEKF2, _tasInnovGate, 1000),
 
     // Rangefinder measurement parameters
 
@@ -285,11 +285,11 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
     // @Param: RNG_GATE
     // @DisplayName: Range finder measurement gate size
-    // @Description: This sets the number of standard deviations applied to the range finder 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.
-    // @Range: 1 - 100
-    // @Increment: 1
+    // @Description: This sets the percentage number of standard deviations applied to the range finder 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.
+    // @Range: 100 - 1000
+    // @Increment: 25
     // @User: Advanced
-    AP_GROUPINFO("RNG_GATE", 19, NavEKF2, _rngInnovGate, 5),
+    AP_GROUPINFO("RNG_GATE", 19, NavEKF2, _rngInnovGate, 500),
 
     // Optical flow measurement parameters
 
@@ -313,9 +313,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
     // @Param: FLOW_GATE
     // @DisplayName: Optical Flow measurement gate size
-    // @Description: This sets the number of standard deviations applied to the optical flow 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.
-    // @Range: 1 - 100
-    // @Increment: 1
+    // @Description: This sets the percentage number of standard deviations applied to the optical flow 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.
+    // @Range: 100 - 1000
+    // @Increment: 25
     // @User: Advanced
     AP_GROUPINFO("FLOW_GATE", 22, NavEKF2, _flowInnovGate, FLOW_GATE_DEFAULT),
 

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -293,17 +293,17 @@ private:
     AP_Int16 _gpsDelay_ms;          // effective average delay of GPS measurements relative to inertial measurement (msec)
     AP_Int16 _hgtDelay_ms;          // effective average delay of Height measurements relative to inertial measurements (msec)
     AP_Int8  _fusionModeGPS;        // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity
-    AP_Int8  _gpsVelInnovGate;      // Number of standard deviations applied to GPS velocity innovation consistency check
-    AP_Int8  _gpsPosInnovGate;      // Number of standard deviations applied to GPS position innovation consistency check
-    AP_Int8  _hgtInnovGate;         // Number of standard deviations applied to height innovation consistency check
-    AP_Int8  _magInnovGate;         // Number of standard deviations applied to magnetometer innovation consistency check
-    AP_Int8  _tasInnovGate;         // Number of standard deviations applied to true airspeed innovation consistency check
+    AP_Int16  _gpsVelInnovGate;     // Percentage number of standard deviations applied to GPS velocity innovation consistency check
+    AP_Int16  _gpsPosInnovGate;     // Percentage number of standard deviations applied to GPS position innovation consistency check
+    AP_Int16  _hgtInnovGate;        // Percentage number of standard deviations applied to height innovation consistency check
+    AP_Int16  _magInnovGate;        // Percentage number of standard deviations applied to magnetometer innovation consistency check
+    AP_Int16  _tasInnovGate;        // Percentage number of standard deviations applied to true airspeed innovation consistency check
     AP_Int8  _magCal;               // Sets activation condition for in-flight magnetometer calibration
     AP_Int8 _gpsGlitchRadiusMax;    // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m
     AP_Float _flowNoise;            // optical flow rate measurement noise
-    AP_Int8  _flowInnovGate;        // Number of standard deviations applied to optical flow innovation consistency check
+    AP_Int16  _flowInnovGate;       // Percentage number of standard deviations applied to optical flow innovation consistency check
     AP_Int8  _flowDelay_ms;         // effective average delay of optical flow measurements rel to IMU (msec)
-    AP_Int8  _rngInnovGate;         // Number of standard deviations applied to range finder innovation consistency check
+    AP_Int16  _rngInnovGate;        // Percentage number of standard deviations applied to range finder innovation consistency check
     AP_Float _maxFlowRate;          // Maximum flow rate magnitude that will be accepted by the filter
     AP_Int8 _altSource;             // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder.
     AP_Float _gyroScaleProcessNoise;// gyro scale factor state process noise : 1/s

libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp

@@ -121,7 +121,7 @@ void NavEKF2_core::FuseAirspeed()
         innovVtas = VtasPred - tasDataDelayed.tas;
 
         // calculate the innovation consistency test ratio
-        tasTestRatio = sq(innovVtas) / (sq(frontend->_tasInnovGate) * varInnovVtas);
+        tasTestRatio = sq(innovVtas) / (sq(max(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
 
         // fail if the ratio is > 1, but don't fail if bad IMU data
         tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -321,7 +321,7 @@ void NavEKF2_core::FuseMagnetometer()
 
         // calculate the innovation test ratios
         for (uint8_t i = 0; i<=2; i++) {
-            magTestRatio[i] = sq(innovMag[i]) / (sq(max(frontend->_magInnovGate,1)) * varInnovMag[i]);
+            magTestRatio[i] = sq(innovMag[i]) / (sq(max(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
         }
 
         // check the last values from all components and set magnetometer health accordingly
@@ -701,7 +701,7 @@ void NavEKF2_core::fuseCompass()
     }
 
     // calculate the innovation test ratio
-    yawTestRatio = sq(innovation) / (sq(max(frontend->_magInnovGate,1)) * varInnov);
+    yawTestRatio = sq(innovation) / (sq(max(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov);
 
     // Declare the magnetometer unhealthy if the innovation test fails
     if (yawTestRatio > 1.0f) {

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -155,7 +155,7 @@ void NavEKF2_core::EstimateTerrainOffset()
             innovRng = predRngMeas - rangeDataDelayed.rng;
 
             // calculate the innovation consistency test ratio
-            auxRngTestRatio = sq(innovRng) / (sq(frontend->_rngInnovGate) * varInnovRng);
+            auxRngTestRatio = sq(innovRng) / (sq(max(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
 
             // Check the innovation for consistency and don't fuse if > 5Sigma
             if ((sq(innovRng)*SK_RNG) < 25.0f)
@@ -243,7 +243,7 @@ void NavEKF2_core::EstimateTerrainOffset()
             K_OPT = Popt*H_OPT/auxFlowObsInnovVar;
 
             // calculate the innovation consistency test ratio
-            auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(frontend->_flowInnovGate) * auxFlowObsInnovVar);
+            auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(max(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar);
 
             // don't fuse if optical flow data is outside valid range
             if (max(flowRadXY[0],flowRadXY[1]) < frontend->_maxFlowRate) {
@@ -627,7 +627,7 @@ void NavEKF2_core::FuseOptFlow()
         }
 
         // calculate the innovation consistency test ratio
-        flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(frontend->_flowInnovGate) * varInnovOptFlow[obsIndex]);
+        flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(max(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
 
         // Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
         if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -304,7 +304,7 @@ void NavEKF2_core::FuseVelPosNED()
             varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
             varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
             // apply an innovation consistency threshold test, but don't fail if bad IMU data
-            float maxPosInnov2 = sq(frontend->_gpsPosInnovGate)*(varInnovVelPos[3] + varInnovVelPos[4]);
+            float maxPosInnov2 = sq(max(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
             posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
             posHealth = ((posTestRatio < 1.0f) || badIMUdata);
             // declare a timeout condition if we have been too long without data or not aiding
@@ -362,7 +362,7 @@ void NavEKF2_core::FuseVelPosNED()
             }
             // apply an innovation consistency threshold test, but don't fail if bad IMU data
             // calculate the test ratio
-            velTestRatio = innovVelSumSq / (varVelSum * sq(frontend->_gpsVelInnovGate));
+            velTestRatio = innovVelSumSq / (varVelSum * sq(max(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
             // fail if the ratio is greater than 1
             velHealth = ((velTestRatio < 1.0f)  || badIMUdata);
             // declare a timeout if we have not fused velocity data for too long or not aiding
@@ -392,7 +392,7 @@ void NavEKF2_core::FuseVelPosNED()
             innovVelPos[5] = stateStruct.position.z - observation[5];
             varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
             // calculate the innovation consistency test ratio
-            hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend->_hgtInnovGate) * varInnovVelPos[5]);
+            hgtTestRatio = sq(innovVelPos[5]) / (sq(max(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
             // fail if the ratio is > 1, but don't fail if bad IMU data
             hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
             // Fuse height data if healthy or timed out or in constant position mode