AP_NavEKF2: Rework selection of height measurements for fusion
GPS height has been added as a measurement option along with range finder and baro Selection of the height measurement source has been moved into a separate function Each height source is assigned its own measurement noise If GPS or baro alt is not able to be used, it reverts to baro When baro is not being used, an offset is continually calculated which enables a switch to baro without a height step.
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
80bc64ee7a
commit
04228e0b3b
@ -18,7 +18,7 @@ extern const AP_HAL::HAL& hal;
|
||||
********************************************************/
|
||||
|
||||
// Read the range finder and take new measurements if available
|
||||
// Read at 20Hz and apply a median filter
|
||||
// Apply a median filter
|
||||
void NavEKF2_core::readRangeFinder(void)
|
||||
{
|
||||
uint8_t midIndex;
|
||||
@ -26,19 +26,31 @@ void NavEKF2_core::readRangeFinder(void)
|
||||
uint8_t minIndex;
|
||||
// get theoretical correct range when the vehicle is on the ground
|
||||
rngOnGnd = frontend->_rng.ground_clearance_cm() * 0.01f;
|
||||
if (frontend->_rng.status() == RangeFinder::RangeFinder_Good && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
|
||||
// store samples and sample time into a ring buffer
|
||||
rngMeasIndex ++;
|
||||
if (rngMeasIndex > 2) {
|
||||
rngMeasIndex = 0;
|
||||
|
||||
// read range finder at 20Hz
|
||||
// TODO better way of knowing if it has new data
|
||||
if ((imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
|
||||
|
||||
// reset the timer used to control the measurement rate
|
||||
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;
|
||||
}
|
||||
storedRngMeasTime_ms[rngMeasIndex] = imuSampleTime_ms;
|
||||
storedRngMeas[rngMeasIndex] = frontend->_rng.distance_cm() * 0.01f;
|
||||
// check for three fresh samples and take median
|
||||
|
||||
// check for three fresh samples
|
||||
bool sampleFresh[3];
|
||||
for (uint8_t index = 0; index <= 2; index++) {
|
||||
sampleFresh[index] = (imuSampleTime_ms - storedRngMeasTime_ms[index]) < 500;
|
||||
}
|
||||
|
||||
// find the median value if we have three fresh samples
|
||||
if (sampleFresh[0] && sampleFresh[1] && sampleFresh[2]) {
|
||||
if (storedRngMeas[0] > storedRngMeas[1]) {
|
||||
minIndex = 1;
|
||||
@ -54,18 +66,20 @@ void NavEKF2_core::readRangeFinder(void)
|
||||
} else {
|
||||
midIndex = 2;
|
||||
}
|
||||
rngMea = max(storedRngMeas[midIndex],rngOnGnd);
|
||||
newDataRng = true;
|
||||
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;
|
||||
} else if (onGround) {
|
||||
// if on ground and no return, we assume on ground range
|
||||
rngMea = rngOnGnd;
|
||||
newDataRng = true;
|
||||
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
|
||||
StoreRange();
|
||||
} 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;
|
||||
} else {
|
||||
newDataRng = false;
|
||||
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
|
||||
StoreRange();
|
||||
}
|
||||
lastRngMeasTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
}
|
||||
|
||||
@ -119,7 +133,7 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
|
||||
// Save data to buffer
|
||||
StoreOF();
|
||||
// Check for data at the fusion time horizon
|
||||
newDataFlow = RecallOF();
|
||||
flowDataToFuse = RecallOF();
|
||||
}
|
||||
}
|
||||
|
||||
@ -243,6 +257,7 @@ void NavEKF2_core::readMagData()
|
||||
StoreMag();
|
||||
}
|
||||
}
|
||||
|
||||
// store magnetometer data in a history array
|
||||
void NavEKF2_core::StoreMag()
|
||||
{
|
||||
@ -526,6 +541,7 @@ void NavEKF2_core::readGpsData()
|
||||
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
|
||||
if (validOrigin) {
|
||||
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
|
||||
gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
|
||||
StoreGPS();
|
||||
// declare GPS available for use
|
||||
gpsNotAvailable = false;
|
||||
@ -652,54 +668,26 @@ bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
|
||||
* Height Measurements *
|
||||
********************************************************/
|
||||
|
||||
// check for new altitude measurement data and update stored measurement if available
|
||||
void NavEKF2_core::readHgtData()
|
||||
// check for new pressure altitude measurement data and update stored measurement if available
|
||||
void NavEKF2_core::readBaroData()
|
||||
{
|
||||
// check to see if baro measurement has changed so we know if a new measurement has arrived
|
||||
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
|
||||
if (frontend->_baro.get_last_update() - lastHgtReceived_ms > 70) {
|
||||
// Don't use Baro height if operating in optical flow mode as we use range finder instead
|
||||
if (frontend->_fusionModeGPS == 3 && frontend->_altSource == 1) {
|
||||
if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) {
|
||||
// adjust range finder measurement to allow for effect of vehicle tilt and height of sensor
|
||||
baroDataNew.hgt = max(rngMea * Tnb_flow.c.z, rngOnGnd);
|
||||
// calculate offset to baro data that enables baro to be used as a backup
|
||||
// filter offset to reduce effect of baro noise and other transient errors on estimate
|
||||
baroHgtOffset = 0.1f * (frontend->_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||||
} else if (isAiding && takeOffDetected) {
|
||||
// we have lost range finder measurements and are in optical flow flight
|
||||
// use baro measurement and correct for baro offset - failsafe use only as baro will drift
|
||||
baroDataNew.hgt = max(frontend->_baro.get_altitude() - baroHgtOffset, rngOnGnd);
|
||||
} else {
|
||||
// If we are on ground and have no range finder reading, assume the nominal on-ground height
|
||||
baroDataNew.hgt = rngOnGnd;
|
||||
// calculate offset to baro data that enables baro to be used as a backup
|
||||
// filter offset to reduce effect of baro noise and other transient errors on estimate
|
||||
baroHgtOffset = 0.1f * (frontend->_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||||
}
|
||||
} else {
|
||||
// Normal operation is to use baro measurement
|
||||
baroDataNew.hgt = frontend->_baro.get_altitude();
|
||||
}
|
||||
if (frontend->_baro.get_last_update() - lastBaroReceived_ms > 70) {
|
||||
|
||||
// filtered baro data used to provide a reference for takeoff
|
||||
// it is is reset to last height measurement on disarming in performArmingChecks()
|
||||
if (!getTakeoffExpected()) {
|
||||
const float gndHgtFiltTC = 0.5f;
|
||||
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
|
||||
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
||||
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
||||
} else if (isAiding && getTakeoffExpected()) {
|
||||
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
||||
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
|
||||
baroDataNew.hgt = frontend->_baro.get_altitude();
|
||||
|
||||
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
||||
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
|
||||
if (isAiding && getTakeoffExpected()) {
|
||||
baroDataNew.hgt = max(baroDataNew.hgt, meaHgtAtTakeOff);
|
||||
}
|
||||
|
||||
// time stamp used to check for new measurement
|
||||
lastHgtReceived_ms = frontend->_baro.get_last_update();
|
||||
lastBaroReceived_ms = frontend->_baro.get_last_update();
|
||||
|
||||
// estimate of time height measurement was taken, allowing for delays
|
||||
baroDataNew.time_ms = lastHgtReceived_ms - frontend->_hgtDelay_ms;
|
||||
baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;
|
||||
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
baroDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
@ -68,26 +68,22 @@ void NavEKF2_core::SelectFlowFusion()
|
||||
|
||||
// if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
|
||||
// we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
|
||||
if ((newDataFlow || newDataRng) && tiltOK) {
|
||||
// fuse range data into the terrain estimator if available
|
||||
fuseRngData = newDataRng;
|
||||
if ((flowDataToFuse || rangeDataToFuse) && tiltOK) {
|
||||
// fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
|
||||
fuseOptFlowData = (newDataFlow && !fuseRngData);
|
||||
fuseOptFlowData = (flowDataToFuse && !rangeDataToFuse);
|
||||
// Estimate the terrain offset (runs a one state EKF)
|
||||
EstimateTerrainOffset();
|
||||
// Indicate we have used the range data
|
||||
newDataRng = false;
|
||||
}
|
||||
|
||||
// Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode
|
||||
if (newDataFlow && tiltOK && PV_AidingMode == AID_RELATIVE)
|
||||
if (flowDataToFuse && tiltOK && PV_AidingMode == AID_RELATIVE)
|
||||
{
|
||||
// Set the flow noise used by the fusion processes
|
||||
R_LOS = sq(max(frontend->_flowNoise, 0.05f));
|
||||
// Fuse the optical flow X and Y axis data into the main filter sequentially
|
||||
FuseOptFlow();
|
||||
// reset flag to indicate that no new flow data is available for fusion
|
||||
newDataFlow = false;
|
||||
flowDataToFuse = false;
|
||||
}
|
||||
|
||||
// stop the performance timer
|
||||
@ -111,7 +107,7 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
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 (!fuseRngData && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f)) {
|
||||
if (!rangeDataToFuse && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f)) {
|
||||
inhibitGndState = true;
|
||||
} else {
|
||||
inhibitGndState = false;
|
||||
@ -132,7 +128,7 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
timeAtLastAuxEKF_ms = imuSampleTime_ms;
|
||||
|
||||
// fuse range finder data
|
||||
if (fuseRngData) {
|
||||
if (rangeDataToFuse) {
|
||||
// predict range
|
||||
float predRngMeas = max((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z;
|
||||
|
||||
@ -156,7 +152,7 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd);
|
||||
|
||||
// Calculate the measurement innovation
|
||||
innovRng = predRngMeas - rngMea;
|
||||
innovRng = predRngMeas - rangeDataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(frontend->_rngInnovGate) * varInnovRng);
|
||||
|
||||
@ -74,7 +74,7 @@ void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
|
||||
auxInnov = auxFlowObsInnov;
|
||||
HAGL = terrainState - stateStruct.position.z;
|
||||
rngInnov = innovRng;
|
||||
range = rngMea;
|
||||
range = rangeDataDelayed.rng;
|
||||
gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
|
||||
}
|
||||
|
||||
@ -87,6 +87,10 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
|
||||
if (frontend->_fusionModeGPS == 3) {
|
||||
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
|
||||
height = max(float(frontend->_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f);
|
||||
// If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin
|
||||
if (frontend->_altSource != 1) {
|
||||
height -= terrainState;
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
@ -443,6 +447,8 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const
|
||||
bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3);
|
||||
bool gpsNavPossible = !gpsNotAvailable && (PV_AidingMode == AID_ABSOLUTE) && gpsGoodToAlign;
|
||||
bool filterHealthy = healthy() && tiltAlignComplete && yawAlignComplete;
|
||||
// If GPS height useage is specified, height is considered to be inaccurate until the GPS passes all checks
|
||||
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
|
||||
|
||||
// set individual flags
|
||||
status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
|
||||
@ -450,7 +456,7 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const
|
||||
status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
|
||||
status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
|
||||
status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
|
||||
status.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid
|
||||
status.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
|
||||
status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
|
||||
status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
|
||||
status.flags.pred_horiz_pos_rel = (optFlowNavPossible || gpsNavPossible) && filterHealthy; // we should be able to estimate a relative position when we enter flight mode
|
||||
|
||||
@ -85,10 +85,8 @@ void NavEKF2_core::ResetPosition(void)
|
||||
// reset the vertical position state using the last height measurement
|
||||
void NavEKF2_core::ResetHeight(void)
|
||||
{
|
||||
// read the altimeter
|
||||
readHgtData();
|
||||
// write to the state vector
|
||||
stateStruct.position.z = -baroDataNew.hgt; // down position from blended accel data
|
||||
stateStruct.position.z = -hgtMea;
|
||||
terrainState = stateStruct.position.z + rngOnGnd;
|
||||
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
||||
storedOutput[i].position.z = stateStruct.position.z;
|
||||
@ -137,11 +135,12 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||
posVelFusionDelayed = false;
|
||||
}
|
||||
|
||||
// check for and read new GPS data
|
||||
// read GPS data from the sensor and check for new data in the buffer
|
||||
readGpsData();
|
||||
gpsDataToFuse = RecallGPS();
|
||||
|
||||
// Determine if we need to fuse position and velocity data on this time step
|
||||
if (RecallGPS() && PV_AidingMode == AID_ABSOLUTE) {
|
||||
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
|
||||
// Don't fuse velocity data if GPS doesn't support it
|
||||
if (frontend->_fusionModeGPS <= 1) {
|
||||
fuseVelData = true;
|
||||
@ -154,22 +153,8 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||
fusePosData = false;
|
||||
}
|
||||
|
||||
// check for and read new height data
|
||||
readHgtData();
|
||||
|
||||
// If we haven't received height data for a while, then declare the height data as being timed out
|
||||
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
|
||||
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
|
||||
if (imuSampleTime_ms - lastHgtReceived_ms > hgtRetryTime_ms) {
|
||||
hgtTimeout = true;
|
||||
}
|
||||
|
||||
// command fusion of height data
|
||||
// wait until the EKF time horizon catches up with the measurement
|
||||
if (RecallBaro()) {
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
}
|
||||
// Select height data to be fused from the available baro, range finder and GPS sources
|
||||
selectHeightForFusion();
|
||||
|
||||
// If we are operating without any aiding, fuse in the last known position and zero velocity
|
||||
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
|
||||
@ -242,7 +227,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
observation[2] = gpsDataDelayed.vel.z;
|
||||
observation[3] = gpsDataDelayed.pos.x;
|
||||
observation[4] = gpsDataDelayed.pos.y;
|
||||
observation[5] = -baroDataDelayed.hgt;
|
||||
observation[5] = -hgtMea;
|
||||
|
||||
// calculate additional error in GPS position caused by manoeuvring
|
||||
posErr = frontend->gpsPosVarAccScale * accNavMag;
|
||||
@ -274,12 +259,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
||||
R_OBS[4] = R_OBS[3];
|
||||
}
|
||||
R_OBS[5] = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
|
||||
|
||||
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
|
||||
if (getTakeoffExpected() || getTouchdownExpected()) {
|
||||
R_OBS[5] *= frontend->gndEffectBaroScaler;
|
||||
}
|
||||
R_OBS[5] = posDownObsNoise;
|
||||
|
||||
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
|
||||
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
|
||||
@ -287,11 +267,10 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
||||
for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
||||
|
||||
|
||||
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
|
||||
// if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter
|
||||
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
|
||||
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
|
||||
if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtReceived_ms) < (2 * frontend->hgtAvg_ms)) {
|
||||
if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
|
||||
// calculate innovations for height and vertical GPS vel measurements
|
||||
float hgtErr = stateStruct.position.z - observation[5];
|
||||
float velDErr = stateStruct.velocity.z - observation[2];
|
||||
@ -403,9 +382,8 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend->_hgtInnovGate) * varInnovVelPos[5]);
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
|
||||
hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime_ms;
|
||||
// Fuse height data if healthy or timed out or in constant position mode
|
||||
if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE)) {
|
||||
if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) {
|
||||
// Calculate a filtered value to be used by pre-flight health checks
|
||||
// We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
|
||||
if (onGround) {
|
||||
@ -416,16 +394,16 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
} else {
|
||||
hgtInnovFiltState = 0.0f;
|
||||
}
|
||||
hgtHealth = true;
|
||||
lastHgtPassTime_ms = imuSampleTime_ms;
|
||||
// if timed out, reset the height, but do not fuse data on this time step
|
||||
|
||||
// if timed out, reset the height
|
||||
if (hgtTimeout) {
|
||||
ResetHeight();
|
||||
fuseHgtData = false;
|
||||
hgtTimeout = false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
hgtHealth = false;
|
||||
|
||||
// If we have got this far then declare the height data as healthy and reset the timeout counter
|
||||
hgtHealth = true;
|
||||
lastHgtPassTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
}
|
||||
|
||||
@ -461,23 +439,21 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
else if (obsIndex == 3 || obsIndex == 4) {
|
||||
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
||||
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
||||
} else {
|
||||
} else if (obsIndex == 5) {
|
||||
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
||||
if (obsIndex == 5) {
|
||||
const float gndMaxBaroErr = 4.0f;
|
||||
const float gndBaroInnovFloor = -0.5f;
|
||||
const float gndMaxBaroErr = 4.0f;
|
||||
const float gndBaroInnovFloor = -0.5f;
|
||||
|
||||
if(getTouchdownExpected()) {
|
||||
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
|
||||
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
|
||||
// this function looks like this:
|
||||
// |/
|
||||
//---------|---------
|
||||
// ____/|
|
||||
// / |
|
||||
// / |
|
||||
innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
|
||||
}
|
||||
if(getTouchdownExpected()) {
|
||||
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
|
||||
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
|
||||
// this function looks like this:
|
||||
// |/
|
||||
//---------|---------
|
||||
// ____/|
|
||||
// / |
|
||||
// / |
|
||||
innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
|
||||
}
|
||||
}
|
||||
|
||||
@ -555,4 +531,83 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
* MISC FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
// select the height measurement to be fused from the available baro, range finder and GPS sources
|
||||
void NavEKF2_core::selectHeightForFusion()
|
||||
{
|
||||
// Read range finder data and check for new data in the buffer
|
||||
// This data is used by both height and optical flow fusion processing
|
||||
readRangeFinder();
|
||||
rangeDataToFuse = RecallRange();
|
||||
|
||||
// read baro height data from the sensor and check for new data in the buffer
|
||||
readBaroData();
|
||||
baroDataToFuse = RecallBaro();
|
||||
|
||||
// determine if we should be using a height source other than baro
|
||||
bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500);
|
||||
bool usingGpsForHgt = (frontend->_altSource == 2 && imuSampleTime_ms - lastTimeGpsReceived_ms < 500);
|
||||
|
||||
// if there is new baro data to fuse, calculate filterred 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) {
|
||||
calcFiltBaroOffset();
|
||||
}
|
||||
// filtered baro data used to provide a reference for takeoff
|
||||
// it is is reset to last height measurement on disarming in performArmingChecks()
|
||||
if (!getTakeoffExpected()) {
|
||||
const float gndHgtFiltTC = 0.5f;
|
||||
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
|
||||
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
||||
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
||||
}
|
||||
}
|
||||
|
||||
// Select the height measurement source
|
||||
if (rangeDataToFuse && usingRangeForHgt) {
|
||||
// using range finder data
|
||||
// correct for tilt using a flat earth model
|
||||
if (prevTnb.c.z >= 0.7) {
|
||||
hgtMea = max(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
// set the observation noise
|
||||
posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
|
||||
} else {
|
||||
// disable fusion if tilted too far
|
||||
fuseHgtData = false;
|
||||
}
|
||||
} else if (gpsDataToFuse && usingGpsForHgt) {
|
||||
// using GPS data
|
||||
hgtMea = gpsDataDelayed.hgt;
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
// set the observation noise to the horizontal GPS noise plus a scaler becasue 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) {
|
||||
// using Baro data
|
||||
hgtMea = baroDataDelayed.hgt - baroHgtOffset;
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
// set the observation noise
|
||||
posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
|
||||
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
|
||||
if (getTakeoffExpected() || getTouchdownExpected()) {
|
||||
posDownObsNoise *= frontend->gndEffectBaroScaler;
|
||||
}
|
||||
} else {
|
||||
fuseHgtData = false;
|
||||
}
|
||||
|
||||
// If we haven't fused height data for a while, then declare the height data as being timed out
|
||||
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
|
||||
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
|
||||
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
|
||||
hgtTimeout = true;
|
||||
} else {
|
||||
hgtTimeout = false;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
|
||||
@ -298,7 +298,7 @@ void NavEKF2_core::detectFlight()
|
||||
}
|
||||
|
||||
// trigger if more than 10m away from initial height
|
||||
if (fabsf(baroDataDelayed.hgt) > 10.0f) {
|
||||
if (fabsf(hgtMea) > 10.0f) {
|
||||
largeHgtChange = true;
|
||||
}
|
||||
|
||||
@ -337,7 +337,7 @@ void NavEKF2_core::detectFlight()
|
||||
}
|
||||
|
||||
// If rangefinder has increased since exiting on-ground, then we definitely are flying
|
||||
if (!onGround && ((rngMea - rngAtStartOfFlight) > 0.5f)) {
|
||||
if (!onGround && ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f)) {
|
||||
inFlight = true;
|
||||
}
|
||||
|
||||
@ -352,7 +352,7 @@ void NavEKF2_core::detectFlight()
|
||||
// store vertical position at start of flight to use as a reference for ground relative checks
|
||||
posDownAtTakeoff = stateStruct.position.z;
|
||||
// store the range finder measurement which will be used as a reference to detect when we have taken off
|
||||
rngAtStartOfFlight = rngMea;
|
||||
rngAtStartOfFlight = rangeDataNew.rng;
|
||||
}
|
||||
|
||||
}
|
||||
@ -411,7 +411,7 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
|
||||
angRateVec = ins.get_gyro() - gyroBias;
|
||||
}
|
||||
|
||||
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rngAtStartOfFlight + 0.1f)));
|
||||
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f)));
|
||||
} else if (onGround) {
|
||||
// we are confidently on the ground so set the takeoff detected status to false
|
||||
takeOffDetected = false;
|
||||
|
||||
@ -103,7 +103,7 @@ void NavEKF2_core::InitialiseVariables()
|
||||
prevTasStep_ms = imuSampleTime_ms;
|
||||
prevBetaStep_ms = imuSampleTime_ms;
|
||||
lastMagUpdate_us = 0;
|
||||
lastHgtReceived_ms = imuSampleTime_ms;
|
||||
lastBaroReceived_ms = imuSampleTime_ms;
|
||||
lastVelPassTime_ms = imuSampleTime_ms;
|
||||
lastPosPassTime_ms = imuSampleTime_ms;
|
||||
lastHgtPassTime_ms = imuSampleTime_ms;
|
||||
@ -145,13 +145,12 @@ void NavEKF2_core::InitialiseVariables()
|
||||
memset(&nextP[0][0], 0, sizeof(nextP));
|
||||
memset(&processNoise[0], 0, sizeof(processNoise));
|
||||
flowDataValid = false;
|
||||
newDataRng = false;
|
||||
rangeDataToFuse = false;
|
||||
fuseOptFlowData = false;
|
||||
Popt = 0.0f;
|
||||
terrainState = 0.0f;
|
||||
prevPosN = stateStruct.position.x;
|
||||
prevPosE = stateStruct.position.y;
|
||||
fuseRngData = false;
|
||||
inhibitGndState = true;
|
||||
flowGyroBias.x = 0;
|
||||
flowGyroBias.y = 0;
|
||||
@ -187,6 +186,7 @@ void NavEKF2_core::InitialiseVariables()
|
||||
yawAlignComplete = false;
|
||||
stateIndexLim = 23;
|
||||
baroStoreIndex = 0;
|
||||
rangeStoreIndex = 0;
|
||||
magStoreIndex = 0;
|
||||
gpsStoreIndex = 0;
|
||||
tasStoreIndex = 0;
|
||||
@ -297,7 +297,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
||||
ResetPosition();
|
||||
|
||||
// read the barometer and set the height state
|
||||
readHgtData();
|
||||
readBaroData();
|
||||
ResetHeight();
|
||||
|
||||
// define Earth rotation vector in the NED navigation frame
|
||||
@ -409,9 +409,6 @@ void NavEKF2_core::UpdateFilter(bool predict)
|
||||
// Predict the covariance growth
|
||||
CovariancePrediction();
|
||||
|
||||
// Read range finder data which is used by both position and optical flow fusion
|
||||
readRangeFinder();
|
||||
|
||||
// Update states using magnetometer data
|
||||
SelectMagFusion();
|
||||
|
||||
|
||||
@ -505,7 +505,7 @@ private:
|
||||
void readGpsData();
|
||||
|
||||
// check for new altitude measurement data and update stored measurement if available
|
||||
void readHgtData();
|
||||
void readBaroData();
|
||||
|
||||
// check for new magnetometer data and update store measurements if available
|
||||
void readMagData();
|
||||
@ -632,6 +632,9 @@ private:
|
||||
// calculate a filtered offset between baro height measurement and EKF height estimate
|
||||
void calcFiltBaroOffset();
|
||||
|
||||
// Select height data to be fused from the available baro, range finder and GPS sources
|
||||
void selectHeightForFusion();
|
||||
|
||||
// Length of FIFO buffers used for non-IMU sensor data.
|
||||
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
|
||||
static const uint32_t OBS_BUFFER_LENGTH = 5;
|
||||
@ -699,7 +702,7 @@ private:
|
||||
uint32_t imuSampleTime_ms; // time that the last IMU value was taken
|
||||
bool newDataTas; // true when new airspeed data has arrived
|
||||
bool tasDataWaiting; // true when new airspeed data is waiting to be fused
|
||||
uint32_t lastHgtReceived_ms; // time last time we received height data
|
||||
uint32_t lastBaroReceived_ms; // time last time we received baro height data
|
||||
uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared
|
||||
uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
|
||||
uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec)
|
||||
@ -796,6 +799,7 @@ private:
|
||||
uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction
|
||||
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele
|
||||
uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
|
||||
float posDownObsNoise; // observationn noise on the vertical position used by the state and covariance update step (m)
|
||||
|
||||
// variables used to calulate a vertical velocity that is kinematically consistent with the verical position
|
||||
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
|
||||
@ -827,7 +831,7 @@ private:
|
||||
of_elements ofDataNew; // OF data at the current time horizon
|
||||
of_elements ofDataDelayed; // OF data at the fusion time horizon
|
||||
uint8_t ofStoreIndex; // OF data storage index
|
||||
bool newDataFlow; // true when new optical flow data has arrived
|
||||
bool flowDataToFuse; // true when optical flow data has is ready for fusion
|
||||
bool flowDataValid; // true while optical flow data is still fresh
|
||||
bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused
|
||||
float auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
|
||||
@ -847,10 +851,9 @@ private:
|
||||
float terrainState; // terrain position state (m)
|
||||
float prevPosN; // north position at last measurement
|
||||
float prevPosE; // east position at last measurement
|
||||
bool fuseRngData; // true when fusion of range data is demanded
|
||||
float varInnovRng; // range finder observation innovation variance (m^2)
|
||||
float innovRng; // range finder observation innovation (m)
|
||||
float rngMea; // range finder measurement (m)
|
||||
float hgtMea; // height measurement derived from either baro, gps or range finder data (m)
|
||||
bool inhibitGndState; // true when the terrain position state is to remain constant
|
||||
uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
|
||||
Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
@ -858,7 +861,9 @@ private:
|
||||
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
|
||||
float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
Vector2f flowGyroBias; // bias error of optical flow sensor gyro output
|
||||
bool newDataRng; // true when new valid range finder data has arrived.
|
||||
bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon.
|
||||
bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon.
|
||||
bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon.
|
||||
Vector2f heldVelNE; // velocity held when no aiding is available
|
||||
enum AidingMode {AID_ABSOLUTE=0, // GPS aiding is being used (optical flow may also be used) so position estimates are absolute.
|
||||
AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
|
||||
|
||||
Loading…
Reference in New Issue
Block a user