diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index cffb5470e9..fd287aa6d0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -1548,6 +1548,8 @@ void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance) // update the yaw angle variance using the variance of the EKF-GSF estimate angleErrVarVec.z = yawVariance; + zeroRows(P,0,3); + zeroCols(P,0,3); initialiseQuatCovariances(angleErrVarVec); // record the yaw reset event