EKF: add mag bias reset helper and update IMU bias reset to match (#924)

EKF/covariance.cpp

@@ -92,9 +92,6 @@ 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;
-
 	resetMagCov();
 
 	// wind

EKF/ekf.h

@@ -165,12 +165,13 @@ public:
 	// get the vehicle control limits required by the estimator to keep within sensor limitations
 	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;
 
-	/*
-	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();
+	// Reset all IMU bias states and covariances to initial alignment values.
+	void resetImuBias();
+	void resetGyroBias();
+	void resetAccelBias();
+
+	// Reset all magnetometer bias states and covariances to initial alignment values.
+	void resetMagBias();
 
 	Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };
 
@@ -208,11 +209,13 @@ public:
 	// get the terrain variance
 	float get_terrain_var() const { return _terrain_var; }
 
-	// get the accelerometer bias in m/s**2
-	Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; }
+	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
+	Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2
+	const Vector3f& getMagBias() const { return _state.mag_B; }
 
-	// get the gyroscope bias in rad/s
-	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; }
+	Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / _dt_ekf_avg; } // get the gyroscope bias variance in rad/s
+	Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / _dt_ekf_avg; } // get the accelerometer bias variance in m/s**2
+	Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; }
 
 	// get GPS check status
 	void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; }
@@ -354,8 +357,6 @@ 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 prediction

EKF/ekf_helper.cpp

@@ -845,26 +845,49 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 	}
 }
 
-bool Ekf::reset_imu_bias()
+void Ekf::resetImuBias()
 {
-	if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < (uint64_t)10e6) {
-		return false;
-	}
+	resetGyroBias();
+	resetAccelBias();
+}
 
+void Ekf::resetGyroBias()
+{
 	// Zero the delta angle and delta velocity bias states
 	_state.delta_ang_bias.zero();
-	_state.delta_vel_bias.zero();
 
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
 	P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
+}
+
+void Ekf::resetAccelBias()
+{
+	// Zero the delta angle and delta velocity bias states
+	_state.delta_vel_bias.zero();
+
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
 	P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias * FILTER_UPDATE_PERIOD_S));
-	_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;
 
 	// Set previous frame values
 	_prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag();
+}
 
-	return true;
+void Ekf::resetMagBias()
+{
+	// Zero the magnetometer bias states
+	_state.mag_B.zero();
+
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
+
+	// reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion
+	// _saved_mag_bf_variance[0] is the the D earth axis
+	_saved_mag_bf_variance[1] = 0;
+	_saved_mag_bf_variance[2] = 0;
+	_saved_mag_bf_variance[3] = 0;
 }
 
 // get EKF innovation consistency check status information comprising of: