From 6f9a37824732a676bf082628ea5ac4b243b44c64 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 5 Mar 2024 16:23:38 +0100 Subject: [PATCH] yaw_est: force set gyro bias when at rest The gyro bias estimate from EFK2 is really good when at rest and should be used by the yaw estimator to prevent heading drifts due to poor heading observability. --- src/modules/ekf2/EKF/EKFGSF_yaw.h | 4 ++-- src/modules/ekf2/EKF/gps_control.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/EKFGSF_yaw.h index 3719aec7ce..7add1dedef 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.h @@ -67,11 +67,11 @@ public: void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; } - void setGyroBias(const Vector3f &imu_gyro_bias) + void setGyroBias(const Vector3f &imu_gyro_bias, const bool force = false) { // Initialise to gyro bias estimate from main filter because there could be a large // uncorrected rate gyro bias error about the gravity vector - if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started) { + if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started || force) { // init gyro bias for each model for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { _ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias; diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index d86020815a..cf2f9eb7b4 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -47,7 +47,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } if (!gyro_bias_inhibited()) { - _yawEstimator.setGyroBias(getGyroBias()); + _yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest); } // run EKF-GSF yaw estimator once per imu_delayed update