diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e88d3b483a..9df8fe1da2 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -898,17 +898,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); }