From b1b6810382ca3b47d8a46ee2a76d566abdbf995b 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 0d8fac2fb5a6..e5954e59b507 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1013,8 +1013,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 }