From b469a80633a123cc1ed5addcc595d9a82d196382 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Tue, 9 Jun 2020 20:03:42 +1000
Subject: [PATCH] AP_NavEKF3: Reset all co-variances when performing a yaw
 reset

---
 libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 2 ++
 1 file changed, 2 insertions(+)

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