AP_NavEKF : Improved Magnetometer Error Handling

(Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector.
Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight.
For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -325,7 +325,9 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
     prevStaticMode(true),       // staticMode from previous filter update
     yawAligned(false),          // set true when heading or yaw angle has been aligned
     inhibitWindStates(true),    // inhibit wind state updates on startup
-    inhibitMagStates(true)      // inhibit magnetometer state updates on startup
+    inhibitMagStates(true),     // inhibit magnetometer state updates on startup
+    onGround(true),             // initialise assuming we are on ground
+    manoeuvring(false)          // initialise assuming we are not maneouvring
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     ,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@@ -495,7 +497,7 @@ void NavEKF::InitialiseFilterDynamic(void)
     ResetVelocity();
     ResetPosition();
     ResetHeight();
-    state.body_magfield  = magBias;
+    state.body_magfield.zero();
 
     // set to true now that states have be initialised
     statesInitialised = true;
@@ -568,7 +570,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
     state.accel_zbias1 = 0;
     state.accel_zbias2 = 0;
     state.wind_vel.zero();
-    state.body_magfield = magBias;
+    state.body_magfield.zero();
 
     // set to true now we have intialised the states
     statesInitialised = true;
@@ -628,9 +630,6 @@ void NavEKF::UpdateFilter()
         ResetPosition();
         ResetHeight();
         StoreStatesReset();
-        // clear the magnetometer failed status as the failure may have been
-        // caused by external field disturbances associated with pre-flight activities
-        magFailed = false;
         calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
         prevStaticMode = staticMode;
     }
@@ -751,51 +750,42 @@ void NavEKF::SelectVelPosFusion()
 // select fusion of magnetometer data
 void NavEKF::SelectMagFusion()
 {
-    if(!magFailed) {
-        // check for and read new magnetometer measurements
-        readMagData();
+    // check for and read new magnetometer measurements
+    readMagData();
 
-        // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
-        // If we have a vehicle that can fly without a compass (a vehicle that doesn't have significant sideslip) then the compass is permanently failed and will not be used until the filter is reset
-        if (magHealth) {
-            lastHealthyMagTime_ms = imuSampleTime_ms;
-        } else {
-            if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) {
-                magTimeout = true;
-                if (assume_zero_sideslip()) {
-                    magFailed = true;
-                }
-            } else {
-                magTimeout = false;
-            }
-        }
+    // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
+    if (magHealth) {
+        magTimeout = false;
+        lastHealthyMagTime_ms = imuSampleTime_ms;
+    } else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) {
+        magTimeout = true;
+    }
 
-        // determine if conditions are right to start a new fusion cycle
-        bool dataReady = statesInitialised && use_compass() && newDataMag;
-        if (dataReady)
-        {
-            fuseMagData = true;
-            // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
-            memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
-            magUpdateCount = 0;
-        }
-        else
-        {
-            fuseMagData = false;
-        }
+    // determine if conditions are right to start a new fusion cycle
+    bool dataReady = statesInitialised && use_compass() && newDataMag;
+    if (dataReady)
+    {
+        fuseMagData = true;
+        // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
+        memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
+        magUpdateCount = 0;
+    }
+    else
+    {
+        fuseMagData = false;
+    }
 
-        // call the function that performs fusion of magnetometer data
-        FuseMagnetometer();
+    // call the function that performs fusion of magnetometer data
+    FuseMagnetometer();
 
-        // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
-        if (magUpdateCount < magUpdateCountMax) {
-            magUpdateCount ++;
-            for (uint8_t i = 0; i <= 9; i++) {
-                states[i] += magIncrStateDelta[i];
-            }
+    // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
+    if (magUpdateCount < magUpdateCountMax) {
+        magUpdateCount ++;
+        for (uint8_t i = 0; i <= 9; i++) {
+            states[i] += magIncrStateDelta[i];
         }
-
     }
+
 }
 
 // select fusion of true airspeed measurements
