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

AP_NavEKF : Reduce weighting on GPS when not enough satellites

Browse Source 
GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less.
The GPS glitch rejection thresholds remain the same
This will reduce the impact of GPS glitches on attitude.
This commit is contained in:
priseborough 2014-09-23 19:30:01 +10:00 committed by Andrew Tridgell
parent 7370e07c8d
commit b61a6c64d7
2 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1763,7 +1763,7 @@ void NavEKF::FuseVelPosNED()
velInnov2[i] = statesAtVelTime.vel2[i] - observation[i]; // IMU2
// calculate innovation variance
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
// calculate error weightings for singloe IMU velocity states using
// calculate error weightings for single IMU velocity states using
// observation error to normalise
float R_hgt;
if (i == 2) {
@ -1856,12 +1856,15 @@ void NavEKF::FuseVelPosNED()
if (fuseData[obsIndex]) {
stateIndex = 4 + obsIndex;
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
// adjust scaling on GPS measurement noise variances if not enough satellites
if (obsIndex <= 2)
{
innovVelPos[obsIndex] = statesAtVelTime.velocity[obsIndex] - observation[obsIndex];
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
}
else if (obsIndex == 3 || obsIndex == 4) {
innovVelPos[obsIndex] = statesAtPosTime.position[obsIndex-3] - observation[obsIndex];
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
} else {
innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex];
}
@ -3119,6 +3122,15 @@ void NavEKF::readGpsData()
// read the NED velocity from the GPS
velNED = _ahrs->get_gps().velocity();
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
if (_ahrs->get_gps().num_sats() >= 6) {
gpsNoiseScaler = 1.0f;
} else if (_ahrs->get_gps().num_sats() == 5) {
gpsNoiseScaler = 1.4f;
} else if (_ahrs->get_gps().num_sats() <= 4) {
gpsNoiseScaler = 2.0f;
}
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
if (!_ahrs->get_gps().have_vertical_velocity()) {
// vertical velocity should not be fused

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -364,6 +364,7 @@ private:
bool filterDiverged; // boolean true if the filter has diverged
bool magFailed; // boolean true if the magnetometer has failed
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Vector31 Kfusion; // Kalman gain vector
Matrix22 KH; // intermediate result used for covariance updates
Matrix22 KHP; // intermediate result used for covariance updates