From a6007e4b93b2a9561940e9bb31e8a4ae2cfc6fbc Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 22 May 2024 10:17:42 +0200 Subject: [PATCH] ekf2-mag: turn around update_all_states condition Non-functional --- .../EKF/aid_sources/magnetometer/mag_fusion.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) 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 ad03b85331..ddb0dd1fb3 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -103,7 +103,13 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima VectorState Kfusion = P * H / aid_src.innovation_variance[index]; - if (!update_all_states) { + if (update_all_states) { + if (!update_tilt) { + Kfusion(State::quat_nominal.idx + 0) = 0.f; + Kfusion(State::quat_nominal.idx + 1) = 0.f; + } + + } else { // zero non-mag Kalman gains if not updating all states // copy mag_I and mag_B Kalman gains @@ -116,11 +122,6 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima Kfusion.slice(State::mag_B.idx, 0) = K_mag_B; } - if (!update_tilt) { - Kfusion(State::quat_nominal.idx + 0) = 0.f; - Kfusion(State::quat_nominal.idx + 1) = 0.f; - } - if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) { fused[index] = true; limitDeclination();