AP_NavEKF2: rework yaw and magnetic heading reset logic

Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags.
Reduce the number of places where decisions to perform a yaw and field reset are made.
Don't perform a reset unless there is is data in the buffer
Don't use 3-axis fusion if the field states still need to be reset.
When starting 3-axis fusion request a reset if not previously performed.
Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -98,12 +98,12 @@ void NavEKF2_core::setWindMagStateLearningMode()
     bool magCalRequested =
             ((magCal == 0) && inFlight) || // when flying
             ((magCal == 1) && manoeuvring)  || // when manoeuvring
-            ((magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
+            ((magCal == 3) && firstInflightYawInit && firstInflightMagInit) || // when initial in-air yaw and mag field reset is complete
             (magCal == 4); // all the time
 
     // 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)
-    bool magCalDenied = !use_compass() || (magCal == 2) ||(onGround && magCal != 4);
+    bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);
 
     // Inhibit the magnetic field calibration if not requested or denied
     bool setMagInhibit = !magCalRequested || magCalDenied;
@@ -115,12 +115,17 @@ void NavEKF2_core::setWindMagStateLearningMode()
         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 || (!firstInflightMagInit && inFlight)) {
+            magYawResetRequest = true;
+        }
     }
 
     // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
     // because we want it re-done for each takeoff
     if (onGround) {
-        firstMagYawInit = false;
+        firstInflightYawInit = false;
+        firstInflightMagInit = false;
     }
 
     // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
@@ -205,7 +210,7 @@ void NavEKF2_core::setAidingMode()
     }
 }
 
-// Check the alignmnent status of the tilt and yaw attitude
+// Check the tilt and yaw alignmnent status
 // Used during initial bootstrap alignment of the filter
 void NavEKF2_core::checkAttitudeAlignmentStatus()
 {
@@ -218,14 +223,15 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
         hal.console->printf("EKF2 IMU%u tilt alignment complete\n",(unsigned)imu_index);
     }
 
-    // Once tilt has converged, align yaw using magnetic field measurements
+    // submit yaw and magnetic field reset requests depending on whether we have compass data
-    if (tiltAlignComplete && !yawAlignComplete && use_compass()) {
+    if (tiltAlignComplete && !yawAlignComplete) {
-        Vector3f eulerAngles;
+        if (use_compass()) {
-        stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+            magYawResetRequest = true;
-        stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+            gpsYawResetRequest = false;
-        StoreQuatReset();
+        } else {
-        yawAlignComplete = true;
+            magYawResetRequest = false;
-        hal.console->printf("EKF2 IMU%u yaw alignment complete\n",(unsigned)imu_index);
+            gpsYawResetRequest = true;
+        }
     }
 }
 
@@ -293,6 +299,15 @@ void NavEKF2_core::setOrigin()
     hal.console->printf("EKF2 IMU%u Origin Set\n",(unsigned)imu_index);
 }
 
+// record a yaw reset event
+void NavEKF2_core::recordYawReset()
+{
+    yawAlignComplete = true;
+    if (inFlight) {
+        firstInflightYawInit = true;
+    }
+}
+
 // Commands the EKF to not use GPS.
 // This command must be sent prior to arming
 // This command is forgotten by the EKF each time the vehicle disarms

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -23,43 +23,81 @@ void NavEKF2_core::controlMagYawReset()
     // Use a quaternion division to calcualte the delta quaternion between the rotation at the current and last time
     Quaternion deltaQuat = stateStruct.quat / prevQuatMagReset;
     prevQuatMagReset = stateStruct.quat;
+
     // convert the quaternion to a rotation vector and find its length
     Vector3f deltaRotVec;
     deltaQuat.to_axis_angle(deltaRotVec);
-    float deltaRot = deltaRotVec.length();
 
