diff --git a/EKF/common.h b/EKF/common.h index 40332488ec..6948f71125 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -219,6 +219,7 @@ struct parameters { float switch_on_gyro_bias{0.1f}; // 1-sigma gyro bias uncertainty at switch on (rad/sec) float switch_on_accel_bias{0.2f}; // 1-sigma accelerometer bias uncertainty at switch on (m/s**2) float initial_tilt_err{0.1f}; // 1-sigma tilt error after initial alignment using gravity vector (rad) + float initial_wind_uncertainty{1.0f}; // 1-sigma initial uncertainty in wind velocity (m/s) // position and velocity fusion float gps_vel_noise{5.0e-1f}; // observation noise for gps velocity fusion (m/sec) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index d6c8f0b9fb..e7f72a553c 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -100,8 +100,8 @@ void Ekf::initialiseCovariance() } // wind - P[22][22] = 1.0f; - P[23][23] = 1.0f; + P[22][22] = sq(_params.initial_wind_uncertainty); + P[23][23] = sq(_params.initial_wind_uncertainty); } @@ -204,7 +204,7 @@ void Ekf::predictCovariance() float wind_vel_sig; // Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned - if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 2.0f) { + if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 2.0f * sq(_params.initial_wind_uncertainty)) { wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f); } else {