diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 949f4ef042..ebd249b679 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/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(ImuCtrl::GyroBias) | static_cast(ImuCtrl::AccelBias)}; + // measurement source control int32_t fusion_mode{}; ///< bitmasked integer that selects some aiding sources int32_t height_sensor_ref{HeightSensor::BARO}; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 7a8edf0c25..234a1e2991 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/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); - P(11,11) = P(10,10); - P(12,12) = P(10,10); + _prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt); + _prev_delta_ang_bias_var(1) = P(11,11) = 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(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(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++) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e79eea97e6..bc15b76216 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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; } diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index f252579f32..f44c91650f 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/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; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7e90b56d93..7d9cb1f86e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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(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(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 EKF2_AID_MASK is set to {1:.0}. + */ + events::send(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 ×tamp) @@ -1320,7 +1342,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) 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(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 ×tamp) _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(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 ×tamp, InFlightCalibration & void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) { // 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(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 ×tamp) void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) { // 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(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); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 5dc81c51f0..731999ba55 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -397,6 +397,8 @@ private: DEFINE_PARAMETERS( (ParamExtInt) _param_ekf2_predict_us, + (ParamExtInt) _param_ekf2_imu_ctrl, + (ParamExtFloat) _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) (ParamExtFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 705871ce98..271fb907d8 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/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