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