AP_NavEKF3: Reset all co-variances when performing a yaw reset

2020-06-09 20:03:42 +10:00 · 2020-06-09 20:03:42 +10:00 · b469a80633
commit b469a80633
parent d8c2096ad8
1 changed files with 2 additions and 0 deletions
--- 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