AP_NavEKF2: remember mag field states between flight on same power cycle

Remember the mag bias and earth field states learned during flight when the vehicle  lands.
This improves performance for vehicles that do multiple flight on one power cycle

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -111,10 +111,20 @@ void NavEKF2_core::setWindMagStateLearningMode()
         inhibitMagStates = true;
     } else if (inhibitMagStates && !setMagInhibit) {
         inhibitMagStates = false;
-        // when commencing use of magnetic field states, set the variances equal to the observation uncertainty
+        if (magFieldLearned) {
+            // if we have already learned the field states, then retain the learned variances
+            P[16][16] = earthMagFieldVar.x;
+            P[17][17] = earthMagFieldVar.y;
+            P[18][18] = earthMagFieldVar.z;
+            P[19][19] = bodyMagFieldVar.x;
+            P[20][20] = bodyMagFieldVar.y;
+            P[21][21] = bodyMagFieldVar.z;
+        } else {
+            // set the variances equal to the observation variances
             for (uint8_t index=16; index<=21; index++) {
                 P[index][index] = sq(frontend->_magNoise);
             }
+        }
         // request a reset of the yaw and magnetic field states if not done before
         if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
             magYawResetRequest = true;

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -265,6 +265,26 @@ void NavEKF2_core::SelectMagFusion()
         }
     }
 
+    // If the final yaw reset has been performed and the state variances are sufficiently low
+    // record that the earth field has been learned.
+    bool earthMagFieldConverged = false;
+    if (!magFieldLearned && finalInflightMagInit) {
+        earthMagFieldConverged = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f));
+    }
+    if (magFieldLearned || earthMagFieldConverged) {
+        magFieldLearned = true;
+    }
+
+    // record the last learned field variances
+    if (magFieldLearned && !inhibitMagStates) {
+        earthMagFieldVar.x = P[16][16];
+        earthMagFieldVar.y = P[17][17];
+        earthMagFieldVar.z = P[18][18];
+        bodyMagFieldVar.x = P[19][19];
+        bodyMagFieldVar.y = P[20][20];
+        bodyMagFieldVar.z = P[21][21];
+    }
+
     // stop performance timer
     hal.util->perf_end(_perf_FuseMagnetometer);
 }
@@ -1062,6 +1082,11 @@ void NavEKF2_core::FuseDeclination(float declErr)
 // align the NE earth magnetic field states with the published declination
 void NavEKF2_core::alignMagStateDeclination()
 {
+    // don't do this if we already have a learned magnetic field
+    if (magFieldLearned) {
+        return;
+    }
+
     // get the magnetic declination
     float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
 

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -261,6 +261,7 @@ void NavEKF2_core::InitialiseVariables()
     posDownAtLastMagReset = stateStruct.position.z;
     yawInnovAtLastMagReset = 0.0f;
     quatAtLastMagReset = stateStruct.quat;
+    magFieldLearned = false;
 
     // zero data buffers
     storedIMU.reset();
@@ -1374,9 +1375,7 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
         // calculate yaw angle rel to true north
         yaw = magDecAng - magHeading;
 
-        // calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy
-        // otherwise use existing heading
-        if (!badMagYaw) {
+        // calculate initial filter quaternion states using yaw from magnetometer
         // store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
         Vector3f tempEuler;
         stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
@@ -1390,12 +1389,11 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
         initQuat.from_euler(roll, pitch, yaw);
         // zero the attitude covariances becasue the corelations will now be invalid
         zeroAttCovOnly();
-        } else {
-            initQuat = stateStruct.quat;
-        }
 
         // calculate initial Tbn matrix and rotate Mag measurements into NED
         // to set initial NED magnetic field states
+        // don't do this if the earth field has already been learned
+        if (!magFieldLearned) {
             initQuat.rotation_matrix(Tbn);
             stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
 
@@ -1414,15 +1412,14 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
             P[20][20] = P[19][19];
             P[21][21] = P[19][19];
 
-        // clear bad magnetic yaw status
-        badMagYaw = false;
-
-        // clear mag state reset request
-        magStateResetRequest = false;
+        }
 
         // record the fact we have initialised the magnetic field states
         recordMagReset();
 
+        // clear mag state reset request
+        magStateResetRequest = false;
+
     } else {
         // this function should not be called if there is no compass data but if is is, return the
         // current attitude

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -795,6 +795,9 @@ private:
     float posDownObsNoise;          // observation noise variance on the vertical position used by the state and covariance update step (m^2)
     Vector3f delAngCorrected;       // corrected IMU delta angle vector at the EKF time horizon (rad)
     Vector3f delVelCorrected;       // corrected IMU delta velocity vector at the EKF time horizon (m/s)
+    bool magFieldLearned;           // true when the magnetic field has been learned
+    Vector3f earthMagFieldVar;      // NED earth mag field variances for last learned field (mGauss^2)
+    Vector3f bodyMagFieldVar;       // XYZ body mag field variances for last learned field (mGauss^2)
 
     Vector3f outputTrackError;