AP_NavEKF2: Enable fusion of external nav position data

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -1468,4 +1468,24 @@ void NavEKF2::getTimingStatistics(int8_t instance, struct ekf_timing &timing)
     }
 }
 
+/*
+ * Write position and quaternion data from an external navigation system
+ *
+ * pos        : XYZ position (m) in a RH navigation frame with the Z axis pointing down and XY axes horizontal. Frame must be aligned with NED if the magnetomer is being used for yaw.
+ * quat       : quaternion describing the the rotation from navigation frame to body frame
+ * posErr     : 1-sigma spherical position error (m)
+ * angErr     : 1-sigma spherical angle error (rad)
+ * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
+ * resetTime_ms : system time of the last position reset request (mSec)
+ *
+*/
+void NavEKF2::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
+{
+    if (core) {
+        for (uint8_t i=0; i<num_cores; i++) {
+            core[i].writeExtNavData(sensOffset, pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms);
+        }
+    }
+}
+
 #endif //HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -323,6 +323,19 @@ public:
     // get timing statistics structure
     void getTimingStatistics(int8_t instance, struct ekf_timing &timing);
 
+    /*
+     * Write position and quaternion data from an external navigation system
+     *
+     * pos        : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
+     * quat       : quaternion desribing the the rotation from navigation frame to body frame
+     * posErr     : 1-sigma spherical position error (m)
+     * angErr     : 1-sigma spherical angle error (rad)
+     * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
+     * resetTime_ms : system time of the last position reset request (mSec)
+     *
+    */
+    void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
+
 private:
     uint8_t num_cores; // number of allocated cores
     uint8_t primary;   // current primary core

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -169,7 +169,8 @@ void NavEKF2_core::setAidingMode()
         // GPS aiding is the preferred option unless excluded by the user
         bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
         bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
-        if(canUseGPS || canUseRangeBeacon) {
+        bool canUseExtNav = readyToUseExtNav();
+        if(canUseGPS || canUseRangeBeacon || canUseExtNav) {
             PV_AidingMode = AID_ABSOLUTE;
         } else if (optFlowDataPresent() && filterIsStable) {
             PV_AidingMode = AID_RELATIVE;
@@ -206,17 +207,17 @@ void NavEKF2_core::setAidingMode()
         bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
 
         // Check if GPS is being used
-        bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
+        bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
         bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
 
         // Check if attitude drift has been constrained by a measurement source
-        bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;
+        bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;
 
         // check if velocity drift has been constrained by a measurement source
         bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
 
         // check if position drift has been constrained by a measurement source
-        bool posAiding = gpsPosUsed || rngBcnUsed;
+        bool posAiding = posUsed || rngBcnUsed;
 
         // Check if the loss of attitude aiding has become critical
         bool attAidLossCritical = false;
@@ -300,6 +301,7 @@ void NavEKF2_core::setAidingMode()
         case AID_ABSOLUTE: {
             bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit);
             bool canUseRangeBeacon = readyToUseRangeBeacon();
+            bool canUseExtNav = readyToUseExtNav();
             // We have commenced aiding and GPS usage is allowed
             if (canUseGPS) {
                 gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
@@ -312,6 +314,18 @@ void NavEKF2_core::setAidingMode()
                 gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
                 gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
             }
+            // We have commenced aiding and external nav usage is allowed
+            if (canUseExtNav) {
+                gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index);
+                gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z);
+                // handle yaw reset as special case
+                extNavYawResetRequest = true;
+                controlMagYawReset();
+                // handle height reset as special case
+                hgtMea = -extNavDataDelayed.pos.z;
+                posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
+                ResetHeight();
+            }
             // reset the last fusion accepted times to prevent unwanted activation of timeout logic
             lastPosPassTime_ms = imuSampleTime_ms;
             lastVelPassTime_ms = imuSampleTime_ms;
@@ -385,6 +399,12 @@ bool NavEKF2_core::readyToUseRangeBeacon(void) const
     return tiltAlignComplete && yawAlignComplete && rngBcnGoodToAlign && rngBcnDataToFuse;
 }
 
