Browse Source

AP_NavEKF : improve logic dealing with lack of flow or range data

mission-4.1.18
priseborough 10 years ago committed by Andrew Tridgell
parent
commit
41f0231cfb
  1. 58
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 5
      libraries/AP_NavEKF/AP_NavEKF.h

58
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -787,8 +787,8 @@ void NavEKF::SelectVelPosFusion()
// use both if GPS is primary sensor // use both if GPS is primary sensor
fuseVelData = true; fuseVelData = true;
fusePosData = true; fusePosData = true;
} else if (imuSampleTime_ms - lastFlowMeasTime_ms > 1000) { } else if (forceUseGPS) {
// only use velocity if GPS is secondary sensor and optical flow sensing has failed // we use GPS velocity data to constrain drift when optical flow measurements have failed
fuseVelData = true; fuseVelData = true;
fusePosData = false; fusePosData = false;
} else { } else {
@ -2539,6 +2539,21 @@ void NavEKF::RunAuxiliaryEKF()
// start performance timer // start performance timer
perf_begin(_perf_OpticalFlowEKF); perf_begin(_perf_OpticalFlowEKF);
// calculate a predicted LOS rate squared
float losRateSq = (sq(state.velocity.x) + sq(state.velocity.y)) / sq(flowStates[1] - state.position[2]);
// 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 ((losRateSq < 0.01f || _fusionModeGPS == 3) && !fuseRngData) {
inhibitGndState = true;
} else {
inhibitGndState = false;
}
// Don't update focal length offset if there is no range finder or not using GPS, or we are not flying fast enough to generate a useful LOS rate
if (!fuseRngData || _fusionModeGPS == 3 || losRateSq < 0.1f || gpsGndSpd < 5.0f) {
fScaleInhibit = true;
} else {
fScaleInhibit = false;
}
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
if (!inhibitGndState) { if (!inhibitGndState) {
@ -3690,19 +3705,6 @@ void NavEKF::SetFlightAndFusionModes()
bool magCalDenied = (!use_compass() || staticMode || (_magCal == 2)); bool magCalDenied = (!use_compass() || staticMode || (_magCal == 2));
// inhibit the magnetic field calibration if not requested or denied // inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied); inhibitMagStates = (!magCalRequested || magCalDenied);
// don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable
if ((!highGndSpdStage2 || (imuSampleTime_ms - lastFixTime_ms) > 1000) && !useRngFinder()) {
inhibitGndState = true;
} else {
inhibitGndState = false;
}
// Don't update focal length offset state if there is no range finder or GPS, or we are not flying fas enough to generate a useful LOS rate
float losRateSq = (sq(state.velocity.x) + sq(state.velocity.y)) / sq(flowStates[1] - state.position[2]);
if (!useRngFinder() || _fusionModeGPS == 3 || losRateSq < 0.09f || gndSpdSq < 9.0f) {
fScaleInhibit = true;
} else {
fScaleInhibit = false;
}
} }
// initialise the covariance matrix // initialise the covariance matrix
@ -3985,7 +3987,6 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// A positive Y rate is produced by a positive velocity over ground in the Y direction // A positive Y rate is produced by a positive velocity over ground in the Y direction
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
flowMeaTime_ms = msecFlowMeas;
flowQuality = rawFlowQuality; flowQuality = rawFlowQuality;
// recall vehicle states at mid sample time for flow observations allowing for delays // recall vehicle states at mid sample time for flow observations allowing for delays
RecallStates(statesAtFlowTime, flowMeaTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2); RecallStates(statesAtFlowTime, flowMeaTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2);
@ -4012,20 +4013,32 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// set flag that will trigger observations // set flag that will trigger observations
newDataFlow = true; newDataFlow = true;
holdVelocity = false; holdVelocity = false;
flowMeaTime_ms = msecFlowMeas;
} else { } else {
newDataFlow = false; newDataFlow = false;
holdVelocity = true;
} }
// Use range finder if 3 or more consecutive good samples. This reduces likelihood of using bad data. // Use range finder if 3 or more consecutive good samples. This reduces likelihood of using bad data.
if (rangeHealth >= 3) { if (rangeHealth >= 3) {
statesAtRngTime = statesAtFlowTime; statesAtRngTime = statesAtFlowTime;
rngMea = rawSonarRange; rngMea = rawSonarRange;
newDataRng = true; newDataRng = true;
rngMeaTime_ms = msecFlowMeas;
} else { } else {
newDataRng = false; newDataRng = false;
} }
// Store time of optical flow measurement. // if we don't have flow measurements we use GPS velocity if available or else
lastFlowMeasTime_ms = imuSampleTime_ms; // dead reckon using current velocity vector
if (imuSampleTime_ms - flowMeaTime_ms > 1000) {
forceUseGPS = true;
if (imuSampleTime_ms - lastFixTime_ms > 1000) {
holdVelocity = true;
} else {
holdVelocity = false;
}
} else {
forceUseGPS = false;
holdVelocity = false;
}
} }
// calculate the NED earth spin vector in rad/sec // calculate the NED earth spin vector in rad/sec
@ -4230,7 +4243,9 @@ void NavEKF::ZeroVariables()
secondLastFixTime_ms = imuSampleTime_ms; secondLastFixTime_ms = imuSampleTime_ms;
lastDecayTime_ms = imuSampleTime_ms; lastDecayTime_ms = imuSampleTime_ms;
timeAtLastAuxEKF_ms = imuSampleTime_ms; timeAtLastAuxEKF_ms = imuSampleTime_ms;
lastFlowMeasTime_ms = imuSampleTime_ms; flowMeaTime_ms = imuSampleTime_ms;
prevFlowFusionTime_ms = imuSampleTime_ms;
rngMeaTime_ms = imuSampleTime_ms;
gpsNoiseScaler = 1.0f; gpsNoiseScaler = 1.0f;
velTimeout = false; velTimeout = false;
@ -4272,7 +4287,6 @@ void NavEKF::ZeroVariables()
newDataRng = false; newDataRng = false;
flowFusePerformed = false; flowFusePerformed = false;
fuseOptFlowData = false; fuseOptFlowData = false;
flowMeaTime_ms = imuSampleTime_ms;
memset(&Popt[0][0], 0, sizeof(Popt)); memset(&Popt[0][0], 0, sizeof(Popt));
flowStates[0] = 1.0f; flowStates[0] = 1.0f;
flowStates[1] = 0.0f; flowStates[1] = 0.0f;
@ -4280,10 +4294,10 @@ void NavEKF::ZeroVariables()
prevPosE = gpsPosNE.y; prevPosE = gpsPosNE.y;
fuseRngData = false; fuseRngData = false;
inhibitGndState = true; inhibitGndState = true;
prevFlowFusionTime_ms = imuSampleTime_ms; // time the last flow measurement was fused
flowGyroBias.x = 0; flowGyroBias.x = 0;
flowGyroBias.y = 0; flowGyroBias.y = 0;
holdVelocity = false; holdVelocity = false;
forceUseGPS = false;
} }
// return true if we should use the airspeed sensor // return true if we should use the airspeed sensor

5
libraries/AP_NavEKF/AP_NavEKF.h

@ -556,6 +556,7 @@ private:
float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec) float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
uint8_t flowQuality; // unsigned integer representing quality of optical flow data. 255 is maximum quality. uint8_t flowQuality; // unsigned integer representing quality of optical flow data. 255 is maximum quality.
uint32_t rngMeaTime_ms; // time stamp from latest range measurement (msec)
float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high. float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step
Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period
@ -587,8 +588,8 @@ private:
uint8_t flowUpdateCountMax; // limit on the number of minor state corrections using optical flow data uint8_t flowUpdateCountMax; // limit on the number of minor state corrections using optical flow data
float flowUpdateCountMaxInv; // floating point inverse of flowUpdateCountMax float flowUpdateCountMaxInv; // floating point inverse of flowUpdateCountMax
bool newDataRng; // true when new valid range finder data has arrived. bool newDataRng; // true when new valid range finder data has arrived.
bool holdVelocity; // true wehn holding velocity in optical flow mode when no flow measurements are available bool holdVelocity; // true when holding velocity in optical flow mode when no flow measurements are available
uint32_t lastFlowMeasTime_ms; // time of last optical flow measurement bool forceUseGPS; // true when lack of optical flow data forces us to use GPS
// 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