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
This commit is contained in:
Daniel Agar
2023-01-14 09:15:46 -05:00
committed by GitHub
parent 22f7d913bd
commit 674a59e48f
5 changed files with 19 additions and 1 deletions
+2
View File
@@ -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)
+1 -1
View File
@@ -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
+1
View File
@@ -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),
+2
View File
@@ -598,6 +598,8 @@ private:
(ParamExtFloat<px4::params::EKF2_ABL_TAU>)
_param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec)
(ParamExtFloat<px4::params::EKF2_GYR_B_LIM>) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s)
// Multi-rotor drag specific force fusion
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
+13
View File
@@ -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
*