AP_NavEKF2: Improve initialisation of magnetic field learning

Use the more robust, but less accurate compass heading fusion up to 5m altitude
Wait for the magnetometer data fusion time offset to be correct before using data to reset states
Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors
When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset.
Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m
Always re-do the yaw and mag reset when leaving the ground.

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -36,9 +36,6 @@ void NavEKF2_core::controlFilterModes()
     // Used during initial bootstrap alignment of the filter
     checkAttitudeAlignmentStatus();
 
-    // Control reset of yaw and magnetic field states
-    controlMagYawReset();
-
     // Set the type of inertial navigation aiding used
     setAidingMode();
 
@@ -65,13 +62,20 @@ void NavEKF2_core::setWindMagStateLearningMode()
             ((frontend._magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
             (frontend._magCal == 4); // all the time
 
-    // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, or we do not have an absolute position reference
+    // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
+    // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
     // If we do nto have absolute position (eg GPS) then the earth field states cannot be learned
-    bool magCalDenied = !use_compass() || (frontend._magCal == 2) || (PV_AidingMode == AID_NONE);
+    bool magCalDenied = !use_compass() || (frontend._magCal == 2) || (PV_AidingMode == AID_NONE) || (onGround && frontend._magCal != 4);
 
     // Inhibit the magnetic field calibration if not requested or denied
     inhibitMagStates = (!magCalRequested || magCalDenied);
 
+    // If magnetometer states are inhibited, we clear the flag indicating that the magnetic field in-flight initialisation has been completed
+    // because it will need to be done again
+    if (inhibitMagStates) {
+        firstMagYawInit = false;
+    }
+
     // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
     // if we are not using those states
     if (inhibitMagStates && inhibitWindStates) {

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -22,24 +22,21 @@ void NavEKF2_core::controlMagYawReset()
 {
     // Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
     // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
-    if (inFlight && !firstMagYawInit && (stateStruct.position.z  - posDownAtTakeoff) < -1.5f) {
-           // Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after commencement of flight
-           Vector3f eulerAngles;
-           stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
-           stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
-           StoreQuatReset();
-           firstMagYawInit = true;
-           magFuseTiltInhibit_ms =  imuSampleTime_ms;
-       } else if (inFlight && !secondMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f) {
-           // Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after commencement of flight
-           // This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
-           Vector3f eulerAngles;
-           stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
-           stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
-           StoreQuatReset();
-           secondMagYawInit = true;
-           magFuseTiltInhibit_ms =  imuSampleTime_ms;
-       }
+    // Delay if rotating rapidly as time offsets will produce errors in the magnetic field states
+    if (inFlight && !firstMagYawInit && (stateStruct.position.z  - posDownAtTakeoff) < -5.0f && (imuDataDelayed.delVel.length())/imuDataDelayed.delAngDT < 1.0f) {
+        firstMagYawInit = true;
+        // reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
+        magFuseTiltInhibit_ms =  imuSampleTime_ms;
+        // Update the yaw  angle and earth field states using the magnetic field measurements
+        Quaternion tempQuat;
+        Vector3f eulerAngles;
+        stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+        tempQuat = stateStruct.quat;
+        stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+        // calculate the change in the quaternion state and apply it to the ouput history buffer
+        tempQuat = stateStruct.quat/tempQuat;
+        StoreQuatRotate(tempQuat);
+    }
 
     // perform a yaw alignment check against GPS if exiting on-ground mode for fly forward type vehicle (plane)
     // this is done to protect against unrecoverable heading alignment errors due to compass faults
@@ -135,11 +132,17 @@ void NavEKF2_core::SelectMagFusion()
         magTimeout = true;
     }
 
-    bool temp = RecallMag();
+    // check for availability of magnetometer data to fuse
+    bool newMagDataAvailable = RecallMag();
+
+    if (newMagDataAvailable) {
+        // Control reset of yaw and magnetic field states
+        controlMagYawReset();
+    }
 
     // determine if conditions are right to start a new fusion cycle
     // wait until the EKF time horizon catches up with the measurement
-    bool dataReady = (temp && statesInitialised && use_compass() && yawAlignComplete);
+    bool dataReady = (newMagDataAvailable && statesInitialised && use_compass() && yawAlignComplete);
     if (dataReady) {
         // ensure that the covariance prediction is up to date before fusing data
         if (!covPredStep) CovariancePrediction();

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -175,7 +175,7 @@ bool NavEKF2_core::RecallOF()
 bool NavEKF2_core::getMagOffsets(Vector3f &magOffsets) const
 {
     // compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
-    if (secondMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy(0)) {
+    if (firstMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy(0)) {
         magOffsets = _ahrs->get_compass()->get_offsets(0) - stateStruct.body_magfield*1000.0f;
         return true;
     } else {
@@ -211,7 +211,7 @@ void NavEKF2_core::readMagData()
             Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
             bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
             // Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
-            if (changeDetected && secondMagYawInit) {
+            if (changeDetected && firstMagYawInit) {
                 stateStruct.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f;
                 stateStruct.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f;
                 stateStruct.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f;

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -102,7 +102,6 @@ void NavEKF2_core::InitialiseVariables()
     badMag = false;
     badIMUdata = false;
     firstMagYawInit = false;
-    secondMagYawInit = false;
     dtIMUavg = 0.0025f;
     dt = 0;
     velDotNEDfilt.zero();