diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index ddc492c698..45e8b1449e 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 6.0E-03f +#define ABIAS_P_NSE_DEFAULT 2.0E-02f #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 6.0E-03f +#define ABIAS_P_NSE_DEFAULT 2.0E-02f #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 6.0E-03f +#define ABIAS_P_NSE_DEFAULT 2.0E-02f #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 6.0E-03f +#define ABIAS_P_NSE_DEFAULT 2.0E-02f #define MAGB_P_NSE_DEFAULT 1.0E-04f #define MAGE_P_NSE_DEFAULT 1.0E-03f #define VEL_I_GATE_DEFAULT 500 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 88b44594bc..c7c190d5dc 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1927,7 +1927,7 @@ void NavEKF3_core::ConstrainVariances() zeroRows(P,10,12); } - const ftype minSafeStateVar = 1E-9; + const ftype minSafeStateVar = 5E-9; if (!inhibitDelVelBiasStates) { // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum