From 05855b7fc15ae76c4ee5efb5c30775f1c6235fa7 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Fri, 3 Jul 2020 17:09:10 +0200 Subject: [PATCH] Update time of last fusion event really only if fused Except for the velpos fusion --- EKF/airspeed_fusion.cpp | 4 +--- EKF/gps_yaw_fusion.cpp | 2 +- EKF/sideslip_fusion.cpp | 5 ++--- 3 files changed, 4 insertions(+), 7 deletions(-) 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; } } }