mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: Update IMU tuning parameter limits
This commit is contained in:
parent
0562529729
commit
e8706db382
|
@ -333,8 +333,8 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||||
// @Param: GYRO_PNOISE
|
// @Param: GYRO_PNOISE
|
||||||
// @DisplayName: Rate gyro noise (rad/s)
|
// @DisplayName: Rate gyro noise (rad/s)
|
||||||
// @Description: This control disturbance noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
|
// @Description: This control disturbance noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
|
||||||
// @Range: 0.001 0.05
|
// @Range: 0.0001 0.01
|
||||||
// @Increment: 0.001
|
// @Increment: 0.0001
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
// @Units: rad/s
|
// @Units: rad/s
|
||||||
AP_GROUPINFO("GYRO_PNOISE", 24, NavEKF2, _gyrNoise, GYRO_PNOISE_DEFAULT),
|
AP_GROUPINFO("GYRO_PNOISE", 24, NavEKF2, _gyrNoise, GYRO_PNOISE_DEFAULT),
|
||||||
|
@ -342,7 +342,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||||
// @Param: ACC_PNOISE
|
// @Param: ACC_PNOISE
|
||||||
// @DisplayName: Accelerometer noise (m/s^2)
|
// @DisplayName: Accelerometer noise (m/s^2)
|
||||||
// @Description: This control disturbance noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
|
// @Description: This control disturbance noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
|
||||||
// @Range: 0.05 1.0
|
// @Range: 0.01 1.0
|
||||||
// @Increment: 0.01
|
// @Increment: 0.01
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
// @Units: m/s/s
|
// @Units: m/s/s
|
||||||
|
|
|
@ -668,9 +668,9 @@ void NavEKF2_core::CovariancePrediction()
|
||||||
day_s = stateStruct.gyro_scale.y;
|
day_s = stateStruct.gyro_scale.y;
|
||||||
daz_s = stateStruct.gyro_scale.z;
|
daz_s = stateStruct.gyro_scale.z;
|
||||||
dvz_b = stateStruct.accel_zbias;
|
dvz_b = stateStruct.accel_zbias;
|
||||||
float _gyrNoise = constrain_float(frontend->_gyrNoise, 1e-3f, 5e-2f);
|
float _gyrNoise = constrain_float(frontend->_gyrNoise, 1e-4f, 1e-2f);
|
||||||
daxNoise = dayNoise = dazNoise = dt*_gyrNoise;
|
daxNoise = dayNoise = dazNoise = dt*_gyrNoise;
|
||||||
float _accNoise = constrain_float(frontend->_accNoise, 5e-2f, 1.0f);
|
float _accNoise = constrain_float(frontend->_accNoise, 1e-2f, 1.0f);
|
||||||
dvxNoise = dvyNoise = dvzNoise = dt*_accNoise;
|
dvxNoise = dvyNoise = dvzNoise = dt*_accNoise;
|
||||||
|
|
||||||
// calculate the predicted covariance due to inertial sensor error propagation
|
// calculate the predicted covariance due to inertial sensor error propagation
|
||||||
|
|
Loading…
Reference in New Issue