From 5964a0d113fda31e62593051422cb40329bebee0 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 1 Feb 2026 14:18:28 -0600 Subject: [PATCH] AP_NavEKF3: use FinishFusion in FuseOptFlow --- .../AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 30 ++----------------- 1 file changed, 3 insertions(+), 27 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 6e0d082f75c..b1fbd4f1f2f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -692,33 +692,9 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus } } - // Check that we are not going to drive any variances negative and skip the update if so - bool healthyFusion = true; - for (uint8_t i= 0; i<=stateIndexLim; i++) { - if (KHP[i][i] > P[i][i]) { - healthyFusion = false; - } - } - - if (healthyFusion) { - // update the covariance matrix - for (uint8_t i= 0; i<=stateIndexLim; i++) { - for (uint8_t j= 0; j<=stateIndexLim; j++) { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - - // correct the state vector - for (uint8_t j= 0; j<=stateIndexLim; j++) { - statesArray[j] = statesArray[j] - Kfusion[j] * flowInnov[obsIndex]; - } - stateStruct.quat.normalize(); - - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. - ForceSymmetry(); - ConstrainVariances(); - } else { - // record bad axis + // finish fusion from KHP and Kfusion + if (FinishFusion(flowInnov[obsIndex])) { + // fault, record bad axis if (obsIndex == 0) { faultStatus.bad_xflow = true; } else if (obsIndex == 1) {