AP_NavEKF2: attitude co-variance reset for all post alignment yaw resets

Whenever the yaw angle is changed, the correlation with other state errors will be incorrect and the terms should be zeroed.

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -86,6 +86,8 @@ void NavEKF2_core::realignYawGPS()
             stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
             yawAlignComplete = true;
 
+            // zero the attitude covariances becasue the corelations will now be invalid
+            zeroAttCovOnly();
         }
 
         // Check the yaw angles for consistency
@@ -100,13 +102,8 @@ void NavEKF2_core::realignYawGPS()
             // calculate new filter quaternion states from Euler angles
             stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
 
-            // The correlations between attitude errors and position and velocity errors in the covariance matrix
-            // are invalid because of the changed yaw angle, so reset the corresponding row and columns
-            zeroCols(P,0,2);
-            zeroRows(P,0,2);
-
-            // Set the initial attitude error covariances
-            P[2][2] = P[1][1] = P[0][0] = sq(radians(5.0f));
+            // zero the attitude covariances becasue the corelations will now be invalid
+            zeroAttCovOnly();
 
             // reset tposition fusion timer to cause the states to be reset to the GPS on the next GPS fusion cycle
             lastPosPassTime_ms = 0;

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -1333,6 +1333,8 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
             lastYawReset_ms = imuSampleTime_ms;
             // calculate an initial quaternion using the new yaw value
             initQuat.from_euler(roll, pitch, yaw);
+            // zero the attitude covariances becasue the corelations will now be invalid
+            zeroAttCovOnly();
         } else {
             initQuat = stateStruct.quat;
         }