Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Don't let reported GPS accuracy modify horiz vel data checks

Browse Source
This commit is contained in:
Paul Riseborough 2015-02-27 09:06:02 +11:00 committed by Andrew Tridgell
parent a607eb8469
commit 961faa59d9
1 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1856,7 +1856,8 @@ void NavEKF::FuseVelPosNED()
// declare variables used by state and covariance update calculations
float posErr;
Vector6 R_OBS;
Vector6 R_OBS; // Measurement variances used for fusion
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
Vector6 observation;
float SK;
@ -1906,9 +1907,9 @@ void NavEKF::FuseVelPosNED()
// if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
// otherwise we scale it using manoeuvre acceleration
if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy - floor at 0.5m/s reported speed accuracy
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy/0.5f * _gpsHorizVelNoise, _gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy/0.5f * _gpsVertVelNoise, _gpsVertVelNoise, 50.0f));
// use GPS receivers reported speed accuracy - floor at value set by gps noise parameter
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, _gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, _gpsVertVelNoise, 50.0f));
} else {
// calculate additional error in GPS velocity caused by manoeuvring
R_OBS[0] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(gpsNEVelVarAccScale * accNavMag);
@ -1923,6 +1924,13 @@ void NavEKF::FuseVelPosNED()
R_OBS[5] *= gndEffectBaroScaler;
}
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(gpsNEVelVarAccScale * accNavMag);
for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
@ -1931,7 +1939,7 @@ void NavEKF::FuseVelPosNED()
float hgtErr = statesAtHgtTime.position.z - observation[5];
float velDErr = statesAtVelTime.velocity.z - observation[2];
// check if they are the same sign and both more than 3-sigma out of bounds
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS[2]))) {
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) {
badIMUdata = true;
} else {
badIMUdata = false;
@ -1944,8 +1952,8 @@ void NavEKF::FuseVelPosNED()
// test horizontal position measurements
posInnov[0] = statesAtPosTime.position.x - observation[3];
posInnov[1] = statesAtPosTime.position.y - observation[4];
varInnovVelPos[3] = P[7][7] + R_OBS[3];
varInnovVelPos[4] = P[8][8] + R_OBS[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
// calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter
// max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring
@ -1997,7 +2005,7 @@ void NavEKF::FuseVelPosNED()
velInnov1[i] = statesAtVelTime.vel1[i] - observation[i]; // IMU1
velInnov2[i] = statesAtVelTime.vel2[i] - observation[i]; // IMU2
// calculate innovation variance
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
// calculate error weightings for single IMU velocity states using
// observation error to normalise
float R_hgt;
@ -2042,7 +2050,7 @@ void NavEKF::FuseVelPosNED()
if (fuseHgtData) {
// calculate height innovations
hgtInnov = statesAtHgtTime.position.z - observation[5];
varInnovVelPos[5] = P[9][9] + R_OBS[5];
varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio
hgtTestRatio = sq(hgtInnov) / (sq(_hgtInnovGate) * varInnovVelPos[5]);
// fail if the ratio is > 1, but don't fail if bad IMU data