From beb7e7d79b7d0dd15b7726855067e6fa612bbc70 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 23 Jan 2025 15:21:41 +0100 Subject: [PATCH] ekf2-gravity: start based on accel LPF instead of peak hold This prevent rapid switching in presence of noise and the innovation filter is good at rejecting spikes --- src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 2326c43557f9..8442e73d3be4 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -54,8 +54,9 @@ void Ekf::controlGravityFusion(const imuSample &imu) const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f; const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f; - const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) - && (_accel_magnitude_filt < upper_accel_limit); + const float accel_lpf_norm_sq = _accel_lpf.getState().norm_squared(); + const bool accel_lpf_norm_good = (accel_lpf_norm_sq > sq(lower_accel_limit)) + && (accel_lpf_norm_sq < sq(upper_accel_limit)); // fuse gravity observation if our overall acceleration isn't too big _control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast(ImuCtrl::GravityVector))