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

AP_NavEKF: floor GPS velocity noise at parameter value for conservatism

This commit is contained in:
Jonathan Challinger 2015-02-12 17:07:27 -08:00 committed by Andrew Tridgell
parent 9a797a5d49
commit 20d92f5f9d
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -27,8 +27,8 @@
*/
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
// copter defaults
#define VELNE_NOISE_DEFAULT 1.0f
#define VELD_NOISE_DEFAULT 1.4f
#define VELNE_NOISE_DEFAULT 0.5f
#define VELD_NOISE_DEFAULT 0.7f
#define POSNE_NOISE_DEFAULT 0.5f
#define ALT_NOISE_DEFAULT 1.0f
#define MAG_NOISE_DEFAULT 0.05f
@ -51,8 +51,8 @@
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
// rover defaults
#define VELNE_NOISE_DEFAULT 1.0f
#define VELD_NOISE_DEFAULT 1.4f
#define VELNE_NOISE_DEFAULT 0.5f
#define VELD_NOISE_DEFAULT 0.7f
#define POSNE_NOISE_DEFAULT 0.5f
#define ALT_NOISE_DEFAULT 1.0f
#define MAG_NOISE_DEFAULT 0.05f
@ -75,8 +75,8 @@
#else
// generic defaults (and for plane)
#define VELNE_NOISE_DEFAULT 1.0f
#define VELD_NOISE_DEFAULT 1.4f
#define VELNE_NOISE_DEFAULT 0.5f
#define VELD_NOISE_DEFAULT 0.7f
#define POSNE_NOISE_DEFAULT 0.5f
#define ALT_NOISE_DEFAULT 0.5f
#define MAG_NOISE_DEFAULT 0.05f
@ -1921,9 +1921,9 @@ void NavEKF::FuseVelPosNED()
// if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
// otherwise we scale it using manoeuvre acceleration
if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy * _gpsHorizVelNoise, 0.05f, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy * _gpsVertVelNoise, 0.05f, 50.0f));
// use GPS receivers reported speed accuracy - floor at 0.5m/s reported speed accuracy
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy/0.5f * _gpsHorizVelNoise, _gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy/0.5f * _gpsVertVelNoise, _gpsVertVelNoise, 50.0f));
} else {
// calculate additional error in GPS velocity caused by manoeuvring
R_OBS[0] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(gpsNEVelVarAccScale * accNavMag);