Browse Source

AP_NavEKF: Enable recovery from extended flow measurement rejection

master
priseborough 10 years ago committed by Andrew Tridgell
parent
commit
2f5aa210ce
  1. 44
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 4
      libraries/AP_NavEKF/AP_NavEKF.h

44
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -902,25 +902,44 @@ void NavEKF::SelectFlowFusion()
{ {
// Perform Data Checks // Perform Data Checks
// Check if the optical flow data is still valid // Check if the optical flow data is still valid
flowDataValid = (imuSampleTime_ms - flowValidMeaTime_ms < 200); flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 200);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowUseTime_ms) > 5000);
// check is the terrain offset estimate is still valid // check is the terrain offset estimate is still valid
gndOffsetValid = (imuSampleTime_ms - gndHgtValidTime_ms < 5000); gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
// Perform tilt check // Perform tilt check
bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin); bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin);
// if we have waited too long, set a timeout flag which will force fusion to occur regardless of load spreading // if we have waited too long, set a timeout flag which will force fusion to occur regardless of load spreading
bool timeout = ((imuSampleTime_ms - prevFlowFusionTime_ms) >= flowIntervalMax_ms); bool flowUseTimeout = ((imuSampleTime_ms - prevFlowUseTime_ms) >= flowIntervalMax_ms);
// check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature. // check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature.
bool delayFusion = ((covPredStep || magFusePerformed) && !(timeout || inhibitLoadLeveling)); bool delayFusion = ((covPredStep || magFusePerformed) && !(flowUseTimeout || inhibitLoadLeveling));
// if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in position hold mode // if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in position hold mode
if (!flowDataValid && !constPosMode && PV_AidingMode == AID_RELATIVE) { if (!flowDataValid && !constPosMode && PV_AidingMode == AID_RELATIVE) {
constVelMode = true; constVelMode = true;
constPosMode = false; // always clear constant position mode if constant velocity mode is active constPosMode = false; // always clear constant position mode if constant velocity mode is active
} else if (flowDataValid && flowFusionTimeout) {
// we need to reset the velocities to a value estimated from the flow data
// estimate the range
float initPredRng = max((terrainState - state.position[2]),0.1f) / 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) { } else if (flowDataValid) {
// clear the constant velocity mode now we have good data // clear the constant velocity mode now we have good data
constVelMode = false; constVelMode = false;
lastConstVelMode = false; lastConstVelMode = false;
} }
// if we do have valid flow measurements
// Fuse data into a 1-state EKF to estimate terrain height // Fuse data into a 1-state EKF to estimate terrain height
if ((newDataFlow || newDataRng) && tiltOK) { if ((newDataFlow || newDataRng) && tiltOK) {
@ -962,7 +981,7 @@ void NavEKF::SelectFlowFusion()
// indicate that flow fusion has been performed. This is used for load spreading. // indicate that flow fusion has been performed. This is used for load spreading.
flowFusePerformed = true; flowFusePerformed = true;
// update the time stamp // update the time stamp
prevFlowFusionTime_ms = imuSampleTime_ms; prevFlowUseTime_ms = imuSampleTime_ms;
} else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode && tiltOK) { } else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode && tiltOK) {
// Fuse the optical flow Y axis data into the main filter // Fuse the optical flow Y axis data into the main filter
FuseOptFlow(); FuseOptFlow();
@ -2936,8 +2955,12 @@ void NavEKF::FuseOptFlow()
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable // Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
if ((flowTestRatio[obsIndex]) < 1.0f && (flowRadXY[obsIndex] < _maxFlowRate)) { if ((flowTestRatio[obsIndex]) < 1.0f && (flowRadXY[obsIndex] < _maxFlowRate)) {
// Reset the timer for velocity fusion timeout. This prevents a velocity timeout from being declared if we have to momentarily go into constant velocity mode. // record the last time both X and Y observations were accepted for fusion
velFailTime = imuSampleTime_ms; if (obsIndex == 0) {
flowXfailed = false;
} else if (!flowXfailed) {
prevFlowFuseTime_ms = imuSampleTime_ms;
}
// correct the state vector // correct the state vector
for (uint8_t j = 0; j <= 21; j++) for (uint8_t j = 0; j <= 21; j++)
{ {
@ -2996,6 +3019,9 @@ void NavEKF::FuseOptFlow()
P[i][j] = P[i][j] - KHP[i][j]; P[i][j] = P[i][j] - KHP[i][j];
} }
} }
} else if (obsIndex == 0) {
// store the fact we have failed the X conponent so that a combined X and Y axis pass/fail can be calculated next time round
flowXfailed = true;
} }
ForceSymmetry(); ForceSymmetry();
@ -4247,7 +4273,8 @@ void NavEKF::InitialiseVariables()
timeAtLastAuxEKF_ms = imuSampleTime_ms; timeAtLastAuxEKF_ms = imuSampleTime_ms;
flowValidMeaTime_ms = imuSampleTime_ms; flowValidMeaTime_ms = imuSampleTime_ms;
flowMeaTime_ms = 0; flowMeaTime_ms = 0;
prevFlowFusionTime_ms = imuSampleTime_ms; prevFlowUseTime_ms = imuSampleTime_ms;
prevFlowFuseTime_ms = imuSampleTime_ms;
gndHgtValidTime_ms = 0; gndHgtValidTime_ms = 0;
ekfStartTime_ms = imuSampleTime_ms; ekfStartTime_ms = imuSampleTime_ms;
@ -4321,6 +4348,7 @@ void NavEKF::InitialiseVariables()
inhibitWindStates = true; inhibitWindStates = true;
inhibitMagStates = true; inhibitMagStates = true;
gndOffsetValid = false; gndOffsetValid = false;
flowXfailed = false;
} }
// return true if we should use the airspeed sensor // return true if we should use the airspeed sensor

4
libraries/AP_NavEKF/AP_NavEKF.h

@ -623,7 +623,8 @@ private:
float innovRng; // range finder observation innovation (m) float innovRng; // range finder observation innovation (m)
float rngMea; // range finder measurement (m) float rngMea; // range finder measurement (m)
bool inhibitGndState; // true when the terrain position state is to remain constant bool inhibitGndState; // true when the terrain position state is to remain constant
uint32_t prevFlowFusionTime_ms; // time the last flow measurement was fused uint32_t prevFlowUseTime_ms; // time the last flow measurement scheduled for fusion (doesn't mean it passed the innovatio consistency checks)
uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
float flowTestRatio[2]; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail float flowTestRatio[2]; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2 float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
@ -643,6 +644,7 @@ private:
}; };
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS 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 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
// states held by optical flow fusion across time steps // states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps // optical flow X,Y motion compensated rate measurements are fused across two time steps

Loading…
Cancel
Save