@@ -2878,38 +2868,55 @@ bool NavEKF::getLLH(struct Location &loc) const
 // calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
 void NavEKF::SetFlightAndFusionModes()
 {
-    const AP_Airspeed *airspeed = _ahrs->get_airspeed();
-    uint8_t highAirSpd = (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f);
-    float gndSpdSq = sq(velNED[0]) + sq(velNED[1]);
-    uint8_t highGndSpdStage1 = (uint8_t)(gndSpdSq > 9.0f);
-    uint8_t highGndSpdStage2 = (uint8_t)(gndSpdSq > 36.0f);
-    uint8_t highGndSpdStage3 = (uint8_t)(gndSpdSq > 81.0f);
-    uint8_t largeHgt = (uint8_t)(fabsf(hgtMea) > 15.0f);
-    uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt;
-    // if magnetometer calibration mode 1 is selected, use a manoeuvre threshold test
-    // otherwise use a height and velocity test
-    if ((_magCal == 1)  && (accNavMagHoriz > 0.5f) && !static_mode_demanded() && use_compass()) {
-        onGround = false;
+    // determine if the vehicle is manoevring
+    if (accNavMagHoriz > 0.5f){
+        manoeuvring = true;
     } else {
-        // detect on-ground to in-air transition
-        // if we are already on the ground then 3 or more out of 5 criteria are required
-        // if we are in the air then only 2 or more are required
-        // this prevents rapid tansitions
-        if ((onGround && (inAirSum >= 3)) || (!onGround && (inAirSum >= 2))) {
+        manoeuvring = false;
+    }
+    // if we are a fly forward type vehicle, then in-air mode can be determined through a combination of speed and height criteria
+    if (assume_zero_sideslip()) {
+        // Evaluate a numerical score that defines the likelihood we are in the air
+        uint8_t highAirSpd = 0;
+        uint8_t heightVarying = 0;
+        uint8_t highGndSpdStage1 = 0;
+        uint8_t highGndSpdStage2 = 0;
+        uint8_t highGndSpdStage3 = 0;
+        uint8_t largeHgt = 0;
+        uint8_t accelerating = 0;
+        const AP_Airspeed *airspeed = _ahrs->get_airspeed();
+        if (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f) highAirSpd = 1;
+        float gndSpdSq = sq(velNED[0]) + sq(velNED[1]);
+        if (fabsf(_baro.get_climb_rate()) > 0.5f) heightVarying = 1; // this will trigger during change in baro height
+        if (gndSpdSq > 9.0f) highGndSpdStage1 = 1; // trigger at 3 m/s GPS velocity
+        if (gndSpdSq > 36.0f) highGndSpdStage2 = 1; // trigger at 6 m/s GPS velocity
+        if (gndSpdSq > 81.0f) highGndSpdStage3 = 1; // trigger at 9 m/s GPS velocity
+        if (fabsf(hgtMea) > 15.0f) largeHgt = 1; // trigger if more than 15m away from initial height
+        if (accNavMag > 0.5f) accelerating = 1; // this will trigger due to air turbulence during flight
+        uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt + heightVarying + accelerating;
+        // if we on-ground then 4 or more out of 7 criteria are required to transition to the
+        // in-air mode and we also need enough GPS velocity to be able to calculate a reliable ground track heading
+        if (onGround && (inAirSum >= 4) && highGndSpdStage2) {
             onGround = false;
-        } else {
+        }
+        // if is possible we are in flight, set the time this condition was last detected
+        if (inAirSum >= 1) {
+            airborneDetectTime_ms = imuSampleTime_ms;
+        }
+        // after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
+        if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
             onGround = true;
         }
-        // force a yaw alignment if exiting onGround without a compass or if compass is timed out and we are a fly forward vehicle
-        if (!onGround && prevOnGround && (!use_compass() || (magTimeout && assume_zero_sideslip()))) {
+        // perform a yaw alignment check against GPS if exiting on-ground mode
+        // this is done to protect against unrecoverable heading alignment errors due to compass faults
+        if (!onGround && prevOnGround) {
             alignYawGPS();
         }
-        // If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround
-        // we set the wind velocity to the reciprocal of the velocity vector and scale states so that the
-        // wind speed is equal to the 6m/s. This prevents gains being too high at the start
-        // of flight if launching into a headwind until the first turn when the EKF can form a  wind speed
-        // estimate
-        if (!onGround && prevOnGround && !useAirspeed() && assume_zero_sideslip()) {
+        // If we aren't using an airspeed sensor we set the wind velocity to the reciprocal
+        // of the velocity vector and scale states so that the wind speed is equal to 3m/s. This helps prevent gains
+        // being too high at the start of flight if launching into a headwind until the first turn when the EKF can form
+        // a wind speed estimate and also corrects bad initial wind estimates due to heading errors
+        if (!onGround && prevOnGround && !useAirspeed()) {
             setWindVelStates();
         }
     }
@@ -2917,8 +2924,12 @@ void NavEKF::SetFlightAndFusionModes()
     prevOnGround = onGround;
     // If we are on ground, or in static mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
     inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || staticMode);
-    // If magnetometer calibration mode is turned off by the user or we are on ground or in static mode, then inhibit magnetometer states
-    inhibitMagStates  = ((_magCal == 2) || !use_compass() || onGround || staticMode);
+    // request mag calibration for both in-air and manoeuvre threshold options
+    bool magCalRequested = ((_magCal == 0) && !onGround) || ((_magCal == 1) && manoeuvring);
+    // deny mag calibration request if we aren't using the compass, are in the pre-arm static mode or it has been inhibited by the user
+    bool magCalDenied = (!use_compass() || staticMode || (_magCal == 2));
+    // inhibit the magnetic field calibration if not requested or denied
+    inhibitMagStates = (!magCalRequested || magCalDenied);
 }
 
 // initialise the covariance matrix
@@ -3153,11 +3164,9 @@ void NavEKF::readMagData()
         // store time of last measurement update
         lastMagUpdate = _ahrs->get_compass()->last_update;
 
-        // read compass data and assign to bias and uncorrected measurement
-        // body fixed magnetic bias is opposite sign to APM compass offsets
-        // we scale compass data to improve numerical conditioning
-        magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
-        magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
+        // Read compass data
+        // We scale compass data to improve numerical conditioning
+        magData = _ahrs->get_compass()->get_field() * 0.001f;
 
         // get states stored at time closest to measurement time after allowance for measurement delay
         RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay));
