From b61a6c64d7ebc1db1da022a30637638a7e199b27 Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 23 Sep 2014 19:30:01 +1000 Subject: [PATCH] AP_NavEKF : Reduce weighting on GPS when not enough satellites 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. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 14 +++++++++++++- libraries/AP_NavEKF/AP_NavEKF.h | 1 + 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index f7457a8792..4da5d3ab84 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -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 diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 5e7e68bc2e..ba27b1bfbe 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -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