From 04f76c932e3032d43bc3874e596a9182c8596bee Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 30 Dec 2022 17:43:30 +0100 Subject: [PATCH] ekf2: decompose drag vector into x-y components The drag force is a vector. drag_x = drag.norm() * vx / v.norm() --- src/modules/ekf2/EKF/drag_fusion.cpp | 17 ++++++++--------- .../mc_wind_estimator_tuning.py | 6 ++++-- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index bf21400e32..9ab27510bd 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -84,15 +84,13 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // calculate relative wind velocity in earth frame and rotate into body frame const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd); const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth); + const float rel_wind_speed = rel_wind_body.norm(); // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { // measured drag acceleration corrected for sensor bias const float mea_acc = drag_sample.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; - // predicted drag force sign is opposite to predicted wind relative velocity - const float drag_sign = (rel_wind_body(axis_index) >= 0.f) ? -1.f : 1.f; - // Drag is modelled as an arbitrary combination of bluff body drag that proportional to // equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed // parallel to the rotor disc and mass flow through the rotor disc. @@ -106,14 +104,15 @@ void Ekf::fuseDrag(const dragSample &drag_sample) const float bcoef_inv = 1.0f / _params.bcoef_x; // The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic // mea_acc = 0.5 * rho * bcoef_inv * airspeed**2 + mcoef_corrrected * airspeed + // TODO: this should be modified as well and in the derivation: mea_acc = 0.5 * rho * bcoef_inv * airspeed * airspeed_norm + mcoef_corrrected * airspeed const float airspeed = (_params.bcoef_x / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc))); Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected); - pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * mcoef_corrrected; + pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(0) * rel_wind_speed - rel_wind_body(0) * mcoef_corrrected; } else if (using_mcoef) { // Use propeller momentum drag only Kacc = fmaxf(1e-1f, mcoef_corrrected); - pred_acc = - rel_wind_body(0) * mcoef_corrrected; + pred_acc = -rel_wind_body(0) * mcoef_corrrected; } else if (using_bcoef_x) { // Use bluff body drag only @@ -122,7 +121,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) const float airspeed = sqrtf((2.0f * _params.bcoef_x * fabsf(mea_acc)) / rho); const float bcoef_inv = 1.0f / _params.bcoef_x; Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed); - pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign; + pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(0) * rel_wind_speed; } else { // skip this axis @@ -221,12 +220,12 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // mea_acc = 0.5 * rho * bcoef_inv * airspeed**2 + mcoef_corrrected * airspeed const float airspeed = (_params.bcoef_y / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc))); Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected); - pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * mcoef_corrrected; + pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(1) * rel_wind_speed - rel_wind_body(1) * mcoef_corrrected; } else if (using_mcoef) { // Use propeller momentum drag only Kacc = fmaxf(1e-1f, mcoef_corrrected); - pred_acc = - rel_wind_body(1) * mcoef_corrrected; + pred_acc = -rel_wind_body(1) * mcoef_corrrected; } else if (using_bcoef_y) { // Use bluff body drag only @@ -235,7 +234,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) const float airspeed = sqrtf((2.0f * _params.bcoef_y * fabsf(mea_acc)) / rho); const float bcoef_inv = 1.0f / _params.bcoef_y; Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed); - pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign; + pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(1) * rel_wind_speed; } else { // nothing more to do diff --git a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py index 7e94765389..61b746fb38 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py +++ b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py @@ -132,11 +132,13 @@ def run(logfile): rho = 1.15 # air densitiy rho15 = 1.225 # air density at 15 degC + rel_wind_speed = np.sqrt(v_body[0]**2 + v_body[1]**2 + v_body[2]**2) + # x[0]: momentum drag, scales with v # x[1]: inverse of ballistic coefficient (X body axis), scales with v^2 # x[2]: inverse of ballistic coefficient (Y body axis), scales with v^2 - predict_acc_x = lambda x: -v_body[0] * x[0] - 0.5 * rho * v_body[0]**2 * np.sign(v_body[0]) * x[1] - predict_acc_y = lambda x: -v_body[1] * x[0] - 0.5 * rho * v_body[1]**2 * np.sign(v_body[1]) * x[2] + predict_acc_x = lambda x: -v_body[0] * x[0] - 0.5 * rho * v_body[0] * rel_wind_speed * x[1] + predict_acc_y = lambda x: -v_body[1] * x[0] - 0.5 * rho * v_body[1] * rel_wind_speed * x[2] J = lambda x: np.sum(np.power(abs(a_body[0]-predict_acc_x(x)), 2.0) + np.power(abs(a_body[1]-predict_acc_y(x)), 2.0)) # cost function