From acfa1f108ada1f522b12f816315a8acd1c85276a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 May 2023 19:46:29 +1000 Subject: [PATCH] AP_NavEKF3: Increase default value of EK3_ABIAS_P_NSE This is required because some hardware setups with RTK GPS have experienced a collapse of the delta velocity state variances. --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index c198b20bc2..ddc492c698 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -25,7 +25,7 @@ #define GYRO_P_NSE_DEFAULT 1.5E-02f #define ACC_P_NSE_DEFAULT 3.5E-01f #define GBIAS_P_NSE_DEFAULT 1.0E-03f -#define ABIAS_P_NSE_DEFAULT 3.0E-03f +#define ABIAS_P_NSE_DEFAULT 6.0E-03f #define MAGB_P_NSE_DEFAULT 1.0E-04f #define MAGE_P_NSE_DEFAULT 1.0E-03f #define VEL_I_GATE_DEFAULT 500 @@ -51,7 +51,7 @@ #define GYRO_P_NSE_DEFAULT 1.5E-02f #define ACC_P_NSE_DEFAULT 3.5E-01f #define GBIAS_P_NSE_DEFAULT 1.0E-03f -#define ABIAS_P_NSE_DEFAULT 3.0E-03f +#define ABIAS_P_NSE_DEFAULT 6.0E-03f #define MAGB_P_NSE_DEFAULT 1.0E-04f #define MAGE_P_NSE_DEFAULT 1.0E-03f #define VEL_I_GATE_DEFAULT 500 @@ -77,7 +77,7 @@ #define GYRO_P_NSE_DEFAULT 1.5E-02f #define ACC_P_NSE_DEFAULT 3.5E-01f #define GBIAS_P_NSE_DEFAULT 1.0E-03f -#define ABIAS_P_NSE_DEFAULT 3.0E-03f +#define ABIAS_P_NSE_DEFAULT 6.0E-03f #define MAGB_P_NSE_DEFAULT 1.0E-04f #define MAGE_P_NSE_DEFAULT 1.0E-03f #define VEL_I_GATE_DEFAULT 500 @@ -103,7 +103,7 @@ #define GYRO_P_NSE_DEFAULT 1.5E-02f #define ACC_P_NSE_DEFAULT 3.5E-01f #define GBIAS_P_NSE_DEFAULT 1.0E-03f -#define ABIAS_P_NSE_DEFAULT 3.0E-03f +#define ABIAS_P_NSE_DEFAULT 6.0E-03f #define MAGB_P_NSE_DEFAULT 1.0E-04f #define MAGE_P_NSE_DEFAULT 1.0E-03f #define VEL_I_GATE_DEFAULT 500