AP_NavEKF3: Fade each vert vel variance clip count over 1 second

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -274,7 +274,7 @@ void NavEKF3_core::ResetHeight(void)
     {
         P[6][6] = sq(frontend->_gpsVertVelNoise);
     }
-    vertVelVarClipCount = 0;
+    vertVelVarClipCounter = 0;
 }
 
 // Zero the EKF height datum

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -232,7 +232,7 @@ void NavEKF3_core::InitialiseVariables()
     hgtTimeout = true;
     tasTimeout = true;
     badIMUdata = false;
-    vertVelVarClipCount = 0;
+    vertVelVarClipCounter = 0;
     finalInflightYawInit = false;
     dtIMUavg = ins.get_loop_delta_t();
     dtEkfAvg = EKF_TARGET_DT;
@@ -1717,6 +1717,10 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr)
     // constrain values to prevent ill-conditioning
     ConstrainVariances();
 
+    if (vertVelVarClipCounter > 0) {
+        vertVelVarClipCounter--;
+    }
+
     calcTiltErrorVariance();
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -1806,8 +1810,11 @@ void NavEKF3_core::ConstrainVariances()
     // check for collapse of the vertical velocity variance
     if (P[6][6] < VEL_STATE_MIN_VARIANCE) {
         P[6][6] = VEL_STATE_MIN_VARIANCE;
-        vertVelVarClipCount++;
-        if (vertVelVarClipCount > VERT_VEL_VAR_CLIP_COUNT_LIM) {
+        // this counter is decremented by 1 each prediction cycle in CovariancePrediction
+        // resulting in the count from each clip event fading to zero over 1 second which
+        // is sufficient to capture collapse from fusion of the lowest update rate sensor
+        vertVelVarClipCounter += EKF_TARGET_RATE_HZ;
+        if (vertVelVarClipCounter > VERT_VEL_VAR_CLIP_COUNT_LIM) {
             // reset the corresponding covariances
             zeroRows(P,6,6);
             zeroCols(P,6,6);
@@ -1821,7 +1828,7 @@ void NavEKF3_core::ConstrainVariances()
             {
                 P[6][6] = sq(frontend->_gpsVertVelNoise);
             }
-            vertVelVarClipCount = 0;
+            vertVelVarClipCounter = 0;
         }
     }
 

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -86,7 +86,8 @@
 
 // maximum number of times the vertical velocity variance can hit the lower limit before the
 // associated states, variances and covariances are reset
-#define VERT_VEL_VAR_CLIP_COUNT_LIM 5
+#define EKF_TARGET_RATE_HZ uint32_t(1.0f / EKF_TARGET_DT)
+#define VERT_VEL_VAR_CLIP_COUNT_LIM (5 * EKF_TARGET_RATE_HZ)
 
 class NavEKF3_core : public NavEKF_core_common
 {
@@ -945,7 +946,7 @@ private:
     bool magTimeout;                // boolean true if magnetometer measurements have failed for too long and have timed out
     bool tasTimeout;                // boolean true if true airspeed measurements have failed for too long and have timed out
     bool badIMUdata;                // boolean true if the bad IMU data is detected
-    uint8_t vertVelVarClipCount;    // number of times the vertical velocity variance has exveeded the lower limit and been clipped since the last reset
+    uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
 
     float gpsNoiseScaler;           // Used to scale the  GPS measurement noise and consistency gates to compensate for operation with small satellite counts
     Matrix24 P;                     // covariance matrix