EKF: Prevent rounding errors causing bad conditioned covariance matrix

The previous practice of relying on the off-diagonals being zero caused problems with conditioning of the magnetometer fusion on one flight. By storing the variances when the learning inhibit becomes active and ensuring that the rows and columns in the covariance matrix for the inhibited states are always zero, the observed numerical conditioning error has been eliminated for replay of the problem flight log .

EKF/covariance.cpp

@@ -87,9 +87,9 @@ void Ekf::initialiseCovariance()
 	P[12][12] = P[10][10];
 
 	// accel bias
-	P[13][13] = sq(_params.switch_on_accel_bias * dt);
-	P[14][14] = P[13][13];
-	P[15][15] = P[13][13];
+	_prev_dvel_bias_var(0) = P[13][13] = sq(_params.switch_on_accel_bias * dt);
+	_prev_dvel_bias_var(1) = P[14][14] = P[13][13];
+	_prev_dvel_bias_var(2) = P[15][15] = P[13][13];
 
 	// variances for optional states
 
@@ -159,8 +159,25 @@ void Ekf::predictCovariance()
 	_ang_rate_mag_filt = fmax(_imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt);
 	_accel_mag_filt = fmax(_imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt);
 	if (_ang_rate_mag_filt > dt * _params.acc_bias_learn_gyr_lim || _accel_mag_filt > dt * _params.acc_bias_learn_acc_lim) {
+		// store the bias state variances to be reinstated later
+		if (!_accel_bias_inhibit) {
+			_prev_dvel_bias_var(0) = P[13][13];
+			_prev_dvel_bias_var(1) = P[14][14];
+			_prev_dvel_bias_var(2) = P[15][15];
+		}
 		_accel_bias_inhibit = true;
 	} else {
+		if (_accel_bias_inhibit) {
+			// reinstate the bias state variances
+			P[13][13] = _prev_dvel_bias_var(0);
+			P[14][14] = _prev_dvel_bias_var(1);
+			P[15][15] = _prev_dvel_bias_var(2);
+		} else {
+			// store the bias state variances to be reinstated later
+			_prev_dvel_bias_var(0) = P[13][13];
+			_prev_dvel_bias_var(1) = P[14][14];
+			_prev_dvel_bias_var(2) = P[15][15];
+		}
 		_accel_bias_inhibit = false;
 	}
 
@@ -436,15 +453,9 @@ void Ekf::predictCovariance()
 		}
 
 	} else {
-		// Inhibit delta velocity bias learning. Zero the covariance terms but preserve the variances from the
-		// previous prediction step which prevents these states being updated by any of the measurement fusion
-		// processes, but  allows estimation to be resumed later.
+		// Inhibit delta velocity bias learning by zeroing the covariance terms
 		zeroRows(nextP,13,15);
 		zeroCols(nextP,13,15);
-		nextP[13][13] = P[13][13];
-		nextP[14][14] = P[14][14];
-		nextP[15][15] = P[15][15];
-
 	}
 
 	// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
@@ -714,7 +725,7 @@ void Ekf::fixCovarianceErrors()
 	// by ensuring the corresponding covariance matrix values are kept at zero
 
 	// accelerometer bias states
-	if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) {
+	if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || _accel_bias_inhibit) {
 		zeroRows(P,13,15);
 		zeroCols(P,13,15);
 	} else {

EKF/ekf.cpp

@@ -198,6 +198,7 @@ bool Ekf::init(uint64_t timestamp)
 
 	_accel_mag_filt = 0.0f;
 	_ang_rate_mag_filt = 0.0f;
+	_prev_dvel_bias_var.zero();
 
 	return ret;
 }

EKF/ekf.h

@@ -325,6 +325,7 @@ private:
 	bool _accel_bias_inhibit;	// true when the accel bias learning is being inhibited
 	float _accel_mag_filt;		// acceleration magnitude after application of a decaying envelope filter (m/sec**2)
 	float _ang_rate_mag_filt;	// angular rate magnitude after application of a decaying envelope filter (rad/sec)
+	Vector3f _prev_dvel_bias_var;	// saved delta velocity XYZ bias variances (m/sec)**2
 
 	// Terrain height state estimation
 	float _terrain_vpos;		// estimated vertical position of the terrain underneath the vehicle in local NED frame (m)