diff --git a/EKF/control.cpp b/EKF/control.cpp index 01b0c4a995..435bb28498 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -927,7 +927,7 @@ void Ekf::checkVerticalAccelerationHealth() // if vertical velocity and position are independent and agree, then do not require evidence of clipping if // innovations are large - const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || _clip_counter > clip_count_limit / 2) && + const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || _clip_counter > 0) && is_inertial_nav_falling; if (bad_vert_accel) { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 11ae26e54b..184fe1147d 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -81,13 +81,24 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); _vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2)); _vert_vel_fuse_time_us = _time_last_imu; - const bool innov_check_pass = (test_ratio(1) <= 1.0f); + bool innov_check_pass = (test_ratio(1) <= 1.0f); + + // if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + float innovation = innov(2); + if (_bad_vert_accel_detected && !innov_check_pass) { + const float innov_limit = innov_gate(1) * sqrtf(innov_var(2)); + innovation = math::constrain(innov(2), -innov_limit, innov_limit); + innov_check_pass = true; + } else { + innovation = innov(2); + } if (innov_check_pass) { _time_last_ver_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_vel = false; - fuseVelPosHeight(innov(2), innov_var(2), 2); + fuseVelPosHeight(innovation, innov_var(2), 2); return true; @@ -141,12 +152,23 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); _vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2)); _vert_pos_fuse_time_us = _time_last_imu; - const bool innov_check_pass = test_ratio(1) <= 1.0f; + bool innov_check_pass = test_ratio(1) <= 1.0f; + + // if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + float innovation = innov(2); + if (_bad_vert_accel_detected && !innov_check_pass) { + const float innov_limit = innov_gate(1) * sqrtf(innov_var(2)); + innovation = math::constrain(innov(2), -innov_limit, innov_limit); + innov_check_pass = true; + } else { + innovation = innov(2); + } if (innov_check_pass) { _time_last_hgt_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_pos = false; - fuseVelPosHeight(innov(2), innov_var(2), 5); + fuseVelPosHeight(innovation, innov_var(2), 5); return true;