dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: decompose drag vector into x-y components 

The drag force is a vector.
drag_x = drag.norm() * vx / v.norm()
This commit is contained in:
bresch 2022-12-30 17:43:30 +01:00 committed by Daniel Agar
parent 1a9c358858
commit 04f76c932e
2 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/ekf2/EKF/drag_fusion.cpp
View File

@ -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

6
src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py
View File

@ -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