From 46a2993a0d2d19dabc23bea62bbd1f9108a4bda2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 8 May 2016 23:46:06 +1000 Subject: [PATCH] AP_NavEKF: use intuitive units for imu bias process noise parameters --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 10 +++++----- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 6f144048de..4ff9b6355f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -350,11 +350,11 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { AP_GROUPINFO("ACC_PNOISE", 25, NavEKF2, _accNoise, ACC_PNOISE_DEFAULT), // @Param: GBIAS_PNOISE - // @DisplayName: Rate gyro bias process noise (rad/s) + // @DisplayName: Rate gyro bias stability (rad/s/s) // @Description: This state process noise controls growth of the gyro delta angle bias state error estimate. Increasing it makes rate gyro bias estimation faster and noisier. - // @Range: 0.0000001 0.00001 + // @Range: 0.00001 0.001 // @User: Advanced - // @Units: rad/s + // @Units: rad/s/s AP_GROUPINFO("GBIAS_PNOISE", 26, NavEKF2, _gyroBiasProcessNoise, GBIAS_PNOISE_DEFAULT), // @Param: GSCL_PNOISE @@ -366,11 +366,11 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { AP_GROUPINFO("GSCL_PNOISE", 27, NavEKF2, _gyroScaleProcessNoise, GSCALE_PNOISE_DEFAULT), // @Param: ABIAS_PNOISE - // @DisplayName: Accelerometer bias process noise (m/s^2) + // @DisplayName: Accelerometer bias stability (m/s^3) // @Description: This noise controls the growth of the vertical accelerometer delta velocity bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier. // @Range: 0.00001 0.001 // @User: Advanced - // @Units: m/s/s + // @Units: m/s/s/s AP_GROUPINFO("ABIAS_PNOISE", 28, NavEKF2, _accelBiasProcessNoise, ABIAS_PNOISE_DEFAULT), // @Param: MAG_PNOISE diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index cd063db2c0..0f2321cd73 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -669,8 +669,8 @@ void NavEKF2_core::CovariancePrediction() // use filtered height rate to increase wind process noise when climbing or descending // this allows for wind gradient effects. windVelSigma = dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)); - dAngBiasSigma = dt * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f); - dVelBiasSigma = dt * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f); + dAngBiasSigma = sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f); + dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f); dAngScaleSigma = dt * constrain_float(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f); magEarthSigma = dt * constrain_float(frontend->_magProcessNoise, 0.0f, 1.0f); magBodySigma = dt * constrain_float(frontend->_magProcessNoise, 0.0f, 1.0f);