+// return true if the filter to be ready to use external nav data
+bool NavEKF2_core::readyToUseExtNav(void) const
+{
+    return tiltAlignComplete && extNavDataToFuse;
+}
+
 // return true if we should use the compass
 bool NavEKF2_core::use_compass(void) const
 {
@@ -405,7 +425,7 @@ bool NavEKF2_core::assume_zero_sideslip(void) const
 // set the LLH location of the filters NED origin
 bool NavEKF2_core::setOriginLLH(const Location &loc)
 {
-    if (PV_AidingMode == AID_ABSOLUTE) {
+    if (PV_AidingMode == AID_ABSOLUTE && !extNavUsedForPos) {
         return false;
     }
     EKF_origin = loc;
@@ -500,7 +520,7 @@ void  NavEKF2_core::updateFilterStatus(void)
     filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
     filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
     filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
-    filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching
+    filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && !extNavUsedForPos; // The GPS is glitching
 }
 
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -87,23 +87,23 @@ void NavEKF2_core::controlMagYawReset()
             finalResetRequest; // the final reset when we have acheived enough height to be in stable magnetic field environment
 
     // Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
-    if (magYawResetRequest || magStateResetRequest) {
-
-        // get the euler angles from the current state estimate
-        Vector3f eulerAngles;
-        stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
-
-        // Use the Euler angles and magnetometer measurement to update the magnetic field states
-        // and get an updated quaternion
-        Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+    if (magYawResetRequest || magStateResetRequest || extNavYawResetRequest) {
 
         // if a yaw reset has been requested, apply the updated quaternion to the current state
-        if (magYawResetRequest) {
+        if (extNavYawResetRequest) {
+            // get the euler angles from the current state estimate
+            Vector3f eulerAnglesOld;
+            stateStruct.quat.to_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesOld.z);
+
             // previous value used to calculate a reset delta
             Quaternion prevQuat = stateStruct.quat;
 
-            // update the quaternion states using the new yaw angle
-            stateStruct.quat = newQuat;
+            // Get the Euler angles from the external vision data
+            Vector3f eulerAnglesNew;
+            extNavDataDelayed.quat.to_euler(eulerAnglesNew.x, eulerAnglesNew.y, eulerAnglesNew.z);
+
+            // the new quaternion uses the old roll/pitch and new yaw angle
+            stateStruct.quat.from_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesNew.z);
 
             // calculate the change in the quaternion state and apply it to the ouput history buffer
             prevQuat = stateStruct.quat/prevQuat;
@@ -111,27 +111,61 @@ void NavEKF2_core::controlMagYawReset()
 
             // send initial alignment status to console
             if (!yawAlignComplete) {
-                gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index);
+                gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u ext nav yaw alignment complete",(unsigned)imu_index);
             }
 
-            // send in-flight yaw alignment status to console
-            if (finalResetRequest) {
-                gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
-            } else if (interimResetRequest) {
-                gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
-            }
-
-            // update the yaw reset completed status
-            recordYawReset();
+            // record the reset as complete and also record the in-flight reset as complete to stop further resets when hight is gained
+            // in-flight reset is unnecessary because we do not need to consider groudn based magnetic anomaly effects
+            yawAlignComplete = true;
+            finalInflightYawInit = true;
 
             // clear the yaw reset request flag
-            magYawResetRequest = false;
+            extNavYawResetRequest = false;
+
+        } else if (magYawResetRequest || magStateResetRequest) {
+            // get the euler angles from the current state estimate
+            Vector3f eulerAngles;
+            stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+
+            // Use the Euler angles and magnetometer measurement to update the magnetic field states
+            // and get an updated quaternion
+            Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+
+            if (magYawResetRequest) {
+                // previous value used to calculate a reset delta
+                Quaternion prevQuat = stateStruct.quat;
+
+                // update the quaternion states using the new yaw angle
+                stateStruct.quat = newQuat;
+
+                // calculate the change in the quaternion state and apply it to the ouput history buffer
+                prevQuat = stateStruct.quat/prevQuat;
+                StoreQuatRotate(prevQuat);
+
+                // send initial alignment status to console
+                if (!yawAlignComplete) {
+                    gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index);
+                }
+
+                // send in-flight yaw alignment status to console
+                if (finalResetRequest) {
+                    gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
+                } else if (interimResetRequest) {
+                    gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
+                }
 
-            // clear the complete flags if an interim reset has been performed to allow subsequent
-            // and final reset to occur
-            if (interimResetRequest) {
-                finalInflightYawInit = false;
-                finalInflightMagInit = false;
+                // update the yaw reset completed status
+                recordYawReset();
+
+                // clear the yaw reset request flag
+                magYawResetRequest = false;
+
+                // clear the complete flags if an interim reset has been performed to allow subsequent
+                // and final reset to occur
+                if (interimResetRequest) {
+                    finalInflightYawInit = false;
+                    finalInflightMagInit = false;
+                }
             }
         }
     }
@@ -256,11 +290,10 @@ void NavEKF2_core::SelectMagFusion()
     // If we have no magnetometer and are on the ground, fuse in a synthetic heading measurement to prevent the
     // filter covariances from becoming badly conditioned
     if (!use_compass()) {
-        if (onGround && (imuSampleTime_ms - lastSynthYawTime_ms > 1000)) {
+        if (onGround && (imuSampleTime_ms - lastYawTime_ms > 1000)) {
             fuseEulerYaw();
             magTestRatio.zero();
             yawTestRatio = 0.0f;
-            lastSynthYawTime_ms = imuSampleTime_ms;
         }
     }
 
@@ -739,6 +772,7 @@ void NavEKF2_core::fuseEulerYaw()
     // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
     // determine if a 321 or 312 Euler sequence is best
     float predicted_yaw;
+    float measured_yaw;
     float H_YAW[3];
     Matrix3f Tbn_zeroYaw;
     if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
@@ -771,14 +805,23 @@ void NavEKF2_core::fuseEulerYaw()
         H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
         H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
 
-        // Get the 321 euler angles
+        // calculate predicted and measured yaw angle
         Vector3f euler321;
         stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
         predicted_yaw = euler321.z;
-
-        // set the yaw to zero and calculate the zero yaw rotation from body to earth frame
-        Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
-
+        if (use_compass() && yawAlignComplete && magStateInitComplete) {
+            // Use measured mag components rotated into earth frame to measure yaw
+            Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
+            Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
+            measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
+        } else if (extNavUsedForYaw) {
+            // Get the yaw angle  from the external vision data
+            extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
+            measured_yaw =  euler321.z;
+        } else {
+            // no data so use predicted to prevent unconstrained variance growth
+            measured_yaw = predicted_yaw;
+        }
     } else {
         // calculate observaton jacobian when we are observing a rotation in a 312 sequence
         float t2 = q0*q0;
@@ -809,25 +852,22 @@ void NavEKF2_core::fuseEulerYaw()
         H_YAW[1] = 0.0f;
         H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
 
-        // Get the 321 euler angles
+        // calculate predicted and measured yaw angle
         Vector3f euler312 = stateStruct.quat.to_vector312();
         predicted_yaw = euler312.z;
-
-        // set the yaw to zero and calculate the zero yaw rotation from body to earth frame
-        Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f);
-    }
-
-    // rotate measured mag components into earth frame
-    Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
-
-    // Use the difference between the horizontal projection and declination to give the measured yaw
-    // If we can't use compass data, set the  meaurement to the predicted
-    // to prevent uncontrolled variance growth whilst on ground without magnetometer
-    float measured_yaw;
-    if (use_compass() && yawAlignComplete && magStateInitComplete) {
-        measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
-    } else {
-        measured_yaw = predicted_yaw;
+        if (use_compass() && yawAlignComplete && magStateInitComplete) {
+            // Use measured mag components rotated into earth frame to measure yaw
+            Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f);
+            Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
+            measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
+        } else if (extNavUsedForYaw) {
+            // Get the yaw angle  from the external vision data
+            euler312 = extNavDataDelayed.quat.to_vector312();
+            measured_yaw =  euler312.z;
+        } else {
+            // no data so use predicted to prevent unconstrained variance growth
+            measured_yaw = predicted_yaw;
+        }
     }
 
     // Calculate the innovation
