AP_NavEKF3: Allow range beacon use with intermittent GPS

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -182,15 +182,16 @@ void NavEKF3_core::setAidingMode()
     // Save the previous status so we can detect when it has changed
     PV_AidingModePrev = PV_AidingMode;
 
+    bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
+    bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
+    bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
+
     // Determine if we should change aiding mode
      if (PV_AidingMode == AID_NONE) {
         // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
         // and IMU gyro bias estimates have stabilised
-        bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
         // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
         // 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) {
             PV_AidingMode = AID_ABSOLUTE;
         } else if (optFlowDataPresent() && filterIsStable) {
@@ -201,9 +202,9 @@ void NavEKF3_core::setAidingMode()
          bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
          // Check if the fusion has timed out (flow measurements have been rejected for too long)
          bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
-         // Enable switch to absolute position mode if GPS is available
-         // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
-         if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
+         // Enable switch to absolute position mode if GPS or range beacon data is available
+         // If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
+         if(canUseGPS || canUseRangeBeacon) {
              PV_AidingMode = AID_ABSOLUTE;
          } else if (flowSensorTimeout || flowFusionTimeout) {
              PV_AidingMode = AID_NONE;
@@ -306,20 +307,24 @@ void NavEKF3_core::setAidingMode()
             // Reset the last valid flow fusion time
             prevFlowFuseTime_ms = imuSampleTime_ms;
         } else if (PV_AidingMode == AID_ABSOLUTE) {
-            bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit);
-            bool canUseRangeBeacon = readyToUseRangeBeacon();
-            // We have commenced aiding and GPS usage is allowed
             if (canUseGPS) {
+                // We are commencing aiding using GPS - this is the preferred method
+                posResetSource = GPS;
+                velResetSource = GPS;
                 GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
-            }
-            posTimeout = false;
-            velTimeout = false;
-            // We have commenced aiding and range beacon usage is allowed
-            if (canUseRangeBeacon) {
+            } else if (canUseRangeBeacon) {
+                // We are commencing aiding using range beacons
+                posResetSource = RNGBCN;
+                velResetSource = DEFAULT;
                 GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
                 GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
-                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosDownOffset);
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z);
             }
+
+            // clear timeout flags as a precaution to avoid triggering any additional transitions
+            posTimeout = false;
+            velTimeout = false;
+
             // reset the last fusion accepted times to prevent unwanted activation of timeout logic
             lastPosPassTime_ms = imuSampleTime_ms;
             lastVelPassTime_ms = imuSampleTime_ms;

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -178,8 +178,10 @@ void NavEKF3_core::realignYawGPS()
             // calculate new filter quaternion states from Euler angles
             stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
 
-            // reset the velocity and posiiton states as they will be inaccurate due to bad yaw
+            // reset the velocity and position states as they will be inaccurate due to bad yaw
+            velResetSource = GPS;
             ResetVelocity();
+            posResetSource = GPS;
             ResetPosition();
 
             // set the yaw angle variance to a larger value to reflect the uncertainty in yaw

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -755,6 +755,11 @@ void NavEKF3_core::readRngBcnData()
     // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
     rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms);
 
+    // Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
+    if (rngBcnDataToFuse) {
+        rngBcnDataDelayed.beacon_posNED += bcnPosOffsetNED;
+    }
+
 }
 
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -35,7 +35,7 @@ void NavEKF3_core::ResetVelocity(void)
         P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise);
     } else {
         // reset horizontal velocity states to the GPS velocity if available
-        if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
+        if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == DEFAULT) || velResetSource == GPS) {
             stateStruct.velocity.x  = gpsDataNew.vel.x;
             stateStruct.velocity.y  = gpsDataNew.vel.y;
             // set the variances using the reported GPS speed accuracy
@@ -69,6 +69,8 @@ void NavEKF3_core::ResetVelocity(void)
     // store the time of the reset
     lastVelReset_ms = imuSampleTime_ms;
 
+    // clear reset data source preference
+    velResetSource = DEFAULT;
 
 }
 
