fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 08:49:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Extend drag fusion to include propeller momnetum drag

Browse Source
This commit is contained in:
Paul Riseborough 2021-06-09 07:58:21 +10:00 committed by Paul Riseborough
parent 91808f7c9e
commit 73d1e514d0
3 changed files with 65 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/common.h
View File

@ -352,8 +352,9 @@ struct parameters {
// multi-rotor drag specific force fusion
float drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2
float bcoef_x{25.0f}; ///< ballistic coefficient along the X-axis (kg/m**2)
float bcoef_y{25.0f}; ///< ballistic coefficient along the Y-axis (kg/m**2)
float bcoef_x{100.0f}; ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
float bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
float mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s)
// control of accel error detection and mitigation (IMU clipping)
const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed

72
EKF/drag_fusion.cpp
View File

@ -51,12 +51,18 @@ void Ekf::fuseDrag()
const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
const float density_ratio = rho / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
// calculate inverse of ballistic coefficient
if (_params.bcoef_x < 1.0f || _params.bcoef_y < 1.0f) {
// drag model parameters
const bool using_bcoef_x = _params.bcoef_x > 1.0f;
const bool using_bcoef_y = _params.bcoef_y > 1.0f;
const bool using_mcoef = _params.mcoef > 0.001f;
if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
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
@ -82,15 +88,36 @@ void Ekf::fuseDrag()
// perform sequential fusion of XY specific forces
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
// measured drag acceleration corrected for sensor bias
const float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
const float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (ballistic_coef_inv_xy(axis_index) * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent arithmetic exceptions
const float Kacc = fmaxf(1e-1f, rho * ballistic_coef_inv_xy(axis_index) * airSpd);
// 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;
float pred_acc; // predicted drag acceleration
if (axis_index == 0) {
// drag can be modelled as an arbitrary combination of bluff body drag that proportional to
// speed squared, and rotor momentum drag that is proportional to speed.
float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_x) {
// 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)));
Kacc = fmaxf(1e-1f, (rho / _params.bcoef_x) * airspeed + _params.mcoef * density_ratio);
pred_acc = (0.5f / _params.bcoef_x ) * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * _params.mcoef * density_ratio;
} else if (using_mcoef) {
// propeller momentum drag only
Kacc = fmaxf(1e-1f, _params.mcoef * density_ratio);
pred_acc = - rel_wind_body(0) * _params.mcoef * density_ratio;
} else if (using_bcoef_x) {
// bluff body drag only
const float airspeed = sqrtf((2.0f * _params.bcoef_x * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / _params.bcoef_x ) * airspeed);
pred_acc = (0.5f / _params.bcoef_x ) * rho * sq(rel_wind_body(0)) * drag_sign;
} else {
// skip this axis
continue;
}
// intermediate variables
const float HK0 = vn - vwn;
const float HK1 = ve - vwe;
@ -173,6 +200,28 @@ void Ekf::fuseDrag()
} else if (axis_index == 1) {
// drag is a combination of bluff body drag proportional to speed squared
// and rotor momentum drag that is proportional to speed.
float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_y) {
// 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)));
Kacc = fmaxf(1e-1f, (rho / _params.bcoef_y) * airspeed + _params.mcoef * density_ratio);
pred_acc = (0.5f / _params.bcoef_y) * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * _params.mcoef * density_ratio;
} else if (using_mcoef) {
// propeller momentum drag only
Kacc = fmaxf(1e-1f, _params.mcoef * density_ratio);
pred_acc = - rel_wind_body(1) * _params.mcoef * density_ratio;
} else if (using_bcoef_y) {
// bluff body drag only
const float airspeed = sqrtf((2.0f * _params.bcoef_y * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / _params.bcoef_y) * airspeed);
pred_acc = (0.5f / _params.bcoef_y) * rho * sq(rel_wind_body(1)) * drag_sign;
} else {
// nothing more to do
return;
}
// intermediate variables
const float HK0 = ve - vwe;
const float HK1 = vn - vwn;
@ -255,12 +304,9 @@ void Ekf::fuseDrag()
}
// calculate the predicted acceleration and innovation measured along body axis
const float drag_sign = (rel_wind_body(axis_index) >= 0.f) ? 1.f : -1.f;
const float predAccel = -0.5f * ballistic_coef_inv_xy(axis_index) * rho * sq(rel_wind_body(axis_index)) * drag_sign;
_drag_innov(axis_index) = predAccel - mea_acc;
_drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (25.0f * _drag_innov_var(axis_index));
// Apply an innovation consistency check with a 5 Sigma threshold
_drag_innov(axis_index) = pred_acc - mea_acc;
_drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index));
// if the innovation consistency check fails then don't fuse the sample
if (_drag_test_ratio(axis_index) <= 1.0f) {

6
EKF/python/ekf_derivation/main.py
View File

@ -247,9 +247,9 @@ def body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy):
vrel = R_to_body*Matrix([vx-wx,vy-wy,vz]) # predicted wind relative velocity
# Use this nonlinear model for the prediction in the implementation only
# It uses a ballistic coefficient for each axis
# accXpred = -0.5*rho*vrel[0]*vrel[0]*BCXinv # predicted acceleration measured along X body axis
# accYpred = -0.5*rho*vrel[1]*vrel[1]*BCYinv # predicted acceleration measured along Y body axis
# It uses a ballistic coefficient for each axis and a propeller momentum drag coefficient
# accXpred = -sign(vrel[0]) * 0.5*rho*vrel[0]*(vrel[0]/BCoefX + MCoef) # predicted acceleration measured along X body axis
# accYpred = -sign(vrel[1]) * 0.5*rho*vrel[1]*(vrel[1]/BCoefY + MCoef) # predicted acceleration measured along Y body axis
# Use a simple viscous drag model for the linear estimator equations
# Use the the derivative from speed to acceleration averaged across the