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;