AP_NavEKF: Use range finder for primary hgt ref in opt flow mode

Falls back to baro if range finder is unavailable
Adds parameter enabling user to select which height source (baro or range finder) will be used during optical flow nav.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -112,6 +112,9 @@ extern const AP_HAL::HAL& hal;
 #define INIT_GYRO_BIAS_UNCERTAINTY  0.1f
 #define INIT_ACCEL_BIAS_UNCERTAINTY 0.3f
 
+// altitude of OF and range finder when on ground
+#define RNG_MEAS_ON_GND 0.1f
+
 // Define tuning parameters
 const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
 
@@ -240,8 +243,8 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
     AP_GROUPINFO("POS_DELAY",    15, NavEKF, _msecPosDelay, 220),
 
     // @Param: GPS_TYPE
-    // @DisplayName: GPS velocity mode control
-    // @Description: This parameter controls use of GPS velocity measurements : 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity, 3 = use no GPS
+    // @DisplayName: GPS mode control
+    // @Description: This parameter controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = use no GPS (optical flow will be used if available)
     // @Range: 0 3
     // @Increment: 1
     // @User: Advanced
@@ -366,6 +369,13 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("FALLBACK",    31, NavEKF, _fallback, 1),
 
+    // @Param: ALT_SOURCE
+    // @DisplayName: Primary height source
+    // @Description: This parameter controls which height sensor is used by the EKF during optical flow navigation (when EKF_GPS_TYPE = 3). A value of will 0 cause it to always use baro altitude. A value of 1 will casue it to use range finder if available.
+    // @Values: 0:Use Baro, 1:Use Range Finder
+    // @User: Advanced
+    AP_GROUPINFO("ALT_SOURCE",    32, NavEKF, _altSource, 1),
+
     AP_GROUPEND
 };
 
@@ -509,7 +519,7 @@ void NavEKF::ResetHeight(void)
     for (uint8_t i=0; i<=49; i++){
         storedStates[i].position.z = -hgtMea;
     }
-    terrainState = states[9] + 0.1f;
+    terrainState = state.position.z + RNG_MEAS_ON_GND;
 }
 
 // this function is used to initialise the filter whilst moving, using the AHRS DCM solution
@@ -948,7 +958,7 @@ void NavEKF::SelectFlowFusion()
     } else if (flowDataValid && flowFusionTimeout && PV_AidingMode == AID_RELATIVE) {
         // we need to reset the velocities to a value estimated from the flow data
         // estimate the range
-        float initPredRng = max((terrainState - state.position[2]),0.1f) / Tnb_flow.c.z;
+        float initPredRng = max((terrainState - state.position[2]),RNG_MEAS_ON_GND) / Tnb_flow.c.z;
         // multiply the range by the LOS rates to get an estimated XY velocity in body frame
         Vector3f initVel;
         initVel.x = -flowRadXYcomp[1]*initPredRng;
@@ -967,6 +977,7 @@ void NavEKF::SelectFlowFusion()
         lastConstVelMode = false;
     }
     // if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
