From 41f0231cfbcd927fd8c06c391f1cbcf8607ea535 Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 28 Oct 2014 18:30:46 +1100 Subject: [PATCH] AP_NavEKF : improve logic dealing with lack of flow or range data --- libraries/AP_NavEKF/AP_NavEKF.cpp | 58 +++++++++++++++++++------------ libraries/AP_NavEKF/AP_NavEKF.h | 5 +-- 2 files changed, 39 insertions(+), 24 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 03395b537b..f69ee7a3a5 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -787,8 +787,8 @@ void NavEKF::SelectVelPosFusion() // use both if GPS is primary sensor fuseVelData = true; fusePosData = true; - } else if (imuSampleTime_ms - lastFlowMeasTime_ms > 1000) { - // only use velocity if GPS is secondary sensor and optical flow sensing has failed + } else if (forceUseGPS) { + // we use GPS velocity data to constrain drift when optical flow measurements have failed fuseVelData = true; fusePosData = false; } else { @@ -2539,6 +2539,21 @@ void NavEKF::RunAuxiliaryEKF() // start performance timer 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 // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning if (!inhibitGndState) { @@ -3690,19 +3705,6 @@ void NavEKF::SetFlightAndFusionModes() bool magCalDenied = (!use_compass() || staticMode || (_magCal == 2)); // inhibit the magnetic field calibration if not requested or denied 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 @@ -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 // 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 - flowMeaTime_ms = msecFlowMeas; flowQuality = rawFlowQuality; // recall vehicle states at mid sample time for flow observations allowing for delays 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 newDataFlow = true; holdVelocity = false; + flowMeaTime_ms = msecFlowMeas; } else { newDataFlow = false; - holdVelocity = true; } // Use range finder if 3 or more consecutive good samples. This reduces likelihood of using bad data. if (rangeHealth >= 3) { statesAtRngTime = statesAtFlowTime; rngMea = rawSonarRange; newDataRng = true; + rngMeaTime_ms = msecFlowMeas; } else { newDataRng = false; } - // Store time of optical flow measurement. - lastFlowMeasTime_ms = imuSampleTime_ms; + // if we don't have flow measurements we use GPS velocity if available or else + // 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 @@ -4230,7 +4243,9 @@ void NavEKF::ZeroVariables() secondLastFixTime_ms = imuSampleTime_ms; lastDecayTime_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; velTimeout = false; @@ -4272,7 +4287,6 @@ void NavEKF::ZeroVariables() newDataRng = false; flowFusePerformed = false; fuseOptFlowData = false; - flowMeaTime_ms = imuSampleTime_ms; memset(&Popt[0][0], 0, sizeof(Popt)); flowStates[0] = 1.0f; flowStates[1] = 0.0f; @@ -4280,10 +4294,10 @@ void NavEKF::ZeroVariables() prevPosE = gpsPosNE.y; fuseRngData = false; inhibitGndState = true; - prevFlowFusionTime_ms = imuSampleTime_ms; // time the last flow measurement was fused flowGyroBias.x = 0; flowGyroBias.y = 0; holdVelocity = false; + forceUseGPS = false; } // return true if we should use the airspeed sensor diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 68cd509a56..54f51a471a 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -556,6 +556,7 @@ private: float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec) 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. + 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 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 @@ -587,8 +588,8 @@ private: uint8_t flowUpdateCountMax; // limit on the number of minor state corrections using optical flow data float flowUpdateCountMaxInv; // floating point inverse of flowUpdateCountMax 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 - uint32_t lastFlowMeasTime_ms; // time of last optical flow measurement + bool holdVelocity; // true when holding velocity in optical flow mode when no flow measurements are available + bool forceUseGPS; // true when lack of optical flow data forces us to use GPS // states held by optical flow fusion across time steps // optical flow X,Y motion compensated rate measurements are fused across two time steps