From 8e0cd1bc39e8e082c0f60bd8467ab1b9695cb2ec Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 30 Jun 2017 10:31:16 +1000 Subject: [PATCH 1/3] 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 { From ba4a31177143d50868936947c2c1fe851803f83d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 30 Jun 2017 10:32:23 +1000 Subject: [PATCH 2/3] EKF: Update comments for wind estimation logic --- EKF/control.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index b5742c3cbf..85bc12f341 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -877,11 +877,10 @@ void Ekf::controlAirDataFusion() { // control activation and initialisation/reset of wind states required for airspeed fusion - // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates + // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { - // if the airspeed or sideslip measurements have timed out for 10 seconds we declare the wind estimate to be invalid _control_status.flags.wind = false; } @@ -911,7 +910,7 @@ void Ekf::controlBetaFusion() { // control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion - // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates + // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { From 59edccca4a931973799b0b040426162cbb0aa613 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 30 Jun 2017 10:32:54 +1000 Subject: [PATCH 3/3] EKF: Fix bug in wind estimation for fixed wing --- EKF/control.cpp | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 85bc12f341..132b230aff 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -947,19 +947,22 @@ void Ekf::controlBetaFusion() void Ekf::controlDragFusion() { - if (_params.fusion_mode & MASK_USE_DRAG && _control_status.flags.in_air) { - if (!_control_status.flags.wind) { - // reset the wind states and covariances when starting drag accel fusion - _control_status.flags.wind = true; - resetWindStates(); - resetWindCovariance(); + if (_params.fusion_mode & MASK_USE_DRAG ) { + if (_control_status.flags.in_air) { + if (!_control_status.flags.wind) { + // reset the wind states and covariances when starting drag accel fusion + _control_status.flags.wind = true; + resetWindStates(); + resetWindCovariance(); - } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { - fuseDrag(); + } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { + fuseDrag(); + + } + } else { + _control_status.flags.wind = false; } - } else { - _control_status.flags.wind = false; } }