From c28972d15e83004072e8d266aaf700c7706aa079 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 18 Jan 2024 15:56:01 +0100 Subject: [PATCH] ekf2-grav: only use filtered accel norm to start/stop the fusion Using the raw data makes the swith too sensitive to noise --- src/modules/ekf2/EKF/ekf.h | 1 - src/modules/ekf2/EKF/ekf_helper.cpp | 1 - src/modules/ekf2/EKF/gravity_fusion.cpp | 7 ++----- 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2e133059c9..f68d4e4034 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -727,7 +727,6 @@ private: // variables used to inhibit accel bias learning bool _accel_bias_inhibit[3] {}; ///< true when the accel bias learning is being inhibited for the specified axis bool _gyro_bias_inhibit[3] {}; ///< true when the gyro bias learning is being inhibited for the specified axis - Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2) float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec) float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index a4ca66b010..9546a6bf28 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -987,7 +987,6 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) const float beta = 1.f - alpha; _accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt); - _accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt; } diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index fdc947b871..2c30d9be7c 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -50,16 +50,13 @@ void Ekf::controlGravityFusion(const imuSample &imu) const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - _state.accel_bias; const float measurement_var = math::max(sq(_params.gravity_noise), sq(0.01f)); - const float accel_lpf_norm_sq = _accel_vec_filt.norm_squared(); - const float accel_norm_sq = measurement.norm_squared(); 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_lpf_norm_sq > sq(lower_accel_limit)) && (accel_lpf_norm_sq < sq(upper_accel_limit)); - const bool accel_norm_good = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit)); + const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) && (_accel_magnitude_filt < 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)) - && ((accel_lpf_norm_good && accel_norm_good) || _control_status.flags.vehicle_at_rest) + && (accel_lpf_norm_good || _control_status.flags.vehicle_at_rest) && !isHorizontalAidingActive(); // calculate kalman gains and innovation variances