From 95c83255d7003db868bb5317a4d69fdf6c401f39 Mon Sep 17 00:00:00 2001 From: priseborough <p_riseborough@live.com.au> Date: Sat, 5 Apr 2014 17:03:50 +1100 Subject: [PATCH] AP_NavEKF : adjust default values for accelerometer process noise Slows down estimate and allows for smaller values to be set --- libraries/AP_NavEKF/AP_NavEKF.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 69f5a058b..808ef68ae 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -35,7 +35,7 @@ #define GYRO_PNOISE_DEFAULT 0.015f #define ACC_PNOISE_DEFAULT 0.25f #define GBIAS_PNOISE_DEFAULT 1E-07f -#define ABIAS_PNOISE_DEFAULT 0.0002f +#define ABIAS_PNOISE_DEFAULT 0.00015f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f #define VEL_GATE_DEFAULT 2 @@ -56,7 +56,7 @@ #define GYRO_PNOISE_DEFAULT 0.015f #define ACC_PNOISE_DEFAULT 0.25f #define GBIAS_PNOISE_DEFAULT 1E-07f -#define ABIAS_PNOISE_DEFAULT 0.0002f +#define ABIAS_PNOISE_DEFAULT 0.00015f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f #define VEL_GATE_DEFAULT 2 @@ -77,7 +77,7 @@ #define GYRO_PNOISE_DEFAULT 0.015f #define ACC_PNOISE_DEFAULT 0.25f #define GBIAS_PNOISE_DEFAULT 1E-07f -#define ABIAS_PNOISE_DEFAULT 0.0002f +#define ABIAS_PNOISE_DEFAULT 0.00015f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f #define VEL_GATE_DEFAULT 5 @@ -188,7 +188,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { // @Param: ABIAS_PNOISE // @DisplayName: Accelerometer bias state process noise (m/s^2) // @Description: This noise controls the growth of the vertical acelerometer bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier. - // @Range: 0.0002 - 0.001 + // @Range: 0.0001 - 0.001 // @User: advanced AP_GROUPINFO("ABIAS_PNOISE", 11, NavEKF, _accelBiasProcessNoise, ABIAS_PNOISE_DEFAULT), @@ -912,7 +912,7 @@ void NavEKF::CovariancePrediction() // this allows for wind gradient effects. windVelSigma = dt * constrain_float(_windVelProcessNoise, 0.01f, 1.0f) * (1.0f + constrain_float(_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)); dAngBiasSigma = dt * constrain_float(_gyroBiasProcessNoise, 1e-7f, 1e-5f); - dVelBiasSigma = dt * constrain_float(_accelBiasProcessNoise, 2e-4f, 1e-3f); + dVelBiasSigma = dt * constrain_float(_accelBiasProcessNoise, 1e-4f, 1e-3f); magEarthSigma = dt * constrain_float(_magEarthProcessNoise, 1e-4f, 1e-2f); magBodySigma = dt * constrain_float(_magBodyProcessNoise, 1e-4f, 1e-2f); for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; -- GitLab