Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Improved handling of noisy GPS speed accuracy data

This commit is contained in:
Paul Riseborough 2015-04-06 21:17:00 +10:00 committed by Randy Mackay
parent 53358a4e10
commit d618c55e2f
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4004,9 +4004,15 @@ void NavEKF::readGpsData()
// read the NED velocity from the GPS
velNED = _ahrs->get_gps().velocity();
// use the speed accuracy from the GPS if available, otherwise set it to zero.
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccuracy)) {
// 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
float alpha = constrain_float(0.0002f * (lastFixTime_ms - secondLastFixTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw;
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f;
} else {
gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw);
}
// check if we have enough GPS satellites and increase the gps noise scaler if we don't