mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-07 08:23:56 -04:00
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.
This commit is contained in:
priseborough
Randy Mackay
parent
1586abab8d
commit
e6f36f04db
@ -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<num_cores; i++) {
|
||||
core[i].setTerrainHgtStable(val);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
return the filter fault status as a bitmasked integer
|
||||
0 = quaternions are NaN
|
||||
|
||||
@ -211,6 +211,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 for the specified instance
|
||||
An out of range instance (eg -1) returns data for the the primary instance
|
||||
@ -330,6 +335,7 @@ private:
|
||||
AP_Float _yawNoise; // magnetic yaw measurement noise : rad
|
||||
AP_Int16 _yawInnovGate; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
|
||||
AP_Int8 _tauVelPosOutput; // Time constant of output complementary filter : csec (centi-seconds)
|
||||
AP_Int8 _useRngSwHgt; // Maximum valid range of the range finder in metres
|
||||
|
||||
// Tuning parameters
|
||||
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
|
||||
|
||||
@ -36,50 +36,70 @@ void NavEKF2_core::readRangeFinder(void)
|
||||
lastRngMeasTime_ms = imuSampleTime_ms;
|
||||
|
||||
// store samples and sample time into a ring buffer if valid
|
||||
if (frontend->_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 *
|
||||
********************************************************/
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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<imu_buffer_length; i++) {
|
||||
storedOutput[i].position.z = stateStruct.position.z;
|
||||
}
|
||||
outputDataNew.position.z = stateStruct.position.z;
|
||||
outputDataDelayed.position.z = stateStruct.position.z;
|
||||
|
||||
// reset the corresponding covariances
|
||||
zeroRows(P,8,8);
|
||||
@ -139,15 +147,12 @@ void NavEKF2_core::ResetHeight(void)
|
||||
|
||||
}
|
||||
|
||||
// Reset the baro so that it reads zero at the current height
|
||||
// Reset the EKF height to zero
|
||||
// Adjust the EKf origin height so that the EKF height + origin height is the same as before
|
||||
// Zero the EKF height datum
|
||||
// Return true if the height datum reset has been performed
|
||||
// If using a range finder for height do not reset and return false
|
||||
bool NavEKF2_core::resetHeightDatum(void)
|
||||
{
|
||||
// if we are using a range finder for height, return false
|
||||
if (frontend->_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
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
@ -29,10 +29,6 @@ extern const AP_HAL::HAL& hal;
|
||||
NavEKF2_core::NavEKF2_core(void) :
|
||||
stateStruct(*reinterpret_cast<struct state_elements *>(&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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user