From c59101e8ed3646e3755ddba1f87e8d199807a62d Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 23 Jan 2025 15:23:03 +0100 Subject: [PATCH] ekf2-gravity: do not estimate accel bias when gravity fusion is active Gravity fusion uses the bias corrected accelerometer data to correct the tilt estimate. We should not continue to estimate the accel bias when this is active as it creates an unwanted feedback loop. --- src/modules/ekf2/EKF/ekf_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index c68423f251..cee2fd05af 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1017,8 +1017,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) } else if (_control_status.flags.fake_hgt) { is_bias_observable = false; - } else if (_control_status.flags.fake_pos) { - // when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector + } else if (_control_status.flags.fake_pos || _control_status.flags.gravity_vector) { + // only consider an accel bias observable if aligned with the gravity vector is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966 }