@@ -3199,7 +3208,7 @@ void NavEKF::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
 
 // initialise the earth magnetic field states using declination, suppled roll/pitch
 // and magnetometer measurements and return initial attitude quaternion
-// if no magnetometer data, do not update amgentic field states and assume zero yaw angle
+// if no magnetometer data, do not update magnetic field states and assume zero yaw angle
 Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
 {
     // declare local variables required to calculate initial orientation and magnetic field
@@ -3216,7 +3225,7 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
         readMagData();
 
         // rotate the magnetic field into NED axes
-        initMagNED = Tbn*(magData - magBias);
+        initMagNED = Tbn * magData;
 
         // calculate heading of mag field rel to body heading
         float magHeading = atan2f(initMagNED.y, initMagNED.x);
@@ -3225,19 +3234,27 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
         float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
 
         // calculate yaw angle rel to true north
-        yaw = magDecAng - magHeading;
-        yawAligned = true;
-
-        // calculate initial filter quaternion states
-        initQuat.from_euler(roll, pitch, yaw);
+        if (!badMag) {
+            // if mag healthy use declination and mag measurements to calculate yaw
+            yaw = magDecAng - magHeading;
+            yawAligned = true;
+            // calculate initial filter quaternion states
+            initQuat.from_euler(roll, pitch, yaw);
+        } else {
+            // if mag failed keep current yaw
+            initQuat = state.quat;
+        }
 
         // calculate initial Tbn matrix and rotate Mag measurements into NED
         // to set initial NED magnetic field states
         initQuat.rotation_matrix(Tbn);
-        initMagNED = Tbn * (magData - magBias);
+        initMagNED = Tbn* magData;
 
         // write to earth magnetic field state vector
         state.earth_magfield = initMagNED;
+
+        // clear bad magnetometer status
+        badMag = false;
     } else {
         initQuat.from_euler(roll, pitch, 0.0f);
         yawAligned = false;
@@ -3247,11 +3264,11 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
     return initQuat;
 }
 