@@ -91,7 +93,7 @@ void NavEKF3_core::ResetPosition(void)
         P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise);
     } else  {
         // Use GPS data as first preference if fresh data is available
-        if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
+        if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == DEFAULT) || posResetSource == GPS) {
             // write to state vector and compensate for offset  between last GPS measurement and the EKF time horizon
             stateStruct.position.x = gpsDataNew.pos.x  + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
             stateStruct.position.y = gpsDataNew.pos.y  + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
@@ -100,7 +102,7 @@ void NavEKF3_core::ResetPosition(void)
             // clear the timeout flags and counters
             posTimeout = false;
             lastPosPassTime_ms = imuSampleTime_ms;
-        } else if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) {
+        } else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == DEFAULT) || posResetSource == RNGBCN) {
             // use the range beacon data as a second preference
             stateStruct.position.x = receiverPos.x;
             stateStruct.position.y = receiverPos.y;
@@ -128,6 +130,9 @@ void NavEKF3_core::ResetPosition(void)
     // store the time of the reset
     lastPosReset_ms = imuSampleTime_ms;
 
+    // clear reset source preference
+    posResetSource = DEFAULT;
+
 }
 
 // reset the vertical position state using the last height measurement

libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp

@@ -25,11 +25,27 @@ void NavEKF3_core::SelectRngBcnFusion()
     // Determine if we need to fuse range beacon data on this time step
     if (rngBcnDataToFuse) {
         if (PV_AidingMode == AID_ABSOLUTE) {
-            // Normal operating mode is to fuse the range data into the main filter
-            FuseRngBcn();
+            if (!filterStatus.flags.using_gps) {
+                if (!bcnOriginEstInit && rngBcnAlignmentCompleted) {
+                    bcnOriginEstInit = true;
+                    bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x;
+                    bcnPosOffsetNED.y = receiverPos.y - stateStruct.position.y;
+                }
+                // If we aren't using GPS, then the beacons are used as the primary means of position reference
+                FuseRngBcn();
+            } else {
+                // If we are using GPS, then GPS is the primary reference, but we continue to use the beacon data
+                // to calculate an indpendant position that is used to update the beacon position offset if we need to
+                // start using beacon data as the primary reference.
+                FuseRngBcnStatic();
+                // record that the beacon origin needs to be initialised
+                bcnOriginEstInit = false;
+            }
         } else {
-            // If we aren't able to use the data in the main filter, use a simple 3-state filter to estimte position only
+            // If we aren't able to use the data in the main filter, use a simple 3-state filter to estimate position only
             FuseRngBcnStatic();
+            // record that the beacon origin needs to be initialised
+            bcnOriginEstInit = false;
         }
     }
 }
@@ -53,7 +69,7 @@ void NavEKF3_core::FuseRngBcn()
         // calculate the vertical offset from EKF datum to beacon datum
         CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
     } else {
-        bcnPosDownOffset = 0.0f;
+        bcnPosOffsetNED.z = 0.0f;
     }
 
     // copy required states to local variable names
@@ -62,7 +78,7 @@ void NavEKF3_core::FuseRngBcn()
     pd = stateStruct.position.z;
     bcn_pn = rngBcnDataDelayed.beacon_posNED.x;
     bcn_pe = rngBcnDataDelayed.beacon_posNED.y;
-    bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosDownOffset;
+    bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z;
 
     // predicted range
     Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
@@ -313,7 +329,7 @@ void NavEKF3_core::FuseRngBcnStatic()
 
             } else {
                 // we are using the beacons as the primary height source, so don't modify their vertical position
-                bcnPosDownOffset = 0.0f;
+                bcnPosOffsetNED.z = 0.0f;
 
             }
         } else {
@@ -344,7 +360,7 @@ void NavEKF3_core::FuseRngBcnStatic()
                 bcnPosDownOffsetMin = stateStruct.position.z - receverPosDownMax;
             } else {
                 // We are using the beacons as the primary height reference, so don't modify their vertical position
-                bcnPosDownOffset = 0.0f;
+                bcnPosOffsetNED.z = 0.0f;
             }
         }
 
@@ -354,7 +370,7 @@ void NavEKF3_core::FuseRngBcnStatic()
         }
 
         // calculate the observation jacobian
-        float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosDownOffset;
+        float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z;
         float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
         float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
         float t5 = t2*t2;
@@ -405,14 +421,13 @@ void NavEKF3_core::FuseRngBcnStatic()
 
         // calculate range measurement innovation
         Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
-        deltaPosNED.z -= bcnPosDownOffset;
+        deltaPosNED.z -= bcnPosOffsetNED.z;
         innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
 
         // update the state
         receiverPos.x -= K_RNG[0] * innovRngBcn;
         receiverPos.y -= K_RNG[1] * innovRngBcn;
         receiverPos.z -= K_RNG[2] * innovRngBcn;
