From 674a59e48ff2dcdc325c395325aaff59e5967496 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 14 Jan 2023 09:15:46 -0500 Subject: [PATCH] ekf2: new gyro bias limit parameter EKF2_GYR_B_LIM - default is now a more conservative 0.15 rad/s (~8.6 deg/s) instead of the previous hardcoded 20 deg/s --- src/modules/ekf2/EKF/common.h | 2 ++ src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 2 ++ src/modules/ekf2/ekf2_params.c | 13 +++++++++++++ 5 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 2451126274..6be3b11113 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -434,6 +434,8 @@ struct parameters { float acc_bias_learn_gyr_lim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec) float acc_bias_learn_tc{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) + float gyro_bias_lim{0.4f}; ///< maximum gyro bias magnitude (rad/sec) + const unsigned reset_timeout_max{7'000'000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec) const unsigned no_aid_timeout_max{1'000'000}; ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec) const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index afdd54a98a..e0b7a55936 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -306,7 +306,7 @@ public: // gyro bias (states 10, 11, 12) Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s - float getGyroBiasLimit() const { return math::radians(20.f); } // 20 degrees/s + float getGyroBiasLimit() const { return _params.gyro_bias_lim; } // accel bias (states 13, 14, 15) Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 4fdaae092a..adf96aadeb 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -160,6 +160,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim), _param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim), _param_ekf2_abl_tau(_params->acc_bias_learn_tc), + _param_ekf2_gyr_b_lim(_params->gyro_bias_lim), _param_ekf2_drag_noise(_params->drag_noise), _param_ekf2_bcoef_x(_params->bcoef_x), _param_ekf2_bcoef_y(_params->bcoef_y), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index b4db8e8f40..e39ddd53b8 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -598,6 +598,8 @@ private: (ParamExtFloat) _param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec) + (ParamExtFloat) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s) + // Multi-rotor drag specific force fusion (ParamExtFloat) _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index f04012c2d9..9783b9c755 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1414,6 +1414,19 @@ PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f); */ PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f); +/** + * Gyro bias learning limit. + * + * The ekf delta angle bias states will be limited to within a range equivalent to +- of this value. + * + * @group EKF2 + * @min 0.0 + * @max 0.4 + * @unit rad/s + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_GYR_B_LIM, 0.15f); + /** * Required GPS health time on startup *