From b38ab873ae50ecb46cf2a5101fe944d291e3db3e Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Mon, 6 Apr 2026 14:13:23 -0500 Subject: [PATCH] AP_NavEKF3: FuseRngBcn: make fusion update more consistent Update `stateStruct` before calling `ConstrainVariances` as the latter can change `stateStruct` in rare cases. --- libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 8973ef17a78..19217deb554 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -278,16 +278,16 @@ void NavEKF3_core::FuseRngBcn() } } - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. - ForceSymmetry(); - ConstrainVariances(); - // correct the state vector for (uint8_t j= 0; j<=stateIndexLim; j++) { statesArray[j] = statesArray[j] - Kfusion[j] * rngBcn.innov; } stateStruct.quat.normalize(); + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. + ForceSymmetry(); + ConstrainVariances(); + // record healthy fusion faultStatus.bad_rngbcn = false;