From 5cf31e439d313fb27ab11bb00f1cc8a034edb3e7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 10 Apr 2017 14:59:44 +1000 Subject: [PATCH] EKF: Add specific drag fusion tuning parameters --- EKF/common.h | 10 ++++++++++ EKF/drag_fusion.cpp | 19 +++++++------------ 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 366c287187..ebb80c911f 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -286,6 +286,11 @@ struct parameters { unsigned no_aid_timeout_max; // maximum lapsed time from last fusion of measurements that constrain drift before // the EKF will report that it is dead-reckoning (usec) + // multi-rotor drag specific force fusion + float drag_noise; // observation noise for drag specific force measurements (m/sec**2) + float bcoef_x; // ballistic coefficient along the X-axis (kg/m**2) + float bcoef_y; // ballistic coefficient along the Y-axis (kg/m**2) + // Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility. parameters() { @@ -397,6 +402,11 @@ struct parameters { no_gps_timeout_max = 7e6; no_aid_timeout_max = 1e6; + // multi-rotor drag specific force fusion + drag_noise = 2.5f; + bcoef_x = 25.0f; + bcoef_y = 25.0f; + } }; diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index e910e85bd6..a62b03dba2 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -48,22 +48,17 @@ void Ekf::fuseDrag() float H_ACC[24] = {}; // Observation Jacobian float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector float Kfusion[24] = {}; // Kalman gain vector - float R_ACC = 2.5f; // observation noise in specific force drag (m/sec**2) TODO add tuning parameter + float R_ACC = _params.drag_noise; // observation noise in specific force drag (m/sec**2) float eas2tas = 1.0f; // conversion factor from EAS to TAS - TODO calculate from barometric pressure float rho = 1.225f / sqrtf(eas2tas); // air density (kg/m**3) - // measured steady state EAS at specified tilt angle in X and Y directions - // TODO make the speeds tuning parameters - const float max_spd_x = 15.0f; - const float max_spd_y = 15.0f; - const float tilt_ref_angle = math::radians(25.0f); - - // calculate inverse of ballistic coefficient from max speed and tilt angle - // used to predict the acceleration for a given predicted state vector - float numerator = 2.0f*_gravity_mss*tanf(tilt_ref_angle); - float BC_inv_x = numerator / (rho * sq(max_spd_x)); - float BC_inv_y = numerator / (rho * sq(max_spd_y)); + // calculate inverse of ballistic coefficient + if (_params.bcoef_x < 1.0f || _params.bcoef_y < 1.0f) { + return; + } + float BC_inv_x = 1.0f / _params.bcoef_x; + float BC_inv_y = 1.0f / _params.bcoef_y; // get latest estimated orientation float q0 = _state.quat_nominal(0);