From 6e992a18f68d512f785683d881caec84f2a49299 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 15 Aug 2020 08:43:15 +1000 Subject: [PATCH] EKF: optflow fusion cleanup Co-authored-by: kritz --- EKF/optflow_fusion.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index b585a33fcd..9205b19a0d 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -175,14 +175,12 @@ void Ekf::fuseOptFlow() // calculate innovation variance for X axis observation and protect against a badly conditioned calculation _flow_innov_var(0) = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS); - float HK50; - if (_flow_innov_var(0) > R_LOS) { - HK50 = HK4/_flow_innov_var(0); - } else { + if (_flow_innov_var(0) < R_LOS) { // we need to reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); return; } + const float HK50 = HK4/_flow_innov_var(0); const float HK51 = Tbs(0,1)*q1; const float HK52 = Tbs(0,2)*q0;