EKF: Improve protection against severe IMU accel errors

Use vertical velocity and position innovation failure to detect bad accelerometer data caused by clipping or aliasing which can cause large vertical acceleration errors and loss of height estimation. When bad accel data is detected:

1) Inhibit accelerometer bias learning
2) Force fusion of vertical velocity and height data
3) Increase accelerometer process noise

EKF/common.h

@@ -169,6 +169,11 @@ struct extVisionSample {
 #define RNG_MAX_INTERVAL	2e5
 #define EV_MAX_INTERVAL		2e5
 
+// bad accelerometer detection and mitigation
+#define BADACC_PROBATION	10E6	// Number of usec that accel data declared bad must continuously pass checks to be declared good
+#define BADACC_HGT_RESET	1E6	// Number of usec that accel data must continuously fail checks to trigger a height reset
+#define BADACC_BIAS_PNOISE_MULT 2.0f	// The delta velocity process noise is multiplied by this value when accel data is declared bad
+
 struct parameters {
 	// measurement source control
 	int fusion_mode;		// bitmasked integer that selects which of the GPS and optical flow aiding sources will be used

EKF/control.cpp

@@ -468,9 +468,26 @@ void Ekf::controlHeightSensorTimeouts()
 	// record time of last bad vert accel
 	if (bad_vert_accel) {
 		_time_bad_vert_accel =  _time_last_imu;
+	} else {
+		_time_good_vert_accel = _time_last_imu;
 	}
 
-	if ((P[9][9] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) {
+	// declare a bad vertical acceleration measurement and make the declaration persist
+	// for a minimum of 10 seconds
+	if (_bad_vert_accel_detected) {
+		_bad_vert_accel_detected = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
+	} else {
+		_bad_vert_accel_detected = bad_vert_accel;
+	}
+
+	// check if height is continuously failing becasue of accel errors
+	bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > BADACC_HGT_RESET);
+
+	// check if height has been inertial deadreckoning for too long
+	bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > 5e6);
+
+	// reset the vertical position and velocity states
+	if ((P[9][9] > sq(_params.hgt_reset_lim)) && (hgt_fusion_timeout || continuous_bad_accel_hgt)) {
 		// boolean that indicates we will do a height reset
 		bool reset_height = false;
 
@@ -484,7 +501,7 @@ void Ekf::controlHeightSensorTimeouts()
 			bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 
 			// check for inertial sensing errors in the last 10 seconds
-			bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < 10E6);
+			bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
 
 			// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
 			bool reset_to_gps = gps_hgt_available && gps_hgt_accurate && !_gps_hgt_faulty && !prev_bad_vert_accel;

EKF/covariance.cpp

@@ -154,11 +154,13 @@ void Ekf::predictCovariance()
 	// convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
 	float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
 
-	// inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities
+	// inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
 	float alpha = 1.0f - math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
 	_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) {
+	if (_ang_rate_mag_filt > dt * _params.acc_bias_learn_gyr_lim
+			|| _accel_mag_filt > dt * _params.acc_bias_learn_acc_lim
+			|| _bad_vert_accel_detected) {
 		// store the bias state variances to be reinstated later
 		if (!_accel_bias_inhibit) {
 			_prev_dvel_bias_var(0) = P[13][13];
@@ -232,6 +234,10 @@ void Ekf::predictCovariance()
 	float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
 	daxVar = dayVar = dazVar = sq(dt * gyro_noise);
 	float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
+	if (_bad_vert_accel_detected) {
+		// increase accelerometer process noise if bad accel data is detected
+		accel_noise *= BADACC_BIAS_PNOISE_MULT;
+	}
 	dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
 
 	// predict the covariance

EKF/ekf.cpp

@@ -123,7 +123,9 @@ Ekf::Ekf():
 	_gps_hgt_faulty(false),
 	_rng_hgt_faulty(false),
 	_primary_hgt_source(VDIST_SENSOR_BARO),
-	_time_bad_vert_accel(0)
+	_time_bad_vert_accel(0),
+	_time_good_vert_accel(0),
+	_bad_vert_accel_detected(false)
 {
 	_state = {};
 	_last_known_posNE.setZero();

EKF/ekf.h

@@ -349,6 +349,8 @@ private:
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel;	// last time a bad vertical accel was detected (usec)
+	uint64_t _time_good_vert_accel;	// last time a good vertical accel was detected (usec)
+	bool _bad_vert_accel_detected;	// true when bad vertical accelerometer data has been detected
 
 	// update the real time complementary filter states. This includes the prediction
 	// and the correction step