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,6 +160,14 @@ 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)));
 
+            if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) {
+                yawAlignGpsValidCount++;
+            } else {
+                yawAlignGpsValidCount = 0;
+            }
+
+            if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) {
+                yawAlignGpsValidCount = 0;
                 // keep roll and pitch and reset yaw
                 rotationOrder order;
                 bestRotationOrder(order);
@@ -180,6 +188,9 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
                 }
             }
         }
+    } else {
+        yawAlignGpsValidCount = 0;
+    }
 }
 
 // align the yaw angle for the quaternion states to the given yaw angle which should be at the fusion horizon

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