ekf2: new EKF2_IMU_CTRL parameter and gyro bias inhibit mechanism

- EKF2_AID_MASK accel bias inhibit moves to EKF2_IMU_CTRL

src/modules/ekf2/EKF/common.h

@@ -126,6 +126,11 @@ enum class PositionSensor : uint8_t {
 	EV      = 2,
 };
 
+enum class ImuCtrl : uint8_t {
+	GyroBias  = (1<<0),
+	AccelBias = (1<<1),
+};
+
 enum GnssCtrl : uint8_t {
 	HPOS  = (1<<0),
 	VPOS  = (1<<1),
@@ -150,7 +155,7 @@ enum SensorFusionMask : uint16_t {
 	// Bit locations for fusion_mode
 	DEPRECATED_USE_GPS = (1<<0),    ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
 	USE_OPT_FLOW     = (1<<1),      ///< set to true to use optical flow data
-	INHIBIT_ACC_BIAS = (1<<2),      ///< set to true to inhibit estimation of accelerometer delta velocity bias
+	DEPRECATED_INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias
 	DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
 	DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
 	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
@@ -279,6 +284,8 @@ struct parameters {
 
 	int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
 
+	int32_t imu_ctrl{static_cast<int32_t>(ImuCtrl::GyroBias) | static_cast<int32_t>(ImuCtrl::AccelBias)};
+
 	// measurement source control
 	int32_t fusion_mode{};         ///< bitmasked integer that selects some aiding sources
 	int32_t height_sensor_ref{HeightSensor::BARO};

src/modules/ekf2/EKF/covariance.cpp

@@ -81,9 +81,9 @@ void Ekf::initialiseCovariance()
 	}
 
 	// gyro bias
-	P(10,10) = sq(_params.switch_on_gyro_bias * dt);
+	_prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
-	P(11,11) = P(10,10);
+	_prev_delta_ang_bias_var(1) = P(11,11) = P(10,10);
-	P(12,12) = P(10,10);
+	_prev_delta_ang_bias_var(2) = P(12,12) = P(10,10);
 
 	// accel bias
 	_prev_dvel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias * dt);
@@ -121,7 +121,36 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
 					     || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
 
-	const bool do_inhibit_all_axes = (_params.fusion_mode & SensorFusionMask::INHIBIT_ACC_BIAS)
+	// gyro bias inhibit
+	const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
+
+	for (unsigned stateIndex = 10; stateIndex <= 12; stateIndex++) {
+		const unsigned index = stateIndex - 10;
+
+		bool is_bias_observable = true;
+
+		// TODO: gyro bias conditions
+
+		const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable;
+
+		if (do_inhibit_axis) {
+			// store the bias state variances to be reinstated later
+			if (!_gyro_bias_inhibit[index]) {
+				_prev_delta_ang_bias_var(index) = P(stateIndex, stateIndex);
+				_gyro_bias_inhibit[index] = true;
+			}
+
+		} else {
+			if (_gyro_bias_inhibit[index]) {
+				// reinstate the bias state variances
+				P(stateIndex, stateIndex) = _prev_delta_ang_bias_var(index);
+				_gyro_bias_inhibit[index] = false;
+			}
+		}
+	}
+
+	// accel bias inhibit
+	const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
 					 || is_manoeuvre_level_high
 					 || _fault_status.flags.bad_acc_vertical;
 
@@ -141,7 +170,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 			is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
 		}
 
-		const bool do_inhibit_axis = do_inhibit_all_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
+		const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
 
 		if (do_inhibit_axis) {
 			// store the bias state variances to be reinstated later
@@ -239,7 +268,17 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	// the variances, therefore use algorithm to minimise numerical error
 	for (unsigned i = 10; i <= 12; i++) {
 		const int index = i - 10;
-		nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
+
+		if (!_gyro_bias_inhibit[index]) {
+			// add process noise that is not from the IMU
+			// process noise contribution for delta velocity states can be very small compared to
+			// the variances, therefore use algorithm to minimise numerical error
+			nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
+
+		} else {
+			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_delta_ang_bias_var(index));
+			_delta_angle_bias_var_accum(index) = 0.f;
+		}
 	}
 
 	for (int i = 13; i <= 15; i++) {

src/modules/ekf2/EKF/ekf.h

@@ -329,6 +329,7 @@ public:
 	float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
 
 	bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
+	bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; }
 
 	const auto &state_reset_status() const { return _state_reset_status; }
 