+    // we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
     if ((newDataFlow || newDataRng) && tiltOK) {
         // fuse range data into the terrain estimator if available
         fuseRngData = newDataRng;
@@ -1936,7 +1947,7 @@ void NavEKF::FuseVelPosNED()
         // if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
         // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
         // the accelerometers and we should disable the GPS and barometer innovation consistency checks.
-        if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtTime_ms) <  (2 * msecHgtAvg)) {
+        if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtMeasTime) <  (2 * msecHgtAvg)) {
             // calculate innovations for height and vertical GPS vel measurements
             float hgtErr  = statesAtHgtTime.position.z - observation[5];
             float velDErr = statesAtVelTime.velocity.z - observation[2];
@@ -2647,7 +2658,7 @@ void NavEKF::EstimateTerrainOffset()
         // fuse range finder data
         if (fuseRngData) {
             // predict range
-            float predRngMeas = max((terrainState - statesAtRngTime.position[2]),0.1f) / Tnb_flow.c.z;
+            float predRngMeas = max((terrainState - statesAtRngTime.position[2]),RNG_MEAS_ON_GND) / Tnb_flow.c.z;
 
             // Copy required states to local variable names
             float q0 = statesAtRngTime.quat[0]; // quaternion at optical flow measurement time
@@ -2666,7 +2677,7 @@ void NavEKF::EstimateTerrainOffset()
             varInnovRng = (R_RNG + Popt/sq(SK_RNG));
 
             // constrain terrain height to be below the vehicle
-            terrainState = max(terrainState, statesAtRngTime.position[2] + 0.1f);
+            terrainState = max(terrainState, statesAtRngTime.position[2] + RNG_MEAS_ON_GND);
 
             // Calculate the measurement innovation
             innovRng = predRngMeas - rngMea;
@@ -2681,7 +2692,7 @@ void NavEKF::EstimateTerrainOffset()
                 terrainState -= K_RNG * innovRng;
 
                 // constrain the state
-                terrainState = max(terrainState, statesAtRngTime.position[2] + 0.1f);
+                terrainState = max(terrainState, statesAtRngTime.position[2] + RNG_MEAS_ON_GND);
 
                 // correct the covariance
                 Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
@@ -2710,10 +2721,10 @@ void NavEKF::EstimateTerrainOffset()
             vel.z          = statesAtFlowTime.velocity[2];
 
             // predict range to centre of image
-            float flowRngPred = max((terrainState - statesAtFlowTime.position[2]),0.1f) / Tnb_flow.c.z;
+            float flowRngPred = max((terrainState - statesAtFlowTime.position[2]),RNG_MEAS_ON_GND) / Tnb_flow.c.z;
 
             // constrain terrain height to be below the vehicle
-            terrainState = max(terrainState, statesAtFlowTime.position[2] + 0.1f);
+            terrainState = max(terrainState, statesAtFlowTime.position[2] + RNG_MEAS_ON_GND);
 
             // calculate relative velocity in sensor frame
             relVelSensor = Tnb_flow*vel;
@@ -2775,7 +2786,7 @@ void NavEKF::EstimateTerrainOffset()
             terrainState -= K_OPT * auxFlowObsInnov;
 
             // constrain the state
-            terrainState = max(terrainState, statesAtFlowTime.position[2] + 0.1f);
+            terrainState = max(terrainState, statesAtFlowTime.position[2] + RNG_MEAS_ON_GND);
 
             // correct the covariance
             Popt = Popt - K_OPT * H_OPT * Popt;
@@ -2824,13 +2835,12 @@ void NavEKF::FuseOptFlow()
     velNED_local.y = ve - gpsVelGlitchOffset.y;
     velNED_local.z = vd;
 
-    // constrain terrain to be below vehicle
-    terrainState = max(terrainState, pd + 0.1f);
-    float heightAboveGndEst = terrainState - pd;
+    // constrain height above ground to be above range measured on ground
+    float heightAboveGndEst = max((terrainState - pd), RNG_MEAS_ON_GND);
     // Calculate observation jacobians and Kalman gains
     if (obsIndex == 0) {
         // calculate range from ground plain to centre of sensor fov assuming flat earth
-        float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),0.1f,1000.0f);
+        float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),RNG_MEAS_ON_GND,1000.0f);
 
         // calculate relative velocity in sensor frame
         relVelSensor = Tnb_flow*velNED_local;
@@ -3562,11 +3572,10 @@ bool NavEKF::getPosNED(Vector3f &pos) const
         pos.x = state.position.x;
         pos.y = state.position.y;
     }
-    // If relying on optical flow, then output ground relative position so that the vehicle does terain following
-    if (_fusionModeGPS == 3) {
-        pos.z = state.position.z - terrainState;
-    } else {
+    if (_fusionModeGPS != 3) {
         pos.z = state.position.z;
+    } else {
+        pos.z = state.position.z - terrainState;
     }
     return true;
 }
