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

Make relative wind computation more compact

Browse Source
This commit is contained in:
kamilritz 2020-06-21 12:00:46 +02:00 committed by Mathieu Bresciani
parent b8f937666a
commit a3706fdcef
2 changed files with 15 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
EKF/drag_fusion.cpp
View File

@ -78,13 +78,10 @@ void Ekf::fuseDrag()
const float vwe = _state.wind_vel(1);
// predicted specific forces
// calculate relative wind velocity in earth frame and rotte into body frame
Vector3f rel_wind;
rel_wind(0) = vn - vwn;
rel_wind(1) = ve - vwe;
rel_wind(2) = vd;
// calculate relative wind velocity in earth frame and rotate into body frame
const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
rel_wind = earth_to_body * rel_wind;
const Vector3f rel_wind_body = earth_to_body * rel_wind_earth;
// perform sequential fusion of XY specific forces
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
@ -151,14 +148,14 @@ void Ekf::fuseDrag()
// calculate the predicted acceleration and innovation measured along the X body axis
float drag_sign;
if (rel_wind(axis_index) >= 0.0f) {
if (rel_wind_body(axis_index) >= 0.0f) {
drag_sign = 1.0f;
} else {
drag_sign = -1.0f;
}
const float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
const float predAccel = -BC_inv_x * 0.5f * 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]);
@ -226,14 +223,14 @@ void Ekf::fuseDrag()
// calculate the predicted acceleration and innovation measured along the Y body axis
float drag_sign;
if (rel_wind(axis_index) >= 0.0f) {
if (rel_wind_body(axis_index) >= 0.0f) {
drag_sign = 1.0f;
} else {
drag_sign = -1.0f;
}
const float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
const float predAccel = -BC_inv_y * 0.5f * 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]);

22
EKF/sideslip_fusion.cpp
View File

@ -67,19 +67,13 @@ void Ekf::fuseSideslip()
const float vwn = _state.wind_vel(0);
const float vwe = _state.wind_vel(1);
// relative wind velocity in earth frame
Vector3f rel_wind;
rel_wind(0) = vn - vwn;
rel_wind(1) = ve - vwe;
rel_wind(2) = vd;
// calculate relative wind velocity in earth frame and rotate into body frame
const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
// rotate into body axes
rel_wind = earth_to_body * rel_wind;
const Vector3f rel_wind_body = earth_to_body * rel_wind_earth;
// perform fusion of assumed sideslip = 0
if (rel_wind.norm() > 7.0f) {
if (rel_wind_body.norm() > 7.0f) {
// Calculate the observation jacobians
// intermediate variable from algebraic optimisation
@ -89,9 +83,9 @@ void Ekf::fuseSideslip()
return;
}
SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - (vn - vwn)*(2.0f*q0*q3 - 2.0f*q1*q2);
SH_BETA[2] = vn - vwn;
SH_BETA[3] = ve - vwe;
SH_BETA[1] = rel_wind_earth(1)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - rel_wind_earth(0)*(2.0f*q0*q3 - 2.0f*q1*q2);
SH_BETA[2] = rel_wind_earth(0);
SH_BETA[3] = rel_wind_earth(1);
SH_BETA[4] = 1.0f/sq(SH_BETA[0]);
SH_BETA[5] = 1.0f/SH_BETA[0];
SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
@ -200,7 +194,7 @@ void Ekf::fuseSideslip()
Kfusion[23] = SK_BETA[0]*(P(23,0)*SK_BETA[5] + P(23,1)*SK_BETA[4] - P(23,4)*SK_BETA[1] + P(23,5)*SK_BETA[2] + P(23,2)*SK_BETA[6] + P(23,6)*SK_BETA[3] - P(23,3)*SK_BETA[7] + P(23,22)*SK_BETA[1] - P(23,23)*SK_BETA[2]);
// Calculate predicted sideslip angle and innovation using small angle approximation
_beta_innov = rel_wind(1) / rel_wind(0);
_beta_innov = rel_wind_body(1) / rel_wind_body(0);
// Compute the ratio of innovation to gate size
_beta_test_ratio = sq(_beta_innov) / (sq(fmaxf(_params.beta_innov_gate, 1.0f)) * _beta_innov_var);