@@ -531,8 +532,8 @@ private:
 
 	SquareMatrix24f P{};	///< state covariance matrix
 
-	Vector3f _delta_vel_bias_var_accum{};		///< kahan summation algorithm accumulator for delta velocity bias variance
 	Vector3f _delta_angle_bias_var_accum{};	///< kahan summation algorithm accumulator for delta angle bias variance
+	Vector3f _delta_vel_bias_var_accum{};   ///< kahan summation algorithm accumulator for delta velocity bias variance
 
 	Vector2f _drag_innov{};		///< multirotor drag measurement innovation (m/sec**2)
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
@@ -614,9 +615,11 @@ private:
 
 	// variables used to inhibit accel bias learning
 	bool _accel_bias_inhibit[3] {};		///< true when the accel bias learning is being inhibited for the specified axis
+	bool _gyro_bias_inhibit[3] {};		///< true when the gyro bias learning is being inhibited for the specified axis
 	Vector3f _accel_vec_filt{};		///< acceleration vector after application of a low pass filter (m/sec**2)
 	float _accel_magnitude_filt{0.0f};	///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
 	float _ang_rate_magnitude_filt{0.0f};		///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
+	Vector3f _prev_delta_ang_bias_var{};	///< saved delta angle XYZ bias variances (rad/sec)
 	Vector3f _prev_dvel_bias_var{};		///< saved delta velocity XYZ bias variances (m/sec)**2
 
 	// Terrain height state estimation