@@ -3618,7 +3627,7 @@ void NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScal
 {
     if (PV_AidingMode == AID_RELATIVE) {
         // allow 1.0 rad/sec margin for angular motion
-        ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * max((terrainState - state.position[2]), 0.1f);
+        ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * max((terrainState - state.position[2]), RNG_MEAS_ON_GND);
         // use standard gains up to 5.0 metres height and reduce above that
         ekfNavVelGainScaler = 4.0f / max((terrainState - state.position[2]),4.0f);
     } else {
@@ -3914,7 +3923,7 @@ void NavEKF::ConstrainStates()
     // position limit 1000 km - TODO apply circular limit
     for (uint8_t i=7; i<=8; i++) states[i] = constrain_float(states[i],-1.0e6f,1.0e6f);
     // height limit covers home alt on everest through to home alt at SL and ballon drop
-    states[9] = constrain_float(states[9],-4.0e4f,1.0e4f);
+    state.position.z = constrain_float(state.position.z,-4.0e4f,1.0e4f);
     // gyro bias limit ~6 deg/sec (this needs to be set based on manufacturers specs)
     for (uint8_t i=10; i<=12; i++) states[i] = constrain_float(states[i],-0.1f*dtIMUavg,0.1f*dtIMUavg);
     // Z accel bias limit 1.0 m/s^2	(this needs to be finalised from test data)
@@ -4075,18 +4084,33 @@ void NavEKF::readHgtData()
 {
     // check to see if baro measurement has changed so we know if a new measurement has arrived
     if (_baro.get_last_update() != lastHgtMeasTime) {
+        // Don't use Baro height if operating in optical flow mode as we use range finder instead
+        if (_fusionModeGPS == 3 && _altSource == 1) {
+            if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) {
+                // adjust range finder measurement to allow for effect of vehicle tilt and height of sensor
+                hgtMea = max(rngMea * Tnb_flow.c.z, RNG_MEAS_ON_GND);
+                // get states that were stored at the time closest to the measurement time, taking measurement delay into account
+                statesAtHgtTime = statesAtFlowTime;
+                // calculate offset to baro data that enables baro to be used as a backup
+                // filter offset to reduce effect of baro noise and other transient errors on estimate
+                baroHgtOffset = 0.2f * (_baro.get_altitude() + state.position.z) + 0.8f * baroHgtOffset;
+            } else {
+                // use baro measurement and correct for baro offset - failsafe use only as baro will drift
+                hgtMea = max(_baro.get_altitude() - baroHgtOffset, RNG_MEAS_ON_GND);
+                // get states that were stored at the time closest to the measurement time, taking measurement delay into account
+                RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay));
+            }
+        } else {
+            // use baro measurement and correct for baro offset
+            hgtMea = _baro.get_altitude() - baroHgtOffset;
+            // get states that were stored at the time closest to the measurement time, taking measurement delay into account
+            RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay));
+        }
+        // set flag to let other functions know new data has arrived
+        newDataHgt = true;
         // time stamp used to check for new measurement
         lastHgtMeasTime = _baro.get_last_update();
 
-        // time stamp used to check for timeout
-        lastHgtTime_ms = imuSampleTime_ms;
-
-        // get measurement and set flag to let other functions know new data has arrived
-        hgtMea = _baro.get_altitude();
-        newDataHgt = true;
-
-        // get states that wer stored at the time closest to the measurement time, taking measurement delay into account
-        RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay));
     } else {
         newDataHgt = false;
     }
@@ -4190,7 +4214,14 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
         statesAtRngTime = statesAtFlowTime;
         rngMea = rawSonarRange;
         newDataRng = true;
+        rngValidMeaTime_ms = imuSampleTime_ms;
+    } else if (!vehicleArmed) {
+        statesAtRngTime = statesAtFlowTime;
+        rngMea = RNG_MEAS_ON_GND;
+        newDataRng = true;
+        rngValidMeaTime_ms = imuSampleTime_ms;
     } else {
+        // set flag that will trigger fusion of height data
         newDataRng = false;
     }
 }
