AP_NavEKF2: fix bug in initialisation of declination co-variances

Co-variances were being re-zeroed after being set. This meant that the initial declination learning was sensitive to measurement errors which could result in poor initial yaw accuracy.

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -121,9 +121,14 @@ void NavEKF2_core::setWindMagStateLearningMode()
             P[21][21] = bodyMagFieldVar.z;
         } else {
             // set the variances equal to the observation variances
-            for (uint8_t index=16; index<=21; index++) {
+            for (uint8_t index=18; index<=21; index++) {
                 P[index][index] = sq(frontend->_magNoise);
             }
+
+            // set the NE earth magnetic field states using the published declination
+            // and set the corresponding variances and covariances
+            alignMagStateDeclination();
+
         }
         // request a reset of the yaw and magnetic field states if not done before
         if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -1098,16 +1098,20 @@ void NavEKF2_core::alignMagStateDeclination()
     stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng);
     stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng);
 
-    // zero the corresponding state covariances
-    float var_16 = P[16][16];
-    float var_17 = P[17][17];
-    zeroRows(P,16,17);
-    zeroCols(P,16,17);
-    P[16][16] = var_16;
-    P[17][17] = var_17;
+    if (!inhibitMagStates) {
+        // zero the corresponding state covariances if magnetic field state learning is active
+        float var_16 = P[16][16];
+        float var_17 = P[17][17];
+        zeroRows(P,16,17);
+        zeroCols(P,16,17);
+        P[16][16] = var_16;
+        P[17][17] = var_17;
 
-    // fuse the declination angle to re-establish valid covariances
-    FuseDeclination(0.1f);
+        // fuse the declination angle to establish covariances and prevent large swings in declination
+        // during initial fusion
+        FuseDeclination(0.1f);
+
+    }
 }
 
 // record a magentic field state reset event

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -428,10 +428,14 @@ void NavEKF2_core::readGpsData()
             // Set the EKF origin and magnetic field declination if not previously set  and GPS checks have passed
             if (gpsGoodToAlign && !validOrigin) {
                 setOrigin();
-                // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
+
+                // set the NE earth magnetic field states using the published declination
+                // and set the corresponding variances and covariances
                 alignMagStateDeclination();
+
                 // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
                 EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
+
             }
 
             // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -1397,20 +1397,17 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
             initQuat.rotation_matrix(Tbn);
             stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
 
-            // align the NE earth magnetic field states with the published declination
+            // set the NE earth magnetic field states using the published declination
+            // and set the corresponding variances and covariances
             alignMagStateDeclination();
 
-            // zero the magnetic field state associated covariances
-            zeroRows(P,16,21);
-            zeroCols(P,16,21);
-            // set initial earth magnetic field variances
-            P[16][16] = sq(frontend->_magNoise);
-            P[17][17] = P[16][16];
-            P[18][18] = P[16][16];
-            // set initial body magnetic field variances
-            P[19][19] = sq(frontend->_magNoise);
-            P[20][20] = P[19][19];
-            P[21][21] = P[19][19];
+            // set the remaining variances and covariances
+            zeroRows(P,18,21);
+            zeroCols(P,18,21);
+            P[18][18] = sq(frontend->_magNoise);
+            P[19][19] = P[18][18];
+            P[20][20] = P[18][18];
+            P[21][21] = P[18][18];
 
         }