diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 676dd32e09..d820b19fec 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -159,9 +159,6 @@ void Ekf::fuseAirspeed() _innov_check_fail_status.flags.reject_airspeed = false; } - // Airspeed measurement sample has passed check so record it - _time_last_arsp_fuse = _time_last_imu; - // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP @@ -211,6 +208,7 @@ void Ekf::fuseAirspeed() // apply the state corrections fuse(Kfusion, _airspeed_innov); + _time_last_arsp_fuse = _time_last_imu; } } } diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index f03b38e2a4..252613fe2d 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -250,7 +250,6 @@ void Ekf::fuseGpsYaw() } if (healthy) { - _time_last_gps_yaw_fuse = _time_last_imu; // apply the covariance corrections P -= KHP; @@ -259,6 +258,7 @@ void Ekf::fuseGpsYaw() // apply the state corrections fuse(Kfusion, _heading_innov); + _time_last_gps_yaw_fuse = _time_last_imu; } } diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 752afa9622..9549bed688 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -192,9 +192,6 @@ void Ekf::fuseSideslip() _innov_check_fail_status.flags.reject_sideslip = false; } - // synthetic sideslip measurement sample has passed check so record it - _time_last_beta_fuse = _time_last_imu; - // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP @@ -249,6 +246,8 @@ void Ekf::fuseSideslip() // apply the state corrections fuse(Kfusion, _beta_innov); + + _time_last_beta_fuse = _time_last_imu; } } }