From 7fa9a1963bd5f42f6136970e0aab5feff099181b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 15 Aug 2020 08:44:02 +1000 Subject: [PATCH] EKF: optflow fusion cleanup Co-authored-by: kritz --- EKF/optflow_fusion.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 7a2e8a5b62..8ea92687af 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -229,14 +229,13 @@ void Ekf::fuseOptFlow() // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation _flow_innov_var(1) = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS); - float HK95; - if (_flow_innov_var(1) >= R_LOS) { - HK95 = HK4/_flow_innov_var(1); - } else { + if (_flow_innov_var(1) < R_LOS) { // we need to reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); return; } + const float HK95 = HK4/_flow_innov_var(1); + // run the innovation consistency check and record result bool flow_fail = false;