-        receiverPos.z = MAX(receiverPos.z, minBcnPosD + 1.2f);
 
         // calculate the covariance correction
         for (unsigned i = 0; i<=2; i++) {
@@ -571,10 +586,10 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
     // calculate the innovation for the main filter using the offset with the smallest innovation history
     // apply hysteresis to prevent rapid switching
     if (!usingMinHypothesis && OffsetMinInnovFilt < 0.8f * OffsetMaxInnovFilt) {
-        bcnPosDownOffset = bcnPosDownOffsetMin;
+        bcnPosOffsetNED.z = bcnPosDownOffsetMin;
         usingMinHypothesis = true;
     } else if (usingMinHypothesis && OffsetMaxInnovFilt < 0.8f * OffsetMinInnovFilt) {
-        bcnPosDownOffset = bcnPosDownOffsetMax;
+        bcnPosOffsetNED.z = bcnPosDownOffsetMax;
         usingMinHypothesis = false;
     }
 

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -265,6 +265,8 @@ void NavEKF3_core::InitialiseVariables()
     ekfOriginHgtVar = 0.0f;
     velOffsetNED.zero();
     posOffsetNED.zero();
+    posResetSource = DEFAULT;
+    velResetSource = DEFAULT;
 
     // range beacon fusion variables
     memset(&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
@@ -294,7 +296,6 @@ void NavEKF3_core::InitialiseVariables()
     N_beacons = 0;
     maxBcnPosD = 0.0f;
     minBcnPosD = 0.0f;
-    bcnPosDownOffset = 0.0f;
     bcnPosDownOffsetMax = 0.0f;
     bcnPosOffsetMaxVar = 0.0f;
     OffsetMaxInnovFilt = 0.0f;
@@ -303,6 +304,8 @@ void NavEKF3_core::InitialiseVariables()
     OffsetMinInnovFilt = 0.0f;
     rngBcnFuseDataReportIndex = 0;
     memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport));
+    bcnPosOffsetNED.zero();
+    bcnOriginEstInit = false;
 
     // zero data buffers
     storedIMU.reset();
@@ -588,6 +591,11 @@ void NavEKF3_core::UpdateStrapdownEquationsNED()
 
     // limit states to protect against divergence
     ConstrainStates();
+
+    // If main filter velocity states are valid, update the range beacon receiver position states
+    if (filterStatus.flags.horiz_vel) {
+        receiverPos += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
+    }
 }
 
 /*

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -468,7 +468,7 @@ private:
     // use range beaon measurements to calculate a static position
     void FuseRngBcnStatic();
 
-    // calculate the offset from EKF vetical position datum to the range beacon system datum
+    // calculate the offset from EKF vertical position datum to the range beacon system datum
     void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning);
 
     // fuse magnetometer measurements
@@ -894,6 +894,19 @@ private:
     Vector3f velOffsetNED;          // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
     Vector3f posOffsetNED;          // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
 
+    // Specify preferred source of data to be used for a state reset
+    enum resetDataSource {
+                    DEFAULT=0,      // Use best available data
+                    GPS=1,          // Use GPS if available
+                    RNGBCN=2,       // Use beacon range data if available
+                    FLOW=3,         // Use optical flow rates if available
+                    BARO=4,         // Use Baro height if available
+                    MAG=5,          // Use magnetometer data if available
+                    RNGFND=6        // Use rangefinder data if available
+                        };
+    resetDataSource posResetSource; // preferred soure of data for position reset
+    resetDataSource velResetSource; // preferred source of data for a velocity reset
+
     // variables used to calculate a vertical velocity that is kinematically consistent with the verical position
     float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
     float posDown;                  // Down position state used in calculation of posDownRate
@@ -1009,7 +1022,6 @@ private:
     uint8_t N_beacons;                  // Number of range beacons in use
     float maxBcnPosD;                   // maximum position of all beacons in the down direction (m)
     float minBcnPosD;                   // minimum position of all beacons in the down direction (m)
-    float bcnPosDownOffset;             // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
     bool usingMinHypothesis;            // true when the min beacob constellatio offset hyopthesis is being used
 
     float bcnPosDownOffsetMax;          // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
@@ -1020,6 +1032,9 @@ private:
     float bcnPosOffsetMinVar;           // Variance of the bcnPosDownOffsetMin state (m)
     float OffsetMinInnovFilt;           // Filtered magnitude of the range innovations using bcnPosOffsetLow
 
+    Vector3f bcnPosOffsetNED;           // NED position of the beacon origin in earth frame (m)
+    bool bcnOriginEstInit;              // True when the beacon origin has been initialised
+
     // Range Beacon Fusion Debug Reporting
     uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
     struct {