AP_NavEKF2: Reduce false positive on EKF health check

Apply filtering to baro innovation check and and don't apply innovation checks once aiding has commenced because GPS and baro disturbances on the ground and during launch could generate a false positive

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -31,9 +31,9 @@ bool NavEKF2_core::healthy(void) const
     if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
         return false;
     }
-    // barometer and position innovations must be within limits when on-ground
+    // position and height innovations must be within limits when on-ground and in a static mode of operation
     float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
-    if (onGround && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
+    if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) {
         return false;
     }
 

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -410,6 +410,16 @@ void NavEKF2_core::FuseVelPosNED()
             hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime_ms;
             // Fuse height data if healthy or timed out or in constant position mode
             if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE)) {
+                // Calculate a filtered value to be used by pre-flight health checks
+                // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
+                if (onGround) {
+                    float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
+                    const float hgtInnovFiltTC = 2.0f;
+                    float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
+                    hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
+                } else {
+                    hgtInnovFiltState = 0.0f;
+                }
                 hgtHealth = true;
                 lastHgtPassTime_ms = imuSampleTime_ms;
                 // if timed out, reset the height, but do not fuse data on this time step

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -203,6 +203,7 @@ void NavEKF2_core::InitialiseVariables()
     magFuseTiltInhibit = false;
     posResetNE.zero();
     velResetNE.zero();
+    hgtInnovFiltState = 0.0f;
 }
 
 // Initialise the states from accelerometer and magnetometer data (if present)

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -782,6 +782,7 @@ private:
     float yawTestRatio;             // square of magnetometer yaw angle innovation divided by fail threshold
     Quaternion prevQuatMagReset;    // Quaternion from the last time the magnetic field state reset condition test was performed
     uint8_t fusionHorizonOffset;    // number of IMU samples that the fusion time horizon  has been shifted forward to prevent multiple EKF instances fusing data at the same time
+    float hgtInnovFiltState;        // state used for fitering of the height innovations used for pre-flight checks
 
     // variables used to calulate a vertical velocity that is kinematically consistent with the verical position
     float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.