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,9 +111,19 @@ 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
-        for (uint8_t index=16; index<=21; index++) {
-            P[index][index] = sq(frontend->_magNoise);
+        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)) {

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,55 +1375,51 @@ 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) {
-            // 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);
-            // this check ensures we accumulate the resets that occur within a single iteration of the EKF
-            if (imuSampleTime_ms != lastYawReset_ms) {
-                yawResetAngle = 0.0f;
-            }
-            yawResetAngle += wrap_PI(yaw - tempEuler.z);
-            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;
+        // 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);
+        // this check ensures we accumulate the resets that occur within a single iteration of the EKF
+        if (imuSampleTime_ms != lastYawReset_ms) {
+            yawResetAngle = 0.0f;
         }
+        yawResetAngle += wrap_PI(yaw - tempEuler.z);
+        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();
 
         // calculate initial Tbn matrix and rotate Mag measurements into NED
         // to set initial NED magnetic field states
-        initQuat.rotation_matrix(Tbn);
-        stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
+        // don't do this if the earth field has already been learned
+        if (!magFieldLearned) {
+            initQuat.rotation_matrix(Tbn);
+            stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
+
+            // align the NE earth magnetic field states with the published declination
+            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];
 
-        // align the NE earth magnetic field states with the published declination
-        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];
-
-        // clear bad magnetic yaw status
-        badMagYaw = false;
+        // record the fact we have initialised the magnetic field states
+        recordMagReset();
 
         // clear mag state reset request
         magStateResetRequest = false;
 
-        // record the fact we have initialised the magnetic field states
-        recordMagReset();
-
     } 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;