Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Improve takeoff with optical flow and range data loss 

Range finder and optical flow data can drop-out and be reliable very close to ground. these patches enable the takeoff to be more relaibly detected and constrain optical flow navigation drift in the first part of takeoff.
This commit is contained in:
Paul Riseborough 2015-04-17 19:31:00 +10:00 committed by Randy Mackay
parent 3e061b174e
commit e48171ab11
3 changed files with 85 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -979,38 +979,34 @@ void NavEKF::SelectFlowFusion()
// Perform Data Checks
// Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// check is the terrain offset estimate is still valid
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
// Perform tilt check
bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin);
// if we don't have valid flow measurements and are relying on them, dead reckon using current velocity vector
// If the flow measurements have been rejected for too long and we are relying on them, then reset the velocities to an estimate based on the flow and range data
if (!flowDataValid && PV_AidingMode == AID_RELATIVE) {
constVelMode = true;
constPosMode = false; // always clear constant position mode if constant velocity mode is active
} else if (flowDataValid && flowFusionTimeout && PV_AidingMode == AID_RELATIVE) {
// we need to reset the velocities to a value estimated from the flow data
// estimate the range
float initPredRng = max((terrainState - state.position[2]),RNG_MEAS_ON_GND) / Tnb_flow.c.z;
// multiply the range by the LOS rates to get an estimated XY velocity in body frame
Vector3f initVel;
initVel.x = -flowRadXYcomp[1]*initPredRng;
initVel.y = flowRadXYcomp[0]*initPredRng;
// rotate into earth frame
initVel = Tbn_flow*initVel;
// set horizontal velocity states
state.velocity.x = initVel.x;
state.velocity.y = initVel.y;
// clear any hold modes
constVelMode = false;
lastConstVelMode = false;
} else if (flowDataValid) {
// clear the constant velocity mode now we have good data
constVelMode = false;
lastConstVelMode = false;
// Constrain measurements to zero if we are using optical flow and are on the ground
if (_fusionModeGPS == 3 && !takeOffDetected && vehicleArmed) {
flowRadXYcomp[0] = 0.0f;
flowRadXYcomp[1] = 0.0f;
flowRadXY[0] = 0.0f;
flowRadXY[1] = 0.0f;
omegaAcrossFlowTime.zero();
}
// If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
constVelMode = false; // always clear constant velocity mode if constant velocity mode is active
constPosMode = true;
PV_AidingMode = AID_NONE;
// reset the velocity
ResetVelocity();
// store the current position to be used to keep reporting the last known position
lastKnownPositionNE.x = state.position.x;
lastKnownPositionNE.y = state.position.y;
// reset the position
ResetPosition();
}
// 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
@ -4137,11 +4133,19 @@ void NavEKF::readHgtData()
// calculate offset to baro data that enables baro to be used as a backup
// filter offset to reduce effect of baro noise and other transient errors on estimate
baroHgtOffset = 0.2f * (_baro.get_altitude() + state.position.z) + 0.8f * baroHgtOffset;
} else {
} else if (vehicleArmed && takeOffDetected) {
// use baro measurement and correct for baro offset - failsafe use only as baro will drift
hgtMea = max(_baro.get_altitude() - baroHgtOffset, rngOnGnd);
// get states that were stored at the time closest to the measurement time, taking measurement delay into account
RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay));
} else {
// If we are on ground and have no range finder reading, assume the nominal on-ground height
hgtMea = rngOnGnd;
// get states that were stored at the time closest to the measurement time, taking measurement delay into account
statesAtHgtTime = state;
// calculate offset to baro data that enables baro to be used as a backup
// filter offset to reduce effect of baro noise and other transient errors on estimate
baroHgtOffset = 0.2f * (_baro.get_altitude() + state.position.z) + 0.8f * baroHgtOffset;
}
} else {
// use baro measurement and correct for baro offset
@ -4228,15 +4232,19 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - omegaAcrossFlowTime.x),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - omegaAcrossFlowTime.y),-0.1f,0.1f);
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sesnor data)
if (rawFlowQuality > 50 && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
// recall vehicle states at mid sample time for flow observations allowing for delays
RecallStates(statesAtFlowTime, imuSampleTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2);
// calculate rotation matrices at mid sample time for flow observations
statesAtFlowTime.quat.rotation_matrix(Tbn_flow);
Tnb_flow = Tbn_flow.transposed();
// check for takeoff if relying on optical flow and zero measurements until takeoff detected
// if we haven't taken off - constrain position and velocity states
if (_fusionModeGPS == 3) {
detectOptFlowTakeoff();
}
// recall vehicle states at mid sample time for flow observations allowing for delays
RecallStates(statesAtFlowTime, imuSampleTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2);
// calculate rotation matrices at mid sample time for flow observations
statesAtFlowTime.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
omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
// write uncorrected flow rate measurements that will be used by the focal length scale factor estimator
@ -4695,13 +4703,14 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const
status.flags.attitude = !state.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
status.flags.horiz_vel = someHorizRefData && notDeadReckoning && filterHealthy; // horizontal velocity estimate valid
status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
status.flags.horiz_pos_rel = (doingFlowNav || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid
status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid
status.flags.horiz_pos_abs = !gpsAidingBad && doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute horizontal position estimate valid
status.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid
status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
status.flags.const_pos_mode = constPosMode && 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
status.flags.pred_horiz_pos_abs = gpsNavPossible && filterHealthy; // we should be able to estimate an absolute position when we enter flight mode
status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
}
// send an EKF_STATUS message to GCS
@ -4753,6 +4762,8 @@ void NavEKF::performArmingChecks()
}
// zero stored velocities used to do dead-reckoning
heldVelNE.zero();
// reset the flag that indicates takeoff for use by optical flow navigation
takeOffDetected = false;
// set various useage modes based on the condition at arming. These are then held until the vehicle is disarmed.
if (!vehicleArmed) {
PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode
@ -4778,6 +4789,16 @@ void NavEKF::performArmingChecks()
constPosMode = true;
constVelMode = false; // always clear constant velocity mode if constant position mode is active
}
// Reset the last valid flow measurement time
flowValidMeaTime_ms = imuSampleTime_ms;
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
// this avoids issues casued by the time delay associated with arming that can trigger short timeouts
rngValidMeaTime_ms = imuSampleTime_ms;
// store the range finder measurement which will be used as a reference to detect when we have taken off
rangeAtArming = rngMea;
// set the time at which we arm to assist with takeoff detection
timeAtArming_ms = imuSampleTime_ms;
} else { // arming when GPS useage is allowed
if (gpsNotAvailable) {
PV_AidingMode = AID_NONE; // we don't have have GPS data and will only be able to estimate orientation and height
@ -4991,4 +5012,22 @@ void NavEKF::readRangeFinder(void)
}
// Detect takeoff for optical flow navigation
void NavEKF::detectOptFlowTakeoff(void)
{
if (vehicleArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
const AP_InertialSensor &ins = _ahrs->get_ins();
Vector3f angRateVec;
Vector3f gyroBias;
getGyroBias(gyroBias);
bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1);
if (dual_ins) {
angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias;
} else {
angRateVec = ins.get_gyro() - gyroBias;
}
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rangeAtArming + 0.1f)));
}
}
#endif // HAL_CPU_CLASS

9
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -429,6 +429,9 @@ private:
// Apply a median filter to range finder data
void readRangeFinder();
// check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
void detectOptFlowTakeoff(void);
// EKF Mavlink Tuneable Parameters
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s
@ -703,9 +706,15 @@ private:
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
bool gndOffsetValid; // true when the ground offset state can still be considered valid
bool flowXfailed; // true when the X optical flow measurement has failed the innovation consistency check
// 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
// Movement detector
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected
float rangeAtArming; // range finder measurement when armed
uint32_t timeAtArming_ms; // time in msec that the vehicle armed
bool haveDeltaAngles;

1
libraries/AP_NavEKF/AP_Nav_Common.h
View File

@ -31,6 +31,7 @@ union nav_filter_status {
uint16_t const_pos_mode : 1; // 7 - true if we are in const position mode
uint16_t pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoff
uint16_t pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoff
uint16_t takeoff_detected : 1; // 10 - true if optical flow takeoff has been detected
} flags;
uint16_t value;
};