diff --git a/EKF/control.cpp b/EKF/control.cpp index 3981d7a487..0d0ff6b0c9 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -898,7 +898,7 @@ void Ekf::checkVerticalAccelerationHealth() // Don't use stale innovation data. bool is_inertial_nav_falling = false; bool are_vertical_pos_and_vel_independant = false; - if (isRecent(_vert_pos_fuse_time_us, 1000000)) { + if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) { if (isRecent(_vert_vel_fuse_time_us, 1000000)) { // If vertical position and velocity come from independent sensors then we can // trust them more if they disagree with the IMU, but need to check that they agree @@ -919,7 +919,7 @@ void Ekf::checkVerticalAccelerationHealth() _imu_sample_delayed.delta_vel_clipping[1] || _imu_sample_delayed.delta_vel_clipping[2]; if (is_clipping &&_clip_counter < clip_count_limit) { - _clip_counter++; + _clip_counter++; } else if (_clip_counter > 0) { _clip_counter--; } diff --git a/EKF/ekf.h b/EKF/ekf.h index db4e692856..e06bee16de 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -392,8 +392,8 @@ private: Vector3f _last_vel_obs; ///< last velocity observation (m/s) Vector2f _last_fail_hvel_innov; ///< last failed horizontal velocity innovation (m/s)**2 - float _vert_pos_innov_ratio; ///< standard deviation of vertical position innovation - uint64_t _vert_pos_fuse_time_us; ///< last system time in usec vertical position measurement fuson was attempted + float _vert_pos_innov_ratio; ///< vertical position innovation divided by estimated standard deviation of innovation + uint64_t _vert_pos_fuse_attempt_time_us; ///< last system time in usec vertical position measurement fuson was attempted float _vert_vel_innov_ratio; ///< standard deviation of vertical velocity innovation uint64_t _vert_vel_fuse_time_us; ///< last system time in usec time vertical velocity measurement fuson was attempted diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 184fe1147d..594608e303 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -151,12 +151,12 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate innov_var(2) = P(9, 9) + obs_var(2); 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; + _vert_pos_fuse_attempt_time_us = _time_last_imu; 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); + float innovation; 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);