EKF: Prevent covariance instability in delta velocity bias state estimation

EKF/common.h

@@ -212,7 +212,7 @@ struct parameters {
 
 	// process noise
 	float gyro_bias_p_noise{1.0e-3f};	// process noise for IMU rate gyro bias prediction (rad/sec**2)
-	float accel_bias_p_noise{3.0e-3f};	// process noise for IMU accelerometer bias prediction (m/sec**3)
+	float accel_bias_p_noise{6.0e-3f};	// process noise for IMU accelerometer bias prediction (m/sec**3)
 	float mage_p_noise{1.0e-3f};		// process noise for earth magnetic field prediction (Guass/sec)
 	float magb_p_noise{1.0e-4};		// process noise for body magnetic field prediction (Guass/sec)
 	float wind_vel_p_noise{1.0e-1f};		// process noise for wind velocity prediction (m/sec/sec)

EKF/covariance.cpp

@@ -729,11 +729,42 @@ void Ekf::fixCovarianceErrors()
 		zeroRows(P,13,15);
 		zeroCols(P,13,15);
 	} else {
-		// constrain variances
-		for (int i = 13; i <= 15; i++) {
-			P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]);
+		// Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
+		const float minSafeStateVar = 1e-9f;
+		float maxStateVar = minSafeStateVar;
+		bool resetRequired = false;
+		for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
+		    if (P[stateIndex][stateIndex] > maxStateVar) {
+			maxStateVar = P[stateIndex][stateIndex];
+		    } else if (P[stateIndex][stateIndex] < minSafeStateVar) {
+			resetRequired = true;
+		    }
+		}
+
+		// To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
+		// not exceed 100 and the minimum variance must not fall below the target minimum
+		const float minStateVarTarget = 1E-8f;
+		float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
+		for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
+		    P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * _dt_ekf_avg));
+		}
+
+		// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
+		if (resetRequired) {
+		    float delVelBiasVar[3];
+		    // store all delta velocity bias variances
+		    for (uint8_t i=0; i<=2; i++) {
+			delVelBiasVar[i] = P[i+13][i+13];
+		    }
+		    // reset all delta velocity bias covariances
+		    zeroCols(P,13,15);
+		    // restore all delta velocity bias variances
+		    for (uint8_t i=0; i<=2; i++) {
+			P[i+13][i+13] = delVelBiasVar[i];
+		    }
 		}
 
+		// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
 		// calculate accel bias term aligned with the gravity vector
 		float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
 		float down_dvel_bias = 0.0f;