@@ -938,8 +978,10 @@ void NavEKF2_core::fuseEulerYaw()
         // is used to correct the estimated quaternion on the current time step
         stateStruct.quat.rotate(stateStruct.angErr);
 
-        // record fusion numerical health status
+        // record fusion event
         faultStatus.bad_yaw = false;
+        lastYawTime_ms = imuSampleTime_ms;
+
 
     } else {
         // record fusion numerical health status

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -803,4 +803,32 @@ void NavEKF2_core::getTimingStatistics(struct ekf_timing &_timing)
     memset(&timing, 0, sizeof(timing));
 }
 
+void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
+{
+    // limit update rate to maximum allowed by sensor buffers and fusion process
+    // don't try to write to buffer until the filter has been initialised
+    if ((timeStamp_ms - extNavMeasTime_ms) < 70) {
+        return;
+    } else {
+        extNavMeasTime_ms = timeStamp_ms;
+    }
+
+    if (resetTime_ms > extNavLastPosResetTime_ms) {
+        extNavDataNew.posReset = true;
+        extNavLastPosResetTime_ms = resetTime_ms;
+    } else {
+        extNavDataNew.posReset = false;
+    }
+
+    extNavDataNew.pos = pos;
+    extNavDataNew.quat = quat;
+    extNavDataNew.posErr = posErr;
+    extNavDataNew.angErr = angErr;
+    extNavDataNew.body_offset = &sensOffset;
+    extNavDataNew.time_ms = timeStamp_ms;
+
+    storedExtNav.push(extNavDataNew);
+
+}
+
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -101,8 +101,8 @@ bool NavEKF2_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
 // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
 bool NavEKF2_core::getHeightControlLimit(float &height) const
 {
-    // only ask for limiting if we are doing optical flow navigation
-    if (frontend->_fusionModeGPS == 3) {
+    // only ask for limiting if we are doing optical flow only navigation
+    if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
         // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
         height = MAX(float(frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f);
         // If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -111,6 +111,12 @@ void NavEKF2_core::ResetPosition(void)
             // clear the timeout flags and counters
             rngBcnTimeout = false;
             lastRngBcnPassTime_ms = imuSampleTime_ms;
+        } else if (imuSampleTime_ms - extNavDataDelayed.time_ms < 250) {
+            // use the range beacon data as a second preference
+            stateStruct.position.x = extNavDataDelayed.pos.x;
+            stateStruct.position.y = extNavDataDelayed.pos.y;
+            // set the variances from the beacon alignment filter
+            P[7][7] = P[6][6] = sq(extNavDataDelayed.posErr);
         }
     }
     for (uint8_t i=0; i<imu_buffer_length; i++) {
@@ -232,35 +238,78 @@ void NavEKF2_core::SelectVelPosFusion()
         posVelFusionDelayed = false;
     }
 
+    // Check for data at the fusion time horizon
+    extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
+
     // read GPS data from the sensor and check for new data in the buffer
     readGpsData();
     gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
     // Determine if we need to fuse position and velocity data on this time step
     if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
+        // set fusion request flags
+        if (frontend->_fusionModeGPS <= 1) {
+            fuseVelData = true;
+        } else {
+            fuseVelData = false;
+        }
+        fusePosData = true;
+        extNavUsedForPos = false;
+
         // correct GPS data for position offset of antenna phase centre relative to the IMU
         Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
         if (!posOffsetBody.is_zero()) {
+            // Don't fuse velocity data if GPS doesn't support it
             if (fuseVelData) {
                 // TODO use a filtered angular rate with a group delay that matches the GPS delay
                 Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
                 Vector3f velOffsetBody = angRate % posOffsetBody;
                 Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
-                gpsDataDelayed.vel -= velOffsetEarth;
+                gpsDataDelayed.vel.x -= velOffsetEarth.x;
+                gpsDataDelayed.vel.y -= velOffsetEarth.y;
+                gpsDataDelayed.vel.z -= velOffsetEarth.z;
             }
+
             Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
             gpsDataDelayed.pos.x -= posOffsetEarth.x;
             gpsDataDelayed.pos.y -= posOffsetEarth.y;
             gpsDataDelayed.hgt += posOffsetEarth.z;
         }
 
+        // copy corrected GPS data to observation vector
+        if (fuseVelData) {
+            velPosObs[0] = gpsDataDelayed.vel.x;
+            velPosObs[1] = gpsDataDelayed.vel.y;
+            velPosObs[2] = gpsDataDelayed.vel.z;
+        }
+        velPosObs[3] = gpsDataDelayed.pos.x;
+        velPosObs[4] = gpsDataDelayed.pos.y;
 
-        // Don't fuse velocity data if GPS doesn't support it
-        if (frontend->_fusionModeGPS <= 1) {
-            fuseVelData = true;
+    } else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
+        // This is a special case that uses and external nav system for position
+        extNavUsedForPos = true;
+        activeHgtSource = HGT_SOURCE_EV;
+        fuseVelData = false;
+        fuseHgtData = true;
+        fusePosData = true;
+        velPosObs[3] = extNavDataDelayed.pos.x;
+        velPosObs[4] = extNavDataDelayed.pos.y;
+        velPosObs[5] = extNavDataDelayed.pos.z;
+
+        // if compass is disabled, also use it for yaw
+        if (!use_compass()) {
+            extNavUsedForYaw = true;
+            if (!yawAlignComplete) {
+                extNavYawResetRequest = true;
+                magYawResetRequest = false;
+                gpsYawResetRequest = false;
+                controlMagYawReset();
+                finalInflightYawInit = true;
+            } else {
+                fuseEulerYaw();
+            }
         } else {
-            fuseVelData = false;
+            extNavUsedForYaw = false;
         }
-        fusePosData = true;
 
     } else {
         fuseVelData = false;
@@ -273,6 +322,7 @@ void NavEKF2_core::SelectVelPosFusion()
     }
 
     // Select height data to be fused from the available baro, range finder and GPS sources