-// this function is used to do a forced alignment of the yaw angle to aligwith the horizontal velocity
-// vector from GPS. It is used to align the yaw angle after launch or takeoff without a magnetometer.
+// this function is used to do a forced alignment of the yaw angle to align with the horizontal velocity
+// vector from GPS. It is used to align the yaw angle after launch or takeoff.
 void NavEKF::alignYawGPS()
 {
-    if ((sq(velNED[0]) + sq(velNED[1])) > 16.0f) {
+    if ((sq(velNED[0]) + sq(velNED[1])) > 25.0f) {
         float roll;
         float pitch;
         float oldYaw;
@@ -3259,21 +3276,26 @@ void NavEKF::alignYawGPS()
         float yawErr;
         // get quaternion from existing filter states and calculate roll, pitch and yaw angles
         state.quat.to_euler(roll, pitch, oldYaw);
+        // calculate course yaw angle
+        oldYaw = atan2f(state.velocity.y,state.velocity.x);
         // calculate yaw angle from GPS velocity
         newYaw = atan2f(velNED[1],velNED[0]);
-        // modify yaw angle using GPS ground course if more than 45 degrees away or if not previously aligned
-        yawErr = fabsf(newYaw - oldYaw);
-        if (((yawErr > 0.7854f) && (yawErr < 5.4978f)) || !yawAligned) {
+        // estimate the yaw error
+        yawErr = wrap_PI(newYaw - oldYaw);
+        // If the inertial course angle disagrees with the GPS by more than 45 degrees, we declare the compass as bad
+        badMag = (fabsf(yawErr) > 0.7854f);
+        // correct yaw angle using GPS ground course compass failed or if not previously aligned
+        if (badMag || !yawAligned) {
+            // correct the yaw angle
+            newYaw = oldYaw + yawErr;
             // calculate new filter quaternion states from Euler angles
             state.quat.from_euler(roll, pitch, newYaw);
             // the yaw angle is now aligned so update its status
             yawAligned =  true;
-            // set the velocity states
-            if (_fusionModeGPS < 2) {
-                state.velocity.x = velNED.x;
-                state.velocity.y = velNED.y;
-            }
-            // reinitialise the quaternion, velocity and position covariances
+            // reset the position and velocity states
+            ResetPosition();
+            ResetVelocity();
+            // reset the covariance for the quaternion, velocity and position states
             // zero the matrix entries
             zeroRows(P,0,9);
             zeroCols(P,0,9);
@@ -3291,6 +3313,10 @@ void NavEKF::alignYawGPS()
             P[8][8]   = P[7][7];
             P[9][9]   = sq(5.0f);
         }
+        // Update magnetic field states if the magnetometer is bad
+        if (badMag) {
+            calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
+        }
     }
 }
 
@@ -3372,13 +3398,14 @@ void NavEKF::ZeroVariables()
     lastFixTime_ms = imuSampleTime_ms;
     secondLastFixTime_ms = imuSampleTime_ms;
     lastDecayTime_ms = imuSampleTime_ms;
+    airborneDetectTime_ms = imuSampleTime_ms;
 
     gpsNoiseScaler = 1.0f;
     velTimeout = false;
     posTimeout = false;
     hgtTimeout = false;
     magTimeout = false;
-    magFailed = false;
+    badMag = false;
     storeIndex = 0;
     dtIMU = 0;
     dt = 0;
@@ -3427,7 +3454,7 @@ bool NavEKF::static_mode_demanded(void) const
 // return true if we should use the compass
 bool NavEKF::use_compass(void) const
 {
-    return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw() && !magFailed;
+    return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw();
 }
 
 // decay GPS horizontal position offset to close to zero at a rate of 1 m/s

libraries/AP_NavEKF/AP_NavEKF.h

@@ -364,7 +364,7 @@ private:
     bool posTimeout;                // boolean true if position measurements have failed innovation consistency check and timed out
     bool hgtTimeout;                // boolean true if height measurements have failed innovation consistency check and timed out
     bool magTimeout;                // boolean true if magnetometer measurements have failed for too long and have timed out
-    bool magFailed;                 // boolean true if the magnetometer has failed
+    bool badMag;                    // boolean true if the magnetometer is declared to be producing bad data
 
     float gpsNoiseScaler;           // Used to scale the  GPS measurement noise and consistency gates to compensate for operation with small satellite counts
     Vector31 Kfusion;               // Kalman gain vector
@@ -393,6 +393,8 @@ private:
     ftype hgtRate;                  // state for rate of change of height filter
     bool onGround;                  // boolean true when the flight vehicle is on the ground (not flying)
     bool prevOnGround;              // value of onGround from previous update
+    bool manoeuvring;               // boolean true when the flight vehicle is performing horizontal changes in velocity
+    uint32_t airborneDetectTime_ms; // last time flight movement was detected
     Vector6 innovVelPos;            // innovation output for a group of measurements
     Vector6 varInnovVelPos;         // innovation variance output for a group of measurements
     bool fuseVelData;               // this boolean causes the velNED measurements to be fused
@@ -414,7 +416,6 @@ private:
     bool fuseVtasData;              // boolean true when airspeed data is to be fused
     float VtasMeas;                 // true airspeed measurement (m/s)
     state_elements statesAtVtasMeasTime;  // filter states at the effective measurement time
-    Vector3f magBias;               // magnetometer bias vector in XYZ body axes
     const ftype covTimeStepMax;     // maximum time allowed between covariance predictions
     const ftype covDelAngMax;       // maximum delta angle between covariance predictions
     bool covPredStep;               // boolean set to true when a covariance prediction step has been performed