From e6f36f04dba5a5bbe46ac26ea73e1f7364b456d7 Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 12 Jul 2016 18:56:58 +1000 Subject: [PATCH] AP_NavEKF2: Enable automatic use of range finder height The EK2_RNG_USE_HGT parameter sets the height (expressed as a percentage of the maximum range of the range finder as set by the RNGFND_MAX_CM parameter) below which the range finder will be used as the primary height source when the vehicle is moving slowly. When using a height reference other than GPS, the height datum can drift due to air pressure changes if using baro, or due to terrain height changes if using range finder as the primary height source. To ensure that a consistent height datum is available when switching between altitude sources, the WGS-84 height estimate of the EKF's local positi norigin is updated using a single state Bayes estimator, If rngfinder or gps height data is lost whilst being used, there will be a fall-back to baro data. --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 24 ++- libraries/AP_NavEKF2/AP_NavEKF2.h | 6 + .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 156 +++++++++++++----- .../AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 22 ++- .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 103 +++++++++--- .../AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 9 + libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 25 ++- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 34 +++- 8 files changed, 292 insertions(+), 87 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index ca37330a1d..b6519e4781 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -201,7 +201,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Param: ALT_SOURCE // @DisplayName: Primary height source - // @Description: This parameter controls which height sensor is used by the EKF. If the selected optionn cannot be used, it will default to Baro as the primary height source. Setting 0 will use the baro altitude at all times. Setting 1 uses the range finder and is only available in combination with optical flow navigation (EK2_GPS_TYPE = 3). Setting 2 uses GPS. + // @Description: This parameter controls the primary height sensor used by the EKF. If the selected option cannot be used, it will default to Baro as the primary height source. Setting 0 will use the baro altitude at all times. Setting 1 uses the range finder and is only available in combination with optical flow navigation (EK2_GPS_TYPE = 3). Setting 2 uses GPS. NOTE - the EK2_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground. // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS // @User: Advanced AP_GROUPINFO("ALT_SOURCE", 9, NavEKF2, _altSource, 0), @@ -476,6 +476,15 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Units: gauss/s AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF2, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT), + // @Param: RNG_USE_HGT + // @DisplayName: Range finder switch height percentage + // @Description: The range finder will be used as the primary height source when below a specified percentage of the sensor maximum as set by the RNGFND_MAX_CM parameter. Set to -1 to prevent range finder use. + // @Range: -1 70 + // @Increment: 1 + // @User: Advanced + // @Units: % + AP_GROUPINFO("RNG_USE_HGT", 42, NavEKF2, _useRngSwHgt, -1), + AP_GROUPEND }; @@ -1025,6 +1034,19 @@ void NavEKF2::setTouchdownExpected(bool val) } } +// Set to true if the terrain underneath is stable enough to be used as a height reference +// in combination with a range finder. Set to false if the terrain underneath the vehicle +// cannot be used as a height reference +void NavEKF2::setTerrainHgtStable(bool val) +{ + if (core) { + for (uint8_t i=0; i_rng.status() == RangeFinder::RangeFinder_Good) { - rngMeasIndex ++; - if (rngMeasIndex > 2) { - rngMeasIndex = 0; - } - storedRngMeasTime_ms[rngMeasIndex] = imuSampleTime_ms - 25; - storedRngMeas[rngMeasIndex] = frontend->_rng.distance_cm() * 0.01f; - } + // use data from two range finders if available - // check for three fresh samples - bool sampleFresh[3]; - for (uint8_t index = 0; index <= 2; index++) { - sampleFresh[index] = (imuSampleTime_ms - storedRngMeasTime_ms[index]) < 500; - } + for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) { + if (frontend->_rng.status(sensorIndex) == RangeFinder::RangeFinder_Good) { + rngMeasIndex[sensorIndex] ++; + if (rngMeasIndex[sensorIndex] > 2) { + rngMeasIndex[sensorIndex] = 0; + } + storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; + storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = frontend->_rng.distance_cm(sensorIndex) * 0.01f; + } - // find the median value if we have three fresh samples - if (sampleFresh[0] && sampleFresh[1] && sampleFresh[2]) { - if (storedRngMeas[0] > storedRngMeas[1]) { - minIndex = 1; - maxIndex = 0; - } else { - maxIndex = 0; - minIndex = 1; + // check for three fresh samples + bool sampleFresh[2][3] = {}; + for (uint8_t index = 0; index <= 2; index++) { + sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500; } - if (storedRngMeas[2] > storedRngMeas[maxIndex]) { - midIndex = maxIndex; - } else if (storedRngMeas[2] < storedRngMeas[minIndex]) { - midIndex = minIndex; - } else { - midIndex = 2; + + // find the median value if we have three fresh samples + if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) { + if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) { + minIndex = 1; + maxIndex = 0; + } else { + maxIndex = 0; + minIndex = 1; + } + if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) { + midIndex = maxIndex; + } else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) { + midIndex = minIndex; + } else { + midIndex = 2; + } + + // don't allow time to go backwards + if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) { + rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex]; + } + + // limit the measured range to be no less than the on-ground range + rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd); + + // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it + storedRange.push(rangeDataNew); + + } else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) { + // before takeoff we assume on-ground range value if there is no data + rangeDataNew.time_ms = imuSampleTime_ms; + rangeDataNew.rng = rngOnGnd; + rangeDataNew.time_ms = imuSampleTime_ms; + + // don't allow time to go backwards + if (imuSampleTime_ms > rangeDataNew.time_ms) { + rangeDataNew.time_ms = imuSampleTime_ms; + } + + // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it + storedRange.push(rangeDataNew); + } - rangeDataNew.time_ms = storedRngMeasTime_ms[midIndex]; - // limit the measured range to be no less than the on-ground range - rangeDataNew.rng = MAX(storedRngMeas[midIndex],rngOnGnd); + rngValidMeaTime_ms = imuSampleTime_ms; - // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it - storedRange.push(rangeDataNew); - } else if (!takeOffDetected) { - // before takeoff we assume on-ground range value if there is no data - rangeDataNew.time_ms = imuSampleTime_ms; - rangeDataNew.rng = rngOnGnd; - rngValidMeaTime_ms = imuSampleTime_ms; - // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it - storedRange.push(rangeDataNew); + } } } @@ -111,7 +131,6 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa } // calculate rotation matrices at mid sample time for flow observations stateStruct.quat.rotation_matrix(Tbn_flow); - Tnb_flow = Tbn_flow.transposed(); // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data) if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) { // correct flow sensor rates for bias @@ -403,6 +422,14 @@ void NavEKF2_core::readGpsData() gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f); } + gpsHgtAccuracy *= (1.0f - alpha); + float gpsHgtAccRaw; + if (!_ahrs->get_gps().vertical_accuracy(gpsHgtAccRaw)) { + gpsHgtAccuracy = 0.0f; + } else { + gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw); + gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f); + } // check if we have enough GPS satellites and increase the gps noise scaler if we don't if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) { @@ -445,6 +472,9 @@ void NavEKF2_core::readGpsData() // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum' EKF_origin.alt = gpsloc.alt - baroDataNew.hgt; + // Set the uncertinty of the GPS origin height + ekfOriginHgtVar = sq(gpsHgtAccuracy); + } // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin @@ -518,13 +548,57 @@ void NavEKF2_core::readBaroData() // calculate filtered offset between baro height measurement and EKF height estimate // offset should be subtracted from baro measurement to match filter estimate -// offset is used to enable reversion to baro if alternate height data sources fail +// offset is used to enable reversion to baro from alternate height data source void NavEKF2_core::calcFiltBaroOffset() { // Apply a first order LPF with spike protection baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f); } +// calculate filtered offset between GPS height measurement and EKF height estimate +// offset should be subtracted from GPS measurement to match filter estimate +// offset is used to switch reversion to GPS from alternate height data source +void NavEKF2_core::calcFiltGpsHgtOffset() +{ + // Estimate the WGS-84 height of the EKF's origin using a Bayes filter + + // calculate the variance of our a-priori estimate of the ekf origin height + float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f); + if (activeHgtSource == HGT_SOURCE_BARO) { + // Use the baro drift rate + const float baroDriftRate = 0.05f; + ekfOriginHgtVar += sq(baroDriftRate * deltaTime); + } else if (activeHgtSource == HGT_SOURCE_RNG) { + // use the worse case expected terrain gradient and vehicle horizontal speed + const float maxTerrGrad = 0.25f; + ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime); + } else if (activeHgtSource == HGT_SOURCE_GPS) { + // by definition we are using GPS height as the EKF datum in this mode + // so cannot run this filter + return; + } + lastOriginHgtTime_ms = imuDataDelayed.time_ms; + + // calculate the observation variance assuming EKF error relative to datum is independant of GPS observation error + // when not using GPS as height source + float originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8]; + + // calculate the correction gain + float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar); + + // calculate the innovation + float innovation = - stateStruct.position.z - gpsDataDelayed.hgt; + + // check the innovation variance ratio + float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar); + + // correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma + if (ratio < 5.0f) { + EKF_origin.alt -= (int)(100.0f * gain * innovation); + ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f); + } +} + /******************************************************** * Air Speed Measurements * ********************************************************/ diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 5dac887b1b..8a2795a4fe 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -42,7 +42,7 @@ void NavEKF2_core::SelectFlowFusion() // check is the terrain offset estimate is still valid gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000); // Perform tilt check - bool tiltOK = (Tnb_flow.c.z > frontend->DCM33FlowMin); + bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin); // Constrain measurements to zero if we are on the ground if (frontend->_fusionModeGPS == 3 && !takeOffDetected) { ofDataDelayed.flowRadXYcomp.zero(); @@ -90,8 +90,12 @@ void NavEKF2_core::EstimateTerrainOffset() float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); float losRateSq = velHorizSq / sq(heightAboveGndEst); - // don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable - if (!rangeDataToFuse && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f)) { + // don't update terrain offset state if there is no range finder + // don't update terrain state if not generating enough LOS rate, or without GPS, as it is poorly observable + // don't update terrain state if we are using it as a height reference in the main filter + bool cantFuseFlowData = (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f); + if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == HGT_SOURCE_RNG)) { + // skip update inhibitGndState = true; } else { inhibitGndState = false; @@ -114,7 +118,7 @@ void NavEKF2_core::EstimateTerrainOffset() // fuse range finder data if (rangeDataToFuse) { // predict range - float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z; + float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z; // Copy required states to local variable names float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time @@ -159,7 +163,7 @@ void NavEKF2_core::EstimateTerrainOffset() } } - if (fuseOptFlowData) { + if (fuseOptFlowData && !cantFuseFlowData) { Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes float losPred; // predicted optical flow angular rate measurement @@ -171,13 +175,13 @@ void NavEKF2_core::EstimateTerrainOffset() float H_OPT; // predict range to centre of image - float flowRngPred = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z; + float flowRngPred = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z; // constrain terrain height to be below the vehicle terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); // calculate relative velocity in sensor frame - relVelSensor = Tnb_flow*stateStruct.velocity; + relVelSensor = prevTnb*stateStruct.velocity; // divide velocity by range, subtract body rates and apply scale factor to // get predicted sensed angular optical rates relative to X and Y sensor axes @@ -297,10 +301,10 @@ void NavEKF2_core::FuseOptFlow() // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first // calculate range from ground plain to centre of sensor fov assuming flat earth - float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),rngOnGnd,1000.0f); + float range = constrain_float((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f); // calculate relative velocity in sensor frame - relVelSensor = Tnb_flow*stateStruct.velocity; + relVelSensor = prevTnb*stateStruct.velocity; // divide velocity by range to get predicted angular LOS rates relative to X and Y axes losPred[0] = relVelSensor.y/range; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 0706b3b022..81b65bb45f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -103,12 +103,20 @@ void NavEKF2_core::ResetHeight(void) { // write to the state vector stateStruct.position.z = -hgtMea; - terrainState = stateStruct.position.z + rngOnGnd; + outputDataNew.position.z = stateStruct.position.z; + outputDataDelayed.position.z = stateStruct.position.z; + + // reset the terrain state height + if (onGround) { + // assume vehicle is sitting on the ground + terrainState = stateStruct.position.z + rngOnGnd; + } else { + // can make no assumption other than vehicle is not below ground level + terrainState = MAX(stateStruct.position.z + rngOnGnd , terrainState); + } for (uint8_t i=0; i_altSource == 1) { + if (activeHgtSource == HGT_SOURCE_RNG) { + // by definition the height dataum is at ground level so cannot perform the reset return false; } // record the old height estimate @@ -160,6 +165,8 @@ bool NavEKF2_core::resetHeightDatum(void) if (validOrigin) { EKF_origin.alt += oldHgt*100; } + // adjust the terrain state + terrainState += oldHgt; return true; } @@ -631,14 +638,57 @@ void NavEKF2_core::selectHeightForFusion() readBaroData(); baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms); - // determine if we should be using a height source other than baro - bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500 && frontend->_fusionModeGPS == 3); - bool usingGpsForHgt = (frontend->_altSource == 2 && imuSampleTime_ms - lastTimeGpsReceived_ms < 500 && validOrigin && gpsAccuracyGood); + // select height source + if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { + if (frontend->_altSource == 1) { + // always use range finder + activeHgtSource = HGT_SOURCE_RNG; + } else { + // determine if we are above or below the height switch region + float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm() * (float)frontend->_useRngSwHgt; + bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; + bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; - // if there is new baro data to fuse, calculate filterred baro data required by other processes + // If the terrain height is consistent and we are moving slowly, then it can be + // used as a height reference in combination with a range finder + float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y); + bool dontTrustTerrain = (horizSpeed > 2.0f) || !terrainHgtStable; + bool trustTerrain = (horizSpeed < 1.0f) && terrainHgtStable; + + /* + * Switch between range finder and primary height source using height above ground and speed thresholds with + * hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height + * which cannot be assumed if the vehicle is moving horizontally. + */ + if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) { + // cannot trust terrain or range finder so stop using range finder height + if (frontend->_altSource == 0) { + activeHgtSource = HGT_SOURCE_BARO; + } else if (frontend->_altSource == 2) { + activeHgtSource = HGT_SOURCE_GPS; + } + } else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG)) { + // reliable terrain and range finder so start using range finder height + activeHgtSource = HGT_SOURCE_RNG; + } + } + } else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) { + activeHgtSource = HGT_SOURCE_GPS; + } else { + activeHgtSource = HGT_SOURCE_BARO; + } + + // Use Baro alt as a fallback if we lose range finder or GPS + 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) { + activeHgtSource = HGT_SOURCE_BARO; + } + + // if there is new baro data to fuse, calculate filtered baro data required by other processes if (baroDataToFuse) { - // calculate offset to baro data that enables baro to be used as a backup if we are using other height sources - if (usingRangeForHgt || usingGpsForHgt) { + // calculate offset to baro data that enables us to switch to Baro height use during operation + if (activeHgtSource != HGT_SOURCE_BARO) { calcFiltBaroOffset(); } // filtered baro data used to provide a reference for takeoff @@ -651,12 +701,20 @@ void NavEKF2_core::selectHeightForFusion() } } + // calculate offset to GPS height data that enables us to switch to GPS height during operation + if (gpsDataToFuse && (activeHgtSource != HGT_SOURCE_GPS)) { + calcFiltGpsHgtOffset(); + } + // Select the height measurement source - if (rangeDataToFuse && usingRangeForHgt) { + if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) { // using range finder data // correct for tilt using a flat earth model if (prevTnb.c.z >= 0.7) { + // calculate height above ground hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); + // correct for terrain position relative to datum + hgtMea -= terrainState; // enable fusion fuseHgtData = true; // set the observation noise @@ -665,15 +723,18 @@ void NavEKF2_core::selectHeightForFusion() // disable fusion if tilted too far fuseHgtData = false; } - } else if (gpsDataToFuse && usingGpsForHgt) { + } else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) { // using GPS data hgtMea = gpsDataDelayed.hgt; // enable fusion fuseHgtData = true; - // set the observation noise to the horizontal GPS noise plus a scaler because GPS vertical position is usually less accurate - // TODO use VDOP/HDOP, reported accuracy or a separate parameter - posDownObsNoise = sq(constrain_float(frontend->_gpsHorizPosNoise * 1.5f, 0.1f, 10.0f)); - } else if (baroDataToFuse && !usingRangeForHgt && !usingGpsForHgt) { + // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio + if (gpsHgtAccuracy > 0.0f) { + posDownObsNoise = sq(constrain_float(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f)); + } else { + posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f)); + } + } else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) { // using Baro data hgtMea = baroDataDelayed.hgt - baroHgtOffset; // enable fusion diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 047196dd09..26ea211dfe 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -414,6 +414,15 @@ void NavEKF2_core::setTouchdownExpected(bool val) expectGndEffectTouchdown = val; } +// Set to true if the terrain underneath is stable enough to be used as a height reference +// in combination with a range finder. Set to false if the terrain underneath the vehicle +// cannot be used as a height reference +void NavEKF2_core::setTerrainHgtStable(bool val) +{ + terrainHgtStableSet_ms = imuSampleTime_ms; + terrainHgtStable = val; +} + // Detect takeoff for optical flow navigation void NavEKF2_core::detectOptFlowTakeoff(void) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 4a7ea44566..3160776271 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -29,10 +29,6 @@ extern const AP_HAL::HAL& hal; NavEKF2_core::NavEKF2_core(void) : stateStruct(*reinterpret_cast(&statesArray)), - //variables - lastRngMeasTime_ms(0), // time in msec that the last range measurement was taken - rngMeasIndex(0), // index into ringbuffer of current range measurement - _perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")), _perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_CovariancePrediction")), _perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseVelPosNED")), @@ -89,7 +85,7 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c if(!storedOF.init(OBS_BUFFER_LENGTH)) { return false; } - if(!storedRange.init(OBS_BUFFER_LENGTH)) { + if(!storedRange.init(2*OBS_BUFFER_LENGTH)) { return false; } if(!storedIMU.init(imu_buffer_length)) { @@ -144,6 +140,8 @@ void NavEKF2_core::InitialiseVariables() lastPreAlignGpsCheckTime_ms = imuSampleTime_ms; lastPosReset_ms = 0; lastVelReset_ms = 0; + lastRngMeasTime_ms = 0; + terrainHgtStableSet_ms = 0; // initialise other variables gpsNoiseScaler = 1.0f; @@ -171,7 +169,7 @@ void NavEKF2_core::InitialiseVariables() terrainState = 0.0f; prevPosN = stateStruct.position.x; prevPosE = stateStruct.position.y; - inhibitGndState = true; + inhibitGndState = false; flowGyroBias.x = 0; flowGyroBias.y = 0; heldVelNE.zero(); @@ -198,6 +196,7 @@ void NavEKF2_core::InitialiseVariables() expectGndEffectTouchdown = false; gpsSpdAccuracy = 0.0f; gpsPosAccuracy = 0.0f; + gpsHgtAccuracy = 0.0f; baroHgtOffset = 0.0f; yawResetAngle = 0.0f; lastYawReset_ms = 0; @@ -264,6 +263,12 @@ void NavEKF2_core::InitialiseVariables() delAngBiasLearned = false; memset(&filterStatus, 0, sizeof(filterStatus)); gpsInhibit = false; + activeHgtSource = 0; + memset(&rngMeasIndex, 0, sizeof(rngMeasIndex)); + memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms)); + memset(&storedRngMeas, 0, sizeof(storedRngMeas)); + terrainHgtStable = true; + ekfOriginHgtVar = 0.0f; // zero data buffers storedIMU.reset(); @@ -1331,8 +1336,12 @@ void NavEKF2_core::ConstrainStates() for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_float(statesArray[i],-0.5f,0.5f); // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_float(statesArray[i],-100.0f,100.0f); - // constrain the terrain offset state - terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); + // constrain the terrain or vertical position state state depending on whether we are using the ground as the height reference + if (inhibitGndState) { + stateStruct.position.z = MIN(stateStruct.position.z, terrainState - rngOnGnd); + } else { + terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); + } } // calculate the NED earth spin vector in rad/sec diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 26078f776d..507c249eb6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -40,6 +40,11 @@ #define MASK_GPS_VERT_SPD (1<<6) #define MASK_GPS_HORIZ_SPD (1<<7) +// active height source +#define HGT_SOURCE_BARO 0 +#define HGT_SOURCE_RNG 1 +#define HGT_SOURCE_GPS 2 + class AP_AHRS; class NavEKF2_core @@ -196,6 +201,11 @@ public: // causes the EKF to compensate for expected barometer errors due to ground effect void setTouchdownExpected(bool val); + // Set to true if the terrain underneath is stable enough to be used as a height reference + // in combination with a range finder. Set to false if the terrain underneath the vehicle + // cannot be used as a height reference + void setTerrainHgtStable(bool val); + /* return the filter fault status as a bitmasked integer 0 = quaternions are NaN @@ -624,6 +634,9 @@ private: // calculate a filtered offset between baro height measurement and EKF height estimate void calcFiltBaroOffset(); + // calculate a filtered offset between GPS height measurement and EKF height estimate + void calcFiltGpsHgtOffset(); + // Select height data to be fused from the available baro, range finder and GPS sources void selectHeightForFusion(); @@ -737,6 +750,7 @@ private: bool validOrigin; // true when the EKF origin is valid float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver + float gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad float posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m) @@ -809,6 +823,8 @@ private: Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2) bool delAngBiasLearned; // true when the gyro bias has been learned nav_filter_status filterStatus; // contains the status of various filter outputs + float ekfOriginHgtVar; // Variance of the the EKF WGS-84 origin height estimate (m^2) + uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected Vector3f outputTrackError; @@ -854,7 +870,6 @@ private: uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec) Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period - Matrix3f Tnb_flow; // transformation matrix from nav to body axes at the middle of the optical flow sample period Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2 Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec) @@ -889,12 +904,17 @@ private: float delTimeOF; // time that delAngBodyOF is summed across // Range finder - float baroHgtOffset; // offset applied when baro height used as a backup height reference if range-finder fails - float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground - float storedRngMeas[3]; // Ringbuffer of stored range measurements - uint32_t storedRngMeasTime_ms[3]; // Ringbuffer of stored range measurement times - uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement - uint8_t rngMeasIndex; // Current range measurement ringbuffer index + float baroHgtOffset; // offset applied when when switching to use of Baro height + float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground + float storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors + uint32_t storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors + uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement + uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors + bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference + uint32_t terrainHgtStableSet_ms; // system time at which terrainHgtStable was set + + // height source selection logic + uint8_t activeHgtSource; // integer defining active height source // Movement detector bool takeOffDetected; // true when takeoff for optical flow navigation has been detected