+
     selectHeightForFusion();
 
     // if we are using GPS, check for a change in receiver and reset position and height
@@ -332,10 +382,8 @@ void NavEKF2_core::SelectVelPosFusion()
     // to constrain tilt drift. This assumes a non-manoeuvring vehicle
     // Do this to coincide with the height fusion
     if (fuseHgtData && PV_AidingMode == AID_NONE) {
-        gpsDataDelayed.vel.zero();
-        gpsDataDelayed.pos.x = lastKnownPositionNE.x;
-        gpsDataDelayed.pos.y = lastKnownPositionNE.y;
-
+        velPosObs[3] = lastKnownPositionNE.x;
+        velPosObs[4] = lastKnownPositionNE.y;
         fusePosData = true;
         fuseVelData = false;
     }
@@ -372,7 +420,6 @@ void NavEKF2_core::FuseVelPosNED()
     // declare variables used by state and covariance update calculations
     Vector6 R_OBS; // Measurement variances used for fusion
     Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
-    Vector6 observation;
     float SK;
 
     // perform sequential fusion of GPS measurements. This assumes that the
@@ -382,13 +429,6 @@ void NavEKF2_core::FuseVelPosNED()
     // so we might as well take advantage of the computational efficiencies
     // associated with sequential fusion
     if (fuseVelData || fusePosData || fuseHgtData) {
-        // form the observation vector
-        observation[0] = gpsDataDelayed.vel.x;
-        observation[1] = gpsDataDelayed.vel.y;
-        observation[2] = gpsDataDelayed.vel.z;
-        observation[3] = gpsDataDelayed.pos.x;
-        observation[4] = gpsDataDelayed.pos.y;
-        observation[5] = -hgtMea;
 
         // calculate additional error in GPS position caused by manoeuvring
         float posErr = frontend->gpsPosVarAccScale * accNavMag;
@@ -439,8 +479,8 @@ void NavEKF2_core::FuseVelPosNED()
         // the accelerometers and we should disable the GPS and barometer innovation consistency checks.
         if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
             // calculate innovations for height and vertical GPS vel measurements
-            float hgtErr  = stateStruct.position.z - observation[5];
-            float velDErr = stateStruct.velocity.z - observation[2];
+            float hgtErr  = stateStruct.position.z - velPosObs[5];
+            float velDErr = stateStruct.velocity.z - velPosObs[2];
             // check if they are the same sign and both more than 3-sigma out of bounds
             if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) {
                 badIMUdata = true;
@@ -453,8 +493,8 @@ void NavEKF2_core::FuseVelPosNED()
         // test position measurements
         if (fusePosData) {
             // test horizontal position measurements
-            innovVelPos[3] = stateStruct.position.x - observation[3];
-            innovVelPos[4] = stateStruct.position.y - observation[4];
+            innovVelPos[3] = stateStruct.position.x - velPosObs[3];
+            innovVelPos[4] = stateStruct.position.y - velPosObs[4];
             varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
             varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
             // apply an innovation consistency threshold test, but don't fail if bad IMU data
@@ -505,7 +545,7 @@ void NavEKF2_core::FuseVelPosNED()
                 // velocity states start at index 3
                 stateIndex   = i + 3;
                 // calculate innovations using blended and single IMU predicted states
-                velInnov[i]  = stateStruct.velocity[i] - observation[i]; // blended
+                velInnov[i]  = stateStruct.velocity[i] - velPosObs[i]; // blended
                 // calculate innovation variance
                 varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
                 // sum the innovation and innovation variances
@@ -539,7 +579,7 @@ void NavEKF2_core::FuseVelPosNED()
         // test height measurements
         if (fuseHgtData) {
             // calculate height innovations
-            innovVelPos[5] = stateStruct.position.z - observation[5];
+            innovVelPos[5] = stateStruct.position.z - velPosObs[5];
             varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
             // calculate the innovation consistency test ratio
             hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
@@ -595,14 +635,14 @@ void NavEKF2_core::FuseVelPosNED()
                 // adjust scaling on GPS measurement noise variances if not enough satellites
                 if (obsIndex <= 2)
                 {
-                    innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex];
+                    innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - velPosObs[obsIndex];
                     R_OBS[obsIndex] *= sq(gpsNoiseScaler);
                 }
                 else if (obsIndex == 3 || obsIndex == 4) {
-                    innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
+                    innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
                     R_OBS[obsIndex] *= sq(gpsNoiseScaler);
                 } else if (obsIndex == 5) {
-                    innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
+                    innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
                     const float gndMaxBaroErr = 4.0f;
                     const float gndBaroInnovFloor = -0.5f;
 
@@ -760,7 +800,10 @@ void NavEKF2_core::selectHeightForFusion()
     baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
 
     // select height source
-    if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
+    if (extNavUsedForPos) {
+        // always use external vision as the hight source if using for position.
+        activeHgtSource = HGT_SOURCE_EV;
+    } else if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
         if (frontend->_altSource == 1) {
             // always use range finder
             activeHgtSource = HGT_SOURCE_RNG;
@@ -803,10 +846,11 @@ void NavEKF2_core::selectHeightForFusion()
         activeHgtSource = HGT_SOURCE_BARO;
     }
 
-    // Use Baro alt as a fallback if we lose range finder or GPS
+    // Use Baro alt as a fallback if we lose range finder, GPS or external nav
     bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500));
     bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
-    if (lostRngHgt || lostGpsHgt) {
+    bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000));
+    if (lostRngHgt || lostGpsHgt || lostExtNavHgt) {
         activeHgtSource = HGT_SOURCE_BARO;
     }
 
