From 1a0b7dae9d43aa27f8616b899f95620f63555260 Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Mon, 9 Mar 2026 20:31:44 +0100 Subject: [PATCH] fix(ekf2): remove heading corrections from gravity fusion (#26694) --- src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index 3a5e5447bb..ffa1fe31fd 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -110,6 +110,9 @@ void Ekf::controlGravityFusion(const imuSample &imu) const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { + // Prevent heading corrections through gravity fusion due to correlations between tilt and heading in P + K(State::quat_nominal.idx + 2) = 0.f; + fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]); }