Merge pull request #254 from PX4/pr-ekfAccelErrorHandling

EKF: improve recovery from bad IMU accel data

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	4.9f	// The delta velocity process noise is set to this when accel data is declared bad (m/s**2)
+
 struct parameters {
 	// measurement source control
 	int fusion_mode;		// bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
@@ -501,6 +506,7 @@ union ekf_solution_status {
 	uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
 	uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
 	uint16_t gps_glitch         : 1; // 10 - True if the EKF has detected a GPS glitch
+	uint16_t accel_error        : 1; // 11 - True if the EKF has detected bad accelerometer data
     } flags;
     uint16_t value;
 };

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,11 @@ 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. Measurement errors due to
+		// vibration induced clipping commonly reach an equivalent 0.5g offset.
+		accel_noise = BADACC_BIAS_PNOISE;
+	}
 	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

EKF/ekf_helper.cpp

@@ -862,6 +862,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
 	bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f);
 	bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);
 	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
+	soln_status.flags.accel_error = _bad_vert_accel_detected;
 	*status = soln_status.value;
 }