@@ -837,7 +881,10 @@ void NavEKF2_core::selectHeightForFusion()
     }
 
     // Select the height measurement source
-    if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
+    if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EV)) {
+        hgtMea = -extNavDataDelayed.pos.z;
+        posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
+    } else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
         // using range finder data
         // correct for tilt using a flat earth model
         if (prevTnb.c.z >= 0.7) {
@@ -847,6 +894,7 @@ void NavEKF2_core::selectHeightForFusion()
             hgtMea -= terrainState;
             // enable fusion
             fuseHgtData = true;
+            velPosObs[5] = -hgtMea;
             // set the observation noise
             posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
             // add uncertainty created by terrain gradient and vehicle tilt
@@ -859,6 +907,7 @@ void NavEKF2_core::selectHeightForFusion()
         // using GPS data
         hgtMea = gpsDataDelayed.hgt;
         // enable fusion
+        velPosObs[5] = -hgtMea;
         fuseHgtData = true;
         // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
         if (gpsHgtAccuracy > 0.0f) {
@@ -870,6 +919,7 @@ void NavEKF2_core::selectHeightForFusion()
         // using Baro data
         hgtMea = baroDataDelayed.hgt - baroHgtOffset;
         // enable fusion
+        velPosObs[5] = -hgtMea;
         fuseHgtData = true;
         // set the observation noise
         posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -91,6 +91,9 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
     if(!storedRangeBeacon.init(imu_buffer_length)) {
         return false;
     }
+    if(!storedExtNav.init(OBS_BUFFER_LENGTH)) {
+        return false;
+    }
     if(!storedIMU.init(imu_buffer_length)) {
         return false;
     }
@@ -125,7 +128,7 @@ void NavEKF2_core::InitialiseVariables()
     lastPosPassTime_ms = 0;
     lastHgtPassTime_ms = 0;
     lastTasPassTime_ms = 0;
-    lastSynthYawTime_ms = imuSampleTime_ms;
+    lastYawTime_ms = imuSampleTime_ms;
     lastTimeGpsReceived_ms = 0;
     secondLastGpsTime_ms = 0;
     lastDecayTime_ms = imuSampleTime_ms;
@@ -277,6 +280,7 @@ void NavEKF2_core::InitialiseVariables()
     ekfGpsRefHgt = 0.0;
     velOffsetNED.zero();
     posOffsetNED.zero();
+    memset(&velPosObs, 0, sizeof(velPosObs));
 
     // range beacon fusion variables
     memset(&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
@@ -317,6 +321,16 @@ void NavEKF2_core::InitialiseVariables()
     memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport));
     last_gps_idx = 0;
 
+    // external nav data fusion
+    memset(&extNavDataNew, 0, sizeof(extNavDataNew));
+    memset(&extNavDataDelayed, 0, sizeof(extNavDataDelayed));
+    extNavDataToFuse = false;
+    extNavMeasTime_ms = 0;
+    extNavLastPosResetTime_ms = 0;
+    extNavUsedForYaw = false;
+    extNavUsedForPos = false;
+    extNavYawResetRequest = false;
+
     // zero data buffers
     storedIMU.reset();
     storedGPS.reset();
@@ -326,6 +340,7 @@ void NavEKF2_core::InitialiseVariables()
     storedRange.reset();
     storedOutput.reset();
     storedRangeBeacon.reset();
+    storedExtNav.reset();
 }
 
 // Initialise the states from accelerometer and magnetometer data (if present)

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -46,6 +46,7 @@
 #define HGT_SOURCE_RNG  1
 #define HGT_SOURCE_GPS  2
 #define HGT_SOURCE_BCN  3
