From a3706fdceff6e93761cc2ae25cedee48e1e658df Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sun, 21 Jun 2020 12:00:46 +0200 Subject: [PATCH] Make relative wind computation more compact --- EKF/drag_fusion.cpp | 17 +++++++---------- EKF/sideslip_fusion.cpp | 22 ++++++++-------------- 2 files changed, 15 insertions(+), 24 deletions(-) diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 2242c1a88a..28f1f04fd4 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -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]); diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 77c59819ce..5d9f9d01ea 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -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);