Skip to content

Commit

Permalink
adjustments to derivation and unittest
Browse files Browse the repository at this point in the history
  • Loading branch information
@haumarco
haumarco committed Jun 14, 2024
1 parent 35c65d7 commit dc3fb65
Show file tree
Hide file tree
Showing 6 changed files with 203 additions and 85 deletions.
6 changes: 4 additions & 2 deletions src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}

Expand Down Expand Up @@ -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);
Expand Down
5 changes: 0 additions & 5 deletions src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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)
Expand All @@ -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)
Expand Down
141 changes: 107 additions & 34 deletions src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,55 +28,128 @@ void ComputeEvBodyVelVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 23, 23>& P,
matrix::Matrix<Scalar, 3, 1>* const incomp_innov_var = nullptr,
matrix::Matrix<Scalar, 23, 1>* 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<Scalar, 3, 1>& _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) {
matrix::Matrix<Scalar, 23, 1>& _h = (*H);

_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)

Expand Down
54 changes: 42 additions & 12 deletions src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hy.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,34 +28,64 @@ void ComputeEvBodyVelVarAndHy(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 23, 23>& P,
Scalar* const incomp_innov_var = nullptr,
matrix::Matrix<Scalar, 23, 1>* 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) {
matrix::Matrix<Scalar, 23, 1>& _h = (*H);

_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)
Expand Down
57 changes: 44 additions & 13 deletions src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hz.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,35 +28,66 @@ void ComputeEvBodyVelVarAndHz(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 23, 23>& P,
Scalar* const incomp_innov_var = nullptr,
matrix::Matrix<Scalar, 23, 1>* 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) {
matrix::Matrix<Scalar, 23, 1>& _h = (*H);

_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)

Expand Down
Loading

0 comments on commit dc3fb65

Please sign in to comment.