From e7250bc8d5d347b4c93cec33e16c62ac7891e918 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 8 Apr 2025 17:07:21 +0200 Subject: [PATCH] ekf2: do not let mag heading and declination update xy gyro biases This can destabilize the tilt estimate when the mag field is disturbed --- .../ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp index 88fa425939..557c1d8a0d 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -108,6 +108,10 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima if (!update_tilt) { Kfusion(State::quat_nominal.idx + 0) = 0.f; Kfusion(State::quat_nominal.idx + 1) = 0.f; + + // Also avoid corruption of the XY gyro biases + Kfusion(State::gyro_bias.idx + 0) = 0.f; + Kfusion(State::gyro_bias.idx + 1) = 0.f; } } else { @@ -164,6 +168,10 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_s if (!update_tilt) { Kfusion(State::quat_nominal.idx + 0) = 0.f; Kfusion(State::quat_nominal.idx + 1) = 0.f; + + // Also avoid corruption of the XY gyro biases + Kfusion(State::gyro_bias.idx + 0) = 0.f; + Kfusion(State::gyro_bias.idx + 1) = 0.f; } } else {