diff --git a/EKF/common.h b/EKF/common.h index f7ac319286..aabf413b82 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -197,6 +197,10 @@ struct parameters { float terrain_p_noise; // process noise for terrain offset (m/sec) float terrain_gradient; // gradient of terrain used to estimate process noise due to changing position (m/m) + // initial switch on bias uncertainty + float switch_on_gyro_bias; // 1-sigma gyro bias uncertainty at switch on (rad/sec) + float switch_on_accel_bias; // 1-sigma accelerometer bias uncertainty at switch on (m/s**2) + // position and velocity fusion float gps_vel_noise; // observation noise for gps velocity fusion (m/sec) float gps_pos_noise; // observation noise for gps position fusion (m) @@ -288,6 +292,10 @@ struct parameters { terrain_p_noise = 5.0f; terrain_gradient = 0.5f; + // initial switch on bias uncertainty + switch_on_gyro_bias = 0.1f; + switch_on_accel_bias = 0.2f; + // position and velocity fusion gps_vel_noise = 5.0e-1f; gps_pos_noise = 0.5f; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 1947505877..1d485963e4 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -80,12 +80,12 @@ void Ekf::initialiseCovariance() } // gyro bias - P[10][10] = sq(0.1f * dt); + P[10][10] = sq(_params.switch_on_gyro_bias * dt); P[11][11] = P[10][10]; P[12][12] = P[10][10]; // accel bias - P[13][13] = sq(0.2f * dt); + P[13][13] = sq(_params.switch_on_accel_bias * dt); P[14][14] = P[13][13]; P[15][15] = P[13][13];