From 996000fb1cb1ab84c07c7a2e13b0dc5e969414c4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 15 Aug 2024 16:51:19 -0400 Subject: [PATCH] ekf2: always use correct accel/gyro for filtered metrics --- src/modules/ekf2/EKF/ekf_helper.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d88f7c400d92..61c648b6b2d5 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -870,17 +870,21 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector { + const Vector3f gyro_corrected = imu_delayed.delta_ang / imu_delayed.delta_ang_dt - _state.gyro_bias; + const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f); const float beta = 1.f - alpha; - _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, - beta * _ang_rate_magnitude_filt); + + _ang_rate_magnitude_filt = fmaxf(gyro_corrected.norm(), beta * _ang_rate_magnitude_filt); } { + const Vector3f accel_corrected = imu_delayed.delta_vel / imu_delayed.delta_vel_dt - _state.accel_bias; + const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f); 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_magnitude_filt = fmaxf(accel_corrected.norm(), beta * _accel_magnitude_filt); }