AP_NavEKF3: separate GPS yaw from ExtNav yaw

libraries/AP_NavEKF/AP_NavEKF_Source.cpp

@@ -491,6 +491,9 @@ bool AP_NavEKF_Source::ext_nav_enabled(void) const
         if (src.velz == SourceZ::EXTNAV) {
             return true;
         }
+        if (src.yaw == SourceYaw::EXTNAV) {
+            return true;
+        }
     }
     return false;
 }

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -531,6 +531,9 @@ bool NavEKF3_core::using_external_yaw(void) const
     if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass()) {
         return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
     }
+    if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
+        return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
+    }
     return false;
 }
 
@@ -595,7 +598,8 @@ void NavEKF3_core::checkGyroCalStatus(void)
     // check delta angle bias variances
     const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
     const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
-    if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
+    if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) &&
+        (yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
         // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
         // which can make this check fail
         Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]);

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -176,10 +176,11 @@ void NavEKF3_core::realignYawGPS()
     }
 }
 
-void NavEKF3_core::alignYawAngle()
+// align the yaw angle for the quaternion states to the given yaw angle which should be at the fusion horizon
+void NavEKF3_core::alignYawAngle(const yaw_elements &yawAngData)
 {
     // update quaternion states and covariances
-    resetQuatStateYawOnly(yawAngDataDelayed.yawAng, sq(MAX(yawAngDataDelayed.yawAngErr, 1.0e-2f)), yawAngDataDelayed.order);
+    resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2f)), yawAngData.order);
 
     // send yaw alignment information to console
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index);
@@ -222,7 +223,8 @@ void NavEKF3_core::SelectMagFusion()
     // flight using the output from the GSF yaw estimator.
     if (!use_compass() &&
         yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL &&
-        yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
+        yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK &&
+        yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV) {
 
         // because this type of reset event is not as time critical, require a continuous history of valid estimates
         if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
@@ -255,12 +257,12 @@ void NavEKF3_core::SelectMagFusion()
         return;
     }
 
-    // Handle case where we are using an external yaw sensor instead of a magnetomer
+    // Handle case where we are using GPS yaw sensor instead of a magnetomer
     if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
         bool have_fused_gps_yaw = false;
         if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
             if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
-                alignYawAngle();
+                alignYawAngle(yawAngDataDelayed);
                 yaw_source_reset = false;
             } else if (tiltAlignComplete && yawAlignComplete) {
                 fuseEulerYaw(yawFusionMethod::EXTERNAL);
@@ -323,6 +325,34 @@ void NavEKF3_core::SelectMagFusion()
         // fall through to magnetometer fusion
     }
 
+    // Handle case where we are using an external nav for yaw
+    const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms);
+    if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
+        if (extNavYawDataToFuse) {
+            if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
+                alignYawAngle(extNavYawAngDataDelayed);
+                yaw_source_reset = false;
+            } else if (tiltAlignComplete && yawAlignComplete) {
+                fuseEulerYaw(yawFusionMethod::EXTNAV);
+            }
+            last_extnav_yaw_fusion_ms = imuSampleTime_ms;
+        } else if (tiltAlignComplete && !yawAlignComplete) {
+            // External yaw sources can take significant time to start providing yaw data so
+            // while waiting, fuse a 'fake' yaw observation at 7Hz to keep the filter stable
+            if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
+                // update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
+                if (!onGroundNotMoving) {
+                    // prevent uncontrolled yaw variance growth by fusing a zero innovation
+                    fuseEulerYaw(yawFusionMethod::PREDICTED);
+                } else {
+                    // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
+                    fuseEulerYaw(yawFusionMethod::STATIC);
+                }
+                lastSynthYawTime_ms = imuSampleTime_ms;
+            }
+        }
+    }
+
     // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
     if (magHealth) {
         magTimeout = false;
@@ -873,6 +903,10 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
     default:
         R_YAW = sq(frontend->_yawNoise);
         break;
+
+    case yawFusionMethod::EXTNAV:
+        R_YAW = sq(MAX(extNavYawAngDataDelayed.yawAngErr, 0.05f));
+        break;
     }
 
     // determine if a 321 or 312 Euler sequence is best
@@ -893,6 +927,10 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
         // determined automatically
         order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
         break;
+
+    case yawFusionMethod::EXTNAV:
+        order = extNavYawAngDataDelayed.order;
+        break;
     }
 
     // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
@@ -1054,6 +1092,10 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
         innovYaw = 0.0f;
         break;
 
