AP_NavEKF3: Improve protection against GPS glitches during yaw alignment

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -231,6 +231,7 @@ void NavEKF3_core::setAidingMode()
     {
         PV_AidingMode = AID_NONE;
         yawAlignComplete = false;
+        yawAlignGpsValidCount = 0;
         finalInflightYawInit = false;
         ResetVelocity(resetDataSource::DEFAULT);
         ResetPosition(resetDataSource::DEFAULT);

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -134,7 +134,7 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
     Vector3F eulerAngles;
     stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
 
-    if (gpsDataDelayed.vel.xy().length_squared() > 25.0f) {
+    if (gpsDataDelayed.vel.xy().length_squared() > sq(GPS_VEL_YAW_ALIGN_MIN_SPD)) {
         // calculate course yaw angle
         ftype velYaw = atan2F(stateStruct.velocity.y,stateStruct.velocity.x);
 
@@ -160,25 +160,36 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
             const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
             const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F)));
 
-            // keep roll and pitch and reset yaw
-            rotationOrder order;
-            bestRotationOrder(order);
-            resetQuatStateYawOnly(gpsYaw, gps_yaw_variance, order);
+            if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) {
+                yawAlignGpsValidCount++;
+            } else {
+                yawAlignGpsValidCount = 0;
+            }
 
-            // reset the velocity and position states as they will be inaccurate due to bad yaw
-            ResetVelocity(resetDataSource::GPS);
-            ResetPosition(resetDataSource::GPS);
+            if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) {
+                yawAlignGpsValidCount = 0;
+                // keep roll and pitch and reset yaw
+                rotationOrder order;
+                bestRotationOrder(order);
+                resetQuatStateYawOnly(gpsYaw, gps_yaw_variance, order);
 
-            // send yaw alignment information to console
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
+                // reset the velocity and position states as they will be inaccurate due to bad yaw
+                ResetVelocity(resetDataSource::GPS);
+                ResetPosition(resetDataSource::GPS);
 
-            if (use_compass()) {
-                // request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation
-                magStateResetRequest = true;
-                // clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying
-                allMagSensorsFailed = false;
+                // send yaw alignment information to console
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
+
+                if (use_compass()) {
+                    // request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation
+                    magStateResetRequest = true;
+                    // clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying
+                    allMagSensorsFailed = false;
+                }
             }
         }
+    } else {
+        yawAlignGpsValidCount = 0;
     }
 }
 

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -303,6 +303,7 @@ void NavEKF3_core::readMagData()
         // force a new yaw alignment 1s after learning completes. The
         // delay is to ensure any buffered mag samples are discarded
         yawAlignComplete = false;
+        yawAlignGpsValidCount = 0;
         InitialiseVariablesMag();
     }
 

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -281,6 +281,7 @@ void NavEKF3_core::InitialiseVariables()
     tiltErrorVariance = sq(M_2PI);
     tiltAlignComplete = false;
     yawAlignComplete = false;
+    yawAlignGpsValidCount = 0;
     have_table_earth_field = false;
     stateIndexLim = 23;
     last_gps_idx = 0;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -115,6 +115,15 @@
 #define DOWNWARD_RANGEFINDER_MAX_INSTANCES 1
 #endif
 
+// number of continuous valid GPS velocity samples required to reset yaw
+#define GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD 5
+
+// minimum GPS horizontal speed required to use GPS ground course for yaw alignment (m/s)
+#define GPS_VEL_YAW_ALIGN_MIN_SPD 5.0F
+
+// maximum GPs ground course uncertainty allowed for yaw alignment (deg)
+#define GPS_VEL_YAW_ALIGN_MAX_ANG_ERR 15.0F
+
 class NavEKF3_core : public NavEKF_core_common
 {
 public:
@@ -1116,6 +1125,7 @@ private:
     uint32_t lastYawReset_ms;       // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
     bool tiltAlignComplete;         // true when tilt alignment is complete
     bool yawAlignComplete;          // true when yaw alignment is complete
+    uint8_t yawAlignGpsValidCount;  // number of continuous good GPS velocity samples used for in flight yaw alignment
     bool magStateInitComplete;      // true when the magnetic field states 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