+#define HGT_SOURCE_EV   4
 
 // target EKF update time step
 #define EKF_TARGET_DT 0.01f
@@ -306,6 +307,20 @@ public:
     // get timing statistics structure
     void getTimingStatistics(struct ekf_timing &timing);
     
+    /*
+     * Write position and quaternion data from an external navigation system
+     *
+     * sensOffset : position of the external navigatoin sensor in body frame (m)
+     * pos        : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
+     * quat       : quaternion desribing the the rotation from navigation frame to body frame
+     * posErr     : 1-sigma spherical position error (m)
+     * angErr     : 1-sigma spherical angle error (rad)
+     * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
+     * resetTime_ms : system time of the last position reset request (mSec)
+     *
+    */
+    void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
+
 private:
     // Reference to the global EKF frontend for parameters
     NavEKF2 *frontend;
@@ -441,6 +456,18 @@ private:
         const Vector3f *body_offset;// 5..7
     };
 
+    struct ext_nav_elements {
+        bool            frameIsNED; // true if the data is in a NED navigation frame
+        bool            unitsAreSI; // true if the data length units are scaled in metres
+        Vector3f        pos;        // XYZ position measured in a RH navigation frame (m)
+        Quaternion      quat;       // quaternion describing the rotation from navigation to body frame
+        float           posErr;     // spherical poition measurement error 1-std (m)
+        float           angErr;     // spherical angular measurement error 1-std (rad)
+        const Vector3f *body_offset;// pointer to XYZ position of the sensor in body frame (m)
+        uint32_t        time_ms;    // measurement timestamp (msec)
+        bool            posReset;   // true when the position measurement has been reset
+    };
+
     // update the navigation filter status
     void  updateFilterStatus(void);
 
