From 6be9eaa5244a70362d69b2084db447d3cd490b61 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 7 May 2016 08:49:36 +1000 Subject: [PATCH] AP_NavEKF2: use receiver estimated accuracy Adjust the GPS observation noise based on receiver accuracy output if available. --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 13 +++++++++++-- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 14 +++++++++----- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 1 + libraries/AP_NavEKF2/AP_NavEKF2_core.h | 3 ++- 4 files changed, 23 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 10e198a2d0..0c975da6ba 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -371,8 +371,8 @@ void NavEKF2_core::readGpsData() // read the NED velocity from the GPS gpsDataNew.vel = _ahrs->get_gps().velocity(); - // Use the speed accuracy from the GPS if available, otherwise set it to zero. - // Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data + // Use the speed and position accuracy from the GPS if available, otherwise set it to zero. + // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f); gpsSpdAccuracy *= (1.0f - alpha); float gpsSpdAccRaw; @@ -380,6 +380,15 @@ void NavEKF2_core::readGpsData() gpsSpdAccuracy = 0.0f; } else { gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); + gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f); + } + gpsPosAccuracy *= (1.0f - alpha); + float gpsPosAccRaw; + if (!_ahrs->get_gps().horizontal_accuracy(gpsPosAccRaw)) { + gpsPosAccuracy = 0.0f; + } else { + gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); + gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f); } // check if we have enough GPS satellites and increase the gps noise scaler if we don't diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index ad970480f0..8f082d7dae 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -209,7 +209,6 @@ void NavEKF2_core::FuseVelPosNED() uint8_t obsIndex; // declare variables used by state and covariance update calculations - float posErr; Vector6 R_OBS; // Measurement variances used for fusion Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only Vector6 observation; @@ -236,7 +235,7 @@ void NavEKF2_core::FuseVelPosNED() observation[5] = -hgtMea; // calculate additional error in GPS position caused by manoeuvring - posErr = frontend->gpsPosVarAccScale * accNavMag; + float posErr = frontend->gpsPosVarAccScale * accNavMag; // estimate the GPS Velocity, GPS horiz position and height measurement variances. // Use different errors if operating without external aiding using an assumed position or velocity of zero @@ -255,7 +254,7 @@ void NavEKF2_core::FuseVelPosNED() for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; } else { if (gpsSpdAccuracy > 0.0f) { - // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter + // use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f)); R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f)); } else { @@ -264,11 +263,16 @@ void NavEKF2_core::FuseVelPosNED() R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag); } R_OBS[1] = R_OBS[0]; + // Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter + if (gpsPosAccuracy > 0.0f) { + R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f)); + } else { + R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); + } + R_OBS[4] = R_OBS[3]; // 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 - R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); - R_OBS[4] = R_OBS[3]; for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); } R_OBS[5] = posDownObsNoise; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 7a5faa3a5a..4d6a613a76 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -197,6 +197,7 @@ void NavEKF2_core::InitialiseVariables() touchdownExpectedSet_ms = 0; expectGndEffectTouchdown = false; gpsSpdAccuracy = 0.0f; + gpsPosAccuracy = 0.0f; baroHgtOffset = 0.0f; yawResetAngle = 0.0f; lastYawReset_ms = 0; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 74f52daa21..47a2d64772 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -708,7 +708,8 @@ private: bool prevIsAiding; // isAiding from previous frame struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset bool validOrigin; // true when the EKF origin is valid - float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the UBlox GPS receiver + float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver + float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad float posDownAtTakeoff; // flight vehicle vertical position at arming used as a reference point