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

AP_NavEKF: Allow raw innovations to be monitored during timeouts

This commit is contained in:
Paul Riseborough 2015-04-15 16:01:25 +10:00 committed by Randy Mackay
parent d3f4b4a02b
commit 0852aeab6e
1 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1858,8 +1858,6 @@ void NavEKF::FuseVelPosNED()
Vector3f velInnov;
Vector3f velInnov1;
Vector3f velInnov2;
Vector2 posInnov;
float hgtInnov = 0;
// declare variables used to control access to arrays
bool fuseData[6] = {false,false,false,false,false,false};
@ -1963,8 +1961,8 @@ void NavEKF::FuseVelPosNED()
// test position measurements
if (fusePosData) {
// test horizontal position measurements
posInnov[0] = statesAtPosTime.position.x - observation[3];
posInnov[1] = statesAtPosTime.position.y - observation[4];
innovVelPos[3] = statesAtPosTime.position.x - observation[3];
innovVelPos[4] = statesAtPosTime.position.y - observation[4];
varInnovVelPos[3] = P[7][7] + R_OBS_DATA_CHECKS[3];
varInnovVelPos[4] = P[8][8] + R_OBS_DATA_CHECKS[4];
// apply an innovation consistency threshold test, but don't fail if bad IMU data
@ -1972,7 +1970,7 @@ void NavEKF::FuseVelPosNED()
// max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring
float accelScale = (1.0f + 0.1f * accNavMag);
float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - lastPosPassTime)));
posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2;
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// declare a timeout condition if we have been too long without data or not aiding
posTimeout = (((imuSampleTime_ms - lastPosPassTime) > gpsRetryTime) || PV_AidingMode == AID_NONE);
@ -1984,8 +1982,8 @@ void NavEKF::FuseVelPosNED()
lastPosPassTime = imuSampleTime_ms;
// if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps
if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) {
gpsPosGlitchOffsetNE.x += posInnov[0];
gpsPosGlitchOffsetNE.y += posInnov[1];
gpsPosGlitchOffsetNE.x += innovVelPos[3];
gpsPosGlitchOffsetNE.y += innovVelPos[4];
// limit the radius of the offset and decay the offset to zero radially
decayGpsOffset();
// reset the position to the current GPS position which will include the glitch correction offset
@ -2062,10 +2060,10 @@ void NavEKF::FuseVelPosNED()
// test height measurements
if (fuseHgtData) {
// calculate height innovations
hgtInnov = statesAtHgtTime.position.z - observation[5];
innovVelPos[5] = statesAtHgtTime.position.z - observation[5];
varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio
hgtTestRatio = sq(hgtInnov) / (sq(_hgtInnovGate) * varInnovVelPos[5]);
hgtTestRatio = sq(innovVelPos[5]) / (sq(_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) > hgtRetryTime;