From 64ab2b087a16cb457fccc6e7ce37906e8648f379 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 15 Aug 2020 22:21:10 +1000 Subject: [PATCH] EKF: Only record successful sideslip fusion when states updated --- EKF/sideslip_fusion.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index e38f9a29e5..254c304a96 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -69,7 +69,7 @@ void Ekf::fuseSideslip() // perform fusion of assumed sideslip = 0 if (rel_wind_body.norm() > 7.0f) { - float R_BETA = sq(_params.beta_noise); // observation noise variance + const float R_BETA = sq(_params.beta_noise); // observation noise variance // determine if we need the sideslip fusion to correct states other than wind bool update_wind_only = !_is_wind_dead_reckoning; @@ -203,9 +203,6 @@ void Ekf::fuseSideslip() Kfusion(22) = HK45*HK52; Kfusion(23) = HK42*HK52; - // 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