AP_NavEKF: Add function to zero attitude state co-variances

When changing the vehicle yaw angle, the correlation between the attitude errors and errors in other states is invalid so the corresponding co-variance terms need to be zeroed.
This needs to be done in more than one place.

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -1368,4 +1368,18 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
     return initQuat;
 }
 
+// zero the attitude covariances, but preserve the variances
+void NavEKF2_core::zeroAttCovOnly()
+{
+    float varTemp[3];
+    for (uint8_t index=0; index<=2; index++) {
+        varTemp[index] = P[index][index];
+    }
+    zeroCols(P,0,2);
+    zeroRows(P,0,2);
+    for (uint8_t index=0; index<=2; index++) {
+        P[index][index] = varTemp[index];
+    }
+}
+
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -608,6 +608,9 @@ private:
     // Select height data to be fused from the available baro, range finder and GPS sources
     void selectHeightForFusion();
 
+    // zero attitude state covariances, but preserve variances
+    void zeroAttCovOnly();
+
     // Length of FIFO buffers used for non-IMU sensor data.
     // Must be larger than the time period defined by IMU_BUFFER_LENGTH
     static const uint32_t OBS_BUFFER_LENGTH = 5;