Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

ekf2: MC drag fusion - interpolate between X an Y "b" coefficients

Browse Source 
Use the current sideslip angle to generate a drag coefficient that is a
mixture of X and Y coefficients, creating an elliptic distribution.
This commit is contained in:
bresch
2023-01-04 14:58:53 +01:00
committed by Daniel Agar
parent 420f5ef2b7
commit 06ef39d05e
1 changed files with 25 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/drag_fusion.cpp
+25 -9
View File
@@ -60,7 +60,6 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
return; return;
} }
// predicted specific forces
// calculate relative wind velocity in earth frame and rotate into body frame // calculate relative wind velocity in earth frame and rotate into body frame
const Vector3f rel_wind_earth(_state.vel(0) - _state.wind_vel(0), const Vector3f rel_wind_earth(_state.vel(0) - _state.wind_vel(0),
_state.vel(1) - _state.wind_vel(1), _state.vel(1) - _state.wind_vel(1),
@@ -69,6 +68,24 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
const float rel_wind_speed = rel_wind_body.norm(); const float rel_wind_speed = rel_wind_body.norm();
const Vector24f state_vector_prev = getStateAtFusionHorizonAsVector(); const Vector24f state_vector_prev = getStateAtFusionHorizonAsVector();
Vector2f bcoef_inv;
if (using_bcoef_x) {
bcoef_inv(0) = 1.0f / _params.bcoef_x;
}
if (using_bcoef_y) {
bcoef_inv(1) = 1.0f / _params.bcoef_y;
}
if (using_bcoef_x && using_bcoef_y) {
// Interpolate between the X and Y bluff body drag coefficients using current relative velocity
// This creates an elliptic drag distribution around the XY plane
bcoef_inv(0) = Vector2f(bcoef_inv.emult(rel_wind_body.xy()) / rel_wind_body.xy().norm()).norm();
bcoef_inv(1) = bcoef_inv(0);
}
Vector24f Kfusion; Vector24f Kfusion;
// perform sequential fusion of XY specific forces // perform sequential fusion of XY specific forces
@@ -79,21 +96,20 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
// Drag is modelled as an arbitrary combination of bluff body drag that proportional to // 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 // 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. // parallel to the rotor disc and mass flow through the rotor disc.
float bcoef_inv = 0.f;
if (axis_index == 0) { if (axis_index == 0) {
if (using_bcoef_x) { if (!using_bcoef_x && !using_mcoef) {
bcoef_inv = 1.0f / _params.bcoef_x; continue;
} }
sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv, mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(0), &Kfusion); sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion);
} else if (axis_index == 1) { } else if (axis_index == 1) {
if (using_bcoef_y) { if (!using_bcoef_y && !using_mcoef) {
bcoef_inv = 1.0f / _params.bcoef_y; continue;
} }
sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv, mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(1), &Kfusion); sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion);
} }
if (_drag_innov_var(axis_index) < R_ACC) { if (_drag_innov_var(axis_index) < R_ACC) {
@@ -101,7 +117,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
return; return;
} }
const float pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected; const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected;
// Apply an innovation consistency check with a 5 Sigma threshold // Apply an innovation consistency check with a 5 Sigma threshold
_drag_innov(axis_index) = pred_acc - mea_acc; _drag_innov(axis_index) = pred_acc - mea_acc;