EKF: inhibit accelerometer bias learning if manoeuvre levels are excessive

This prevents bad scale factors and other errors associated with rapid manoeuvres corrupting the accelerometer bias estimates.

EKF/common.h

@@ -269,8 +269,11 @@ struct parameters {
 	float vel_Tau;	// velocity state correction time constant (1/sec)
 	float pos_Tau;	// postion state correction time constant (1/sec)
 
-	// state limits
-	float acc_bias_lim;	// maximum accel bias magnitude (m/s/s)
+	// accel bias learning control
+	float acc_bias_lim;		// maximum accel bias magnitude (m/s/s)
+	float acc_bias_learn_acc_lim;	// learning is disabled if the magnitude of the IMU acceleration vector is greeater than this (m/sec**2)
+	float acc_bias_learn_gyr_lim;	// learning is disabled if the magnitude of the IMU angular rate vector is greeater than this (rad/sec)
+	float acc_bias_learn_tc;	// time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
 
 	unsigned no_gps_timeout_max;	// maximum time we allow dead reckoning while both gps position and velocity measurements are being
 									// rejected
@@ -376,8 +379,11 @@ struct parameters {
 		vel_Tau = 0.25f;
 		pos_Tau = 0.25f;
 
-		// state limiting
+		// accel bias state limiting
 		acc_bias_lim = 0.4f;
+		acc_bias_learn_acc_lim = 25.0f;
+		acc_bias_learn_gyr_lim = 3.0f;
+		acc_bias_learn_tc = 0.5f;
 
 		no_gps_timeout_max = 7e6;	// maximum seven seconds of dead reckoning time for gps
 

EKF/covariance.cpp

@@ -151,9 +151,19 @@ void Ekf::predictCovariance()
 	// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
 	float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
 
-	// convert rate of change of acceerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
+	// 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
+	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) {
+		_accel_bias_inhibit = true;
+	} else {
+		_accel_bias_inhibit = false;
+	}
+
 	// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
 	float mag_I_sig;
 	if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][18]) < 0.1f) {
@@ -371,7 +381,7 @@ void Ekf::predictCovariance()
 	}
 
 	// Don't calculate these covariance terms if IMU delta vlocity bias estimation is inhibited
-	if (!(_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) {
+	if (!(_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) && !_accel_bias_inhibit) {
 
 		// calculate variances and upper diagonal covariances for IMU delta velocity bias states
 		nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10];
@@ -425,6 +435,13 @@ void Ekf::predictCovariance()
 			nextP[i][i] += process_noise[i];
 		}
 
+	} else {
+		zeroRows(nextP,13,15);
+		zeroCols(nextP,13,15);
+		nextP[13][13] = P[14][14];
+		nextP[14][14] = P[15][15];
+		nextP[15][15] = P[15][15];
+
 	}
 
 	// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion

EKF/ekf.cpp

@@ -196,6 +196,9 @@ bool Ekf::init(uint64_t timestamp)
 	_fault_status.value = 0;
 	_innov_check_fail_status.value = 0;
 
+	_accel_mag_filt = 0.0f;
+	_ang_rate_mag_filt = 0.0f;
+
 	return ret;
 }
 

EKF/ekf.h

@@ -321,6 +321,11 @@ private:
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
+	// variables used to inhibit accel bias learning
+	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)
+
 	// Terrain height state estimation
 	float _terrain_vpos;		// estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
 	float _terrain_var;		// variance of terrain position estimate (m^2)