AP_NavEKF: Improved magnetometer consistency checks

A magnetometer axis that fails the innovation consistency check will cause
all axes not to be used. If this condition continues for 10 seconds, a
magnetometer timeout condition will be declared. When the timeout has
occurred, if it is not a fly forward vehicle, then individual channels
will be used again, but with a reduced weighting.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -329,16 +329,17 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
 {
     AP_Param::setup_object_defaults(this, var_info);
     // Tuning parameters
-    _gpsNEVelVarAccScale    = 0.05f;     // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
+    _gpsNEVelVarAccScale    = 0.05f;    // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
     _gpsDVelVarAccScale     = 0.07f;    // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
-    _gpsPosVarAccScale      = 0.05f;     // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
+    _gpsPosVarAccScale      = 0.05f;    // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
     _msecHgtDelay           = 60;       // Height measurement delay (msec)
     _msecMagDelay           = 40;       // Magnetometer measurement delay (msec)
     _msecTasDelay           = 240;      // Airspeed measurement delay (msec)
     _gpsRetryTimeUseTAS     = 20000;    // GPS retry time with airspeed measurements (msec)
-    _gpsRetryTimeNoTAS      = 10000;     // GPS retry time without airspeed measurements (msec)
+    _gpsRetryTimeNoTAS      = 10000;    // GPS retry time without airspeed measurements (msec)
     _hgtRetryTimeMode0      = 10000;    // Height retry time with vertical velocity measurement (msec)
     _hgtRetryTimeMode12     = 5000;     // Height retry time without vertical velocity measurement (msec)
+    _magFailTimeLimit       = 10000;     // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
     _magVarRateScale        = 0.05f;    // scale factor applied to magnetometer variance due to angular rate
     _gyroBiasNoiseScaler    = 3.0f;     // scale factor applied to gyro bias state process variance when on ground
     _msecGpsAvg             = 200;      // average number of msec between GPS measurements
@@ -722,6 +723,18 @@ void NavEKF::SelectMagFusion()
     // 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 it failed
+    if (magHealth) {
+        lastHealthyMagTime_ms = hal.scheduler->millis();
+    } else {
+        if ((hal.scheduler->millis() - lastHealthyMagTime_ms) > uint32_t(_magFailTimeLimit) && use_compass()) {
+            magTimeout = true;
+        } else {
+            magTimeout = false;
+        }
+    }
+
+
     // determine if conditions are right to start a new fusion cycle
     bool dataReady = statesInitialised && use_compass() && newDataMag;
     if (dataReady)
@@ -2149,12 +2162,19 @@ void NavEKF::FuseMagnetometer()
         innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex];
         // calculate the innovation test ratio
         magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(_magInnovGate) * varInnovMag[obsIndex]);
-        // test the ratio before fusing data
-        if (magTestRatio[obsIndex] < 1.0f)
+        // check the last values from all componenets and set magnetometer health accordingly
+        magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
+        // Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
+        // In this case we might as well try using the magnetometer, but with a reduced weighting
+        if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !_ahrs->get_fly_forward() && magTimeout))
         {
             // correct the state vector
             for (uint8_t j= 0; j<=indexLimit; j++)
             {
+                // If we are forced to use a bad compass, we reduce the weighting by a factor of 4
+                if (!magHealth) {
+                    Kfusion[j] *= 0.25f;
+                }
                 states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
             }
             // normalise the quaternion states
@@ -2724,8 +2744,8 @@ void NavEKF::OnGroundCheck()
         } else {
             onGround = true;
         }
-        // force a yaw alignment if exiting onGround without a compass
-        if (!onGround && prevOnGround && !use_compass()) {
+        // 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 && _ahrs->get_fly_forward()))) {
             alignYawGPS();
         }
         // If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround

libraries/AP_NavEKF/AP_NavEKF.h

@@ -320,6 +320,7 @@ private:
     AP_Int16 _gpsRetryTimeNoTAS;    // GPS retry time following innovation consistency fail if no TAS measurements are used (msec)
     AP_Int16 _hgtRetryTimeMode0;    // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec)
     AP_Int16 _hgtRetryTimeMode12;   // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec)
+    AP_Int16 _magFailTimeLimit;     // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
     float _gyroBiasNoiseScaler;     // scale factor applied to gyro bias state process variance when on ground
     float _magVarRateScale;         // scale factor applied to magnetometer variance due to angular rate
     uint16_t _msecGpsAvg;           // average number of msec between GPS measurements
@@ -330,12 +331,14 @@ private:
     // Variables
     uint8_t skipCounter;            // counter used to skip position and height corrections to achieve _skipRatio
     bool statesInitialised;         // boolean true when filter states have been initialised
-    bool velHealth;                 // boolean true if velocity measurements have failed innovation consistency check
-    bool posHealth;                 // boolean true if position measurements have failed innovation consistency check
-    bool hgtHealth;                 // boolean true if height measurements have failed innovation consistency check
+    bool velHealth;                 // boolean true if velocity measurements have passed innovation consistency check
+    bool posHealth;                 // boolean true if position measurements have passed innovation consistency check
+    bool hgtHealth;                 // boolean true if height measurements have passed innovation consistency check
+    bool magHealth;                 // boolean true if magnetometer has passed innovation consistency check
     bool velTimeout;                // boolean true if velocity measurements have failed innovation consistency check and timed out
-    bool posTimeout;            // boolean true if position measurements have failed innovation consistency check and timed out
+    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
 
     Vector31 Kfusion;               // Kalman gain vector
     Matrix22 KH;                    // intermediate result used for covariance updates
@@ -422,6 +425,7 @@ private:
     uint32_t lastStateStoreTime_ms; // time of last state vector storage
     uint32_t lastFixTime_ms;        // time of last GPS fix used to determine if new data has arrived
     uint32_t secondLastFixTime_ms;  // time of second last GPS fix used to determine how long since last update
+    uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
     Vector3f lastAngRate;           // angular rate from previous IMU sample used for trapezoidal integrator
     Vector3f lastAccel1;            // acceleration from previous IMU1 sample used for trapezoidal integrator
     Vector3f lastAccel2;            // acceleration from previous IMU2 sample used for trapezoidal integrator