From fecad463367986eb511bc1c18fbe095a3af5bf52 Mon Sep 17 00:00:00 2001 From: priseborough <p_riseborough@live.com.au> Date: Wed, 29 Oct 2014 19:32:57 +1100 Subject: [PATCH] AP_NavEKF : add INIT_GYRO_BIAS_UNCERTAINTY definition --- libraries/AP_NavEKF/AP_NavEKF.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 42e03aaa3..017789f5c 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -102,6 +102,9 @@ extern const AP_HAL::HAL& hal; // assume 3m/s to start #define STARTUP_WIND_SPEED 3.0f +// initial gyro bias uncertainty (deg/sec) +#define INIT_GYRO_BIAS_UNCERTAINTY 0.1f + // Define tuning parameters const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -2816,7 +2819,7 @@ void NavEKF::resetGyroBias(void) state.gyro_bias.zero(); zeroRows(P,10,12); zeroCols(P,10,12); - P[10][10] = sq(radians(0.1f * dtIMU)); + P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU)); P[11][11] = P[10][10]; P[12][12] = P[10][10]; @@ -2938,7 +2941,7 @@ void NavEKF::CovarianceInit() P[8][8] = P[7][7]; P[9][9] = sq(5.0f); // delta angle biases - P[10][10] = sq(radians(0.1f * dtIMU)); + P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU)); P[11][11] = P[10][10]; P[12][12] = P[10][10]; // Z delta velocity bias -- GitLab