mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-17 03:40:35 +08:00
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:
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user