From 2565f3917cbaa942779b19b16076442dbb3bb473 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Thu, 17 Jun 2021 10:21:22 +1000
Subject: [PATCH] AP_NavEKF3: Protect against collapse of velocity and position
 variances

---
 .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp    |  1 +
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp      | 29 +++++++++++++++++--
 libraries/AP_NavEKF3/AP_NavEKF3_core.h        |  9 ++++++
 3 files changed, 36 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index addab78a0c..b153010a84 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -274,6 +274,7 @@ void NavEKF3_core::ResetHeight(void)
     {
         P[6][6] = sq(frontend->_gpsVertVelNoise);
     }
+    vertVelVarClipCount = 0;
 }
 
 // Zero the EKF height datum
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 09fb19cf48..d354b36444 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -232,6 +232,7 @@ void NavEKF3_core::InitialiseVariables()
     hgtTimeout = true;
     tasTimeout = true;
     badIMUdata = false;
+    vertVelVarClipCount = 0;
     finalInflightYawInit = false;
     dtIMUavg = ins.get_loop_delta_t();
     dtEkfAvg = EKF_TARGET_DT;
@@ -1800,9 +1801,31 @@ void NavEKF3_core::ForceSymmetry()
 void NavEKF3_core::ConstrainVariances()
 {
     for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error
-    for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
-    for (uint8_t i=7; i<=8; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
-    P[9][9] = constrain_float(P[9][9],0.0f,1.0e6f); // vertical position
+    for (uint8_t i=4; i<=5; i++) P[i][i] = constrain_float(P[i][i], VEL_STATE_MIN_VARIANCE, 1.0e3f); // NE velocity
+
+    // 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) {
+            // reset the corresponding covariances
+            zeroRows(P,6,6);
+            zeroCols(P,6,6);
+
+            // set the variances to the measurement variance
+        #if EK3_FEATURE_EXTERNAL_NAV
+            if (useExtNavVel) {
+                P[6][6] = sq(extNavVelDelayed.err);
+            } else
+        #endif
+            {
+                P[6][6] = sq(frontend->_gpsVertVelNoise);
+            }
+            vertVelVarClipCount = 0;
+        }
+    }
+
+    for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_float(P[i][i], POS_STATE_MIN_VARIANCE, 1.0e6f); // NED position
 
     if (!inhibitDelAngBiasStates) {
         for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg));
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 7ba8f4ccff..257ed7d9a0 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -80,6 +80,14 @@
 // number of continuous valid GSF yaw estimates required to confirm valid hostory
 #define GSF_YAW_VALID_HISTORY_THRESHOLD 5
 
+// minimum variances allowed for velocity and position states
+#define VEL_STATE_MIN_VARIANCE 1E-4f
+#define POS_STATE_MIN_VARIANCE 1E-4f
+
+// 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
+
 class NavEKF3_core : public NavEKF_core_common
 {
 public:
@@ -937,6 +945,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
 
     float gpsNoiseScaler;           // Used to scale the  GPS measurement noise and consistency gates to compensate for operation with small satellite counts
     Matrix24 P;                     // covariance matrix