diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index 8a09b72f79..0017835d9e 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -82,7 +82,9 @@ void Ekf::controlGravityFusion(const imuSample &imu) _aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector; - if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) { + const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; + + if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected && !accel_clipping) { // perform fusion for each axis _aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0)) && measurementUpdate(Ky, innovation_variance(1), innovation(1))