From a3b78e6231d9ca24d3e0bed6e481f636e927a951 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 8 May 2016 23:53:49 +1000 Subject: [PATCH] AP_NavEKF2: update tuning defaults --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 74 ++++++++++++------------ libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 2 +- 2 files changed, 38 insertions(+), 38 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 3ad0168a21..ff4fe79c60 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -18,23 +18,23 @@ #define VELNE_NOISE_DEFAULT 0.5f #define VELD_NOISE_DEFAULT 0.7f #define POSNE_NOISE_DEFAULT 1.0f -#define ALT_NOISE_DEFAULT 5.0f +#define ALT_NOISE_DEFAULT 3.0f #define MAG_NOISE_DEFAULT 0.05f -#define GYRO_PNOISE_DEFAULT 0.001f -#define ACC_PNOISE_DEFAULT 0.25f -#define GBIAS_PNOISE_DEFAULT 7.0E-05f -#define ABIAS_PNOISE_DEFAULT 1.0E-04f +#define GYRO_PNOISE_DEFAULT 3.0E-02f +#define ACC_PNOISE_DEFAULT 6.0E-01f +#define GBIAS_PNOISE_DEFAULT 1.0E-04f +#define GSCALE_PNOISE_DEFAULT 1.0E-05f +#define ABIAS_PNOISE_DEFAULT 1.0E-03f #define MAG_PNOISE_DEFAULT 2.5E-02f -#define VEL_GATE_DEFAULT 200 -#define POS_GATE_DEFAULT 300 -#define HGT_GATE_DEFAULT 300 +#define VEL_GATE_DEFAULT 500 +#define POS_GATE_DEFAULT 500 +#define HGT_GATE_DEFAULT 500 #define MAG_GATE_DEFAULT 300 #define MAG_CAL_DEFAULT 3 #define GLITCH_RADIUS_DEFAULT 25 #define FLOW_MEAS_DELAY 10 #define FLOW_NOISE_DEFAULT 0.25f #define FLOW_GATE_DEFAULT 300 -#define GSCALE_PNOISE_DEFAULT 3.0E-03f #define CHECK_SCALER_DEFAULT 100 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2) @@ -42,23 +42,23 @@ #define VELNE_NOISE_DEFAULT 0.5f #define VELD_NOISE_DEFAULT 0.7f #define POSNE_NOISE_DEFAULT 1.0f -#define ALT_NOISE_DEFAULT 2.0f +#define ALT_NOISE_DEFAULT 3.0f #define MAG_NOISE_DEFAULT 0.05f -#define GYRO_PNOISE_DEFAULT 0.001f -#define ACC_PNOISE_DEFAULT 0.25f -#define GBIAS_PNOISE_DEFAULT 7.0E-05f -#define ABIAS_PNOISE_DEFAULT 1.0E-04f +#define GYRO_PNOISE_DEFAULT 3.0E-02f +#define ACC_PNOISE_DEFAULT 6.0E-01f +#define GBIAS_PNOISE_DEFAULT 1.0E-04f +#define GSCALE_PNOISE_DEFAULT 1.0E-05f +#define ABIAS_PNOISE_DEFAULT 1.0E-03f #define MAG_PNOISE_DEFAULT 2.5E-02f -#define VEL_GATE_DEFAULT 200 -#define POS_GATE_DEFAULT 300 -#define HGT_GATE_DEFAULT 300 +#define VEL_GATE_DEFAULT 500 +#define POS_GATE_DEFAULT 500 +#define HGT_GATE_DEFAULT 500 #define MAG_GATE_DEFAULT 300 #define MAG_CAL_DEFAULT 2 #define GLITCH_RADIUS_DEFAULT 25 #define FLOW_MEAS_DELAY 10 #define FLOW_NOISE_DEFAULT 0.25f #define FLOW_GATE_DEFAULT 300 -#define GSCALE_PNOISE_DEFAULT 3.0E-03f #define CHECK_SCALER_DEFAULT 100 #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) @@ -66,23 +66,23 @@ #define VELNE_NOISE_DEFAULT 0.5f #define VELD_NOISE_DEFAULT 0.7f #define POSNE_NOISE_DEFAULT 1.0f -#define ALT_NOISE_DEFAULT 5.0f +#define ALT_NOISE_DEFAULT 3.0f #define MAG_NOISE_DEFAULT 0.05f -#define GYRO_PNOISE_DEFAULT 0.001f -#define ACC_PNOISE_DEFAULT 0.25f -#define GBIAS_PNOISE_DEFAULT 7.0E-05f -#define ABIAS_PNOISE_DEFAULT 1.0E-04f +#define GYRO_PNOISE_DEFAULT 3.0E-02f +#define ACC_PNOISE_DEFAULT 6.0E-01f +#define GBIAS_PNOISE_DEFAULT 1.0E-04f +#define GSCALE_PNOISE_DEFAULT 1.0E-05f +#define ABIAS_PNOISE_DEFAULT 1.0E-03f #define MAG_PNOISE_DEFAULT 2.5E-02f -#define VEL_GATE_DEFAULT 200 -#define POS_GATE_DEFAULT 300 -#define HGT_GATE_DEFAULT 400 -#define MAG_GATE_DEFAULT 200 +#define VEL_GATE_DEFAULT 500 +#define POS_GATE_DEFAULT 500 +#define HGT_GATE_DEFAULT 500 +#define MAG_GATE_DEFAULT 300 #define MAG_CAL_DEFAULT 0 #define GLITCH_RADIUS_DEFAULT 25 #define FLOW_MEAS_DELAY 10 #define FLOW_NOISE_DEFAULT 0.25f #define FLOW_GATE_DEFAULT 300 -#define GSCALE_PNOISE_DEFAULT 3.0E-03f #define CHECK_SCALER_DEFAULT 150 #else @@ -90,23 +90,23 @@ #define VELNE_NOISE_DEFAULT 0.5f #define VELD_NOISE_DEFAULT 0.7f #define POSNE_NOISE_DEFAULT 1.0f -#define ALT_NOISE_DEFAULT 5.0f +#define ALT_NOISE_DEFAULT 3.0f #define MAG_NOISE_DEFAULT 0.05f -#define GYRO_PNOISE_DEFAULT 0.001f -#define ACC_PNOISE_DEFAULT 0.25f -#define GBIAS_PNOISE_DEFAULT 3.5E-05f -#define ABIAS_PNOISE_DEFAULT 1.0E-04f +#define GYRO_PNOISE_DEFAULT 3.0E-02f +#define ACC_PNOISE_DEFAULT 6.0E-01f +#define GBIAS_PNOISE_DEFAULT 1.0E-04f +#define GSCALE_PNOISE_DEFAULT 1.0E-05f +#define ABIAS_PNOISE_DEFAULT 1.0E-03f #define MAG_PNOISE_DEFAULT 2.5E-02f -#define VEL_GATE_DEFAULT 200 -#define POS_GATE_DEFAULT 300 -#define HGT_GATE_DEFAULT 300 +#define VEL_GATE_DEFAULT 500 +#define POS_GATE_DEFAULT 500 +#define HGT_GATE_DEFAULT 500 #define MAG_GATE_DEFAULT 300 #define MAG_CAL_DEFAULT 3 #define GLITCH_RADIUS_DEFAULT 25 #define FLOW_MEAS_DELAY 10 #define FLOW_NOISE_DEFAULT 0.25f #define FLOW_GATE_DEFAULT 300 -#define GSCALE_PNOISE_DEFAULT 3.0E-03f #define CHECK_SCALER_DEFAULT 100 #endif // APM_BUILD_DIRECTORY diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 0f2321cd73..6f8eee7c69 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -708,7 +708,7 @@ void NavEKF2_core::CovariancePrediction() dvz_b = stateStruct.accel_zbias; float _gyrNoise = constrain_float(frontend->_gyrNoise, 0.0f, 1.0f); daxNoise = dayNoise = dazNoise = sq(dt*_gyrNoise); - float _accNoise = constrain_float(frontend->_accNoise, 0.0f, 1.0f); + float _accNoise = constrain_float(frontend->_accNoise, 0.0f, 10.0f); dvxNoise = dvyNoise = dvzNoise = sq(dt*_accNoise); // calculate the predicted covariance due to inertial sensor error propagation