-    // In-Flight reset for vehicle that cannot use a zero sideslip assumption
+    // check if the spin rate is OK - high spin rates can cause angular alignment errors
-    // Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
+    bool angRateOK = deltaRotVec.length() < 0.1745f;
-    // Perform the reset earlier if high yaw and velocity innovations indicate that 'toilet bowling' is occurring
-    // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
-    // Delay if rotated too far since the last check as rapid rotations will produce errors in the magnetic field states
-    if (!firstMagYawInit && !assume_zero_sideslip() && inFlight && deltaRot < 0.1745f) {
-        // check that we have reached a height where ground magnetic interference effects are insignificant
-        bool hgtCheckPassed = (stateStruct.position.z  - posDownAtTakeoff) < -5.0f;
 
-        // check for 'toilet bowling' which is characterised by large yaw and velocity innovations and caused by bad yaw alignment
+    // a flight reset is allowed if we are not spinning too fast and are in flight
-        // this can occur if there is severe magnetic interference on the ground
+    bool flightResetAllowed = angRateOK && inFlight;
-        bool toiletBowling = (yawTestRatio > 1.0f) && (velTestRatio > 1.0f);
+
+    // check that we have reached a height where ground magnetic interference effects are insignificant
+    bool hgtCheckPassed = (stateStruct.position.z  - posDownAtTakeoff) < -5.0f;
+
+    // check for 'toilet bowling' which is characterised by large yaw and velocity innovations and caused by bad yaw alignment
+    // this can occur if there is severe magnetic interference on the ground
+    bool toiletBowling = (yawTestRatio > 1.0f) && (velTestRatio > 1.0f);
+
+    // calculate if an in flight reset is required
+    bool flightResetRequired = (!firstInflightYawInit && (hgtCheckPassed || toiletBowling));
+
+    // an initial reset is required if we have not yet aligned the yaw angle
+    bool initialResetRequired = !yawAlignComplete;
+
+    // a combined yaw angle and magnetic field reset can be initiated by:
+    magYawResetRequest = magYawResetRequest || // an external request
+            (initialResetRequired && angRateOK) || // we need to do an initial alignment and aren't rotating too fast
+            (flightResetRequired && flightResetAllowed && !assume_zero_sideslip()); // conditions are right to do the in-flight re-alignment
+
+    // Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
+    if (magYawResetRequest || magStateResetRequest) {
+
+        // gett he eeruler angles from the current state estimate
+        Vector3f eulerAngles;
+        stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+
+        // Use the Euler angles and magnetometer measurement to update the magnetic field states
+        // and get an updated quaternion
+        Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+
+        // if a yaw reset has been requested, apply the updated quaterniont the current state
+        if (magYawResetRequest) {
+            // previous value used to calculate a reset delta
+            Quaternion prevQuat = stateStruct.quat;
+
+            // update the quaternion states using the new yaw angle
+            stateStruct.quat = newQuat;
 
-        if (hgtCheckPassed || toiletBowling) {
-            firstMagYawInit = true;
-            // 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;
+            prevQuat = stateStruct.quat/prevQuat;
-            StoreQuatRotate(tempQuat);
+            StoreQuatRotate(prevQuat);
-        }
-    }
 
-    // In-Flight reset for vehicles that can use a zero sideslip assumption (Planes)
+            // send initial alignment status to console
-    // this is done to protect against unrecoverable heading alignment errors due to compass faults
+            if (!yawAlignComplete) {
-    if (!firstMagYawInit && assume_zero_sideslip() && inFlight) {
+                hal.console->printf("EKF2 IMU%u initial yaw alignment complete\n",(unsigned)imu_index);
-        realignYawGPS();
+            }
-        firstMagYawInit = true;
+
+            // send initial in-flight yaw alignment status to console
+            if (!firstInflightYawInit && inFlight) {
+                hal.console->printf("EKF2 IMU%u in-flight yaw alignment complete\n",(unsigned)imu_index);
+            }
+
+            // update the yaw reset completed status
+            recordYawReset();
+
+            // clear the yaw reset request flag
+            magYawResetRequest = false;
+        }
+}
+
+    // Request an in-flight check of heading against GPS and reset if necessary
+    // this can only be used by vehicles that can use a zero sideslip assumption (Planes)
+    // this allows recovery for heading alignment errors due to compass faults
+    if (!firstInflightYawInit && inFlight && assume_zero_sideslip()) {
+        gpsYawResetRequest = true;
     }
 
 }
@@ -78,23 +116,13 @@ void NavEKF2_core::realignYawGPS()
         float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
 
         // calculate course yaw angle from GPS velocity
-        float gpsYaw = atan2f(gpsDataNew.vel.y,gpsDataNew.vel.x);
+        float gpsYaw = atan2f(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
-
-        // check if this is our first alignment and we are not using the compass
-        if (!yawAlignComplete && !use_compass()) {
-            // calculate new filter quaternion states from Euler angles
-            stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
-            yawAlignComplete = true;
-
-            // zero the attitude covariances becasue the corelations will now be invalid
-            zeroAttCovOnly();
-        }
 
         // Check the yaw angles for consistency
         float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),MAX(fabsf(wrap_PI(gpsYaw - eulerAngles.z)),fabsf(wrap_PI(velYaw - eulerAngles.z))));
 
-        // If the angles disagree by more than 45 degrees and GPS innovations are large or no compass, we declare the magnetic yaw as bad
+        // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
-        badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && !(PV_AidingMode == AID_NONE)) || !use_compass();
+        badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
 
         // correct yaw angle using GPS ground course if compass yaw bad
         if (badMagYaw) {
@@ -102,18 +130,27 @@ void NavEKF2_core::realignYawGPS()
             // calculate new filter quaternion states from Euler angles
             stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
 
+            // send yaw alignment information to console
+            hal.console->printf("EKF2 IMU%u yaw aligned to GPS velocity\n",(unsigned)imu_index);
+
             // zero the attitude covariances becasue the corelations will now be invalid
             zeroAttCovOnly();
 
             // reset tposition fusion timer to cause the states to be reset to the GPS on the next GPS fusion cycle
             lastPosPassTime_ms = 0;
         }
+
+        // record the yaw reset event
+        recordYawReset();
+
+        // clear any GPS yaw requests
+        gpsYawResetRequest = false;
     }
 
     // fix magnetic field states and clear any compass fault conditions
     if (use_compass()) {
-        // reset the magnetometer field states - we could have got bad external interference when initialising on-ground
+        // submit a request to reset the magnetic field states
-        calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+        magStateResetRequest = true;
 
         // We shoud retry the primary magnetometer if previously switched or failed
         magSelectIndex = 0;
@@ -149,8 +186,8 @@ void NavEKF2_core::SelectMagFusion()
     // check for availability of magnetometer data to fuse
     magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
 
-    if (magDataToFuse) {
+    // Control reset of yaw and magnetic field states if we are using compass data
-        // Control reset of yaw and magnetic field states
+    if (magDataToFuse && use_compass()) {
         controlMagYawReset();
     }
 
@@ -158,8 +195,8 @@ void NavEKF2_core::SelectMagFusion()
     // wait until the EKF time horizon catches up with the measurement
     bool dataReady = (magDataToFuse && statesInitialised && use_compass() && yawAlignComplete);
     if (dataReady) {
-        // If we haven't performed the first airborne magnetic field update or have inhibited magnetic field learning, then we use the simple method of declination to maintain heading
+        // use the simple method of declination to maintain heading if we cannot use the magnetic field states
-        if(inhibitMagStates) {
+        if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) {
             fuseEulerYaw();
             // zero the test ratio output from the inactive 3-axis magneteometer fusion
             magTestRatio.zero();
@@ -193,12 +230,6 @@ void NavEKF2_core::SelectMagFusion()
             yawTestRatio = 0.0f;
             lastSynthYawTime_ms = imuSampleTime_ms;
         }
-        // In-Flight yaw alignment for vehicles that can use a zero sideslip assumption (Planes)
-        // and are not using a compass
-        if (!yawAlignComplete && assume_zero_sideslip() && inFlight) {
-            realignYawGPS();
-            firstMagYawInit = yawAlignComplete;
-        }
     }
 
     // stop performance timer
@@ -739,8 +770,9 @@ void NavEKF2_core::fuseEulerYaw()
 
     // Use the difference between the horizontal projection and declination to give the measured yaw
     // If we can't use compass data, set the  meaurement to the predicted
+    // to prevent uncontrolled variance growth whilst on ground without magnetometer
     float measured_yaw;
-    if (use_compass()) {
+    if (use_compass() && yawAlignComplete && magStateInitComplete) {
         measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
     } else {
         measured_yaw = predicted_yaw;
@@ -1007,5 +1039,14 @@ void NavEKF2_core::alignMagStateDeclination()
     stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng);
 }
 
+// record a magentic field state reset event
+void NavEKF2_core::recordMagReset()
+{
+    magStateInitComplete = true;
+    if (inFlight) {
+        firstInflightMagInit = true;
+    }
+}
+
 
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -350,7 +350,7 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
 bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, 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 (mag_idx == magSelectIndex && firstMagYawInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
+    if (mag_idx == magSelectIndex && firstInflightMagInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
         magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
         return true;
     } else {

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -196,6 +196,11 @@ void NavEKF2_core::SelectVelPosFusion()
         fusePosData = false;
     }
 
+    // we have GPS data to fuse and a request to align the yaw using the GPS course
+    if (gpsYawResetRequest) {
+        realignYawGPS();
+    }
+
     // Select height data to be fused from the available baro, range finder and GPS sources
     selectHeightForFusion();
 

libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

@@ -39,10 +39,8 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
         magYawResetTimer_ms = imuSampleTime_ms;
     }
     if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
-        // reset heading and field states
+        // request reset of heading and magnetic field states
-        Vector3f eulerAngles;
+        magYawResetRequest = true;
-        getEulerAngles(eulerAngles);
-        stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
         // reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
         magYawResetTimer_ms = imuSampleTime_ms;
     }

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -153,7 +153,8 @@ void NavEKF2_core::InitialiseVariables()
     tasTimeout = true;
     badMagYaw = false;
     badIMUdata = false;
-    firstMagYawInit = false;
+    firstInflightYawInit = false;
+    firstInflightMagInit = false;
     dtIMUavg = 0.0025f;
     dtEkfAvg = 0.01f;
     dt = 0;
@@ -253,6 +254,10 @@ void NavEKF2_core::InitialiseVariables()
     runUpdates = false;
     framesSincePredict = 0;
     lastMagOffsetsValid = false;
+    magStateResetRequest = false;
+    magStateInitComplete = false;
+    magYawResetRequest = false;
+    gpsYawResetRequest = false;
 
     // zero data buffers
     storedIMU.reset();
@@ -1317,7 +1322,7 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
 
         // calculate yaw angle rel to true north
         yaw = magDecAng - magHeading;
-        yawAlignComplete = true;
+
         // calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy
         // otherwise use existing heading
         if (!badMagYaw) {
@@ -1360,8 +1365,15 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
 
         // 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();
     } else {
-        // no magnetoemter data so there is nothing we can do
+        // this function should not be called if there is no compass data but if is is, return the
+        // current attitude
         initQuat = stateStruct.quat;
     }
 

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -560,7 +560,7 @@ private:
     // avoid unnecessary operations
     void setWindMagStateLearningMode();
 
-    // Check the alignmnent status of the tilt and yaw attitude
+    // Check the alignmnent status of the tilt attitude
     // Used during initial bootstrap alignment of the filter
     void checkAttitudeAlignmentStatus();
 
@@ -611,6 +611,12 @@ private:
     // zero attitude state covariances, but preserve variances
     void zeroAttCovOnly();
 
+    // record a yaw reset event
+    void recordYawReset();
+
+    // record a magnetic field state reset event
+    void recordMagReset();
+
     // effective value of MAG_CAL
     uint8_t effective_magCal(void) const;
     
@@ -708,7 +714,8 @@ private:
     float tasTestRatio;             // sum of squares of true airspeed innovation divided by fail threshold
     bool inhibitWindStates;         // true when wind states and covariances are to remain constant
     bool inhibitMagStates;          // true when magnetic field states and covariances are to remain constant
-    bool firstMagYawInit;           // true when the first post takeoff initialisation of earth field and yaw angle has been performed
+    bool firstInflightYawInit;      // true when the first post takeoff initialisation of yaw angle has been performed
+    bool firstInflightMagInit;      // true when the first post takeoff initialisation of magnetic field states been performed
     bool gpsNotAvailable;           // bool true when valid GPS data is not available
     bool isAiding;                  // true when the filter is fusing position, velocity or flow measurements
     bool prevIsAiding;              // isAiding from previous frame
@@ -726,6 +733,7 @@ private:
     float tiltErrFilt;              // Filtered tilt error metric
     bool tiltAlignComplete;         // true when tilt alignment is complete
     bool yawAlignComplete;          // true when yaw alignment is complete
+    bool magStateInitComplete;      // true when the magnetic field sttes have been initialised
     uint8_t stateIndexLim;          // Max state index used during matrix and array operations
     imu_elements imuDataDelayed;    // IMU data at the fusion time horizon
     imu_elements imuDataNew;        // IMU data at the current time horizon
@@ -780,6 +788,9 @@ private:
     bool startPredictEnabled;       // boolean true when the frontend has given permission to start a new state prediciton cycele
     uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
     float posDownObsNoise;          // observation noise variance on the vertical position used by the state and covariance update step (m^2)
+    bool magStateResetRequest;      // true if magnetic field states need to be reset using the magneteomter measurements
+    bool magYawResetRequest;        // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
+    bool gpsYawResetRequest;        // true if the vehicle yaw needs to be reset to the GPS course
 
     // variables used to calculate a vertical velocity that is kinematically consistent with the verical position
     float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.