@@ -4387,7 +4418,6 @@ void NavEKF::InitialiseVariables()
     BETAmsecPrev = imuSampleTime_ms;
     lastMagUpdate = 0;
     lastHgtMeasTime = imuSampleTime_ms;
-    lastHgtTime_ms = 0;
     lastAirspeedUpdate = 0;
     velFailTime = imuSampleTime_ms;
     posFailTime = imuSampleTime_ms;
@@ -4399,6 +4429,7 @@ void NavEKF::InitialiseVariables()
     lastDecayTime_ms = imuSampleTime_ms;
     timeAtLastAuxEKF_ms = imuSampleTime_ms;
     flowValidMeaTime_ms = imuSampleTime_ms;
+    rngValidMeaTime_ms = imuSampleTime_ms;
     flowMeaTime_ms = 0;
     prevFlowFuseTime_ms = imuSampleTime_ms;
     gndHgtValidTime_ms = 0;
@@ -4482,6 +4513,7 @@ void NavEKF::InitialiseVariables()
     validOrigin = false;
     gndEffectMode = false;
     gpsSpdAccuracy = 0.0f;
+    baroHgtOffset = 0.0f;
 }
 
 // return true if we should use the airspeed sensor
@@ -4735,6 +4767,7 @@ void NavEKF::performArmingChecks()
         // Reset filter positon, height and velocity states on arming or disarming
         ResetVelocity();
         ResetPosition();
+        baroHgtOffset = 0.0f;
         ResetHeight();
         StoreStatesReset();
 

libraries/AP_NavEKF/AP_NavEKF.h

@@ -457,6 +457,7 @@ private:
     AP_Int8  _rngInnovGate;         // Number of standard deviations applied to range finder innovation consistency check
     AP_Float _maxFlowRate;          // Maximum flow rate magnitude that will be accepted by the filter
     AP_Int8 _fallback;              // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively.
+    AP_Int8 _altSource;             // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder.
 
     // Tuning parameters
     const float gpsNEVelVarAccScale;    // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
@@ -579,7 +580,6 @@ private:
     bool tasDataWaiting;            // true when new airspeed data is waiting to be fused
     bool newDataHgt;                // true when new height data has arrived
     uint32_t lastHgtMeasTime;       // time of last height measurement used to determine if new data has arrived
-    uint32_t lastHgtTime_ms;        // time of last height update (msec) used to calculate timeout
     uint16_t hgtRetryTime;          // time allowed without use of height measurements before a height timeout is declared
     uint32_t velFailTime;           // time stamp when GPS velocity measurement last failed covaraiance consistency check (msec)
     uint32_t posFailTime;           // time stamp when GPS position measurement last failed covaraiance consistency check (msec)
@@ -655,6 +655,7 @@ private:
     Vector2 flowRadXYcomp;         // motion compensated optical flow angular rates(rad/sec)
     Vector2 flowRadXY;             // raw (non motion compensated) optical flow angular rates (rad/sec)
     uint32_t flowValidMeaTime_ms;   // time stamp from latest valid flow measurement (msec)
+    uint32_t rngValidMeaTime_ms;    // time stamp from latest valid range measurement (msec)
     uint32_t flowMeaTime_ms;        // time stamp from latest flow measurement (msec)
     uint8_t flowQuality;            // unsigned integer representing quality of optical flow data. 255 is maximum quality.
     uint32_t gndHgtValidTime_ms;    // time stamp from last terrain offset state update (msec)
@@ -694,6 +695,7 @@ private:
     AidingMode PV_AidingMode;           // Defines the preferred mode for aiding of velocity and position estimates from the INS
     bool gndOffsetValid;            // true when the ground offset state can still be considered valid
     bool flowXfailed;               // true when the X optical flow measurement has failed the innovation consistency check
+    float baroHgtOffset;            // offset applied when baro height used as a backup height reference if range-finder fails
 
     bool haveDeltaAngles;