@@ -611,6 +638,9 @@ private:
     // return true if the filter to be ready to use the beacon range measurements
     bool readyToUseRangeBeacon(void) const;
 
+    // return true if the filter to be ready to use external nav data
+    bool readyToUseExtNav(void) const;
+
     // Check for filter divergence
     void checkDivergence(void);
 
@@ -762,6 +792,7 @@ private:
     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
+    Vector6 velPosObs;              // observations for combined velocity and positon group of measurements (3x1 m , 3x1 m/s)
     bool fuseVelData;               // this boolean causes the velNED measurements to be fused
     bool fusePosData;               // this boolean causes the posNE measurements to be fused
     bool fuseHgtData;               // this boolean causes the hgtMea measurements to be fused
@@ -789,7 +820,7 @@ private:
     uint32_t secondLastGpsTime_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
     bool allMagSensorsFailed;       // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
-    uint32_t lastSynthYawTime_ms;   // time stamp when synthetic yaw measurement was last fused to maintain covariance health (msec)
+    uint32_t lastYawTime_ms;        // time stamp when yaw observation was last fused (msec)
     uint32_t ekfStartTime_ms;       // time the EKF was started (msec)
     Matrix24 nextP;                 // Predicted covariance matrix before addition of process noise to diagonals
     Vector24 processNoise;          // process noise added to diagonals of predicted covariance matrix
@@ -1052,6 +1083,17 @@ private:
     float yawInnovAtLastMagReset;   // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
     Quaternion quatAtLastMagReset;  // quaternion states last time the mag states were reset
 
+    // external navigation fusion
+    obs_ring_buffer_t<ext_nav_elements> storedExtNav; // external navigation data buffer
+    ext_nav_elements extNavDataNew;     // External nav data at the current time horizon
+    ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon
+    uint32_t extNavMeasTime_ms;         // time external measurements were accepted for input to the data buffer (msec)
+    uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec)
+    bool extNavDataToFuse;              // true when there is new external nav data to fuse
+    bool extNavUsedForYaw;              // true when the external nav data is also being used as a yaw observation
+    bool extNavUsedForPos;              // true when the external nav data is being used as a position reference.
+    bool extNavYawResetRequest;         // true when a reset of vehicle yaw using the external nav data is requested
+
     // flags indicating severe numerical errors in innovation variance calculation for different fusion operations
     struct {
         bool bad_xmag:1;