From 804c3563fb9355db2c7fb9019ab21ada1888b7ad Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 16 Jun 2021 12:06:55 +0200 Subject: [PATCH] heading_fusion: fix yaw gyro bias oscillation on ground When the mag is pulled away before takeoff, the EKF can end up learning a bad yaw gyro bias. This bad gyro bias makes the estimated heading spin and even if the bias continues to be learned, it can end up being stuck in limit cycle due to the innovation changing its sign every time the heading estimate overshoots the measurement. To prevent this, the Z gyro bias variance is reset when the mag heading fusion test ratio fails on ground. --- EKF/covariance.cpp | 7 +++++++ EKF/ekf.h | 2 ++ EKF/mag_fusion.cpp | 4 ++++ 3 files changed, 13 insertions(+) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 94bafced3e..1408019240 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -1084,6 +1084,13 @@ void Ekf::zeroMagCov() P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); } +void Ekf::resetZDeltaAngBiasCov() +{ + const float init_delta_ang_bias_var = sq(_params.switch_on_gyro_bias * _dt_ekf_avg); + + P.uncorrelateCovarianceSetVariance<1>(12, init_delta_ang_bias_var); +} + void Ekf::resetWindCovariance() { if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { diff --git a/EKF/ekf.h b/EKF/ekf.h index af858cf585..58054d69a5 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -926,6 +926,8 @@ private: void clearMagCov(); void zeroMagCov(); + void resetZDeltaAngBiasCov(); + // uncorrelate quaternion states from other states void uncorrelateQuatFromOtherStates(); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 9d3a55aa2e..5a799f8119 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -653,6 +653,10 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var)); _heading_innov = math::constrain(innovation, -gate_limit, gate_limit); + // also reset the yaw gyro variance to converge faster and avoid + // being stuck on a previous bad estimate + resetZDeltaAngBiasCov(); + } else { return; }