Browse Source

AP_NavEKF: use intuitive units for imu bias process noise parameters

mission-4.1.18
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
commit
46a2993a0d
  1. 10
      libraries/AP_NavEKF2/AP_NavEKF2.cpp
  2. 4
      libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

10
libraries/AP_NavEKF2/AP_NavEKF2.cpp

@ -350,11 +350,11 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { @@ -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[] = { @@ -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

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@ -669,8 +669,8 @@ void NavEKF2_core::CovariancePrediction() @@ -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);

Loading…
Cancel
Save