@@ -765,6 +768,12 @@ private:
 	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
 	{
 		for (unsigned i = 0; i < 3; i++) {
+			// gyro bias: states 10, 11, 12
+			if (_gyro_bias_inhibit[i]) {
+				K(10 + i) = 0.0f;
+			}
+
+			// accel bias: states 13, 14, 15
 			if (_accel_bias_inhibit[i]) {
 				K(13 + i) = 0.0f;
 			}

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -201,6 +201,12 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 	}
 
 	for (unsigned i = 0; i < 3; i++) {
+		// gyro bias: states 10, 11, 12
+		if (_gyro_bias_inhibit[i]) {
+			Kfusion(10 + i) = 0.0f;
+		}
+
+		// accel bias: states 13, 14, 15
 		if (_accel_bias_inhibit[i]) {
 			Kfusion(13 + i) = 0.0f;
 		}

src/modules/ekf2/EKF2.cpp

@@ -59,6 +59,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
 	_params(_ekf.getParamHandle()),
 	_param_ekf2_predict_us(_params->filter_update_interval_us),
+	_param_ekf2_imu_ctrl(_params->imu_ctrl),
 	_param_ekf2_mag_delay(_params->mag_delay_ms),
 	_param_ekf2_baro_delay(_params->baro_delay_ms),
 	_param_ekf2_gps_delay(_params->gps_delay_ms),
@@ -744,6 +745,27 @@ void EKF2::VerifyParams()
 		events::send<float>(events::ID("ekf2_aid_mask_ev"), events::Log::Warning,
 				    "Use EKF2_EV_CTRL instead", _param_ekf2_aid_mask.get());
 	}
+
+
+	// IMU EKF2_AID_MASK -> EKF2_IMU_CTRL (2023-01-31)
+	if (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_INHIBIT_ACC_BIAS) {
+
+		// EKF2_IMU_CTRL set disable accel bias bit
+		_param_ekf2_imu_ctrl.set(_param_ekf2_imu_ctrl.get() & ~(static_cast<int32_t>(ImuCtrl::AccelBias)));
+
+		// EKF2_AID_MASK clear inhibit accel bias bit
+		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_INHIBIT_ACC_BIAS));
+
+		_param_ekf2_imu_ctrl.commit();
+		_param_ekf2_aid_mask.commit();
+
+		mavlink_log_critical(&_mavlink_log_pub, "EKF2 IMU accel bias inhibit use EKF2_IMU_CTRL instead of EKF2_AID_MASK\n");
+		/* EVENT
+		 * @description <param>EKF2_AID_MASK</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_aid_mask_imu"), events::Log::Warning,
+				    "Use EKF2_IMU_CTRL instead", _param_ekf2_aid_mask.get());
+	}
 }
 
 void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
@@ -1320,7 +1342,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 		bias.timestamp_sample = _ekf.time_delayed_us();
 
 		// take device ids from sensor_selection_s if not using specific vehicle_imu_s
-		if (_device_id_gyro != 0) {
+		if ((_device_id_gyro != 0) && (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::GyroBias))) {
 			bias.gyro_device_id = _device_id_gyro;
 			gyro_bias.copyTo(bias.gyro_bias);
 			bias.gyro_bias_limit = _ekf.getGyroBiasLimit();
@@ -1330,7 +1352,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			_last_gyro_bias_published = gyro_bias;
 		}
 
-		if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & SensorFusionMask::INHIBIT_ACC_BIAS)) {
+		if ((_device_id_accel != 0) && (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::AccelBias))) {
 			bias.accel_device_id = _device_id_accel;
 			accel_bias.copyTo(bias.accel_bias);
 			bias.accel_bias_limit = _ekf.getAccelBiasLimit();
@@ -2212,7 +2234,7 @@ void EKF2::UpdateCalibration(const hrt_abstime &timestamp, InFlightCalibration &
 void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 {
 	// the EKF is operating in the correct mode and there are no filter faults
-	const bool bias_valid = !(_param_ekf2_aid_mask.get() & SensorFusionMask::INHIBIT_ACC_BIAS)
+	const bool bias_valid = (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::AccelBias))
 				&& _ekf.control_status_flags().tilt_align
 				&& (_ekf.fault_status().value == 0)
 				&& !_ekf.fault_status_flags().bad_acc_bias
@@ -2229,10 +2251,11 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 {
 	// the EKF is operating in the correct mode and there are no filter faults
-	const bool bias_valid = _ekf.control_status_flags().tilt_align
+	const bool bias_valid = (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::GyroBias))
+				&& _ekf.control_status_flags().tilt_align
 				&& (_ekf.fault_status().value == 0);
 
-	const bool learning_valid = bias_valid; // TODO
+	const bool learning_valid = bias_valid && !_ekf.gyro_bias_inhibited();
 
 	UpdateCalibration(timestamp, _gyro_cal, _ekf.getGyroBias(), _ekf.getGyroBiasVariance(), _ekf.getGyroBiasLimit(),
 			  bias_valid, learning_valid);

src/modules/ekf2/EKF2.hpp

@@ -397,6 +397,8 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
+		(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
+
 		(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
 		_param_ekf2_mag_delay,	///< magnetometer measurement delay relative to the IMU (mSec)
 		(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)

src/modules/ekf2/ekf2_params.c

@@ -52,6 +52,17 @@
  */
 PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000);
 
+/**
+ * IMU control
+ *
+ * @group EKF2
+ * @min 0
+ * @max 3
+ * @bit 0 Gyro Bias
+ * @bit 1 Accel Bias
+ */
+PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 3);
+
 /**
  * Magnetometer measurement delay relative to IMU measurements
  *
@@ -602,7 +613,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * Set bits in the following positions to enable:
  * 0 : Deprecated, use EKF2_GPS_CTRL instead
  * 1 : Set to true to use optical flow data if available
- * 2 : Set to true to inhibit IMU delta velocity bias estimation
+ * 2 : Deprecated, use EKF2_IMU_CTRL instead
  * 3 : Deprecated, use EKF2_EV_CTRL instead
  * 4 : Deprecated, use EKF2_EV_CTRL instead
  * 5 : Set to true to enable multi-rotor drag specific force fusion
@@ -615,7 +626,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * @max 511
  * @bit 0 unused
  * @bit 1 use optical flow
- * @bit 2 inhibit IMU bias estimation
+ * @bit 2 unused
  * @bit 3 unused
  * @bit 4 unused
  * @bit 5 multi-rotor drag fusion