+    case yawFusionMethod::EXTNAV:
+        // both external sensor yaw and last yaw when static are stored in yawAngDataDelayed.yawAng
+        innovYaw = wrap_PI(yawAngPredicted - extNavYawAngDataDelayed.yawAng);
+        break;
     }
 
     // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -1028,10 +1028,12 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
         // extract yaw from the attitude
         float roll_rad, pitch_rad, yaw_rad;
         quat.to_euler(roll_rad, pitch_rad, yaw_rad);
-
+        yaw_elements extNavYawAngDataNew;
-        // ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
+        extNavYawAngDataNew.yawAng = yaw_rad;
-        const float yaw_accuracy_rad = MAX(angErr, radians(5.0f));
+        extNavYawAngDataNew.yawAngErr = MAX(angErr, radians(5.0f)); // ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
-        writeEulerYawAngle(yaw_rad, yaw_accuracy_rad, timeStamp_ms, 2);
+        extNavYawAngDataNew.order = rotationOrder::TAIT_BRYAN_321; // Euler rotation order is 321 (ZYX)
+        extNavYawAngDataNew.time_ms = timeStamp_ms;
+        storedExtNavYawAng.push(extNavYawAngDataNew);
     }
 }
 
@@ -1288,7 +1290,8 @@ float NavEKF3_core::MagDeclination(void) const
 void NavEKF3_core::updateMovementCheck(void)
 {
     const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
-    const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass());
+    const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK ||
+                                       yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || !use_compass());
     if (!runCheck)
     {
         onGroundNotMoving = false;

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -101,9 +101,6 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
     // calculate buffer size for external nav data
     const uint8_t extnav_buffer_length = MIN((ekf_delay_ms / frontend->extNavIntervalMin_ms) + 1, imu_buffer_length);
 
-    // buffer size for external yaw
-    const uint8_t yaw_angle_buffer_length = MAX(obs_buffer_length, extnav_buffer_length);
-
     if(!storedGPS.init(obs_buffer_length)) {
         return false;
     }
@@ -126,7 +123,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
         // initialise to same length of IMU to allow for multiple wheel sensors
         return false;
     }
-    if(frontend->sources.ext_yaw_enabled() && !storedYawAng.init(yaw_angle_buffer_length)) {
+    if(frontend->sources.ext_yaw_enabled() && !storedYawAng.init(obs_buffer_length)) {
         return false;
     }
     // Note: the use of dual range finders potentially doubles the amount of data to be stored
@@ -143,6 +140,9 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
     if (frontend->sources.ext_nav_enabled() && !storedExtNavVel.init(extnav_buffer_length)) {
         return false;
     }
+    if(frontend->sources.ext_nav_enabled() && !storedExtNavYawAng.init(extnav_buffer_length)) {
+        return false;
+    }
     if(!storedIMU.init(imu_buffer_length)) {
         return false;
     }
@@ -452,6 +452,7 @@ void NavEKF3_core::InitialiseVariablesMag()
     magFieldLearned = false;
     storedMag.reset();
     storedYawAng.reset();
+    storedExtNavYawAng.reset();
     needMagBodyVarReset = false;
     needEarthBodyVarReset = false;
 }

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -625,6 +625,7 @@ private:
         GSF=2,
         STATIC=3,
         PREDICTED=4,
+        EXTNAV=5,
     };
 
     // update the navigation filter status
@@ -747,8 +748,8 @@ private:
 
     // initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
 
-    // align the yaw angle for the quaternion states using the external yaw sensor
+    // align the yaw angle for the quaternion states to the given yaw angle which should be at the fusion horizon
-    void alignYawAngle();
+    void alignYawAngle(const yaw_elements &yawAngData);
 
     // update mag field states and associated variances using magnetomer and declination data
     void resetMagFieldStates();
@@ -1344,6 +1345,9 @@ private:
     Vector3f extNavVelInnov;            // external nav velocity innovations
     Vector3f extNavVelVarInnov;         // external nav velocity innovation variances
     uint32_t extNavVelInnovTime_ms;     // system time that external nav velocity innovations were recorded (to detect timeouts)
+    EKF_obs_buffer_t<yaw_elements> storedExtNavYawAng;  // external navigation yaw angle buffer
+    yaw_elements extNavYawAngDataDelayed;   // external navigation yaw angle at the fusion time horizon
+    uint32_t last_extnav_yaw_fusion_ms; // system time that external nav yaw was last fused
 
     // flags indicating severe numerical errors in innovation variance calculation for different fusion operations
     struct {