From da0622827dbfe3f1dff3ae6e7b7f8bff5b02a311 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 21 May 2016 22:10:32 +1000 Subject: [PATCH] AP_NavEKF: update tuning defaults Increase speed of scale factor learning --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 4bfd45d281..30aeb7c802 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -23,7 +23,7 @@ #define GYRO_P_NSE_DEFAULT 3.0E-02f #define ACC_P_NSE_DEFAULT 6.0E-01f #define GBIAS_P_NSE_DEFAULT 1.0E-04f -#define GSCALE_P_NSE_DEFAULT 1.0E-05f +#define GSCALE_P_NSE_DEFAULT 5.0E-04f #define ABIAS_P_NSE_DEFAULT 1.0E-03f #define MAG_P_NSE_DEFAULT 2.5E-02f #define VEL_I_GATE_DEFAULT 500 @@ -47,7 +47,7 @@ #define GYRO_P_NSE_DEFAULT 3.0E-02f #define ACC_P_NSE_DEFAULT 6.0E-01f #define GBIAS_P_NSE_DEFAULT 1.0E-04f -#define GSCALE_P_NSE_DEFAULT 1.0E-05f +#define GSCALE_P_NSE_DEFAULT 5.0E-04f #define ABIAS_P_NSE_DEFAULT 1.0E-03f #define MAG_P_NSE_DEFAULT 2.5E-02f #define VEL_I_GATE_DEFAULT 500 @@ -71,7 +71,7 @@ #define GYRO_P_NSE_DEFAULT 3.0E-02f #define ACC_P_NSE_DEFAULT 6.0E-01f #define GBIAS_P_NSE_DEFAULT 1.0E-04f -#define GSCALE_P_NSE_DEFAULT 1.0E-05f +#define GSCALE_P_NSE_DEFAULT 5.0E-04f #define ABIAS_P_NSE_DEFAULT 1.0E-03f #define MAG_P_NSE_DEFAULT 2.5E-02f #define VEL_I_GATE_DEFAULT 500 @@ -95,7 +95,7 @@ #define GYRO_P_NSE_DEFAULT 3.0E-02f #define ACC_P_NSE_DEFAULT 6.0E-01f #define GBIAS_P_NSE_DEFAULT 1.0E-04f -#define GSCALE_P_NSE_DEFAULT 1.0E-05f +#define GSCALE_P_NSE_DEFAULT 5.0E-04f #define ABIAS_P_NSE_DEFAULT 1.0E-03f #define MAG_P_NSE_DEFAULT 2.5E-02f #define VEL_I_GATE_DEFAULT 500