Merge pull request #259 from PX4/pr-ekfBugFix

EKF: Protect against covariance prediction and update errors

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);
+	_prev_dvel_bias_var(0) = P[13][13] = sq(_params.switch_on_accel_bias * dt);
-	P[14][14] = P[13][13];
+	_prev_dvel_bias_var(1) = P[14][14] = P[13][13];
-	P[15][15] = P[13][13];
+	_prev_dvel_bias_var(2) = P[15][15] = P[13][13];
 
 	// variances for optional states
 
@@ -143,7 +143,7 @@ void Ekf::predictCovariance()
 	float dvy_b = _state.accel_bias(1);
 	float dvz_b = _state.accel_bias(2);
 
-	float dt = _imu_sample_delayed.delta_ang_dt;
+	float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.0005f * FILTER_UPDATE_PERIOD_MS, 0.002f * FILTER_UPDATE_PERIOD_MS);
 
 	// compute noise variance for stationary processes
 	float process_noise[_k_num_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
+		// Inhibit delta velocity bias learning by zeroing the covariance terms
-		// previous prediction step which prevents these states being updated by any of the measurement fusion
-		// processes, but  allows estimation to be resumed later.
 		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)