From 8e0cd1bc39e8e082c0f60bd8467ab1b9695cb2ec Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 30 Jun 2017 10:31:16 +1000 Subject: [PATCH] EKF: Add parameter to set initial and max allowed wind uncertainty This enables the initial uncertainty to be set based on application and also ensures that the max allowed growth in wind state variance is consistent with the initial uncertainty specified. --- EKF/common.h | 1 + EKF/covariance.cpp | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) 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 {