Merge pull request #348 from PX4/pr-ekfAddImuBiasReset

EKF: Add method to enable the IMU bias states to be reset externally

EKF/covariance.cpp

@@ -91,6 +91,9 @@ void Ekf::initialiseCovariance()
 	_prev_dvel_bias_var(1) = P[14][14] = P[13][13];
 	_prev_dvel_bias_var(2) = P[15][15] = P[13][13];
 
+	// record IMU bias state covariance reset time - used to prevent resets being performed too often
+	_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;
+
 	// variances for optional states
 
 	// earth frame and body frame magnetic field

EKF/ekf.h

@@ -145,6 +145,13 @@ public:
 	*/
 	void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl);
 
+	/*
+	Reset all IMU bias states and covariances to initial alignment values.
+	Use when the IMU sensor has changed.
+	Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset.
+	*/
+	bool reset_imu_bias();
+
 	void get_vel_var(Vector3f &vel_var);
 
 	void get_pos_var(Vector3f &pos_var);
@@ -282,6 +289,8 @@ private:
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
 	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)
 
+	uint64_t _last_imu_bias_cov_reset_us{0};	///< time the last reset of IMU delta angle and velocity state covariances was performed (uSec)
+
 	Vector3f _earth_rate_NED;	///< earth rotation vector (NED) in rad/s
 
 	Dcmf _R_to_earth;	///< transformation matrix from body frame to earth frame from last EKF predition

EKF/ekf_helper.cpp

@@ -1039,6 +1039,36 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl)
 
 }
 
+bool Ekf::reset_imu_bias()
+{
+	if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < 10 * (1000 * 1000)) {
+		return false;
+
+	}
+
+	// Zero the delta angle and delta velocity bias states
+	_state.gyro_bias.zero();
+	_state.accel_bias.zero();
+
+	// Zero the corresponding covariances
+	zeroCols(P, 10, 15);
+	zeroRows(P, 10, 15);
+
+	// Set the corresponding variances to the values use for initial alignment
+	float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
+	P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt);
+	P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt);
+	_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;
+
+	// Set previous frame values
+	_prev_dvel_bias_var(0) = P[13][13];
+	_prev_dvel_bias_var(1) = P[14][14];
+	_prev_dvel_bias_var(2) = P[15][15];
+
+	return true;
+
+}
+
 // get EKF innovation consistency check status information comprising of:
 // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
 // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.

EKF/estimator_interface.h

@@ -192,6 +192,13 @@ public:
 	// set vehicle landed status data
 	void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;}
 
+	/*
+	Reset all IMU bias states and covariances to initial alignment values.
+	Use when the IMU sensor has changed.
+	Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset.
+	*/
+	virtual bool reset_imu_bias() = 0;
+
 	// get vehicle landed status data
 	bool get_in_air_status() {return _control_status.flags.in_air;}