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

AP_NavEKF3: apply min GPS accuracy at measurement point

Browse Source 
this fixes an issue a RTK GPS gives 1cm horizontal and vertical
accuracy and that causes the variances to get too small
This commit is contained in:
Andrew Tridgell 2020-08-20 11:31:56 +10:00
parent 9553bc14d0
commit f8ef56be1b
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -543,6 +543,7 @@ void NavEKF3_core::readGpsData()
} else { } else {
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f); gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
gpsSpdAccuracy = MAX(gpsSpdAccuracy,frontend->_gpsHorizVelNoise);
} }
gpsPosAccuracy *= (1.0f - alpha); gpsPosAccuracy *= (1.0f - alpha);
float gpsPosAccRaw; float gpsPosAccRaw;
@ -551,6 +552,7 @@ void NavEKF3_core::readGpsData()
} else { } else {
gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f); gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f);
gpsPosAccuracy = MAX(gpsPosAccuracy, frontend->_gpsHorizPosNoise);
} }
gpsHgtAccuracy *= (1.0f - alpha); gpsHgtAccuracy *= (1.0f - alpha);
float gpsHgtAccRaw; float gpsHgtAccRaw;
@ -559,6 +561,7 @@ void NavEKF3_core::readGpsData()
} else { } else {
gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw); gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f); gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f);
gpsHgtAccuracy = MAX(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise);
} }
// check if we have enough GPS satellites and increase the gps noise scaler if we don't // check if we have enough GPS satellites and increase the gps noise scaler if we don't