fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 04:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: prevent /0 and use pre-computed inverses

Browse Source
This commit is contained in:
Paul Riseborough
2021-06-22 18:50:56 +10:00
committed by Paul Riseborough
parent de8625d255
commit 58d376e2be
1 changed files with 14 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/drag_fusion.cpp
+14 -13
View File
@@ -62,9 +62,6 @@ void Ekf::fuseDrag()
return; return;
} }
// calculate inverse of ballistic coefficient
const Vector2f ballistic_coef_inv_xy(1.f / _params.bcoef_x, 1.f / _params.bcoef_y);
// get latest estimated orientation // get latest estimated orientation
const float &q0 = _state.quat_nominal(0); const float &q0 = _state.quat_nominal(0);
const float &q1 = _state.quat_nominal(1); const float &q1 = _state.quat_nominal(1);
@@ -101,9 +98,10 @@ void Ekf::fuseDrag()
float Kacc; // Derivative of specific force wrt airspeed float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_x) { if (using_mcoef && using_bcoef_x) {
// bluff body and propeller momentum drag // bluff body and propeller momentum drag
const float airspeed = (_params.bcoef_x / rho) * (- _params.mcoef + sqrtf(sq(_params.mcoef ) + 2.0f * (rho / _params.bcoef_x ) * fabsf(mea_acc))); const float bcoef_inv = 1.0f / _params.bcoef_x;
Kacc = fmaxf(1e-1f, (rho / _params.bcoef_x) * airspeed + _params.mcoef * density_ratio); const float airspeed = (_params.bcoef_x / rho) * (- _params.mcoef + sqrtf(sq(_params.mcoef) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
pred_acc = (0.5f / _params.bcoef_x ) * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * _params.mcoef * density_ratio; Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + _params.mcoef * density_ratio);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * _params.mcoef * density_ratio;
} else if (using_mcoef) { } else if (using_mcoef) {
// propeller momentum drag only // propeller momentum drag only
Kacc = fmaxf(1e-1f, _params.mcoef * density_ratio); Kacc = fmaxf(1e-1f, _params.mcoef * density_ratio);
@@ -111,8 +109,9 @@ void Ekf::fuseDrag()
} else if (using_bcoef_x) { } else if (using_bcoef_x) {
// bluff body drag only // bluff body drag only
const float airspeed = sqrtf((2.0f * _params.bcoef_x * fabsf(mea_acc)) / rho); const float airspeed = sqrtf((2.0f * _params.bcoef_x * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / _params.bcoef_x ) * airspeed); const float bcoef_inv = 1.0f / _params.bcoef_x;
pred_acc = (0.5f / _params.bcoef_x ) * rho * sq(rel_wind_body(0)) * drag_sign; Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign;
} else { } else {
// skip this axis // skip this axis
continue; continue;
@@ -205,9 +204,10 @@ void Ekf::fuseDrag()
float Kacc; // Derivative of specific force wrt airspeed float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_y) { if (using_mcoef && using_bcoef_y) {
// bluff body and propeller momentum drag // bluff body and propeller momentum drag
const float airspeed = (_params.bcoef_y / rho) * (- _params.mcoef + sqrtf(sq(_params.mcoef ) + 2.0f * (rho / _params.bcoef_y) * fabsf(mea_acc))); const float bcoef_inv = 1.0f / _params.bcoef_y;
Kacc = fmaxf(1e-1f, (rho / _params.bcoef_y) * airspeed + _params.mcoef * density_ratio); const float airspeed = (_params.bcoef_y / rho) * (- _params.mcoef + sqrtf(sq(_params.mcoef) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
pred_acc = (0.5f / _params.bcoef_y) * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * _params.mcoef * density_ratio; Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + _params.mcoef * density_ratio);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * _params.mcoef * density_ratio;
} else if (using_mcoef) { } else if (using_mcoef) {
// propeller momentum drag only // propeller momentum drag only
Kacc = fmaxf(1e-1f, _params.mcoef * density_ratio); Kacc = fmaxf(1e-1f, _params.mcoef * density_ratio);
@@ -215,8 +215,9 @@ void Ekf::fuseDrag()
} else if (using_bcoef_y) { } else if (using_bcoef_y) {
// bluff body drag only // bluff body drag only
const float airspeed = sqrtf((2.0f * _params.bcoef_y * fabsf(mea_acc)) / rho); const float airspeed = sqrtf((2.0f * _params.bcoef_y * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / _params.bcoef_y) * airspeed); const float bcoef_inv = 1.0f / _params.bcoef_y;
pred_acc = (0.5f / _params.bcoef_y) * rho * sq(rel_wind_body(1)) * drag_sign; Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign;
} else { } else {
// nothing more to do // nothing more to do
return; return;