From d9169ba07d6e2c1d1657a41c5db8a3365299406f Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Mar 2024 18:57:19 +0100 Subject: [PATCH] ekf2: use global definition of quaternion error --- src/modules/ekf2/EKF/covariance.cpp | 3 +- src/modules/ekf2/EKF/ekf.h | 7 +- src/modules/ekf2/EKF/ekf_helper.cpp | 27 +- .../EKF/python/ekf_derivation/derivation.py | 77 +- .../compute_drag_x_innov_var_and_h.h | 227 ++--- .../compute_drag_y_innov_var_and_h.h | 231 +++--- .../compute_flow_xy_innov_var_and_hx.h | 138 ++-- .../compute_flow_y_innov_var_and_h.h | 82 +- .../compute_gnss_yaw_pred_innov_var_and_h.h | 85 +- .../compute_gravity_xyz_innov_var_and_hx.h | 49 +- .../compute_gravity_y_innov_var_and_h.h | 14 +- .../compute_gravity_z_innov_var_and_h.h | 4 +- .../compute_mag_innov_innov_var_and_hx.h | 234 +++--- .../generated/compute_mag_y_innov_var_and_h.h | 82 +- .../generated/compute_mag_z_innov_var_and_h.h | 86 +- .../generated/compute_sideslip_h_and_k.h | 219 ++--- .../compute_sideslip_innov_and_innov_var.h | 141 ++-- .../generated/compute_yaw_innov_var_and_h.h | 21 +- .../generated/predict_covariance.h | 772 ++++++++++-------- src/modules/ekf2/EKF2.cpp | 2 +- src/modules/ekf2/test/test_EKF_flow.cpp | 6 +- .../ekf2/test/test_EKF_initialization.cpp | 2 +- .../test/test_EKF_yaw_fusion_generated.cpp | 5 +- 23 files changed, 1346 insertions(+), 1168 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index c6ba03f256..482596070b 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -279,8 +279,7 @@ void Ekf::resetQuatCov(const float yaw_noise) void Ekf::resetQuatCov(const Vector3f &rot_var_ned) { - matrix::SquareMatrix q_cov_ned = diag(rot_var_ned); - resetStateCovariance(_R_to_earth.T() * q_cov_ned * _R_to_earth); + P.uncorrelateCovarianceSetVariance(State::quat_nominal.idx, rot_var_ned); } #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index bc087558e9..b8b02a0634 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -254,7 +254,7 @@ public: // get the diagonal elements of the covariance matrix matrix::Vector covariances_diagonal() const { return P.diag(); } - matrix::Vector getQuaternionVariance() const { return getStateVariance(); } + matrix::Vector3f getRotVarBody() const; matrix::Vector3f getRotVarNed() const; float getYawVar() const; float getTiltVariance() const; @@ -1109,7 +1109,7 @@ private: #endif // CONFIG_EKF2_GRAVITY_FUSION void resetQuatCov(const float yaw_noise = NAN); - void resetQuatCov(const Vector3f &euler_noise_ned); + void resetQuatCov(const Vector3f &rot_var_ned); #if defined(CONFIG_EKF2_MAGNETOMETER) void resetMagCov(); @@ -1124,9 +1124,6 @@ private: void resetGyroBiasZCov(); - // uncorrelate quaternion states from other states - void uncorrelateQuatFromOtherStates(); - bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const { return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index bb9e901f57..5cf357a870 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -552,7 +552,7 @@ void Ekf::fuse(const VectorState &K, float innovation) { // quat_nominal Quatf delta_quat(matrix::AxisAnglef(K.slice(State::quat_nominal.idx, 0) * (-1.f * innovation))); - _state.quat_nominal *= delta_quat; + _state.quat_nominal = delta_quat * _state.quat_nominal; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); @@ -586,11 +586,6 @@ void Ekf::fuse(const VectorState &K, float innovation) #endif // CONFIG_EKF2_WIND } -void Ekf::uncorrelateQuatFromOtherStates() -{ - P.uncorrelateCovarianceBlock(State::quat_nominal.idx); -} - void Ekf::updateDeadReckoningStatus() { updateHorizontalDeadReckoningstatus(); @@ -660,10 +655,16 @@ void Ekf::updateVerticalDeadReckoningStatus() } } -Vector3f Ekf::getRotVarNed() const +Vector3f Ekf::getRotVarBody() const { const matrix::SquareMatrix3f rot_cov_body = getStateCovariance(); - return matrix::SquareMatrix(_R_to_earth * rot_cov_body * _R_to_earth.T()).diag(); + return matrix::SquareMatrix3f(_R_to_earth.T() * rot_cov_body * _R_to_earth).diag(); +} + +Vector3f Ekf::getRotVarNed() const +{ + const matrix::SquareMatrix3f rot_cov_ned = getStateCovariance(); + return rot_cov_ned.diag(); } float Ekf::getYawVar() const @@ -707,12 +708,10 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) // save a copy of the quaternion state for later use in calculating the amount of reset change const Quatf quat_before_reset = _state.quat_nominal; - // save a copy of covariance in NED frame to restore it after the quat reset - Vector3f rot_var_ned_before_reset = getRotVarNed(); - // update the yaw angle variance if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) { - rot_var_ned_before_reset(2) = yaw_variance; + // rot_var_ned_before_reset(2) = yaw_variance; + P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance); } // update transformation matrix from body to world frame using the current estimate @@ -725,10 +724,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) // update quaternion states _state.quat_nominal = quat_after_reset; - uncorrelateQuatFromOtherStates(); - - // restore covariance - resetQuatCov(rot_var_ned_before_reset); // add the reset amount to the output observer buffered data _output_predictor.resetQuaternion(q_error); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 5cbe1a127f..df3947e459 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -148,7 +148,7 @@ def predict_covariance( for key in state.keys(): if key == "quat_nominal": # Create true quaternion using small angle approximation of the error rotation - state_t["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(state_error["theta"] / 2), w=1)) + state_t["quat_nominal"] = sf.Rot3(sf.Quaternion(xyz=(state_error["theta"] / 2), w=1)) * state["quat_nominal"] else: state_t[key] = state[key] + state_error[key] @@ -184,7 +184,7 @@ def predict_covariance( state_error_pred = Values() for key in state_error.keys(): if key == "theta": - delta_q = sf.Quaternion.from_storage(state_pred["quat_nominal"].to_storage()).conj() * sf.Quaternion.from_storage(state_t_pred["quat_nominal"].to_storage()) + delta_q = sf.Quaternion.from_storage(state_t_pred["quat_nominal"].to_storage()) * sf.Quaternion.from_storage(state_pred["quat_nominal"].to_storage()).conj() state_error_pred["theta"] = 2 * sf.V3(delta_q.x, delta_q.y, delta_q.z) # Use small angle approximation to obtain a simpler jacobian else: state_error_pred[key] = state_t_pred[key] - state_pred[key] @@ -216,6 +216,36 @@ def predict_covariance( return P_new +def jacobian_chain_rule(expr: sf.Scalar , state: State): + # First compute the jacobian in the parameter space + dh_dx = sf.V1(expr).jacobian(state, tangent_space=False) + + class MStorageTangent(sf.Matrix): + SHAPE = (State.storage_dim(), State.tangent_dim()) + + # Then compute the jarobian mapping infinitesimal elements of the parameter space to the error state + # Note that this jacobian only depends on the structure of the EKF + dx_derror = MStorageTangent() + q = sf.Quaternion.from_storage(state["quat_nominal"].to_storage()) + p = sf.Quaternion.symbolic('p') + + pq = p * q + qR = sf.M41(pq.to_storage()).jacobian(sf.M41(p.to_storage())) # Right quaternion product matrix + dx_derror[0:4, 0:3] = qR / 2 * sf.M43([[1, 0, 0], + [0, 1, 0], + [0, 0, 1], + [0, 0, 0]]) + + # The rest of the matrix is trivial + for i in range(4, State.storage_dim()): + for j in range(3, State.tangent_dim()): + if (i == j+1): + dx_derror[i, j] = 1 + + # Finally use the chain rule: dh/derror = dh/dx * dx/derror + H = dh_dx * dx_derror + return H + def compute_airspeed_innov_and_innov_var( state: VState, P: MTangent, @@ -231,7 +261,7 @@ def compute_airspeed_innov_and_innov_var( innov = airspeed_pred - airspeed - H = sf.V1(airspeed_pred).jacobian(state) + H = jacobian_chain_rule(airspeed_pred, state) innov_var = (H * P * H.T + R)[0,0] return (innov, innov_var) @@ -247,7 +277,7 @@ def compute_airspeed_h_and_k( wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) vel_rel = state["vel"] - wind airspeed_pred = vel_rel.norm(epsilon=epsilon) - H = sf.V1(airspeed_pred).jacobian(state) + H = jacobian_chain_rule(airspeed_pred, state) K = P * H.T / sf.Max(innov_var, epsilon) @@ -309,7 +339,7 @@ def compute_sideslip_innov_and_innov_var( innov = sideslip_pred - 0.0 - H = sf.V1(sideslip_pred).jacobian(state) + H = jacobian_chain_rule(sideslip_pred, state) innov_var = (H * P * H.T + R)[0,0] return (innov, innov_var) @@ -324,7 +354,7 @@ def compute_sideslip_h_and_k( state = vstate_to_state(state) sideslip_pred = predict_sideslip(state, epsilon); - H = sf.V1(sideslip_pred).jacobian(state) + H = jacobian_chain_rule(sideslip_pred, state) K = P * H.T / sf.Max(innov_var, epsilon) @@ -351,11 +381,11 @@ def compute_mag_innov_innov_var_and_hx( innov = meas_pred - meas innov_var = sf.V3() - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = jacobian_chain_rule(meas_pred[0], state) innov_var[0] = (Hx * P * Hx.T + R)[0,0] - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = jacobian_chain_rule(meas_pred[1], state) innov_var[1] = (Hy * P * Hy.T + R)[0,0] - Hz = sf.V1(meas_pred[2]).jacobian(state) + Hz = jacobian_chain_rule(meas_pred[2], state) innov_var[2] = (Hz * P * Hz.T + R)[0,0] return (innov, innov_var, Hx.T) @@ -370,7 +400,7 @@ def compute_mag_y_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_mag_body(state); - H = sf.V1(meas_pred[1]).jacobian(state) + H = jacobian_chain_rule(meas_pred[1], state) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -385,7 +415,7 @@ def compute_mag_z_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_mag_body(state); - H = sf.V1(meas_pred[2]).jacobian(state) + H = jacobian_chain_rule(meas_pred[2], state) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -402,8 +432,11 @@ def compute_yaw_innov_var_and_h( delta_q = q * r.conj() # create a quaternion error of the measurement at the origin delta_meas_pred = 2 * delta_q.z # Use small angle approximation to obtain a simpler jacobian - H = sf.V1(delta_meas_pred).jacobian(state) + H = jacobian_chain_rule(delta_meas_pred, state) H = H.subs({r.w: q.w, r.x: q.x, r.y: q.y, r.z: q.z}) # assume innovation is small + + for i in range(State.tangent_dim()): + H[i] = sp.factor(H[i]).subs(q.w**2 + q.x**2 + q.y**2 + q.z**2, 1) # unit norm quaternion innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -418,7 +451,7 @@ def compute_mag_declination_pred_innov_var_and_h( state = vstate_to_state(state) meas_pred = sf.atan2(state["mag_I"][1], state["mag_I"][0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = jacobian_chain_rule(meas_pred, state) innov_var = (H * P * H.T + R)[0,0] return (meas_pred, innov_var, H.T) @@ -450,9 +483,9 @@ def compute_flow_xy_innov_var_and_hx( meas_pred = predict_opt_flow(state, distance, epsilon); innov_var = sf.V2() - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = jacobian_chain_rule(meas_pred[0], state) innov_var[0] = (Hx * P * Hx.T + R)[0,0] - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = jacobian_chain_rule(meas_pred[1], state) innov_var[1] = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hx.T) @@ -467,7 +500,7 @@ def compute_flow_y_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_opt_flow(state, distance, epsilon); - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = jacobian_chain_rule(meas_pred[1], state) innov_var = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hy.T) @@ -492,7 +525,7 @@ def compute_gnss_yaw_pred_innov_var_and_h( # Calculate the yaw angle from the projection meas_pred = sf.atan2(ant_vec_ef[1], ant_vec_ef[0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = jacobian_chain_rule(meas_pred, state) innov_var = (H * P * H.T + R)[0,0] return (meas_pred, innov_var, H.T) @@ -529,7 +562,7 @@ def compute_drag_x_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_drag(state, rho, cd, cm, epsilon) - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = jacobian_chain_rule(meas_pred[0], state) innov_var = (Hx * P * Hx.T + R)[0,0] return (innov_var, Hx.T) @@ -546,7 +579,7 @@ def compute_drag_y_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_drag(state, rho, cd, cm, epsilon) - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = jacobian_chain_rule(meas_pred[1], state) innov_var = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hy.T) @@ -575,7 +608,7 @@ def compute_gravity_xyz_innov_var_and_hx( # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) # for each axis for i in range(3): - H[i] = sf.V1(meas_pred[i]).jacobian(state) + H[i] = jacobian_chain_rule(meas_pred[i], state) innov_var[i] = (H[i] * P * H[i].T + R)[0,0] return (innov_var, H[0].T) @@ -590,7 +623,7 @@ def compute_gravity_y_innov_var_and_h( meas_pred = predict_gravity_direction(state) # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) - H = sf.V1(meas_pred[1]).jacobian(state) + H = jacobian_chain_rule(meas_pred[1], state) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -605,7 +638,7 @@ def compute_gravity_z_innov_var_and_h( meas_pred = predict_gravity_direction(state) # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) - H = sf.V1(meas_pred[2]).jacobian(state) + H = jacobian_chain_rule(meas_pred[2], state) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h index 4a08ef76f1..1b7c762f2c 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h @@ -34,95 +34,108 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix& state, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const Hx = nullptr) { - // Total ops: 317 + // Total ops: 357 // Input arrays - // Intermediate terms (73) - const Scalar _tmp0 = 2 * state(3, 0); - const Scalar _tmp1 = _tmp0 * state(0, 0); - const Scalar _tmp2 = 2 * state(2, 0); - const Scalar _tmp3 = _tmp2 * state(1, 0); + // Intermediate terms (79) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = _tmp0 * state(3, 0); + const Scalar _tmp2 = 2 * state(1, 0); + const Scalar _tmp3 = _tmp2 * state(2, 0); const Scalar _tmp4 = _tmp1 + _tmp3; const Scalar _tmp5 = _tmp4 * cm; - const Scalar _tmp6 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp7 = -2 * _tmp6; - const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp9 = -2 * _tmp8; - const Scalar _tmp10 = _tmp7 + _tmp9 + 1; - const Scalar _tmp11 = -state(22, 0) + state(4, 0); - const Scalar _tmp12 = -state(23, 0) + state(5, 0); - const Scalar _tmp13 = _tmp2 * state(0, 0); - const Scalar _tmp14 = -_tmp13; - const Scalar _tmp15 = _tmp0 * state(1, 0); - const Scalar _tmp16 = _tmp14 + _tmp15; - const Scalar _tmp17 = _tmp12 * _tmp4 + _tmp16 * state(6, 0); - const Scalar _tmp18 = _tmp10 * _tmp11 + _tmp17; - const Scalar _tmp19 = 2 * _tmp18; - const Scalar _tmp20 = _tmp19 * _tmp4; - const Scalar _tmp21 = _tmp0 * state(2, 0); - const Scalar _tmp22 = 2 * state(0, 0) * state(1, 0); - const Scalar _tmp23 = -_tmp22; - const Scalar _tmp24 = _tmp21 + _tmp23; - const Scalar _tmp25 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp26 = 1 - 2 * _tmp25; - const Scalar _tmp27 = _tmp26 + _tmp9; - const Scalar _tmp28 = _tmp13 + _tmp15; - const Scalar _tmp29 = _tmp11 * _tmp28 + _tmp12 * _tmp24; - const Scalar _tmp30 = _tmp27 * state(6, 0) + _tmp29; + const Scalar _tmp6 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp7 = -2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp8 = _tmp6 + _tmp7 + 1; + const Scalar _tmp9 = -state(22, 0) + state(4, 0); + const Scalar _tmp10 = -state(23, 0) + state(5, 0); + const Scalar _tmp11 = 2 * state(2, 0); + const Scalar _tmp12 = _tmp11 * state(0, 0); + const Scalar _tmp13 = _tmp2 * state(3, 0); + const Scalar _tmp14 = -_tmp12 + _tmp13; + const Scalar _tmp15 = _tmp10 * _tmp4 + _tmp14 * state(6, 0) + _tmp8 * _tmp9; + const Scalar _tmp16 = 2 * _tmp15; + const Scalar _tmp17 = _tmp16 * _tmp4; + const Scalar _tmp18 = _tmp11 * state(3, 0); + const Scalar _tmp19 = _tmp2 * state(0, 0); + const Scalar _tmp20 = _tmp18 - _tmp19; + const Scalar _tmp21 = _tmp12 + _tmp13; + const Scalar _tmp22 = 1 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp23 = _tmp22 + _tmp7; + const Scalar _tmp24 = _tmp10 * _tmp20 + _tmp21 * _tmp9 + _tmp23 * state(6, 0); + const Scalar _tmp25 = 2 * _tmp24; + const Scalar _tmp26 = _tmp20 * _tmp25; + const Scalar _tmp27 = _tmp22 + _tmp6; + const Scalar _tmp28 = -_tmp1 + _tmp3; + const Scalar _tmp29 = _tmp18 + _tmp19; + const Scalar _tmp30 = _tmp10 * _tmp27 + _tmp28 * _tmp9 + _tmp29 * state(6, 0); const Scalar _tmp31 = 2 * _tmp30; - const Scalar _tmp32 = _tmp24 * _tmp31; - const Scalar _tmp33 = _tmp26 + _tmp7; - const Scalar _tmp34 = -_tmp1; - const Scalar _tmp35 = _tmp3 + _tmp34; - const Scalar _tmp36 = _tmp21 + _tmp22; - const Scalar _tmp37 = _tmp11 * _tmp35 + _tmp36 * state(6, 0); - const Scalar _tmp38 = _tmp12 * _tmp33 + _tmp37; - const Scalar _tmp39 = 2 * _tmp38; - const Scalar _tmp40 = _tmp33 * _tmp39; - const Scalar _tmp41 = std::sqrt(Scalar(std::pow(_tmp18, Scalar(2)) + std::pow(_tmp30, Scalar(2)) + - std::pow(_tmp38, Scalar(2)) + epsilon)); - const Scalar _tmp42 = cd * rho; - const Scalar _tmp43 = Scalar(0.25) * _tmp18 * _tmp42 / _tmp41; - const Scalar _tmp44 = Scalar(0.5) * _tmp41 * _tmp42; - const Scalar _tmp45 = _tmp4 * _tmp44; - const Scalar _tmp46 = -_tmp43 * (_tmp20 + _tmp32 + _tmp40) - _tmp45 - _tmp5; - const Scalar _tmp47 = -_tmp25; - const Scalar _tmp48 = _tmp47 + _tmp6; - const Scalar _tmp49 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp50 = -_tmp49; - const Scalar _tmp51 = _tmp50 + _tmp8; - const Scalar _tmp52 = -_tmp3; - const Scalar _tmp53 = -_tmp15; - const Scalar _tmp54 = -_tmp6; - const Scalar _tmp55 = _tmp12 * (_tmp47 + _tmp49 + _tmp54 + _tmp8) + _tmp37; - const Scalar _tmp56 = -_tmp43 * (_tmp19 * _tmp55 + _tmp39 * (_tmp11 * (_tmp48 + _tmp51) + - _tmp12 * (_tmp34 + _tmp52) + - state(6, 0) * (_tmp13 + _tmp53))) - - _tmp44 * _tmp55 - _tmp55 * cm; - const Scalar _tmp57 = -_tmp43 * (-_tmp20 - _tmp32 - _tmp40) + _tmp45 + _tmp5; - const Scalar _tmp58 = _tmp10 * cm; - const Scalar _tmp59 = _tmp10 * _tmp19; - const Scalar _tmp60 = _tmp28 * _tmp31; - const Scalar _tmp61 = _tmp35 * _tmp39; - const Scalar _tmp62 = _tmp10 * _tmp44; - const Scalar _tmp63 = -_tmp43 * (-_tmp59 - _tmp60 - _tmp61) + _tmp58 + _tmp62; - const Scalar _tmp64 = -_tmp8; - const Scalar _tmp65 = -_tmp21; - const Scalar _tmp66 = _tmp49 + _tmp64; - const Scalar _tmp67 = - _tmp43 * (_tmp31 * (_tmp11 * (_tmp1 + _tmp52) + _tmp12 * (_tmp25 + _tmp50 + _tmp6 + _tmp64) + - state(6, 0) * (_tmp23 + _tmp65)) + - _tmp39 * (_tmp29 + state(6, 0) * (_tmp48 + _tmp66))); - const Scalar _tmp68 = _tmp25 + _tmp54; - const Scalar _tmp69 = - _tmp11 * (_tmp14 + _tmp53) + _tmp12 * (_tmp22 + _tmp65) + state(6, 0) * (_tmp51 + _tmp68); - const Scalar _tmp70 = - -_tmp43 * (_tmp19 * _tmp69 + _tmp31 * (_tmp11 * (_tmp66 + _tmp68) + _tmp17)) - - _tmp44 * _tmp69 - _tmp69 * cm; - const Scalar _tmp71 = -_tmp43 * (_tmp59 + _tmp60 + _tmp61) - _tmp58 - _tmp62; - const Scalar _tmp72 = -_tmp16 * _tmp44 - _tmp16 * cm - - _tmp43 * (_tmp16 * _tmp19 + _tmp27 * _tmp31 + _tmp36 * _tmp39); + const Scalar _tmp32 = _tmp27 * _tmp31; + const Scalar _tmp33 = std::sqrt(Scalar(std::pow(_tmp15, Scalar(2)) + std::pow(_tmp24, Scalar(2)) + + std::pow(_tmp30, Scalar(2)) + epsilon)); + const Scalar _tmp34 = cd * rho; + const Scalar _tmp35 = Scalar(0.25) * _tmp15 * _tmp34 / _tmp33; + const Scalar _tmp36 = Scalar(0.5) * _tmp33 * _tmp34; + const Scalar _tmp37 = _tmp36 * _tmp4; + const Scalar _tmp38 = -_tmp35 * (-_tmp17 - _tmp26 - _tmp32) + _tmp37 + _tmp5; + const Scalar _tmp39 = -_tmp35 * (_tmp17 + _tmp26 + _tmp32) - _tmp37 - _tmp5; + const Scalar _tmp40 = _tmp8 * cm; + const Scalar _tmp41 = _tmp16 * _tmp8; + const Scalar _tmp42 = _tmp21 * _tmp25; + const Scalar _tmp43 = _tmp28 * _tmp31; + const Scalar _tmp44 = _tmp36 * _tmp8; + const Scalar _tmp45 = -_tmp35 * (-_tmp41 - _tmp42 - _tmp43) + _tmp40 + _tmp44; + const Scalar _tmp46 = 2 * state(3, 0); + const Scalar _tmp47 = _tmp10 * _tmp46; + const Scalar _tmp48 = 2 * state(6, 0); + const Scalar _tmp49 = _tmp48 * state(2, 0); + const Scalar _tmp50 = _tmp47 - _tmp49; + const Scalar _tmp51 = _tmp10 * _tmp2; + const Scalar _tmp52 = _tmp11 * _tmp9; + const Scalar _tmp53 = _tmp46 * _tmp9; + const Scalar _tmp54 = _tmp2 * state(6, 0); + const Scalar _tmp55 = + -_tmp35 * (_tmp16 * _tmp50 + _tmp25 * (-_tmp51 + _tmp52) + _tmp31 * (-_tmp53 + _tmp54)) - + _tmp36 * _tmp50 - _tmp50 * cm; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp55; + const Scalar _tmp57 = _tmp10 * _tmp11; + const Scalar _tmp58 = _tmp48 * state(3, 0); + const Scalar _tmp59 = _tmp57 + _tmp58; + const Scalar _tmp60 = _tmp0 * _tmp10; + const Scalar _tmp61 = 4 * state(6, 0); + const Scalar _tmp62 = 4 * _tmp10; + const Scalar _tmp63 = _tmp48 * state(0, 0); + const Scalar _tmp64 = + -_tmp35 * (_tmp16 * _tmp59 + _tmp25 * (_tmp53 - _tmp60 - _tmp61 * state(1, 0)) + + _tmp31 * (_tmp52 - _tmp62 * state(1, 0) + _tmp63)) - + _tmp36 * _tmp59 - _tmp59 * cm; + const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp66 = 4 * _tmp9; + const Scalar _tmp67 = _tmp54 + _tmp60 - _tmp66 * state(3, 0); + const Scalar _tmp68 = _tmp2 * _tmp9; + const Scalar _tmp69 = _tmp0 * _tmp9; + const Scalar _tmp70 = -Scalar(1) / Scalar(2) * _tmp35 * + (_tmp16 * _tmp67 + _tmp25 * (_tmp57 + _tmp68) + + _tmp31 * (_tmp49 - _tmp62 * state(3, 0) - _tmp69)) - + Scalar(1) / Scalar(2) * _tmp36 * _tmp67 - + Scalar(1) / Scalar(2) * _tmp67 * cm; + const Scalar _tmp71 = _tmp51 - _tmp63 - _tmp66 * state(2, 0); + const Scalar _tmp72 = -Scalar(1) / Scalar(2) * _tmp35 * + (_tmp16 * _tmp71 + _tmp25 * (_tmp47 - _tmp61 * state(2, 0) + _tmp69) + + _tmp31 * (_tmp58 + _tmp68)) - + Scalar(1) / Scalar(2) * _tmp36 * _tmp71 - + Scalar(1) / Scalar(2) * _tmp71 * cm; + const Scalar _tmp73 = + -_tmp56 * state(3, 0) - _tmp64 * _tmp65 + _tmp70 * state(0, 0) + _tmp72 * state(1, 0); + const Scalar _tmp74 = (Scalar(1) / Scalar(2)) * _tmp64; + const Scalar _tmp75 = + -_tmp55 * _tmp65 - _tmp70 * state(1, 0) + _tmp72 * state(0, 0) + _tmp74 * state(3, 0); + const Scalar _tmp76 = + -_tmp56 * state(1, 0) + _tmp70 * state(2, 0) - _tmp72 * state(3, 0) + _tmp74 * state(0, 0); + const Scalar _tmp77 = -_tmp35 * (_tmp41 + _tmp42 + _tmp43) - _tmp40 - _tmp44; + const Scalar _tmp78 = -_tmp14 * _tmp36 - _tmp14 * cm - + _tmp35 * (_tmp14 * _tmp16 + _tmp23 * _tmp25 + _tmp29 * _tmp31); // Output terms (2) if (innov_var != nullptr) { @@ -130,22 +143,22 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix& state, _innov_var = R + - _tmp46 * (-P(0, 4) * _tmp67 + P(1, 4) * _tmp70 + P(2, 4) * _tmp56 + P(21, 4) * _tmp63 + - P(22, 4) * _tmp57 + P(3, 4) * _tmp71 + P(4, 4) * _tmp46 + P(5, 4) * _tmp72) + - _tmp56 * (-P(0, 2) * _tmp67 + P(1, 2) * _tmp70 + P(2, 2) * _tmp56 + P(21, 2) * _tmp63 + - P(22, 2) * _tmp57 + P(3, 2) * _tmp71 + P(4, 2) * _tmp46 + P(5, 2) * _tmp72) + - _tmp57 * (-P(0, 22) * _tmp67 + P(1, 22) * _tmp70 + P(2, 22) * _tmp56 + P(21, 22) * _tmp63 + - P(22, 22) * _tmp57 + P(3, 22) * _tmp71 + P(4, 22) * _tmp46 + P(5, 22) * _tmp72) + - _tmp63 * (-P(0, 21) * _tmp67 + P(1, 21) * _tmp70 + P(2, 21) * _tmp56 + P(21, 21) * _tmp63 + - P(22, 21) * _tmp57 + P(3, 21) * _tmp71 + P(4, 21) * _tmp46 + P(5, 21) * _tmp72) - - _tmp67 * (-P(0, 0) * _tmp67 + P(1, 0) * _tmp70 + P(2, 0) * _tmp56 + P(21, 0) * _tmp63 + - P(22, 0) * _tmp57 + P(3, 0) * _tmp71 + P(4, 0) * _tmp46 + P(5, 0) * _tmp72) + - _tmp70 * (-P(0, 1) * _tmp67 + P(1, 1) * _tmp70 + P(2, 1) * _tmp56 + P(21, 1) * _tmp63 + - P(22, 1) * _tmp57 + P(3, 1) * _tmp71 + P(4, 1) * _tmp46 + P(5, 1) * _tmp72) + - _tmp71 * (-P(0, 3) * _tmp67 + P(1, 3) * _tmp70 + P(2, 3) * _tmp56 + P(21, 3) * _tmp63 + - P(22, 3) * _tmp57 + P(3, 3) * _tmp71 + P(4, 3) * _tmp46 + P(5, 3) * _tmp72) + - _tmp72 * (-P(0, 5) * _tmp67 + P(1, 5) * _tmp70 + P(2, 5) * _tmp56 + P(21, 5) * _tmp63 + - P(22, 5) * _tmp57 + P(3, 5) * _tmp71 + P(4, 5) * _tmp46 + P(5, 5) * _tmp72); + _tmp38 * (P(0, 22) * _tmp76 + P(1, 22) * _tmp75 + P(2, 22) * _tmp73 + P(21, 22) * _tmp45 + + P(22, 22) * _tmp38 + P(3, 22) * _tmp77 + P(4, 22) * _tmp39 + P(5, 22) * _tmp78) + + _tmp39 * (P(0, 4) * _tmp76 + P(1, 4) * _tmp75 + P(2, 4) * _tmp73 + P(21, 4) * _tmp45 + + P(22, 4) * _tmp38 + P(3, 4) * _tmp77 + P(4, 4) * _tmp39 + P(5, 4) * _tmp78) + + _tmp45 * (P(0, 21) * _tmp76 + P(1, 21) * _tmp75 + P(2, 21) * _tmp73 + P(21, 21) * _tmp45 + + P(22, 21) * _tmp38 + P(3, 21) * _tmp77 + P(4, 21) * _tmp39 + P(5, 21) * _tmp78) + + _tmp73 * (P(0, 2) * _tmp76 + P(1, 2) * _tmp75 + P(2, 2) * _tmp73 + P(21, 2) * _tmp45 + + P(22, 2) * _tmp38 + P(3, 2) * _tmp77 + P(4, 2) * _tmp39 + P(5, 2) * _tmp78) + + _tmp75 * (P(0, 1) * _tmp76 + P(1, 1) * _tmp75 + P(2, 1) * _tmp73 + P(21, 1) * _tmp45 + + P(22, 1) * _tmp38 + P(3, 1) * _tmp77 + P(4, 1) * _tmp39 + P(5, 1) * _tmp78) + + _tmp76 * (P(0, 0) * _tmp76 + P(1, 0) * _tmp75 + P(2, 0) * _tmp73 + P(21, 0) * _tmp45 + + P(22, 0) * _tmp38 + P(3, 0) * _tmp77 + P(4, 0) * _tmp39 + P(5, 0) * _tmp78) + + _tmp77 * (P(0, 3) * _tmp76 + P(1, 3) * _tmp75 + P(2, 3) * _tmp73 + P(21, 3) * _tmp45 + + P(22, 3) * _tmp38 + P(3, 3) * _tmp77 + P(4, 3) * _tmp39 + P(5, 3) * _tmp78) + + _tmp78 * (P(0, 5) * _tmp76 + P(1, 5) * _tmp75 + P(2, 5) * _tmp73 + P(21, 5) * _tmp45 + + P(22, 5) * _tmp38 + P(3, 5) * _tmp77 + P(4, 5) * _tmp39 + P(5, 5) * _tmp78); } if (Hx != nullptr) { @@ -153,14 +166,14 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix& state, _hx.setZero(); - _hx(0, 0) = -_tmp67; - _hx(1, 0) = _tmp70; - _hx(2, 0) = _tmp56; - _hx(3, 0) = _tmp71; - _hx(4, 0) = _tmp46; - _hx(5, 0) = _tmp72; - _hx(21, 0) = _tmp63; - _hx(22, 0) = _tmp57; + _hx(0, 0) = _tmp76; + _hx(1, 0) = _tmp75; + _hx(2, 0) = _tmp73; + _hx(3, 0) = _tmp77; + _hx(4, 0) = _tmp39; + _hx(5, 0) = _tmp78; + _hx(21, 0) = _tmp45; + _hx(22, 0) = _tmp38; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h index c504d0db63..505f4ed4de 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h @@ -34,97 +34,108 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix& state, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const Hy = nullptr) { - // Total ops: 317 + // Total ops: 360 // Input arrays - // Intermediate terms (73) - const Scalar _tmp0 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = _tmp1 * state(0, 0); - const Scalar _tmp3 = _tmp0 + _tmp2; - const Scalar _tmp4 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp5 = -2 * _tmp4; - const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp7 = -2 * _tmp6; - const Scalar _tmp8 = _tmp5 + _tmp7 + 1; + // Intermediate terms (76) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = _tmp0 * state(3, 0); + const Scalar _tmp2 = 2 * state(1, 0); + const Scalar _tmp3 = _tmp2 * state(2, 0); + const Scalar _tmp4 = -_tmp1 + _tmp3; + const Scalar _tmp5 = _tmp4 * cm; + const Scalar _tmp6 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp7 = -2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp8 = _tmp6 + _tmp7 + 1; const Scalar _tmp9 = -state(22, 0) + state(4, 0); - const Scalar _tmp10 = 2 * state(0, 0); - const Scalar _tmp11 = _tmp10 * state(3, 0); - const Scalar _tmp12 = _tmp1 * state(2, 0); - const Scalar _tmp13 = _tmp11 + _tmp12; - const Scalar _tmp14 = -state(23, 0) + state(5, 0); - const Scalar _tmp15 = _tmp10 * state(2, 0); - const Scalar _tmp16 = -_tmp15; - const Scalar _tmp17 = _tmp1 * state(3, 0); - const Scalar _tmp18 = _tmp16 + _tmp17; - const Scalar _tmp19 = _tmp13 * _tmp14 + _tmp18 * state(6, 0); - const Scalar _tmp20 = _tmp19 + _tmp8 * _tmp9; - const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp22 = 1 - 2 * _tmp21; - const Scalar _tmp23 = _tmp22 + _tmp7; - const Scalar _tmp24 = -_tmp2; - const Scalar _tmp25 = _tmp0 + _tmp24; - const Scalar _tmp26 = _tmp15 + _tmp17; - const Scalar _tmp27 = _tmp14 * _tmp25 + _tmp26 * _tmp9; - const Scalar _tmp28 = _tmp23 * state(6, 0) + _tmp27; - const Scalar _tmp29 = _tmp22 + _tmp5; - const Scalar _tmp30 = -_tmp11; - const Scalar _tmp31 = _tmp12 + _tmp30; - const Scalar _tmp32 = _tmp3 * state(6, 0) + _tmp31 * _tmp9; - const Scalar _tmp33 = _tmp14 * _tmp29 + _tmp32; - const Scalar _tmp34 = std::sqrt(Scalar(std::pow(_tmp20, Scalar(2)) + std::pow(_tmp28, Scalar(2)) + - std::pow(_tmp33, Scalar(2)) + epsilon)); - const Scalar _tmp35 = cd * rho; - const Scalar _tmp36 = Scalar(0.5) * _tmp34 * _tmp35; - const Scalar _tmp37 = 2 * _tmp20; - const Scalar _tmp38 = 2 * _tmp28; - const Scalar _tmp39 = 2 * _tmp33; - const Scalar _tmp40 = Scalar(0.25) * _tmp33 * _tmp35 / _tmp34; - const Scalar _tmp41 = - -_tmp3 * _tmp36 - _tmp3 * cm - _tmp40 * (_tmp18 * _tmp37 + _tmp23 * _tmp38 + _tmp3 * _tmp39); - const Scalar _tmp42 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp43 = -_tmp42; - const Scalar _tmp44 = -_tmp6; - const Scalar _tmp45 = -_tmp12; - const Scalar _tmp46 = -_tmp0; - const Scalar _tmp47 = -_tmp21; - const Scalar _tmp48 = _tmp4 + _tmp47; - const Scalar _tmp49 = _tmp42 + _tmp44; - const Scalar _tmp50 = _tmp27 + state(6, 0) * (_tmp48 + _tmp49); - const Scalar _tmp51 = - -_tmp36 * _tmp50 - - _tmp40 * (_tmp38 * (_tmp14 * (_tmp21 + _tmp4 + _tmp43 + _tmp44) + _tmp9 * (_tmp11 + _tmp45) + - state(6, 0) * (_tmp24 + _tmp46)) + - _tmp39 * _tmp50) - - _tmp50 * cm; - const Scalar _tmp52 = _tmp43 + _tmp6; - const Scalar _tmp53 = -_tmp17; - const Scalar _tmp54 = - _tmp14 * (_tmp30 + _tmp45) + _tmp9 * (_tmp48 + _tmp52) + state(6, 0) * (_tmp15 + _tmp53); - const Scalar _tmp55 = -_tmp4; + const Scalar _tmp10 = _tmp1 + _tmp3; + const Scalar _tmp11 = -state(23, 0) + state(5, 0); + const Scalar _tmp12 = _tmp0 * state(2, 0); + const Scalar _tmp13 = _tmp2 * state(3, 0); + const Scalar _tmp14 = -_tmp12 + _tmp13; + const Scalar _tmp15 = _tmp10 * _tmp11 + _tmp14 * state(6, 0) + _tmp8 * _tmp9; + const Scalar _tmp16 = 2 * state(3, 0); + const Scalar _tmp17 = _tmp16 * state(2, 0); + const Scalar _tmp18 = _tmp2 * state(0, 0); + const Scalar _tmp19 = _tmp17 - _tmp18; + const Scalar _tmp20 = _tmp12 + _tmp13; + const Scalar _tmp21 = 1 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp22 = _tmp21 + _tmp7; + const Scalar _tmp23 = _tmp11 * _tmp19 + _tmp20 * _tmp9 + _tmp22 * state(6, 0); + const Scalar _tmp24 = _tmp21 + _tmp6; + const Scalar _tmp25 = _tmp17 + _tmp18; + const Scalar _tmp26 = _tmp11 * _tmp24 + _tmp25 * state(6, 0) + _tmp4 * _tmp9; + const Scalar _tmp27 = std::sqrt(Scalar(std::pow(_tmp15, Scalar(2)) + std::pow(_tmp23, Scalar(2)) + + std::pow(_tmp26, Scalar(2)) + epsilon)); + const Scalar _tmp28 = cd * rho; + const Scalar _tmp29 = Scalar(0.5) * _tmp27 * _tmp28; + const Scalar _tmp30 = _tmp29 * _tmp4; + const Scalar _tmp31 = 2 * _tmp15; + const Scalar _tmp32 = _tmp31 * _tmp8; + const Scalar _tmp33 = 2 * _tmp23; + const Scalar _tmp34 = _tmp20 * _tmp33; + const Scalar _tmp35 = 2 * _tmp26; + const Scalar _tmp36 = _tmp35 * _tmp4; + const Scalar _tmp37 = Scalar(0.25) * _tmp26 * _tmp28 / _tmp27; + const Scalar _tmp38 = _tmp30 - _tmp37 * (-_tmp32 - _tmp34 - _tmp36) + _tmp5; + const Scalar _tmp39 = -_tmp30 - _tmp37 * (_tmp32 + _tmp34 + _tmp36) - _tmp5; + const Scalar _tmp40 = _tmp24 * cm; + const Scalar _tmp41 = _tmp10 * _tmp31; + const Scalar _tmp42 = _tmp19 * _tmp33; + const Scalar _tmp43 = _tmp24 * _tmp35; + const Scalar _tmp44 = _tmp24 * _tmp29; + const Scalar _tmp45 = -_tmp37 * (_tmp41 + _tmp42 + _tmp43) - _tmp40 - _tmp44; + const Scalar _tmp46 = _tmp16 * _tmp9; + const Scalar _tmp47 = _tmp0 * _tmp11; + const Scalar _tmp48 = state(1, 0) * state(6, 0); + const Scalar _tmp49 = 2 * state(2, 0); + const Scalar _tmp50 = _tmp11 * _tmp49; + const Scalar _tmp51 = _tmp16 * state(6, 0); + const Scalar _tmp52 = 4 * _tmp11; + const Scalar _tmp53 = _tmp49 * _tmp9; + const Scalar _tmp54 = _tmp0 * state(6, 0); + const Scalar _tmp55 = -_tmp52 * state(1, 0) + _tmp53 + _tmp54; const Scalar _tmp56 = - -_tmp36 * _tmp54 - - _tmp40 * (_tmp37 * (_tmp14 * (_tmp42 + _tmp47 + _tmp55 + _tmp6) + _tmp32) + _tmp39 * _tmp54) - - _tmp54 * cm; - const Scalar _tmp57 = _tmp29 * cm; - const Scalar _tmp58 = _tmp13 * _tmp37; - const Scalar _tmp59 = _tmp25 * _tmp38; - const Scalar _tmp60 = _tmp29 * _tmp39; - const Scalar _tmp61 = _tmp29 * _tmp36; - const Scalar _tmp62 = -_tmp40 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61; - const Scalar _tmp63 = _tmp21 + _tmp55; - const Scalar _tmp64 = _tmp40 * (_tmp37 * (_tmp14 * (_tmp2 + _tmp46) + _tmp9 * (_tmp16 + _tmp53) + - state(6, 0) * (_tmp52 + _tmp63)) + - _tmp38 * (_tmp19 + _tmp9 * (_tmp49 + _tmp63))); - const Scalar _tmp65 = -_tmp40 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61; - const Scalar _tmp66 = _tmp31 * cm; - const Scalar _tmp67 = _tmp31 * _tmp36; - const Scalar _tmp68 = _tmp37 * _tmp8; - const Scalar _tmp69 = _tmp26 * _tmp38; - const Scalar _tmp70 = _tmp31 * _tmp39; - const Scalar _tmp71 = -_tmp40 * (_tmp68 + _tmp69 + _tmp70) - _tmp66 - _tmp67; - const Scalar _tmp72 = -_tmp40 * (-_tmp68 - _tmp69 - _tmp70) + _tmp66 + _tmp67; + -Scalar(1) / Scalar(2) * _tmp29 * _tmp55 - + Scalar(1) / Scalar(2) * _tmp37 * + (_tmp31 * (_tmp50 + _tmp51) + _tmp33 * (_tmp46 - _tmp47 - 4 * _tmp48) + _tmp35 * _tmp55) - + Scalar(1) / Scalar(2) * _tmp55 * cm; + const Scalar _tmp57 = 2 * _tmp48; + const Scalar _tmp58 = -_tmp46 + _tmp57; + const Scalar _tmp59 = _tmp11 * _tmp2; + const Scalar _tmp60 = _tmp11 * _tmp16; + const Scalar _tmp61 = state(2, 0) * state(6, 0); + const Scalar _tmp62 = 2 * _tmp61; + const Scalar _tmp63 = + -Scalar(1) / Scalar(2) * _tmp29 * _tmp58 - + Scalar(1) / Scalar(2) * _tmp37 * + (_tmp31 * (_tmp60 - _tmp62) + _tmp33 * (_tmp53 - _tmp59) + _tmp35 * _tmp58) - + Scalar(1) / Scalar(2) * _tmp58 * cm; + const Scalar _tmp64 = _tmp2 * _tmp9; + const Scalar _tmp65 = _tmp51 + _tmp64; + const Scalar _tmp66 = _tmp0 * _tmp9; + const Scalar _tmp67 = 4 * _tmp9; + const Scalar _tmp68 = -Scalar(1) / Scalar(2) * _tmp29 * _tmp65 - + Scalar(1) / Scalar(2) * _tmp37 * + (_tmp31 * (-_tmp54 + _tmp59 - _tmp67 * state(2, 0)) + + _tmp33 * (_tmp60 - 4 * _tmp61 + _tmp66) + _tmp35 * _tmp65) - + Scalar(1) / Scalar(2) * _tmp65 * cm; + const Scalar _tmp69 = -_tmp52 * state(3, 0) + _tmp62 - _tmp66; + const Scalar _tmp70 = -Scalar(1) / Scalar(2) * _tmp29 * _tmp69 - + Scalar(1) / Scalar(2) * _tmp37 * + (_tmp31 * (_tmp47 + _tmp57 - _tmp67 * state(3, 0)) + + _tmp33 * (_tmp50 + _tmp64) + _tmp35 * _tmp69) - + Scalar(1) / Scalar(2) * _tmp69 * cm; + const Scalar _tmp71 = + -_tmp56 * state(2, 0) - _tmp63 * state(3, 0) + _tmp68 * state(1, 0) + _tmp70 * state(0, 0); + const Scalar _tmp72 = + _tmp56 * state(3, 0) - _tmp63 * state(2, 0) + _tmp68 * state(0, 0) - _tmp70 * state(1, 0); + const Scalar _tmp73 = + _tmp56 * state(0, 0) - _tmp63 * state(1, 0) - _tmp68 * state(3, 0) + _tmp70 * state(2, 0); + const Scalar _tmp74 = -_tmp37 * (-_tmp41 - _tmp42 - _tmp43) + _tmp40 + _tmp44; + const Scalar _tmp75 = -_tmp25 * _tmp29 - _tmp25 * cm - + _tmp37 * (_tmp14 * _tmp31 + _tmp22 * _tmp33 + _tmp25 * _tmp35); // Output terms (2) if (innov_var != nullptr) { @@ -132,22 +143,22 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix& state, _innov_var = R + - _tmp41 * (P(0, 5) * _tmp51 - P(1, 5) * _tmp64 + P(2, 5) * _tmp56 + P(21, 5) * _tmp72 + - P(22, 5) * _tmp62 + P(3, 5) * _tmp71 + P(4, 5) * _tmp65 + P(5, 5) * _tmp41) + - _tmp51 * (P(0, 0) * _tmp51 - P(1, 0) * _tmp64 + P(2, 0) * _tmp56 + P(21, 0) * _tmp72 + - P(22, 0) * _tmp62 + P(3, 0) * _tmp71 + P(4, 0) * _tmp65 + P(5, 0) * _tmp41) + - _tmp56 * (P(0, 2) * _tmp51 - P(1, 2) * _tmp64 + P(2, 2) * _tmp56 + P(21, 2) * _tmp72 + - P(22, 2) * _tmp62 + P(3, 2) * _tmp71 + P(4, 2) * _tmp65 + P(5, 2) * _tmp41) + - _tmp62 * (P(0, 22) * _tmp51 - P(1, 22) * _tmp64 + P(2, 22) * _tmp56 + P(21, 22) * _tmp72 + - P(22, 22) * _tmp62 + P(3, 22) * _tmp71 + P(4, 22) * _tmp65 + P(5, 22) * _tmp41) - - _tmp64 * (P(0, 1) * _tmp51 - P(1, 1) * _tmp64 + P(2, 1) * _tmp56 + P(21, 1) * _tmp72 + - P(22, 1) * _tmp62 + P(3, 1) * _tmp71 + P(4, 1) * _tmp65 + P(5, 1) * _tmp41) + - _tmp65 * (P(0, 4) * _tmp51 - P(1, 4) * _tmp64 + P(2, 4) * _tmp56 + P(21, 4) * _tmp72 + - P(22, 4) * _tmp62 + P(3, 4) * _tmp71 + P(4, 4) * _tmp65 + P(5, 4) * _tmp41) + - _tmp71 * (P(0, 3) * _tmp51 - P(1, 3) * _tmp64 + P(2, 3) * _tmp56 + P(21, 3) * _tmp72 + - P(22, 3) * _tmp62 + P(3, 3) * _tmp71 + P(4, 3) * _tmp65 + P(5, 3) * _tmp41) + - _tmp72 * (P(0, 21) * _tmp51 - P(1, 21) * _tmp64 + P(2, 21) * _tmp56 + P(21, 21) * _tmp72 + - P(22, 21) * _tmp62 + P(3, 21) * _tmp71 + P(4, 21) * _tmp65 + P(5, 21) * _tmp41); + _tmp38 * (P(0, 21) * _tmp73 + P(1, 21) * _tmp72 + P(2, 21) * _tmp71 + P(21, 21) * _tmp38 + + P(22, 21) * _tmp74 + P(3, 21) * _tmp39 + P(4, 21) * _tmp45 + P(5, 21) * _tmp75) + + _tmp39 * (P(0, 3) * _tmp73 + P(1, 3) * _tmp72 + P(2, 3) * _tmp71 + P(21, 3) * _tmp38 + + P(22, 3) * _tmp74 + P(3, 3) * _tmp39 + P(4, 3) * _tmp45 + P(5, 3) * _tmp75) + + _tmp45 * (P(0, 4) * _tmp73 + P(1, 4) * _tmp72 + P(2, 4) * _tmp71 + P(21, 4) * _tmp38 + + P(22, 4) * _tmp74 + P(3, 4) * _tmp39 + P(4, 4) * _tmp45 + P(5, 4) * _tmp75) + + _tmp71 * (P(0, 2) * _tmp73 + P(1, 2) * _tmp72 + P(2, 2) * _tmp71 + P(21, 2) * _tmp38 + + P(22, 2) * _tmp74 + P(3, 2) * _tmp39 + P(4, 2) * _tmp45 + P(5, 2) * _tmp75) + + _tmp72 * (P(0, 1) * _tmp73 + P(1, 1) * _tmp72 + P(2, 1) * _tmp71 + P(21, 1) * _tmp38 + + P(22, 1) * _tmp74 + P(3, 1) * _tmp39 + P(4, 1) * _tmp45 + P(5, 1) * _tmp75) + + _tmp73 * (P(0, 0) * _tmp73 + P(1, 0) * _tmp72 + P(2, 0) * _tmp71 + P(21, 0) * _tmp38 + + P(22, 0) * _tmp74 + P(3, 0) * _tmp39 + P(4, 0) * _tmp45 + P(5, 0) * _tmp75) + + _tmp74 * (P(0, 22) * _tmp73 + P(1, 22) * _tmp72 + P(2, 22) * _tmp71 + P(21, 22) * _tmp38 + + P(22, 22) * _tmp74 + P(3, 22) * _tmp39 + P(4, 22) * _tmp45 + P(5, 22) * _tmp75) + + _tmp75 * (P(0, 5) * _tmp73 + P(1, 5) * _tmp72 + P(2, 5) * _tmp71 + P(21, 5) * _tmp38 + + P(22, 5) * _tmp74 + P(3, 5) * _tmp39 + P(4, 5) * _tmp45 + P(5, 5) * _tmp75); } if (Hy != nullptr) { @@ -155,14 +166,14 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix& state, _hy.setZero(); - _hy(0, 0) = _tmp51; - _hy(1, 0) = -_tmp64; - _hy(2, 0) = _tmp56; - _hy(3, 0) = _tmp71; - _hy(4, 0) = _tmp65; - _hy(5, 0) = _tmp41; - _hy(21, 0) = _tmp72; - _hy(22, 0) = _tmp62; + _hy(0, 0) = _tmp73; + _hy(1, 0) = _tmp72; + _hy(2, 0) = _tmp71; + _hy(3, 0) = _tmp39; + _hy(4, 0) = _tmp45; + _hy(5, 0) = _tmp75; + _hy(21, 0) = _tmp38; + _hy(22, 0) = _tmp74; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h index 11dfaacfcc..856096baf1 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h @@ -32,77 +32,88 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, const Scalar R, const Scalar epsilon, matrix::Matrix* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 196 + // Total ops: 275 // Input arrays - // Intermediate terms (33) - const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp2 = 1 - 2 * _tmp1; - const Scalar _tmp3 = + // Intermediate terms (42) + const Scalar _tmp0 = 2 * state(4, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = _tmp1 * state(6, 0); + const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2; + const Scalar _tmp4 = Scalar(1.0) / (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); - const Scalar _tmp4 = _tmp3 * (-2 * _tmp0 + _tmp2); - const Scalar _tmp5 = 2 * state(2, 0); - const Scalar _tmp6 = _tmp5 * state(0, 0); - const Scalar _tmp7 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp8 = _tmp5 * state(3, 0); + const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * _tmp4; + const Scalar _tmp6 = _tmp5 * state(2, 0); + const Scalar _tmp7 = 2 * state(3, 0) * state(6, 0); + const Scalar _tmp8 = _tmp5 * (_tmp0 * state(1, 0) + _tmp7); const Scalar _tmp9 = 2 * state(0, 0); - const Scalar _tmp10 = _tmp9 * state(1, 0); - const Scalar _tmp11 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp13 = -_tmp0; - const Scalar _tmp14 = _tmp12 + _tmp13; - const Scalar _tmp15 = _tmp3 * (state(4, 0) * (_tmp6 + _tmp7) + state(5, 0) * (-_tmp10 + _tmp8) + - state(6, 0) * (_tmp1 - _tmp11 + _tmp14)); - const Scalar _tmp16 = _tmp9 * state(3, 0); - const Scalar _tmp17 = -_tmp16; - const Scalar _tmp18 = _tmp5 * state(1, 0); - const Scalar _tmp19 = _tmp17 + _tmp18; - const Scalar _tmp20 = _tmp19 * _tmp3; - const Scalar _tmp21 = _tmp10 + _tmp8; - const Scalar _tmp22 = _tmp21 * _tmp3; - const Scalar _tmp23 = -_tmp7; - const Scalar _tmp24 = -_tmp12; - const Scalar _tmp25 = _tmp3 * (state(4, 0) * (_tmp1 + _tmp11 + _tmp13 + _tmp24) + - state(5, 0) * (_tmp17 - _tmp18) + state(6, 0) * (_tmp23 + _tmp6)); - const Scalar _tmp26 = _tmp3 * (-2 * _tmp11 + _tmp2); - const Scalar _tmp27 = -_tmp6; - const Scalar _tmp28 = -_tmp1 + _tmp11; - const Scalar _tmp29 = _tmp3 * (state(4, 0) * (_tmp23 + _tmp27) + state(5, 0) * (_tmp10 - _tmp8) + - state(6, 0) * (_tmp0 + _tmp24 + _tmp28)); - const Scalar _tmp30 = - _tmp3 * (_tmp19 * state(4, 0) + _tmp21 * state(6, 0) + state(5, 0) * (_tmp14 + _tmp28)); - const Scalar _tmp31 = _tmp3 * (_tmp16 + _tmp18); - const Scalar _tmp32 = _tmp3 * (_tmp27 + _tmp7); + const Scalar _tmp10 = state(3, 0) * state(5, 0); + const Scalar _tmp11 = 2 * state(2, 0); + const Scalar _tmp12 = _tmp11 * state(6, 0); + const Scalar _tmp13 = -4 * _tmp10 + _tmp12 - _tmp9 * state(4, 0); + const Scalar _tmp14 = _tmp5 * state(1, 0); + const Scalar _tmp15 = _tmp9 * state(6, 0); + const Scalar _tmp16 = _tmp0 * state(2, 0) + _tmp15 - 4 * state(1, 0) * state(5, 0); + const Scalar _tmp17 = _tmp5 * state(3, 0); + const Scalar _tmp18 = -_tmp13 * _tmp14 + _tmp16 * _tmp17 - _tmp3 * _tmp6 + _tmp8 * state(0, 0); + const Scalar _tmp19 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp20 = _tmp4 * (_tmp19 - 2 * std::pow(state(1, 0), Scalar(2))); + const Scalar _tmp21 = _tmp3 * _tmp5; + const Scalar _tmp22 = _tmp5 * state(0, 0); + const Scalar _tmp23 = + _tmp13 * _tmp6 + _tmp16 * _tmp22 - _tmp21 * state(1, 0) - _tmp8 * state(3, 0); + const Scalar _tmp24 = + _tmp13 * _tmp22 - _tmp16 * _tmp6 - _tmp21 * state(3, 0) + _tmp8 * state(1, 0); + const Scalar _tmp25 = _tmp9 * state(3, 0); + const Scalar _tmp26 = _tmp1 * state(2, 0); + const Scalar _tmp27 = _tmp4 * (-_tmp25 + _tmp26); + const Scalar _tmp28 = _tmp4 * (_tmp11 * state(3, 0) + _tmp9 * state(1, 0)); + const Scalar _tmp29 = _tmp4 * (_tmp19 - 2 * std::pow(state(2, 0), Scalar(2))); + const Scalar _tmp30 = 4 * state(4, 0); + const Scalar _tmp31 = _tmp2 - _tmp30 * state(3, 0) + _tmp9 * state(5, 0); + const Scalar _tmp32 = 2 * state(5, 0); + const Scalar _tmp33 = -_tmp15 - _tmp30 * state(2, 0) + _tmp32 * state(1, 0); + const Scalar _tmp34 = _tmp32 * state(2, 0) + _tmp7; + const Scalar _tmp35 = 2 * _tmp10 - _tmp12; + const Scalar _tmp36 = _tmp35 * _tmp5; + const Scalar _tmp37 = _tmp17 * _tmp33 - _tmp22 * _tmp34 - _tmp31 * _tmp6 + _tmp36 * state(1, 0); + const Scalar _tmp38 = -_tmp14 * _tmp33 - _tmp22 * _tmp31 + _tmp34 * _tmp6 + _tmp36 * state(3, 0); + const Scalar _tmp39 = _tmp14 * _tmp31 - _tmp17 * _tmp34 - _tmp22 * _tmp33 + _tmp35 * _tmp6; + const Scalar _tmp40 = _tmp4 * (_tmp25 + _tmp26); + const Scalar _tmp41 = _tmp4 * (_tmp1 * state(3, 0) - _tmp9 * state(2, 0)); // Output terms (2) if (innov_var != nullptr) { matrix::Matrix& _innov_var = (*innov_var); _innov_var(0, 0) = R + - _tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp25 + P(3, 0) * _tmp20 + - P(4, 0) * _tmp4 + P(5, 0) * _tmp22) + - _tmp20 * (P(0, 3) * _tmp15 + P(2, 3) * _tmp25 + P(3, 3) * _tmp20 + - P(4, 3) * _tmp4 + P(5, 3) * _tmp22) + - _tmp22 * (P(0, 5) * _tmp15 + P(2, 5) * _tmp25 + P(3, 5) * _tmp20 + - P(4, 5) * _tmp4 + P(5, 5) * _tmp22) + - _tmp25 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp25 + P(3, 2) * _tmp20 + - P(4, 2) * _tmp4 + P(5, 2) * _tmp22) + - _tmp4 * (P(0, 4) * _tmp15 + P(2, 4) * _tmp25 + P(3, 4) * _tmp20 + - P(4, 4) * _tmp4 + P(5, 4) * _tmp22); + _tmp18 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp18 + P(2, 1) * _tmp24 + + P(3, 1) * _tmp27 + P(4, 1) * _tmp20 + P(5, 1) * _tmp28) + + _tmp20 * (P(0, 4) * _tmp23 + P(1, 4) * _tmp18 + P(2, 4) * _tmp24 + + P(3, 4) * _tmp27 + P(4, 4) * _tmp20 + P(5, 4) * _tmp28) + + _tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp18 + P(2, 0) * _tmp24 + + P(3, 0) * _tmp27 + P(4, 0) * _tmp20 + P(5, 0) * _tmp28) + + _tmp24 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp18 + P(2, 2) * _tmp24 + + P(3, 2) * _tmp27 + P(4, 2) * _tmp20 + P(5, 2) * _tmp28) + + _tmp27 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp18 + P(2, 3) * _tmp24 + + P(3, 3) * _tmp27 + P(4, 3) * _tmp20 + P(5, 3) * _tmp28) + + _tmp28 * (P(0, 5) * _tmp23 + P(1, 5) * _tmp18 + P(2, 5) * _tmp24 + + P(3, 5) * _tmp27 + P(4, 5) * _tmp20 + P(5, 5) * _tmp28); _innov_var(1, 0) = R - - _tmp26 * (-P(1, 3) * _tmp29 - P(2, 3) * _tmp30 - P(3, 3) * _tmp26 - - P(4, 3) * _tmp31 - P(5, 3) * _tmp32) - - _tmp29 * (-P(1, 1) * _tmp29 - P(2, 1) * _tmp30 - P(3, 1) * _tmp26 - - P(4, 1) * _tmp31 - P(5, 1) * _tmp32) - - _tmp30 * (-P(1, 2) * _tmp29 - P(2, 2) * _tmp30 - P(3, 2) * _tmp26 - - P(4, 2) * _tmp31 - P(5, 2) * _tmp32) - - _tmp31 * (-P(1, 4) * _tmp29 - P(2, 4) * _tmp30 - P(3, 4) * _tmp26 - - P(4, 4) * _tmp31 - P(5, 4) * _tmp32) - - _tmp32 * (-P(1, 5) * _tmp29 - P(2, 5) * _tmp30 - P(3, 5) * _tmp26 - - P(4, 5) * _tmp31 - P(5, 5) * _tmp32); + _tmp29 * (P(0, 3) * _tmp37 + P(1, 3) * _tmp39 + P(2, 3) * _tmp38 - + P(3, 3) * _tmp29 - P(4, 3) * _tmp40 - P(5, 3) * _tmp41) + + _tmp37 * (P(0, 0) * _tmp37 + P(1, 0) * _tmp39 + P(2, 0) * _tmp38 - + P(3, 0) * _tmp29 - P(4, 0) * _tmp40 - P(5, 0) * _tmp41) + + _tmp38 * (P(0, 2) * _tmp37 + P(1, 2) * _tmp39 + P(2, 2) * _tmp38 - + P(3, 2) * _tmp29 - P(4, 2) * _tmp40 - P(5, 2) * _tmp41) + + _tmp39 * (P(0, 1) * _tmp37 + P(1, 1) * _tmp39 + P(2, 1) * _tmp38 - + P(3, 1) * _tmp29 - P(4, 1) * _tmp40 - P(5, 1) * _tmp41) - + _tmp40 * (P(0, 4) * _tmp37 + P(1, 4) * _tmp39 + P(2, 4) * _tmp38 - + P(3, 4) * _tmp29 - P(4, 4) * _tmp40 - P(5, 4) * _tmp41) - + _tmp41 * (P(0, 5) * _tmp37 + P(1, 5) * _tmp39 + P(2, 5) * _tmp38 - + P(3, 5) * _tmp29 - P(4, 5) * _tmp40 - P(5, 5) * _tmp41); } if (H != nullptr) { @@ -110,11 +121,12 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp15; - _h(2, 0) = _tmp25; - _h(3, 0) = _tmp20; - _h(4, 0) = _tmp4; - _h(5, 0) = _tmp22; + _h(0, 0) = _tmp23; + _h(1, 0) = _tmp18; + _h(2, 0) = _tmp24; + _h(3, 0) = _tmp27; + _h(4, 0) = _tmp20; + _h(5, 0) = _tmp28; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h index 3e00a2f604..2de18c1f18 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h @@ -32,51 +32,56 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 116 + // Total ops: 151 // Input arrays - // Intermediate terms (19) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp2 = + // Intermediate terms (21) + const Scalar _tmp0 = Scalar(1.0) / (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); - const Scalar _tmp3 = _tmp2 * (-2 * _tmp0 - 2 * _tmp1 + 1); - const Scalar _tmp4 = -2 * state(0, 0) * state(2, 0); - const Scalar _tmp5 = 2 * state(3, 0); - const Scalar _tmp6 = _tmp5 * state(1, 0); - const Scalar _tmp7 = _tmp5 * state(2, 0); + const Scalar _tmp1 = + _tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1); + const Scalar _tmp2 = 4 * state(4, 0); + const Scalar _tmp3 = 2 * state(0, 0); + const Scalar _tmp4 = 2 * state(6, 0); + const Scalar _tmp5 = -_tmp2 * state(3, 0) + _tmp3 * state(5, 0) + _tmp4 * state(1, 0); + const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp7 = _tmp5 * _tmp6; const Scalar _tmp8 = 2 * state(1, 0); - const Scalar _tmp9 = _tmp8 * state(0, 0); - const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp12 = -_tmp0 + _tmp1; - const Scalar _tmp13 = _tmp2 * (state(4, 0) * (_tmp4 - _tmp6) + state(5, 0) * (-_tmp7 + _tmp9) + - state(6, 0) * (-_tmp10 + _tmp11 + _tmp12)); - const Scalar _tmp14 = _tmp5 * state(0, 0); - const Scalar _tmp15 = _tmp8 * state(2, 0); - const Scalar _tmp16 = - _tmp2 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 - _tmp11 + _tmp12) + - state(6, 0) * (_tmp7 + _tmp9)); - const Scalar _tmp17 = _tmp2 * (_tmp14 + _tmp15); - const Scalar _tmp18 = _tmp2 * (_tmp4 + _tmp6); + const Scalar _tmp9 = -_tmp2 * state(2, 0) - _tmp4 * state(0, 0) + _tmp8 * state(5, 0); + const Scalar _tmp10 = _tmp6 * _tmp9; + const Scalar _tmp11 = 2 * state(5, 0); + const Scalar _tmp12 = _tmp6 * (_tmp11 * state(2, 0) + _tmp4 * state(3, 0)); + const Scalar _tmp13 = _tmp11 * state(3, 0) - _tmp4 * state(2, 0); + const Scalar _tmp14 = _tmp6 * state(1, 0); + const Scalar _tmp15 = + _tmp10 * state(3, 0) - _tmp12 * state(0, 0) + _tmp13 * _tmp14 - _tmp7 * state(2, 0); + const Scalar _tmp16 = _tmp13 * _tmp6; + const Scalar _tmp17 = + _tmp12 * state(2, 0) - _tmp14 * _tmp9 + _tmp16 * state(3, 0) - _tmp7 * state(0, 0); + const Scalar _tmp18 = + -_tmp10 * state(0, 0) - _tmp12 * state(3, 0) + _tmp14 * _tmp5 + _tmp16 * state(2, 0); + const Scalar _tmp19 = _tmp0 * (_tmp3 * state(3, 0) + _tmp8 * state(2, 0)); + const Scalar _tmp20 = _tmp0 * (-_tmp3 * state(2, 0) + _tmp8 * state(3, 0)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); _innov_var = R - - _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp16 - P(3, 1) * _tmp3 - - P(4, 1) * _tmp17 - P(5, 1) * _tmp18) - - _tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(3, 2) * _tmp3 - - P(4, 2) * _tmp17 - P(5, 2) * _tmp18) - - _tmp17 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(3, 4) * _tmp3 - - P(4, 4) * _tmp17 - P(5, 4) * _tmp18) - - _tmp18 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(3, 5) * _tmp3 - - P(4, 5) * _tmp17 - P(5, 5) * _tmp18) - - _tmp3 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(3, 3) * _tmp3 - - P(4, 3) * _tmp17 - P(5, 3) * _tmp18); + _tmp1 * (P(0, 3) * _tmp15 + P(1, 3) * _tmp18 + P(2, 3) * _tmp17 - P(3, 3) * _tmp1 - + P(4, 3) * _tmp19 - P(5, 3) * _tmp20) + + _tmp15 * (P(0, 0) * _tmp15 + P(1, 0) * _tmp18 + P(2, 0) * _tmp17 - + P(3, 0) * _tmp1 - P(4, 0) * _tmp19 - P(5, 0) * _tmp20) + + _tmp17 * (P(0, 2) * _tmp15 + P(1, 2) * _tmp18 + P(2, 2) * _tmp17 - + P(3, 2) * _tmp1 - P(4, 2) * _tmp19 - P(5, 2) * _tmp20) + + _tmp18 * (P(0, 1) * _tmp15 + P(1, 1) * _tmp18 + P(2, 1) * _tmp17 - + P(3, 1) * _tmp1 - P(4, 1) * _tmp19 - P(5, 1) * _tmp20) - + _tmp19 * (P(0, 4) * _tmp15 + P(1, 4) * _tmp18 + P(2, 4) * _tmp17 - + P(3, 4) * _tmp1 - P(4, 4) * _tmp19 - P(5, 4) * _tmp20) - + _tmp20 * (P(0, 5) * _tmp15 + P(1, 5) * _tmp18 + P(2, 5) * _tmp17 - + P(3, 5) * _tmp1 - P(4, 5) * _tmp19 - P(5, 5) * _tmp20); } if (H != nullptr) { @@ -84,11 +89,12 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(1, 0) = -_tmp13; - _h(2, 0) = -_tmp16; - _h(3, 0) = -_tmp3; - _h(4, 0) = -_tmp17; - _h(5, 0) = -_tmp18; + _h(0, 0) = _tmp15; + _h(1, 0) = _tmp18; + _h(2, 0) = _tmp17; + _h(3, 0) = -_tmp1; + _h(4, 0) = -_tmp19; + _h(5, 0) = -_tmp20; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h index d1be17dc35..d11b74d3e3 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h @@ -34,57 +34,62 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, const Scalar epsilon, Scalar* const meas_pred = nullptr, Scalar* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 95 + // Total ops: 114 // Input arrays - // Intermediate terms (28) - const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp2 = 1 - 2 * _tmp1; - const Scalar _tmp3 = std::sin(antenna_yaw_offset); - const Scalar _tmp4 = 2 * state(0, 0); - const Scalar _tmp5 = _tmp4 * state(3, 0); - const Scalar _tmp6 = 2 * state(2, 0); - const Scalar _tmp7 = _tmp6 * state(1, 0); - const Scalar _tmp8 = std::cos(antenna_yaw_offset); - const Scalar _tmp9 = _tmp3 * (-2 * _tmp0 + _tmp2) + _tmp8 * (_tmp5 + _tmp7); - const Scalar _tmp10 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp11 = -_tmp5; - const Scalar _tmp12 = _tmp11 + _tmp7; - const Scalar _tmp13 = _tmp12 * _tmp3 + _tmp8 * (-2 * _tmp10 + _tmp2); - const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5)); - const Scalar _tmp15 = _tmp6 * state(0, 0); - const Scalar _tmp16 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp17 = std::pow(_tmp14, Scalar(2)); - const Scalar _tmp18 = _tmp9 / _tmp17; - const Scalar _tmp19 = _tmp6 * state(3, 0); - const Scalar _tmp20 = _tmp4 * state(1, 0); - const Scalar _tmp21 = Scalar(1.0) / (_tmp14); - const Scalar _tmp22 = _tmp17 / (_tmp17 + std::pow(_tmp9, Scalar(2))); - const Scalar _tmp23 = - _tmp22 * (-_tmp18 * _tmp8 * (-_tmp15 - _tmp16) + _tmp21 * _tmp8 * (-_tmp19 + _tmp20)); - const Scalar _tmp24 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp25 = -_tmp0 + _tmp10; - const Scalar _tmp26 = - _tmp22 * (-_tmp18 * (_tmp12 * _tmp8 + _tmp3 * (_tmp1 - _tmp24 + _tmp25)) + - _tmp21 * (_tmp3 * (_tmp11 - _tmp7) + _tmp8 * (-_tmp1 + _tmp24 + _tmp25))); + // Intermediate terms (29) + const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = std::sin(antenna_yaw_offset); + const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0); + const Scalar _tmp3 = 2 * state(1, 0) * state(2, 0); + const Scalar _tmp4 = std::cos(antenna_yaw_offset); + const Scalar _tmp5 = + _tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2))) + _tmp4 * (_tmp2 + _tmp3); + const Scalar _tmp6 = + _tmp1 * (-_tmp2 + _tmp3) + _tmp4 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2))); + const Scalar _tmp7 = _tmp6 + epsilon * ((((_tmp6) > 0) - ((_tmp6) < 0)) + Scalar(0.5)); + const Scalar _tmp8 = 2 * _tmp1; + const Scalar _tmp9 = std::pow(_tmp7, Scalar(2)); + const Scalar _tmp10 = _tmp5 / _tmp9; + const Scalar _tmp11 = _tmp10 * _tmp8; + const Scalar _tmp12 = 4 * _tmp1; + const Scalar _tmp13 = 2 * _tmp4; + const Scalar _tmp14 = Scalar(1.0) / (_tmp7); + const Scalar _tmp15 = + -_tmp11 * state(2, 0) + _tmp14 * (-_tmp12 * state(1, 0) + _tmp13 * state(2, 0)); + const Scalar _tmp16 = (Scalar(1) / Scalar(2)) * _tmp9 / (std::pow(_tmp5, Scalar(2)) + _tmp9); + const Scalar _tmp17 = _tmp15 * _tmp16; + const Scalar _tmp18 = _tmp13 * _tmp14; + const Scalar _tmp19 = _tmp11 * state(3, 0) + _tmp18 * state(3, 0); + const Scalar _tmp20 = _tmp16 * _tmp19; + const Scalar _tmp21 = 4 * _tmp4; + const Scalar _tmp22 = -_tmp10 * (-_tmp21 * state(3, 0) - _tmp8 * state(0, 0)) + + _tmp14 * (-_tmp12 * state(3, 0) + _tmp13 * state(0, 0)); + const Scalar _tmp23 = _tmp16 * state(2, 0); + const Scalar _tmp24 = + _tmp16 * (-_tmp10 * (-_tmp21 * state(2, 0) + _tmp8 * state(1, 0)) + _tmp18 * state(1, 0)); + const Scalar _tmp25 = + _tmp17 * state(0, 0) - _tmp20 * state(1, 0) + _tmp22 * _tmp23 - _tmp24 * state(3, 0); + const Scalar _tmp26 = _tmp16 * _tmp22; const Scalar _tmp27 = - _tmp22 * (-_tmp18 * _tmp3 * (_tmp15 + _tmp16) + _tmp21 * _tmp3 * (_tmp19 - _tmp20)); + _tmp17 * state(3, 0) - _tmp19 * _tmp23 + _tmp24 * state(0, 0) - _tmp26 * state(1, 0); + const Scalar _tmp28 = + -_tmp15 * _tmp23 - _tmp20 * state(3, 0) + _tmp24 * state(1, 0) + _tmp26 * state(0, 0); // Output terms (3) if (meas_pred != nullptr) { Scalar& _meas_pred = (*meas_pred); - _meas_pred = std::atan2(_tmp9, _tmp14); + _meas_pred = std::atan2(_tmp5, _tmp7); } if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R + _tmp23 * (P(0, 1) * _tmp27 + P(1, 1) * _tmp23 + P(2, 1) * _tmp26) + - _tmp26 * (P(0, 2) * _tmp27 + P(1, 2) * _tmp23 + P(2, 2) * _tmp26) + - _tmp27 * (P(0, 0) * _tmp27 + P(1, 0) * _tmp23 + P(2, 0) * _tmp26); + _innov_var = R + _tmp25 * (P(0, 0) * _tmp25 + P(1, 0) * _tmp27 + P(2, 0) * _tmp28) + + _tmp27 * (P(0, 1) * _tmp25 + P(1, 1) * _tmp27 + P(2, 1) * _tmp28) + + _tmp28 * (P(0, 2) * _tmp25 + P(1, 2) * _tmp27 + P(2, 2) * _tmp28); } if (H != nullptr) { @@ -92,9 +97,9 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp27; - _h(1, 0) = _tmp23; - _h(2, 0) = _tmp26; + _h(0, 0) = _tmp25; + _h(1, 0) = _tmp27; + _h(2, 0) = _tmp28; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h index f3f074e82d..6d47ae4ab4 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h @@ -29,38 +29,35 @@ void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar R, matrix::Matrix* const innov_var = nullptr, matrix::Matrix* const Hx = nullptr) { - // Total ops: 51 + // Total ops: 53 // Input arrays - // Intermediate terms (16) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3; - const Scalar _tmp5 = 2 * state(2, 0); - const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(0, 0); - const Scalar _tmp9 = -_tmp6 - _tmp8; - const Scalar _tmp10 = -_tmp0 - _tmp1 + _tmp2 + _tmp3; - const Scalar _tmp11 = _tmp5 * state(0, 0); - const Scalar _tmp12 = _tmp7 * state(3, 0); - const Scalar _tmp13 = -_tmp11 + _tmp12; - const Scalar _tmp14 = _tmp6 + _tmp8; - const Scalar _tmp15 = _tmp11 - _tmp12; + // Intermediate terms (13) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = -_tmp0 * state(3, 0); + const Scalar _tmp2 = 2 * state(2, 0); + const Scalar _tmp3 = _tmp2 * state(1, 0); + const Scalar _tmp4 = _tmp1 - _tmp3; + const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp6 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp7 = std::pow(state(1, 0), Scalar(2)) - std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp8 = -_tmp5 + _tmp6 + _tmp7; + const Scalar _tmp9 = _tmp1 + _tmp3; + const Scalar _tmp10 = _tmp5 - _tmp6 + _tmp7; + const Scalar _tmp11 = _tmp0 * state(1, 0) - _tmp2 * state(3, 0); + const Scalar _tmp12 = _tmp2 * state(0, 0) + 2 * state(1, 0) * state(3, 0); // Output terms (2) if (innov_var != nullptr) { matrix::Matrix& _innov_var = (*innov_var); - _innov_var(0, 0) = R + _tmp4 * (P(1, 1) * _tmp4 + P(2, 1) * _tmp9) + - _tmp9 * (P(1, 2) * _tmp4 + P(2, 2) * _tmp9); - _innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(2, 0) * _tmp13) + - _tmp13 * (P(0, 2) * _tmp10 + P(2, 2) * _tmp13); - _innov_var(2, 0) = R + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp15) + - _tmp15 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp15); + _innov_var(0, 0) = R + _tmp4 * (P(0, 0) * _tmp4 + P(1, 0) * _tmp8) + + _tmp8 * (P(0, 1) * _tmp4 + P(1, 1) * _tmp8); + _innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(1, 0) * _tmp9) + + _tmp9 * (P(0, 1) * _tmp10 + P(1, 1) * _tmp9); + _innov_var(2, 0) = R + _tmp11 * (P(0, 0) * _tmp11 + P(1, 0) * _tmp12) + + _tmp12 * (P(0, 1) * _tmp11 + P(1, 1) * _tmp12); } if (Hx != nullptr) { @@ -68,8 +65,8 @@ void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix& state, _hx.setZero(); - _hx(1, 0) = _tmp4; - _hx(2, 0) = _tmp9; + _hx(0, 0) = _tmp4; + _hx(1, 0) = _tmp8; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h index 2c3a11b682..9aeb442d69 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h @@ -34,16 +34,16 @@ void ComputeGravityYInnovVarAndH(const matrix::Matrix& state, // Input arrays // Intermediate terms (2) - const Scalar _tmp0 = -std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) + - std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = -2 * state(0, 0) * state(2, 0) + 2 * state(1, 0) * state(3, 0); + const Scalar _tmp0 = -2 * state(0, 0) * state(3, 0) + 2 * state(1, 0) * state(2, 0); + const Scalar _tmp1 = -std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) - + std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R + _tmp0 * (P(0, 0) * _tmp0 + P(2, 0) * _tmp1) + - _tmp1 * (P(0, 2) * _tmp0 + P(2, 2) * _tmp1); + _innov_var = R + _tmp0 * (P(0, 1) * _tmp1 + P(1, 1) * _tmp0) + + _tmp1 * (P(0, 0) * _tmp1 + P(1, 0) * _tmp0); } if (Hy != nullptr) { @@ -51,8 +51,8 @@ void ComputeGravityYInnovVarAndH(const matrix::Matrix& state, _hy.setZero(); - _hy(0, 0) = _tmp0; - _hy(2, 0) = _tmp1; + _hy(0, 0) = _tmp1; + _hy(1, 0) = _tmp0; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h index 43d48e1283..e62e3e7212 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h @@ -36,8 +36,8 @@ void ComputeGravityZInnovVarAndH(const matrix::Matrix& state, // Intermediate terms (4) const Scalar _tmp0 = 2 * state(2, 0); const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); - const Scalar _tmp3 = _tmp0 * state(0, 0) - _tmp1 * state(3, 0); + const Scalar _tmp2 = -_tmp0 * state(3, 0) + _tmp1 * state(0, 0); + const Scalar _tmp3 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0); // Output terms (2) if (innov_var != nullptr) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h index dde9a8cd9e..bdad52b5a5 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h @@ -35,114 +35,153 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, matrix::Matrix* const innov = nullptr, matrix::Matrix* const innov_var = nullptr, matrix::Matrix* const Hx = nullptr) { - // Total ops: 314 + // Total ops: 461 // Unused inputs (void)epsilon; // Input arrays - // Intermediate terms (47) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = -2 * _tmp0; - const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp3 = -2 * _tmp2; - const Scalar _tmp4 = _tmp1 + _tmp3 + 1; - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(2, 0); - const Scalar _tmp9 = _tmp6 + _tmp8; - const Scalar _tmp10 = _tmp5 * state(2, 0); - const Scalar _tmp11 = -_tmp10; - const Scalar _tmp12 = _tmp7 * state(3, 0); - const Scalar _tmp13 = _tmp11 + _tmp12; - const Scalar _tmp14 = _tmp13 * state(18, 0) + _tmp9 * state(17, 0); - const Scalar _tmp15 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp16 = 1 - 2 * _tmp15; - const Scalar _tmp17 = _tmp1 + _tmp16; - const Scalar _tmp18 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp19 = _tmp7 * state(0, 0); - const Scalar _tmp20 = _tmp18 + _tmp19; - const Scalar _tmp21 = -_tmp6; - const Scalar _tmp22 = _tmp21 + _tmp8; - const Scalar _tmp23 = _tmp20 * state(18, 0) + _tmp22 * state(16, 0); - const Scalar _tmp24 = _tmp16 + _tmp3; - const Scalar _tmp25 = -_tmp19; - const Scalar _tmp26 = _tmp18 + _tmp25; - const Scalar _tmp27 = _tmp10 + _tmp12; - const Scalar _tmp28 = _tmp26 * state(17, 0) + _tmp27 * state(16, 0); - const Scalar _tmp29 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp30 = -_tmp29; - const Scalar _tmp31 = _tmp2 + _tmp30; - const Scalar _tmp32 = -_tmp0; - const Scalar _tmp33 = _tmp15 + _tmp32; - const Scalar _tmp34 = -_tmp18; - const Scalar _tmp35 = -_tmp12; - const Scalar _tmp36 = state(16, 0) * (_tmp11 + _tmp35) + state(17, 0) * (_tmp19 + _tmp34) + - state(18, 0) * (_tmp31 + _tmp33); - const Scalar _tmp37 = -_tmp15; - const Scalar _tmp38 = _tmp23 + state(17, 0) * (_tmp2 + _tmp29 + _tmp32 + _tmp37); - const Scalar _tmp39 = _tmp0 + _tmp37; - const Scalar _tmp40 = -_tmp8; - const Scalar _tmp41 = state(16, 0) * (_tmp31 + _tmp39) + state(17, 0) * (_tmp21 + _tmp40) + - state(18, 0) * (_tmp10 + _tmp35); - const Scalar _tmp42 = -_tmp2; - const Scalar _tmp43 = _tmp29 + _tmp42; - const Scalar _tmp44 = _tmp28 + state(18, 0) * (_tmp39 + _tmp43); - const Scalar _tmp45 = _tmp14 + state(16, 0) * (_tmp33 + _tmp43); - const Scalar _tmp46 = state(16, 0) * (_tmp40 + _tmp6) + - state(17, 0) * (_tmp0 + _tmp15 + _tmp30 + _tmp42) + - state(18, 0) * (_tmp25 + _tmp34); + // Intermediate terms (68) + 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(3, 0); + const Scalar _tmp4 = _tmp3 * state(0, 0); + const Scalar _tmp5 = 2 * state(2, 0); + const Scalar _tmp6 = _tmp5 * state(1, 0); + const Scalar _tmp7 = _tmp4 + _tmp6; + const Scalar _tmp8 = _tmp5 * state(0, 0); + const Scalar _tmp9 = 2 * state(1, 0); + const Scalar _tmp10 = _tmp9 * state(3, 0); + const Scalar _tmp11 = _tmp10 - _tmp8; + const Scalar _tmp12 = -2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp13 = _tmp0 + _tmp12 + 1; + const Scalar _tmp14 = _tmp5 * state(3, 0); + const Scalar _tmp15 = _tmp9 * state(0, 0); + const Scalar _tmp16 = _tmp14 + _tmp15; + const Scalar _tmp17 = -_tmp4 + _tmp6; + const Scalar _tmp18 = _tmp1 + _tmp12; + const Scalar _tmp19 = _tmp14 - _tmp15; + const Scalar _tmp20 = _tmp10 + _tmp8; + const Scalar _tmp21 = _tmp3 * state(18, 0); + const Scalar _tmp22 = _tmp5 * state(17, 0); + const Scalar _tmp23 = _tmp21 + _tmp22; + const Scalar _tmp24 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp25 = 2 * state(17, 0); + const Scalar _tmp26 = _tmp25 * state(3, 0); + const Scalar _tmp27 = _tmp5 * state(18, 0); + const Scalar _tmp28 = _tmp26 - _tmp27; + const Scalar _tmp29 = (Scalar(1) / Scalar(2)) * _tmp28; + const Scalar _tmp30 = 4 * state(2, 0); + const Scalar _tmp31 = _tmp9 * state(17, 0); + const Scalar _tmp32 = 2 * state(0, 0); + const Scalar _tmp33 = _tmp32 * state(18, 0); + const Scalar _tmp34 = -_tmp30 * state(16, 0) + _tmp31 - _tmp33; + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp36 = _tmp9 * state(18, 0); + const Scalar _tmp37 = _tmp25 * state(0, 0); + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp36 + (Scalar(1) / Scalar(2)) * _tmp37 - + 2 * state(16, 0) * state(3, 0); + const Scalar _tmp39 = + -_tmp23 * _tmp24 - _tmp29 * state(3, 0) + _tmp34 * _tmp35 + _tmp38 * state(0, 0); + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * state(0, 0); + const Scalar _tmp42 = + -_tmp24 * _tmp28 + _tmp34 * _tmp41 - _tmp38 * state(1, 0) + _tmp40 * state(3, 0); + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp44 = + -_tmp29 * state(1, 0) - _tmp34 * _tmp43 + _tmp38 * state(2, 0) + _tmp40 * state(0, 0); + const Scalar _tmp45 = _tmp3 * state(16, 0); + const Scalar _tmp46 = _tmp36 - _tmp45; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp46; + const Scalar _tmp48 = _tmp9 * state(16, 0); + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp21 + (Scalar(1) / Scalar(2)) * _tmp48; + const Scalar _tmp50 = _tmp5 * state(16, 0); + const Scalar _tmp51 = 4 * state(17, 0); + const Scalar _tmp52 = _tmp33 + _tmp50 - _tmp51 * state(1, 0); + const Scalar _tmp53 = _tmp32 * state(16, 0); + const Scalar _tmp54 = _tmp27 - _tmp51 * state(3, 0) - _tmp53; + const Scalar _tmp55 = + _tmp24 * _tmp54 + _tmp41 * _tmp52 - _tmp47 * state(1, 0) - _tmp49 * state(3, 0); + const Scalar _tmp56 = + -_tmp24 * _tmp52 + _tmp41 * _tmp54 - _tmp47 * state(3, 0) + _tmp49 * state(1, 0); + const Scalar _tmp57 = -_tmp24 * _tmp46 - _tmp35 * _tmp54 + _tmp43 * _tmp52 + _tmp49 * state(0, 0); + const Scalar _tmp58 = -_tmp31 + _tmp50; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp58; + const Scalar _tmp60 = _tmp22 + _tmp48; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp60; + const Scalar _tmp62 = _tmp26 - _tmp30 * state(18, 0) + _tmp53; + const Scalar _tmp63 = -Scalar(1) / Scalar(2) * _tmp37 + (Scalar(1) / Scalar(2)) * _tmp45 - + 2 * state(1, 0) * state(18, 0); + const Scalar _tmp64 = + _tmp35 * _tmp62 - _tmp59 * state(3, 0) + _tmp61 * state(0, 0) - _tmp63 * state(2, 0); + const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp62; + const Scalar _tmp66 = + _tmp24 * _tmp60 - _tmp59 * state(1, 0) + _tmp63 * state(0, 0) - _tmp65 * state(3, 0); + const Scalar _tmp67 = + -_tmp24 * _tmp58 - _tmp61 * state(1, 0) + _tmp63 * state(3, 0) + _tmp65 * state(0, 0); // Output terms (3) if (innov != nullptr) { matrix::Matrix& _innov = (*innov); - _innov(0, 0) = _tmp14 + _tmp4 * state(16, 0) - meas(0, 0) + state(19, 0); - _innov(1, 0) = _tmp17 * state(17, 0) + _tmp23 - meas(1, 0) + state(20, 0); - _innov(2, 0) = _tmp24 * state(18, 0) + _tmp28 - meas(2, 0) + state(21, 0); + _innov(0, 0) = _tmp11 * state(18, 0) + _tmp2 * state(16, 0) + _tmp7 * state(17, 0) - + meas(0, 0) + state(19, 0); + _innov(1, 0) = _tmp13 * state(17, 0) + _tmp16 * state(18, 0) + _tmp17 * state(16, 0) - + meas(1, 0) + state(20, 0); + _innov(2, 0) = _tmp18 * state(18, 0) + _tmp19 * state(17, 0) + _tmp20 * state(16, 0) - + meas(2, 0) + state(21, 0); } if (innov_var != nullptr) { matrix::Matrix& _innov_var = (*innov_var); - _innov_var(0, 0) = P(1, 18) * _tmp36 + P(15, 18) * _tmp4 + P(16, 18) * _tmp9 + - P(17, 18) * _tmp13 + P(18, 18) + P(2, 18) * _tmp38 + R + - _tmp13 * (P(1, 17) * _tmp36 + P(15, 17) * _tmp4 + P(16, 17) * _tmp9 + - P(17, 17) * _tmp13 + P(18, 17) + P(2, 17) * _tmp38) + - _tmp36 * (P(1, 1) * _tmp36 + P(15, 1) * _tmp4 + P(16, 1) * _tmp9 + - P(17, 1) * _tmp13 + P(18, 1) + P(2, 1) * _tmp38) + - _tmp38 * (P(1, 2) * _tmp36 + P(15, 2) * _tmp4 + P(16, 2) * _tmp9 + - P(17, 2) * _tmp13 + P(18, 2) + P(2, 2) * _tmp38) + - _tmp4 * (P(1, 15) * _tmp36 + P(15, 15) * _tmp4 + P(16, 15) * _tmp9 + - P(17, 15) * _tmp13 + P(18, 15) + P(2, 15) * _tmp38) + - _tmp9 * (P(1, 16) * _tmp36 + P(15, 16) * _tmp4 + P(16, 16) * _tmp9 + - P(17, 16) * _tmp13 + P(18, 16) + P(2, 16) * _tmp38); - _innov_var(1, 0) = P(0, 19) * _tmp44 + P(15, 19) * _tmp22 + P(16, 19) * _tmp17 + - P(17, 19) * _tmp20 + P(19, 19) + P(2, 19) * _tmp41 + R + - _tmp17 * (P(0, 16) * _tmp44 + P(15, 16) * _tmp22 + P(16, 16) * _tmp17 + - P(17, 16) * _tmp20 + P(19, 16) + P(2, 16) * _tmp41) + - _tmp20 * (P(0, 17) * _tmp44 + P(15, 17) * _tmp22 + P(16, 17) * _tmp17 + - P(17, 17) * _tmp20 + P(19, 17) + P(2, 17) * _tmp41) + - _tmp22 * (P(0, 15) * _tmp44 + P(15, 15) * _tmp22 + P(16, 15) * _tmp17 + - P(17, 15) * _tmp20 + P(19, 15) + P(2, 15) * _tmp41) + - _tmp41 * (P(0, 2) * _tmp44 + P(15, 2) * _tmp22 + P(16, 2) * _tmp17 + - P(17, 2) * _tmp20 + P(19, 2) + P(2, 2) * _tmp41) + - _tmp44 * (P(0, 0) * _tmp44 + P(15, 0) * _tmp22 + P(16, 0) * _tmp17 + - P(17, 0) * _tmp20 + P(19, 0) + P(2, 0) * _tmp41); - _innov_var(2, 0) = P(0, 20) * _tmp46 + P(1, 20) * _tmp45 + P(15, 20) * _tmp27 + - P(16, 20) * _tmp26 + P(17, 20) * _tmp24 + P(20, 20) + R + - _tmp24 * (P(0, 17) * _tmp46 + P(1, 17) * _tmp45 + P(15, 17) * _tmp27 + - P(16, 17) * _tmp26 + P(17, 17) * _tmp24 + P(20, 17)) + - _tmp26 * (P(0, 16) * _tmp46 + P(1, 16) * _tmp45 + P(15, 16) * _tmp27 + - P(16, 16) * _tmp26 + P(17, 16) * _tmp24 + P(20, 16)) + - _tmp27 * (P(0, 15) * _tmp46 + P(1, 15) * _tmp45 + P(15, 15) * _tmp27 + - P(16, 15) * _tmp26 + P(17, 15) * _tmp24 + P(20, 15)) + - _tmp45 * (P(0, 1) * _tmp46 + P(1, 1) * _tmp45 + P(15, 1) * _tmp27 + - P(16, 1) * _tmp26 + P(17, 1) * _tmp24 + P(20, 1)) + - _tmp46 * (P(0, 0) * _tmp46 + P(1, 0) * _tmp45 + P(15, 0) * _tmp27 + - P(16, 0) * _tmp26 + P(17, 0) * _tmp24 + P(20, 0)); + _innov_var(0, 0) = + P(0, 18) * _tmp44 + P(1, 18) * _tmp42 + P(15, 18) * _tmp2 + P(16, 18) * _tmp7 + + P(17, 18) * _tmp11 + P(18, 18) + P(2, 18) * _tmp39 + R + + _tmp11 * (P(0, 17) * _tmp44 + P(1, 17) * _tmp42 + P(15, 17) * _tmp2 + P(16, 17) * _tmp7 + + P(17, 17) * _tmp11 + P(18, 17) + P(2, 17) * _tmp39) + + _tmp2 * (P(0, 15) * _tmp44 + P(1, 15) * _tmp42 + P(15, 15) * _tmp2 + P(16, 15) * _tmp7 + + P(17, 15) * _tmp11 + P(18, 15) + P(2, 15) * _tmp39) + + _tmp39 * (P(0, 2) * _tmp44 + P(1, 2) * _tmp42 + P(15, 2) * _tmp2 + P(16, 2) * _tmp7 + + P(17, 2) * _tmp11 + P(18, 2) + P(2, 2) * _tmp39) + + _tmp42 * (P(0, 1) * _tmp44 + P(1, 1) * _tmp42 + P(15, 1) * _tmp2 + P(16, 1) * _tmp7 + + P(17, 1) * _tmp11 + P(18, 1) + P(2, 1) * _tmp39) + + _tmp44 * (P(0, 0) * _tmp44 + P(1, 0) * _tmp42 + P(15, 0) * _tmp2 + P(16, 0) * _tmp7 + + P(17, 0) * _tmp11 + P(18, 0) + P(2, 0) * _tmp39) + + _tmp7 * (P(0, 16) * _tmp44 + P(1, 16) * _tmp42 + P(15, 16) * _tmp2 + P(16, 16) * _tmp7 + + P(17, 16) * _tmp11 + P(18, 16) + P(2, 16) * _tmp39); + _innov_var(1, 0) = + P(0, 19) * _tmp55 + P(1, 19) * _tmp57 + P(15, 19) * _tmp17 + P(16, 19) * _tmp13 + + P(17, 19) * _tmp16 + P(19, 19) + P(2, 19) * _tmp56 + R + + _tmp13 * (P(0, 16) * _tmp55 + P(1, 16) * _tmp57 + P(15, 16) * _tmp17 + P(16, 16) * _tmp13 + + P(17, 16) * _tmp16 + P(19, 16) + P(2, 16) * _tmp56) + + _tmp16 * (P(0, 17) * _tmp55 + P(1, 17) * _tmp57 + P(15, 17) * _tmp17 + P(16, 17) * _tmp13 + + P(17, 17) * _tmp16 + P(19, 17) + P(2, 17) * _tmp56) + + _tmp17 * (P(0, 15) * _tmp55 + P(1, 15) * _tmp57 + P(15, 15) * _tmp17 + P(16, 15) * _tmp13 + + P(17, 15) * _tmp16 + P(19, 15) + P(2, 15) * _tmp56) + + _tmp55 * (P(0, 0) * _tmp55 + P(1, 0) * _tmp57 + P(15, 0) * _tmp17 + P(16, 0) * _tmp13 + + P(17, 0) * _tmp16 + P(19, 0) + P(2, 0) * _tmp56) + + _tmp56 * (P(0, 2) * _tmp55 + P(1, 2) * _tmp57 + P(15, 2) * _tmp17 + P(16, 2) * _tmp13 + + P(17, 2) * _tmp16 + P(19, 2) + P(2, 2) * _tmp56) + + _tmp57 * (P(0, 1) * _tmp55 + P(1, 1) * _tmp57 + P(15, 1) * _tmp17 + P(16, 1) * _tmp13 + + P(17, 1) * _tmp16 + P(19, 1) + P(2, 1) * _tmp56); + _innov_var(2, 0) = + P(0, 20) * _tmp66 + P(1, 20) * _tmp67 + P(15, 20) * _tmp20 + P(16, 20) * _tmp19 + + P(17, 20) * _tmp18 + P(2, 20) * _tmp64 + P(20, 20) + R + + _tmp18 * (P(0, 17) * _tmp66 + P(1, 17) * _tmp67 + P(15, 17) * _tmp20 + P(16, 17) * _tmp19 + + P(17, 17) * _tmp18 + P(2, 17) * _tmp64 + P(20, 17)) + + _tmp19 * (P(0, 16) * _tmp66 + P(1, 16) * _tmp67 + P(15, 16) * _tmp20 + P(16, 16) * _tmp19 + + P(17, 16) * _tmp18 + P(2, 16) * _tmp64 + P(20, 16)) + + _tmp20 * (P(0, 15) * _tmp66 + P(1, 15) * _tmp67 + P(15, 15) * _tmp20 + P(16, 15) * _tmp19 + + P(17, 15) * _tmp18 + P(2, 15) * _tmp64 + P(20, 15)) + + _tmp64 * (P(0, 2) * _tmp66 + P(1, 2) * _tmp67 + P(15, 2) * _tmp20 + P(16, 2) * _tmp19 + + P(17, 2) * _tmp18 + P(2, 2) * _tmp64 + P(20, 2)) + + _tmp66 * (P(0, 0) * _tmp66 + P(1, 0) * _tmp67 + P(15, 0) * _tmp20 + P(16, 0) * _tmp19 + + P(17, 0) * _tmp18 + P(2, 0) * _tmp64 + P(20, 0)) + + _tmp67 * (P(0, 1) * _tmp66 + P(1, 1) * _tmp67 + P(15, 1) * _tmp20 + P(16, 1) * _tmp19 + + P(17, 1) * _tmp18 + P(2, 1) * _tmp64 + P(20, 1)); } if (Hx != nullptr) { @@ -150,11 +189,12 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, _hx.setZero(); - _hx(1, 0) = _tmp36; - _hx(2, 0) = _tmp38; - _hx(15, 0) = _tmp4; - _hx(16, 0) = _tmp9; - _hx(17, 0) = _tmp13; + _hx(0, 0) = _tmp44; + _hx(1, 0) = _tmp42; + _hx(2, 0) = _tmp39; + _hx(15, 0) = _tmp2; + _hx(16, 0) = _tmp7; + _hx(17, 0) = _tmp11; _hx(18, 0) = 1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h index 9f22c4f1fa..a490ca61ea 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h @@ -30,7 +30,7 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 110 + // Total ops: 159 // Unused inputs (void)epsilon; @@ -38,43 +38,50 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix& state, // Input arrays // Intermediate terms (18) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1; - const Scalar _tmp3 = 2 * state(2, 0); - const Scalar _tmp4 = _tmp3 * state(3, 0); - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = _tmp5 * state(1, 0); - const Scalar _tmp7 = _tmp4 + _tmp6; - const Scalar _tmp8 = -_tmp5 * state(3, 0); - const Scalar _tmp9 = _tmp3 * state(1, 0); - const Scalar _tmp10 = _tmp8 + _tmp9; - const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp13 = _tmp0 - _tmp1; - const Scalar _tmp14 = _tmp3 * state(0, 0); - const Scalar _tmp15 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp16 = state(16, 0) * (-_tmp11 + _tmp12 + _tmp13) + - state(17, 0) * (_tmp8 - _tmp9) + state(18, 0) * (_tmp14 - _tmp15); - const Scalar _tmp17 = state(16, 0) * (_tmp14 + _tmp15) + state(17, 0) * (_tmp4 - _tmp6) + - state(18, 0) * (_tmp11 - _tmp12 + _tmp13); + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); + const Scalar _tmp3 = 2 * state(3, 0); + const Scalar _tmp4 = _tmp0 * state(1, 0) - _tmp3 * state(0, 0); + const Scalar _tmp5 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + const Scalar _tmp6 = _tmp1 * state(18, 0) - _tmp3 * state(16, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp1 * state(16, 0) + + (Scalar(1) / Scalar(2)) * _tmp3 * state(18, 0); + const Scalar _tmp9 = 4 * state(17, 0); + const Scalar _tmp10 = 2 * state(0, 0); + const Scalar _tmp11 = _tmp0 * state(16, 0) + _tmp10 * state(18, 0) - _tmp9 * state(1, 0); + const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp13 = (Scalar(1) / Scalar(2)) * _tmp0 * state(18, 0) - + Scalar(1) / Scalar(2) * _tmp10 * state(16, 0) - + Scalar(1) / Scalar(2) * _tmp9 * state(3, 0); + const Scalar _tmp14 = + _tmp12 * state(0, 0) + _tmp13 * state(2, 0) - _tmp7 * state(1, 0) - _tmp8 * state(3, 0); + const Scalar _tmp15 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp16 = + -_tmp11 * _tmp15 + _tmp13 * state(0, 0) - _tmp7 * state(3, 0) + _tmp8 * state(1, 0); + const Scalar _tmp17 = + _tmp12 * state(3, 0) - _tmp13 * state(1, 0) - _tmp15 * _tmp6 + _tmp8 * state(0, 0); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = P(0, 19) * _tmp17 + P(15, 19) * _tmp10 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 + - P(19, 19) + P(2, 19) * _tmp16 + R + - _tmp10 * (P(0, 15) * _tmp17 + P(15, 15) * _tmp10 + P(16, 15) * _tmp2 + - P(17, 15) * _tmp7 + P(19, 15) + P(2, 15) * _tmp16) + - _tmp16 * (P(0, 2) * _tmp17 + P(15, 2) * _tmp10 + P(16, 2) * _tmp2 + - P(17, 2) * _tmp7 + P(19, 2) + P(2, 2) * _tmp16) + - _tmp17 * (P(0, 0) * _tmp17 + P(15, 0) * _tmp10 + P(16, 0) * _tmp2 + - P(17, 0) * _tmp7 + P(19, 0) + P(2, 0) * _tmp16) + - _tmp2 * (P(0, 16) * _tmp17 + P(15, 16) * _tmp10 + P(16, 16) * _tmp2 + - P(17, 16) * _tmp7 + P(19, 16) + P(2, 16) * _tmp16) + - _tmp7 * (P(0, 17) * _tmp17 + P(15, 17) * _tmp10 + P(16, 17) * _tmp2 + - P(17, 17) * _tmp7 + P(19, 17) + P(2, 17) * _tmp16); + _innov_var = P(0, 19) * _tmp14 + P(1, 19) * _tmp17 + P(15, 19) * _tmp4 + P(16, 19) * _tmp5 + + P(17, 19) * _tmp2 + P(19, 19) + P(2, 19) * _tmp16 + R + + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp17 + P(15, 0) * _tmp4 + + P(16, 0) * _tmp5 + P(17, 0) * _tmp2 + P(19, 0) + P(2, 0) * _tmp16) + + _tmp16 * (P(0, 2) * _tmp14 + P(1, 2) * _tmp17 + P(15, 2) * _tmp4 + + P(16, 2) * _tmp5 + P(17, 2) * _tmp2 + P(19, 2) + P(2, 2) * _tmp16) + + _tmp17 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp17 + P(15, 1) * _tmp4 + + P(16, 1) * _tmp5 + P(17, 1) * _tmp2 + P(19, 1) + P(2, 1) * _tmp16) + + _tmp2 * (P(0, 17) * _tmp14 + P(1, 17) * _tmp17 + P(15, 17) * _tmp4 + + P(16, 17) * _tmp5 + P(17, 17) * _tmp2 + P(19, 17) + P(2, 17) * _tmp16) + + _tmp4 * (P(0, 15) * _tmp14 + P(1, 15) * _tmp17 + P(15, 15) * _tmp4 + + P(16, 15) * _tmp5 + P(17, 15) * _tmp2 + P(19, 15) + P(2, 15) * _tmp16) + + _tmp5 * (P(0, 16) * _tmp14 + P(1, 16) * _tmp17 + P(15, 16) * _tmp4 + + P(16, 16) * _tmp5 + P(17, 16) * _tmp2 + P(19, 16) + P(2, 16) * _tmp16); } if (H != nullptr) { @@ -82,11 +89,12 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp17; + _h(0, 0) = _tmp14; + _h(1, 0) = _tmp17; _h(2, 0) = _tmp16; - _h(15, 0) = _tmp10; - _h(16, 0) = _tmp2; - _h(17, 0) = _tmp7; + _h(15, 0) = _tmp4; + _h(16, 0) = _tmp5; + _h(17, 0) = _tmp2; _h(19, 0) = 1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h index 0563f1df9e..0abdc1d9ca 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h @@ -30,51 +30,58 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 110 + // Total ops: 161 // Unused inputs (void)epsilon; // Input arrays - // Intermediate terms (18) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1; - const Scalar _tmp3 = 2 * state(2, 0); - const Scalar _tmp4 = _tmp3 * state(3, 0); - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = -_tmp5 * state(1, 0); - const Scalar _tmp7 = _tmp4 + _tmp6; - const Scalar _tmp8 = _tmp3 * state(0, 0); - const Scalar _tmp9 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp10 = _tmp8 + _tmp9; - const Scalar _tmp11 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp13 = -_tmp0 + _tmp1; - const Scalar _tmp14 = _tmp5 * state(3, 0); - const Scalar _tmp15 = _tmp3 * state(1, 0); - const Scalar _tmp16 = state(16, 0) * (-_tmp11 + _tmp12 + _tmp13) + - state(17, 0) * (_tmp14 + _tmp15) + state(18, 0) * (-_tmp8 + _tmp9); - const Scalar _tmp17 = state(16, 0) * (_tmp14 - _tmp15) + - state(17, 0) * (_tmp11 - _tmp12 + _tmp13) + state(18, 0) * (-_tmp4 + _tmp6); + // Intermediate terms (15) + const Scalar _tmp0 = 2 * state(16, 0); + const Scalar _tmp1 = 2 * state(17, 0); + const Scalar _tmp2 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - Scalar(1) / Scalar(2) * _tmp1 * state(1, 0); + const Scalar _tmp3 = 2 * state(1, 0); + const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0) + + (Scalar(1) / Scalar(2)) * _tmp3 * state(16, 0); + const Scalar _tmp5 = 4 * state(18, 0); + const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0 * state(0, 0) + + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(2, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp0 * state(3, 0) - + Scalar(1) / Scalar(2) * _tmp1 * state(0, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(1, 0); + const Scalar _tmp8 = + -_tmp2 * state(3, 0) + _tmp4 * state(0, 0) + _tmp6 * state(1, 0) - _tmp7 * state(2, 0); + const Scalar _tmp9 = + -_tmp2 * state(1, 0) + _tmp4 * state(2, 0) - _tmp6 * state(3, 0) + _tmp7 * state(0, 0); + const Scalar _tmp10 = + -_tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp6 * state(0, 0) + _tmp7 * state(3, 0); + const Scalar _tmp11 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; + const Scalar _tmp12 = 2 * state(2, 0); + const Scalar _tmp13 = _tmp12 * state(3, 0) - _tmp3 * state(0, 0); + const Scalar _tmp14 = _tmp12 * state(0, 0) + _tmp3 * state(3, 0); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = P(0, 20) * _tmp17 + P(1, 20) * _tmp16 + P(15, 20) * _tmp10 + P(16, 20) * _tmp7 + - P(17, 20) * _tmp2 + P(20, 20) + R + - _tmp10 * (P(0, 15) * _tmp17 + P(1, 15) * _tmp16 + P(15, 15) * _tmp10 + - P(16, 15) * _tmp7 + P(17, 15) * _tmp2 + P(20, 15)) + - _tmp16 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp16 + P(15, 1) * _tmp10 + - P(16, 1) * _tmp7 + P(17, 1) * _tmp2 + P(20, 1)) + - _tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp16 + P(15, 0) * _tmp10 + - P(16, 0) * _tmp7 + P(17, 0) * _tmp2 + P(20, 0)) + - _tmp2 * (P(0, 17) * _tmp17 + P(1, 17) * _tmp16 + P(15, 17) * _tmp10 + - P(16, 17) * _tmp7 + P(17, 17) * _tmp2 + P(20, 17)) + - _tmp7 * (P(0, 16) * _tmp17 + P(1, 16) * _tmp16 + P(15, 16) * _tmp10 + - P(16, 16) * _tmp7 + P(17, 16) * _tmp2 + P(20, 16)); + _innov_var = P(0, 20) * _tmp9 + P(1, 20) * _tmp10 + P(15, 20) * _tmp14 + P(16, 20) * _tmp13 + + P(17, 20) * _tmp11 + P(2, 20) * _tmp8 + P(20, 20) + R + + _tmp10 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp10 + P(15, 1) * _tmp14 + + P(16, 1) * _tmp13 + P(17, 1) * _tmp11 + P(2, 1) * _tmp8 + P(20, 1)) + + _tmp11 * (P(0, 17) * _tmp9 + P(1, 17) * _tmp10 + P(15, 17) * _tmp14 + + P(16, 17) * _tmp13 + P(17, 17) * _tmp11 + P(2, 17) * _tmp8 + P(20, 17)) + + _tmp13 * (P(0, 16) * _tmp9 + P(1, 16) * _tmp10 + P(15, 16) * _tmp14 + + P(16, 16) * _tmp13 + P(17, 16) * _tmp11 + P(2, 16) * _tmp8 + P(20, 16)) + + _tmp14 * (P(0, 15) * _tmp9 + P(1, 15) * _tmp10 + P(15, 15) * _tmp14 + + P(16, 15) * _tmp13 + P(17, 15) * _tmp11 + P(2, 15) * _tmp8 + P(20, 15)) + + _tmp8 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp10 + P(15, 2) * _tmp14 + + P(16, 2) * _tmp13 + P(17, 2) * _tmp11 + P(2, 2) * _tmp8 + P(20, 2)) + + _tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp10 + P(15, 0) * _tmp14 + + P(16, 0) * _tmp13 + P(17, 0) * _tmp11 + P(2, 0) * _tmp8 + P(20, 0)); } if (H != nullptr) { @@ -82,11 +89,12 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp17; - _h(1, 0) = _tmp16; - _h(15, 0) = _tmp10; - _h(16, 0) = _tmp7; - _h(17, 0) = _tmp2; + _h(0, 0) = _tmp9; + _h(1, 0) = _tmp10; + _h(2, 0) = _tmp8; + _h(15, 0) = _tmp14; + _h(16, 0) = _tmp13; + _h(17, 0) = _tmp11; _h(20, 0) = 1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h index 30acca8d22..1cd242c727 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h @@ -30,63 +30,66 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar innov_var, const Scalar epsilon, matrix::Matrix* const H = nullptr, matrix::Matrix* const K = nullptr) { - // Total ops: 469 + // Total ops: 497 // Input arrays - // Intermediate terms (46) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp2 = 1 - 2 * _tmp1; - const Scalar _tmp3 = -2 * _tmp0 + _tmp2; - const Scalar _tmp4 = -state(22, 0) + state(4, 0); - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(2, 0); - const Scalar _tmp8 = _tmp7 * state(1, 0); - const Scalar _tmp9 = _tmp6 + _tmp8; - const Scalar _tmp10 = -state(23, 0) + state(5, 0); - const Scalar _tmp11 = _tmp7 * state(0, 0); - const Scalar _tmp12 = -_tmp11; - const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp14 = _tmp12 + _tmp13; - const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4; - const Scalar _tmp16 = - _tmp15 + epsilon * (2 * math::min(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1); - const Scalar _tmp17 = Scalar(1.0) / (_tmp16); - const Scalar _tmp18 = _tmp7 * state(3, 0); - const Scalar _tmp19 = _tmp5 * state(1, 0); - const Scalar _tmp20 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp22 = -_tmp21; - const Scalar _tmp23 = _tmp20 + _tmp22; - const Scalar _tmp24 = _tmp17 * (_tmp10 * (_tmp18 - _tmp19) + _tmp4 * (_tmp11 + _tmp13) + - state(6, 0) * (-_tmp0 + _tmp1 + _tmp23)); - const Scalar _tmp25 = -_tmp13; - const Scalar _tmp26 = -_tmp20; - const Scalar _tmp27 = _tmp0 - _tmp1; - const Scalar _tmp28 = _tmp2 - 2 * _tmp21; - const Scalar _tmp29 = -_tmp6; - const Scalar _tmp30 = _tmp29 + _tmp8; - const Scalar _tmp31 = _tmp18 + _tmp19; - const Scalar _tmp32 = _tmp30 * _tmp4 + _tmp31 * state(6, 0); - const Scalar _tmp33 = (_tmp10 * _tmp28 + _tmp32) / std::pow(_tmp16, Scalar(2)); - const Scalar _tmp34 = _tmp33 * (_tmp10 * (-_tmp18 + _tmp19) + _tmp4 * (_tmp12 + _tmp25) + - state(6, 0) * (_tmp21 + _tmp26 + _tmp27)); - const Scalar _tmp35 = - _tmp17 * (_tmp10 * (_tmp29 - _tmp8) + _tmp4 * (_tmp0 + _tmp1 + _tmp22 + _tmp26) + - state(6, 0) * (_tmp11 + _tmp25)) - - _tmp33 * (_tmp10 * (_tmp23 + _tmp27) + _tmp32); - const Scalar _tmp36 = _tmp3 * _tmp33; - const Scalar _tmp37 = _tmp17 * _tmp30; - const Scalar _tmp38 = -_tmp36 + _tmp37; - const Scalar _tmp39 = _tmp33 * _tmp9; - const Scalar _tmp40 = _tmp17 * _tmp28; - const Scalar _tmp41 = -_tmp39 + _tmp40; - const Scalar _tmp42 = -_tmp14 * _tmp33 + _tmp17 * _tmp31; - const Scalar _tmp43 = _tmp36 - _tmp37; - const Scalar _tmp44 = _tmp39 - _tmp40; - const Scalar _tmp45 = Scalar(1.0) / (math::max(epsilon, innov_var)); + // Intermediate terms (45) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = -state(22, 0) + state(4, 0); + const Scalar _tmp3 = 2 * state(6, 0); + const Scalar _tmp4 = _tmp3 * state(0, 0); + const Scalar _tmp5 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp6 = _tmp5 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp7 = 2 * state(0, 0); + const Scalar _tmp8 = _tmp7 * state(3, 0); + const Scalar _tmp9 = 2 * state(2, 0); + const Scalar _tmp10 = _tmp9 * state(1, 0); + const Scalar _tmp11 = _tmp10 + _tmp8; + const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp9 * state(0, 0); + const Scalar _tmp13 = _tmp0 * _tmp11 + _tmp12 * state(6, 0) + _tmp2 * _tmp6; + const Scalar _tmp14 = + _tmp13 + epsilon * (2 * math::min(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1); + const Scalar _tmp15 = _tmp5 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp16 = _tmp10 - _tmp8; + const Scalar _tmp17 = _tmp7 * state(1, 0) + _tmp9 * state(3, 0); + const Scalar _tmp18 = + (_tmp0 * _tmp15 + _tmp16 * _tmp2 + _tmp17 * state(6, 0)) / std::pow(_tmp14, Scalar(2)); + const Scalar _tmp19 = _tmp3 * state(3, 0); + const Scalar _tmp20 = Scalar(1.0) / (_tmp14); + const Scalar _tmp21 = -_tmp18 * (_tmp0 * _tmp1 - 4 * _tmp2 * state(2, 0) - _tmp4) + + _tmp20 * (_tmp1 * _tmp2 + _tmp19); + const Scalar _tmp22 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp23 = + -Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp9 + _tmp19) + + (Scalar(1) / Scalar(2)) * _tmp20 * (-4 * _tmp0 * state(1, 0) + _tmp2 * _tmp9 + _tmp4); + const Scalar _tmp24 = 2 * state(3, 0); + const Scalar _tmp25 = _tmp3 * state(2, 0); + const Scalar _tmp26 = _tmp3 * state(1, 0); + const Scalar _tmp27 = -_tmp18 * (_tmp0 * _tmp24 - _tmp25) + _tmp20 * (-_tmp2 * _tmp24 + _tmp26); + const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp29 = 4 * state(3, 0); + const Scalar _tmp30 = + -Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp7 - _tmp2 * _tmp29 + _tmp26) + + (Scalar(1) / Scalar(2)) * _tmp20 * (-_tmp0 * _tmp29 - _tmp2 * _tmp7 + _tmp25); + const Scalar _tmp31 = + -_tmp21 * _tmp22 + _tmp23 * state(0, 0) - _tmp28 * state(1, 0) + _tmp30 * state(2, 0); + const Scalar _tmp32 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp33 = + _tmp23 * state(3, 0) - _tmp28 * state(2, 0) - _tmp30 * state(1, 0) + _tmp32 * state(0, 0); + const Scalar _tmp34 = + -_tmp22 * _tmp27 - _tmp23 * state(2, 0) + _tmp30 * state(0, 0) + _tmp32 * state(1, 0); + const Scalar _tmp35 = _tmp18 * _tmp6; + const Scalar _tmp36 = _tmp16 * _tmp20; + const Scalar _tmp37 = -_tmp35 + _tmp36; + const Scalar _tmp38 = _tmp11 * _tmp18; + const Scalar _tmp39 = _tmp15 * _tmp20; + const Scalar _tmp40 = -_tmp38 + _tmp39; + const Scalar _tmp41 = -_tmp12 * _tmp18 + _tmp17 * _tmp20; + const Scalar _tmp42 = _tmp35 - _tmp36; + const Scalar _tmp43 = _tmp38 - _tmp39; + const Scalar _tmp44 = Scalar(1.0) / (math::max(epsilon, innov_var)); // Output terms (2) if (H != nullptr) { @@ -94,88 +97,88 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp24; - _h(1, 0) = -_tmp34; - _h(2, 0) = _tmp35; - _h(3, 0) = _tmp38; - _h(4, 0) = _tmp41; - _h(5, 0) = _tmp42; - _h(21, 0) = _tmp43; - _h(22, 0) = _tmp44; + _h(0, 0) = _tmp31; + _h(1, 0) = _tmp33; + _h(2, 0) = _tmp34; + _h(3, 0) = _tmp37; + _h(4, 0) = _tmp40; + _h(5, 0) = _tmp41; + _h(21, 0) = _tmp42; + _h(22, 0) = _tmp43; } if (K != nullptr) { matrix::Matrix& _k = (*K); _k(0, 0) = - _tmp45 * (P(0, 0) * _tmp24 - P(0, 1) * _tmp34 + P(0, 2) * _tmp35 + P(0, 21) * _tmp43 + - P(0, 22) * _tmp44 + P(0, 3) * _tmp38 + P(0, 4) * _tmp41 + P(0, 5) * _tmp42); + _tmp44 * (P(0, 0) * _tmp31 + P(0, 1) * _tmp33 + P(0, 2) * _tmp34 + P(0, 21) * _tmp42 + + P(0, 22) * _tmp43 + P(0, 3) * _tmp37 + P(0, 4) * _tmp40 + P(0, 5) * _tmp41); _k(1, 0) = - _tmp45 * (P(1, 0) * _tmp24 - P(1, 1) * _tmp34 + P(1, 2) * _tmp35 + P(1, 21) * _tmp43 + - P(1, 22) * _tmp44 + P(1, 3) * _tmp38 + P(1, 4) * _tmp41 + P(1, 5) * _tmp42); + _tmp44 * (P(1, 0) * _tmp31 + P(1, 1) * _tmp33 + P(1, 2) * _tmp34 + P(1, 21) * _tmp42 + + P(1, 22) * _tmp43 + P(1, 3) * _tmp37 + P(1, 4) * _tmp40 + P(1, 5) * _tmp41); _k(2, 0) = - _tmp45 * (P(2, 0) * _tmp24 - P(2, 1) * _tmp34 + P(2, 2) * _tmp35 + P(2, 21) * _tmp43 + - P(2, 22) * _tmp44 + P(2, 3) * _tmp38 + P(2, 4) * _tmp41 + P(2, 5) * _tmp42); + _tmp44 * (P(2, 0) * _tmp31 + P(2, 1) * _tmp33 + P(2, 2) * _tmp34 + P(2, 21) * _tmp42 + + P(2, 22) * _tmp43 + P(2, 3) * _tmp37 + P(2, 4) * _tmp40 + P(2, 5) * _tmp41); _k(3, 0) = - _tmp45 * (P(3, 0) * _tmp24 - P(3, 1) * _tmp34 + P(3, 2) * _tmp35 + P(3, 21) * _tmp43 + - P(3, 22) * _tmp44 + P(3, 3) * _tmp38 + P(3, 4) * _tmp41 + P(3, 5) * _tmp42); + _tmp44 * (P(3, 0) * _tmp31 + P(3, 1) * _tmp33 + P(3, 2) * _tmp34 + P(3, 21) * _tmp42 + + P(3, 22) * _tmp43 + P(3, 3) * _tmp37 + P(3, 4) * _tmp40 + P(3, 5) * _tmp41); _k(4, 0) = - _tmp45 * (P(4, 0) * _tmp24 - P(4, 1) * _tmp34 + P(4, 2) * _tmp35 + P(4, 21) * _tmp43 + - P(4, 22) * _tmp44 + P(4, 3) * _tmp38 + P(4, 4) * _tmp41 + P(4, 5) * _tmp42); + _tmp44 * (P(4, 0) * _tmp31 + P(4, 1) * _tmp33 + P(4, 2) * _tmp34 + P(4, 21) * _tmp42 + + P(4, 22) * _tmp43 + P(4, 3) * _tmp37 + P(4, 4) * _tmp40 + P(4, 5) * _tmp41); _k(5, 0) = - _tmp45 * (P(5, 0) * _tmp24 - P(5, 1) * _tmp34 + P(5, 2) * _tmp35 + P(5, 21) * _tmp43 + - P(5, 22) * _tmp44 + P(5, 3) * _tmp38 + P(5, 4) * _tmp41 + P(5, 5) * _tmp42); + _tmp44 * (P(5, 0) * _tmp31 + P(5, 1) * _tmp33 + P(5, 2) * _tmp34 + P(5, 21) * _tmp42 + + P(5, 22) * _tmp43 + P(5, 3) * _tmp37 + P(5, 4) * _tmp40 + P(5, 5) * _tmp41); _k(6, 0) = - _tmp45 * (P(6, 0) * _tmp24 - P(6, 1) * _tmp34 + P(6, 2) * _tmp35 + P(6, 21) * _tmp43 + - P(6, 22) * _tmp44 + P(6, 3) * _tmp38 + P(6, 4) * _tmp41 + P(6, 5) * _tmp42); + _tmp44 * (P(6, 0) * _tmp31 + P(6, 1) * _tmp33 + P(6, 2) * _tmp34 + P(6, 21) * _tmp42 + + P(6, 22) * _tmp43 + P(6, 3) * _tmp37 + P(6, 4) * _tmp40 + P(6, 5) * _tmp41); _k(7, 0) = - _tmp45 * (P(7, 0) * _tmp24 - P(7, 1) * _tmp34 + P(7, 2) * _tmp35 + P(7, 21) * _tmp43 + - P(7, 22) * _tmp44 + P(7, 3) * _tmp38 + P(7, 4) * _tmp41 + P(7, 5) * _tmp42); + _tmp44 * (P(7, 0) * _tmp31 + P(7, 1) * _tmp33 + P(7, 2) * _tmp34 + P(7, 21) * _tmp42 + + P(7, 22) * _tmp43 + P(7, 3) * _tmp37 + P(7, 4) * _tmp40 + P(7, 5) * _tmp41); _k(8, 0) = - _tmp45 * (P(8, 0) * _tmp24 - P(8, 1) * _tmp34 + P(8, 2) * _tmp35 + P(8, 21) * _tmp43 + - P(8, 22) * _tmp44 + P(8, 3) * _tmp38 + P(8, 4) * _tmp41 + P(8, 5) * _tmp42); + _tmp44 * (P(8, 0) * _tmp31 + P(8, 1) * _tmp33 + P(8, 2) * _tmp34 + P(8, 21) * _tmp42 + + P(8, 22) * _tmp43 + P(8, 3) * _tmp37 + P(8, 4) * _tmp40 + P(8, 5) * _tmp41); _k(9, 0) = - _tmp45 * (P(9, 0) * _tmp24 - P(9, 1) * _tmp34 + P(9, 2) * _tmp35 + P(9, 21) * _tmp43 + - P(9, 22) * _tmp44 + P(9, 3) * _tmp38 + P(9, 4) * _tmp41 + P(9, 5) * _tmp42); + _tmp44 * (P(9, 0) * _tmp31 + P(9, 1) * _tmp33 + P(9, 2) * _tmp34 + P(9, 21) * _tmp42 + + P(9, 22) * _tmp43 + P(9, 3) * _tmp37 + P(9, 4) * _tmp40 + P(9, 5) * _tmp41); _k(10, 0) = - _tmp45 * (P(10, 0) * _tmp24 - P(10, 1) * _tmp34 + P(10, 2) * _tmp35 + P(10, 21) * _tmp43 + - P(10, 22) * _tmp44 + P(10, 3) * _tmp38 + P(10, 4) * _tmp41 + P(10, 5) * _tmp42); + _tmp44 * (P(10, 0) * _tmp31 + P(10, 1) * _tmp33 + P(10, 2) * _tmp34 + P(10, 21) * _tmp42 + + P(10, 22) * _tmp43 + P(10, 3) * _tmp37 + P(10, 4) * _tmp40 + P(10, 5) * _tmp41); _k(11, 0) = - _tmp45 * (P(11, 0) * _tmp24 - P(11, 1) * _tmp34 + P(11, 2) * _tmp35 + P(11, 21) * _tmp43 + - P(11, 22) * _tmp44 + P(11, 3) * _tmp38 + P(11, 4) * _tmp41 + P(11, 5) * _tmp42); + _tmp44 * (P(11, 0) * _tmp31 + P(11, 1) * _tmp33 + P(11, 2) * _tmp34 + P(11, 21) * _tmp42 + + P(11, 22) * _tmp43 + P(11, 3) * _tmp37 + P(11, 4) * _tmp40 + P(11, 5) * _tmp41); _k(12, 0) = - _tmp45 * (P(12, 0) * _tmp24 - P(12, 1) * _tmp34 + P(12, 2) * _tmp35 + P(12, 21) * _tmp43 + - P(12, 22) * _tmp44 + P(12, 3) * _tmp38 + P(12, 4) * _tmp41 + P(12, 5) * _tmp42); + _tmp44 * (P(12, 0) * _tmp31 + P(12, 1) * _tmp33 + P(12, 2) * _tmp34 + P(12, 21) * _tmp42 + + P(12, 22) * _tmp43 + P(12, 3) * _tmp37 + P(12, 4) * _tmp40 + P(12, 5) * _tmp41); _k(13, 0) = - _tmp45 * (P(13, 0) * _tmp24 - P(13, 1) * _tmp34 + P(13, 2) * _tmp35 + P(13, 21) * _tmp43 + - P(13, 22) * _tmp44 + P(13, 3) * _tmp38 + P(13, 4) * _tmp41 + P(13, 5) * _tmp42); + _tmp44 * (P(13, 0) * _tmp31 + P(13, 1) * _tmp33 + P(13, 2) * _tmp34 + P(13, 21) * _tmp42 + + P(13, 22) * _tmp43 + P(13, 3) * _tmp37 + P(13, 4) * _tmp40 + P(13, 5) * _tmp41); _k(14, 0) = - _tmp45 * (P(14, 0) * _tmp24 - P(14, 1) * _tmp34 + P(14, 2) * _tmp35 + P(14, 21) * _tmp43 + - P(14, 22) * _tmp44 + P(14, 3) * _tmp38 + P(14, 4) * _tmp41 + P(14, 5) * _tmp42); + _tmp44 * (P(14, 0) * _tmp31 + P(14, 1) * _tmp33 + P(14, 2) * _tmp34 + P(14, 21) * _tmp42 + + P(14, 22) * _tmp43 + P(14, 3) * _tmp37 + P(14, 4) * _tmp40 + P(14, 5) * _tmp41); _k(15, 0) = - _tmp45 * (P(15, 0) * _tmp24 - P(15, 1) * _tmp34 + P(15, 2) * _tmp35 + P(15, 21) * _tmp43 + - P(15, 22) * _tmp44 + P(15, 3) * _tmp38 + P(15, 4) * _tmp41 + P(15, 5) * _tmp42); + _tmp44 * (P(15, 0) * _tmp31 + P(15, 1) * _tmp33 + P(15, 2) * _tmp34 + P(15, 21) * _tmp42 + + P(15, 22) * _tmp43 + P(15, 3) * _tmp37 + P(15, 4) * _tmp40 + P(15, 5) * _tmp41); _k(16, 0) = - _tmp45 * (P(16, 0) * _tmp24 - P(16, 1) * _tmp34 + P(16, 2) * _tmp35 + P(16, 21) * _tmp43 + - P(16, 22) * _tmp44 + P(16, 3) * _tmp38 + P(16, 4) * _tmp41 + P(16, 5) * _tmp42); + _tmp44 * (P(16, 0) * _tmp31 + P(16, 1) * _tmp33 + P(16, 2) * _tmp34 + P(16, 21) * _tmp42 + + P(16, 22) * _tmp43 + P(16, 3) * _tmp37 + P(16, 4) * _tmp40 + P(16, 5) * _tmp41); _k(17, 0) = - _tmp45 * (P(17, 0) * _tmp24 - P(17, 1) * _tmp34 + P(17, 2) * _tmp35 + P(17, 21) * _tmp43 + - P(17, 22) * _tmp44 + P(17, 3) * _tmp38 + P(17, 4) * _tmp41 + P(17, 5) * _tmp42); + _tmp44 * (P(17, 0) * _tmp31 + P(17, 1) * _tmp33 + P(17, 2) * _tmp34 + P(17, 21) * _tmp42 + + P(17, 22) * _tmp43 + P(17, 3) * _tmp37 + P(17, 4) * _tmp40 + P(17, 5) * _tmp41); _k(18, 0) = - _tmp45 * (P(18, 0) * _tmp24 - P(18, 1) * _tmp34 + P(18, 2) * _tmp35 + P(18, 21) * _tmp43 + - P(18, 22) * _tmp44 + P(18, 3) * _tmp38 + P(18, 4) * _tmp41 + P(18, 5) * _tmp42); + _tmp44 * (P(18, 0) * _tmp31 + P(18, 1) * _tmp33 + P(18, 2) * _tmp34 + P(18, 21) * _tmp42 + + P(18, 22) * _tmp43 + P(18, 3) * _tmp37 + P(18, 4) * _tmp40 + P(18, 5) * _tmp41); _k(19, 0) = - _tmp45 * (P(19, 0) * _tmp24 - P(19, 1) * _tmp34 + P(19, 2) * _tmp35 + P(19, 21) * _tmp43 + - P(19, 22) * _tmp44 + P(19, 3) * _tmp38 + P(19, 4) * _tmp41 + P(19, 5) * _tmp42); + _tmp44 * (P(19, 0) * _tmp31 + P(19, 1) * _tmp33 + P(19, 2) * _tmp34 + P(19, 21) * _tmp42 + + P(19, 22) * _tmp43 + P(19, 3) * _tmp37 + P(19, 4) * _tmp40 + P(19, 5) * _tmp41); _k(20, 0) = - _tmp45 * (P(20, 0) * _tmp24 - P(20, 1) * _tmp34 + P(20, 2) * _tmp35 + P(20, 21) * _tmp43 + - P(20, 22) * _tmp44 + P(20, 3) * _tmp38 + P(20, 4) * _tmp41 + P(20, 5) * _tmp42); + _tmp44 * (P(20, 0) * _tmp31 + P(20, 1) * _tmp33 + P(20, 2) * _tmp34 + P(20, 21) * _tmp42 + + P(20, 22) * _tmp43 + P(20, 3) * _tmp37 + P(20, 4) * _tmp40 + P(20, 5) * _tmp41); _k(21, 0) = - _tmp45 * (P(21, 0) * _tmp24 - P(21, 1) * _tmp34 + P(21, 2) * _tmp35 + P(21, 21) * _tmp43 + - P(21, 22) * _tmp44 + P(21, 3) * _tmp38 + P(21, 4) * _tmp41 + P(21, 5) * _tmp42); + _tmp44 * (P(21, 0) * _tmp31 + P(21, 1) * _tmp33 + P(21, 2) * _tmp34 + P(21, 21) * _tmp42 + + P(21, 22) * _tmp43 + P(21, 3) * _tmp37 + P(21, 4) * _tmp40 + P(21, 5) * _tmp41); _k(22, 0) = - _tmp45 * (P(22, 0) * _tmp24 - P(22, 1) * _tmp34 + P(22, 2) * _tmp35 + P(22, 21) * _tmp43 + - P(22, 22) * _tmp44 + P(22, 3) * _tmp38 + P(22, 4) * _tmp41 + P(22, 5) * _tmp42); + _tmp44 * (P(22, 0) * _tmp31 + P(22, 1) * _tmp33 + P(22, 2) * _tmp34 + P(22, 21) * _tmp42 + + P(22, 22) * _tmp43 + P(22, 3) * _tmp37 + P(22, 4) * _tmp40 + P(22, 5) * _tmp41); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h index 48dd3e284f..b93febcede 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h @@ -30,69 +30,70 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov = nullptr, Scalar* const innov_var = nullptr) { - // Total ops: 235 + // Total ops: 265 // Input arrays - // Intermediate terms (46) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp2 = 1 - 2 * _tmp1; - const Scalar _tmp3 = -2 * _tmp0 + _tmp2; - const Scalar _tmp4 = -state(22, 0) + state(4, 0); - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(2, 0); - const Scalar _tmp9 = _tmp6 + _tmp8; - const Scalar _tmp10 = -state(23, 0) + state(5, 0); - const Scalar _tmp11 = _tmp5 * state(2, 0); - const Scalar _tmp12 = -_tmp11; - const Scalar _tmp13 = _tmp7 * state(3, 0); - const Scalar _tmp14 = _tmp12 + _tmp13; - const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4; - const Scalar _tmp16 = - _tmp15 + epsilon * (2 * math::min(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1); - const Scalar _tmp17 = Scalar(1.0) / (_tmp16); - const Scalar _tmp18 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp19 = -2 * _tmp18 + _tmp2; - const Scalar _tmp20 = -_tmp6; - const Scalar _tmp21 = _tmp20 + _tmp8; - const Scalar _tmp22 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp23 = _tmp5 * state(1, 0); - const Scalar _tmp24 = _tmp22 + _tmp23; - const Scalar _tmp25 = _tmp21 * _tmp4 + _tmp24 * state(6, 0); - const Scalar _tmp26 = _tmp10 * _tmp19 + _tmp25; - const Scalar _tmp27 = _tmp26 / std::pow(_tmp16, Scalar(2)); - const Scalar _tmp28 = -_tmp14 * _tmp27 + _tmp17 * _tmp24; - const Scalar _tmp29 = _tmp27 * _tmp3; - const Scalar _tmp30 = _tmp17 * _tmp21; - const Scalar _tmp31 = -_tmp29 + _tmp30; - const Scalar _tmp32 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp33 = -_tmp32; - const Scalar _tmp34 = -_tmp18; - const Scalar _tmp35 = -_tmp13; - const Scalar _tmp36 = _tmp0 - _tmp1; - const Scalar _tmp37 = _tmp32 + _tmp34; + // Intermediate terms (42) + const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp2 = -state(22, 0) + state(4, 0); + const Scalar _tmp3 = 2 * state(0, 0); + const Scalar _tmp4 = _tmp3 * state(3, 0); + const Scalar _tmp5 = 2 * state(1, 0); + const Scalar _tmp6 = _tmp5 * state(2, 0); + const Scalar _tmp7 = _tmp4 + _tmp6; + const Scalar _tmp8 = -state(23, 0) + state(5, 0); + const Scalar _tmp9 = 2 * state(2, 0); + const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0); + const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8; + const Scalar _tmp12 = + _tmp11 + epsilon * (2 * math::min(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1); + const Scalar _tmp13 = Scalar(1.0) / (_tmp12); + const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp15 = -_tmp4 + _tmp6; + const Scalar _tmp16 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0); + const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0); + const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2)); + const Scalar _tmp19 = _tmp18 * _tmp7; + const Scalar _tmp20 = _tmp13 * _tmp14; + const Scalar _tmp21 = _tmp19 - _tmp20; + const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16; + const Scalar _tmp23 = _tmp1 * _tmp18; + const Scalar _tmp24 = _tmp13 * _tmp15; + const Scalar _tmp25 = -_tmp23 + _tmp24; + const Scalar _tmp26 = 2 * state(6, 0); + const Scalar _tmp27 = _tmp26 * state(0, 0); + const Scalar _tmp28 = _tmp26 * state(3, 0); + const Scalar _tmp29 = + (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) - + Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8); + const Scalar _tmp30 = + (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) - + Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9); + const Scalar _tmp31 = 2 * state(3, 0); + const Scalar _tmp32 = _tmp26 * state(2, 0); + const Scalar _tmp33 = _tmp26 * state(1, 0); + const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) - + Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32); + const Scalar _tmp35 = 4 * state(3, 0); + const Scalar _tmp36 = + (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) - + Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33); + const Scalar _tmp37 = + _tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0); const Scalar _tmp38 = - _tmp17 * (_tmp10 * (_tmp20 - _tmp8) + _tmp4 * (_tmp0 + _tmp1 + _tmp33 + _tmp34) + - state(6, 0) * (_tmp11 + _tmp35)) - - _tmp27 * (_tmp10 * (_tmp36 + _tmp37) + _tmp25); - const Scalar _tmp39 = _tmp29 - _tmp30; - const Scalar _tmp40 = _tmp27 * _tmp9; - const Scalar _tmp41 = _tmp17 * _tmp19; - const Scalar _tmp42 = -_tmp40 + _tmp41; - const Scalar _tmp43 = _tmp27 * (_tmp10 * (-_tmp22 + _tmp23) + _tmp4 * (_tmp12 + _tmp35) + - state(6, 0) * (_tmp18 + _tmp33 + _tmp36)); - const Scalar _tmp44 = _tmp40 - _tmp41; - const Scalar _tmp45 = _tmp17 * (_tmp10 * (_tmp22 - _tmp23) + _tmp4 * (_tmp11 + _tmp13) + - state(6, 0) * (-_tmp0 + _tmp1 + _tmp37)); + -_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0); + const Scalar _tmp39 = + _tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0); + const Scalar _tmp40 = _tmp23 - _tmp24; + const Scalar _tmp41 = -_tmp19 + _tmp20; // Output terms (2) if (innov != nullptr) { Scalar& _innov = (*innov); - _innov = _tmp17 * _tmp26; + _innov = _tmp13 * _tmp17; } if (innov_var != nullptr) { @@ -100,22 +101,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, _innov_var = R + - _tmp28 * (P(0, 5) * _tmp45 - P(1, 5) * _tmp43 + P(2, 5) * _tmp38 + P(21, 5) * _tmp39 + - P(22, 5) * _tmp44 + P(3, 5) * _tmp31 + P(4, 5) * _tmp42 + P(5, 5) * _tmp28) + - _tmp31 * (P(0, 3) * _tmp45 - P(1, 3) * _tmp43 + P(2, 3) * _tmp38 + P(21, 3) * _tmp39 + - P(22, 3) * _tmp44 + P(3, 3) * _tmp31 + P(4, 3) * _tmp42 + P(5, 3) * _tmp28) + - _tmp38 * (P(0, 2) * _tmp45 - P(1, 2) * _tmp43 + P(2, 2) * _tmp38 + P(21, 2) * _tmp39 + - P(22, 2) * _tmp44 + P(3, 2) * _tmp31 + P(4, 2) * _tmp42 + P(5, 2) * _tmp28) + - _tmp39 * (P(0, 21) * _tmp45 - P(1, 21) * _tmp43 + P(2, 21) * _tmp38 + P(21, 21) * _tmp39 + - P(22, 21) * _tmp44 + P(3, 21) * _tmp31 + P(4, 21) * _tmp42 + P(5, 21) * _tmp28) + - _tmp42 * (P(0, 4) * _tmp45 - P(1, 4) * _tmp43 + P(2, 4) * _tmp38 + P(21, 4) * _tmp39 + - P(22, 4) * _tmp44 + P(3, 4) * _tmp31 + P(4, 4) * _tmp42 + P(5, 4) * _tmp28) - - _tmp43 * (P(0, 1) * _tmp45 - P(1, 1) * _tmp43 + P(2, 1) * _tmp38 + P(21, 1) * _tmp39 + - P(22, 1) * _tmp44 + P(3, 1) * _tmp31 + P(4, 1) * _tmp42 + P(5, 1) * _tmp28) + - _tmp44 * (P(0, 22) * _tmp45 - P(1, 22) * _tmp43 + P(2, 22) * _tmp38 + P(21, 22) * _tmp39 + - P(22, 22) * _tmp44 + P(3, 22) * _tmp31 + P(4, 22) * _tmp42 + P(5, 22) * _tmp28) + - _tmp45 * (P(0, 0) * _tmp45 - P(1, 0) * _tmp43 + P(2, 0) * _tmp38 + P(21, 0) * _tmp39 + - P(22, 0) * _tmp44 + P(3, 0) * _tmp31 + P(4, 0) * _tmp42 + P(5, 0) * _tmp28); + _tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 + + P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) + + _tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 + + P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) + + _tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 + + P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) + + _tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 + + P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) + + _tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 + + P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) + + _tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 + + P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) + + _tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 + + P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) + + _tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 + + P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h index 443fb0b03c..e082438f09 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h @@ -29,25 +29,20 @@ void ComputeYawInnovVarAndH(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar R, Scalar* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 36 + // Total ops: 1 + + // Unused inputs + (void)state; // 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 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) - - std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2)); + // Intermediate terms (0) // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3 + P(2, 0) * _tmp4) + - _tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3 + P(2, 1) * _tmp4) + - _tmp4 * (P(0, 2) * _tmp2 + P(1, 2) * _tmp3 + P(2, 2) * _tmp4); + _innov_var = P(2, 2) + R; } if (H != nullptr) { @@ -55,9 +50,7 @@ void ComputeYawInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp2; - _h(1, 0) = _tmp3; - _h(2, 0) = _tmp4; + _h(2, 0) = 1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h index 338ee277c0..5fa0ad1772 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h @@ -34,172 +34,211 @@ matrix::Matrix PredictCovariance(const matrix::Matrix& accel_var, const matrix::Matrix& gyro, const Scalar gyro_var, const Scalar dt) { - // Total ops: 1793 + // Total ops: 1754 + + // Unused inputs + (void)gyro; // Input arrays - // Intermediate terms (134) - const Scalar _tmp0 = dt * gyro(1, 0); - const Scalar _tmp1 = dt * state(11, 0); - const Scalar _tmp2 = -_tmp0 + _tmp1; - const Scalar _tmp3 = dt * gyro(2, 0); - const Scalar _tmp4 = dt * state(12, 0); - const Scalar _tmp5 = _tmp3 - _tmp4; - const Scalar _tmp6 = P(0, 2) + P(1, 2) * _tmp5 + P(2, 2) * _tmp2 - P(9, 2) * dt; - const Scalar _tmp7 = P(0, 1) + P(1, 1) * _tmp5 + P(2, 1) * _tmp2 - P(9, 1) * dt; - const Scalar _tmp8 = P(0, 9) + P(1, 9) * _tmp5 + P(2, 9) * _tmp2 - P(9, 9) * dt; - const Scalar _tmp9 = std::pow(dt, Scalar(2)); - const Scalar _tmp10 = _tmp9 * gyro_var; - const Scalar _tmp11 = P(0, 0) + P(1, 0) * _tmp5 + P(2, 0) * _tmp2 - P(9, 0) * dt; - const Scalar _tmp12 = P(0, 10) + P(1, 10) * _tmp5 + P(2, 10) * _tmp2 - P(9, 10) * dt; - const Scalar _tmp13 = -_tmp3 + _tmp4; - const Scalar _tmp14 = dt * gyro(0, 0); - const Scalar _tmp15 = dt * state(10, 0); - const Scalar _tmp16 = _tmp14 - _tmp15; - const Scalar _tmp17 = P(0, 10) * _tmp13 + P(1, 10) - P(10, 10) * dt + P(2, 10) * _tmp16; - const Scalar _tmp18 = P(0, 0) * _tmp13 + P(1, 0) - P(10, 0) * dt + P(2, 0) * _tmp16; - const Scalar _tmp19 = P(0, 2) * _tmp13 + P(1, 2) - P(10, 2) * dt + P(2, 2) * _tmp16; - const Scalar _tmp20 = P(0, 1) * _tmp13 + P(1, 1) - P(10, 1) * dt + P(2, 1) * _tmp16; - const Scalar _tmp21 = P(0, 11) + P(1, 11) * _tmp5 + P(2, 11) * _tmp2 - P(9, 11) * dt; - const Scalar _tmp22 = _tmp0 - _tmp1; - const Scalar _tmp23 = -_tmp14 + _tmp15; - const Scalar _tmp24 = P(0, 11) * _tmp13 + P(1, 11) - P(10, 11) * dt + P(2, 11) * _tmp16; - const Scalar _tmp25 = P(0, 11) * _tmp22 + P(1, 11) * _tmp23 - P(11, 11) * dt + P(2, 11); - const Scalar _tmp26 = P(0, 1) * _tmp22 + P(1, 1) * _tmp23 - P(11, 1) * dt + P(2, 1); - const Scalar _tmp27 = P(0, 0) * _tmp22 + P(1, 0) * _tmp23 - P(11, 0) * dt + P(2, 0); - const Scalar _tmp28 = P(0, 2) * _tmp22 + P(1, 2) * _tmp23 - P(11, 2) * dt + P(2, 2); - const Scalar _tmp29 = 2 * state(0, 0); - const Scalar _tmp30 = _tmp29 * state(3, 0); - const Scalar _tmp31 = -_tmp30; - const Scalar _tmp32 = 2 * state(1, 0); - const Scalar _tmp33 = _tmp32 * state(2, 0); + // Intermediate terms (147) + const Scalar _tmp0 = 2 * state(1, 0); + const Scalar _tmp1 = _tmp0 * state(3, 0); + const Scalar _tmp2 = -_tmp1 * dt; + const Scalar _tmp3 = 2 * state(0, 0); + const Scalar _tmp4 = _tmp3 * state(2, 0); + const Scalar _tmp5 = _tmp4 * dt; + const Scalar _tmp6 = _tmp2 - _tmp5; + const Scalar _tmp7 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp8 = _tmp7 * dt; + const Scalar _tmp9 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp10 = -_tmp9 * dt; + const Scalar _tmp11 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp12 = _tmp11 * dt; + const Scalar _tmp13 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp14 = _tmp13 * dt; + const Scalar _tmp15 = _tmp10 + _tmp12 - _tmp14 + _tmp8; + const Scalar _tmp16 = _tmp13 + _tmp7; + const Scalar _tmp17 = _tmp11 + _tmp9; + const Scalar _tmp18 = _tmp16 + _tmp17; + const Scalar _tmp19 = _tmp0 * state(2, 0); + const Scalar _tmp20 = -_tmp19 * dt; + const Scalar _tmp21 = _tmp3 * state(3, 0); + const Scalar _tmp22 = _tmp21 * dt; + const Scalar _tmp23 = _tmp20 + _tmp22; + const Scalar _tmp24 = + P(0, 11) * _tmp18 + P(10, 11) * _tmp23 + P(11, 11) * _tmp6 + P(9, 11) * _tmp15; + const Scalar _tmp25 = + P(0, 10) * _tmp18 + P(10, 10) * _tmp23 + P(11, 10) * _tmp6 + P(9, 10) * _tmp15; + const Scalar _tmp26 = P(0, 0) * _tmp18 + P(10, 0) * _tmp23 + P(11, 0) * _tmp6 + P(9, 0) * _tmp15; + const Scalar _tmp27 = P(0, 9) * _tmp18 + P(10, 9) * _tmp23 + P(11, 9) * _tmp6 + P(9, 9) * _tmp15; + const Scalar _tmp28 = _tmp10 + _tmp14; + const Scalar _tmp29 = -_tmp12 + _tmp28 + _tmp8; + const Scalar _tmp30 = _tmp0 * state(0, 0); + const Scalar _tmp31 = _tmp30 * dt; + const Scalar _tmp32 = 2 * state(2, 0) * state(3, 0); + const Scalar _tmp33 = -_tmp32 * dt; const Scalar _tmp34 = _tmp31 + _tmp33; - const Scalar _tmp35 = accel(0, 0) - state(13, 0); - const Scalar _tmp36 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp37 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp38 = -_tmp37; - const Scalar _tmp39 = _tmp36 + _tmp38; - const Scalar _tmp40 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp41 = -_tmp40; - const Scalar _tmp42 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp43 = _tmp41 + _tmp42; - const Scalar _tmp44 = accel(1, 0) - state(14, 0); - const Scalar _tmp45 = dt * (_tmp34 * _tmp35 + _tmp44 * (_tmp39 + _tmp43)); - const Scalar _tmp46 = P(0, 12) + P(1, 12) * _tmp5 + P(2, 12) * _tmp2 - P(9, 12) * dt; - const Scalar _tmp47 = -2 * _tmp42; - const Scalar _tmp48 = -2 * _tmp36; - const Scalar _tmp49 = _tmp47 + _tmp48 + 1; - const Scalar _tmp50 = _tmp49 * dt; - const Scalar _tmp51 = _tmp29 * state(2, 0); - const Scalar _tmp52 = -_tmp51; - const Scalar _tmp53 = _tmp32 * state(3, 0); - const Scalar _tmp54 = -_tmp53; - const Scalar _tmp55 = -_tmp42; - const Scalar _tmp56 = _tmp40 + _tmp55; - const Scalar _tmp57 = -_tmp36; - const Scalar _tmp58 = _tmp37 + _tmp57; - const Scalar _tmp59 = accel(2, 0) - state(15, 0); - const Scalar _tmp60 = dt * (_tmp35 * (_tmp52 + _tmp54) + _tmp59 * (_tmp56 + _tmp58)); - const Scalar _tmp61 = _tmp51 + _tmp53; - const Scalar _tmp62 = -_tmp33; - const Scalar _tmp63 = dt * (_tmp44 * _tmp61 + _tmp59 * (_tmp30 + _tmp62)); - const Scalar _tmp64 = P(0, 13) + P(1, 13) * _tmp5 + P(2, 13) * _tmp2 - P(9, 13) * dt; - const Scalar _tmp65 = _tmp34 * dt; - const Scalar _tmp66 = P(0, 14) + P(1, 14) * _tmp5 + P(2, 14) * _tmp2 - P(9, 14) * dt; - const Scalar _tmp67 = _tmp61 * dt; - const Scalar _tmp68 = P(0, 3) + P(1, 3) * _tmp5 + P(2, 3) * _tmp2 - P(9, 3) * dt; - const Scalar _tmp69 = P(0, 13) * _tmp13 + P(1, 13) - P(10, 13) * dt + P(2, 13) * _tmp16; - const Scalar _tmp70 = P(0, 14) * _tmp13 + P(1, 14) - P(10, 14) * dt + P(2, 14) * _tmp16; - const Scalar _tmp71 = P(0, 12) * _tmp13 + P(1, 12) - P(10, 12) * dt + P(2, 12) * _tmp16; - const Scalar _tmp72 = P(0, 3) * _tmp13 + P(1, 3) - P(10, 3) * dt + P(2, 3) * _tmp16; - const Scalar _tmp73 = P(0, 14) * _tmp22 + P(1, 14) * _tmp23 - P(11, 14) * dt + P(2, 14); - const Scalar _tmp74 = P(0, 13) * _tmp22 + P(1, 13) * _tmp23 - P(11, 13) * dt + P(2, 13); - const Scalar _tmp75 = P(0, 12) * _tmp22 + P(1, 12) * _tmp23 - P(11, 12) * dt + P(2, 12); - const Scalar _tmp76 = P(0, 3) * _tmp22 + P(1, 3) * _tmp23 - P(11, 3) * dt + P(2, 3); - const Scalar _tmp77 = P(0, 1) * _tmp63 + P(1, 1) * _tmp60 - P(12, 1) * _tmp50 - - P(13, 1) * _tmp65 - P(14, 1) * _tmp67 + P(2, 1) * _tmp45 + P(3, 1); - const Scalar _tmp78 = P(0, 14) * _tmp63 + P(1, 14) * _tmp60 - P(12, 14) * _tmp50 - - P(13, 14) * _tmp65 - P(14, 14) * _tmp67 + P(2, 14) * _tmp45 + P(3, 14); - const Scalar _tmp79 = P(0, 13) * _tmp63 + P(1, 13) * _tmp60 - P(12, 13) * _tmp50 - - P(13, 13) * _tmp65 - P(14, 13) * _tmp67 + P(2, 13) * _tmp45 + P(3, 13); - const Scalar _tmp80 = P(0, 0) * _tmp63 + P(1, 0) * _tmp60 - P(12, 0) * _tmp50 - - P(13, 0) * _tmp65 - P(14, 0) * _tmp67 + P(2, 0) * _tmp45 + P(3, 0); - const Scalar _tmp81 = _tmp9 * accel_var(0, 0); - const Scalar _tmp82 = P(0, 2) * _tmp63 + P(1, 2) * _tmp60 - P(12, 2) * _tmp50 - - P(13, 2) * _tmp65 - P(14, 2) * _tmp67 + P(2, 2) * _tmp45 + P(3, 2); - const Scalar _tmp83 = _tmp9 * accel_var(1, 0); - const Scalar _tmp84 = _tmp9 * accel_var(2, 0); - const Scalar _tmp85 = P(0, 12) * _tmp63 + P(1, 12) * _tmp60 - P(12, 12) * _tmp50 - - P(13, 12) * _tmp65 - P(14, 12) * _tmp67 + P(2, 12) * _tmp45 + P(3, 12); - const Scalar _tmp86 = P(0, 3) * _tmp63 + P(1, 3) * _tmp60 - P(12, 3) * _tmp50 - - P(13, 3) * _tmp65 - P(14, 3) * _tmp67 + P(2, 3) * _tmp45 + P(3, 3); - const Scalar _tmp87 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp88 = _tmp29 * state(1, 0); - const Scalar _tmp89 = -_tmp88; - const Scalar _tmp90 = _tmp87 + _tmp89; - const Scalar _tmp91 = dt * (_tmp44 * _tmp90 + _tmp59 * (_tmp43 + _tmp58)); - const Scalar _tmp92 = dt * (_tmp35 * (_tmp39 + _tmp56) + _tmp44 * (_tmp31 + _tmp62)); - const Scalar _tmp93 = 1 - 2 * _tmp37; - const Scalar _tmp94 = _tmp47 + _tmp93; - const Scalar _tmp95 = _tmp94 * dt; - const Scalar _tmp96 = _tmp90 * dt; - const Scalar _tmp97 = _tmp30 + _tmp33; - const Scalar _tmp98 = -_tmp87; - const Scalar _tmp99 = dt * (_tmp35 * (_tmp88 + _tmp98) + _tmp59 * _tmp97); - const Scalar _tmp100 = _tmp97 * dt; - const Scalar _tmp101 = P(0, 4) + P(1, 4) * _tmp5 + P(2, 4) * _tmp2 - P(9, 4) * dt; - const Scalar _tmp102 = P(0, 4) * _tmp13 + P(1, 4) - P(10, 4) * dt + P(2, 4) * _tmp16; - const Scalar _tmp103 = _tmp74 * dt; - const Scalar _tmp104 = P(0, 4) * _tmp22 + P(1, 4) * _tmp23 - P(11, 4) * dt + P(2, 4); - const Scalar _tmp105 = _tmp81 * _tmp97; - const Scalar _tmp106 = _tmp84 * _tmp90; - const Scalar _tmp107 = P(0, 4) * _tmp63 + P(1, 4) * _tmp60 - P(12, 4) * _tmp50 - - P(13, 4) * _tmp65 - P(14, 4) * _tmp67 + P(2, 4) * _tmp45 + P(3, 4); - const Scalar _tmp108 = P(0, 14) * _tmp91 + P(1, 14) * _tmp99 - P(12, 14) * _tmp100 - - P(13, 14) * _tmp95 - P(14, 14) * _tmp96 + P(2, 14) * _tmp92 + P(4, 14); - const Scalar _tmp109 = P(0, 0) * _tmp91 + P(1, 0) * _tmp99 - P(12, 0) * _tmp100 - - P(13, 0) * _tmp95 - P(14, 0) * _tmp96 + P(2, 0) * _tmp92 + P(4, 0); - const Scalar _tmp110 = P(0, 13) * _tmp91 + P(1, 13) * _tmp99 - P(12, 13) * _tmp100 - - P(13, 13) * _tmp95 - P(14, 13) * _tmp96 + P(2, 13) * _tmp92 + P(4, 13); - const Scalar _tmp111 = P(0, 12) * _tmp91 + P(1, 12) * _tmp99 - P(12, 12) * _tmp100 - - P(13, 12) * _tmp95 - P(14, 12) * _tmp96 + P(2, 12) * _tmp92 + P(4, 12); - const Scalar _tmp112 = P(0, 1) * _tmp91 + P(1, 1) * _tmp99 - P(12, 1) * _tmp100 - - P(13, 1) * _tmp95 - P(14, 1) * _tmp96 + P(2, 1) * _tmp92 + P(4, 1); - const Scalar _tmp113 = P(0, 2) * _tmp91 + P(1, 2) * _tmp99 - P(12, 2) * _tmp100 - - P(13, 2) * _tmp95 - P(14, 2) * _tmp96 + P(2, 2) * _tmp92 + P(4, 2); - const Scalar _tmp114 = P(0, 4) * _tmp91 + P(1, 4) * _tmp99 - P(12, 4) * _tmp100 - - P(13, 4) * _tmp95 - P(14, 4) * _tmp96 + P(2, 4) * _tmp92 + P(4, 4); - const Scalar _tmp115 = - dt * (_tmp44 * (_tmp38 + _tmp40 + _tmp42 + _tmp57) + _tmp59 * (_tmp89 + _tmp98)); - const Scalar _tmp116 = _tmp48 + _tmp93; - const Scalar _tmp117 = _tmp116 * dt; - const Scalar _tmp118 = _tmp87 + _tmp88; - const Scalar _tmp119 = _tmp118 * dt; - const Scalar _tmp120 = dt * (_tmp118 * _tmp35 + _tmp44 * (_tmp51 + _tmp54)); - const Scalar _tmp121 = _tmp52 + _tmp53; - const Scalar _tmp122 = dt * (_tmp121 * _tmp59 + _tmp35 * (_tmp36 + _tmp37 + _tmp41 + _tmp55)); - const Scalar _tmp123 = _tmp121 * dt; - const Scalar _tmp124 = P(0, 5) + P(1, 5) * _tmp5 + P(2, 5) * _tmp2 - P(9, 5) * dt; - const Scalar _tmp125 = P(0, 5) * _tmp13 + P(1, 5) - P(10, 5) * dt + P(2, 5) * _tmp16; - const Scalar _tmp126 = P(0, 5) * _tmp22 + P(1, 5) * _tmp23 - P(11, 5) * dt + P(2, 5); - const Scalar _tmp127 = _tmp118 * _tmp83; - const Scalar _tmp128 = P(0, 5) * _tmp63 + P(1, 5) * _tmp60 - P(12, 5) * _tmp50 - - P(13, 5) * _tmp65 - P(14, 5) * _tmp67 + P(2, 5) * _tmp45 + P(3, 5); - const Scalar _tmp129 = P(0, 5) * _tmp91 + P(1, 5) * _tmp99 - P(12, 5) * _tmp100 - - P(13, 5) * _tmp95 - P(14, 5) * _tmp96 + P(2, 5) * _tmp92 + P(4, 5); - const Scalar _tmp130 = P(0, 13) * _tmp115 + P(1, 13) * _tmp122 - P(12, 13) * _tmp123 - - P(13, 13) * _tmp119 - P(14, 13) * _tmp117 + P(2, 13) * _tmp120 + P(5, 13); - const Scalar _tmp131 = P(0, 14) * _tmp115 + P(1, 14) * _tmp122 - P(12, 14) * _tmp123 - - P(13, 14) * _tmp119 - P(14, 14) * _tmp117 + P(2, 14) * _tmp120 + P(5, 14); - const Scalar _tmp132 = P(0, 12) * _tmp115 + P(1, 12) * _tmp122 - P(12, 12) * _tmp123 - - P(13, 12) * _tmp119 - P(14, 12) * _tmp117 + P(2, 12) * _tmp120 + P(5, 12); - const Scalar _tmp133 = P(0, 5) * _tmp115 + P(1, 5) * _tmp122 - P(12, 5) * _tmp123 - - P(13, 5) * _tmp119 - P(14, 5) * _tmp117 + P(2, 5) * _tmp120 + P(5, 5); + const Scalar _tmp35 = P(0, 1) * _tmp18 + P(10, 1) * _tmp23 + P(11, 1) * _tmp6 + P(9, 1) * _tmp15; + const Scalar _tmp36 = _tmp20 - _tmp22; + const Scalar _tmp37 = _tmp23 * gyro_var; + const Scalar _tmp38 = _tmp36 * gyro_var; + const Scalar _tmp39 = _tmp6 * gyro_var; + const Scalar _tmp40 = + P(1, 10) * _tmp18 + P(10, 10) * _tmp29 + P(11, 10) * _tmp34 + P(9, 10) * _tmp36; + const Scalar _tmp41 = + P(1, 11) * _tmp18 + P(10, 11) * _tmp29 + P(11, 11) * _tmp34 + P(9, 11) * _tmp36; + const Scalar _tmp42 = P(1, 9) * _tmp18 + P(10, 9) * _tmp29 + P(11, 9) * _tmp34 + P(9, 9) * _tmp36; + const Scalar _tmp43 = P(1, 1) * _tmp18 + P(10, 1) * _tmp29 + P(11, 1) * _tmp34 + P(9, 1) * _tmp36; + const Scalar _tmp44 = _tmp12 + _tmp28 - _tmp8; + const Scalar _tmp45 = -_tmp31 + _tmp33; + const Scalar _tmp46 = P(0, 2) * _tmp18 + P(10, 2) * _tmp23 + P(11, 2) * _tmp6 + P(9, 2) * _tmp15; + const Scalar _tmp47 = _tmp2 + _tmp5; + const Scalar _tmp48 = P(1, 2) * _tmp18 + P(10, 2) * _tmp29 + P(11, 2) * _tmp34 + P(9, 2) * _tmp36; + const Scalar _tmp49 = + P(10, 10) * _tmp45 + P(11, 10) * _tmp44 + P(2, 10) * _tmp18 + P(9, 10) * _tmp47; + const Scalar _tmp50 = + P(10, 11) * _tmp45 + P(11, 11) * _tmp44 + P(2, 11) * _tmp18 + P(9, 11) * _tmp47; + const Scalar _tmp51 = P(10, 2) * _tmp45 + P(11, 2) * _tmp44 + P(2, 2) * _tmp18 + P(9, 2) * _tmp47; + const Scalar _tmp52 = P(10, 9) * _tmp45 + P(11, 9) * _tmp44 + P(2, 9) * _tmp18 + P(9, 9) * _tmp47; + const Scalar _tmp53 = + P(0, 12) * _tmp18 + P(10, 12) * _tmp23 + P(11, 12) * _tmp6 + P(9, 12) * _tmp15; + const Scalar _tmp54 = -2 * _tmp7; + const Scalar _tmp55 = -2 * _tmp11; + const Scalar _tmp56 = _tmp54 + _tmp55 + 1; + const Scalar _tmp57 = _tmp56 * dt; + const Scalar _tmp58 = + P(0, 13) * _tmp18 + P(10, 13) * _tmp23 + P(11, 13) * _tmp6 + P(9, 13) * _tmp15; + const Scalar _tmp59 = -_tmp21; + const Scalar _tmp60 = _tmp19 + _tmp59; + const Scalar _tmp61 = _tmp60 * dt; + const Scalar _tmp62 = + P(0, 14) * _tmp18 + P(10, 14) * _tmp23 + P(11, 14) * _tmp6 + P(9, 14) * _tmp15; + const Scalar _tmp63 = _tmp1 + _tmp4; + const Scalar _tmp64 = _tmp63 * dt; + const Scalar _tmp65 = -_tmp4; + const Scalar _tmp66 = _tmp1 + _tmp65; + const Scalar _tmp67 = accel(0, 0) - state(13, 0); + const Scalar _tmp68 = -_tmp13; + const Scalar _tmp69 = _tmp68 + _tmp7; + const Scalar _tmp70 = -_tmp11; + const Scalar _tmp71 = _tmp70 + _tmp9; + const Scalar _tmp72 = accel(2, 0) - state(15, 0); + const Scalar _tmp73 = _tmp30 + _tmp32; + const Scalar _tmp74 = accel(1, 0) - state(14, 0); + const Scalar _tmp75 = dt * (_tmp66 * _tmp67 + _tmp72 * (_tmp69 + _tmp71) + _tmp73 * _tmp74); + const Scalar _tmp76 = -_tmp19; + const Scalar _tmp77 = -_tmp9; + const Scalar _tmp78 = -_tmp32; + const Scalar _tmp79 = dt * (_tmp67 * (_tmp59 + _tmp76) + _tmp72 * (_tmp30 + _tmp78) + + _tmp74 * (_tmp16 + _tmp70 + _tmp77)); + const Scalar _tmp80 = P(0, 3) * _tmp18 + P(10, 3) * _tmp23 + P(11, 3) * _tmp6 + P(9, 3) * _tmp15; + const Scalar _tmp81 = + P(1, 12) * _tmp18 + P(10, 12) * _tmp29 + P(11, 12) * _tmp34 + P(9, 12) * _tmp36; + const Scalar _tmp82 = + P(1, 13) * _tmp18 + P(10, 13) * _tmp29 + P(11, 13) * _tmp34 + P(9, 13) * _tmp36; + const Scalar _tmp83 = + P(1, 14) * _tmp18 + P(10, 14) * _tmp29 + P(11, 14) * _tmp34 + P(9, 14) * _tmp36; + const Scalar _tmp84 = P(1, 3) * _tmp18 + P(10, 3) * _tmp29 + P(11, 3) * _tmp34 + P(9, 3) * _tmp36; + const Scalar _tmp85 = + P(10, 12) * _tmp45 + P(11, 12) * _tmp44 + P(2, 12) * _tmp18 + P(9, 12) * _tmp47; + const Scalar _tmp86 = P(10, 1) * _tmp45 + P(11, 1) * _tmp44 + P(2, 1) * _tmp18 + P(9, 1) * _tmp47; + const Scalar _tmp87 = + P(10, 13) * _tmp45 + P(11, 13) * _tmp44 + P(2, 13) * _tmp18 + P(9, 13) * _tmp47; + const Scalar _tmp88 = + P(10, 14) * _tmp45 + P(11, 14) * _tmp44 + P(2, 14) * _tmp18 + P(9, 14) * _tmp47; + const Scalar _tmp89 = P(10, 3) * _tmp45 + P(11, 3) * _tmp44 + P(2, 3) * _tmp18 + P(9, 3) * _tmp47; + const Scalar _tmp90 = P(1, 12) * _tmp75 - P(12, 12) * _tmp57 - P(13, 12) * _tmp61 - + P(14, 12) * _tmp64 + P(2, 12) * _tmp79 + P(3, 12); + const Scalar _tmp91 = P(1, 1) * _tmp75 - P(12, 1) * _tmp57 - P(13, 1) * _tmp61 - + P(14, 1) * _tmp64 + P(2, 1) * _tmp79 + P(3, 1); + const Scalar _tmp92 = P(1, 2) * _tmp75 - P(12, 2) * _tmp57 - P(13, 2) * _tmp61 - + P(14, 2) * _tmp64 + P(2, 2) * _tmp79 + P(3, 2); + const Scalar _tmp93 = P(1, 13) * _tmp75 - P(12, 13) * _tmp57 - P(13, 13) * _tmp61 - + P(14, 13) * _tmp64 + P(2, 13) * _tmp79 + P(3, 13); + const Scalar _tmp94 = P(1, 14) * _tmp75 - P(12, 14) * _tmp57 - P(13, 14) * _tmp61 - + P(14, 14) * _tmp64 + P(2, 14) * _tmp79 + P(3, 14); + const Scalar _tmp95 = std::pow(dt, Scalar(2)); + const Scalar _tmp96 = _tmp95 * accel_var(0, 0); + const Scalar _tmp97 = _tmp95 * accel_var(1, 0); + const Scalar _tmp98 = _tmp95 * accel_var(2, 0); + const Scalar _tmp99 = P(1, 3) * _tmp75 - P(12, 3) * _tmp57 - P(13, 3) * _tmp61 - + P(14, 3) * _tmp64 + P(2, 3) * _tmp79 + P(3, 3); + const Scalar _tmp100 = 1 - 2 * _tmp13; + const Scalar _tmp101 = _tmp100 + _tmp54; + const Scalar _tmp102 = _tmp101 * dt; + const Scalar _tmp103 = -_tmp7; + const Scalar _tmp104 = _tmp103 + _tmp13; + const Scalar _tmp105 = dt * (_tmp60 * _tmp74 + _tmp63 * _tmp72 + _tmp67 * (_tmp104 + _tmp71)); + const Scalar _tmp106 = -_tmp30; + const Scalar _tmp107 = _tmp106 + _tmp32; + const Scalar _tmp108 = _tmp107 * dt; + const Scalar _tmp109 = _tmp19 + _tmp21; + const Scalar _tmp110 = _tmp109 * dt; + const Scalar _tmp111 = -_tmp1; + const Scalar _tmp112 = _tmp11 + _tmp77; + const Scalar _tmp113 = + _tmp67 * (_tmp111 + _tmp4) + _tmp72 * (_tmp104 + _tmp112) + _tmp74 * (_tmp106 + _tmp78); + const Scalar _tmp114 = _tmp26 * dt; + const Scalar _tmp115 = P(0, 4) * _tmp18 + P(10, 4) * _tmp23 + P(11, 4) * _tmp6 + P(9, 4) * _tmp15; + const Scalar _tmp116 = + P(1, 0) * _tmp18 + P(10, 0) * _tmp29 + P(11, 0) * _tmp34 + P(9, 0) * _tmp36; + const Scalar _tmp117 = _tmp113 * dt; + const Scalar _tmp118 = + P(1, 4) * _tmp18 + P(10, 4) * _tmp29 + P(11, 4) * _tmp34 + P(9, 4) * _tmp36; + const Scalar _tmp119 = + P(10, 0) * _tmp45 + P(11, 0) * _tmp44 + P(2, 0) * _tmp18 + P(9, 0) * _tmp47; + const Scalar _tmp120 = + P(10, 4) * _tmp45 + P(11, 4) * _tmp44 + P(2, 4) * _tmp18 + P(9, 4) * _tmp47; + const Scalar _tmp121 = _tmp56 * _tmp96; + const Scalar _tmp122 = _tmp60 * _tmp97; + const Scalar _tmp123 = P(1, 0) * _tmp75 - P(12, 0) * _tmp57 - P(13, 0) * _tmp61 - + P(14, 0) * _tmp64 + P(2, 0) * _tmp79 + P(3, 0); + const Scalar _tmp124 = _tmp63 * _tmp98; + const Scalar _tmp125 = P(1, 4) * _tmp75 - P(12, 4) * _tmp57 - P(13, 4) * _tmp61 - + P(14, 4) * _tmp64 + P(2, 4) * _tmp79 + P(3, 4); + const Scalar _tmp126 = P(0, 0) * _tmp117 - P(12, 0) * _tmp110 - P(13, 0) * _tmp102 - + P(14, 0) * _tmp108 + P(2, 0) * _tmp105 + P(4, 0); + const Scalar _tmp127 = P(0, 13) * _tmp117 - P(12, 13) * _tmp110 - P(13, 13) * _tmp102 - + P(14, 13) * _tmp108 + P(2, 13) * _tmp105 + P(4, 13); + const Scalar _tmp128 = P(0, 14) * _tmp117 - P(12, 14) * _tmp110 - P(13, 14) * _tmp102 - + P(14, 14) * _tmp108 + P(2, 14) * _tmp105 + P(4, 14); + const Scalar _tmp129 = P(0, 12) * _tmp117 - P(12, 12) * _tmp110 - P(13, 12) * _tmp102 - + P(14, 12) * _tmp108 + P(2, 12) * _tmp105 + P(4, 12); + const Scalar _tmp130 = P(0, 4) * _tmp117 - P(12, 4) * _tmp110 - P(13, 4) * _tmp102 - + P(14, 4) * _tmp108 + P(2, 4) * _tmp105 + P(4, 4); + const Scalar _tmp131 = _tmp100 + _tmp55; + const Scalar _tmp132 = _tmp131 * dt; + const Scalar _tmp133 = + dt * (_tmp67 * (_tmp112 + _tmp69) + _tmp72 * (_tmp111 + _tmp65) + _tmp74 * (_tmp21 + _tmp76)); + const Scalar _tmp134 = _tmp73 * dt; + const Scalar _tmp135 = _tmp66 * dt; + const Scalar _tmp136 = _tmp107 * _tmp72 + _tmp109 * _tmp67 + _tmp74 * (_tmp103 + _tmp17 + _tmp68); + const Scalar _tmp137 = P(0, 5) * _tmp18 + P(10, 5) * _tmp23 + P(11, 5) * _tmp6 + P(9, 5) * _tmp15; + const Scalar _tmp138 = _tmp136 * dt; + const Scalar _tmp139 = + P(1, 5) * _tmp18 + P(10, 5) * _tmp29 + P(11, 5) * _tmp34 + P(9, 5) * _tmp36; + const Scalar _tmp140 = + P(10, 5) * _tmp45 + P(11, 5) * _tmp44 + P(2, 5) * _tmp18 + P(9, 5) * _tmp47; + const Scalar _tmp141 = P(1, 5) * _tmp75 - P(12, 5) * _tmp57 - P(13, 5) * _tmp61 - + P(14, 5) * _tmp64 + P(2, 5) * _tmp79 + P(3, 5); + const Scalar _tmp142 = P(0, 5) * _tmp117 - P(12, 5) * _tmp110 - P(13, 5) * _tmp102 - + P(14, 5) * _tmp108 + P(2, 5) * _tmp105 + P(4, 5); + const Scalar _tmp143 = P(0, 14) * _tmp138 + P(1, 14) * _tmp133 - P(12, 14) * _tmp135 - + P(13, 14) * _tmp134 - P(14, 14) * _tmp132 + P(5, 14); + const Scalar _tmp144 = P(0, 13) * _tmp138 + P(1, 13) * _tmp133 - P(12, 13) * _tmp135 - + P(13, 13) * _tmp134 - P(14, 13) * _tmp132 + P(5, 13); + const Scalar _tmp145 = P(0, 12) * _tmp138 + P(1, 12) * _tmp133 - P(12, 12) * _tmp135 - + P(13, 12) * _tmp134 - P(14, 12) * _tmp132 + P(5, 12); + const Scalar _tmp146 = P(0, 5) * _tmp138 + P(1, 5) * _tmp133 - P(12, 5) * _tmp135 - + P(13, 5) * _tmp134 - P(14, 5) * _tmp132 + P(5, 5); // Output terms (1) matrix::Matrix _res; - _res(0, 0) = _tmp10 + _tmp11 + _tmp2 * _tmp6 + _tmp5 * _tmp7 - _tmp8 * dt; + _res(0, 0) = std::pow(_tmp15, Scalar(2)) * gyro_var + _tmp15 * _tmp27 + _tmp18 * _tmp26 + + std::pow(_tmp23, Scalar(2)) * gyro_var + _tmp23 * _tmp25 + _tmp24 * _tmp6 + + std::pow(_tmp6, Scalar(2)) * gyro_var; _res(1, 0) = 0; _res(2, 0) = 0; _res(3, 0) = 0; @@ -222,8 +261,11 @@ matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::MatrixgetVelocity()); - EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f) + EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.02f) << "estimated vel = " << estimated_horz_velocity(0); - EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f) + EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.02f) << "estimated vel = " << estimated_horz_velocity(1); } diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp index 142a4ea3ef..fb1990f90b 100644 --- a/src/modules/ekf2/test/test_EKF_initialization.cpp +++ b/src/modules/ekf2/test/test_EKF_initialization.cpp @@ -78,7 +78,7 @@ public: void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f) { - const matrix::Vector3f quat_variance = _ekf->getQuaternionVariance(); + const matrix::Vector3f quat_variance = _ekf->getRotVarBody(); EXPECT_TRUE(quat_variance(0) > quat_variance_limit) << "quat_variance(3): " << quat_variance(0); EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1): " << quat_variance(1); EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2): " << quat_variance(2); diff --git a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp index 7e536e62bb..77d1f10687 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp @@ -42,9 +42,8 @@ using namespace matrix; Vector3f getRotVarNed(const Quatf &q, const SquareMatrixState &P) { constexpr auto S = State::quat_nominal; - matrix::SquareMatrix3f rot_cov_body = P.slice(S.idx, S.idx); - auto R_to_earth = Dcmf(q); - return matrix::SquareMatrix(R_to_earth * rot_cov_body * R_to_earth.T()).diag(); + matrix::SquareMatrix3f rot_cov_ned = P.slice(S.idx, S.idx); + return rot_cov_ned.diag(); } TEST(YawFusionGenerated, positiveVarianceAllOrientations)