aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 1 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 Randy Mackay
parent 62339c217c
commit 3233416e43
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

@ -1746,7 +1746,7 @@ void NavEKF::FuseVelPosNED()
velInnov2[i] = statesAtVelTime.vel2[i] - observation[i]; // IMU2 velInnov2[i] = statesAtVelTime.vel2[i] - observation[i]; // IMU2
// calculate innovation variance // calculate innovation variance
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; 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 // observation error to normalise
float R_hgt; float R_hgt;
if (i == 2) { if (i == 2) {
@ -1839,12 +1839,15 @@ void NavEKF::FuseVelPosNED()
if (fuseData[obsIndex]) { if (fuseData[obsIndex]) {
stateIndex = 4 + obsIndex; stateIndex = 4 + obsIndex;
// calculate the measurement innovation, using states from a different time coordinate if fusing height data // 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) if (obsIndex <= 2)
{ {
innovVelPos[obsIndex] = statesAtVelTime.velocity[obsIndex] - observation[obsIndex]; innovVelPos[obsIndex] = statesAtVelTime.velocity[obsIndex] - observation[obsIndex];
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
} }
else if (obsIndex == 3 || obsIndex == 4) { else if (obsIndex == 3 || obsIndex == 4) {
innovVelPos[obsIndex] = statesAtPosTime.position[obsIndex-3] - observation[obsIndex]; innovVelPos[obsIndex] = statesAtPosTime.position[obsIndex-3] - observation[obsIndex];
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
} else { } else {
innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex]; innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex];
} }
@ -3102,6 +3105,15 @@ void NavEKF::readGpsData()
// read the NED velocity from the GPS // read the NED velocity from the GPS
velNED = _ahrs->get_gps().velocity(); 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 // Check if GPS can output vertical velocity and set GPS fusion mode accordingly
if (!_ahrs->get_gps().have_vertical_velocity()) { if (!_ahrs->get_gps().have_vertical_velocity()) {
// vertical velocity should not be fused // 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 filterDiverged; // boolean true if the filter has diverged
bool magFailed; // boolean true if the magnetometer has failed 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 Vector31 Kfusion; // Kalman gain vector
Matrix22 KH; // intermediate result used for covariance updates Matrix22 KH; // intermediate result used for covariance updates
Matrix22 KHP; // intermediate result used for covariance updates Matrix22 KHP; // intermediate result used for covariance updates