From dc3fb650d531a547f3bbb7bb6278978d72dff458 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Mon, 3 Jun 2024 16:22:16 +0200 Subject: [PATCH] adjustments to derivation and unittest --- .../external_vision/ev_vel_control.cpp | 6 +- .../EKF/python/ekf_derivation/derivation.py | 5 - .../compute_ev_body_vel_var_and_hx.h | 141 +++++++++++++----- .../compute_ev_body_vel_var_and_hy.h | 54 +++++-- .../compute_ev_body_vel_var_and_hz.h | 57 +++++-- .../ekf2/test/test_EKF_externalVision.cpp | 25 +--- 6 files changed, 203 insertions(+), 85 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index 7ef1a7835ef3..b34362b7a0e5 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp @@ -85,7 +85,8 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState()); measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; - measurement_var = rotateVarianceToEkf(ev_sample.velocity_var); + measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * + R_ev_to_ekf.transpose()).diag(); minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max()); } @@ -241,7 +242,8 @@ void Ekf::stopEvVelFusion() } } -void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame) +void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, + const VelocityFrame &vel_frame) { if (vel_frame == VelocityFrame::BODY_FRAME_FRD) { const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 9aae28155c07..0205dd65dc87 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -377,9 +377,6 @@ def compute_ev_body_vel_var_and_hx( Hx = jacobian_chain_rule(meas_pred[0], state) Hy = jacobian_chain_rule(meas_pred[1], state) Hz = jacobian_chain_rule(meas_pred[2], state) - Hx[:3] = [0,0,0] - Hy[:3] = [0,0,0] - Hz[:3] = [0,0,0] body_vel_var = sf.V3() # attention: +R needs to be added to the variance in the calling function body_vel_var[0] = (Hx * P * Hx.T)[0,0] @@ -395,7 +392,6 @@ def compute_ev_body_vel_var_and_hy( state = vstate_to_state(state) meas_pred = predict_vel_body(state)[1] Hy = jacobian_chain_rule(meas_pred, state) - Hy[:3] = [0,0,0] # attention: +R needs to be added to the variance in the calling function body_vel_var = (Hy * P * Hy.T)[0,0] return (body_vel_var, Hy.T) @@ -408,7 +404,6 @@ def compute_ev_body_vel_var_and_hz( state = vstate_to_state(state) meas_pred = predict_vel_body(state)[2] Hz = jacobian_chain_rule(meas_pred, state) - Hz[:3] = [0,0,0] # attention: +R needs to be added to the variance in the calling function body_vel_var = (Hz * P * Hz.T)[0,0] return (body_vel_var, Hz.T) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hx.h index 5aea65061f0c..bf23c5566083 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hx.h @@ -28,45 +28,115 @@ void ComputeEvBodyVelVarAndHx(const matrix::Matrix& state, const matrix::Matrix& P, matrix::Matrix* const incomp_innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 88 + // Total ops: 368 // Input arrays - // Intermediate terms (20) - const Scalar _tmp0 = -2 * std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = 1 - 2 * std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp2 = _tmp0 + _tmp1; - const Scalar _tmp3 = 2 * state(2, 0); - const Scalar _tmp4 = _tmp3 * state(0, 0); - const Scalar _tmp5 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp6 = -_tmp4 + _tmp5; - const Scalar _tmp7 = 2 * state(0, 0); - const Scalar _tmp8 = _tmp7 * state(3, 0); - const Scalar _tmp9 = _tmp3 * state(1, 0); - const Scalar _tmp10 = _tmp8 + _tmp9; - const Scalar _tmp11 = -2 * std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp12 = _tmp0 + _tmp11 + 1; - const Scalar _tmp13 = _tmp3 * state(3, 0); - const Scalar _tmp14 = _tmp7 * state(1, 0); - const Scalar _tmp15 = _tmp13 + _tmp14; - const Scalar _tmp16 = -_tmp8 + _tmp9; - const Scalar _tmp17 = _tmp4 + _tmp5; - const Scalar _tmp18 = _tmp13 - _tmp14; - const Scalar _tmp19 = _tmp1 + _tmp11; + // Intermediate terms (63) + const Scalar _tmp0 = 2 * state(5, 0); + const Scalar _tmp1 = _tmp0 * state(3, 0); + const Scalar _tmp2 = 2 * state(6, 0); + const Scalar _tmp3 = _tmp2 * state(2, 0); + const Scalar _tmp4 = _tmp1 - _tmp3; + const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp6 = _tmp0 * state(2, 0); + const Scalar _tmp7 = _tmp2 * state(3, 0); + const Scalar _tmp8 = _tmp6 + _tmp7; + const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp10 = 4 * state(4, 0); + const Scalar _tmp11 = _tmp0 * state(0, 0); + const Scalar _tmp12 = 2 * state(1, 0); + const Scalar _tmp13 = _tmp12 * state(6, 0); + const Scalar _tmp14 = -_tmp10 * state(3, 0) + _tmp11 + _tmp13; + const Scalar _tmp15 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp16 = _tmp0 * state(1, 0); + const Scalar _tmp17 = _tmp2 * state(0, 0); + const Scalar _tmp18 = -_tmp10 * state(2, 0) + _tmp16 - _tmp17; + const Scalar _tmp19 = (Scalar(1) / Scalar(2)) * state(0, 0); + const Scalar _tmp20 = -_tmp14 * _tmp15 + _tmp18 * _tmp19 - _tmp4 * _tmp5 + _tmp8 * _tmp9; + const Scalar _tmp21 = 2 * state(2, 0); + const Scalar _tmp22 = _tmp21 * state(0, 0); + const Scalar _tmp23 = _tmp12 * state(3, 0); + const Scalar _tmp24 = -_tmp22 + _tmp23; + const Scalar _tmp25 = 2 * state(0, 0) * state(3, 0); + const Scalar _tmp26 = _tmp12 * state(2, 0); + const Scalar _tmp27 = _tmp25 + _tmp26; + const Scalar _tmp28 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp29 = 1 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp30 = _tmp28 + _tmp29; + const Scalar _tmp31 = _tmp14 * _tmp19 + _tmp15 * _tmp18 - _tmp4 * _tmp9 - _tmp5 * _tmp8; + const Scalar _tmp32 = _tmp14 * _tmp5 - _tmp15 * _tmp4 - _tmp18 * _tmp9 + _tmp19 * _tmp8; + const Scalar _tmp33 = -_tmp25 + _tmp26; + const Scalar _tmp34 = _tmp21 * state(3, 0); + const Scalar _tmp35 = _tmp12 * state(0, 0); + const Scalar _tmp36 = _tmp34 + _tmp35; + const Scalar _tmp37 = -2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp38 = _tmp28 + _tmp37 + 1; + const Scalar _tmp39 = 2 * state(4, 0); + const Scalar _tmp40 = _tmp39 * state(3, 0); + const Scalar _tmp41 = _tmp13 - _tmp40; + const Scalar _tmp42 = _tmp39 * state(1, 0); + const Scalar _tmp43 = _tmp42 + _tmp7; + const Scalar _tmp44 = _tmp39 * state(0, 0); + const Scalar _tmp45 = 4 * state(5, 0); + const Scalar _tmp46 = _tmp3 - _tmp44 - _tmp45 * state(3, 0); + const Scalar _tmp47 = _tmp39 * state(2, 0); + const Scalar _tmp48 = _tmp17 - _tmp45 * state(1, 0) + _tmp47; + const Scalar _tmp49 = _tmp15 * _tmp43 + _tmp19 * _tmp46 - _tmp41 * _tmp9 - _tmp48 * _tmp5; + const Scalar _tmp50 = -_tmp15 * _tmp46 + _tmp19 * _tmp43 - _tmp41 * _tmp5 + _tmp48 * _tmp9; + const Scalar _tmp51 = -_tmp15 * _tmp41 + _tmp19 * _tmp48 - _tmp43 * _tmp9 + _tmp46 * _tmp5; + const Scalar _tmp52 = _tmp42 + _tmp6; + const Scalar _tmp53 = -_tmp16 + _tmp47; + const Scalar _tmp54 = 4 * state(6, 0); + const Scalar _tmp55 = _tmp1 + _tmp44 - _tmp54 * state(2, 0); + const Scalar _tmp56 = -_tmp11 + _tmp40 - _tmp54 * state(1, 0); + const Scalar _tmp57 = -_tmp15 * _tmp52 + _tmp19 * _tmp55 - _tmp5 * _tmp53 + _tmp56 * _tmp9; + const Scalar _tmp58 = _tmp34 - _tmp35; + const Scalar _tmp59 = _tmp22 + _tmp23; + const Scalar _tmp60 = _tmp29 + _tmp37; + const Scalar _tmp61 = _tmp15 * _tmp55 + _tmp19 * _tmp52 - _tmp5 * _tmp56 - _tmp53 * _tmp9; + const Scalar _tmp62 = -_tmp15 * _tmp53 + _tmp19 * _tmp56 + _tmp5 * _tmp52 - _tmp55 * _tmp9; // Output terms (2) if (incomp_innov_var != nullptr) { matrix::Matrix& _incomp_innov_var = (*incomp_innov_var); - _incomp_innov_var(0, 0) = _tmp10 * (P(3, 4) * _tmp2 + P(4, 4) * _tmp10 + P(5, 4) * _tmp6) + - _tmp2 * (P(3, 3) * _tmp2 + P(4, 3) * _tmp10 + P(5, 3) * _tmp6) + - _tmp6 * (P(3, 5) * _tmp2 + P(4, 5) * _tmp10 + P(5, 5) * _tmp6); - _incomp_innov_var(1, 0) = _tmp12 * (P(3, 4) * _tmp16 + P(4, 4) * _tmp12 + P(5, 4) * _tmp15) + - _tmp15 * (P(3, 5) * _tmp16 + P(4, 5) * _tmp12 + P(5, 5) * _tmp15) + - _tmp16 * (P(3, 3) * _tmp16 + P(4, 3) * _tmp12 + P(5, 3) * _tmp15); - _incomp_innov_var(2, 0) = _tmp17 * (P(3, 3) * _tmp17 + P(4, 3) * _tmp18 + P(5, 3) * _tmp19) + - _tmp18 * (P(3, 4) * _tmp17 + P(4, 4) * _tmp18 + P(5, 4) * _tmp19) + - _tmp19 * (P(3, 5) * _tmp17 + P(4, 5) * _tmp18 + P(5, 5) * _tmp19); + _incomp_innov_var(0, 0) = _tmp20 * (P(0, 1) * _tmp32 + P(1, 1) * _tmp20 + P(2, 1) * _tmp31 + + P(3, 1) * _tmp30 + P(4, 1) * _tmp27 + P(5, 1) * _tmp24) + + _tmp24 * (P(0, 5) * _tmp32 + P(1, 5) * _tmp20 + P(2, 5) * _tmp31 + + P(3, 5) * _tmp30 + P(4, 5) * _tmp27 + P(5, 5) * _tmp24) + + _tmp27 * (P(0, 4) * _tmp32 + P(1, 4) * _tmp20 + P(2, 4) * _tmp31 + + P(3, 4) * _tmp30 + P(4, 4) * _tmp27 + P(5, 4) * _tmp24) + + _tmp30 * (P(0, 3) * _tmp32 + P(1, 3) * _tmp20 + P(2, 3) * _tmp31 + + P(3, 3) * _tmp30 + P(4, 3) * _tmp27 + P(5, 3) * _tmp24) + + _tmp31 * (P(0, 2) * _tmp32 + P(1, 2) * _tmp20 + P(2, 2) * _tmp31 + + P(3, 2) * _tmp30 + P(4, 2) * _tmp27 + P(5, 2) * _tmp24) + + _tmp32 * (P(0, 0) * _tmp32 + P(1, 0) * _tmp20 + P(2, 0) * _tmp31 + + P(3, 0) * _tmp30 + P(4, 0) * _tmp27 + P(5, 0) * _tmp24); + _incomp_innov_var(1, 0) = _tmp33 * (P(0, 3) * _tmp51 + P(1, 3) * _tmp50 + P(2, 3) * _tmp49 + + P(3, 3) * _tmp33 + P(4, 3) * _tmp38 + P(5, 3) * _tmp36) + + _tmp36 * (P(0, 5) * _tmp51 + P(1, 5) * _tmp50 + P(2, 5) * _tmp49 + + P(3, 5) * _tmp33 + P(4, 5) * _tmp38 + P(5, 5) * _tmp36) + + _tmp38 * (P(0, 4) * _tmp51 + P(1, 4) * _tmp50 + P(2, 4) * _tmp49 + + P(3, 4) * _tmp33 + P(4, 4) * _tmp38 + P(5, 4) * _tmp36) + + _tmp49 * (P(0, 2) * _tmp51 + P(1, 2) * _tmp50 + P(2, 2) * _tmp49 + + P(3, 2) * _tmp33 + P(4, 2) * _tmp38 + P(5, 2) * _tmp36) + + _tmp50 * (P(0, 1) * _tmp51 + P(1, 1) * _tmp50 + P(2, 1) * _tmp49 + + P(3, 1) * _tmp33 + P(4, 1) * _tmp38 + P(5, 1) * _tmp36) + + _tmp51 * (P(0, 0) * _tmp51 + P(1, 0) * _tmp50 + P(2, 0) * _tmp49 + + P(3, 0) * _tmp33 + P(4, 0) * _tmp38 + P(5, 0) * _tmp36); + _incomp_innov_var(2, 0) = _tmp57 * (P(0, 1) * _tmp62 + P(1, 1) * _tmp57 + P(2, 1) * _tmp61 + + P(3, 1) * _tmp59 + P(4, 1) * _tmp58 + P(5, 1) * _tmp60) + + _tmp58 * (P(0, 4) * _tmp62 + P(1, 4) * _tmp57 + P(2, 4) * _tmp61 + + P(3, 4) * _tmp59 + P(4, 4) * _tmp58 + P(5, 4) * _tmp60) + + _tmp59 * (P(0, 3) * _tmp62 + P(1, 3) * _tmp57 + P(2, 3) * _tmp61 + + P(3, 3) * _tmp59 + P(4, 3) * _tmp58 + P(5, 3) * _tmp60) + + _tmp60 * (P(0, 5) * _tmp62 + P(1, 5) * _tmp57 + P(2, 5) * _tmp61 + + P(3, 5) * _tmp59 + P(4, 5) * _tmp58 + P(5, 5) * _tmp60) + + _tmp61 * (P(0, 2) * _tmp62 + P(1, 2) * _tmp57 + P(2, 2) * _tmp61 + + P(3, 2) * _tmp59 + P(4, 2) * _tmp58 + P(5, 2) * _tmp60) + + _tmp62 * (P(0, 0) * _tmp62 + P(1, 0) * _tmp57 + P(2, 0) * _tmp61 + + P(3, 0) * _tmp59 + P(4, 0) * _tmp58 + P(5, 0) * _tmp60); } if (H != nullptr) { @@ -74,9 +144,12 @@ void ComputeEvBodyVelVarAndHx(const matrix::Matrix& state, _h.setZero(); - _h(3, 0) = _tmp2; - _h(4, 0) = _tmp10; - _h(5, 0) = _tmp6; + _h(0, 0) = _tmp32; + _h(1, 0) = _tmp20; + _h(2, 0) = _tmp31; + _h(3, 0) = _tmp30; + _h(4, 0) = _tmp27; + _h(5, 0) = _tmp24; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hy.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hy.h index 37916096c178..7d4b63428935 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hy.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hy.h @@ -28,25 +28,52 @@ void ComputeEvBodyVelVarAndHy(const matrix::Matrix& state, const matrix::Matrix& P, Scalar* const incomp_innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 34 + // Total ops: 137 // Input arrays - // Intermediate terms (5) - const Scalar _tmp0 = + // Intermediate terms (19) + const Scalar _tmp0 = 2 * state(3, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(2, 0); + const Scalar _tmp3 = _tmp0 * state(2, 0) + _tmp1 * state(0, 0); + const Scalar _tmp4 = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; - const Scalar _tmp1 = 2 * state(3, 0); - const Scalar _tmp2 = 2 * state(1, 0); - const Scalar _tmp3 = _tmp1 * state(2, 0) + _tmp2 * state(0, 0); - const Scalar _tmp4 = -_tmp1 * state(0, 0) + _tmp2 * state(2, 0); + const Scalar _tmp5 = 2 * state(4, 0); + const Scalar _tmp6 = _tmp1 * state(6, 0) - _tmp5 * state(3, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp8 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(6, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(4, 0); + const Scalar _tmp9 = 4 * state(5, 0); + const Scalar _tmp10 = 2 * state(6, 0); + const Scalar _tmp11 = _tmp10 * state(2, 0) - _tmp5 * state(0, 0) - _tmp9 * state(3, 0); + const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp13 = _tmp10 * state(0, 0) + _tmp5 * state(2, 0) - _tmp9 * state(1, 0); + const Scalar _tmp14 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp15 = + _tmp12 * state(0, 0) - _tmp13 * _tmp14 - _tmp7 * state(3, 0) + _tmp8 * state(1, 0); + const Scalar _tmp16 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp17 = + -_tmp12 * state(1, 0) - _tmp14 * _tmp6 + _tmp16 * state(3, 0) + _tmp8 * state(0, 0); + const Scalar _tmp18 = + _tmp11 * _tmp14 + _tmp16 * state(0, 0) - _tmp7 * state(1, 0) - _tmp8 * state(3, 0); // Output terms (2) if (incomp_innov_var != nullptr) { Scalar& _incomp_innov_var = (*incomp_innov_var); - _incomp_innov_var = _tmp0 * (P(3, 4) * _tmp4 + P(4, 4) * _tmp0 + P(5, 4) * _tmp3) + - _tmp3 * (P(3, 5) * _tmp4 + P(4, 5) * _tmp0 + P(5, 5) * _tmp3) + - _tmp4 * (P(3, 3) * _tmp4 + P(4, 3) * _tmp0 + P(5, 3) * _tmp3); + _incomp_innov_var = _tmp15 * (P(0, 2) * _tmp18 + P(1, 2) * _tmp17 + P(2, 2) * _tmp15 + + P(3, 2) * _tmp2 + P(4, 2) * _tmp4 + P(5, 2) * _tmp3) + + _tmp17 * (P(0, 1) * _tmp18 + P(1, 1) * _tmp17 + P(2, 1) * _tmp15 + + P(3, 1) * _tmp2 + P(4, 1) * _tmp4 + P(5, 1) * _tmp3) + + _tmp18 * (P(0, 0) * _tmp18 + P(1, 0) * _tmp17 + P(2, 0) * _tmp15 + + P(3, 0) * _tmp2 + P(4, 0) * _tmp4 + P(5, 0) * _tmp3) + + _tmp2 * (P(0, 3) * _tmp18 + P(1, 3) * _tmp17 + P(2, 3) * _tmp15 + + P(3, 3) * _tmp2 + P(4, 3) * _tmp4 + P(5, 3) * _tmp3) + + _tmp3 * (P(0, 5) * _tmp18 + P(1, 5) * _tmp17 + P(2, 5) * _tmp15 + + P(3, 5) * _tmp2 + P(4, 5) * _tmp4 + P(5, 5) * _tmp3) + + _tmp4 * (P(0, 4) * _tmp18 + P(1, 4) * _tmp17 + P(2, 4) * _tmp15 + + P(3, 4) * _tmp2 + P(4, 4) * _tmp4 + P(5, 4) * _tmp3); } if (H != nullptr) { @@ -54,8 +81,11 @@ void ComputeEvBodyVelVarAndHy(const matrix::Matrix& state, _h.setZero(); - _h(3, 0) = _tmp4; - _h(4, 0) = _tmp0; + _h(0, 0) = _tmp18; + _h(1, 0) = _tmp17; + _h(2, 0) = _tmp15; + _h(3, 0) = _tmp2; + _h(4, 0) = _tmp4; _h(5, 0) = _tmp3; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hz.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hz.h index 37f44b40b2cc..370b3a96fa27 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hz.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hz.h @@ -28,25 +28,53 @@ void ComputeEvBodyVelVarAndHz(const matrix::Matrix& state, const matrix::Matrix& P, Scalar* const incomp_innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 34 + // Total ops: 141 // Input arrays - // Intermediate terms (5) - const Scalar _tmp0 = 2 * state(2, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0); - const Scalar _tmp3 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0); - const Scalar _tmp4 = + // Intermediate terms (15) + const Scalar _tmp0 = 2 * state(4, 0); + const Scalar _tmp1 = 2 * state(5, 0); + const Scalar _tmp2 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0); + const Scalar _tmp3 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - Scalar(1) / Scalar(2) * _tmp1 * state(1, 0); + const Scalar _tmp4 = 2 * state(0, 0); + const Scalar _tmp5 = 4 * state(6, 0); + const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0) + + (Scalar(1) / Scalar(2)) * _tmp4 * state(4, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(2, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp0 * state(3, 0) - + Scalar(1) / Scalar(2) * _tmp4 * state(5, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(1, 0); + const Scalar _tmp8 = + -_tmp2 * state(1, 0) - _tmp3 * state(2, 0) + _tmp6 * state(0, 0) + _tmp7 * state(3, 0); + const Scalar _tmp9 = 2 * state(3, 0); + const Scalar _tmp10 = -_tmp4 * state(1, 0) + _tmp9 * state(2, 0); + const Scalar _tmp11 = _tmp4 * state(2, 0) + _tmp9 * state(1, 0); + const Scalar _tmp12 = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; + const Scalar _tmp13 = + _tmp2 * state(0, 0) - _tmp3 * state(3, 0) + _tmp6 * state(1, 0) - _tmp7 * state(2, 0); + const Scalar _tmp14 = + _tmp2 * state(2, 0) - _tmp3 * state(1, 0) - _tmp6 * state(3, 0) + _tmp7 * state(0, 0); // Output terms (2) if (incomp_innov_var != nullptr) { Scalar& _incomp_innov_var = (*incomp_innov_var); - _incomp_innov_var = _tmp2 * (P(3, 3) * _tmp2 + P(4, 3) * _tmp3 + P(5, 3) * _tmp4) + - _tmp3 * (P(3, 4) * _tmp2 + P(4, 4) * _tmp3 + P(5, 4) * _tmp4) + - _tmp4 * (P(3, 5) * _tmp2 + P(4, 5) * _tmp3 + P(5, 5) * _tmp4); + _incomp_innov_var = _tmp10 * (P(0, 4) * _tmp14 + P(1, 4) * _tmp8 + P(2, 4) * _tmp13 + + P(3, 4) * _tmp11 + P(4, 4) * _tmp10 + P(5, 4) * _tmp12) + + _tmp11 * (P(0, 3) * _tmp14 + P(1, 3) * _tmp8 + P(2, 3) * _tmp13 + + P(3, 3) * _tmp11 + P(4, 3) * _tmp10 + P(5, 3) * _tmp12) + + _tmp12 * (P(0, 5) * _tmp14 + P(1, 5) * _tmp8 + P(2, 5) * _tmp13 + + P(3, 5) * _tmp11 + P(4, 5) * _tmp10 + P(5, 5) * _tmp12) + + _tmp13 * (P(0, 2) * _tmp14 + P(1, 2) * _tmp8 + P(2, 2) * _tmp13 + + P(3, 2) * _tmp11 + P(4, 2) * _tmp10 + P(5, 2) * _tmp12) + + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp8 + P(2, 0) * _tmp13 + + P(3, 0) * _tmp11 + P(4, 0) * _tmp10 + P(5, 0) * _tmp12) + + _tmp8 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp8 + P(2, 1) * _tmp13 + + P(3, 1) * _tmp11 + P(4, 1) * _tmp10 + P(5, 1) * _tmp12); } if (H != nullptr) { @@ -54,9 +82,12 @@ void ComputeEvBodyVelVarAndHz(const matrix::Matrix& state, _h.setZero(); - _h(3, 0) = _tmp2; - _h(4, 0) = _tmp3; - _h(5, 0) = _tmp4; + _h(0, 0) = _tmp14; + _h(1, 0) = _tmp8; + _h(2, 0) = _tmp13; + _h(3, 0) = _tmp11; + _h(4, 0) = _tmp10; + _h(5, 0) = _tmp12; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 734d485fbcd3..5b1439edd9f2 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -259,37 +259,24 @@ TEST_F(EkfExternalVisionTest, visionAlignment) TEST_F(EkfExternalVisionTest, velocityFrameBody) { - // GIVEN: Drone is turned 90 degrees - const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f))); - _sensor_simulator.simulateOrientation(quat_sim); + _ekf_wrapper.setMagFuseTypeNone(); _sensor_simulator.runSeconds(_tilt_align_time); - _ekf->set_vehicle_at_rest(false); - // Without any measurement x and y velocity variance are close - const Vector3f velVar_init = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); - - // WHEN: measurement is given in BODY-FRAME and - // x variance is bigger than y variance - _sensor_simulator._vio.setVelocityFrameToBody(); + float yaw_var0 = _ekf->getYawVar(); const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f); const Vector3f vel_body(1.0f, 0.0f, 0.0f); + _sensor_simulator._vio.setVelocityFrameToBody(); _sensor_simulator._vio.setVelocityVariance(vel_cov_body); _sensor_simulator._vio.setVelocity(vel_body); _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(4); - // THEN: As the drone is turned 90 degrees, velocity variance - // along local y axis is expected to be bigger - const Vector3f velVar_new = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_new(1) / velVar_new(0), 30.f, 15.f); - - const Vector3f vel_earth_est = _ekf->getVelocity(); - EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f); - EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f); + const Vector3f vel_var = _ekf->getVelocityVariance(); + EXPECT_TRUE(yaw_var0 < _ekf->getYawVar()); + EXPECT_TRUE(vel_var(1) < vel_var(0)); } TEST_F(EkfExternalVisionTest, velocityFrameLocal)