From d745dc2b6ff59bc9f13e0674b4bd730cd61abd37 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 12 Apr 2014 14:26:49 +1000 Subject: [PATCH] AP_NavEKF : Increased position gate default to reduce impact of accel errors Flight testing with windup turns has shown that the position gate threshold can be tripped with good GPS data causing position jerks. This increases the initial GPS glitch rejection threshold to effectively 5m when using the default POSNE_NOISE value of 0.5m. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index ab946b7496..e23215dcb2 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -39,7 +39,7 @@ #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f #define VEL_GATE_DEFAULT 2 -#define POS_GATE_DEFAULT 5 +#define POS_GATE_DEFAULT 10 #define HGT_GATE_DEFAULT 5 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 1 @@ -60,7 +60,7 @@ #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f #define VEL_GATE_DEFAULT 2 -#define POS_GATE_DEFAULT 5 +#define POS_GATE_DEFAULT 10 #define HGT_GATE_DEFAULT 5 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 1 @@ -81,7 +81,7 @@ #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f #define VEL_GATE_DEFAULT 3 -#define POS_GATE_DEFAULT 5 +#define POS_GATE_DEFAULT 10 #define HGT_GATE_DEFAULT 10 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 0