From 9d7abf25528950f943cc02b4a8ac4b645e5ed3d8 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 22 Mar 2023 15:54:57 +0100 Subject: [PATCH] ekf2: symforce - use builtin Rot3 and Quaternion operations --- .../EKF/python/ekf_derivation/derivation.py | 46 +- .../python/ekf_derivation/derivation_utils.py | 38 - .../compute_drag_x_innov_var_and_k.h | 251 ++-- .../compute_drag_y_innov_var_and_k.h | 247 ++-- .../compute_flow_xy_innov_var_and_hx.h | 132 +- .../compute_flow_y_innov_var_and_h.h | 63 +- .../compute_gnss_yaw_pred_innov_var_and_h.h | 75 +- .../compute_gravity_innov_var_and_k_and_h.h | 298 ++--- .../compute_mag_innov_innov_var_and_hx.h | 215 ++-- .../generated/compute_mag_y_innov_var_and_h.h | 75 +- .../generated/compute_mag_z_innov_var_and_h.h | 75 +- .../generated/compute_sideslip_h_and_k.h | 257 ++-- .../compute_sideslip_innov_and_innov_var.h | 146 ++- .../compute_yaw_312_innov_var_and_h.h | 50 +- ...ompute_yaw_312_innov_var_and_h_alternate.h | 48 +- .../compute_yaw_321_innov_var_and_h.h | 50 +- ...ompute_yaw_321_innov_var_and_h_alternate.h | 48 +- .../generated/predict_covariance.h | 1096 ++++++++--------- 18 files changed, 1566 insertions(+), 1644 deletions(-) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index d4d094207d..5568fe27d8 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -69,6 +69,12 @@ class VState(sf.Matrix): class MState(sf.Matrix): SHAPE = (State.n_states, State.n_states) +def state_to_quat(state: VState) -> sf.Quaternion: + return sf.Quaternion(sf.V3(state[State.qx], state[State.qy], state[State.qz]), state[State.qw]) + +def state_to_rot3(state: VState) -> sf.Rot3: + return sf.Rot3(state_to_quat(state)) + def predict_covariance( state: VState, P: MState, @@ -86,17 +92,17 @@ def predict_covariance( d_ang_b = sf.V3(state[State.d_ang_bx], state[State.d_ang_by], state[State.d_ang_bz]) d_ang_true = d_ang - d_ang_b - q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_earth = quat_to_rot_simplified(q) + q = state_to_quat(state) + R_to_earth = sf.Rot3(q) v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) p = sf.V3(state[State.px], state[State.py], state[State.pz]) - q_new = quat_mult(q, sf.V4(1, 0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2])) + q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1) v_new = v + R_to_earth * d_vel_true + sf.V3(0 ,0 ,g) * dt p_new = p + v * dt # Predicted state vector at time t + dt - state_new = VState.block_matrix([[q_new], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]]) + state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]]) # State propagation jacobian A = state_new.jacobian(state) @@ -155,8 +161,7 @@ def predict_sideslip( ) -> (sf.Scalar): vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - relative_wind_body = quat_to_rot(q_att).T * vel_rel + relative_wind_body = state_to_rot3(state).inverse() * vel_rel # Small angle approximation of side slip model # Protect division by zero using epsilon @@ -196,11 +201,10 @@ def compute_sideslip_h_and_k( return (H.T, K) def predict_mag_body(state) -> sf.V3: - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) mag_field_earth = sf.V3(state[State.ix], state[State.iy], state[State.iz]) mag_bias_body = sf.V3(state[State.ibx], state[State.iby], state[State.ibz]) - mag_body = quat_to_rot(q_att).T * mag_field_earth + mag_bias_body + mag_body = state_to_rot3(state).inverse() * mag_field_earth + mag_bias_body return mag_body def compute_mag_innov_innov_var_and_hx( @@ -260,8 +264,7 @@ def compute_yaw_321_innov_var_and_h( epsilon: sf.Scalar ) -> (sf.Scalar, VState): - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_earth = quat_to_rot(q_att) + R_to_earth = state_to_rot3(state).to_rotation_matrix() # Fix the singularity at pi/2 by inserting epsilon meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon) @@ -277,8 +280,7 @@ def compute_yaw_321_innov_var_and_h_alternate( epsilon: sf.Scalar ) -> (sf.Scalar, VState): - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_earth = quat_to_rot(q_att) + R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form that has a singularity at yaw 0 instead of pi/2 meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon) @@ -294,8 +296,7 @@ def compute_yaw_312_innov_var_and_h( epsilon: sf.Scalar ) -> (sf.Scalar, VState): - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_earth = quat_to_rot(q_att) + R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form to be used when close to pitch +-pi/2 meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon) @@ -311,8 +312,7 @@ def compute_yaw_312_innov_var_and_h_alternate( epsilon: sf.Scalar ) -> (sf.Scalar, VState): - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_earth = quat_to_rot(q_att) + R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form to be used when close to pitch +-pi/2 meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon) @@ -336,9 +336,7 @@ def compute_mag_declination_pred_innov_var_and_h( return (meas_pred, innov_var, H.T) def predict_opt_flow(state, distance, epsilon): - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_earth = quat_to_rot(q_att) - R_to_body = R_to_earth.T + R_to_body = state_to_rot3(state).inverse() # Calculate earth relative velocity in a non-rotating sensor frame v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) @@ -393,8 +391,7 @@ def compute_gnss_yaw_pred_innov_var_and_h( epsilon: sf.Scalar ) -> (sf.Scalar, sf.Scalar, VState): - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_earth = quat_to_rot(q_att) + R_to_earth = state_to_rot3(state) # define antenna vector in body frame ant_vec_bf = sf.V3(sf.cos(antenna_yaw_offset), sf.sin(antenna_yaw_offset), 0) @@ -417,9 +414,7 @@ def predict_drag( cm: sf.Scalar, epsilon: sf.Scalar ): - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_earth = quat_to_rot(q_att) - R_to_body = R_to_earth.T + R_to_body = state_to_rot3(state).inverse() vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], @@ -481,8 +476,7 @@ def compute_gravity_innov_var_and_k_and_h( ) -> (sf.V3, sf.V3, VState, VState, VState): # get transform from earth to body frame - q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) - R_to_body = quat_to_rot(q_att).T + R_to_body = state_to_rot3(state).inverse() # the innovation is the error between measured acceleration # and predicted (body frame), assuming no body acceleration diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py index 963a139f55..ca2987e00f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -38,44 +38,6 @@ import symforce.symbolic as sf import re -# q: quaternion describing rotation from frame 1 to frame 2 -# returns a rotation matrix derived form q which describes the same -# rotation -def quat_to_rot(q): - q0 = q[0] - q1 = q[1] - q2 = q[2] - q3 = q[3] - - Rot = sf.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], - [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], - [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) - - return Rot - -def quat_to_rot_simplified(q): - q0 = q[0] - q1 = q[1] - q2 = q[2] - q3 = q[3] - - # Use the simplified formula for unit quaternion to rotation matrix - # as it produces a simpler and more stable EKF derivation given - # the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1 - Rot = sf.Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], - [2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)], - [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]]) - - return Rot - -def quat_mult(p,q): - r = sf.Matrix([p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3], - p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2], - p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1], - p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0]]) - - return r - def sign_no_zero(x) -> sf.Scalar: """ Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h index bcff0c6179..1ca82c4021 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h @@ -34,136 +34,133 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix& state, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const K = nullptr) { - // Total ops: 407 + // Total ops: 398 // Input arrays - // Intermediate terms (76) - const Scalar _tmp0 = -state(23, 0) + state(5, 0); - const Scalar _tmp1 = 2 * _tmp0; - const Scalar _tmp2 = _tmp1 * state(1, 0); - const Scalar _tmp3 = -state(22, 0) + state(4, 0); - const Scalar _tmp4 = 2 * _tmp3; - const Scalar _tmp5 = _tmp4 * state(2, 0); - const Scalar _tmp6 = 2 * state(6, 0); - const Scalar _tmp7 = _tmp6 * state(0, 0); - const Scalar _tmp8 = _tmp2 - _tmp5 - _tmp7; - const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp10 = -_tmp9; - const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp13 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp14 = -_tmp12 + _tmp13; - const Scalar _tmp15 = _tmp10 + _tmp11 + _tmp14; - const Scalar _tmp16 = state(0, 0) * state(3, 0); - const Scalar _tmp17 = state(1, 0) * state(2, 0); - const Scalar _tmp18 = _tmp16 + _tmp17; - const Scalar _tmp19 = state(0, 0) * state(2, 0); - const Scalar _tmp20 = state(1, 0) * state(3, 0); - const Scalar _tmp21 = _tmp1 * _tmp18 + _tmp15 * _tmp3 + _tmp6 * (-_tmp19 + _tmp20); - const Scalar _tmp22 = state(2, 0) * state(3, 0); - const Scalar _tmp23 = state(0, 0) * state(1, 0); - const Scalar _tmp24 = _tmp22 - _tmp23; - const Scalar _tmp25 = _tmp19 + _tmp20; - const Scalar _tmp26 = -_tmp11; - const Scalar _tmp27 = _tmp10 + _tmp12 + _tmp13 + _tmp26; - const Scalar _tmp28 = _tmp1 * _tmp24 + _tmp25 * _tmp4 + _tmp27 * state(6, 0); - const Scalar _tmp29 = _tmp14 + _tmp26 + _tmp9; - const Scalar _tmp30 = -_tmp16 + _tmp17; - const Scalar _tmp31 = _tmp0 * _tmp29 + _tmp30 * _tmp4 + _tmp6 * (_tmp22 + _tmp23); - const Scalar _tmp32 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp28, Scalar(2)) + - std::pow(_tmp31, Scalar(2)) + epsilon)); - const Scalar _tmp33 = cd * rho; - const Scalar _tmp34 = _tmp32 * _tmp33; - const Scalar _tmp35 = Scalar(0.5) * _tmp34; - const Scalar _tmp36 = _tmp4 * state(0, 0); - const Scalar _tmp37 = _tmp1 * state(3, 0); - const Scalar _tmp38 = _tmp6 * state(2, 0); - const Scalar _tmp39 = _tmp36 + _tmp37 - _tmp38; - const Scalar _tmp40 = 2 * _tmp28; - const Scalar _tmp41 = _tmp1 * state(2, 0) + _tmp4 * state(1, 0) + _tmp6 * state(3, 0); - const Scalar _tmp42 = 2 * _tmp31; - const Scalar _tmp43 = 2 * _tmp21; - const Scalar _tmp44 = Scalar(0.25) * _tmp21 * _tmp33 / _tmp32; - const Scalar _tmp45 = - -_tmp35 * _tmp8 - _tmp44 * (_tmp39 * _tmp40 + _tmp41 * _tmp42 + _tmp43 * _tmp8) - _tmp8 * cm; - const Scalar _tmp46 = 2 * _tmp19; - const Scalar _tmp47 = 2 * _tmp20; - const Scalar _tmp48 = -_tmp46 + _tmp47; - const Scalar _tmp49 = 2 * _tmp22; - const Scalar _tmp50 = 2 * _tmp23; - const Scalar _tmp51 = -_tmp35 * _tmp48 - - _tmp44 * (_tmp27 * _tmp40 + _tmp42 * (_tmp49 + _tmp50) + _tmp43 * _tmp48) - - _tmp48 * cm; - const Scalar _tmp52 = -_tmp2 + _tmp5 + _tmp7; - const Scalar _tmp53 = _tmp4 * state(3, 0); - const Scalar _tmp54 = _tmp1 * state(0, 0); - const Scalar _tmp55 = _tmp6 * state(1, 0); - const Scalar _tmp56 = -_tmp53 + _tmp54 + _tmp55; - const Scalar _tmp57 = -_tmp35 * _tmp39 - _tmp39 * cm - - _tmp44 * (_tmp39 * _tmp43 + _tmp40 * _tmp52 + _tmp42 * _tmp56); - const Scalar _tmp58 = _tmp15 * cm; - const Scalar _tmp59 = _tmp15 * _tmp35; - const Scalar _tmp60 = _tmp15 * _tmp43; - const Scalar _tmp61 = 2 * _tmp16; - const Scalar _tmp62 = 2 * _tmp17; - const Scalar _tmp63 = - -_tmp44 * (_tmp40 * (_tmp46 + _tmp47) + _tmp42 * (-_tmp61 + _tmp62) + _tmp60) - _tmp58 - - _tmp59; - const Scalar _tmp64 = 4 * _tmp28; - const Scalar _tmp65 = _tmp29 * _tmp42; - const Scalar _tmp66 = Scalar(1.0) * _tmp18 * _tmp34 + 2 * _tmp18 * cm - - _tmp44 * (-4 * _tmp18 * _tmp21 - _tmp24 * _tmp64 - _tmp65); - const Scalar _tmp67 = - -_tmp35 * _tmp41 - _tmp41 * cm - - _tmp44 * (_tmp40 * (_tmp53 - _tmp54 - _tmp55) + _tmp41 * _tmp43 + _tmp42 * _tmp52); - const Scalar _tmp68 = _tmp61 + _tmp62; - const Scalar _tmp69 = -_tmp35 * _tmp68 - - _tmp44 * (_tmp40 * (_tmp49 - _tmp50) + _tmp43 * _tmp68 + _tmp65) - - _tmp68 * cm; + // Intermediate terms (77) + const Scalar _tmp0 = -state(22, 0) + state(4, 0); + const Scalar _tmp1 = 4 * _tmp0; + const Scalar _tmp2 = -state(23, 0) + state(5, 0); + const Scalar _tmp3 = 2 * state(0, 0); + const Scalar _tmp4 = _tmp2 * _tmp3; + const Scalar _tmp5 = 2 * state(6, 0); + const Scalar _tmp6 = _tmp5 * state(1, 0); + const Scalar _tmp7 = -_tmp1 * state(3, 0) + _tmp4 + _tmp6; + const Scalar _tmp8 = 2 * state(2, 0); + const Scalar _tmp9 = _tmp2 * _tmp8; + const Scalar _tmp10 = 2 * state(1, 0); + const Scalar _tmp11 = _tmp0 * _tmp10; + const Scalar _tmp12 = _tmp8 * state(3, 0); + const Scalar _tmp13 = _tmp3 * state(1, 0); + const Scalar _tmp14 = _tmp12 - _tmp13; + const Scalar _tmp15 = _tmp8 * state(0, 0); + const Scalar _tmp16 = _tmp10 * state(3, 0); + const Scalar _tmp17 = _tmp15 + _tmp16; + const Scalar _tmp18 = -2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp19 = 1 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp20 = _tmp18 + _tmp19; + const Scalar _tmp21 = _tmp0 * _tmp17 + _tmp14 * _tmp2 + _tmp20 * state(6, 0); + const Scalar _tmp22 = 2 * _tmp21; + const Scalar _tmp23 = _tmp0 * _tmp3; + const Scalar _tmp24 = 4 * _tmp2; + const Scalar _tmp25 = _tmp8 * state(6, 0); + const Scalar _tmp26 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp27 = _tmp19 + _tmp26; + const Scalar _tmp28 = _tmp3 * state(3, 0); + const Scalar _tmp29 = _tmp8 * state(1, 0); + const Scalar _tmp30 = -_tmp28 + _tmp29; + const Scalar _tmp31 = _tmp12 + _tmp13; + const Scalar _tmp32 = _tmp0 * _tmp30 + _tmp2 * _tmp27 + _tmp31 * state(6, 0); + const Scalar _tmp33 = 2 * _tmp32; + const Scalar _tmp34 = _tmp18 + _tmp26 + 1; + const Scalar _tmp35 = _tmp28 + _tmp29; + const Scalar _tmp36 = -_tmp15 + _tmp16; + const Scalar _tmp37 = _tmp0 * _tmp34 + _tmp2 * _tmp35 + _tmp36 * state(6, 0); + const Scalar _tmp38 = 2 * _tmp37; + const Scalar _tmp39 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp32, Scalar(2)) + + std::pow(_tmp37, Scalar(2)) + epsilon)); + const Scalar _tmp40 = cd * rho; + const Scalar _tmp41 = Scalar(0.25) * _tmp37 * _tmp40 / _tmp39; + const Scalar _tmp42 = Scalar(0.5) * _tmp39 * _tmp40; + const Scalar _tmp43 = + -_tmp41 * (_tmp22 * (_tmp11 + _tmp9) + _tmp33 * (-_tmp23 - _tmp24 * state(3, 0) + _tmp25) + + _tmp38 * _tmp7) - + _tmp42 * _tmp7 - _tmp7 * cm; + const Scalar _tmp44 = _tmp35 * cm; + const Scalar _tmp45 = _tmp35 * _tmp38; + const Scalar _tmp46 = _tmp14 * _tmp22; + const Scalar _tmp47 = _tmp27 * _tmp33; + const Scalar _tmp48 = _tmp35 * _tmp42; + const Scalar _tmp49 = -_tmp41 * (-_tmp45 - _tmp46 - _tmp47) + _tmp44 + _tmp48; + const Scalar _tmp50 = -_tmp41 * (_tmp45 + _tmp46 + _tmp47) - _tmp44 - _tmp48; + const Scalar _tmp51 = _tmp5 * state(3, 0); + const Scalar _tmp52 = _tmp51 + _tmp9; + const Scalar _tmp53 = 2 * state(3, 0); + const Scalar _tmp54 = _tmp0 * _tmp53; + const Scalar _tmp55 = 4 * state(6, 0); + const Scalar _tmp56 = _tmp0 * _tmp8; + const Scalar _tmp57 = _tmp3 * state(6, 0); + const Scalar _tmp58 = + -_tmp41 * (_tmp22 * (-_tmp4 + _tmp54 - _tmp55 * state(1, 0)) + + _tmp33 * (-_tmp24 * state(1, 0) + _tmp56 + _tmp57) + _tmp38 * _tmp52) - + _tmp42 * _tmp52 - _tmp52 * cm; + const Scalar _tmp59 = _tmp10 * _tmp2; + const Scalar _tmp60 = -_tmp1 * state(2, 0) - _tmp57 + _tmp59; + const Scalar _tmp61 = _tmp2 * _tmp53; + const Scalar _tmp62 = -_tmp41 * (_tmp22 * (_tmp23 - _tmp55 * state(2, 0) + _tmp61) + + _tmp33 * (_tmp11 + _tmp51) + _tmp38 * _tmp60) - + _tmp42 * _tmp60 - _tmp60 * cm; + const Scalar _tmp63 = _tmp34 * cm; + const Scalar _tmp64 = _tmp34 * _tmp38; + const Scalar _tmp65 = _tmp17 * _tmp22; + const Scalar _tmp66 = _tmp30 * _tmp33; + const Scalar _tmp67 = _tmp34 * _tmp42; + const Scalar _tmp68 = -_tmp41 * (-_tmp64 - _tmp65 - _tmp66) + _tmp63 + _tmp67; + const Scalar _tmp69 = -_tmp25 + _tmp61; const Scalar _tmp70 = - -_tmp35 * _tmp56 - - _tmp44 * (_tmp40 * _tmp41 + _tmp42 * (-_tmp36 - _tmp37 + _tmp38) + _tmp43 * _tmp56) - - _tmp56 * cm; - const Scalar _tmp71 = - -_tmp44 * (-_tmp25 * _tmp64 - 4 * _tmp30 * _tmp31 - _tmp60) + _tmp58 + _tmp59; - const Scalar _tmp72 = P(23, 23) * _tmp66; - const Scalar _tmp73 = P(22, 22) * _tmp71; - const Scalar _tmp74 = R + - _tmp45 * (P(0, 2) * _tmp57 + P(1, 2) * _tmp67 + P(2, 2) * _tmp45 + - P(22, 2) * _tmp71 + P(23, 2) * _tmp66 + P(3, 2) * _tmp70 + - P(4, 2) * _tmp63 + P(5, 2) * _tmp69 + P(6, 2) * _tmp51) + - _tmp51 * (P(0, 6) * _tmp57 + P(1, 6) * _tmp67 + P(2, 6) * _tmp45 + - P(22, 6) * _tmp71 + P(23, 6) * _tmp66 + P(3, 6) * _tmp70 + - P(4, 6) * _tmp63 + P(5, 6) * _tmp69 + P(6, 6) * _tmp51) + - _tmp57 * (P(0, 0) * _tmp57 + P(1, 0) * _tmp67 + P(2, 0) * _tmp45 + - P(22, 0) * _tmp71 + P(23, 0) * _tmp66 + P(3, 0) * _tmp70 + - P(4, 0) * _tmp63 + P(5, 0) * _tmp69 + P(6, 0) * _tmp51) + - _tmp63 * (P(0, 4) * _tmp57 + P(1, 4) * _tmp67 + P(2, 4) * _tmp45 + - P(22, 4) * _tmp71 + P(23, 4) * _tmp66 + P(3, 4) * _tmp70 + - P(4, 4) * _tmp63 + P(5, 4) * _tmp69 + P(6, 4) * _tmp51) + - _tmp66 * (P(0, 23) * _tmp57 + P(1, 23) * _tmp67 + P(2, 23) * _tmp45 + - P(22, 23) * _tmp71 + P(3, 23) * _tmp70 + P(4, 23) * _tmp63 + - P(5, 23) * _tmp69 + P(6, 23) * _tmp51 + _tmp72) + - _tmp67 * (P(0, 1) * _tmp57 + P(1, 1) * _tmp67 + P(2, 1) * _tmp45 + - P(22, 1) * _tmp71 + P(23, 1) * _tmp66 + P(3, 1) * _tmp70 + - P(4, 1) * _tmp63 + P(5, 1) * _tmp69 + P(6, 1) * _tmp51) + - _tmp69 * (P(0, 5) * _tmp57 + P(1, 5) * _tmp67 + P(2, 5) * _tmp45 + - P(22, 5) * _tmp71 + P(23, 5) * _tmp66 + P(3, 5) * _tmp70 + - P(4, 5) * _tmp63 + P(5, 5) * _tmp69 + P(6, 5) * _tmp51) + - _tmp70 * (P(0, 3) * _tmp57 + P(1, 3) * _tmp67 + P(2, 3) * _tmp45 + - P(22, 3) * _tmp71 + P(23, 3) * _tmp66 + P(3, 3) * _tmp70 + - P(4, 3) * _tmp63 + P(5, 3) * _tmp69 + P(6, 3) * _tmp51) + - _tmp71 * (P(0, 22) * _tmp57 + P(1, 22) * _tmp67 + P(2, 22) * _tmp45 + - P(23, 22) * _tmp66 + P(3, 22) * _tmp70 + P(4, 22) * _tmp63 + - P(5, 22) * _tmp69 + P(6, 22) * _tmp51 + _tmp73); - const Scalar _tmp75 = Scalar(1.0) / (math::max(_tmp74, epsilon)); + -_tmp41 * (_tmp22 * (_tmp56 - _tmp59) + _tmp33 * (-_tmp54 + _tmp6) + _tmp38 * _tmp69) - + _tmp42 * _tmp69 - _tmp69 * cm; + const Scalar _tmp71 = -_tmp41 * (_tmp64 + _tmp65 + _tmp66) - _tmp63 - _tmp67; + const Scalar _tmp72 = -_tmp36 * _tmp42 - _tmp36 * cm - + _tmp41 * (_tmp20 * _tmp22 + _tmp31 * _tmp33 + _tmp36 * _tmp38); + const Scalar _tmp73 = P(23, 23) * _tmp49; + const Scalar _tmp74 = P(22, 22) * _tmp68; + const Scalar _tmp75 = R + + _tmp43 * (P(0, 3) * _tmp70 + P(1, 3) * _tmp58 + P(2, 3) * _tmp62 + + P(22, 3) * _tmp68 + P(23, 3) * _tmp49 + P(3, 3) * _tmp43 + + P(4, 3) * _tmp71 + P(5, 3) * _tmp50 + P(6, 3) * _tmp72) + + _tmp49 * (P(0, 23) * _tmp70 + P(1, 23) * _tmp58 + P(2, 23) * _tmp62 + + P(22, 23) * _tmp68 + P(3, 23) * _tmp43 + P(4, 23) * _tmp71 + + P(5, 23) * _tmp50 + P(6, 23) * _tmp72 + _tmp73) + + _tmp50 * (P(0, 5) * _tmp70 + P(1, 5) * _tmp58 + P(2, 5) * _tmp62 + + P(22, 5) * _tmp68 + P(23, 5) * _tmp49 + P(3, 5) * _tmp43 + + P(4, 5) * _tmp71 + P(5, 5) * _tmp50 + P(6, 5) * _tmp72) + + _tmp58 * (P(0, 1) * _tmp70 + P(1, 1) * _tmp58 + P(2, 1) * _tmp62 + + P(22, 1) * _tmp68 + P(23, 1) * _tmp49 + P(3, 1) * _tmp43 + + P(4, 1) * _tmp71 + P(5, 1) * _tmp50 + P(6, 1) * _tmp72) + + _tmp62 * (P(0, 2) * _tmp70 + P(1, 2) * _tmp58 + P(2, 2) * _tmp62 + + P(22, 2) * _tmp68 + P(23, 2) * _tmp49 + P(3, 2) * _tmp43 + + P(4, 2) * _tmp71 + P(5, 2) * _tmp50 + P(6, 2) * _tmp72) + + _tmp68 * (P(0, 22) * _tmp70 + P(1, 22) * _tmp58 + P(2, 22) * _tmp62 + + P(23, 22) * _tmp49 + P(3, 22) * _tmp43 + P(4, 22) * _tmp71 + + P(5, 22) * _tmp50 + P(6, 22) * _tmp72 + _tmp74) + + _tmp70 * (P(0, 0) * _tmp70 + P(1, 0) * _tmp58 + P(2, 0) * _tmp62 + + P(22, 0) * _tmp68 + P(23, 0) * _tmp49 + P(3, 0) * _tmp43 + + P(4, 0) * _tmp71 + P(5, 0) * _tmp50 + P(6, 0) * _tmp72) + + _tmp71 * (P(0, 4) * _tmp70 + P(1, 4) * _tmp58 + P(2, 4) * _tmp62 + + P(22, 4) * _tmp68 + P(23, 4) * _tmp49 + P(3, 4) * _tmp43 + + P(4, 4) * _tmp71 + P(5, 4) * _tmp50 + P(6, 4) * _tmp72) + + _tmp72 * (P(0, 6) * _tmp70 + P(1, 6) * _tmp58 + P(2, 6) * _tmp62 + + P(22, 6) * _tmp68 + P(23, 6) * _tmp49 + P(3, 6) * _tmp43 + + P(4, 6) * _tmp71 + P(5, 6) * _tmp50 + P(6, 6) * _tmp72); + const Scalar _tmp76 = Scalar(1.0) / (math::max(_tmp75, epsilon)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = _tmp74; + _innov_var = _tmp75; } if (K != nullptr) { @@ -171,12 +168,12 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix& state, _k.setZero(); - _k(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 + - P(22, 23) * _tmp66 + P(22, 3) * _tmp70 + P(22, 4) * _tmp63 + - P(22, 5) * _tmp69 + P(22, 6) * _tmp51 + _tmp73); - _k(23, 0) = _tmp75 * (P(23, 0) * _tmp57 + P(23, 1) * _tmp67 + P(23, 2) * _tmp45 + - P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp63 + - P(23, 5) * _tmp69 + P(23, 6) * _tmp51 + _tmp72); + _k(22, 0) = _tmp76 * (P(22, 0) * _tmp70 + P(22, 1) * _tmp58 + P(22, 2) * _tmp62 + + P(22, 23) * _tmp49 + P(22, 3) * _tmp43 + P(22, 4) * _tmp71 + + P(22, 5) * _tmp50 + P(22, 6) * _tmp72 + _tmp74); + _k(23, 0) = _tmp76 * (P(23, 0) * _tmp70 + P(23, 1) * _tmp58 + P(23, 2) * _tmp62 + + P(23, 22) * _tmp68 + P(23, 3) * _tmp43 + P(23, 4) * _tmp71 + + P(23, 5) * _tmp50 + P(23, 6) * _tmp72 + _tmp73); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h index 8cbabba2f2..6eb4b77b42 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h @@ -34,130 +34,129 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix& state, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const K = nullptr) { - // Total ops: 407 + // Total ops: 397 // Input arrays // Intermediate terms (76) - const Scalar _tmp0 = -state(23, 0) + state(5, 0); - const Scalar _tmp1 = 2 * _tmp0; - const Scalar _tmp2 = _tmp1 * state(1, 0); - const Scalar _tmp3 = -state(22, 0) + state(4, 0); - const Scalar _tmp4 = 2 * _tmp3; - const Scalar _tmp5 = _tmp4 * state(2, 0); - const Scalar _tmp6 = 2 * state(6, 0); - const Scalar _tmp7 = _tmp6 * state(0, 0); - const Scalar _tmp8 = -_tmp2 + _tmp5 + _tmp7; - const Scalar _tmp9 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp10 = -_tmp9; - const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp13 = -_tmp12; - const Scalar _tmp14 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp15 = _tmp10 + _tmp11 + _tmp13 + _tmp14; - const Scalar _tmp16 = state(0, 0) * state(3, 0); - const Scalar _tmp17 = state(1, 0) * state(2, 0); - const Scalar _tmp18 = _tmp16 + _tmp17; - const Scalar _tmp19 = state(0, 0) * state(2, 0); - const Scalar _tmp20 = state(1, 0) * state(3, 0); - const Scalar _tmp21 = _tmp1 * _tmp18 + _tmp15 * _tmp3 + _tmp6 * (-_tmp19 + _tmp20); - const Scalar _tmp22 = state(2, 0) * state(3, 0); - const Scalar _tmp23 = state(0, 0) * state(1, 0); - const Scalar _tmp24 = _tmp22 - _tmp23; - const Scalar _tmp25 = _tmp19 + _tmp20; - const Scalar _tmp26 = _tmp11 - _tmp14; - const Scalar _tmp27 = _tmp13 + _tmp26 + _tmp9; - const Scalar _tmp28 = _tmp1 * _tmp24 + _tmp25 * _tmp4 + _tmp27 * state(6, 0); - const Scalar _tmp29 = _tmp10 + _tmp12 + _tmp26; - const Scalar _tmp30 = -_tmp16 + _tmp17; - const Scalar _tmp31 = _tmp0 * _tmp29 + _tmp30 * _tmp4 + _tmp6 * (_tmp22 + _tmp23); - const Scalar _tmp32 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp28, Scalar(2)) + - std::pow(_tmp31, Scalar(2)) + epsilon)); - const Scalar _tmp33 = cd * rho; - const Scalar _tmp34 = _tmp32 * _tmp33; - const Scalar _tmp35 = Scalar(0.5) * _tmp34; - const Scalar _tmp36 = _tmp4 * state(3, 0); - const Scalar _tmp37 = _tmp1 * state(0, 0); - const Scalar _tmp38 = _tmp6 * state(1, 0); - const Scalar _tmp39 = 2 * _tmp28; - const Scalar _tmp40 = 2 * _tmp31; - const Scalar _tmp41 = _tmp1 * state(2, 0) + _tmp4 * state(1, 0) + _tmp6 * state(3, 0); - const Scalar _tmp42 = 2 * _tmp21; - const Scalar _tmp43 = Scalar(0.25) * _tmp31 * _tmp33 / _tmp32; - const Scalar _tmp44 = - -_tmp35 * _tmp8 - - _tmp43 * (_tmp39 * (_tmp36 - _tmp37 - _tmp38) + _tmp40 * _tmp8 + _tmp41 * _tmp42) - - _tmp8 * cm; - const Scalar _tmp45 = -_tmp36 + _tmp37 + _tmp38; - const Scalar _tmp46 = _tmp4 * state(0, 0); - const Scalar _tmp47 = _tmp1 * state(3, 0); - const Scalar _tmp48 = _tmp6 * state(2, 0); - const Scalar _tmp49 = _tmp46 + _tmp47 - _tmp48; - const Scalar _tmp50 = -_tmp35 * _tmp45 - - _tmp43 * (_tmp39 * _tmp8 + _tmp40 * _tmp45 + _tmp42 * _tmp49) - _tmp45 * cm; - const Scalar _tmp51 = - -_tmp35 * _tmp41 - _tmp41 * cm - - _tmp43 * (_tmp39 * _tmp49 + _tmp40 * _tmp41 + _tmp42 * (_tmp2 - _tmp5 - _tmp7)); - const Scalar _tmp52 = -_tmp46 - _tmp47 + _tmp48; - const Scalar _tmp53 = -_tmp35 * _tmp52 - - _tmp43 * (_tmp39 * _tmp41 + _tmp40 * _tmp52 + _tmp42 * _tmp45) - - _tmp52 * cm; - const Scalar _tmp54 = _tmp29 * cm; - const Scalar _tmp55 = _tmp29 * _tmp35; - const Scalar _tmp56 = 4 * _tmp28; - const Scalar _tmp57 = _tmp29 * _tmp40; - const Scalar _tmp58 = - -_tmp43 * (-4 * _tmp18 * _tmp21 - _tmp24 * _tmp56 - _tmp57) + _tmp54 + _tmp55; - const Scalar _tmp59 = 2 * _tmp16; - const Scalar _tmp60 = 2 * _tmp17; - const Scalar _tmp61 = -_tmp59 + _tmp60; - const Scalar _tmp62 = _tmp15 * _tmp42; - const Scalar _tmp63 = 2 * _tmp19; - const Scalar _tmp64 = 2 * _tmp20; - const Scalar _tmp65 = -_tmp35 * _tmp61 - - _tmp43 * (_tmp39 * (_tmp63 + _tmp64) + _tmp40 * _tmp61 + _tmp62) - - _tmp61 * cm; - const Scalar _tmp66 = Scalar(1.0) * _tmp30 * _tmp34 + 2 * _tmp30 * cm - - _tmp43 * (-_tmp25 * _tmp56 - 4 * _tmp30 * _tmp31 - _tmp62); - const Scalar _tmp67 = 2 * _tmp22; - const Scalar _tmp68 = 2 * _tmp23; - const Scalar _tmp69 = _tmp67 + _tmp68; - const Scalar _tmp70 = -_tmp35 * _tmp69 - - _tmp43 * (_tmp27 * _tmp39 + _tmp40 * _tmp69 + _tmp42 * (-_tmp63 + _tmp64)) - + 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 = _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(2, 0); + const Scalar _tmp17 = _tmp16 * state(3, 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 = -_tmp25 * _tmp29 - _tmp25 * cm - + _tmp37 * (_tmp14 * _tmp31 + _tmp22 * _tmp33 + _tmp25 * _tmp35); + const Scalar _tmp40 = 2 * state(3, 0); + const Scalar _tmp41 = _tmp40 * _tmp9; + const Scalar _tmp42 = _tmp2 * state(6, 0); + const Scalar _tmp43 = -_tmp41 + _tmp42; + const Scalar _tmp44 = _tmp11 * _tmp2; + const Scalar _tmp45 = _tmp16 * _tmp9; + const Scalar _tmp46 = _tmp11 * _tmp40; + const Scalar _tmp47 = _tmp16 * state(6, 0); + const Scalar _tmp48 = + -_tmp29 * _tmp43 - + _tmp37 * (_tmp31 * (_tmp46 - _tmp47) + _tmp33 * (-_tmp44 + _tmp45) + _tmp35 * _tmp43) - + _tmp43 * cm; + const Scalar _tmp49 = _tmp2 * _tmp9; + const Scalar _tmp50 = _tmp40 * state(6, 0); + const Scalar _tmp51 = _tmp49 + _tmp50; + const Scalar _tmp52 = _tmp0 * _tmp9; + const Scalar _tmp53 = 4 * state(6, 0); + const Scalar _tmp54 = 4 * _tmp9; + const Scalar _tmp55 = _tmp0 * state(6, 0); + const Scalar _tmp56 = + -_tmp29 * _tmp51 - + _tmp37 * (_tmp31 * (_tmp44 - _tmp54 * state(2, 0) - _tmp55) + + _tmp33 * (_tmp46 + _tmp52 - _tmp53 * state(2, 0)) + _tmp35 * _tmp51) - + _tmp51 * cm; + const Scalar _tmp57 = _tmp24 * cm; + const Scalar _tmp58 = _tmp10 * _tmp31; + const Scalar _tmp59 = _tmp19 * _tmp33; + const Scalar _tmp60 = _tmp24 * _tmp35; + const Scalar _tmp61 = _tmp24 * _tmp29; + const Scalar _tmp62 = -_tmp37 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61; + const Scalar _tmp63 = _tmp0 * _tmp11; + const Scalar _tmp64 = _tmp11 * _tmp16; + const Scalar _tmp65 = 4 * _tmp11; + const Scalar _tmp66 = _tmp45 + _tmp55 - _tmp65 * state(1, 0); + const Scalar _tmp67 = + -_tmp29 * _tmp66 - + _tmp37 * (_tmp31 * (_tmp50 + _tmp64) + _tmp33 * (_tmp41 - _tmp53 * state(1, 0) - _tmp63) + + _tmp35 * _tmp66) - + _tmp66 * cm; + const Scalar _tmp68 = -_tmp37 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61; + const Scalar _tmp69 = _tmp47 - _tmp52 - _tmp65 * state(3, 0); + const Scalar _tmp70 = -_tmp29 * _tmp69 - + _tmp37 * (_tmp31 * (_tmp42 - _tmp54 * state(3, 0) + _tmp63) + + _tmp33 * (_tmp49 + _tmp64) + _tmp35 * _tmp69) - _tmp69 * cm; - const Scalar _tmp71 = - -_tmp43 * (_tmp39 * (_tmp67 - _tmp68) + _tmp42 * (_tmp59 + _tmp60) + _tmp57) - _tmp54 - - _tmp55; - const Scalar _tmp72 = P(22, 22) * _tmp66; - const Scalar _tmp73 = P(23, 23) * _tmp58; + const Scalar _tmp71 = _tmp30 - _tmp37 * (-_tmp32 - _tmp34 - _tmp36) + _tmp5; + const Scalar _tmp72 = P(22, 22) * _tmp71; + const Scalar _tmp73 = P(23, 23) * _tmp62; const Scalar _tmp74 = R + - _tmp44 * (P(0, 1) * _tmp50 + P(1, 1) * _tmp44 + P(2, 1) * _tmp51 + - P(22, 1) * _tmp66 + P(23, 1) * _tmp58 + P(3, 1) * _tmp53 + - P(4, 1) * _tmp65 + P(5, 1) * _tmp71 + P(6, 1) * _tmp70) + - _tmp50 * (P(0, 0) * _tmp50 + P(1, 0) * _tmp44 + P(2, 0) * _tmp51 + - P(22, 0) * _tmp66 + P(23, 0) * _tmp58 + P(3, 0) * _tmp53 + - P(4, 0) * _tmp65 + P(5, 0) * _tmp71 + P(6, 0) * _tmp70) + - _tmp51 * (P(0, 2) * _tmp50 + P(1, 2) * _tmp44 + P(2, 2) * _tmp51 + - P(22, 2) * _tmp66 + P(23, 2) * _tmp58 + P(3, 2) * _tmp53 + - P(4, 2) * _tmp65 + P(5, 2) * _tmp71 + P(6, 2) * _tmp70) + - _tmp53 * (P(0, 3) * _tmp50 + P(1, 3) * _tmp44 + P(2, 3) * _tmp51 + - P(22, 3) * _tmp66 + P(23, 3) * _tmp58 + P(3, 3) * _tmp53 + - P(4, 3) * _tmp65 + P(5, 3) * _tmp71 + P(6, 3) * _tmp70) + - _tmp58 * (P(0, 23) * _tmp50 + P(1, 23) * _tmp44 + P(2, 23) * _tmp51 + - P(22, 23) * _tmp66 + P(3, 23) * _tmp53 + P(4, 23) * _tmp65 + - P(5, 23) * _tmp71 + P(6, 23) * _tmp70 + _tmp73) + - _tmp65 * (P(0, 4) * _tmp50 + P(1, 4) * _tmp44 + P(2, 4) * _tmp51 + - P(22, 4) * _tmp66 + P(23, 4) * _tmp58 + P(3, 4) * _tmp53 + - P(4, 4) * _tmp65 + P(5, 4) * _tmp71 + P(6, 4) * _tmp70) + - _tmp66 * (P(0, 22) * _tmp50 + P(1, 22) * _tmp44 + P(2, 22) * _tmp51 + - P(23, 22) * _tmp58 + P(3, 22) * _tmp53 + P(4, 22) * _tmp65 + - P(5, 22) * _tmp71 + P(6, 22) * _tmp70 + _tmp72) + - _tmp70 * (P(0, 6) * _tmp50 + P(1, 6) * _tmp44 + P(2, 6) * _tmp51 + - P(22, 6) * _tmp66 + P(23, 6) * _tmp58 + P(3, 6) * _tmp53 + - P(4, 6) * _tmp65 + P(5, 6) * _tmp71 + P(6, 6) * _tmp70) + - _tmp71 * (P(0, 5) * _tmp50 + P(1, 5) * _tmp44 + P(2, 5) * _tmp51 + - P(22, 5) * _tmp66 + P(23, 5) * _tmp58 + P(3, 5) * _tmp53 + - P(4, 5) * _tmp65 + P(5, 5) * _tmp71 + P(6, 5) * _tmp70); + _tmp38 * (P(0, 4) * _tmp48 + P(1, 4) * _tmp67 + P(2, 4) * _tmp56 + + P(22, 4) * _tmp71 + P(23, 4) * _tmp62 + P(3, 4) * _tmp70 + + P(4, 4) * _tmp38 + P(5, 4) * _tmp68 + P(6, 4) * _tmp39) + + _tmp39 * (P(0, 6) * _tmp48 + P(1, 6) * _tmp67 + P(2, 6) * _tmp56 + + P(22, 6) * _tmp71 + P(23, 6) * _tmp62 + P(3, 6) * _tmp70 + + P(4, 6) * _tmp38 + P(5, 6) * _tmp68 + P(6, 6) * _tmp39) + + _tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp67 + P(2, 0) * _tmp56 + + P(22, 0) * _tmp71 + P(23, 0) * _tmp62 + P(3, 0) * _tmp70 + + P(4, 0) * _tmp38 + P(5, 0) * _tmp68 + P(6, 0) * _tmp39) + + _tmp56 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp67 + P(2, 2) * _tmp56 + + P(22, 2) * _tmp71 + P(23, 2) * _tmp62 + P(3, 2) * _tmp70 + + P(4, 2) * _tmp38 + P(5, 2) * _tmp68 + P(6, 2) * _tmp39) + + _tmp62 * (P(0, 23) * _tmp48 + P(1, 23) * _tmp67 + P(2, 23) * _tmp56 + + P(22, 23) * _tmp71 + P(3, 23) * _tmp70 + P(4, 23) * _tmp38 + + P(5, 23) * _tmp68 + P(6, 23) * _tmp39 + _tmp73) + + _tmp67 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp67 + P(2, 1) * _tmp56 + + P(22, 1) * _tmp71 + P(23, 1) * _tmp62 + P(3, 1) * _tmp70 + + P(4, 1) * _tmp38 + P(5, 1) * _tmp68 + P(6, 1) * _tmp39) + + _tmp68 * (P(0, 5) * _tmp48 + P(1, 5) * _tmp67 + P(2, 5) * _tmp56 + + P(22, 5) * _tmp71 + P(23, 5) * _tmp62 + P(3, 5) * _tmp70 + + P(4, 5) * _tmp38 + P(5, 5) * _tmp68 + P(6, 5) * _tmp39) + + _tmp70 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp67 + P(2, 3) * _tmp56 + + P(22, 3) * _tmp71 + P(23, 3) * _tmp62 + P(3, 3) * _tmp70 + + P(4, 3) * _tmp38 + P(5, 3) * _tmp68 + P(6, 3) * _tmp39) + + _tmp71 * (P(0, 22) * _tmp48 + P(1, 22) * _tmp67 + P(2, 22) * _tmp56 + + P(23, 22) * _tmp62 + P(3, 22) * _tmp70 + P(4, 22) * _tmp38 + + P(5, 22) * _tmp68 + P(6, 22) * _tmp39 + _tmp72); const Scalar _tmp75 = Scalar(1.0) / (math::max(_tmp74, epsilon)); // Output terms (2) @@ -172,12 +171,12 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix& state, _k.setZero(); - _k(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 + - P(22, 23) * _tmp58 + P(22, 3) * _tmp53 + P(22, 4) * _tmp65 + - P(22, 5) * _tmp71 + P(22, 6) * _tmp70 + _tmp72); - _k(23, 0) = _tmp75 * (P(23, 0) * _tmp50 + P(23, 1) * _tmp44 + P(23, 2) * _tmp51 + - P(23, 22) * _tmp66 + P(23, 3) * _tmp53 + P(23, 4) * _tmp65 + - P(23, 5) * _tmp71 + P(23, 6) * _tmp70 + _tmp73); + _k(22, 0) = _tmp75 * (P(22, 0) * _tmp48 + P(22, 1) * _tmp67 + P(22, 2) * _tmp56 + + P(22, 23) * _tmp62 + P(22, 3) * _tmp70 + P(22, 4) * _tmp38 + + P(22, 5) * _tmp68 + P(22, 6) * _tmp39 + _tmp72); + _k(23, 0) = _tmp75 * (P(23, 0) * _tmp48 + P(23, 1) * _tmp67 + P(23, 2) * _tmp56 + + P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp38 + + P(23, 5) * _tmp68 + P(23, 6) * _tmp39 + _tmp73); } } // 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 6ef629a65a..964b7751ac 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,43 +32,41 @@ 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: 285 + // Total ops: 291 // Input arrays - // Intermediate terms (29) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp3 = + // Intermediate terms (28) + const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = Scalar(1.0) / (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); - const Scalar _tmp4 = _tmp3 * (_tmp0 - _tmp1 + _tmp2); - const Scalar _tmp5 = 2 * state(3, 0); - const Scalar _tmp6 = _tmp5 * state(0, 0); - const Scalar _tmp7 = 2 * state(2, 0); - const Scalar _tmp8 = _tmp7 * state(1, 0); - const Scalar _tmp9 = _tmp3 * (-_tmp6 + _tmp8); - const Scalar _tmp10 = 2 * state(4, 0); - const Scalar _tmp11 = _tmp10 * state(0, 0); - const Scalar _tmp12 = 2 * state(5, 0); - const Scalar _tmp13 = _tmp12 * state(3, 0); - const Scalar _tmp14 = _tmp7 * state(6, 0); - const Scalar _tmp15 = _tmp3 * (-_tmp11 - _tmp13 + _tmp14); - const Scalar _tmp16 = 2 * state(1, 0); - const Scalar _tmp17 = - _tmp3 * (-_tmp10 * state(3, 0) + _tmp12 * state(0, 0) + _tmp16 * state(6, 0)); - const Scalar _tmp18 = _tmp7 * state(4, 0); - const Scalar _tmp19 = _tmp12 * state(1, 0); - const Scalar _tmp20 = 2 * state(0, 0) * state(6, 0); - const Scalar _tmp21 = _tmp3 * (_tmp18 - _tmp19 + _tmp20); - const Scalar _tmp22 = _tmp3 * (_tmp10 * state(1, 0) + _tmp5 * state(6, 0) + _tmp7 * state(5, 0)); - const Scalar _tmp23 = _tmp3 * (_tmp16 * state(0, 0) + _tmp7 * state(3, 0)); - const Scalar _tmp24 = _tmp3 * (-_tmp0 + _tmp1 + _tmp2); - const Scalar _tmp25 = _tmp3 * (_tmp6 + _tmp8); - const Scalar _tmp26 = _tmp3 * (_tmp16 * state(3, 0) - _tmp7 * state(0, 0)); - const Scalar _tmp27 = _tmp3 * (_tmp11 + _tmp13 - _tmp14); - const Scalar _tmp28 = _tmp3 * (-_tmp18 + _tmp19 - _tmp20); + const Scalar _tmp2 = _tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2))); + const Scalar _tmp3 = 2 * state(1, 0); + const Scalar _tmp4 = 2 * state(3, 0); + const Scalar _tmp5 = _tmp4 * state(6, 0); + const Scalar _tmp6 = _tmp1 * (_tmp3 * state(4, 0) + _tmp5); + const Scalar _tmp7 = _tmp3 * state(6, 0); + const Scalar _tmp8 = _tmp1 * (-_tmp4 * state(4, 0) + _tmp7); + const Scalar _tmp9 = _tmp4 * state(0, 0); + const Scalar _tmp10 = _tmp3 * state(2, 0); + const Scalar _tmp11 = _tmp1 * (_tmp10 - _tmp9); + const Scalar _tmp12 = 2 * state(0, 0); + const Scalar _tmp13 = 4 * state(5, 0); + const Scalar _tmp14 = 2 * state(2, 0); + const Scalar _tmp15 = _tmp14 * state(6, 0); + const Scalar _tmp16 = _tmp1 * (-_tmp12 * state(4, 0) - _tmp13 * state(3, 0) + _tmp15); + const Scalar _tmp17 = _tmp12 * state(6, 0); + const Scalar _tmp18 = _tmp1 * (-_tmp13 * state(1, 0) + _tmp14 * state(4, 0) + _tmp17); + const Scalar _tmp19 = _tmp1 * (_tmp12 * state(1, 0) + _tmp4 * state(2, 0)); + const Scalar _tmp20 = _tmp1 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2))); + const Scalar _tmp21 = _tmp1 * (_tmp10 + _tmp9); + const Scalar _tmp22 = _tmp1 * (-_tmp12 * state(2, 0) + _tmp4 * state(1, 0)); + const Scalar _tmp23 = 4 * state(4, 0); + const Scalar _tmp24 = _tmp1 * (_tmp12 * state(5, 0) - _tmp23 * state(3, 0) + _tmp7); + const Scalar _tmp25 = _tmp1 * (-_tmp17 - _tmp23 * state(2, 0) + _tmp3 * state(5, 0)); + const Scalar _tmp26 = _tmp1 * (-_tmp15 + _tmp4 * state(5, 0)); + const Scalar _tmp27 = _tmp1 * (_tmp14 * state(5, 0) + _tmp5); // Output terms (2) if (innov_var != nullptr) { @@ -76,36 +74,36 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, _innov_var(0, 0) = R + - _tmp15 * (P(0, 3) * _tmp17 + P(1, 3) * _tmp21 + P(2, 3) * _tmp22 + P(3, 3) * _tmp15 + - P(4, 3) * _tmp9 + P(5, 3) * _tmp4 + P(6, 3) * _tmp23) + - _tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp21 + P(2, 0) * _tmp22 + P(3, 0) * _tmp15 + - P(4, 0) * _tmp9 + P(5, 0) * _tmp4 + P(6, 0) * _tmp23) + - _tmp21 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp21 + P(2, 1) * _tmp22 + P(3, 1) * _tmp15 + - P(4, 1) * _tmp9 + P(5, 1) * _tmp4 + P(6, 1) * _tmp23) + - _tmp22 * (P(0, 2) * _tmp17 + P(1, 2) * _tmp21 + P(2, 2) * _tmp22 + P(3, 2) * _tmp15 + - P(4, 2) * _tmp9 + P(5, 2) * _tmp4 + P(6, 2) * _tmp23) + - _tmp23 * (P(0, 6) * _tmp17 + P(1, 6) * _tmp21 + P(2, 6) * _tmp22 + P(3, 6) * _tmp15 + - P(4, 6) * _tmp9 + P(5, 6) * _tmp4 + P(6, 6) * _tmp23) + - _tmp4 * (P(0, 5) * _tmp17 + P(1, 5) * _tmp21 + P(2, 5) * _tmp22 + P(3, 5) * _tmp15 + - P(4, 5) * _tmp9 + P(5, 5) * _tmp4 + P(6, 5) * _tmp23) + - _tmp9 * (P(0, 4) * _tmp17 + P(1, 4) * _tmp21 + P(2, 4) * _tmp22 + P(3, 4) * _tmp15 + - P(4, 4) * _tmp9 + P(5, 4) * _tmp4 + P(6, 4) * _tmp23); + _tmp11 * (P(0, 4) * _tmp8 + P(1, 4) * _tmp18 + P(2, 4) * _tmp6 + P(3, 4) * _tmp16 + + P(4, 4) * _tmp11 + P(5, 4) * _tmp2 + P(6, 4) * _tmp19) + + _tmp16 * (P(0, 3) * _tmp8 + P(1, 3) * _tmp18 + P(2, 3) * _tmp6 + P(3, 3) * _tmp16 + + P(4, 3) * _tmp11 + P(5, 3) * _tmp2 + P(6, 3) * _tmp19) + + _tmp18 * (P(0, 1) * _tmp8 + P(1, 1) * _tmp18 + P(2, 1) * _tmp6 + P(3, 1) * _tmp16 + + P(4, 1) * _tmp11 + P(5, 1) * _tmp2 + P(6, 1) * _tmp19) + + _tmp19 * (P(0, 6) * _tmp8 + P(1, 6) * _tmp18 + P(2, 6) * _tmp6 + P(3, 6) * _tmp16 + + P(4, 6) * _tmp11 + P(5, 6) * _tmp2 + P(6, 6) * _tmp19) + + _tmp2 * (P(0, 5) * _tmp8 + P(1, 5) * _tmp18 + P(2, 5) * _tmp6 + P(3, 5) * _tmp16 + + P(4, 5) * _tmp11 + P(5, 5) * _tmp2 + P(6, 5) * _tmp19) + + _tmp6 * (P(0, 2) * _tmp8 + P(1, 2) * _tmp18 + P(2, 2) * _tmp6 + P(3, 2) * _tmp16 + + P(4, 2) * _tmp11 + P(5, 2) * _tmp2 + P(6, 2) * _tmp19) + + _tmp8 * (P(0, 0) * _tmp8 + P(1, 0) * _tmp18 + P(2, 0) * _tmp6 + P(3, 0) * _tmp16 + + P(4, 0) * _tmp11 + P(5, 0) * _tmp2 + P(6, 0) * _tmp19); _innov_var(1, 0) = R - - _tmp17 * (-P(0, 3) * _tmp27 - P(1, 3) * _tmp22 - P(2, 3) * _tmp28 - P(3, 3) * _tmp17 - - P(4, 3) * _tmp24 - P(5, 3) * _tmp25 - P(6, 3) * _tmp26) - - _tmp22 * (-P(0, 1) * _tmp27 - P(1, 1) * _tmp22 - P(2, 1) * _tmp28 - P(3, 1) * _tmp17 - - P(4, 1) * _tmp24 - P(5, 1) * _tmp25 - P(6, 1) * _tmp26) - - _tmp24 * (-P(0, 4) * _tmp27 - P(1, 4) * _tmp22 - P(2, 4) * _tmp28 - P(3, 4) * _tmp17 - - P(4, 4) * _tmp24 - P(5, 4) * _tmp25 - P(6, 4) * _tmp26) - - _tmp25 * (-P(0, 5) * _tmp27 - P(1, 5) * _tmp22 - P(2, 5) * _tmp28 - P(3, 5) * _tmp17 - - P(4, 5) * _tmp24 - P(5, 5) * _tmp25 - P(6, 5) * _tmp26) - - _tmp26 * (-P(0, 6) * _tmp27 - P(1, 6) * _tmp22 - P(2, 6) * _tmp28 - P(3, 6) * _tmp17 - - P(4, 6) * _tmp24 - P(5, 6) * _tmp25 - P(6, 6) * _tmp26) - - _tmp27 * (-P(0, 0) * _tmp27 - P(1, 0) * _tmp22 - P(2, 0) * _tmp28 - P(3, 0) * _tmp17 - - P(4, 0) * _tmp24 - P(5, 0) * _tmp25 - P(6, 0) * _tmp26) - - _tmp28 * (-P(0, 2) * _tmp27 - P(1, 2) * _tmp22 - P(2, 2) * _tmp28 - P(3, 2) * _tmp17 - - P(4, 2) * _tmp24 - P(5, 2) * _tmp25 - P(6, 2) * _tmp26); + _tmp20 * (-P(0, 4) * _tmp26 - P(1, 4) * _tmp27 - P(2, 4) * _tmp25 - P(3, 4) * _tmp24 - + P(4, 4) * _tmp20 - P(5, 4) * _tmp21 - P(6, 4) * _tmp22) - + _tmp21 * (-P(0, 5) * _tmp26 - P(1, 5) * _tmp27 - P(2, 5) * _tmp25 - P(3, 5) * _tmp24 - + P(4, 5) * _tmp20 - P(5, 5) * _tmp21 - P(6, 5) * _tmp22) - + _tmp22 * (-P(0, 6) * _tmp26 - P(1, 6) * _tmp27 - P(2, 6) * _tmp25 - P(3, 6) * _tmp24 - + P(4, 6) * _tmp20 - P(5, 6) * _tmp21 - P(6, 6) * _tmp22) - + _tmp24 * (-P(0, 3) * _tmp26 - P(1, 3) * _tmp27 - P(2, 3) * _tmp25 - P(3, 3) * _tmp24 - + P(4, 3) * _tmp20 - P(5, 3) * _tmp21 - P(6, 3) * _tmp22) - + _tmp25 * (-P(0, 2) * _tmp26 - P(1, 2) * _tmp27 - P(2, 2) * _tmp25 - P(3, 2) * _tmp24 - + P(4, 2) * _tmp20 - P(5, 2) * _tmp21 - P(6, 2) * _tmp22) - + _tmp26 * (-P(0, 0) * _tmp26 - P(1, 0) * _tmp27 - P(2, 0) * _tmp25 - P(3, 0) * _tmp24 - + P(4, 0) * _tmp20 - P(5, 0) * _tmp21 - P(6, 0) * _tmp22) - + _tmp27 * (-P(0, 1) * _tmp26 - P(1, 1) * _tmp27 - P(2, 1) * _tmp25 - P(3, 1) * _tmp24 - + P(4, 1) * _tmp20 - P(5, 1) * _tmp21 - P(6, 1) * _tmp22); } if (H != nullptr) { @@ -113,13 +111,13 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp17; - _h(1, 0) = _tmp21; - _h(2, 0) = _tmp22; - _h(3, 0) = _tmp15; - _h(4, 0) = _tmp9; - _h(5, 0) = _tmp4; - _h(6, 0) = _tmp23; + _h(0, 0) = _tmp8; + _h(1, 0) = _tmp18; + _h(2, 0) = _tmp6; + _h(3, 0) = _tmp16; + _h(4, 0) = _tmp11; + _h(5, 0) = _tmp2; + _h(6, 0) = _tmp19; } } // 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 c8ffe911b4..fa96926e2d 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,7 +32,7 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 171 + // Total ops: 166 // Input arrays @@ -41,39 +41,38 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, Scalar(1.0) / (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); const Scalar _tmp1 = - _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 _tmp2 = 2 * state(0, 0); - const Scalar _tmp3 = 2 * state(1, 0); - const Scalar _tmp4 = _tmp0 * (_tmp2 * state(3, 0) + _tmp3 * state(2, 0)); - const Scalar _tmp5 = _tmp0 * (-_tmp2 * state(2, 0) + _tmp3 * state(3, 0)); - const Scalar _tmp6 = 2 * state(4, 0); - const Scalar _tmp7 = 2 * state(6, 0); - const Scalar _tmp8 = _tmp0 * (_tmp2 * state(5, 0) - _tmp6 * state(3, 0) + _tmp7 * state(1, 0)); - const Scalar _tmp9 = 2 * state(5, 0); - const Scalar _tmp10 = _tmp0 * (_tmp2 * state(4, 0) - _tmp7 * state(2, 0) + _tmp9 * state(3, 0)); - const Scalar _tmp11 = _tmp0 * (_tmp3 * state(4, 0) + _tmp7 * state(3, 0) + _tmp9 * state(2, 0)); - const Scalar _tmp12 = _tmp0 * (_tmp3 * state(5, 0) - _tmp6 * state(2, 0) - _tmp7 * state(0, 0)); + _tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1); + const Scalar _tmp2 = 2 * state(3, 0); + const Scalar _tmp3 = 2 * state(2, 0); + const Scalar _tmp4 = _tmp0 * (_tmp2 * state(0, 0) + _tmp3 * state(1, 0)); + const Scalar _tmp5 = _tmp0 * (_tmp2 * state(1, 0) - _tmp3 * state(0, 0)); + const Scalar _tmp6 = 4 * state(4, 0); + const Scalar _tmp7 = 2 * state(5, 0); + const Scalar _tmp8 = 2 * state(6, 0); + const Scalar _tmp9 = _tmp0 * (-_tmp6 * state(3, 0) + _tmp7 * state(0, 0) + _tmp8 * state(1, 0)); + const Scalar _tmp10 = _tmp0 * (-_tmp6 * state(2, 0) + _tmp7 * state(1, 0) - _tmp8 * state(0, 0)); + const Scalar _tmp11 = _tmp0 * (-_tmp3 * state(6, 0) + _tmp7 * state(3, 0)); + const Scalar _tmp12 = _tmp0 * (_tmp2 * state(6, 0) + _tmp3 * state(5, 0)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); _innov_var = R - - _tmp1 * (-P(0, 4) * _tmp10 - P(1, 4) * _tmp11 - P(2, 4) * _tmp12 - - P(3, 4) * _tmp8 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) - - _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp11 - P(2, 0) * _tmp12 - - P(3, 0) * _tmp8 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) - - _tmp11 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp11 - P(2, 1) * _tmp12 - - P(3, 1) * _tmp8 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) - - _tmp12 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp11 - P(2, 2) * _tmp12 - - P(3, 2) * _tmp8 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) - - _tmp4 * (-P(0, 5) * _tmp10 - P(1, 5) * _tmp11 - P(2, 5) * _tmp12 - - P(3, 5) * _tmp8 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) - - _tmp5 * (-P(0, 6) * _tmp10 - P(1, 6) * _tmp11 - P(2, 6) * _tmp12 - - P(3, 6) * _tmp8 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) - - _tmp8 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp11 - P(2, 3) * _tmp12 - - P(3, 3) * _tmp8 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5); + _tmp1 * (-P(0, 4) * _tmp11 - P(1, 4) * _tmp12 - P(2, 4) * _tmp10 - + P(3, 4) * _tmp9 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) - + _tmp10 * (-P(0, 2) * _tmp11 - P(1, 2) * _tmp12 - P(2, 2) * _tmp10 - + P(3, 2) * _tmp9 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) - + _tmp11 * (-P(0, 0) * _tmp11 - P(1, 0) * _tmp12 - P(2, 0) * _tmp10 - + P(3, 0) * _tmp9 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) - + _tmp12 * (-P(0, 1) * _tmp11 - P(1, 1) * _tmp12 - P(2, 1) * _tmp10 - + P(3, 1) * _tmp9 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) - + _tmp4 * (-P(0, 5) * _tmp11 - P(1, 5) * _tmp12 - P(2, 5) * _tmp10 - + P(3, 5) * _tmp9 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) - + _tmp5 * (-P(0, 6) * _tmp11 - P(1, 6) * _tmp12 - P(2, 6) * _tmp10 - + P(3, 6) * _tmp9 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) - + _tmp9 * (-P(0, 3) * _tmp11 - P(1, 3) * _tmp12 - P(2, 3) * _tmp10 - + P(3, 3) * _tmp9 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5); } if (H != nullptr) { @@ -81,10 +80,10 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = -_tmp10; - _h(1, 0) = -_tmp11; - _h(2, 0) = -_tmp12; - _h(3, 0) = -_tmp8; + _h(0, 0) = -_tmp11; + _h(1, 0) = -_tmp12; + _h(2, 0) = -_tmp10; + _h(3, 0) = -_tmp9; _h(4, 0) = -_tmp1; _h(5, 0) = -_tmp4; _h(6, 0) = -_tmp5; 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 9fd7866e54..286312543b 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,53 +34,54 @@ 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: 101 + // Total ops: 105 // Input arrays - // Intermediate terms (26) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp3 = std::sin(antenna_yaw_offset); - const Scalar _tmp4 = state(0, 0) * state(3, 0); - const Scalar _tmp5 = state(1, 0) * state(2, 0); - const Scalar _tmp6 = std::cos(antenna_yaw_offset); - const Scalar _tmp7 = 2 * _tmp6; - const Scalar _tmp8 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + _tmp7 * (_tmp4 + _tmp5); - const Scalar _tmp9 = 2 * _tmp3; - const Scalar _tmp10 = _tmp6 * (-_tmp0 + _tmp1 + _tmp2) + _tmp9 * (-_tmp4 + _tmp5); - const Scalar _tmp11 = _tmp10 + epsilon * ((((_tmp10) > 0) - ((_tmp10) < 0)) + Scalar(0.5)); - const Scalar _tmp12 = _tmp7 * state(0, 0) - _tmp9 * state(3, 0); - const Scalar _tmp13 = Scalar(1.0) / (_tmp11); - const Scalar _tmp14 = _tmp7 * state(3, 0); - const Scalar _tmp15 = _tmp9 * state(0, 0); - const Scalar _tmp16 = std::pow(_tmp11, Scalar(2)); - const Scalar _tmp17 = _tmp8 / _tmp16; - const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp8, Scalar(2))); - const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15)); - const Scalar _tmp20 = _tmp7 * state(1, 0) + _tmp9 * state(2, 0); - const Scalar _tmp21 = _tmp9 * state(1, 0); - const Scalar _tmp22 = _tmp7 * state(2, 0); - const Scalar _tmp23 = _tmp18 * (_tmp13 * (-_tmp21 + _tmp22) - _tmp17 * _tmp20); - const Scalar _tmp24 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15)); - const Scalar _tmp25 = _tmp18 * (_tmp13 * _tmp20 - _tmp17 * (_tmp21 - _tmp22)); + // Intermediate terms (22) + 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 = 4 * _tmp1; + const Scalar _tmp9 = 2 * _tmp4; + const Scalar _tmp10 = Scalar(1.0) / (_tmp7); + const Scalar _tmp11 = 4 * _tmp4; + const Scalar _tmp12 = 2 * _tmp1; + const Scalar _tmp13 = std::pow(_tmp7, Scalar(2)); + const Scalar _tmp14 = _tmp5 / _tmp13; + const Scalar _tmp15 = _tmp13 / (_tmp13 + std::pow(_tmp5, Scalar(2))); + const Scalar _tmp16 = _tmp15 * (_tmp10 * (-_tmp8 * state(3, 0) + _tmp9 * state(0, 0)) - + _tmp14 * (-_tmp11 * state(3, 0) - _tmp12 * state(0, 0))); + const Scalar _tmp17 = _tmp12 * _tmp14; + const Scalar _tmp18 = _tmp10 * _tmp9; + const Scalar _tmp19 = _tmp15 * (_tmp17 * state(3, 0) + _tmp18 * state(3, 0)); + const Scalar _tmp20 = + _tmp15 * (-_tmp14 * (-_tmp11 * state(2, 0) + _tmp12 * state(1, 0)) + _tmp18 * state(1, 0)); + const Scalar _tmp21 = + _tmp15 * (_tmp10 * (-_tmp8 * state(1, 0) + _tmp9 * state(2, 0)) - _tmp17 * state(2, 0)); // Output terms (3) if (meas_pred != nullptr) { Scalar& _meas_pred = (*meas_pred); - _meas_pred = std::atan2(_tmp8, _tmp11); + _meas_pred = std::atan2(_tmp5, _tmp7); } if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); _innov_var = - R + _tmp19 * (P(0, 3) * _tmp24 + P(1, 3) * _tmp23 + P(2, 3) * _tmp25 + P(3, 3) * _tmp19) + - _tmp23 * (P(0, 1) * _tmp24 + P(1, 1) * _tmp23 + P(2, 1) * _tmp25 + P(3, 1) * _tmp19) + - _tmp24 * (P(0, 0) * _tmp24 + P(1, 0) * _tmp23 + P(2, 0) * _tmp25 + P(3, 0) * _tmp19) + - _tmp25 * (P(0, 2) * _tmp24 + P(1, 2) * _tmp23 + P(2, 2) * _tmp25 + P(3, 2) * _tmp19); + R + _tmp16 * (P(0, 3) * _tmp19 + P(1, 3) * _tmp21 + P(2, 3) * _tmp20 + P(3, 3) * _tmp16) + + _tmp19 * (P(0, 0) * _tmp19 + P(1, 0) * _tmp21 + P(2, 0) * _tmp20 + P(3, 0) * _tmp16) + + _tmp20 * (P(0, 2) * _tmp19 + P(1, 2) * _tmp21 + P(2, 2) * _tmp20 + P(3, 2) * _tmp16) + + _tmp21 * (P(0, 1) * _tmp19 + P(1, 1) * _tmp21 + P(2, 1) * _tmp20 + P(3, 1) * _tmp16); } if (H != nullptr) { @@ -88,10 +89,10 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp24; - _h(1, 0) = _tmp23; - _h(2, 0) = _tmp25; - _h(3, 0) = _tmp19; + _h(0, 0) = _tmp19; + _h(1, 0) = _tmp21; + _h(2, 0) = _tmp20; + _h(3, 0) = _tmp16; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h index 5ced3fdb16..a64929958a 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h @@ -39,236 +39,192 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, matrix::Matrix* const Kx = nullptr, matrix::Matrix* const Ky = nullptr, matrix::Matrix* const Kz = nullptr) { - // Total ops: 736 + // Total ops: 617 // Input arrays - // Intermediate terms (54) - const Scalar _tmp0 = + // Intermediate terms (34) + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = Scalar(9.8066499999999994) / std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) + std::pow(meas(2, 0), Scalar(2)))); - const Scalar _tmp1 = Scalar(19.613299999999999) * state(1, 0); - const Scalar _tmp2 = -P(3, 0) * _tmp1; - const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0); - const Scalar _tmp4 = P(0, 0) * _tmp3; - const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0); - const Scalar _tmp6 = P(2, 0) * _tmp5; - const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0); - const Scalar _tmp8 = P(3, 1) * _tmp1; - const Scalar _tmp9 = P(2, 1) * _tmp5; - const Scalar _tmp10 = -P(1, 1) * _tmp7; - const Scalar _tmp11 = P(0, 2) * _tmp3; - const Scalar _tmp12 = P(2, 2) * _tmp5; - const Scalar _tmp13 = -P(1, 2) * _tmp7; - const Scalar _tmp14 = -P(3, 3) * _tmp1; - const Scalar _tmp15 = P(0, 3) * _tmp3; - const Scalar _tmp16 = -P(1, 3) * _tmp7; - const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) + - _tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) + - _tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) - - _tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9); - const Scalar _tmp18 = P(3, 0) * _tmp3; - const Scalar _tmp19 = -P(0, 0) * _tmp1; - const Scalar _tmp20 = -P(1, 0) * _tmp5; - const Scalar _tmp21 = P(3, 2) * _tmp3; - const Scalar _tmp22 = -P(2, 2) * _tmp7; - const Scalar _tmp23 = P(1, 2) * _tmp5; - const Scalar _tmp24 = P(0, 1) * _tmp1; - const Scalar _tmp25 = -P(2, 1) * _tmp7; - const Scalar _tmp26 = -P(1, 1) * _tmp5; - const Scalar _tmp27 = -P(3, 3) * _tmp3; - const Scalar _tmp28 = -P(0, 3) * _tmp1; - const Scalar _tmp29 = -P(2, 3) * _tmp7; - const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) - - _tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) - - _tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) - - _tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23); - const Scalar _tmp31 = -P(0, 0) * _tmp5; - const Scalar _tmp32 = P(2, 0) * _tmp3; - const Scalar _tmp33 = P(1, 0) * _tmp1; - const Scalar _tmp34 = -P(3, 2) * _tmp7; - const Scalar _tmp35 = P(0, 2) * _tmp5; - const Scalar _tmp36 = P(2, 2) * _tmp3; - const Scalar _tmp37 = -P(3, 1) * _tmp7; - const Scalar _tmp38 = -P(0, 1) * _tmp5; - const Scalar _tmp39 = P(1, 1) * _tmp1; - const Scalar _tmp40 = -P(3, 3) * _tmp7; - const Scalar _tmp41 = P(2, 3) * _tmp3; - const Scalar _tmp42 = P(1, 3) * _tmp1; - const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) + - _tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) - - _tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) - - _tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42); - const Scalar _tmp44 = Scalar(1.0) / (_tmp17); - const Scalar _tmp45 = Scalar(19.613299999999999) * P(8, 3); - const Scalar _tmp46 = Scalar(19.613299999999999) * P(8, 0); - const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 2); - const Scalar _tmp48 = Scalar(19.613299999999999) * P(9, 3); - const Scalar _tmp49 = Scalar(19.613299999999999) * P(9, 2); - const Scalar _tmp50 = Scalar(19.613299999999999) * P(9, 0); - const Scalar _tmp51 = Scalar(1.0) / (_tmp30); - const Scalar _tmp52 = Scalar(19.613299999999999) * P(4, 0); - const Scalar _tmp53 = Scalar(1.0) / (_tmp43); + const Scalar _tmp3 = Scalar(19.613299999999999) * state(1, 0); + const Scalar _tmp4 = -P(3, 0) * _tmp3; + const Scalar _tmp5 = Scalar(19.613299999999999) * state(2, 0); + const Scalar _tmp6 = P(0, 0) * _tmp5; + const Scalar _tmp7 = Scalar(19.613299999999999) * state(0, 0); + const Scalar _tmp8 = Scalar(19.613299999999999) * state(3, 0); + const Scalar _tmp9 = P(2, 1) * _tmp7; + const Scalar _tmp10 = -P(1, 1) * _tmp8; + const Scalar _tmp11 = P(2, 2) * _tmp7; + const Scalar _tmp12 = -P(1, 2) * _tmp8; + const Scalar _tmp13 = -P(3, 3) * _tmp3; + const Scalar _tmp14 = P(0, 3) * _tmp5; + const Scalar _tmp15 = R - _tmp3 * (-P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + _tmp13 + _tmp14) + + _tmp5 * (-P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + _tmp4 + _tmp6) + + _tmp7 * (P(0, 2) * _tmp5 - P(3, 2) * _tmp3 + _tmp11 + _tmp12) - + _tmp8 * (P(0, 1) * _tmp5 - P(3, 1) * _tmp3 + _tmp10 + _tmp9); + const Scalar _tmp16 = P(3, 0) * _tmp5; + const Scalar _tmp17 = -P(0, 0) * _tmp3; + const Scalar _tmp18 = -P(2, 2) * _tmp8; + const Scalar _tmp19 = P(1, 2) * _tmp7; + const Scalar _tmp20 = -P(2, 1) * _tmp8; + const Scalar _tmp21 = -P(1, 1) * _tmp7; + const Scalar _tmp22 = -P(3, 3) * _tmp5; + const Scalar _tmp23 = -P(0, 3) * _tmp3; + const Scalar _tmp24 = R - _tmp3 * (-P(1, 0) * _tmp7 - P(2, 0) * _tmp8 - _tmp16 + _tmp17) - + _tmp5 * (-P(1, 3) * _tmp7 - P(2, 3) * _tmp8 + _tmp22 + _tmp23) - + _tmp7 * (-P(0, 1) * _tmp3 - P(3, 1) * _tmp5 + _tmp20 + _tmp21) - + _tmp8 * (-P(0, 2) * _tmp3 - P(3, 2) * _tmp5 + _tmp18 - _tmp19); + const Scalar _tmp25 = Scalar(39.226599999999998) * state(2, 0); + const Scalar _tmp26 = P(2, 2) * _tmp25; + const Scalar _tmp27 = Scalar(39.226599999999998) * state(1, 0); + const Scalar _tmp28 = P(1, 1) * _tmp27; + const Scalar _tmp29 = + R + _tmp25 * (P(1, 2) * _tmp27 + _tmp26) + _tmp27 * (P(2, 1) * _tmp25 + _tmp28); + const Scalar _tmp30 = Scalar(1.0) / (_tmp15); + const Scalar _tmp31 = Scalar(19.613299999999999) * P(6, 3); + const Scalar _tmp32 = Scalar(1.0) / (_tmp24); + const Scalar _tmp33 = Scalar(1.0) / (_tmp29); // Output terms (5) if (innov != nullptr) { matrix::Matrix& _innov = (*innov); - _innov(0, 0) = -_tmp0 * meas(0, 0) + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) - - Scalar(19.613299999999999) * state(1, 0) * state(3, 0); - _innov(1, 0) = -_tmp0 * meas(1, 0) - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) - - Scalar(19.613299999999999) * state(2, 0) * state(3, 0); - _innov(2, 0) = -_tmp0 * meas(2, 0) - - Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) + - Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) + - Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) - - Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2)); + _innov(0, 0) = Scalar(9.8066499999999994) * _tmp0 * state(0, 0) - + Scalar(9.8066499999999994) * _tmp1 * state(3, 0) - _tmp2 * meas(0, 0); + _innov(1, 0) = -Scalar(9.8066499999999994) * _tmp0 * state(3, 0) - + Scalar(9.8066499999999994) * _tmp1 * state(0, 0) - _tmp2 * meas(1, 0); + _innov(2, 0) = + -_tmp2 * meas(2, 0) + Scalar(19.613299999999999) * std::pow(state(1, 0), Scalar(2)) + + Scalar(19.613299999999999) * std::pow(state(2, 0), Scalar(2)) + Scalar(-9.8066499999999994); } if (innov_var != nullptr) { matrix::Matrix& _innov_var = (*innov_var); - _innov_var(0, 0) = _tmp17; - _innov_var(1, 0) = _tmp30; - _innov_var(2, 0) = _tmp43; + _innov_var(0, 0) = _tmp15; + _innov_var(1, 0) = _tmp24; + _innov_var(2, 0) = _tmp29; } if (Kx != nullptr) { matrix::Matrix& _kx = (*Kx); - _kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4); - _kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42); - _kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32); - _kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37); - _kx(4, 0) = _tmp44 * (P(4, 0) * _tmp3 - P(4, 1) * _tmp7 + P(4, 2) * _tmp5 - P(4, 3) * _tmp1); - _kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1); - _kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1); - _kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1); - _kx(8, 0) = _tmp44 * (-P(8, 1) * _tmp7 - _tmp45 * state(1, 0) + _tmp46 * state(2, 0) + - _tmp47 * state(0, 0)); - _kx(9, 0) = _tmp44 * (-P(9, 1) * _tmp7 - _tmp48 * state(1, 0) + _tmp49 * state(0, 0) + - _tmp50 * state(2, 0)); + _kx(0, 0) = _tmp30 * (-P(0, 1) * _tmp8 + P(0, 2) * _tmp7 + _tmp23 + _tmp6); + _kx(1, 0) = _tmp30 * (P(1, 0) * _tmp5 - P(1, 3) * _tmp3 + _tmp10 + _tmp19); + _kx(2, 0) = _tmp30 * (P(2, 0) * _tmp5 - P(2, 3) * _tmp3 + _tmp11 + _tmp20); + _kx(3, 0) = _tmp30 * (-P(3, 1) * _tmp8 + P(3, 2) * _tmp7 + _tmp13 + _tmp16); + _kx(4, 0) = _tmp30 * (P(4, 0) * _tmp5 - P(4, 1) * _tmp8 + P(4, 2) * _tmp7 - P(4, 3) * _tmp3); + _kx(5, 0) = _tmp30 * (P(5, 0) * _tmp5 - P(5, 1) * _tmp8 + P(5, 2) * _tmp7 - P(5, 3) * _tmp3); + _kx(6, 0) = + _tmp30 * (P(6, 0) * _tmp5 - P(6, 1) * _tmp8 + P(6, 2) * _tmp7 - _tmp31 * state(1, 0)); + _kx(7, 0) = _tmp30 * (P(7, 0) * _tmp5 - P(7, 1) * _tmp8 + P(7, 2) * _tmp7 - P(7, 3) * _tmp3); + _kx(8, 0) = _tmp30 * (P(8, 0) * _tmp5 - P(8, 1) * _tmp8 + P(8, 2) * _tmp7 - P(8, 3) * _tmp3); + _kx(9, 0) = _tmp30 * (P(9, 0) * _tmp5 - P(9, 1) * _tmp8 + P(9, 2) * _tmp7 - P(9, 3) * _tmp3); _kx(10, 0) = - _tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1); + _tmp30 * (P(10, 0) * _tmp5 - P(10, 1) * _tmp8 + P(10, 2) * _tmp7 - P(10, 3) * _tmp3); _kx(11, 0) = - _tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1); + _tmp30 * (P(11, 0) * _tmp5 - P(11, 1) * _tmp8 + P(11, 2) * _tmp7 - P(11, 3) * _tmp3); _kx(12, 0) = - _tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1); + _tmp30 * (P(12, 0) * _tmp5 - P(12, 1) * _tmp8 + P(12, 2) * _tmp7 - P(12, 3) * _tmp3); _kx(13, 0) = - _tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1); + _tmp30 * (P(13, 0) * _tmp5 - P(13, 1) * _tmp8 + P(13, 2) * _tmp7 - P(13, 3) * _tmp3); _kx(14, 0) = - _tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1); + _tmp30 * (P(14, 0) * _tmp5 - P(14, 1) * _tmp8 + P(14, 2) * _tmp7 - P(14, 3) * _tmp3); _kx(15, 0) = - _tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1); + _tmp30 * (P(15, 0) * _tmp5 - P(15, 1) * _tmp8 + P(15, 2) * _tmp7 - P(15, 3) * _tmp3); _kx(16, 0) = - _tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1); + _tmp30 * (P(16, 0) * _tmp5 - P(16, 1) * _tmp8 + P(16, 2) * _tmp7 - P(16, 3) * _tmp3); _kx(17, 0) = - _tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1); + _tmp30 * (P(17, 0) * _tmp5 - P(17, 1) * _tmp8 + P(17, 2) * _tmp7 - P(17, 3) * _tmp3); _kx(18, 0) = - _tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1); + _tmp30 * (P(18, 0) * _tmp5 - P(18, 1) * _tmp8 + P(18, 2) * _tmp7 - P(18, 3) * _tmp3); _kx(19, 0) = - _tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1); + _tmp30 * (P(19, 0) * _tmp5 - P(19, 1) * _tmp8 + P(19, 2) * _tmp7 - P(19, 3) * _tmp3); _kx(20, 0) = - _tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1); + _tmp30 * (P(20, 0) * _tmp5 - P(20, 1) * _tmp8 + P(20, 2) * _tmp7 - P(20, 3) * _tmp3); _kx(21, 0) = - _tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1); + _tmp30 * (P(21, 0) * _tmp5 - P(21, 1) * _tmp8 + P(21, 2) * _tmp7 - P(21, 3) * _tmp3); _kx(22, 0) = - _tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1); + _tmp30 * (P(22, 0) * _tmp5 - P(22, 1) * _tmp8 + P(22, 2) * _tmp7 - P(22, 3) * _tmp3); _kx(23, 0) = - _tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1); + _tmp30 * (P(23, 0) * _tmp5 - P(23, 1) * _tmp8 + P(23, 2) * _tmp7 - P(23, 3) * _tmp3); } if (Ky != nullptr) { matrix::Matrix& _ky = (*Ky); - _ky(0, 0) = _tmp51 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38); - _ky(1, 0) = _tmp51 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33); - _ky(2, 0) = _tmp51 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9); - _ky(3, 0) = _tmp51 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34); - _ky(4, 0) = - _tmp51 * (-P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3 - _tmp52 * state(1, 0)); - _ky(5, 0) = _tmp51 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3); - _ky(6, 0) = _tmp51 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3); - _ky(7, 0) = _tmp51 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3); - _ky(8, 0) = - _tmp51 * (-P(8, 1) * _tmp5 - P(8, 2) * _tmp7 - _tmp45 * state(2, 0) - _tmp46 * state(1, 0)); - _ky(9, 0) = - _tmp51 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - _tmp48 * state(2, 0) - _tmp50 * state(1, 0)); + _ky(0, 0) = _tmp32 * (-P(0, 1) * _tmp7 - P(0, 2) * _tmp8 - _tmp14 + _tmp17); + _ky(1, 0) = _tmp32 * (-P(1, 0) * _tmp3 - P(1, 3) * _tmp5 + _tmp12 + _tmp21); + _ky(2, 0) = _tmp32 * (-P(2, 0) * _tmp3 - P(2, 3) * _tmp5 + _tmp18 - _tmp9); + _ky(3, 0) = _tmp32 * (-P(3, 1) * _tmp7 - P(3, 2) * _tmp8 + _tmp22 + _tmp4); + _ky(4, 0) = _tmp32 * (-P(4, 0) * _tmp3 - P(4, 1) * _tmp7 - P(4, 2) * _tmp8 - P(4, 3) * _tmp5); + _ky(5, 0) = _tmp32 * (-P(5, 0) * _tmp3 - P(5, 1) * _tmp7 - P(5, 2) * _tmp8 - P(5, 3) * _tmp5); + _ky(6, 0) = + _tmp32 * (-P(6, 0) * _tmp3 - P(6, 1) * _tmp7 - P(6, 2) * _tmp8 - _tmp31 * state(2, 0)); + _ky(7, 0) = _tmp32 * (-P(7, 0) * _tmp3 - P(7, 1) * _tmp7 - P(7, 2) * _tmp8 - P(7, 3) * _tmp5); + _ky(8, 0) = _tmp32 * (-P(8, 0) * _tmp3 - P(8, 1) * _tmp7 - P(8, 2) * _tmp8 - P(8, 3) * _tmp5); + _ky(9, 0) = _tmp32 * (-P(9, 0) * _tmp3 - P(9, 1) * _tmp7 - P(9, 2) * _tmp8 - P(9, 3) * _tmp5); _ky(10, 0) = - _tmp51 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3); + _tmp32 * (-P(10, 0) * _tmp3 - P(10, 1) * _tmp7 - P(10, 2) * _tmp8 - P(10, 3) * _tmp5); _ky(11, 0) = - _tmp51 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3); + _tmp32 * (-P(11, 0) * _tmp3 - P(11, 1) * _tmp7 - P(11, 2) * _tmp8 - P(11, 3) * _tmp5); _ky(12, 0) = - _tmp51 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3); + _tmp32 * (-P(12, 0) * _tmp3 - P(12, 1) * _tmp7 - P(12, 2) * _tmp8 - P(12, 3) * _tmp5); _ky(13, 0) = - _tmp51 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3); + _tmp32 * (-P(13, 0) * _tmp3 - P(13, 1) * _tmp7 - P(13, 2) * _tmp8 - P(13, 3) * _tmp5); _ky(14, 0) = - _tmp51 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3); + _tmp32 * (-P(14, 0) * _tmp3 - P(14, 1) * _tmp7 - P(14, 2) * _tmp8 - P(14, 3) * _tmp5); _ky(15, 0) = - _tmp51 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3); + _tmp32 * (-P(15, 0) * _tmp3 - P(15, 1) * _tmp7 - P(15, 2) * _tmp8 - P(15, 3) * _tmp5); _ky(16, 0) = - _tmp51 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3); + _tmp32 * (-P(16, 0) * _tmp3 - P(16, 1) * _tmp7 - P(16, 2) * _tmp8 - P(16, 3) * _tmp5); _ky(17, 0) = - _tmp51 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3); + _tmp32 * (-P(17, 0) * _tmp3 - P(17, 1) * _tmp7 - P(17, 2) * _tmp8 - P(17, 3) * _tmp5); _ky(18, 0) = - _tmp51 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3); + _tmp32 * (-P(18, 0) * _tmp3 - P(18, 1) * _tmp7 - P(18, 2) * _tmp8 - P(18, 3) * _tmp5); _ky(19, 0) = - _tmp51 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3); + _tmp32 * (-P(19, 0) * _tmp3 - P(19, 1) * _tmp7 - P(19, 2) * _tmp8 - P(19, 3) * _tmp5); _ky(20, 0) = - _tmp51 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3); + _tmp32 * (-P(20, 0) * _tmp3 - P(20, 1) * _tmp7 - P(20, 2) * _tmp8 - P(20, 3) * _tmp5); _ky(21, 0) = - _tmp51 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3); + _tmp32 * (-P(21, 0) * _tmp3 - P(21, 1) * _tmp7 - P(21, 2) * _tmp8 - P(21, 3) * _tmp5); _ky(22, 0) = - _tmp51 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3); + _tmp32 * (-P(22, 0) * _tmp3 - P(22, 1) * _tmp7 - P(22, 2) * _tmp8 - P(22, 3) * _tmp5); _ky(23, 0) = - _tmp51 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3); + _tmp32 * (-P(23, 0) * _tmp3 - P(23, 1) * _tmp7 - P(23, 2) * _tmp8 - P(23, 3) * _tmp5); } if (Kz != nullptr) { matrix::Matrix& _kz = (*Kz); - _kz(0, 0) = _tmp53 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31); - _kz(1, 0) = _tmp53 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39); - _kz(2, 0) = _tmp53 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6); - _kz(3, 0) = _tmp53 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8); - _kz(4, 0) = - _tmp53 * (P(4, 1) * _tmp1 + P(4, 2) * _tmp3 - P(4, 3) * _tmp7 - _tmp52 * state(0, 0)); - _kz(5, 0) = _tmp53 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7); - _kz(6, 0) = _tmp53 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7); - _kz(7, 0) = _tmp53 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7); - _kz(8, 0) = - _tmp53 * (P(8, 1) * _tmp1 - P(8, 3) * _tmp7 - _tmp46 * state(0, 0) + _tmp47 * state(2, 0)); - _kz(9, 0) = - _tmp53 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp49 * state(2, 0) - _tmp50 * state(0, 0)); - _kz(10, 0) = - _tmp53 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7); - _kz(11, 0) = - _tmp53 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7); - _kz(12, 0) = - _tmp53 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7); - _kz(13, 0) = - _tmp53 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7); - _kz(14, 0) = - _tmp53 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7); - _kz(15, 0) = - _tmp53 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7); - _kz(16, 0) = - _tmp53 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7); - _kz(17, 0) = - _tmp53 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7); - _kz(18, 0) = - _tmp53 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7); - _kz(19, 0) = - _tmp53 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7); - _kz(20, 0) = - _tmp53 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7); - _kz(21, 0) = - _tmp53 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7); - _kz(22, 0) = - _tmp53 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7); - _kz(23, 0) = - _tmp53 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7); + _kz(0, 0) = _tmp33 * (P(0, 1) * _tmp27 + P(0, 2) * _tmp25); + _kz(1, 0) = _tmp33 * (P(1, 2) * _tmp25 + _tmp28); + _kz(2, 0) = _tmp33 * (P(2, 1) * _tmp27 + _tmp26); + _kz(3, 0) = _tmp33 * (P(3, 1) * _tmp27 + P(3, 2) * _tmp25); + _kz(4, 0) = _tmp33 * (P(4, 1) * _tmp27 + P(4, 2) * _tmp25); + _kz(5, 0) = _tmp33 * (P(5, 1) * _tmp27 + P(5, 2) * _tmp25); + _kz(6, 0) = _tmp33 * (P(6, 1) * _tmp27 + P(6, 2) * _tmp25); + _kz(7, 0) = _tmp33 * (P(7, 1) * _tmp27 + P(7, 2) * _tmp25); + _kz(8, 0) = _tmp33 * (P(8, 1) * _tmp27 + P(8, 2) * _tmp25); + _kz(9, 0) = _tmp33 * (P(9, 1) * _tmp27 + P(9, 2) * _tmp25); + _kz(10, 0) = _tmp33 * (P(10, 1) * _tmp27 + P(10, 2) * _tmp25); + _kz(11, 0) = _tmp33 * (P(11, 1) * _tmp27 + P(11, 2) * _tmp25); + _kz(12, 0) = _tmp33 * (P(12, 1) * _tmp27 + P(12, 2) * _tmp25); + _kz(13, 0) = _tmp33 * (P(13, 1) * _tmp27 + P(13, 2) * _tmp25); + _kz(14, 0) = _tmp33 * (P(14, 1) * _tmp27 + P(14, 2) * _tmp25); + _kz(15, 0) = _tmp33 * (P(15, 1) * _tmp27 + P(15, 2) * _tmp25); + _kz(16, 0) = _tmp33 * (P(16, 1) * _tmp27 + P(16, 2) * _tmp25); + _kz(17, 0) = _tmp33 * (P(17, 1) * _tmp27 + P(17, 2) * _tmp25); + _kz(18, 0) = _tmp33 * (P(18, 1) * _tmp27 + P(18, 2) * _tmp25); + _kz(19, 0) = _tmp33 * (P(19, 1) * _tmp27 + P(19, 2) * _tmp25); + _kz(20, 0) = _tmp33 * (P(20, 1) * _tmp27 + P(20, 2) * _tmp25); + _kz(21, 0) = _tmp33 * (P(21, 1) * _tmp27 + P(21, 2) * _tmp25); + _kz(22, 0) = _tmp33 * (P(22, 1) * _tmp27 + P(22, 2) * _tmp25); + _kz(23, 0) = _tmp33 * (P(23, 1) * _tmp27 + P(23, 2) * _tmp25); } } // NOLINT(readability/fn_size) 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 a838bb1886..3b7c2f98f4 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,69 +35,70 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, matrix::Matrix* const innov = nullptr, matrix::Matrix* const innov_var = nullptr, matrix::Matrix* const Hx = nullptr) { - // Total ops: 470 + // Total ops: 471 // Input arrays - // Intermediate terms (48) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = -_tmp0; - const Scalar _tmp2 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp3 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp5 = -_tmp3 + _tmp4; - const Scalar _tmp6 = _tmp1 + _tmp2 + _tmp5; - const Scalar _tmp7 = state(0, 0) * state(3, 0); - const Scalar _tmp8 = state(1, 0) * state(2, 0); - const Scalar _tmp9 = 2 * state(17, 0); - const Scalar _tmp10 = state(0, 0) * state(2, 0); - const Scalar _tmp11 = state(1, 0) * state(3, 0); - const Scalar _tmp12 = 2 * state(18, 0); - const Scalar _tmp13 = -_tmp2; - const Scalar _tmp14 = _tmp0 + _tmp13 + _tmp5; - const Scalar _tmp15 = state(2, 0) * state(3, 0); - const Scalar _tmp16 = state(0, 0) * state(1, 0); - const Scalar _tmp17 = 2 * state(16, 0); - const Scalar _tmp18 = _tmp1 + _tmp13 + _tmp3 + _tmp4; - const Scalar _tmp19 = _tmp9 * state(3, 0); - const Scalar _tmp20 = _tmp12 * state(2, 0); - const Scalar _tmp21 = 2 * state(0, 0); - const Scalar _tmp22 = _tmp21 * state(16, 0); - const Scalar _tmp23 = _tmp19 - _tmp20 + _tmp22; - const Scalar _tmp24 = _tmp12 * state(3, 0) + _tmp17 * state(1, 0) + _tmp9 * state(2, 0); - const Scalar _tmp25 = _tmp17 * state(3, 0); - const Scalar _tmp26 = _tmp12 * state(1, 0); - const Scalar _tmp27 = _tmp21 * state(17, 0); - const Scalar _tmp28 = -_tmp25 + _tmp26 + _tmp27; - const Scalar _tmp29 = _tmp17 * state(2, 0); - const Scalar _tmp30 = _tmp9 * state(1, 0); - const Scalar _tmp31 = _tmp12 * state(0, 0); - const Scalar _tmp32 = -_tmp29 + _tmp30 - _tmp31; - const Scalar _tmp33 = 2 * _tmp7; - const Scalar _tmp34 = 2 * _tmp8; + // Intermediate terms (49) + const Scalar _tmp0 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = -2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp2 = _tmp0 + _tmp1 + 1; + const Scalar _tmp3 = 2 * state(0, 0); + const Scalar _tmp4 = _tmp3 * state(3, 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 = 1 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp13 = _tmp0 + _tmp12; + const Scalar _tmp14 = _tmp5 * state(3, 0); + const Scalar _tmp15 = _tmp3 * state(1, 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 = _tmp9 * state(18, 0); + const Scalar _tmp22 = _tmp3 * state(17, 0); + const Scalar _tmp23 = _tmp21 + _tmp22 - 4 * state(16, 0) * state(3, 0); + const Scalar _tmp24 = 4 * state(2, 0); + const Scalar _tmp25 = _tmp9 * state(17, 0); + const Scalar _tmp26 = _tmp3 * state(18, 0); + const Scalar _tmp27 = -_tmp24 * state(16, 0) + _tmp25 - _tmp26; + const Scalar _tmp28 = state(17, 0) * state(3, 0); + const Scalar _tmp29 = 2 * _tmp28; + const Scalar _tmp30 = _tmp5 * state(18, 0); + const Scalar _tmp31 = _tmp29 - _tmp30; + const Scalar _tmp32 = 2 * state(3, 0); + const Scalar _tmp33 = _tmp32 * state(18, 0); + const Scalar _tmp34 = _tmp5 * state(17, 0); const Scalar _tmp35 = _tmp33 + _tmp34; - const Scalar _tmp36 = 2 * _tmp10; - const Scalar _tmp37 = 2 * _tmp11; - const Scalar _tmp38 = -_tmp36 + _tmp37; - const Scalar _tmp39 = _tmp29 - _tmp30 + _tmp31; - const Scalar _tmp40 = -_tmp19 + _tmp20 - _tmp22; - const Scalar _tmp41 = -_tmp33 + _tmp34; - const Scalar _tmp42 = 2 * _tmp15; - const Scalar _tmp43 = 2 * _tmp16; - const Scalar _tmp44 = _tmp42 + _tmp43; - const Scalar _tmp45 = _tmp25 - _tmp26 - _tmp27; - const Scalar _tmp46 = _tmp36 + _tmp37; - const Scalar _tmp47 = _tmp42 - _tmp43; + const Scalar _tmp36 = _tmp5 * state(16, 0); + const Scalar _tmp37 = 4 * state(1, 0); + const Scalar _tmp38 = _tmp26 + _tmp36 - _tmp37 * state(17, 0); + const Scalar _tmp39 = _tmp3 * state(16, 0); + const Scalar _tmp40 = -4 * _tmp28 + _tmp30 - _tmp39; + const Scalar _tmp41 = _tmp32 * state(16, 0); + const Scalar _tmp42 = _tmp21 - _tmp41; + const Scalar _tmp43 = _tmp9 * state(16, 0); + const Scalar _tmp44 = _tmp33 + _tmp43; + const Scalar _tmp45 = -_tmp22 - _tmp37 * state(18, 0) + _tmp41; + const Scalar _tmp46 = -_tmp24 * state(18, 0) + _tmp29 + _tmp39; + const Scalar _tmp47 = _tmp34 + _tmp43; + const Scalar _tmp48 = -_tmp25 + _tmp36; // Output terms (3) if (innov != nullptr) { matrix::Matrix& _innov = (*innov); - _innov(0, 0) = _tmp12 * (-_tmp10 + _tmp11) + _tmp6 * state(16, 0) + _tmp9 * (_tmp7 + _tmp8) - + _innov(0, 0) = _tmp11 * state(18, 0) + _tmp2 * state(16, 0) + _tmp7 * state(17, 0) - meas(0, 0) + state(19, 0); - _innov(1, 0) = _tmp12 * (_tmp15 + _tmp16) + _tmp14 * state(17, 0) + _tmp17 * (-_tmp7 + _tmp8) - + _innov(1, 0) = _tmp13 * state(17, 0) + _tmp16 * state(18, 0) + _tmp17 * state(16, 0) - meas(1, 0) + state(20, 0); - _innov(2, 0) = _tmp17 * (_tmp10 + _tmp11) + _tmp18 * state(18, 0) + _tmp9 * (_tmp15 - _tmp16) - + _innov(2, 0) = _tmp18 * state(18, 0) + _tmp19 * state(17, 0) + _tmp20 * state(16, 0) - meas(2, 0) + state(21, 0); } @@ -105,56 +106,56 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, matrix::Matrix& _innov_var = (*innov_var); _innov_var(0, 0) = - P(0, 19) * _tmp23 + P(1, 19) * _tmp24 + P(16, 19) * _tmp6 + P(17, 19) * _tmp35 + - P(18, 19) * _tmp38 + P(19, 19) + P(2, 19) * _tmp32 + P(3, 19) * _tmp28 + R + - _tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp24 + P(16, 0) * _tmp6 + P(17, 0) * _tmp35 + - P(18, 0) * _tmp38 + P(19, 0) + P(2, 0) * _tmp32 + P(3, 0) * _tmp28) + - _tmp24 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp24 + P(16, 1) * _tmp6 + P(17, 1) * _tmp35 + - P(18, 1) * _tmp38 + P(19, 1) + P(2, 1) * _tmp32 + P(3, 1) * _tmp28) + - _tmp28 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp24 + P(16, 3) * _tmp6 + P(17, 3) * _tmp35 + - P(18, 3) * _tmp38 + P(19, 3) + P(2, 3) * _tmp32 + P(3, 3) * _tmp28) + - _tmp32 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp24 + P(16, 2) * _tmp6 + P(17, 2) * _tmp35 + - P(18, 2) * _tmp38 + P(19, 2) + P(2, 2) * _tmp32 + P(3, 2) * _tmp28) + - _tmp35 * (P(0, 17) * _tmp23 + P(1, 17) * _tmp24 + P(16, 17) * _tmp6 + P(17, 17) * _tmp35 + - P(18, 17) * _tmp38 + P(19, 17) + P(2, 17) * _tmp32 + P(3, 17) * _tmp28) + - _tmp38 * (P(0, 18) * _tmp23 + P(1, 18) * _tmp24 + P(16, 18) * _tmp6 + P(17, 18) * _tmp35 + - P(18, 18) * _tmp38 + P(19, 18) + P(2, 18) * _tmp32 + P(3, 18) * _tmp28) + - _tmp6 * (P(0, 16) * _tmp23 + P(1, 16) * _tmp24 + P(16, 16) * _tmp6 + P(17, 16) * _tmp35 + - P(18, 16) * _tmp38 + P(19, 16) + P(2, 16) * _tmp32 + P(3, 16) * _tmp28); + P(0, 19) * _tmp31 + P(1, 19) * _tmp35 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 + + P(18, 19) * _tmp11 + P(19, 19) + P(2, 19) * _tmp27 + P(3, 19) * _tmp23 + R + + _tmp11 * (P(0, 18) * _tmp31 + P(1, 18) * _tmp35 + P(16, 18) * _tmp2 + P(17, 18) * _tmp7 + + P(18, 18) * _tmp11 + P(19, 18) + P(2, 18) * _tmp27 + P(3, 18) * _tmp23) + + _tmp2 * (P(0, 16) * _tmp31 + P(1, 16) * _tmp35 + P(16, 16) * _tmp2 + P(17, 16) * _tmp7 + + P(18, 16) * _tmp11 + P(19, 16) + P(2, 16) * _tmp27 + P(3, 16) * _tmp23) + + _tmp23 * (P(0, 3) * _tmp31 + P(1, 3) * _tmp35 + P(16, 3) * _tmp2 + P(17, 3) * _tmp7 + + P(18, 3) * _tmp11 + P(19, 3) + P(2, 3) * _tmp27 + P(3, 3) * _tmp23) + + _tmp27 * (P(0, 2) * _tmp31 + P(1, 2) * _tmp35 + P(16, 2) * _tmp2 + P(17, 2) * _tmp7 + + P(18, 2) * _tmp11 + P(19, 2) + P(2, 2) * _tmp27 + P(3, 2) * _tmp23) + + _tmp31 * (P(0, 0) * _tmp31 + P(1, 0) * _tmp35 + P(16, 0) * _tmp2 + P(17, 0) * _tmp7 + + P(18, 0) * _tmp11 + P(19, 0) + P(2, 0) * _tmp27 + P(3, 0) * _tmp23) + + _tmp35 * (P(0, 1) * _tmp31 + P(1, 1) * _tmp35 + P(16, 1) * _tmp2 + P(17, 1) * _tmp7 + + P(18, 1) * _tmp11 + P(19, 1) + P(2, 1) * _tmp27 + P(3, 1) * _tmp23) + + _tmp7 * (P(0, 17) * _tmp31 + P(1, 17) * _tmp35 + P(16, 17) * _tmp2 + P(17, 17) * _tmp7 + + P(18, 17) * _tmp11 + P(19, 17) + P(2, 17) * _tmp27 + P(3, 17) * _tmp23); _innov_var(1, 0) = - P(0, 20) * _tmp28 + P(1, 20) * _tmp39 + P(16, 20) * _tmp41 + P(17, 20) * _tmp14 + - P(18, 20) * _tmp44 + P(2, 20) * _tmp24 + P(20, 20) + P(3, 20) * _tmp40 + R + - _tmp14 * (P(0, 17) * _tmp28 + P(1, 17) * _tmp39 + P(16, 17) * _tmp41 + P(17, 17) * _tmp14 + - P(18, 17) * _tmp44 + P(2, 17) * _tmp24 + P(20, 17) + P(3, 17) * _tmp40) + - _tmp24 * (P(0, 2) * _tmp28 + P(1, 2) * _tmp39 + P(16, 2) * _tmp41 + P(17, 2) * _tmp14 + - P(18, 2) * _tmp44 + P(2, 2) * _tmp24 + P(20, 2) + P(3, 2) * _tmp40) + - _tmp28 * (P(0, 0) * _tmp28 + P(1, 0) * _tmp39 + P(16, 0) * _tmp41 + P(17, 0) * _tmp14 + - P(18, 0) * _tmp44 + P(2, 0) * _tmp24 + P(20, 0) + P(3, 0) * _tmp40) + - _tmp39 * (P(0, 1) * _tmp28 + P(1, 1) * _tmp39 + P(16, 1) * _tmp41 + P(17, 1) * _tmp14 + - P(18, 1) * _tmp44 + P(2, 1) * _tmp24 + P(20, 1) + P(3, 1) * _tmp40) + - _tmp40 * (P(0, 3) * _tmp28 + P(1, 3) * _tmp39 + P(16, 3) * _tmp41 + P(17, 3) * _tmp14 + - P(18, 3) * _tmp44 + P(2, 3) * _tmp24 + P(20, 3) + P(3, 3) * _tmp40) + - _tmp41 * (P(0, 16) * _tmp28 + P(1, 16) * _tmp39 + P(16, 16) * _tmp41 + P(17, 16) * _tmp14 + - P(18, 16) * _tmp44 + P(2, 16) * _tmp24 + P(20, 16) + P(3, 16) * _tmp40) + - _tmp44 * (P(0, 18) * _tmp28 + P(1, 18) * _tmp39 + P(16, 18) * _tmp41 + P(17, 18) * _tmp14 + - P(18, 18) * _tmp44 + P(2, 18) * _tmp24 + P(20, 18) + P(3, 18) * _tmp40); + P(0, 20) * _tmp42 + P(1, 20) * _tmp38 + P(16, 20) * _tmp17 + P(17, 20) * _tmp13 + + P(18, 20) * _tmp16 + P(2, 20) * _tmp44 + P(20, 20) + P(3, 20) * _tmp40 + R + + _tmp13 * (P(0, 17) * _tmp42 + P(1, 17) * _tmp38 + P(16, 17) * _tmp17 + P(17, 17) * _tmp13 + + P(18, 17) * _tmp16 + P(2, 17) * _tmp44 + P(20, 17) + P(3, 17) * _tmp40) + + _tmp16 * (P(0, 18) * _tmp42 + P(1, 18) * _tmp38 + P(16, 18) * _tmp17 + P(17, 18) * _tmp13 + + P(18, 18) * _tmp16 + P(2, 18) * _tmp44 + P(20, 18) + P(3, 18) * _tmp40) + + _tmp17 * (P(0, 16) * _tmp42 + P(1, 16) * _tmp38 + P(16, 16) * _tmp17 + P(17, 16) * _tmp13 + + P(18, 16) * _tmp16 + P(2, 16) * _tmp44 + P(20, 16) + P(3, 16) * _tmp40) + + _tmp38 * (P(0, 1) * _tmp42 + P(1, 1) * _tmp38 + P(16, 1) * _tmp17 + P(17, 1) * _tmp13 + + P(18, 1) * _tmp16 + P(2, 1) * _tmp44 + P(20, 1) + P(3, 1) * _tmp40) + + _tmp40 * (P(0, 3) * _tmp42 + P(1, 3) * _tmp38 + P(16, 3) * _tmp17 + P(17, 3) * _tmp13 + + P(18, 3) * _tmp16 + P(2, 3) * _tmp44 + P(20, 3) + P(3, 3) * _tmp40) + + _tmp42 * (P(0, 0) * _tmp42 + P(1, 0) * _tmp38 + P(16, 0) * _tmp17 + P(17, 0) * _tmp13 + + P(18, 0) * _tmp16 + P(2, 0) * _tmp44 + P(20, 0) + P(3, 0) * _tmp40) + + _tmp44 * (P(0, 2) * _tmp42 + P(1, 2) * _tmp38 + P(16, 2) * _tmp17 + P(17, 2) * _tmp13 + + P(18, 2) * _tmp16 + P(2, 2) * _tmp44 + P(20, 2) + P(3, 2) * _tmp40); _innov_var(2, 0) = - P(0, 21) * _tmp39 + P(1, 21) * _tmp45 + P(16, 21) * _tmp46 + P(17, 21) * _tmp47 + - P(18, 21) * _tmp18 + P(2, 21) * _tmp23 + P(21, 21) + P(3, 21) * _tmp24 + R + - _tmp18 * (P(0, 18) * _tmp39 + P(1, 18) * _tmp45 + P(16, 18) * _tmp46 + P(17, 18) * _tmp47 + - P(18, 18) * _tmp18 + P(2, 18) * _tmp23 + P(21, 18) + P(3, 18) * _tmp24) + - _tmp23 * (P(0, 2) * _tmp39 + P(1, 2) * _tmp45 + P(16, 2) * _tmp46 + P(17, 2) * _tmp47 + - P(18, 2) * _tmp18 + P(2, 2) * _tmp23 + P(21, 2) + P(3, 2) * _tmp24) + - _tmp24 * (P(0, 3) * _tmp39 + P(1, 3) * _tmp45 + P(16, 3) * _tmp46 + P(17, 3) * _tmp47 + - P(18, 3) * _tmp18 + P(2, 3) * _tmp23 + P(21, 3) + P(3, 3) * _tmp24) + - _tmp39 * (P(0, 0) * _tmp39 + P(1, 0) * _tmp45 + P(16, 0) * _tmp46 + P(17, 0) * _tmp47 + - P(18, 0) * _tmp18 + P(2, 0) * _tmp23 + P(21, 0) + P(3, 0) * _tmp24) + - _tmp45 * (P(0, 1) * _tmp39 + P(1, 1) * _tmp45 + P(16, 1) * _tmp46 + P(17, 1) * _tmp47 + - P(18, 1) * _tmp18 + P(2, 1) * _tmp23 + P(21, 1) + P(3, 1) * _tmp24) + - _tmp46 * (P(0, 16) * _tmp39 + P(1, 16) * _tmp45 + P(16, 16) * _tmp46 + P(17, 16) * _tmp47 + - P(18, 16) * _tmp18 + P(2, 16) * _tmp23 + P(21, 16) + P(3, 16) * _tmp24) + - _tmp47 * (P(0, 17) * _tmp39 + P(1, 17) * _tmp45 + P(16, 17) * _tmp46 + P(17, 17) * _tmp47 + - P(18, 17) * _tmp18 + P(2, 17) * _tmp23 + P(21, 17) + P(3, 17) * _tmp24); + P(0, 21) * _tmp48 + P(1, 21) * _tmp45 + P(16, 21) * _tmp20 + P(17, 21) * _tmp19 + + P(18, 21) * _tmp18 + P(2, 21) * _tmp46 + P(21, 21) + P(3, 21) * _tmp47 + R + + _tmp18 * (P(0, 18) * _tmp48 + P(1, 18) * _tmp45 + P(16, 18) * _tmp20 + P(17, 18) * _tmp19 + + P(18, 18) * _tmp18 + P(2, 18) * _tmp46 + P(21, 18) + P(3, 18) * _tmp47) + + _tmp19 * (P(0, 17) * _tmp48 + P(1, 17) * _tmp45 + P(16, 17) * _tmp20 + P(17, 17) * _tmp19 + + P(18, 17) * _tmp18 + P(2, 17) * _tmp46 + P(21, 17) + P(3, 17) * _tmp47) + + _tmp20 * (P(0, 16) * _tmp48 + P(1, 16) * _tmp45 + P(16, 16) * _tmp20 + P(17, 16) * _tmp19 + + P(18, 16) * _tmp18 + P(2, 16) * _tmp46 + P(21, 16) + P(3, 16) * _tmp47) + + _tmp45 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp45 + P(16, 1) * _tmp20 + P(17, 1) * _tmp19 + + P(18, 1) * _tmp18 + P(2, 1) * _tmp46 + P(21, 1) + P(3, 1) * _tmp47) + + _tmp46 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp45 + P(16, 2) * _tmp20 + P(17, 2) * _tmp19 + + P(18, 2) * _tmp18 + P(2, 2) * _tmp46 + P(21, 2) + P(3, 2) * _tmp47) + + _tmp47 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp45 + P(16, 3) * _tmp20 + P(17, 3) * _tmp19 + + P(18, 3) * _tmp18 + P(2, 3) * _tmp46 + P(21, 3) + P(3, 3) * _tmp47) + + _tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp45 + P(16, 0) * _tmp20 + P(17, 0) * _tmp19 + + P(18, 0) * _tmp18 + P(2, 0) * _tmp46 + P(21, 0) + P(3, 0) * _tmp47); } if (Hx != nullptr) { @@ -162,13 +163,13 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, _hx.setZero(); - _hx(0, 0) = _tmp23; - _hx(1, 0) = _tmp24; - _hx(2, 0) = _tmp32; - _hx(3, 0) = _tmp28; - _hx(16, 0) = _tmp6; - _hx(17, 0) = _tmp35; - _hx(18, 0) = _tmp38; + _hx(0, 0) = _tmp31; + _hx(1, 0) = _tmp35; + _hx(2, 0) = _tmp27; + _hx(3, 0) = _tmp23; + _hx(16, 0) = _tmp2; + _hx(17, 0) = _tmp7; + _hx(18, 0) = _tmp11; _hx(19, 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 083ae8d006..330532de8a 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,45 +30,46 @@ 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: 164 + // Total ops: 160 // Input arrays - // Intermediate terms (11) - const Scalar _tmp0 = 2 * state(3, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = 2 * state(0, 0); - const Scalar _tmp3 = -_tmp0 * state(16, 0) + _tmp1 * state(18, 0) + _tmp2 * state(17, 0); - const Scalar _tmp4 = 2 * state(2, 0); - const Scalar _tmp5 = -_tmp1 * state(17, 0) + _tmp2 * state(18, 0) + _tmp4 * state(16, 0); - const Scalar _tmp6 = -_tmp0 * state(17, 0) - _tmp2 * state(16, 0) + _tmp4 * state(18, 0); - const Scalar _tmp7 = _tmp0 * state(18, 0) + _tmp1 * state(16, 0) + _tmp4 * state(17, 0); - const Scalar _tmp8 = -_tmp0 * state(0, 0) + _tmp1 * state(2, 0); - const Scalar _tmp9 = _tmp0 * state(2, 0) + _tmp1 * state(0, 0); - const Scalar _tmp10 = 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 (12) + const Scalar _tmp0 = 2 * state(16, 0); + const Scalar _tmp1 = 4 * state(17, 0); + const Scalar _tmp2 = 2 * state(18, 0); + const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0) + _tmp2 * state(0, 0); + const Scalar _tmp4 = -_tmp0 * state(0, 0) - _tmp1 * state(3, 0) + _tmp2 * state(2, 0); + const Scalar _tmp5 = -_tmp0 * state(3, 0) + _tmp2 * state(1, 0); + const Scalar _tmp6 = _tmp0 * state(1, 0) + _tmp2 * state(3, 0); + const Scalar _tmp7 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + const Scalar _tmp8 = 2 * state(3, 0); + const Scalar _tmp9 = 2 * state(1, 0); + const Scalar _tmp10 = _tmp8 * state(2, 0) + _tmp9 * state(0, 0); + const Scalar _tmp11 = -_tmp8 * state(0, 0) + _tmp9 * state(2, 0); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); _innov_var = - P(0, 20) * _tmp3 + P(1, 20) * _tmp5 + P(16, 20) * _tmp8 + P(17, 20) * _tmp10 + - P(18, 20) * _tmp9 + P(2, 20) * _tmp7 + P(20, 20) + P(3, 20) * _tmp6 + R + - _tmp10 * (P(0, 17) * _tmp3 + P(1, 17) * _tmp5 + P(16, 17) * _tmp8 + P(17, 17) * _tmp10 + - P(18, 17) * _tmp9 + P(2, 17) * _tmp7 + P(20, 17) + P(3, 17) * _tmp6) + - _tmp3 * (P(0, 0) * _tmp3 + P(1, 0) * _tmp5 + P(16, 0) * _tmp8 + P(17, 0) * _tmp10 + - P(18, 0) * _tmp9 + P(2, 0) * _tmp7 + P(20, 0) + P(3, 0) * _tmp6) + - _tmp5 * (P(0, 1) * _tmp3 + P(1, 1) * _tmp5 + P(16, 1) * _tmp8 + P(17, 1) * _tmp10 + - P(18, 1) * _tmp9 + P(2, 1) * _tmp7 + P(20, 1) + P(3, 1) * _tmp6) + - _tmp6 * (P(0, 3) * _tmp3 + P(1, 3) * _tmp5 + P(16, 3) * _tmp8 + P(17, 3) * _tmp10 + - P(18, 3) * _tmp9 + P(2, 3) * _tmp7 + P(20, 3) + P(3, 3) * _tmp6) + - _tmp7 * (P(0, 2) * _tmp3 + P(1, 2) * _tmp5 + P(16, 2) * _tmp8 + P(17, 2) * _tmp10 + - P(18, 2) * _tmp9 + P(2, 2) * _tmp7 + P(20, 2) + P(3, 2) * _tmp6) + - _tmp8 * (P(0, 16) * _tmp3 + P(1, 16) * _tmp5 + P(16, 16) * _tmp8 + P(17, 16) * _tmp10 + - P(18, 16) * _tmp9 + P(2, 16) * _tmp7 + P(20, 16) + P(3, 16) * _tmp6) + - _tmp9 * (P(0, 18) * _tmp3 + P(1, 18) * _tmp5 + P(16, 18) * _tmp8 + P(17, 18) * _tmp10 + - P(18, 18) * _tmp9 + P(2, 18) * _tmp7 + P(20, 18) + P(3, 18) * _tmp6); + P(0, 20) * _tmp5 + P(1, 20) * _tmp3 + P(16, 20) * _tmp11 + P(17, 20) * _tmp7 + + P(18, 20) * _tmp10 + P(2, 20) * _tmp6 + P(20, 20) + P(3, 20) * _tmp4 + R + + _tmp10 * (P(0, 18) * _tmp5 + P(1, 18) * _tmp3 + P(16, 18) * _tmp11 + P(17, 18) * _tmp7 + + P(18, 18) * _tmp10 + P(2, 18) * _tmp6 + P(20, 18) + P(3, 18) * _tmp4) + + _tmp11 * (P(0, 16) * _tmp5 + P(1, 16) * _tmp3 + P(16, 16) * _tmp11 + P(17, 16) * _tmp7 + + P(18, 16) * _tmp10 + P(2, 16) * _tmp6 + P(20, 16) + P(3, 16) * _tmp4) + + _tmp3 * (P(0, 1) * _tmp5 + P(1, 1) * _tmp3 + P(16, 1) * _tmp11 + P(17, 1) * _tmp7 + + P(18, 1) * _tmp10 + P(2, 1) * _tmp6 + P(20, 1) + P(3, 1) * _tmp4) + + _tmp4 * (P(0, 3) * _tmp5 + P(1, 3) * _tmp3 + P(16, 3) * _tmp11 + P(17, 3) * _tmp7 + + P(18, 3) * _tmp10 + P(2, 3) * _tmp6 + P(20, 3) + P(3, 3) * _tmp4) + + _tmp5 * (P(0, 0) * _tmp5 + P(1, 0) * _tmp3 + P(16, 0) * _tmp11 + P(17, 0) * _tmp7 + + P(18, 0) * _tmp10 + P(2, 0) * _tmp6 + P(20, 0) + P(3, 0) * _tmp4) + + _tmp6 * (P(0, 2) * _tmp5 + P(1, 2) * _tmp3 + P(16, 2) * _tmp11 + P(17, 2) * _tmp7 + + P(18, 2) * _tmp10 + P(2, 2) * _tmp6 + P(20, 2) + P(3, 2) * _tmp4) + + _tmp7 * (P(0, 17) * _tmp5 + P(1, 17) * _tmp3 + P(16, 17) * _tmp11 + P(17, 17) * _tmp7 + + P(18, 17) * _tmp10 + P(2, 17) * _tmp6 + P(20, 17) + P(3, 17) * _tmp4); } if (H != nullptr) { @@ -76,13 +77,13 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp3; - _h(1, 0) = _tmp5; - _h(2, 0) = _tmp7; - _h(3, 0) = _tmp6; - _h(16, 0) = _tmp8; - _h(17, 0) = _tmp10; - _h(18, 0) = _tmp9; + _h(0, 0) = _tmp5; + _h(1, 0) = _tmp3; + _h(2, 0) = _tmp6; + _h(3, 0) = _tmp4; + _h(16, 0) = _tmp11; + _h(17, 0) = _tmp7; + _h(18, 0) = _tmp10; _h(20, 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 90c896b946..fcff6cc92e 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,45 +30,46 @@ 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: 164 + // Total ops: 160 // Input arrays - // Intermediate terms (11) - const Scalar _tmp0 = 2 * state(2, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = 2 * state(0, 0); - const Scalar _tmp3 = _tmp0 * state(16, 0) - _tmp1 * state(17, 0) + _tmp2 * state(18, 0); - const Scalar _tmp4 = 2 * state(3, 0); - const Scalar _tmp5 = -_tmp1 * state(18, 0) - _tmp2 * state(17, 0) + _tmp4 * state(16, 0); - const Scalar _tmp6 = _tmp0 * state(17, 0) + _tmp1 * state(16, 0) + _tmp4 * state(18, 0); - const Scalar _tmp7 = -_tmp0 * state(18, 0) + _tmp2 * state(16, 0) + _tmp4 * state(17, 0); - const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0); - const Scalar _tmp9 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0); - const Scalar _tmp10 = 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 (12) + const Scalar _tmp0 = 2 * state(16, 0); + const Scalar _tmp1 = 4 * state(18, 0); + const Scalar _tmp2 = 2 * state(17, 0); + const Scalar _tmp3 = _tmp0 * state(3, 0) - _tmp1 * state(1, 0) - _tmp2 * state(0, 0); + const Scalar _tmp4 = _tmp0 * state(0, 0) - _tmp1 * state(2, 0) + _tmp2 * state(3, 0); + const Scalar _tmp5 = _tmp0 * state(1, 0) + _tmp2 * state(2, 0); + const Scalar _tmp6 = _tmp0 * state(2, 0) - _tmp2 * state(1, 0); + const Scalar _tmp7 = 2 * state(2, 0); + const Scalar _tmp8 = 2 * state(1, 0); + const Scalar _tmp9 = _tmp7 * state(0, 0) + _tmp8 * state(3, 0); + const Scalar _tmp10 = _tmp7 * state(3, 0) - _tmp8 * state(0, 0); + const Scalar _tmp11 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); _innov_var = - P(0, 21) * _tmp3 + P(1, 21) * _tmp5 + P(16, 21) * _tmp8 + P(17, 21) * _tmp9 + - P(18, 21) * _tmp10 + P(2, 21) * _tmp7 + P(21, 21) + P(3, 21) * _tmp6 + R + - _tmp10 * (P(0, 18) * _tmp3 + P(1, 18) * _tmp5 + P(16, 18) * _tmp8 + P(17, 18) * _tmp9 + - P(18, 18) * _tmp10 + P(2, 18) * _tmp7 + P(21, 18) + P(3, 18) * _tmp6) + - _tmp3 * (P(0, 0) * _tmp3 + P(1, 0) * _tmp5 + P(16, 0) * _tmp8 + P(17, 0) * _tmp9 + - P(18, 0) * _tmp10 + P(2, 0) * _tmp7 + P(21, 0) + P(3, 0) * _tmp6) + - _tmp5 * (P(0, 1) * _tmp3 + P(1, 1) * _tmp5 + P(16, 1) * _tmp8 + P(17, 1) * _tmp9 + - P(18, 1) * _tmp10 + P(2, 1) * _tmp7 + P(21, 1) + P(3, 1) * _tmp6) + - _tmp6 * (P(0, 3) * _tmp3 + P(1, 3) * _tmp5 + P(16, 3) * _tmp8 + P(17, 3) * _tmp9 + - P(18, 3) * _tmp10 + P(2, 3) * _tmp7 + P(21, 3) + P(3, 3) * _tmp6) + - _tmp7 * (P(0, 2) * _tmp3 + P(1, 2) * _tmp5 + P(16, 2) * _tmp8 + P(17, 2) * _tmp9 + - P(18, 2) * _tmp10 + P(2, 2) * _tmp7 + P(21, 2) + P(3, 2) * _tmp6) + - _tmp8 * (P(0, 16) * _tmp3 + P(1, 16) * _tmp5 + P(16, 16) * _tmp8 + P(17, 16) * _tmp9 + - P(18, 16) * _tmp10 + P(2, 16) * _tmp7 + P(21, 16) + P(3, 16) * _tmp6) + - _tmp9 * (P(0, 17) * _tmp3 + P(1, 17) * _tmp5 + P(16, 17) * _tmp8 + P(17, 17) * _tmp9 + - P(18, 17) * _tmp10 + P(2, 17) * _tmp7 + P(21, 17) + P(3, 17) * _tmp6); + P(0, 21) * _tmp6 + P(1, 21) * _tmp3 + P(16, 21) * _tmp9 + P(17, 21) * _tmp10 + + P(18, 21) * _tmp11 + P(2, 21) * _tmp4 + P(21, 21) + P(3, 21) * _tmp5 + R + + _tmp10 * (P(0, 17) * _tmp6 + P(1, 17) * _tmp3 + P(16, 17) * _tmp9 + P(17, 17) * _tmp10 + + P(18, 17) * _tmp11 + P(2, 17) * _tmp4 + P(21, 17) + P(3, 17) * _tmp5) + + _tmp11 * (P(0, 18) * _tmp6 + P(1, 18) * _tmp3 + P(16, 18) * _tmp9 + P(17, 18) * _tmp10 + + P(18, 18) * _tmp11 + P(2, 18) * _tmp4 + P(21, 18) + P(3, 18) * _tmp5) + + _tmp3 * (P(0, 1) * _tmp6 + P(1, 1) * _tmp3 + P(16, 1) * _tmp9 + P(17, 1) * _tmp10 + + P(18, 1) * _tmp11 + P(2, 1) * _tmp4 + P(21, 1) + P(3, 1) * _tmp5) + + _tmp4 * (P(0, 2) * _tmp6 + P(1, 2) * _tmp3 + P(16, 2) * _tmp9 + P(17, 2) * _tmp10 + + P(18, 2) * _tmp11 + P(2, 2) * _tmp4 + P(21, 2) + P(3, 2) * _tmp5) + + _tmp5 * (P(0, 3) * _tmp6 + P(1, 3) * _tmp3 + P(16, 3) * _tmp9 + P(17, 3) * _tmp10 + + P(18, 3) * _tmp11 + P(2, 3) * _tmp4 + P(21, 3) + P(3, 3) * _tmp5) + + _tmp6 * (P(0, 0) * _tmp6 + P(1, 0) * _tmp3 + P(16, 0) * _tmp9 + P(17, 0) * _tmp10 + + P(18, 0) * _tmp11 + P(2, 0) * _tmp4 + P(21, 0) + P(3, 0) * _tmp5) + + _tmp9 * (P(0, 16) * _tmp6 + P(1, 16) * _tmp3 + P(16, 16) * _tmp9 + P(17, 16) * _tmp10 + + P(18, 16) * _tmp11 + P(2, 16) * _tmp4 + P(21, 16) + P(3, 16) * _tmp5); } if (H != nullptr) { @@ -76,13 +77,13 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp3; - _h(1, 0) = _tmp5; - _h(2, 0) = _tmp7; - _h(3, 0) = _tmp6; - _h(16, 0) = _tmp8; - _h(17, 0) = _tmp9; - _h(18, 0) = _tmp10; + _h(0, 0) = _tmp6; + _h(1, 0) = _tmp3; + _h(2, 0) = _tmp4; + _h(3, 0) = _tmp5; + _h(16, 0) = _tmp9; + _h(17, 0) = _tmp10; + _h(18, 0) = _tmp11; _h(21, 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 2f00cb2b6e..1ffb4ab457 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,57 +30,56 @@ 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: 539 + // Total ops: 533 // Input arrays - // Intermediate terms (44) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2; - const Scalar _tmp4 = -state(22, 0) + state(4, 0); - const Scalar _tmp5 = state(0, 0) * state(3, 0); - const Scalar _tmp6 = state(1, 0) * state(2, 0); - const Scalar _tmp7 = _tmp5 + _tmp6; - const Scalar _tmp8 = -state(23, 0) + state(5, 0); - const Scalar _tmp9 = 2 * _tmp8; - const Scalar _tmp10 = state(0, 0) * state(2, 0); - const Scalar _tmp11 = state(1, 0) * state(3, 0); - const Scalar _tmp12 = 2 * state(6, 0); - const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9; - const Scalar _tmp14 = - _tmp13 + epsilon * (2 * math::min(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1); - const Scalar _tmp15 = Scalar(1.0) / (_tmp14); - const Scalar _tmp16 = 2 * _tmp4; - const Scalar _tmp17 = _tmp12 * state(1, 0) - _tmp16 * state(3, 0) + _tmp9 * state(0, 0); - const Scalar _tmp18 = _tmp16 * state(0, 0); - const Scalar _tmp19 = _tmp9 * state(3, 0); - const Scalar _tmp20 = _tmp12 * state(2, 0); - const Scalar _tmp21 = _tmp0 - _tmp1 + _tmp2; - const Scalar _tmp22 = -2 * _tmp5 + 2 * _tmp6; - const Scalar _tmp23 = state(2, 0) * state(3, 0); - const Scalar _tmp24 = state(0, 0) * state(1, 0); - const Scalar _tmp25 = - (_tmp12 * (_tmp23 + _tmp24) + _tmp21 * _tmp8 + _tmp22 * _tmp4) / std::pow(_tmp14, Scalar(2)); - const Scalar _tmp26 = _tmp15 * _tmp17 - _tmp25 * (_tmp18 + _tmp19 - _tmp20); - const Scalar _tmp27 = _tmp9 * state(1, 0); - const Scalar _tmp28 = _tmp16 * state(2, 0); - const Scalar _tmp29 = _tmp12 * state(0, 0); - const Scalar _tmp30 = _tmp12 * state(3, 0) + _tmp16 * state(1, 0) + _tmp9 * state(2, 0); - const Scalar _tmp31 = _tmp15 * (-_tmp27 + _tmp28 + _tmp29) - _tmp25 * _tmp30; - const Scalar _tmp32 = _tmp15 * _tmp30 - _tmp25 * (_tmp27 - _tmp28 - _tmp29); - const Scalar _tmp33 = _tmp15 * (-_tmp18 - _tmp19 + _tmp20) - _tmp17 * _tmp25; - const Scalar _tmp34 = _tmp25 * _tmp3; - const Scalar _tmp35 = 2 * _tmp5; - const Scalar _tmp36 = 2 * _tmp6; - const Scalar _tmp37 = _tmp15 * (-_tmp35 + _tmp36) - _tmp34; - const Scalar _tmp38 = _tmp15 * _tmp21; - const Scalar _tmp39 = -_tmp25 * (_tmp35 + _tmp36) + _tmp38; - const Scalar _tmp40 = _tmp15 * (2 * _tmp23 + 2 * _tmp24) - _tmp25 * (-2 * _tmp10 + 2 * _tmp11); - const Scalar _tmp41 = -_tmp15 * _tmp22 + _tmp34; - const Scalar _tmp42 = 2 * _tmp25 * _tmp7 - _tmp38; - const Scalar _tmp43 = Scalar(1.0) / (math::max(epsilon, innov_var)); + // Intermediate terms (40) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = 2 * state(3, 0); + const Scalar _tmp2 = 2 * state(6, 0); + const Scalar _tmp3 = _tmp2 * state(2, 0); + const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp6 = -state(22, 0) + state(4, 0); + const Scalar _tmp7 = _tmp1 * state(0, 0); + const Scalar _tmp8 = 2 * state(1, 0) * state(2, 0); + const Scalar _tmp9 = _tmp7 + _tmp8; + const Scalar _tmp10 = 2 * state(0, 0); + const Scalar _tmp11 = _tmp1 * state(1, 0) - _tmp10 * state(2, 0); + const Scalar _tmp12 = _tmp0 * _tmp9 + _tmp11 * state(6, 0) + _tmp5 * _tmp6; + const Scalar _tmp13 = + _tmp12 + epsilon * (2 * math::min(0, (((_tmp12) > 0) - ((_tmp12) < 0))) + 1); + const Scalar _tmp14 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp15 = -_tmp7 + _tmp8; + const Scalar _tmp16 = _tmp1 * state(2, 0) + _tmp10 * state(1, 0); + const Scalar _tmp17 = + (_tmp0 * _tmp14 + _tmp15 * _tmp6 + _tmp16 * state(6, 0)) / std::pow(_tmp13, Scalar(2)); + const Scalar _tmp18 = _tmp2 * state(1, 0); + const Scalar _tmp19 = Scalar(1.0) / (_tmp13); + const Scalar _tmp20 = -_tmp17 * (_tmp0 * _tmp1 - _tmp3) + _tmp19 * (-_tmp1 * _tmp6 + _tmp18); + const Scalar _tmp21 = 2 * _tmp0; + const Scalar _tmp22 = _tmp1 * state(6, 0); + const Scalar _tmp23 = 4 * _tmp0; + const Scalar _tmp24 = 2 * _tmp6; + const Scalar _tmp25 = _tmp2 * state(0, 0); + const Scalar _tmp26 = -_tmp17 * (_tmp21 * state(2, 0) + _tmp22) + + _tmp19 * (-_tmp23 * state(1, 0) + _tmp24 * state(2, 0) + _tmp25); + const Scalar _tmp27 = 4 * _tmp6; + const Scalar _tmp28 = -_tmp17 * (_tmp21 * state(1, 0) - _tmp25 - _tmp27 * state(2, 0)) + + _tmp19 * (_tmp22 + _tmp24 * state(1, 0)); + const Scalar _tmp29 = -_tmp17 * (_tmp18 + _tmp21 * state(0, 0) - _tmp27 * state(3, 0)) + + _tmp19 * (-_tmp23 * state(3, 0) - _tmp24 * state(0, 0) + _tmp3); + const Scalar _tmp30 = _tmp17 * _tmp5; + const Scalar _tmp31 = _tmp15 * _tmp19; + const Scalar _tmp32 = -_tmp30 + _tmp31; + const Scalar _tmp33 = _tmp17 * _tmp9; + const Scalar _tmp34 = _tmp14 * _tmp19; + const Scalar _tmp35 = -_tmp33 + _tmp34; + const Scalar _tmp36 = -_tmp11 * _tmp17 + _tmp16 * _tmp19; + const Scalar _tmp37 = _tmp30 - _tmp31; + const Scalar _tmp38 = _tmp33 - _tmp34; + const Scalar _tmp39 = Scalar(1.0) / (math::max(epsilon, innov_var)); // Output terms (2) if (H != nullptr) { @@ -88,92 +87,92 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp26; - _h(1, 0) = _tmp31; - _h(2, 0) = _tmp32; - _h(3, 0) = _tmp33; - _h(4, 0) = _tmp37; - _h(5, 0) = _tmp39; - _h(6, 0) = _tmp40; - _h(22, 0) = _tmp41; - _h(23, 0) = _tmp42; + _h(0, 0) = _tmp20; + _h(1, 0) = _tmp26; + _h(2, 0) = _tmp28; + _h(3, 0) = _tmp29; + _h(4, 0) = _tmp32; + _h(5, 0) = _tmp35; + _h(6, 0) = _tmp36; + _h(22, 0) = _tmp37; + _h(23, 0) = _tmp38; } if (K != nullptr) { matrix::Matrix& _k = (*K); - _k(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + - P(0, 22) * _tmp41 + P(0, 23) * _tmp42 + P(0, 3) * _tmp33 + - P(0, 4) * _tmp37 + P(0, 5) * _tmp39 + P(0, 6) * _tmp40); - _k(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + - P(1, 22) * _tmp41 + P(1, 23) * _tmp42 + P(1, 3) * _tmp33 + - P(1, 4) * _tmp37 + P(1, 5) * _tmp39 + P(1, 6) * _tmp40); - _k(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + - P(2, 22) * _tmp41 + P(2, 23) * _tmp42 + P(2, 3) * _tmp33 + - P(2, 4) * _tmp37 + P(2, 5) * _tmp39 + P(2, 6) * _tmp40); - _k(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + - P(3, 22) * _tmp41 + P(3, 23) * _tmp42 + P(3, 3) * _tmp33 + - P(3, 4) * _tmp37 + P(3, 5) * _tmp39 + P(3, 6) * _tmp40); - _k(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + - P(4, 22) * _tmp41 + P(4, 23) * _tmp42 + P(4, 3) * _tmp33 + - P(4, 4) * _tmp37 + P(4, 5) * _tmp39 + P(4, 6) * _tmp40); - _k(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + - P(5, 22) * _tmp41 + P(5, 23) * _tmp42 + P(5, 3) * _tmp33 + - P(5, 4) * _tmp37 + P(5, 5) * _tmp39 + P(5, 6) * _tmp40); - _k(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + - P(6, 22) * _tmp41 + P(6, 23) * _tmp42 + P(6, 3) * _tmp33 + - P(6, 4) * _tmp37 + P(6, 5) * _tmp39 + P(6, 6) * _tmp40); - _k(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + - P(7, 22) * _tmp41 + P(7, 23) * _tmp42 + P(7, 3) * _tmp33 + - P(7, 4) * _tmp37 + P(7, 5) * _tmp39 + P(7, 6) * _tmp40); - _k(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + - P(8, 22) * _tmp41 + P(8, 23) * _tmp42 + P(8, 3) * _tmp33 + - P(8, 4) * _tmp37 + P(8, 5) * _tmp39 + P(8, 6) * _tmp40); - _k(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + - P(9, 22) * _tmp41 + P(9, 23) * _tmp42 + P(9, 3) * _tmp33 + - P(9, 4) * _tmp37 + P(9, 5) * _tmp39 + P(9, 6) * _tmp40); - _k(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + - P(10, 22) * _tmp41 + P(10, 23) * _tmp42 + P(10, 3) * _tmp33 + - P(10, 4) * _tmp37 + P(10, 5) * _tmp39 + P(10, 6) * _tmp40); - _k(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + - P(11, 22) * _tmp41 + P(11, 23) * _tmp42 + P(11, 3) * _tmp33 + - P(11, 4) * _tmp37 + P(11, 5) * _tmp39 + P(11, 6) * _tmp40); - _k(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + - P(12, 22) * _tmp41 + P(12, 23) * _tmp42 + P(12, 3) * _tmp33 + - P(12, 4) * _tmp37 + P(12, 5) * _tmp39 + P(12, 6) * _tmp40); - _k(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + - P(13, 22) * _tmp41 + P(13, 23) * _tmp42 + P(13, 3) * _tmp33 + - P(13, 4) * _tmp37 + P(13, 5) * _tmp39 + P(13, 6) * _tmp40); - _k(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + - P(14, 22) * _tmp41 + P(14, 23) * _tmp42 + P(14, 3) * _tmp33 + - P(14, 4) * _tmp37 + P(14, 5) * _tmp39 + P(14, 6) * _tmp40); - _k(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + - P(15, 22) * _tmp41 + P(15, 23) * _tmp42 + P(15, 3) * _tmp33 + - P(15, 4) * _tmp37 + P(15, 5) * _tmp39 + P(15, 6) * _tmp40); - _k(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + - P(16, 22) * _tmp41 + P(16, 23) * _tmp42 + P(16, 3) * _tmp33 + - P(16, 4) * _tmp37 + P(16, 5) * _tmp39 + P(16, 6) * _tmp40); - _k(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + - P(17, 22) * _tmp41 + P(17, 23) * _tmp42 + P(17, 3) * _tmp33 + - P(17, 4) * _tmp37 + P(17, 5) * _tmp39 + P(17, 6) * _tmp40); - _k(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + - P(18, 22) * _tmp41 + P(18, 23) * _tmp42 + P(18, 3) * _tmp33 + - P(18, 4) * _tmp37 + P(18, 5) * _tmp39 + P(18, 6) * _tmp40); - _k(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + - P(19, 22) * _tmp41 + P(19, 23) * _tmp42 + P(19, 3) * _tmp33 + - P(19, 4) * _tmp37 + P(19, 5) * _tmp39 + P(19, 6) * _tmp40); - _k(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + - P(20, 22) * _tmp41 + P(20, 23) * _tmp42 + P(20, 3) * _tmp33 + - P(20, 4) * _tmp37 + P(20, 5) * _tmp39 + P(20, 6) * _tmp40); - _k(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + - P(21, 22) * _tmp41 + P(21, 23) * _tmp42 + P(21, 3) * _tmp33 + - P(21, 4) * _tmp37 + P(21, 5) * _tmp39 + P(21, 6) * _tmp40); - _k(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + - P(22, 22) * _tmp41 + P(22, 23) * _tmp42 + P(22, 3) * _tmp33 + - P(22, 4) * _tmp37 + P(22, 5) * _tmp39 + P(22, 6) * _tmp40); - _k(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + - P(23, 22) * _tmp41 + P(23, 23) * _tmp42 + P(23, 3) * _tmp33 + - P(23, 4) * _tmp37 + P(23, 5) * _tmp39 + P(23, 6) * _tmp40); + _k(0, 0) = _tmp39 * (P(0, 0) * _tmp20 + P(0, 1) * _tmp26 + P(0, 2) * _tmp28 + + P(0, 22) * _tmp37 + P(0, 23) * _tmp38 + P(0, 3) * _tmp29 + + P(0, 4) * _tmp32 + P(0, 5) * _tmp35 + P(0, 6) * _tmp36); + _k(1, 0) = _tmp39 * (P(1, 0) * _tmp20 + P(1, 1) * _tmp26 + P(1, 2) * _tmp28 + + P(1, 22) * _tmp37 + P(1, 23) * _tmp38 + P(1, 3) * _tmp29 + + P(1, 4) * _tmp32 + P(1, 5) * _tmp35 + P(1, 6) * _tmp36); + _k(2, 0) = _tmp39 * (P(2, 0) * _tmp20 + P(2, 1) * _tmp26 + P(2, 2) * _tmp28 + + P(2, 22) * _tmp37 + P(2, 23) * _tmp38 + P(2, 3) * _tmp29 + + P(2, 4) * _tmp32 + P(2, 5) * _tmp35 + P(2, 6) * _tmp36); + _k(3, 0) = _tmp39 * (P(3, 0) * _tmp20 + P(3, 1) * _tmp26 + P(3, 2) * _tmp28 + + P(3, 22) * _tmp37 + P(3, 23) * _tmp38 + P(3, 3) * _tmp29 + + P(3, 4) * _tmp32 + P(3, 5) * _tmp35 + P(3, 6) * _tmp36); + _k(4, 0) = _tmp39 * (P(4, 0) * _tmp20 + P(4, 1) * _tmp26 + P(4, 2) * _tmp28 + + P(4, 22) * _tmp37 + P(4, 23) * _tmp38 + P(4, 3) * _tmp29 + + P(4, 4) * _tmp32 + P(4, 5) * _tmp35 + P(4, 6) * _tmp36); + _k(5, 0) = _tmp39 * (P(5, 0) * _tmp20 + P(5, 1) * _tmp26 + P(5, 2) * _tmp28 + + P(5, 22) * _tmp37 + P(5, 23) * _tmp38 + P(5, 3) * _tmp29 + + P(5, 4) * _tmp32 + P(5, 5) * _tmp35 + P(5, 6) * _tmp36); + _k(6, 0) = _tmp39 * (P(6, 0) * _tmp20 + P(6, 1) * _tmp26 + P(6, 2) * _tmp28 + + P(6, 22) * _tmp37 + P(6, 23) * _tmp38 + P(6, 3) * _tmp29 + + P(6, 4) * _tmp32 + P(6, 5) * _tmp35 + P(6, 6) * _tmp36); + _k(7, 0) = _tmp39 * (P(7, 0) * _tmp20 + P(7, 1) * _tmp26 + P(7, 2) * _tmp28 + + P(7, 22) * _tmp37 + P(7, 23) * _tmp38 + P(7, 3) * _tmp29 + + P(7, 4) * _tmp32 + P(7, 5) * _tmp35 + P(7, 6) * _tmp36); + _k(8, 0) = _tmp39 * (P(8, 0) * _tmp20 + P(8, 1) * _tmp26 + P(8, 2) * _tmp28 + + P(8, 22) * _tmp37 + P(8, 23) * _tmp38 + P(8, 3) * _tmp29 + + P(8, 4) * _tmp32 + P(8, 5) * _tmp35 + P(8, 6) * _tmp36); + _k(9, 0) = _tmp39 * (P(9, 0) * _tmp20 + P(9, 1) * _tmp26 + P(9, 2) * _tmp28 + + P(9, 22) * _tmp37 + P(9, 23) * _tmp38 + P(9, 3) * _tmp29 + + P(9, 4) * _tmp32 + P(9, 5) * _tmp35 + P(9, 6) * _tmp36); + _k(10, 0) = _tmp39 * (P(10, 0) * _tmp20 + P(10, 1) * _tmp26 + P(10, 2) * _tmp28 + + P(10, 22) * _tmp37 + P(10, 23) * _tmp38 + P(10, 3) * _tmp29 + + P(10, 4) * _tmp32 + P(10, 5) * _tmp35 + P(10, 6) * _tmp36); + _k(11, 0) = _tmp39 * (P(11, 0) * _tmp20 + P(11, 1) * _tmp26 + P(11, 2) * _tmp28 + + P(11, 22) * _tmp37 + P(11, 23) * _tmp38 + P(11, 3) * _tmp29 + + P(11, 4) * _tmp32 + P(11, 5) * _tmp35 + P(11, 6) * _tmp36); + _k(12, 0) = _tmp39 * (P(12, 0) * _tmp20 + P(12, 1) * _tmp26 + P(12, 2) * _tmp28 + + P(12, 22) * _tmp37 + P(12, 23) * _tmp38 + P(12, 3) * _tmp29 + + P(12, 4) * _tmp32 + P(12, 5) * _tmp35 + P(12, 6) * _tmp36); + _k(13, 0) = _tmp39 * (P(13, 0) * _tmp20 + P(13, 1) * _tmp26 + P(13, 2) * _tmp28 + + P(13, 22) * _tmp37 + P(13, 23) * _tmp38 + P(13, 3) * _tmp29 + + P(13, 4) * _tmp32 + P(13, 5) * _tmp35 + P(13, 6) * _tmp36); + _k(14, 0) = _tmp39 * (P(14, 0) * _tmp20 + P(14, 1) * _tmp26 + P(14, 2) * _tmp28 + + P(14, 22) * _tmp37 + P(14, 23) * _tmp38 + P(14, 3) * _tmp29 + + P(14, 4) * _tmp32 + P(14, 5) * _tmp35 + P(14, 6) * _tmp36); + _k(15, 0) = _tmp39 * (P(15, 0) * _tmp20 + P(15, 1) * _tmp26 + P(15, 2) * _tmp28 + + P(15, 22) * _tmp37 + P(15, 23) * _tmp38 + P(15, 3) * _tmp29 + + P(15, 4) * _tmp32 + P(15, 5) * _tmp35 + P(15, 6) * _tmp36); + _k(16, 0) = _tmp39 * (P(16, 0) * _tmp20 + P(16, 1) * _tmp26 + P(16, 2) * _tmp28 + + P(16, 22) * _tmp37 + P(16, 23) * _tmp38 + P(16, 3) * _tmp29 + + P(16, 4) * _tmp32 + P(16, 5) * _tmp35 + P(16, 6) * _tmp36); + _k(17, 0) = _tmp39 * (P(17, 0) * _tmp20 + P(17, 1) * _tmp26 + P(17, 2) * _tmp28 + + P(17, 22) * _tmp37 + P(17, 23) * _tmp38 + P(17, 3) * _tmp29 + + P(17, 4) * _tmp32 + P(17, 5) * _tmp35 + P(17, 6) * _tmp36); + _k(18, 0) = _tmp39 * (P(18, 0) * _tmp20 + P(18, 1) * _tmp26 + P(18, 2) * _tmp28 + + P(18, 22) * _tmp37 + P(18, 23) * _tmp38 + P(18, 3) * _tmp29 + + P(18, 4) * _tmp32 + P(18, 5) * _tmp35 + P(18, 6) * _tmp36); + _k(19, 0) = _tmp39 * (P(19, 0) * _tmp20 + P(19, 1) * _tmp26 + P(19, 2) * _tmp28 + + P(19, 22) * _tmp37 + P(19, 23) * _tmp38 + P(19, 3) * _tmp29 + + P(19, 4) * _tmp32 + P(19, 5) * _tmp35 + P(19, 6) * _tmp36); + _k(20, 0) = _tmp39 * (P(20, 0) * _tmp20 + P(20, 1) * _tmp26 + P(20, 2) * _tmp28 + + P(20, 22) * _tmp37 + P(20, 23) * _tmp38 + P(20, 3) * _tmp29 + + P(20, 4) * _tmp32 + P(20, 5) * _tmp35 + P(20, 6) * _tmp36); + _k(21, 0) = _tmp39 * (P(21, 0) * _tmp20 + P(21, 1) * _tmp26 + P(21, 2) * _tmp28 + + P(21, 22) * _tmp37 + P(21, 23) * _tmp38 + P(21, 3) * _tmp29 + + P(21, 4) * _tmp32 + P(21, 5) * _tmp35 + P(21, 6) * _tmp36); + _k(22, 0) = _tmp39 * (P(22, 0) * _tmp20 + P(22, 1) * _tmp26 + P(22, 2) * _tmp28 + + P(22, 22) * _tmp37 + P(22, 23) * _tmp38 + P(22, 3) * _tmp29 + + P(22, 4) * _tmp32 + P(22, 5) * _tmp35 + P(22, 6) * _tmp36); + _k(23, 0) = _tmp39 * (P(23, 0) * _tmp20 + P(23, 1) * _tmp26 + P(23, 2) * _tmp28 + + P(23, 22) * _tmp37 + P(23, 23) * _tmp38 + P(23, 3) * _tmp29 + + P(23, 4) * _tmp32 + P(23, 5) * _tmp35 + P(23, 6) * _tmp36); } } // 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 3d181c5c80..2126bf1f23 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,95 +30,93 @@ 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: 276 + // Total ops: 269 // Input arrays - // Intermediate terms (44) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2; - const Scalar _tmp4 = -state(22, 0) + state(4, 0); - const Scalar _tmp5 = state(0, 0) * state(3, 0); - const Scalar _tmp6 = state(1, 0) * state(2, 0); - const Scalar _tmp7 = _tmp5 + _tmp6; + // Intermediate terms (39) + 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(3, 0); + const Scalar _tmp4 = _tmp3 * state(0, 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 * _tmp8; - const Scalar _tmp10 = state(0, 0) * state(2, 0); - const Scalar _tmp11 = state(1, 0) * state(3, 0); - const Scalar _tmp12 = 2 * state(6, 0); - const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9; - const Scalar _tmp14 = - _tmp13 + epsilon * (2 * math::min(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1); - const Scalar _tmp15 = Scalar(1.0) / (_tmp14); - const Scalar _tmp16 = _tmp0 - _tmp1 + _tmp2; - const Scalar _tmp17 = -_tmp5 + _tmp6; - const Scalar _tmp18 = 2 * _tmp4; - const Scalar _tmp19 = state(2, 0) * state(3, 0); - const Scalar _tmp20 = state(0, 0) * state(1, 0); - const Scalar _tmp21 = _tmp12 * (_tmp19 + _tmp20) + _tmp16 * _tmp8 + _tmp17 * _tmp18; - const Scalar _tmp22 = _tmp9 * state(1, 0); - const Scalar _tmp23 = _tmp18 * state(2, 0); - const Scalar _tmp24 = _tmp12 * state(0, 0); - const Scalar _tmp25 = _tmp12 * state(3, 0) + _tmp18 * state(1, 0) + _tmp9 * state(2, 0); - const Scalar _tmp26 = _tmp21 / std::pow(_tmp14, Scalar(2)); - const Scalar _tmp27 = _tmp15 * (-_tmp22 + _tmp23 + _tmp24) - _tmp25 * _tmp26; - const Scalar _tmp28 = _tmp15 * _tmp16; - const Scalar _tmp29 = 2 * _tmp5; - const Scalar _tmp30 = 2 * _tmp6; - const Scalar _tmp31 = -_tmp26 * (_tmp29 + _tmp30) + _tmp28; - const Scalar _tmp32 = _tmp12 * state(1, 0) - _tmp18 * state(3, 0) + _tmp9 * state(0, 0); - const Scalar _tmp33 = _tmp18 * state(0, 0); - const Scalar _tmp34 = _tmp9 * state(3, 0); - const Scalar _tmp35 = _tmp12 * state(2, 0); - const Scalar _tmp36 = _tmp15 * _tmp32 - _tmp26 * (_tmp33 + _tmp34 - _tmp35); - const Scalar _tmp37 = 2 * _tmp26 * _tmp7 - _tmp28; - const Scalar _tmp38 = _tmp26 * _tmp3; - const Scalar _tmp39 = -2 * _tmp15 * _tmp17 + _tmp38; - const Scalar _tmp40 = _tmp15 * _tmp25 - _tmp26 * (_tmp22 - _tmp23 - _tmp24); - const Scalar _tmp41 = _tmp15 * (-_tmp33 - _tmp34 + _tmp35) - _tmp26 * _tmp32; - const Scalar _tmp42 = _tmp15 * (2 * _tmp19 + 2 * _tmp20) - _tmp26 * (-2 * _tmp10 + 2 * _tmp11); - const Scalar _tmp43 = _tmp15 * (-_tmp29 + _tmp30) - _tmp38; + const Scalar _tmp9 = 2 * state(2, 0); + const Scalar _tmp10 = _tmp3 * state(1, 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 = _tmp3 * state(2, 0) + _tmp5 * state(0, 0); + const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0); + const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2)); + const Scalar _tmp19 = _tmp1 * _tmp18; + const Scalar _tmp20 = _tmp13 * _tmp15; + const Scalar _tmp21 = -_tmp19 + _tmp20; + const Scalar _tmp22 = _tmp3 * state(6, 0); + const Scalar _tmp23 = 4 * _tmp8; + const Scalar _tmp24 = 2 * state(0, 0); + const Scalar _tmp25 = _tmp24 * state(6, 0); + const Scalar _tmp26 = + _tmp13 * (_tmp2 * _tmp9 - _tmp23 * state(1, 0) + _tmp25) - _tmp18 * (_tmp22 + _tmp8 * _tmp9); + const Scalar _tmp27 = -_tmp10 * _tmp18 + _tmp13 * _tmp16; + const Scalar _tmp28 = 4 * _tmp2; + const Scalar _tmp29 = + _tmp13 * (_tmp2 * _tmp5 + _tmp22) - _tmp18 * (-_tmp25 - _tmp28 * state(2, 0) + _tmp5 * _tmp8); + const Scalar _tmp30 = _tmp9 * state(6, 0); + const Scalar _tmp31 = _tmp5 * state(6, 0); + const Scalar _tmp32 = _tmp13 * (-_tmp2 * _tmp3 + _tmp31) - _tmp18 * (_tmp3 * _tmp8 - _tmp30); + const Scalar _tmp33 = _tmp19 - _tmp20; + const Scalar _tmp34 = _tmp18 * _tmp7; + const Scalar _tmp35 = _tmp13 * _tmp14; + const Scalar _tmp36 = -_tmp34 + _tmp35; + const Scalar _tmp37 = _tmp34 - _tmp35; + const Scalar _tmp38 = _tmp13 * (-_tmp2 * _tmp24 - _tmp23 * state(3, 0) + _tmp30) - + _tmp18 * (_tmp24 * _tmp8 - _tmp28 * state(3, 0) + _tmp31); // Output terms (2) if (innov != nullptr) { Scalar& _innov = (*innov); - _innov = _tmp15 * _tmp21; + _innov = _tmp13 * _tmp17; } if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); _innov_var = R + - _tmp27 * (P(0, 1) * _tmp36 + P(1, 1) * _tmp27 + P(2, 1) * _tmp40 + - P(22, 1) * _tmp39 + P(23, 1) * _tmp37 + P(3, 1) * _tmp41 + - P(4, 1) * _tmp43 + P(5, 1) * _tmp31 + P(6, 1) * _tmp42) + - _tmp31 * (P(0, 5) * _tmp36 + P(1, 5) * _tmp27 + P(2, 5) * _tmp40 + - P(22, 5) * _tmp39 + P(23, 5) * _tmp37 + P(3, 5) * _tmp41 + - P(4, 5) * _tmp43 + P(5, 5) * _tmp31 + P(6, 5) * _tmp42) + - _tmp36 * (P(0, 0) * _tmp36 + P(1, 0) * _tmp27 + P(2, 0) * _tmp40 + - P(22, 0) * _tmp39 + P(23, 0) * _tmp37 + P(3, 0) * _tmp41 + - P(4, 0) * _tmp43 + P(5, 0) * _tmp31 + P(6, 0) * _tmp42) + - _tmp37 * (P(0, 23) * _tmp36 + P(1, 23) * _tmp27 + P(2, 23) * _tmp40 + - P(22, 23) * _tmp39 + P(23, 23) * _tmp37 + P(3, 23) * _tmp41 + - P(4, 23) * _tmp43 + P(5, 23) * _tmp31 + P(6, 23) * _tmp42) + - _tmp39 * (P(0, 22) * _tmp36 + P(1, 22) * _tmp27 + P(2, 22) * _tmp40 + - P(22, 22) * _tmp39 + P(23, 22) * _tmp37 + P(3, 22) * _tmp41 + - P(4, 22) * _tmp43 + P(5, 22) * _tmp31 + P(6, 22) * _tmp42) + - _tmp40 * (P(0, 2) * _tmp36 + P(1, 2) * _tmp27 + P(2, 2) * _tmp40 + - P(22, 2) * _tmp39 + P(23, 2) * _tmp37 + P(3, 2) * _tmp41 + - P(4, 2) * _tmp43 + P(5, 2) * _tmp31 + P(6, 2) * _tmp42) + - _tmp41 * (P(0, 3) * _tmp36 + P(1, 3) * _tmp27 + P(2, 3) * _tmp40 + - P(22, 3) * _tmp39 + P(23, 3) * _tmp37 + P(3, 3) * _tmp41 + - P(4, 3) * _tmp43 + P(5, 3) * _tmp31 + P(6, 3) * _tmp42) + - _tmp42 * (P(0, 6) * _tmp36 + P(1, 6) * _tmp27 + P(2, 6) * _tmp40 + - P(22, 6) * _tmp39 + P(23, 6) * _tmp37 + P(3, 6) * _tmp41 + - P(4, 6) * _tmp43 + P(5, 6) * _tmp31 + P(6, 6) * _tmp42) + - _tmp43 * (P(0, 4) * _tmp36 + P(1, 4) * _tmp27 + P(2, 4) * _tmp40 + - P(22, 4) * _tmp39 + P(23, 4) * _tmp37 + P(3, 4) * _tmp41 + - P(4, 4) * _tmp43 + P(5, 4) * _tmp31 + P(6, 4) * _tmp42); + _tmp21 * (P(0, 4) * _tmp32 + P(1, 4) * _tmp26 + P(2, 4) * _tmp29 + + P(22, 4) * _tmp33 + P(23, 4) * _tmp37 + P(3, 4) * _tmp38 + + P(4, 4) * _tmp21 + P(5, 4) * _tmp36 + P(6, 4) * _tmp27) + + _tmp26 * (P(0, 1) * _tmp32 + P(1, 1) * _tmp26 + P(2, 1) * _tmp29 + + P(22, 1) * _tmp33 + P(23, 1) * _tmp37 + P(3, 1) * _tmp38 + + P(4, 1) * _tmp21 + P(5, 1) * _tmp36 + P(6, 1) * _tmp27) + + _tmp27 * (P(0, 6) * _tmp32 + P(1, 6) * _tmp26 + P(2, 6) * _tmp29 + + P(22, 6) * _tmp33 + P(23, 6) * _tmp37 + P(3, 6) * _tmp38 + + P(4, 6) * _tmp21 + P(5, 6) * _tmp36 + P(6, 6) * _tmp27) + + _tmp29 * (P(0, 2) * _tmp32 + P(1, 2) * _tmp26 + P(2, 2) * _tmp29 + + P(22, 2) * _tmp33 + P(23, 2) * _tmp37 + P(3, 2) * _tmp38 + + P(4, 2) * _tmp21 + P(5, 2) * _tmp36 + P(6, 2) * _tmp27) + + _tmp32 * (P(0, 0) * _tmp32 + P(1, 0) * _tmp26 + P(2, 0) * _tmp29 + + P(22, 0) * _tmp33 + P(23, 0) * _tmp37 + P(3, 0) * _tmp38 + + P(4, 0) * _tmp21 + P(5, 0) * _tmp36 + P(6, 0) * _tmp27) + + _tmp33 * (P(0, 22) * _tmp32 + P(1, 22) * _tmp26 + P(2, 22) * _tmp29 + + P(22, 22) * _tmp33 + P(23, 22) * _tmp37 + P(3, 22) * _tmp38 + + P(4, 22) * _tmp21 + P(5, 22) * _tmp36 + P(6, 22) * _tmp27) + + _tmp36 * (P(0, 5) * _tmp32 + P(1, 5) * _tmp26 + P(2, 5) * _tmp29 + + P(22, 5) * _tmp33 + P(23, 5) * _tmp37 + P(3, 5) * _tmp38 + + P(4, 5) * _tmp21 + P(5, 5) * _tmp36 + P(6, 5) * _tmp27) + + _tmp37 * (P(0, 23) * _tmp32 + P(1, 23) * _tmp26 + P(2, 23) * _tmp29 + + P(22, 23) * _tmp33 + P(23, 23) * _tmp37 + P(3, 23) * _tmp38 + + P(4, 23) * _tmp21 + P(5, 23) * _tmp36 + P(6, 23) * _tmp27) + + _tmp38 * (P(0, 3) * _tmp32 + P(1, 3) * _tmp26 + P(2, 3) * _tmp29 + + P(22, 3) * _tmp33 + P(23, 3) * _tmp37 + P(3, 3) * _tmp38 + + P(4, 3) * _tmp21 + P(5, 3) * _tmp36 + P(6, 3) * _tmp27); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h index 30c08f9c25..45946c4ebc 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h @@ -30,33 +30,37 @@ void ComputeYaw312InnovVarAndH(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: 75 + // Total ops: 73 // Input arrays - // Intermediate terms (11) - const Scalar _tmp0 = -state(0, 0) * state(3, 0) + 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)); - const Scalar _tmp2 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5)); - const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); - const Scalar _tmp4 = 4 * _tmp0 / _tmp3; - const Scalar _tmp5 = 2 / _tmp2; - const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3); - const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0)); - const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) - _tmp5 * state(1, 0)); - const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0)); - const Scalar _tmp10 = _tmp6 * (_tmp4 * state(0, 0) + _tmp5 * state(3, 0)); + // Intermediate terms (15) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = 2 * state(2, 0); + const Scalar _tmp2 = -_tmp0 * state(3, 0) + _tmp1 * state(1, 0); + const Scalar _tmp3 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp6 = 4 * _tmp2 / _tmp5; + const Scalar _tmp7 = Scalar(1.0) / (_tmp4); + const Scalar _tmp8 = Scalar(1.0) / (std::pow(_tmp2, Scalar(2)) + _tmp5); + const Scalar _tmp9 = _tmp5 * _tmp8; + const Scalar _tmp10 = _tmp9 * (-_tmp1 * _tmp7 - _tmp6 * state(1, 0)); + const Scalar _tmp11 = _tmp9 * (_tmp0 * _tmp7 - _tmp6 * state(3, 0)); + const Scalar _tmp12 = 2 * _tmp4 * _tmp8; + const Scalar _tmp13 = _tmp12 * state(3, 0); + const Scalar _tmp14 = _tmp12 * state(1, 0); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R + - _tmp10 * (P(0, 0) * _tmp10 + P(1, 0) * _tmp7 + P(2, 0) * _tmp8 + P(3, 0) * _tmp9) + - _tmp7 * (P(0, 1) * _tmp10 + P(1, 1) * _tmp7 + P(2, 1) * _tmp8 + P(3, 1) * _tmp9) + - _tmp8 * (P(0, 2) * _tmp10 + P(1, 2) * _tmp7 + P(2, 2) * _tmp8 + P(3, 2) * _tmp9) + - _tmp9 * (P(0, 3) * _tmp10 + P(1, 3) * _tmp7 + P(2, 3) * _tmp8 + P(3, 3) * _tmp9); + _innov_var = + R + _tmp10 * (P(0, 1) * _tmp13 + P(1, 1) * _tmp10 - P(2, 1) * _tmp14 + P(3, 1) * _tmp11) + + _tmp11 * (P(0, 3) * _tmp13 + P(1, 3) * _tmp10 - P(2, 3) * _tmp14 + P(3, 3) * _tmp11) + + _tmp13 * (P(0, 0) * _tmp13 + P(1, 0) * _tmp10 - P(2, 0) * _tmp14 + P(3, 0) * _tmp11) - + _tmp14 * (P(0, 2) * _tmp13 + P(1, 2) * _tmp10 - P(2, 2) * _tmp14 + P(3, 2) * _tmp11); } if (H != nullptr) { @@ -64,10 +68,10 @@ void ComputeYaw312InnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp10; - _h(1, 0) = _tmp7; - _h(2, 0) = _tmp8; - _h(3, 0) = _tmp9; + _h(0, 0) = _tmp13; + _h(1, 0) = _tmp10; + _h(2, 0) = -_tmp14; + _h(3, 0) = _tmp11; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h index 862b67c7c4..4caba10bd7 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h @@ -30,33 +30,37 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix& sta const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 83 + // Total ops: 73 // Input arrays - // Intermediate terms (11) - 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 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0); - const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5)); - const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); - const Scalar _tmp4 = 2 * _tmp0 / _tmp3; - const Scalar _tmp5 = 2 / _tmp2; - const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3); - const Scalar _tmp7 = _tmp6 * (_tmp4 * state(1, 0) - _tmp5 * state(2, 0)); - const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0)); - const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0)); - const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(3, 0) - _tmp5 * state(0, 0)); + // Intermediate terms (15) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + const Scalar _tmp2 = 2 * state(2, 0); + const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2 * state(1, 0); + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp6 = _tmp1 / _tmp5; + const Scalar _tmp7 = 4 / _tmp4; + const Scalar _tmp8 = Scalar(1.0) / (std::pow(_tmp1, Scalar(2)) + _tmp5); + const Scalar _tmp9 = _tmp5 * _tmp8; + const Scalar _tmp10 = _tmp9 * (-_tmp0 * _tmp6 + _tmp7 * state(3, 0)); + const Scalar _tmp11 = 2 * _tmp1 * _tmp8; + const Scalar _tmp12 = _tmp11 * state(3, 0); + const Scalar _tmp13 = _tmp11 * state(1, 0); + const Scalar _tmp14 = _tmp9 * (_tmp2 * _tmp6 + _tmp7 * state(1, 0)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); _innov_var = - R - _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp8 - P(2, 0) * _tmp7 - P(3, 0) * _tmp9) - - _tmp7 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp8 - P(2, 2) * _tmp7 - P(3, 2) * _tmp9) - - _tmp8 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp8 - P(2, 1) * _tmp7 - P(3, 1) * _tmp9) - - _tmp9 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp8 - P(2, 3) * _tmp7 - P(3, 3) * _tmp9); + R - _tmp10 * (P(0, 3) * _tmp12 - P(1, 3) * _tmp14 - P(2, 3) * _tmp13 - P(3, 3) * _tmp10) + + _tmp12 * (P(0, 0) * _tmp12 - P(1, 0) * _tmp14 - P(2, 0) * _tmp13 - P(3, 0) * _tmp10) - + _tmp13 * (P(0, 2) * _tmp12 - P(1, 2) * _tmp14 - P(2, 2) * _tmp13 - P(3, 2) * _tmp10) - + _tmp14 * (P(0, 1) * _tmp12 - P(1, 1) * _tmp14 - P(2, 1) * _tmp13 - P(3, 1) * _tmp10); } if (H != nullptr) { @@ -64,10 +68,10 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix& sta _h.setZero(); - _h(0, 0) = -_tmp10; - _h(1, 0) = -_tmp8; - _h(2, 0) = -_tmp7; - _h(3, 0) = -_tmp9; + _h(0, 0) = _tmp12; + _h(1, 0) = -_tmp14; + _h(2, 0) = -_tmp13; + _h(3, 0) = -_tmp10; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h index 066b5166da..b3aef591f4 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h @@ -30,33 +30,37 @@ void ComputeYaw321InnovVarAndH(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: 74 + // Total ops: 70 // Input arrays - // Intermediate terms (11) - const Scalar _tmp0 = state(0, 0) * state(3, 0) + 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)); - const Scalar _tmp2 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5)); - const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); - const Scalar _tmp4 = 4 * _tmp0 / _tmp3; - const Scalar _tmp5 = 2 / _tmp2; - const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3); - const Scalar _tmp7 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0)); - const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(1, 0) + _tmp5 * state(2, 0)); - const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0)); - const Scalar _tmp10 = _tmp6 * (_tmp4 * state(3, 0) + _tmp5 * state(0, 0)); + // Intermediate terms (15) + const Scalar _tmp0 = 2 * state(3, 0); + const Scalar _tmp1 = 2 * state(2, 0); + const Scalar _tmp2 = _tmp0 * state(0, 0) + _tmp1 * state(1, 0); + const Scalar _tmp3 = + -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp6 = Scalar(1.0) / (std::pow(_tmp2, Scalar(2)) + _tmp5); + const Scalar _tmp7 = _tmp4 * _tmp6; + const Scalar _tmp8 = _tmp0 * _tmp7; + const Scalar _tmp9 = _tmp1 * _tmp7; + const Scalar _tmp10 = 4 * _tmp2 / _tmp5; + const Scalar _tmp11 = 2 / _tmp4; + const Scalar _tmp12 = _tmp5 * _tmp6; + const Scalar _tmp13 = _tmp12 * (_tmp10 * state(3, 0) + _tmp11 * state(0, 0)); + const Scalar _tmp14 = _tmp12 * (_tmp10 * state(2, 0) + _tmp11 * state(1, 0)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R + - _tmp10 * (P(0, 3) * _tmp9 + P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + P(3, 3) * _tmp10) + - _tmp7 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp8 + P(2, 2) * _tmp7 + P(3, 2) * _tmp10) + - _tmp8 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp8 + P(2, 1) * _tmp7 + P(3, 1) * _tmp10) + - _tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + P(3, 0) * _tmp10); + _innov_var = + R + _tmp13 * (P(0, 3) * _tmp8 + P(1, 3) * _tmp9 + P(2, 3) * _tmp14 + P(3, 3) * _tmp13) + + _tmp14 * (P(0, 2) * _tmp8 + P(1, 2) * _tmp9 + P(2, 2) * _tmp14 + P(3, 2) * _tmp13) + + _tmp8 * (P(0, 0) * _tmp8 + P(1, 0) * _tmp9 + P(2, 0) * _tmp14 + P(3, 0) * _tmp13) + + _tmp9 * (P(0, 1) * _tmp8 + P(1, 1) * _tmp9 + P(2, 1) * _tmp14 + P(3, 1) * _tmp13); } if (H != nullptr) { @@ -64,10 +68,10 @@ void ComputeYaw321InnovVarAndH(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp9; - _h(1, 0) = _tmp8; - _h(2, 0) = _tmp7; - _h(3, 0) = _tmp10; + _h(0, 0) = _tmp8; + _h(1, 0) = _tmp9; + _h(2, 0) = _tmp14; + _h(3, 0) = _tmp13; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h index d29921a775..b2bbca9c78 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h @@ -30,33 +30,37 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix& sta const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, matrix::Matrix* const H = nullptr) { - // Total ops: 84 + // Total ops: 75 // Input arrays - // Intermediate terms (11) - 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 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0); - const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5)); - const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); - const Scalar _tmp4 = 2 * _tmp0 / _tmp3; - const Scalar _tmp5 = 2 / _tmp2; - const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3); - const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(0, 0) - _tmp5 * state(3, 0)); - const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(2, 0) + _tmp5 * state(1, 0)); - const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0)); - const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0)); + // Intermediate terms (15) + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = + -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + const Scalar _tmp2 = 2 * state(0, 0); + const Scalar _tmp3 = _tmp0 * state(1, 0) + _tmp2 * state(3, 0); + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp6 = Scalar(1.0) / (std::pow(_tmp1, Scalar(2)) + _tmp5); + const Scalar _tmp7 = _tmp1 * _tmp6; + const Scalar _tmp8 = _tmp0 * _tmp7; + const Scalar _tmp9 = 2 * _tmp7 * state(3, 0); + const Scalar _tmp10 = _tmp1 / _tmp5; + const Scalar _tmp11 = 4 / _tmp4; + const Scalar _tmp12 = _tmp5 * _tmp6; + const Scalar _tmp13 = _tmp12 * (-_tmp10 * _tmp2 - _tmp11 * state(3, 0)); + const Scalar _tmp14 = _tmp12 * (-2 * _tmp10 * state(1, 0) - _tmp11 * state(2, 0)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); _innov_var = - R - _tmp10 * (-P(0, 2) * _tmp9 - P(1, 2) * _tmp8 - P(2, 2) * _tmp10 - P(3, 2) * _tmp7) - - _tmp7 * (-P(0, 3) * _tmp9 - P(1, 3) * _tmp8 - P(2, 3) * _tmp10 - P(3, 3) * _tmp7) - - _tmp8 * (-P(0, 1) * _tmp9 - P(1, 1) * _tmp8 - P(2, 1) * _tmp10 - P(3, 1) * _tmp7) - - _tmp9 * (-P(0, 0) * _tmp9 - P(1, 0) * _tmp8 - P(2, 0) * _tmp10 - P(3, 0) * _tmp7); + R - _tmp13 * (P(0, 3) * _tmp9 + P(1, 3) * _tmp8 - P(2, 3) * _tmp14 - P(3, 3) * _tmp13) - + _tmp14 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp8 - P(2, 2) * _tmp14 - P(3, 2) * _tmp13) + + _tmp8 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp8 - P(2, 1) * _tmp14 - P(3, 1) * _tmp13) + + _tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp8 - P(2, 0) * _tmp14 - P(3, 0) * _tmp13); } if (H != nullptr) { @@ -64,10 +68,10 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix& sta _h.setZero(); - _h(0, 0) = -_tmp9; - _h(1, 0) = -_tmp8; - _h(2, 0) = -_tmp10; - _h(3, 0) = -_tmp7; + _h(0, 0) = _tmp9; + _h(1, 0) = _tmp8; + _h(2, 0) = -_tmp14; + _h(3, 0) = -_tmp13; } } // 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 dfe663070b..84526a15d7 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,7 +34,7 @@ void PredictCovariance(const matrix::Matrix& state, const matrix::Matrix& d_vel_var, const matrix::Matrix& d_ang, const Scalar d_ang_var, const Scalar dt, matrix::Matrix* const P_new = nullptr) { - // Total ops: 2887 + // Total ops: 2876 // Input arrays @@ -59,262 +59,262 @@ void PredictCovariance(const matrix::Matrix& state, P(12, 12) * _tmp9 + P(2, 12) * _tmp5 + P(3, 12) * _tmp2; const Scalar _tmp15 = P(0, 2) + P(1, 2) * _tmp8 + P(10, 2) * _tmp11 + P(11, 2) * _tmp10 + P(12, 2) * _tmp9 + P(2, 2) * _tmp5 + P(3, 2) * _tmp2; - const Scalar _tmp16 = P(0, 3) + P(1, 3) * _tmp8 + P(10, 3) * _tmp11 + P(11, 3) * _tmp10 + - P(12, 3) * _tmp9 + P(2, 3) * _tmp5 + P(3, 3) * _tmp2; - const Scalar _tmp17 = P(0, 1) + P(1, 1) * _tmp8 + P(10, 1) * _tmp11 + P(11, 1) * _tmp10 + - P(12, 1) * _tmp9 + P(2, 1) * _tmp5 + P(3, 1) * _tmp2; - const Scalar _tmp18 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp19 = Scalar(0.25) * d_ang_var; - const Scalar _tmp20 = _tmp18 * _tmp19; - const Scalar _tmp21 = P(0, 0) + P(1, 0) * _tmp8 + P(10, 0) * _tmp11 + P(11, 0) * _tmp10 + - P(12, 0) * _tmp9 + P(2, 0) * _tmp5 + P(3, 0) * _tmp2; - const Scalar _tmp22 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp23 = _tmp19 * _tmp22; - const Scalar _tmp24 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp25 = _tmp19 * _tmp24; - const Scalar _tmp26 = _tmp23 + _tmp25; - const Scalar _tmp27 = Scalar(0.5) * state(0, 0); - const Scalar _tmp28 = state(0, 0) * state(1, 0); - const Scalar _tmp29 = _tmp0 - _tmp1; - const Scalar _tmp30 = _tmp6 - _tmp7; - const Scalar _tmp31 = P(0, 12) * _tmp30 + P(1, 12) - P(10, 12) * _tmp27 + P(11, 12) * _tmp9 - - P(12, 12) * _tmp10 + P(2, 12) * _tmp29 + P(3, 12) * _tmp5; - const Scalar _tmp32 = P(0, 11) * _tmp30 + P(1, 11) - P(10, 11) * _tmp27 + P(11, 11) * _tmp9 - - P(12, 11) * _tmp10 + P(2, 11) * _tmp29 + P(3, 11) * _tmp5; - const Scalar _tmp33 = P(0, 10) * _tmp30 + P(1, 10) - P(10, 10) * _tmp27 + P(11, 10) * _tmp9 - - P(12, 10) * _tmp10 + P(2, 10) * _tmp29 + P(3, 10) * _tmp5; - const Scalar _tmp34 = P(0, 3) * _tmp30 + P(1, 3) - P(10, 3) * _tmp27 + P(11, 3) * _tmp9 - - P(12, 3) * _tmp10 + P(2, 3) * _tmp29 + P(3, 3) * _tmp5; - const Scalar _tmp35 = P(0, 2) * _tmp30 + P(1, 2) - P(10, 2) * _tmp27 + P(11, 2) * _tmp9 - - P(12, 2) * _tmp10 + P(2, 2) * _tmp29 + P(3, 2) * _tmp5; - const Scalar _tmp36 = P(0, 0) * _tmp30 + P(1, 0) - P(10, 0) * _tmp27 + P(11, 0) * _tmp9 - - P(12, 0) * _tmp10 + P(2, 0) * _tmp29 + P(3, 0) * _tmp5; - const Scalar _tmp37 = P(0, 1) * _tmp30 + P(1, 1) - P(10, 1) * _tmp27 + P(11, 1) * _tmp9 - - P(12, 1) * _tmp10 + P(2, 1) * _tmp29 + P(3, 1) * _tmp5; - const Scalar _tmp38 = _tmp19 * std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp39 = _tmp20 + _tmp38; - const Scalar _tmp40 = state(0, 0) * state(2, 0); - const Scalar _tmp41 = _tmp3 - _tmp4; - const Scalar _tmp42 = state(1, 0) * state(2, 0); - const Scalar _tmp43 = P(0, 12) * _tmp41 + P(1, 12) * _tmp2 - P(10, 12) * _tmp9 - - P(11, 12) * _tmp27 + P(12, 12) * _tmp11 + P(2, 12) + P(3, 12) * _tmp30; - const Scalar _tmp44 = P(0, 11) * _tmp41 + P(1, 11) * _tmp2 - P(10, 11) * _tmp9 - - P(11, 11) * _tmp27 + P(12, 11) * _tmp11 + P(2, 11) + P(3, 11) * _tmp30; - const Scalar _tmp45 = P(0, 10) * _tmp41 + P(1, 10) * _tmp2 - P(10, 10) * _tmp9 - - P(11, 10) * _tmp27 + P(12, 10) * _tmp11 + P(2, 10) + P(3, 10) * _tmp30; - const Scalar _tmp46 = P(0, 0) * _tmp41 + P(1, 0) * _tmp2 - P(10, 0) * _tmp9 - P(11, 0) * _tmp27 + - P(12, 0) * _tmp11 + P(2, 0) + P(3, 0) * _tmp30; - const Scalar _tmp47 = P(0, 1) * _tmp41 + P(1, 1) * _tmp2 - P(10, 1) * _tmp9 - P(11, 1) * _tmp27 + - P(12, 1) * _tmp11 + P(2, 1) + P(3, 1) * _tmp30; - const Scalar _tmp48 = P(0, 3) * _tmp41 + P(1, 3) * _tmp2 - P(10, 3) * _tmp9 - P(11, 3) * _tmp27 + - P(12, 3) * _tmp11 + P(2, 3) + P(3, 3) * _tmp30; - const Scalar _tmp49 = P(0, 2) * _tmp41 + P(1, 2) * _tmp2 - P(10, 2) * _tmp9 - P(11, 2) * _tmp27 + - P(12, 2) * _tmp11 + P(2, 2) + P(3, 2) * _tmp30; - const Scalar _tmp50 = state(0, 0) * state(3, 0); - const Scalar _tmp51 = state(1, 0) * state(3, 0); - const Scalar _tmp52 = state(2, 0) * state(3, 0); - const Scalar _tmp53 = P(0, 12) * _tmp29 + P(1, 12) * _tmp41 + P(10, 12) * _tmp10 - - P(11, 12) * _tmp11 - P(12, 12) * _tmp27 + P(2, 12) * _tmp8 + P(3, 12); - const Scalar _tmp54 = P(0, 11) * _tmp29 + P(1, 11) * _tmp41 + P(10, 11) * _tmp10 - - P(11, 11) * _tmp11 - P(12, 11) * _tmp27 + P(2, 11) * _tmp8 + P(3, 11); - const Scalar _tmp55 = P(0, 10) * _tmp29 + P(1, 10) * _tmp41 + P(10, 10) * _tmp10 - - P(11, 10) * _tmp11 - P(12, 10) * _tmp27 + P(2, 10) * _tmp8 + P(3, 10); - const Scalar _tmp56 = P(0, 2) * _tmp29 + P(1, 2) * _tmp41 + P(10, 2) * _tmp10 - - P(11, 2) * _tmp11 - P(12, 2) * _tmp27 + P(2, 2) * _tmp8 + P(3, 2); - const Scalar _tmp57 = P(0, 0) * _tmp29 + P(1, 0) * _tmp41 + P(10, 0) * _tmp10 - - P(11, 0) * _tmp11 - P(12, 0) * _tmp27 + P(2, 0) * _tmp8 + P(3, 0); - const Scalar _tmp58 = P(0, 1) * _tmp29 + P(1, 1) * _tmp41 + P(10, 1) * _tmp10 - - P(11, 1) * _tmp11 - P(12, 1) * _tmp27 + P(2, 1) * _tmp8 + P(3, 1); - const Scalar _tmp59 = P(0, 3) * _tmp29 + P(1, 3) * _tmp41 + P(10, 3) * _tmp10 - - P(11, 3) * _tmp11 - P(12, 3) * _tmp27 + P(2, 3) * _tmp8 + P(3, 3); - const Scalar _tmp60 = d_vel(1, 0) - state(14, 0); - const Scalar _tmp61 = 2 * _tmp60; - const Scalar _tmp62 = _tmp61 * state(2, 0); - const Scalar _tmp63 = d_vel(2, 0) - state(15, 0); - const Scalar _tmp64 = 2 * _tmp63; - const Scalar _tmp65 = _tmp64 * state(3, 0); - const Scalar _tmp66 = _tmp62 + _tmp65; - const Scalar _tmp67 = _tmp64 * state(2, 0); - const Scalar _tmp68 = _tmp61 * state(3, 0); - const Scalar _tmp69 = _tmp67 - _tmp68; - const Scalar _tmp70 = _tmp64 * state(1, 0); - const Scalar _tmp71 = _tmp61 * state(0, 0); - const Scalar _tmp72 = d_vel(0, 0) - state(13, 0); - const Scalar _tmp73 = 4 * state(3, 0); - const Scalar _tmp74 = _tmp70 - _tmp71 - _tmp72 * _tmp73; - const Scalar _tmp75 = P(0, 15) + P(1, 15) * _tmp8 + P(10, 15) * _tmp11 + P(11, 15) * _tmp10 + + const Scalar _tmp16 = Scalar(0.5) * P(11, 3); + const Scalar _tmp17 = P(0, 3) + P(1, 3) * _tmp8 + P(10, 3) * _tmp11 + P(12, 3) * _tmp9 + + P(2, 3) * _tmp5 + P(3, 3) * _tmp2 + _tmp16 * state(2, 0); + const Scalar _tmp18 = Scalar(0.5) * P(11, 1); + const Scalar _tmp19 = P(0, 1) + P(1, 1) * _tmp8 + P(10, 1) * _tmp11 + P(12, 1) * _tmp9 + + P(2, 1) * _tmp5 + P(3, 1) * _tmp2 + _tmp18 * state(2, 0); + const Scalar _tmp20 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp21 = Scalar(0.25) * d_ang_var; + const Scalar _tmp22 = _tmp20 * _tmp21; + const Scalar _tmp23 = Scalar(0.5) * P(11, 0); + const Scalar _tmp24 = P(0, 0) + P(1, 0) * _tmp8 + P(10, 0) * _tmp11 + P(12, 0) * _tmp9 + + P(2, 0) * _tmp5 + P(3, 0) * _tmp2 + _tmp23 * state(2, 0); + const Scalar _tmp25 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp26 = _tmp21 * _tmp25; + const Scalar _tmp27 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp28 = _tmp21 * _tmp27; + const Scalar _tmp29 = _tmp26 + _tmp28; + const Scalar _tmp30 = Scalar(0.5) * state(0, 0); + const Scalar _tmp31 = _tmp21 * state(0, 0); + const Scalar _tmp32 = _tmp0 - _tmp1; + const Scalar _tmp33 = _tmp6 - _tmp7; + const Scalar _tmp34 = P(0, 12) * _tmp33 + P(1, 12) - P(10, 12) * _tmp30 + P(11, 12) * _tmp9 - + P(12, 12) * _tmp10 + P(2, 12) * _tmp32 + P(3, 12) * _tmp5; + const Scalar _tmp35 = P(0, 11) * _tmp33 + P(1, 11) - P(10, 11) * _tmp30 + P(11, 11) * _tmp9 - + P(12, 11) * _tmp10 + P(2, 11) * _tmp32 + P(3, 11) * _tmp5; + const Scalar _tmp36 = P(0, 10) * _tmp33 + P(1, 10) - P(10, 10) * _tmp30 + P(11, 10) * _tmp9 - + P(12, 10) * _tmp10 + P(2, 10) * _tmp32 + P(3, 10) * _tmp5; + const Scalar _tmp37 = P(0, 3) * _tmp33 + P(1, 3) - P(10, 3) * _tmp30 + P(11, 3) * _tmp9 - + P(12, 3) * _tmp10 + P(2, 3) * _tmp32 + P(3, 3) * _tmp5; + const Scalar _tmp38 = P(0, 2) * _tmp33 + P(1, 2) - P(10, 2) * _tmp30 + P(11, 2) * _tmp9 - + P(12, 2) * _tmp10 + P(2, 2) * _tmp32 + P(3, 2) * _tmp5; + const Scalar _tmp39 = P(0, 0) * _tmp33 + P(1, 0) - P(10, 0) * _tmp30 - P(12, 0) * _tmp10 + + P(2, 0) * _tmp32 + P(3, 0) * _tmp5 + _tmp23 * state(3, 0); + const Scalar _tmp40 = P(0, 1) * _tmp33 + P(1, 1) - P(10, 1) * _tmp30 - P(12, 1) * _tmp10 + + P(2, 1) * _tmp32 + P(3, 1) * _tmp5 + _tmp18 * state(3, 0); + const Scalar _tmp41 = _tmp21 * std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp42 = _tmp22 + _tmp41; + const Scalar _tmp43 = _tmp3 - _tmp4; + const Scalar _tmp44 = P(0, 12) * _tmp43 + P(1, 12) * _tmp2 - P(10, 12) * _tmp9 - + P(11, 12) * _tmp30 + P(12, 12) * _tmp11 + P(2, 12) + P(3, 12) * _tmp33; + const Scalar _tmp45 = P(0, 11) * _tmp43 + P(1, 11) * _tmp2 - P(10, 11) * _tmp9 - + P(11, 11) * _tmp30 + P(12, 11) * _tmp11 + P(2, 11) + P(3, 11) * _tmp33; + const Scalar _tmp46 = P(0, 10) * _tmp43 + P(1, 10) * _tmp2 - P(10, 10) * _tmp9 - + P(11, 10) * _tmp30 + P(12, 10) * _tmp11 + P(2, 10) + P(3, 10) * _tmp33; + const Scalar _tmp47 = P(0, 0) * _tmp43 + P(1, 0) * _tmp2 - P(10, 0) * _tmp9 + P(12, 0) * _tmp11 + + P(2, 0) + P(3, 0) * _tmp33 - _tmp23 * state(0, 0); + const Scalar _tmp48 = P(0, 1) * _tmp43 + P(1, 1) * _tmp2 - P(10, 1) * _tmp9 + P(12, 1) * _tmp11 + + P(2, 1) + P(3, 1) * _tmp33 - _tmp18 * state(0, 0); + const Scalar _tmp49 = P(0, 3) * _tmp43 + P(1, 3) * _tmp2 - P(10, 3) * _tmp9 + P(12, 3) * _tmp11 + + P(2, 3) + P(3, 3) * _tmp33 - _tmp16 * state(0, 0); + const Scalar _tmp50 = P(0, 2) * _tmp43 + P(1, 2) * _tmp2 - P(10, 2) * _tmp9 - P(11, 2) * _tmp30 + + P(12, 2) * _tmp11 + P(2, 2) + P(3, 2) * _tmp33; + const Scalar _tmp51 = _tmp21 * state(3, 0); + const Scalar _tmp52 = P(0, 12) * _tmp32 + P(1, 12) * _tmp43 + P(10, 12) * _tmp10 - + P(11, 12) * _tmp11 - P(12, 12) * _tmp30 + P(2, 12) * _tmp8 + P(3, 12); + const Scalar _tmp53 = P(0, 11) * _tmp32 + P(1, 11) * _tmp43 + P(10, 11) * _tmp10 - + P(11, 11) * _tmp11 - P(12, 11) * _tmp30 + P(2, 11) * _tmp8 + P(3, 11); + const Scalar _tmp54 = P(0, 10) * _tmp32 + P(1, 10) * _tmp43 + P(10, 10) * _tmp10 - + P(11, 10) * _tmp11 - P(12, 10) * _tmp30 + P(2, 10) * _tmp8 + P(3, 10); + const Scalar _tmp55 = P(0, 2) * _tmp32 + P(1, 2) * _tmp43 + P(10, 2) * _tmp10 - + P(11, 2) * _tmp11 - P(12, 2) * _tmp30 + P(2, 2) * _tmp8 + P(3, 2); + const Scalar _tmp56 = P(0, 0) * _tmp32 + P(1, 0) * _tmp43 + P(10, 0) * _tmp10 - + P(12, 0) * _tmp30 + P(2, 0) * _tmp8 + P(3, 0) - _tmp23 * state(1, 0); + const Scalar _tmp57 = P(0, 1) * _tmp32 + P(1, 1) * _tmp43 + P(10, 1) * _tmp10 - + P(12, 1) * _tmp30 + P(2, 1) * _tmp8 + P(3, 1) - _tmp18 * state(1, 0); + const Scalar _tmp58 = P(0, 3) * _tmp32 + P(1, 3) * _tmp43 + P(10, 3) * _tmp10 - + P(11, 3) * _tmp11 - P(12, 3) * _tmp30 + P(2, 3) * _tmp8 + P(3, 3); + const Scalar _tmp59 = d_vel(1, 0) - state(14, 0); + const Scalar _tmp60 = 2 * state(2, 0); + const Scalar _tmp61 = _tmp59 * _tmp60; + const Scalar _tmp62 = d_vel(2, 0) - state(15, 0); + const Scalar _tmp63 = 2 * state(3, 0); + const Scalar _tmp64 = _tmp62 * _tmp63; + const Scalar _tmp65 = _tmp61 + _tmp64; + const Scalar _tmp66 = _tmp60 * _tmp62; + const Scalar _tmp67 = _tmp59 * _tmp63; + const Scalar _tmp68 = _tmp66 - _tmp67; + const Scalar _tmp69 = 2 * state(1, 0); + const Scalar _tmp70 = _tmp62 * _tmp69; + const Scalar _tmp71 = 2 * state(0, 0); + const Scalar _tmp72 = _tmp59 * _tmp71; + const Scalar _tmp73 = d_vel(0, 0) - state(13, 0); + const Scalar _tmp74 = 4 * state(3, 0); + const Scalar _tmp75 = _tmp70 - _tmp72 - _tmp73 * _tmp74; + const Scalar _tmp76 = _tmp71 * state(2, 0); + const Scalar _tmp77 = _tmp69 * state(3, 0); + const Scalar _tmp78 = _tmp76 + _tmp77; + const Scalar _tmp79 = P(0, 15) + P(1, 15) * _tmp8 + P(10, 15) * _tmp11 + P(11, 15) * _tmp10 + P(12, 15) * _tmp9 + P(2, 15) * _tmp5 + P(3, 15) * _tmp2; - const Scalar _tmp76 = 2 * _tmp40 + 2 * _tmp51; - const Scalar _tmp77 = P(0, 14) + P(1, 14) * _tmp8 + P(10, 14) * _tmp11 + P(11, 14) * _tmp10 + + const Scalar _tmp80 = _tmp71 * state(3, 0); + const Scalar _tmp81 = _tmp69 * state(2, 0); + const Scalar _tmp82 = -_tmp80 + _tmp81; + const Scalar _tmp83 = P(0, 14) + P(1, 14) * _tmp8 + P(10, 14) * _tmp11 + P(11, 14) * _tmp10 + P(12, 14) * _tmp9 + P(2, 14) * _tmp5 + P(3, 14) * _tmp2; - const Scalar _tmp78 = 2 * _tmp42 - 2 * _tmp50; - const Scalar _tmp79 = -2 * _tmp18; - const Scalar _tmp80 = -2 * _tmp22; - const Scalar _tmp81 = _tmp79 + _tmp80 + 1; - const Scalar _tmp82 = P(0, 13) + P(1, 13) * _tmp8 + P(10, 13) * _tmp11 + P(11, 13) * _tmp10 + + const Scalar _tmp84 = -2 * _tmp20; + const Scalar _tmp85 = -2 * _tmp25; + const Scalar _tmp86 = _tmp84 + _tmp85 + 1; + const Scalar _tmp87 = P(0, 13) + P(1, 13) * _tmp8 + P(10, 13) * _tmp11 + P(11, 13) * _tmp10 + P(12, 13) * _tmp9 + P(2, 13) * _tmp5 + P(3, 13) * _tmp2; - const Scalar _tmp83 = _tmp61 * state(1, 0); - const Scalar _tmp84 = _tmp64 * state(0, 0); - const Scalar _tmp85 = 4 * state(2, 0); - const Scalar _tmp86 = -_tmp72 * _tmp85 + _tmp83 + _tmp84; - const Scalar _tmp87 = P(0, 4) + P(1, 4) * _tmp8 + P(10, 4) * _tmp11 + P(11, 4) * _tmp10 + - P(12, 4) * _tmp9 + P(2, 4) * _tmp5 + P(3, 4) * _tmp2; - const Scalar _tmp88 = P(0, 13) * _tmp30 + P(1, 13) - P(10, 13) * _tmp27 + P(11, 13) * _tmp9 - - P(12, 13) * _tmp10 + P(2, 13) * _tmp29 + P(3, 13) * _tmp5; - const Scalar _tmp89 = P(0, 15) * _tmp30 + P(1, 15) - P(10, 15) * _tmp27 + P(11, 15) * _tmp9 - - P(12, 15) * _tmp10 + P(2, 15) * _tmp29 + P(3, 15) * _tmp5; - const Scalar _tmp90 = P(0, 14) * _tmp30 + P(1, 14) - P(10, 14) * _tmp27 + P(11, 14) * _tmp9 - - P(12, 14) * _tmp10 + P(2, 14) * _tmp29 + P(3, 14) * _tmp5; - const Scalar _tmp91 = P(0, 4) * _tmp30 + P(1, 4) - P(10, 4) * _tmp27 + P(11, 4) * _tmp9 - - P(12, 4) * _tmp10 + P(2, 4) * _tmp29 + P(3, 4) * _tmp5; - const Scalar _tmp92 = P(0, 15) * _tmp41 + P(1, 15) * _tmp2 - P(10, 15) * _tmp9 - - P(11, 15) * _tmp27 + P(12, 15) * _tmp11 + P(2, 15) + P(3, 15) * _tmp30; - const Scalar _tmp93 = P(0, 14) * _tmp41 + P(1, 14) * _tmp2 - P(10, 14) * _tmp9 - - P(11, 14) * _tmp27 + P(12, 14) * _tmp11 + P(2, 14) + P(3, 14) * _tmp30; - const Scalar _tmp94 = P(0, 13) * _tmp41 + P(1, 13) * _tmp2 - P(10, 13) * _tmp9 - - P(11, 13) * _tmp27 + P(12, 13) * _tmp11 + P(2, 13) + P(3, 13) * _tmp30; - const Scalar _tmp95 = Scalar(0.5) * P(11, 4); - const Scalar _tmp96 = P(0, 4) * _tmp41 + P(1, 4) * _tmp2 - P(10, 4) * _tmp9 + P(12, 4) * _tmp11 + - P(2, 4) + P(3, 4) * _tmp30 - _tmp95 * state(0, 0); - const Scalar _tmp97 = P(0, 13) * _tmp29 + P(1, 13) * _tmp41 + P(10, 13) * _tmp10 - - P(11, 13) * _tmp11 - P(12, 13) * _tmp27 + P(2, 13) * _tmp8 + P(3, 13); - const Scalar _tmp98 = P(0, 14) * _tmp29 + P(1, 14) * _tmp41 + P(10, 14) * _tmp10 - - P(11, 14) * _tmp11 - P(12, 14) * _tmp27 + P(2, 14) * _tmp8 + P(3, 14); - const Scalar _tmp99 = P(0, 15) * _tmp29 + P(1, 15) * _tmp41 + P(10, 15) * _tmp10 - - P(11, 15) * _tmp11 - P(12, 15) * _tmp27 + P(2, 15) * _tmp8 + P(3, 15); - const Scalar _tmp100 = P(0, 4) * _tmp29 + P(1, 4) * _tmp41 + P(10, 4) * _tmp10 - - P(12, 4) * _tmp27 + P(2, 4) * _tmp8 + P(3, 4) - _tmp95 * state(1, 0); - const Scalar _tmp101 = P(0, 13) * _tmp69 + P(1, 13) * _tmp66 - P(13, 13) * _tmp81 - - P(14, 13) * _tmp78 - P(15, 13) * _tmp76 + P(2, 13) * _tmp86 + - P(3, 13) * _tmp74 + P(4, 13); - const Scalar _tmp102 = P(0, 14) * _tmp69 + P(1, 14) * _tmp66 - P(13, 14) * _tmp81 - - P(14, 14) * _tmp78 - P(15, 14) * _tmp76 + P(2, 14) * _tmp86 + - P(3, 14) * _tmp74 + P(4, 14); - const Scalar _tmp103 = P(0, 15) * _tmp69 + P(1, 15) * _tmp66 - P(13, 15) * _tmp81 - - P(14, 15) * _tmp78 - P(15, 15) * _tmp76 + P(2, 15) * _tmp86 + - P(3, 15) * _tmp74 + P(4, 15); - const Scalar _tmp104 = 2 * _tmp40; - const Scalar _tmp105 = 2 * _tmp51; - const Scalar _tmp106 = _tmp104 + _tmp105; - const Scalar _tmp107 = 2 * _tmp50; - const Scalar _tmp108 = 2 * _tmp42; - const Scalar _tmp109 = -_tmp107 + _tmp108; - const Scalar _tmp110 = P(0, 0) * _tmp69 + P(1, 0) * _tmp66 - P(13, 0) * _tmp81 - - P(14, 0) * _tmp78 - P(15, 0) * _tmp76 + P(2, 0) * _tmp86 + - P(3, 0) * _tmp74 + P(4, 0); - const Scalar _tmp111 = P(0, 1) * _tmp69 + P(1, 1) * _tmp66 - P(13, 1) * _tmp81 - - P(14, 1) * _tmp78 - P(15, 1) * _tmp76 + P(2, 1) * _tmp86 + - P(3, 1) * _tmp74 + P(4, 1); - const Scalar _tmp112 = P(0, 2) * _tmp69 + P(1, 2) * _tmp66 - P(13, 2) * _tmp81 - - P(14, 2) * _tmp78 - P(15, 2) * _tmp76 + P(2, 2) * _tmp86 + - P(3, 2) * _tmp74 + P(4, 2); - const Scalar _tmp113 = P(0, 3) * _tmp69 + P(1, 3) * _tmp66 - P(13, 3) * _tmp81 - - P(14, 3) * _tmp78 - P(15, 3) * _tmp76 + P(2, 3) * _tmp86 + - P(3, 3) * _tmp74 + P(4, 3); - const Scalar _tmp114 = P(0, 4) * _tmp69 + P(1, 4) * _tmp66 - P(13, 4) * _tmp81 - - P(14, 4) * _tmp78 - P(15, 4) * _tmp76 + P(2, 4) * _tmp86 + - P(3, 4) * _tmp74 + P(4, 4); - const Scalar _tmp115 = 2 * _tmp72; - const Scalar _tmp116 = _tmp115 * state(1, 0); - const Scalar _tmp117 = _tmp116 + _tmp65; - const Scalar _tmp118 = _tmp115 * state(3, 0); - const Scalar _tmp119 = _tmp118 - _tmp70; - const Scalar _tmp120 = _tmp115 * state(0, 0); - const Scalar _tmp121 = _tmp120 - _tmp60 * _tmp73 + _tmp67; - const Scalar _tmp122 = 2 * _tmp42 + 2 * _tmp50; - const Scalar _tmp123 = -2 * _tmp28 + 2 * _tmp52; - const Scalar _tmp124 = 1 - 2 * _tmp24; - const Scalar _tmp125 = _tmp124 + _tmp79; - const Scalar _tmp126 = 4 * state(1, 0); - const Scalar _tmp127 = _tmp115 * state(2, 0); - const Scalar _tmp128 = -_tmp126 * _tmp60 + _tmp127 - _tmp84; - const Scalar _tmp129 = P(0, 5) + P(1, 5) * _tmp8 + P(10, 5) * _tmp11 + P(11, 5) * _tmp10 + - P(12, 5) * _tmp9 + P(2, 5) * _tmp5 + P(3, 5) * _tmp2; - const Scalar _tmp130 = P(0, 5) * _tmp30 + P(1, 5) - P(10, 5) * _tmp27 + P(11, 5) * _tmp9 - - P(12, 5) * _tmp10 + P(2, 5) * _tmp29 + P(3, 5) * _tmp5; - const Scalar _tmp131 = Scalar(0.5) * P(11, 5); - const Scalar _tmp132 = P(0, 5) * _tmp41 + P(1, 5) * _tmp2 - P(10, 5) * _tmp9 + P(12, 5) * _tmp11 + - P(2, 5) + P(3, 5) * _tmp30 - _tmp131 * state(0, 0); - const Scalar _tmp133 = P(0, 5) * _tmp29 + P(1, 5) * _tmp41 + P(10, 5) * _tmp10 - - P(12, 5) * _tmp27 + P(2, 5) * _tmp8 + P(3, 5) - _tmp131 * state(1, 0); - const Scalar _tmp134 = _tmp107 + _tmp108; - const Scalar _tmp135 = _tmp81 * d_vel_var(0, 0); - const Scalar _tmp136 = 2 * _tmp52; - const Scalar _tmp137 = 2 * _tmp28; - const Scalar _tmp138 = _tmp136 - _tmp137; - const Scalar _tmp139 = _tmp138 * d_vel_var(2, 0); - const Scalar _tmp140 = P(0, 5) * _tmp69 + P(1, 5) * _tmp66 - P(13, 5) * _tmp81 - - P(14, 5) * _tmp78 - P(15, 5) * _tmp76 + P(2, 5) * _tmp86 + - P(3, 5) * _tmp74 + P(4, 5); - const Scalar _tmp141 = P(0, 3) * _tmp119 + P(1, 3) * _tmp128 - P(13, 3) * _tmp122 - - P(14, 3) * _tmp125 - P(15, 3) * _tmp123 + P(2, 3) * _tmp117 + - P(3, 3) * _tmp121 + P(5, 3); - const Scalar _tmp142 = P(0, 1) * _tmp119 + P(1, 1) * _tmp128 - P(13, 1) * _tmp122 - - P(14, 1) * _tmp125 - P(15, 1) * _tmp123 + P(2, 1) * _tmp117 + - P(3, 1) * _tmp121 + P(5, 1); - const Scalar _tmp143 = P(0, 13) * _tmp119 + P(1, 13) * _tmp128 - P(13, 13) * _tmp122 - - P(14, 13) * _tmp125 - P(15, 13) * _tmp123 + P(2, 13) * _tmp117 + - P(3, 13) * _tmp121 + P(5, 13); - const Scalar _tmp144 = P(0, 15) * _tmp119 + P(1, 15) * _tmp128 - P(13, 15) * _tmp122 - - P(14, 15) * _tmp125 - P(15, 15) * _tmp123 + P(2, 15) * _tmp117 + - P(3, 15) * _tmp121 + P(5, 15); - const Scalar _tmp145 = P(0, 14) * _tmp119 + P(1, 14) * _tmp128 - P(13, 14) * _tmp122 - - P(14, 14) * _tmp125 - P(15, 14) * _tmp123 + P(2, 14) * _tmp117 + - P(3, 14) * _tmp121 + P(5, 14); - const Scalar _tmp146 = P(0, 0) * _tmp119 + P(1, 0) * _tmp128 - P(13, 0) * _tmp122 - - P(14, 0) * _tmp125 - P(15, 0) * _tmp123 + P(2, 0) * _tmp117 + - P(3, 0) * _tmp121 + P(5, 0); - const Scalar _tmp147 = P(0, 2) * _tmp119 + P(1, 2) * _tmp128 - P(13, 2) * _tmp122 - - P(14, 2) * _tmp125 - P(15, 2) * _tmp123 + P(2, 2) * _tmp117 + - P(3, 2) * _tmp121 + P(5, 2); - const Scalar _tmp148 = P(0, 5) * _tmp119 + P(1, 5) * _tmp128 - P(13, 5) * _tmp122 - - P(14, 5) * _tmp125 - P(15, 5) * _tmp123 + P(2, 5) * _tmp117 + - P(3, 5) * _tmp121 + P(5, 5); - const Scalar _tmp149 = _tmp116 + _tmp62; - const Scalar _tmp150 = -_tmp127 + _tmp83; - const Scalar _tmp151 = -_tmp120 - _tmp63 * _tmp85 + _tmp68; - const Scalar _tmp152 = _tmp118 - _tmp126 * _tmp63 + _tmp71; - const Scalar _tmp153 = -2 * _tmp40 + 2 * _tmp51; - const Scalar _tmp154 = 2 * _tmp28 + 2 * _tmp52; - const Scalar _tmp155 = _tmp124 + _tmp80; - const Scalar _tmp156 = P(0, 6) + P(1, 6) * _tmp8 + P(10, 6) * _tmp11 + P(11, 6) * _tmp10 + - P(12, 6) * _tmp9 + P(2, 6) * _tmp5 + P(3, 6) * _tmp2; - const Scalar _tmp157 = P(0, 6) * _tmp30 + P(1, 6) - P(10, 6) * _tmp27 + P(11, 6) * _tmp9 - - P(12, 6) * _tmp10 + P(2, 6) * _tmp29 + P(3, 6) * _tmp5; - const Scalar _tmp158 = Scalar(0.5) * P(11, 6); - const Scalar _tmp159 = P(0, 6) * _tmp41 + P(1, 6) * _tmp2 - P(10, 6) * _tmp9 + P(12, 6) * _tmp11 + - P(2, 6) + P(3, 6) * _tmp30 - _tmp158 * state(0, 0); - const Scalar _tmp160 = P(0, 6) * _tmp29 + P(1, 6) * _tmp41 + P(10, 6) * _tmp10 - - P(12, 6) * _tmp27 + P(2, 6) * _tmp8 + P(3, 6) - _tmp158 * state(1, 0); - const Scalar _tmp161 = -_tmp104 + _tmp105; - const Scalar _tmp162 = _tmp136 + _tmp137; - const Scalar _tmp163 = _tmp162 * d_vel_var(1, 0); - const Scalar _tmp164 = P(0, 6) * _tmp69 + P(1, 6) * _tmp66 - P(13, 6) * _tmp81 - - P(14, 6) * _tmp78 - P(15, 6) * _tmp76 + P(2, 6) * _tmp86 + - P(3, 6) * _tmp74 + P(4, 6); - const Scalar _tmp165 = P(0, 6) * _tmp119 + P(1, 6) * _tmp128 - P(13, 6) * _tmp122 - - P(14, 6) * _tmp125 - P(15, 6) * _tmp123 + P(2, 6) * _tmp117 + - P(3, 6) * _tmp121 + P(5, 6); - const Scalar _tmp166 = P(0, 13) * _tmp150 + P(1, 13) * _tmp152 - P(13, 13) * _tmp153 - - P(14, 13) * _tmp154 - P(15, 13) * _tmp155 + P(2, 13) * _tmp151 + - P(3, 13) * _tmp149 + P(6, 13); - const Scalar _tmp167 = P(0, 14) * _tmp150 + P(1, 14) * _tmp152 - P(13, 14) * _tmp153 - - P(14, 14) * _tmp154 - P(15, 14) * _tmp155 + P(2, 14) * _tmp151 + - P(3, 14) * _tmp149 + P(6, 14); - const Scalar _tmp168 = P(0, 15) * _tmp150 + P(1, 15) * _tmp152 - P(13, 15) * _tmp153 - - P(14, 15) * _tmp154 - P(15, 15) * _tmp155 + P(2, 15) * _tmp151 + - P(3, 15) * _tmp149 + P(6, 15); - const Scalar _tmp169 = P(0, 6) * _tmp150 + P(1, 6) * _tmp152 - P(13, 6) * _tmp153 - - P(14, 6) * _tmp154 - P(15, 6) * _tmp155 + P(2, 6) * _tmp151 + - P(3, 6) * _tmp149 + P(6, 6); - const Scalar _tmp170 = Scalar(0.5) * P(11, 8); + const Scalar _tmp88 = _tmp59 * _tmp69; + const Scalar _tmp89 = _tmp62 * _tmp71; + const Scalar _tmp90 = 4 * state(2, 0); + const Scalar _tmp91 = -_tmp73 * _tmp90 + _tmp88 + _tmp89; + const Scalar _tmp92 = Scalar(0.5) * P(11, 4); + const Scalar _tmp93 = P(0, 4) + P(1, 4) * _tmp8 + P(10, 4) * _tmp11 + P(12, 4) * _tmp9 + + P(2, 4) * _tmp5 + P(3, 4) * _tmp2 + _tmp92 * state(2, 0); + const Scalar _tmp94 = P(0, 13) * _tmp33 + P(1, 13) - P(10, 13) * _tmp30 + P(11, 13) * _tmp9 - + P(12, 13) * _tmp10 + P(2, 13) * _tmp32 + P(3, 13) * _tmp5; + const Scalar _tmp95 = P(0, 15) * _tmp33 + P(1, 15) - P(10, 15) * _tmp30 + P(11, 15) * _tmp9 - + P(12, 15) * _tmp10 + P(2, 15) * _tmp32 + P(3, 15) * _tmp5; + const Scalar _tmp96 = P(0, 14) * _tmp33 + P(1, 14) - P(10, 14) * _tmp30 + P(11, 14) * _tmp9 - + P(12, 14) * _tmp10 + P(2, 14) * _tmp32 + P(3, 14) * _tmp5; + const Scalar _tmp97 = Scalar(0.5) * P(10, 4); + const Scalar _tmp98 = P(0, 4) * _tmp33 + P(1, 4) - P(12, 4) * _tmp10 + P(2, 4) * _tmp32 + + P(3, 4) * _tmp5 + _tmp92 * state(3, 0) - _tmp97 * state(0, 0); + const Scalar _tmp99 = P(0, 15) * _tmp43 + P(1, 15) * _tmp2 - P(10, 15) * _tmp9 - + P(11, 15) * _tmp30 + P(12, 15) * _tmp11 + P(2, 15) + P(3, 15) * _tmp33; + const Scalar _tmp100 = P(0, 14) * _tmp43 + P(1, 14) * _tmp2 - P(10, 14) * _tmp9 - + P(11, 14) * _tmp30 + P(12, 14) * _tmp11 + P(2, 14) + P(3, 14) * _tmp33; + const Scalar _tmp101 = P(0, 13) * _tmp43 + P(1, 13) * _tmp2 - P(10, 13) * _tmp9 - + P(11, 13) * _tmp30 + P(12, 13) * _tmp11 + P(2, 13) + P(3, 13) * _tmp33; + const Scalar _tmp102 = P(0, 4) * _tmp43 + P(1, 4) * _tmp2 - P(10, 4) * _tmp9 + P(12, 4) * _tmp11 + + P(2, 4) + P(3, 4) * _tmp33 - _tmp92 * state(0, 0); + const Scalar _tmp103 = P(0, 13) * _tmp32 + P(1, 13) * _tmp43 + P(10, 13) * _tmp10 - + P(11, 13) * _tmp11 - P(12, 13) * _tmp30 + P(2, 13) * _tmp8 + P(3, 13); + const Scalar _tmp104 = P(0, 14) * _tmp32 + P(1, 14) * _tmp43 + P(10, 14) * _tmp10 - + P(11, 14) * _tmp11 - P(12, 14) * _tmp30 + P(2, 14) * _tmp8 + P(3, 14); + const Scalar _tmp105 = P(0, 15) * _tmp32 + P(1, 15) * _tmp43 + P(10, 15) * _tmp10 - + P(11, 15) * _tmp11 - P(12, 15) * _tmp30 + P(2, 15) * _tmp8 + P(3, 15); + const Scalar _tmp106 = P(0, 4) * _tmp32 + P(1, 4) * _tmp43 - P(12, 4) * _tmp30 + P(2, 4) * _tmp8 + + P(3, 4) - _tmp92 * state(1, 0) + _tmp97 * state(2, 0); + const Scalar _tmp107 = P(0, 13) * _tmp68 + P(1, 13) * _tmp65 - P(13, 13) * _tmp86 - + P(14, 13) * _tmp82 - P(15, 13) * _tmp78 + P(2, 13) * _tmp91 + + P(3, 13) * _tmp75 + P(4, 13); + const Scalar _tmp108 = P(0, 14) * _tmp68 + P(1, 14) * _tmp65 - P(13, 14) * _tmp86 - + P(14, 14) * _tmp82 - P(15, 14) * _tmp78 + P(2, 14) * _tmp91 + + P(3, 14) * _tmp75 + P(4, 14); + const Scalar _tmp109 = P(0, 15) * _tmp68 + P(1, 15) * _tmp65 - P(13, 15) * _tmp86 - + P(14, 15) * _tmp82 - P(15, 15) * _tmp78 + P(2, 15) * _tmp91 + + P(3, 15) * _tmp75 + P(4, 15); + const Scalar _tmp110 = P(0, 0) * _tmp68 + P(1, 0) * _tmp65 - P(13, 0) * _tmp86 - + P(14, 0) * _tmp82 - P(15, 0) * _tmp78 + P(2, 0) * _tmp91 + + P(3, 0) * _tmp75 + P(4, 0); + const Scalar _tmp111 = P(0, 1) * _tmp68 + P(1, 1) * _tmp65 - P(13, 1) * _tmp86 - + P(14, 1) * _tmp82 - P(15, 1) * _tmp78 + P(2, 1) * _tmp91 + + P(3, 1) * _tmp75 + P(4, 1); + const Scalar _tmp112 = P(0, 2) * _tmp68 + P(1, 2) * _tmp65 - P(13, 2) * _tmp86 - + P(14, 2) * _tmp82 - P(15, 2) * _tmp78 + P(2, 2) * _tmp91 + + P(3, 2) * _tmp75 + P(4, 2); + const Scalar _tmp113 = P(0, 3) * _tmp68 + P(1, 3) * _tmp65 - P(13, 3) * _tmp86 - + P(14, 3) * _tmp82 - P(15, 3) * _tmp78 + P(2, 3) * _tmp91 + + P(3, 3) * _tmp75 + P(4, 3); + const Scalar _tmp114 = P(0, 4) * _tmp68 + P(1, 4) * _tmp65 - P(13, 4) * _tmp86 - + P(14, 4) * _tmp82 - P(15, 4) * _tmp78 + P(2, 4) * _tmp91 + + P(3, 4) * _tmp75 + P(4, 4); + const Scalar _tmp115 = _tmp69 * _tmp73; + const Scalar _tmp116 = _tmp115 + _tmp64; + const Scalar _tmp117 = _tmp63 * _tmp73; + const Scalar _tmp118 = _tmp117 - _tmp70; + const Scalar _tmp119 = _tmp71 * _tmp73; + const Scalar _tmp120 = _tmp119 - _tmp59 * _tmp74 + _tmp66; + const Scalar _tmp121 = _tmp80 + _tmp81; + const Scalar _tmp122 = _tmp60 * state(3, 0); + const Scalar _tmp123 = _tmp69 * state(0, 0); + const Scalar _tmp124 = _tmp122 - _tmp123; + const Scalar _tmp125 = 1 - 2 * _tmp27; + const Scalar _tmp126 = _tmp125 + _tmp84; + const Scalar _tmp127 = 4 * state(1, 0); + const Scalar _tmp128 = _tmp60 * _tmp73; + const Scalar _tmp129 = -_tmp127 * _tmp59 + _tmp128 - _tmp89; + const Scalar _tmp130 = Scalar(0.5) * P(11, 5); + const Scalar _tmp131 = P(0, 5) + P(1, 5) * _tmp8 + P(10, 5) * _tmp11 + P(12, 5) * _tmp9 + + P(2, 5) * _tmp5 + P(3, 5) * _tmp2 + _tmp130 * state(2, 0); + const Scalar _tmp132 = P(0, 5) * _tmp33 + P(1, 5) - P(10, 5) * _tmp30 - P(12, 5) * _tmp10 + + P(2, 5) * _tmp32 + P(3, 5) * _tmp5 + _tmp130 * state(3, 0); + const Scalar _tmp133 = P(0, 5) * _tmp43 + P(1, 5) * _tmp2 - P(10, 5) * _tmp9 + P(12, 5) * _tmp11 + + P(2, 5) + P(3, 5) * _tmp33 - _tmp130 * state(0, 0); + const Scalar _tmp134 = P(0, 5) * _tmp32 + P(1, 5) * _tmp43 + P(10, 5) * _tmp10 - + P(12, 5) * _tmp30 + P(2, 5) * _tmp8 + P(3, 5) - _tmp130 * state(1, 0); + const Scalar _tmp135 = _tmp86 * d_vel_var(0, 0); + const Scalar _tmp136 = _tmp82 * d_vel_var(1, 0); + const Scalar _tmp137 = _tmp78 * d_vel_var(2, 0); + const Scalar _tmp138 = P(0, 5) * _tmp68 + P(1, 5) * _tmp65 - P(13, 5) * _tmp86 - + P(14, 5) * _tmp82 - P(15, 5) * _tmp78 + P(2, 5) * _tmp91 + + P(3, 5) * _tmp75 + P(4, 5); + const Scalar _tmp139 = P(0, 1) * _tmp118 + P(1, 1) * _tmp129 - P(13, 1) * _tmp121 - + P(14, 1) * _tmp126 - P(15, 1) * _tmp124 + P(2, 1) * _tmp116 + + P(3, 1) * _tmp120 + P(5, 1); + const Scalar _tmp140 = P(0, 3) * _tmp118 + P(1, 3) * _tmp129 - P(13, 3) * _tmp121 - + P(14, 3) * _tmp126 - P(15, 3) * _tmp124 + P(2, 3) * _tmp116 + + P(3, 3) * _tmp120 + P(5, 3); + const Scalar _tmp141 = P(0, 13) * _tmp118 + P(1, 13) * _tmp129 - P(13, 13) * _tmp121 - + P(14, 13) * _tmp126 - P(15, 13) * _tmp124 + P(2, 13) * _tmp116 + + P(3, 13) * _tmp120 + P(5, 13); + const Scalar _tmp142 = P(0, 15) * _tmp118 + P(1, 15) * _tmp129 - P(13, 15) * _tmp121 - + P(14, 15) * _tmp126 - P(15, 15) * _tmp124 + P(2, 15) * _tmp116 + + P(3, 15) * _tmp120 + P(5, 15); + const Scalar _tmp143 = P(0, 14) * _tmp118 + P(1, 14) * _tmp129 - P(13, 14) * _tmp121 - + P(14, 14) * _tmp126 - P(15, 14) * _tmp124 + P(2, 14) * _tmp116 + + P(3, 14) * _tmp120 + P(5, 14); + const Scalar _tmp144 = P(0, 0) * _tmp118 + P(1, 0) * _tmp129 - P(13, 0) * _tmp121 - + P(14, 0) * _tmp126 - P(15, 0) * _tmp124 + P(2, 0) * _tmp116 + + P(3, 0) * _tmp120 + P(5, 0); + const Scalar _tmp145 = P(0, 2) * _tmp118 + P(1, 2) * _tmp129 - P(13, 2) * _tmp121 - + P(14, 2) * _tmp126 - P(15, 2) * _tmp124 + P(2, 2) * _tmp116 + + P(3, 2) * _tmp120 + P(5, 2); + const Scalar _tmp146 = P(0, 5) * _tmp118 + P(1, 5) * _tmp129 - P(13, 5) * _tmp121 - + P(14, 5) * _tmp126 - P(15, 5) * _tmp124 + P(2, 5) * _tmp116 + + P(3, 5) * _tmp120 + P(5, 5); + const Scalar _tmp147 = _tmp115 + _tmp61; + const Scalar _tmp148 = -_tmp128 + _tmp88; + const Scalar _tmp149 = -_tmp119 - _tmp62 * _tmp90 + _tmp67; + const Scalar _tmp150 = _tmp117 - _tmp127 * _tmp62 + _tmp72; + const Scalar _tmp151 = -_tmp76 + _tmp77; + const Scalar _tmp152 = _tmp122 + _tmp123; + const Scalar _tmp153 = _tmp125 + _tmp85; + const Scalar _tmp154 = Scalar(0.5) * P(11, 6); + const Scalar _tmp155 = P(0, 6) + P(1, 6) * _tmp8 + P(10, 6) * _tmp11 + P(12, 6) * _tmp9 + + P(2, 6) * _tmp5 + P(3, 6) * _tmp2 + _tmp154 * state(2, 0); + const Scalar _tmp156 = P(0, 6) * _tmp33 + P(1, 6) - P(10, 6) * _tmp30 - P(12, 6) * _tmp10 + + P(2, 6) * _tmp32 + P(3, 6) * _tmp5 + _tmp154 * state(3, 0); + const Scalar _tmp157 = P(0, 6) * _tmp43 + P(1, 6) * _tmp2 - P(10, 6) * _tmp9 + P(12, 6) * _tmp11 + + P(2, 6) + P(3, 6) * _tmp33 - _tmp154 * state(0, 0); + const Scalar _tmp158 = P(0, 6) * _tmp32 + P(1, 6) * _tmp43 + P(10, 6) * _tmp10 - + P(12, 6) * _tmp30 + P(2, 6) * _tmp8 + P(3, 6) - _tmp154 * state(1, 0); + const Scalar _tmp159 = P(0, 6) * _tmp68 + P(1, 6) * _tmp65 - P(13, 6) * _tmp86 - + P(14, 6) * _tmp82 - P(15, 6) * _tmp78 + P(2, 6) * _tmp91 + + P(3, 6) * _tmp75 + P(4, 6); + const Scalar _tmp160 = P(0, 6) * _tmp118 + P(1, 6) * _tmp129 - P(13, 6) * _tmp121 - + P(14, 6) * _tmp126 - P(15, 6) * _tmp124 + P(2, 6) * _tmp116 + + P(3, 6) * _tmp120 + P(5, 6); + const Scalar _tmp161 = P(0, 13) * _tmp148 + P(1, 13) * _tmp150 - P(13, 13) * _tmp151 - + P(14, 13) * _tmp152 - P(15, 13) * _tmp153 + P(2, 13) * _tmp149 + + P(3, 13) * _tmp147 + P(6, 13); + const Scalar _tmp162 = P(0, 14) * _tmp148 + P(1, 14) * _tmp150 - P(13, 14) * _tmp151 - + P(14, 14) * _tmp152 - P(15, 14) * _tmp153 + P(2, 14) * _tmp149 + + P(3, 14) * _tmp147 + P(6, 14); + const Scalar _tmp163 = P(0, 15) * _tmp148 + P(1, 15) * _tmp150 - P(13, 15) * _tmp151 - + P(14, 15) * _tmp152 - P(15, 15) * _tmp153 + P(2, 15) * _tmp149 + + P(3, 15) * _tmp147 + P(6, 15); + const Scalar _tmp164 = P(0, 6) * _tmp148 + P(1, 6) * _tmp150 - P(13, 6) * _tmp151 - + P(14, 6) * _tmp152 - P(15, 6) * _tmp153 + P(2, 6) * _tmp149 + + P(3, 6) * _tmp147 + P(6, 6); + const Scalar _tmp165 = Scalar(0.5) * P(12, 7); + const Scalar _tmp166 = Scalar(0.5) * P(11, 8); + const Scalar _tmp167 = Scalar(0.5) * P(12, 8); + const Scalar _tmp168 = Scalar(0.5) * P(11, 9); + const Scalar _tmp169 = Scalar(0.5) * P(10, 9); + const Scalar _tmp170 = Scalar(0.5) * P(12, 9); // Output terms (1) if (P_new != nullptr) { matrix::Matrix& _p_new = (*P_new); _p_new(0, 0) = _tmp10 * _tmp12 + _tmp11 * _tmp13 + _tmp14 * _tmp9 + _tmp15 * _tmp5 + - _tmp16 * _tmp2 + _tmp17 * _tmp8 + _tmp20 + _tmp21 + _tmp26; + _tmp17 * _tmp2 + _tmp19 * _tmp8 + _tmp22 + _tmp24 + _tmp29; _p_new(1, 0) = 0; _p_new(2, 0) = 0; _p_new(3, 0) = 0; @@ -338,10 +338,10 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 0) = 0; _p_new(22, 0) = 0; _p_new(23, 0) = 0; - _p_new(0, 1) = -_tmp10 * _tmp14 + _tmp12 * _tmp9 - _tmp13 * _tmp27 + _tmp15 * _tmp29 + - _tmp16 * _tmp5 + _tmp17 - _tmp19 * _tmp28 + _tmp21 * _tmp30; - _p_new(1, 1) = -_tmp10 * _tmp31 + _tmp23 - _tmp27 * _tmp33 + _tmp29 * _tmp35 + _tmp30 * _tmp36 + - _tmp32 * _tmp9 + _tmp34 * _tmp5 + _tmp37 + _tmp39; + _p_new(0, 1) = -_tmp10 * _tmp14 + _tmp12 * _tmp9 - _tmp13 * _tmp30 + _tmp15 * _tmp32 + + _tmp17 * _tmp5 + _tmp19 + _tmp24 * _tmp33 - _tmp31 * state(1, 0); + _p_new(1, 1) = -_tmp10 * _tmp34 + _tmp26 - _tmp30 * _tmp36 + _tmp32 * _tmp38 + _tmp33 * _tmp39 + + _tmp35 * _tmp9 + _tmp37 * _tmp5 + _tmp40 + _tmp42; _p_new(2, 1) = 0; _p_new(3, 1) = 0; _p_new(4, 1) = 0; @@ -364,12 +364,12 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 1) = 0; _p_new(22, 1) = 0; _p_new(23, 1) = 0; - _p_new(0, 2) = _tmp11 * _tmp14 - _tmp12 * _tmp27 - _tmp13 * _tmp9 + _tmp15 + _tmp16 * _tmp30 + - _tmp17 * _tmp2 - _tmp19 * _tmp40 + _tmp21 * _tmp41; - _p_new(1, 2) = _tmp11 * _tmp31 - _tmp19 * _tmp42 + _tmp2 * _tmp37 - _tmp27 * _tmp32 + - _tmp30 * _tmp34 - _tmp33 * _tmp9 + _tmp35 + _tmp36 * _tmp41; - _p_new(2, 2) = _tmp11 * _tmp43 + _tmp2 * _tmp47 + _tmp25 - _tmp27 * _tmp44 + _tmp30 * _tmp48 + - _tmp39 + _tmp41 * _tmp46 - _tmp45 * _tmp9 + _tmp49; + _p_new(0, 2) = _tmp11 * _tmp14 - _tmp12 * _tmp30 - _tmp13 * _tmp9 + _tmp15 + _tmp17 * _tmp33 + + _tmp19 * _tmp2 + _tmp24 * _tmp43 - _tmp31 * state(2, 0); + _p_new(1, 2) = _tmp11 * _tmp34 + _tmp2 * _tmp40 - _tmp21 * state(1, 0) * state(2, 0) - + _tmp30 * _tmp35 + _tmp33 * _tmp37 - _tmp36 * _tmp9 + _tmp38 + _tmp39 * _tmp43; + _p_new(2, 2) = _tmp11 * _tmp44 + _tmp2 * _tmp48 + _tmp28 - _tmp30 * _tmp45 + _tmp33 * _tmp49 + + _tmp42 + _tmp43 * _tmp47 - _tmp46 * _tmp9 + _tmp50; _p_new(3, 2) = 0; _p_new(4, 2) = 0; _p_new(5, 2) = 0; @@ -391,14 +391,14 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 2) = 0; _p_new(22, 2) = 0; _p_new(23, 2) = 0; - _p_new(0, 3) = _tmp10 * _tmp13 - _tmp11 * _tmp12 - _tmp14 * _tmp27 + _tmp15 * _tmp8 + _tmp16 + - _tmp17 * _tmp41 - _tmp19 * _tmp50 + _tmp21 * _tmp29; - _p_new(1, 3) = _tmp10 * _tmp33 - _tmp11 * _tmp32 - _tmp19 * _tmp51 - _tmp27 * _tmp31 + - _tmp29 * _tmp36 + _tmp34 + _tmp35 * _tmp8 + _tmp37 * _tmp41; - _p_new(2, 3) = _tmp10 * _tmp45 - _tmp11 * _tmp44 - _tmp19 * _tmp52 - _tmp27 * _tmp43 + - _tmp29 * _tmp46 + _tmp41 * _tmp47 + _tmp48 + _tmp49 * _tmp8; - _p_new(3, 3) = _tmp10 * _tmp55 - _tmp11 * _tmp54 + _tmp26 - _tmp27 * _tmp53 + _tmp29 * _tmp57 + - _tmp38 + _tmp41 * _tmp58 + _tmp56 * _tmp8 + _tmp59; + _p_new(0, 3) = _tmp10 * _tmp13 - _tmp11 * _tmp12 - _tmp14 * _tmp30 + _tmp15 * _tmp8 + _tmp17 + + _tmp19 * _tmp43 + _tmp24 * _tmp32 - _tmp51 * state(0, 0); + _p_new(1, 3) = _tmp10 * _tmp36 - _tmp11 * _tmp35 - _tmp30 * _tmp34 + _tmp32 * _tmp39 + _tmp37 + + _tmp38 * _tmp8 + _tmp40 * _tmp43 - _tmp51 * state(1, 0); + _p_new(2, 3) = _tmp10 * _tmp46 - _tmp11 * _tmp45 - _tmp30 * _tmp44 + _tmp32 * _tmp47 + + _tmp43 * _tmp48 + _tmp49 + _tmp50 * _tmp8 - _tmp51 * state(2, 0); + _p_new(3, 3) = _tmp10 * _tmp54 - _tmp11 * _tmp53 + _tmp29 - _tmp30 * _tmp52 + _tmp32 * _tmp56 + + _tmp41 + _tmp43 * _tmp57 + _tmp55 * _tmp8 + _tmp58; _p_new(4, 3) = 0; _p_new(5, 3) = 0; _p_new(6, 3) = 0; @@ -419,19 +419,19 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 3) = 0; _p_new(22, 3) = 0; _p_new(23, 3) = 0; - _p_new(0, 4) = _tmp15 * _tmp86 + _tmp16 * _tmp74 + _tmp17 * _tmp66 + _tmp21 * _tmp69 - - _tmp75 * _tmp76 - _tmp77 * _tmp78 - _tmp81 * _tmp82 + _tmp87; - _p_new(1, 4) = _tmp34 * _tmp74 + _tmp35 * _tmp86 + _tmp36 * _tmp69 + _tmp37 * _tmp66 - - _tmp76 * _tmp89 - _tmp78 * _tmp90 - _tmp81 * _tmp88 + _tmp91; - _p_new(2, 4) = _tmp46 * _tmp69 + _tmp47 * _tmp66 + _tmp48 * _tmp74 + _tmp49 * _tmp86 - - _tmp76 * _tmp92 - _tmp78 * _tmp93 - _tmp81 * _tmp94 + _tmp96; - _p_new(3, 4) = _tmp100 + _tmp56 * _tmp86 + _tmp57 * _tmp69 + _tmp58 * _tmp66 + _tmp59 * _tmp74 - - _tmp76 * _tmp99 - _tmp78 * _tmp98 - _tmp81 * _tmp97; - _p_new(4, 4) = -_tmp101 * _tmp81 - _tmp102 * _tmp78 - _tmp103 * _tmp76 + - std::pow(_tmp106, Scalar(2)) * d_vel_var(2, 0) + - std::pow(_tmp109, Scalar(2)) * d_vel_var(1, 0) + _tmp110 * _tmp69 + - _tmp111 * _tmp66 + _tmp112 * _tmp86 + _tmp113 * _tmp74 + _tmp114 + - std::pow(_tmp81, Scalar(2)) * d_vel_var(0, 0); + _p_new(0, 4) = _tmp15 * _tmp91 + _tmp17 * _tmp75 + _tmp19 * _tmp65 + _tmp24 * _tmp68 - + _tmp78 * _tmp79 - _tmp82 * _tmp83 - _tmp86 * _tmp87 + _tmp93; + _p_new(1, 4) = _tmp37 * _tmp75 + _tmp38 * _tmp91 + _tmp39 * _tmp68 + _tmp40 * _tmp65 - + _tmp78 * _tmp95 - _tmp82 * _tmp96 - _tmp86 * _tmp94 + _tmp98; + _p_new(2, 4) = -_tmp100 * _tmp82 - _tmp101 * _tmp86 + _tmp102 + _tmp47 * _tmp68 + + _tmp48 * _tmp65 + _tmp49 * _tmp75 + _tmp50 * _tmp91 - _tmp78 * _tmp99; + _p_new(3, 4) = -_tmp103 * _tmp86 - _tmp104 * _tmp82 - _tmp105 * _tmp78 + _tmp106 + + _tmp55 * _tmp91 + _tmp56 * _tmp68 + _tmp57 * _tmp65 + _tmp58 * _tmp75; + _p_new(4, 4) = -_tmp107 * _tmp86 - _tmp108 * _tmp82 - _tmp109 * _tmp78 + _tmp110 * _tmp68 + + _tmp111 * _tmp65 + _tmp112 * _tmp91 + _tmp113 * _tmp75 + _tmp114 + + std::pow(_tmp78, Scalar(2)) * d_vel_var(2, 0) + + std::pow(_tmp82, Scalar(2)) * d_vel_var(1, 0) + + std::pow(_tmp86, Scalar(2)) * d_vel_var(0, 0); _p_new(5, 4) = 0; _p_new(6, 4) = 0; _p_new(7, 4) = 0; @@ -451,22 +451,22 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 4) = 0; _p_new(22, 4) = 0; _p_new(23, 4) = 0; - _p_new(0, 5) = _tmp117 * _tmp15 + _tmp119 * _tmp21 + _tmp121 * _tmp16 - _tmp122 * _tmp82 - - _tmp123 * _tmp75 - _tmp125 * _tmp77 + _tmp128 * _tmp17 + _tmp129; - _p_new(1, 5) = _tmp117 * _tmp35 + _tmp119 * _tmp36 + _tmp121 * _tmp34 - _tmp122 * _tmp88 - - _tmp123 * _tmp89 - _tmp125 * _tmp90 + _tmp128 * _tmp37 + _tmp130; - _p_new(2, 5) = _tmp117 * _tmp49 + _tmp119 * _tmp46 + _tmp121 * _tmp48 - _tmp122 * _tmp94 - - _tmp123 * _tmp92 - _tmp125 * _tmp93 + _tmp128 * _tmp47 + _tmp132; - _p_new(3, 5) = _tmp117 * _tmp56 + _tmp119 * _tmp57 + _tmp121 * _tmp59 - _tmp122 * _tmp97 - - _tmp123 * _tmp99 - _tmp125 * _tmp98 + _tmp128 * _tmp58 + _tmp133; - _p_new(4, 5) = -_tmp101 * _tmp122 - _tmp102 * _tmp125 - _tmp103 * _tmp123 + _tmp106 * _tmp139 + - _tmp109 * _tmp125 * d_vel_var(1, 0) + _tmp110 * _tmp119 + _tmp111 * _tmp128 + - _tmp112 * _tmp117 + _tmp113 * _tmp121 + _tmp134 * _tmp135 + _tmp140; - _p_new(5, 5) = _tmp117 * _tmp147 + _tmp119 * _tmp146 + _tmp121 * _tmp141 - _tmp122 * _tmp143 - - _tmp123 * _tmp144 + std::pow(_tmp125, Scalar(2)) * d_vel_var(1, 0) - - _tmp125 * _tmp145 + _tmp128 * _tmp142 + - std::pow(_tmp134, Scalar(2)) * d_vel_var(0, 0) + - std::pow(_tmp138, Scalar(2)) * d_vel_var(2, 0) + _tmp148; + _p_new(0, 5) = _tmp116 * _tmp15 + _tmp118 * _tmp24 + _tmp120 * _tmp17 - _tmp121 * _tmp87 - + _tmp124 * _tmp79 - _tmp126 * _tmp83 + _tmp129 * _tmp19 + _tmp131; + _p_new(1, 5) = _tmp116 * _tmp38 + _tmp118 * _tmp39 + _tmp120 * _tmp37 - _tmp121 * _tmp94 - + _tmp124 * _tmp95 - _tmp126 * _tmp96 + _tmp129 * _tmp40 + _tmp132; + _p_new(2, 5) = -_tmp100 * _tmp126 - _tmp101 * _tmp121 + _tmp116 * _tmp50 + _tmp118 * _tmp47 + + _tmp120 * _tmp49 - _tmp124 * _tmp99 + _tmp129 * _tmp48 + _tmp133; + _p_new(3, 5) = -_tmp103 * _tmp121 - _tmp104 * _tmp126 - _tmp105 * _tmp124 + _tmp116 * _tmp55 + + _tmp118 * _tmp56 + _tmp120 * _tmp58 + _tmp129 * _tmp57 + _tmp134; + _p_new(4, 5) = -_tmp107 * _tmp121 - _tmp108 * _tmp126 - _tmp109 * _tmp124 + _tmp110 * _tmp118 + + _tmp111 * _tmp129 + _tmp112 * _tmp116 + _tmp113 * _tmp120 + _tmp121 * _tmp135 + + _tmp124 * _tmp137 + _tmp126 * _tmp136 + _tmp138; + _p_new(5, 5) = _tmp116 * _tmp145 + _tmp118 * _tmp144 + _tmp120 * _tmp140 + + std::pow(_tmp121, Scalar(2)) * d_vel_var(0, 0) - _tmp121 * _tmp141 + + std::pow(_tmp124, Scalar(2)) * d_vel_var(2, 0) - _tmp124 * _tmp142 + + std::pow(_tmp126, Scalar(2)) * d_vel_var(1, 0) - _tmp126 * _tmp143 + + _tmp129 * _tmp139 + _tmp146; _p_new(6, 5) = 0; _p_new(7, 5) = 0; _p_new(8, 5) = 0; @@ -485,33 +485,33 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 5) = 0; _p_new(22, 5) = 0; _p_new(23, 5) = 0; - _p_new(0, 6) = _tmp149 * _tmp16 + _tmp15 * _tmp151 + _tmp150 * _tmp21 + _tmp152 * _tmp17 - - _tmp153 * _tmp82 - _tmp154 * _tmp77 - _tmp155 * _tmp75 + _tmp156; - _p_new(1, 6) = _tmp149 * _tmp34 + _tmp150 * _tmp36 + _tmp151 * _tmp35 + _tmp152 * _tmp37 - - _tmp153 * _tmp88 - _tmp154 * _tmp90 - _tmp155 * _tmp89 + _tmp157; - _p_new(2, 6) = _tmp149 * _tmp48 + _tmp150 * _tmp46 + _tmp151 * _tmp49 + _tmp152 * _tmp47 - - _tmp153 * _tmp94 - _tmp154 * _tmp93 - _tmp155 * _tmp92 + _tmp159; - _p_new(3, 6) = _tmp149 * _tmp59 + _tmp150 * _tmp57 + _tmp151 * _tmp56 + _tmp152 * _tmp58 - - _tmp153 * _tmp97 - _tmp154 * _tmp98 - _tmp155 * _tmp99 + _tmp160; - _p_new(4, 6) = -_tmp101 * _tmp153 - _tmp102 * _tmp154 - _tmp103 * _tmp155 + - _tmp106 * _tmp155 * d_vel_var(2, 0) + _tmp109 * _tmp163 + _tmp110 * _tmp150 + - _tmp111 * _tmp152 + _tmp112 * _tmp151 + _tmp113 * _tmp149 + _tmp135 * _tmp161 + - _tmp164; - _p_new(5, 6) = _tmp125 * _tmp163 + _tmp134 * _tmp161 * d_vel_var(0, 0) + _tmp139 * _tmp155 + - _tmp141 * _tmp149 + _tmp142 * _tmp152 - _tmp143 * _tmp153 - _tmp144 * _tmp155 - - _tmp145 * _tmp154 + _tmp146 * _tmp150 + _tmp147 * _tmp151 + _tmp165; + _p_new(0, 6) = _tmp147 * _tmp17 + _tmp148 * _tmp24 + _tmp149 * _tmp15 + _tmp150 * _tmp19 - + _tmp151 * _tmp87 - _tmp152 * _tmp83 - _tmp153 * _tmp79 + _tmp155; + _p_new(1, 6) = _tmp147 * _tmp37 + _tmp148 * _tmp39 + _tmp149 * _tmp38 + _tmp150 * _tmp40 - + _tmp151 * _tmp94 - _tmp152 * _tmp96 - _tmp153 * _tmp95 + _tmp156; + _p_new(2, 6) = -_tmp100 * _tmp152 - _tmp101 * _tmp151 + _tmp147 * _tmp49 + _tmp148 * _tmp47 + + _tmp149 * _tmp50 + _tmp150 * _tmp48 - _tmp153 * _tmp99 + _tmp157; + _p_new(3, 6) = -_tmp103 * _tmp151 - _tmp104 * _tmp152 - _tmp105 * _tmp153 + _tmp147 * _tmp58 + + _tmp148 * _tmp56 + _tmp149 * _tmp55 + _tmp150 * _tmp57 + _tmp158; + _p_new(4, 6) = -_tmp107 * _tmp151 - _tmp108 * _tmp152 - _tmp109 * _tmp153 + _tmp110 * _tmp148 + + _tmp111 * _tmp150 + _tmp112 * _tmp149 + _tmp113 * _tmp147 + _tmp135 * _tmp151 + + _tmp136 * _tmp152 + _tmp137 * _tmp153 + _tmp159; + _p_new(5, 6) = _tmp121 * _tmp151 * d_vel_var(0, 0) + _tmp124 * _tmp153 * d_vel_var(2, 0) + + _tmp126 * _tmp152 * d_vel_var(1, 0) + _tmp139 * _tmp150 + _tmp140 * _tmp147 - + _tmp141 * _tmp151 - _tmp142 * _tmp153 - _tmp143 * _tmp152 + _tmp144 * _tmp148 + + _tmp145 * _tmp149 + _tmp160; _p_new(6, 6) = - _tmp149 * (P(0, 3) * _tmp150 + P(1, 3) * _tmp152 - P(13, 3) * _tmp153 - P(14, 3) * _tmp154 - - P(15, 3) * _tmp155 + P(2, 3) * _tmp151 + P(3, 3) * _tmp149 + P(6, 3)) + - _tmp150 * (P(0, 0) * _tmp150 + P(1, 0) * _tmp152 - P(13, 0) * _tmp153 - P(14, 0) * _tmp154 - - P(15, 0) * _tmp155 + P(2, 0) * _tmp151 + P(3, 0) * _tmp149 + P(6, 0)) + - _tmp151 * (P(0, 2) * _tmp150 + P(1, 2) * _tmp152 - P(13, 2) * _tmp153 - P(14, 2) * _tmp154 - - P(15, 2) * _tmp155 + P(2, 2) * _tmp151 + P(3, 2) * _tmp149 + P(6, 2)) + - _tmp152 * (P(0, 1) * _tmp150 + P(1, 1) * _tmp152 - P(13, 1) * _tmp153 - P(14, 1) * _tmp154 - - P(15, 1) * _tmp155 + P(2, 1) * _tmp151 + P(3, 1) * _tmp149 + P(6, 1)) - - _tmp153 * _tmp166 - _tmp154 * _tmp167 + std::pow(_tmp155, Scalar(2)) * d_vel_var(2, 0) - - _tmp155 * _tmp168 + std::pow(_tmp161, Scalar(2)) * d_vel_var(0, 0) + - std::pow(_tmp162, Scalar(2)) * d_vel_var(1, 0) + _tmp169; + _tmp147 * (P(0, 3) * _tmp148 + P(1, 3) * _tmp150 - P(13, 3) * _tmp151 - P(14, 3) * _tmp152 - + P(15, 3) * _tmp153 + P(2, 3) * _tmp149 + P(3, 3) * _tmp147 + P(6, 3)) + + _tmp148 * (P(0, 0) * _tmp148 + P(1, 0) * _tmp150 - P(13, 0) * _tmp151 - P(14, 0) * _tmp152 - + P(15, 0) * _tmp153 + P(2, 0) * _tmp149 + P(3, 0) * _tmp147 + P(6, 0)) + + _tmp149 * (P(0, 2) * _tmp148 + P(1, 2) * _tmp150 - P(13, 2) * _tmp151 - P(14, 2) * _tmp152 - + P(15, 2) * _tmp153 + P(2, 2) * _tmp149 + P(3, 2) * _tmp147 + P(6, 2)) + + _tmp150 * (P(0, 1) * _tmp148 + P(1, 1) * _tmp150 - P(13, 1) * _tmp151 - P(14, 1) * _tmp152 - + P(15, 1) * _tmp153 + P(2, 1) * _tmp149 + P(3, 1) * _tmp147 + P(6, 1)) + + std::pow(_tmp151, Scalar(2)) * d_vel_var(0, 0) - _tmp151 * _tmp161 + + std::pow(_tmp152, Scalar(2)) * d_vel_var(1, 0) - _tmp152 * _tmp162 + + std::pow(_tmp153, Scalar(2)) * d_vel_var(2, 0) - _tmp153 * _tmp163 + _tmp164; _p_new(7, 6) = 0; _p_new(8, 6) = 0; _p_new(9, 6) = 0; @@ -530,25 +530,25 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(22, 6) = 0; _p_new(23, 6) = 0; _p_new(0, 7) = P(0, 7) + P(1, 7) * _tmp8 + P(10, 7) * _tmp11 + P(11, 7) * _tmp10 + - P(12, 7) * _tmp9 + P(2, 7) * _tmp5 + P(3, 7) * _tmp2 + _tmp87 * dt; - _p_new(1, 7) = P(0, 7) * _tmp30 + P(1, 7) - P(10, 7) * _tmp27 + P(11, 7) * _tmp9 - - P(12, 7) * _tmp10 + P(2, 7) * _tmp29 + P(3, 7) * _tmp5 + _tmp91 * dt; - _p_new(2, 7) = P(0, 7) * _tmp41 + P(1, 7) * _tmp2 - P(10, 7) * _tmp9 - P(11, 7) * _tmp27 + - P(12, 7) * _tmp11 + P(2, 7) + P(3, 7) * _tmp30 + _tmp96 * dt; - _p_new(3, 7) = P(0, 7) * _tmp29 + P(1, 7) * _tmp41 + P(10, 7) * _tmp10 - P(11, 7) * _tmp11 - - P(12, 7) * _tmp27 + P(2, 7) * _tmp8 + P(3, 7) + _tmp100 * dt; - _p_new(4, 7) = P(0, 7) * _tmp69 + P(1, 7) * _tmp66 - P(13, 7) * _tmp81 - P(14, 7) * _tmp78 - - P(15, 7) * _tmp76 + P(2, 7) * _tmp86 + P(3, 7) * _tmp74 + P(4, 7) + _tmp114 * dt; + P(2, 7) * _tmp5 + P(3, 7) * _tmp2 + _tmp165 * state(3, 0) + _tmp93 * dt; + _p_new(1, 7) = P(0, 7) * _tmp33 + P(1, 7) - P(10, 7) * _tmp30 + P(11, 7) * _tmp9 + + P(2, 7) * _tmp32 + P(3, 7) * _tmp5 - _tmp165 * state(2, 0) + _tmp98 * dt; + _p_new(2, 7) = P(0, 7) * _tmp43 + P(1, 7) * _tmp2 - P(10, 7) * _tmp9 - P(11, 7) * _tmp30 + + P(2, 7) + P(3, 7) * _tmp33 + _tmp102 * dt + _tmp165 * state(1, 0); + _p_new(3, 7) = P(0, 7) * _tmp32 + P(1, 7) * _tmp43 + P(10, 7) * _tmp10 - P(11, 7) * _tmp11 + + P(2, 7) * _tmp8 + P(3, 7) + _tmp106 * dt - _tmp165 * state(0, 0); + _p_new(4, 7) = P(0, 7) * _tmp68 + P(1, 7) * _tmp65 - P(13, 7) * _tmp86 - P(14, 7) * _tmp82 - + P(15, 7) * _tmp78 + P(2, 7) * _tmp91 + P(3, 7) * _tmp75 + P(4, 7) + _tmp114 * dt; _p_new(5, 7) = - P(0, 7) * _tmp119 + P(1, 7) * _tmp128 - P(13, 7) * _tmp122 - P(14, 7) * _tmp125 - - P(15, 7) * _tmp123 + P(2, 7) * _tmp117 + P(3, 7) * _tmp121 + P(5, 7) + - dt * (P(0, 4) * _tmp119 + P(1, 4) * _tmp128 - P(13, 4) * _tmp122 - P(14, 4) * _tmp125 - - P(15, 4) * _tmp123 + P(2, 4) * _tmp117 + P(3, 4) * _tmp121 + P(5, 4)); + P(0, 7) * _tmp118 + P(1, 7) * _tmp129 - P(13, 7) * _tmp121 - P(14, 7) * _tmp126 - + P(15, 7) * _tmp124 + P(2, 7) * _tmp116 + P(3, 7) * _tmp120 + P(5, 7) + + dt * (P(0, 4) * _tmp118 + P(1, 4) * _tmp129 - P(13, 4) * _tmp121 - P(14, 4) * _tmp126 - + P(15, 4) * _tmp124 + P(2, 4) * _tmp116 + P(3, 4) * _tmp120 + P(5, 4)); _p_new(6, 7) = - P(0, 7) * _tmp150 + P(1, 7) * _tmp152 - P(13, 7) * _tmp153 - P(14, 7) * _tmp154 - - P(15, 7) * _tmp155 + P(2, 7) * _tmp151 + P(3, 7) * _tmp149 + P(6, 7) + - dt * (P(0, 4) * _tmp150 + P(1, 4) * _tmp152 - P(13, 4) * _tmp153 - P(14, 4) * _tmp154 - - P(15, 4) * _tmp155 + P(2, 4) * _tmp151 + P(3, 4) * _tmp149 + P(6, 4)); + P(0, 7) * _tmp148 + P(1, 7) * _tmp150 - P(13, 7) * _tmp151 - P(14, 7) * _tmp152 - + P(15, 7) * _tmp153 + P(2, 7) * _tmp149 + P(3, 7) * _tmp147 + P(6, 7) + + dt * (P(0, 4) * _tmp148 + P(1, 4) * _tmp150 - P(13, 4) * _tmp151 - P(14, 4) * _tmp152 - + P(15, 4) * _tmp153 + P(2, 4) * _tmp149 + P(3, 4) * _tmp147 + P(6, 4)); _p_new(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4)); _p_new(8, 7) = 0; _p_new(9, 7) = 0; @@ -566,24 +566,24 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 7) = 0; _p_new(22, 7) = 0; _p_new(23, 7) = 0; - _p_new(0, 8) = P(0, 8) + P(1, 8) * _tmp8 + P(10, 8) * _tmp11 + P(11, 8) * _tmp10 + - P(12, 8) * _tmp9 + P(2, 8) * _tmp5 + P(3, 8) * _tmp2 + _tmp129 * dt; - _p_new(1, 8) = P(0, 8) * _tmp30 + P(1, 8) - P(10, 8) * _tmp27 + P(11, 8) * _tmp9 - - P(12, 8) * _tmp10 + P(2, 8) * _tmp29 + P(3, 8) * _tmp5 + _tmp130 * dt; - _p_new(2, 8) = P(0, 8) * _tmp41 + P(1, 8) * _tmp2 - P(10, 8) * _tmp9 + P(12, 8) * _tmp11 + - P(2, 8) + P(3, 8) * _tmp30 + _tmp132 * dt - _tmp170 * state(0, 0); - _p_new(3, 8) = P(0, 8) * _tmp29 + P(1, 8) * _tmp41 + P(10, 8) * _tmp10 - P(12, 8) * _tmp27 + - P(2, 8) * _tmp8 + P(3, 8) + _tmp133 * dt - _tmp170 * state(1, 0); - _p_new(4, 8) = P(0, 8) * _tmp69 + P(1, 8) * _tmp66 - P(13, 8) * _tmp81 - P(14, 8) * _tmp78 - - P(15, 8) * _tmp76 + P(2, 8) * _tmp86 + P(3, 8) * _tmp74 + P(4, 8) + _tmp140 * dt; - _p_new(5, 8) = P(0, 8) * _tmp119 + P(1, 8) * _tmp128 - P(13, 8) * _tmp122 - P(14, 8) * _tmp125 - - P(15, 8) * _tmp123 + P(2, 8) * _tmp117 + P(3, 8) * _tmp121 + P(5, 8) + - _tmp148 * dt; + _p_new(0, 8) = P(0, 8) + P(1, 8) * _tmp8 + P(10, 8) * _tmp11 + P(12, 8) * _tmp9 + + P(2, 8) * _tmp5 + P(3, 8) * _tmp2 + _tmp131 * dt + _tmp166 * state(2, 0); + _p_new(1, 8) = P(0, 8) * _tmp33 + P(1, 8) - P(10, 8) * _tmp30 + P(2, 8) * _tmp32 + + P(3, 8) * _tmp5 + _tmp132 * dt + _tmp166 * state(3, 0) - _tmp167 * state(2, 0); + _p_new(2, 8) = P(0, 8) * _tmp43 + P(1, 8) * _tmp2 - P(10, 8) * _tmp9 + P(12, 8) * _tmp11 + + P(2, 8) + P(3, 8) * _tmp33 + _tmp133 * dt - _tmp166 * state(0, 0); + _p_new(3, 8) = P(0, 8) * _tmp32 + P(1, 8) * _tmp43 + P(10, 8) * _tmp10 + P(2, 8) * _tmp8 + + P(3, 8) + _tmp134 * dt - _tmp166 * state(1, 0) - _tmp167 * state(0, 0); + _p_new(4, 8) = P(0, 8) * _tmp68 + P(1, 8) * _tmp65 - P(13, 8) * _tmp86 - P(14, 8) * _tmp82 - + P(15, 8) * _tmp78 + P(2, 8) * _tmp91 + P(3, 8) * _tmp75 + P(4, 8) + _tmp138 * dt; + _p_new(5, 8) = P(0, 8) * _tmp118 + P(1, 8) * _tmp129 - P(13, 8) * _tmp121 - P(14, 8) * _tmp126 - + P(15, 8) * _tmp124 + P(2, 8) * _tmp116 + P(3, 8) * _tmp120 + P(5, 8) + + _tmp146 * dt; _p_new(6, 8) = - P(0, 8) * _tmp150 + P(1, 8) * _tmp152 - P(13, 8) * _tmp153 - P(14, 8) * _tmp154 - - P(15, 8) * _tmp155 + P(2, 8) * _tmp151 + P(3, 8) * _tmp149 + P(6, 8) + - dt * (P(0, 5) * _tmp150 + P(1, 5) * _tmp152 - P(13, 5) * _tmp153 - P(14, 5) * _tmp154 - - P(15, 5) * _tmp155 + P(2, 5) * _tmp151 + P(3, 5) * _tmp149 + P(6, 5)); + P(0, 8) * _tmp148 + P(1, 8) * _tmp150 - P(13, 8) * _tmp151 - P(14, 8) * _tmp152 - + P(15, 8) * _tmp153 + P(2, 8) * _tmp149 + P(3, 8) * _tmp147 + P(6, 8) + + dt * (P(0, 5) * _tmp148 + P(1, 5) * _tmp150 - P(13, 5) * _tmp151 - P(14, 5) * _tmp152 - + P(15, 5) * _tmp153 + P(2, 5) * _tmp149 + P(3, 5) * _tmp147 + P(6, 5)); _p_new(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5)); _p_new(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5)); _p_new(9, 8) = 0; @@ -601,22 +601,22 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 8) = 0; _p_new(22, 8) = 0; _p_new(23, 8) = 0; - _p_new(0, 9) = P(0, 9) + P(1, 9) * _tmp8 + P(10, 9) * _tmp11 + P(11, 9) * _tmp10 + - P(12, 9) * _tmp9 + P(2, 9) * _tmp5 + P(3, 9) * _tmp2 + _tmp156 * dt; - _p_new(1, 9) = P(0, 9) * _tmp30 + P(1, 9) - P(10, 9) * _tmp27 + P(11, 9) * _tmp9 - - P(12, 9) * _tmp10 + P(2, 9) * _tmp29 + P(3, 9) * _tmp5 + _tmp157 * dt; - _p_new(2, 9) = P(0, 9) * _tmp41 + P(1, 9) * _tmp2 - P(10, 9) * _tmp9 - P(11, 9) * _tmp27 + - P(12, 9) * _tmp11 + P(2, 9) + P(3, 9) * _tmp30 + _tmp159 * dt; - _p_new(3, 9) = P(0, 9) * _tmp29 + P(1, 9) * _tmp41 + P(10, 9) * _tmp10 - P(11, 9) * _tmp11 - - P(12, 9) * _tmp27 + P(2, 9) * _tmp8 + P(3, 9) + _tmp160 * dt; - _p_new(4, 9) = P(0, 9) * _tmp69 + P(1, 9) * _tmp66 - P(13, 9) * _tmp81 - P(14, 9) * _tmp78 - - P(15, 9) * _tmp76 + P(2, 9) * _tmp86 + P(3, 9) * _tmp74 + P(4, 9) + _tmp164 * dt; - _p_new(5, 9) = P(0, 9) * _tmp119 + P(1, 9) * _tmp128 - P(13, 9) * _tmp122 - P(14, 9) * _tmp125 - - P(15, 9) * _tmp123 + P(2, 9) * _tmp117 + P(3, 9) * _tmp121 + P(5, 9) + - _tmp165 * dt; - _p_new(6, 9) = P(0, 9) * _tmp150 + P(1, 9) * _tmp152 - P(13, 9) * _tmp153 - P(14, 9) * _tmp154 - - P(15, 9) * _tmp155 + P(2, 9) * _tmp151 + P(3, 9) * _tmp149 + P(6, 9) + - _tmp169 * dt; + _p_new(0, 9) = P(0, 9) + P(1, 9) * _tmp8 + P(10, 9) * _tmp11 + P(12, 9) * _tmp9 + + P(2, 9) * _tmp5 + P(3, 9) * _tmp2 + _tmp155 * dt + _tmp168 * state(2, 0); + _p_new(1, 9) = P(0, 9) * _tmp33 + P(1, 9) + P(11, 9) * _tmp9 + P(2, 9) * _tmp32 + + P(3, 9) * _tmp5 + _tmp156 * dt - _tmp169 * state(0, 0) - _tmp170 * state(2, 0); + _p_new(2, 9) = P(0, 9) * _tmp43 + P(1, 9) * _tmp2 - P(10, 9) * _tmp9 + P(12, 9) * _tmp11 + + P(2, 9) + P(3, 9) * _tmp33 + _tmp157 * dt - _tmp168 * state(0, 0); + _p_new(3, 9) = P(0, 9) * _tmp32 + P(1, 9) * _tmp43 - P(11, 9) * _tmp11 + P(2, 9) * _tmp8 + + P(3, 9) + _tmp158 * dt + _tmp169 * state(2, 0) - _tmp170 * state(0, 0); + _p_new(4, 9) = P(0, 9) * _tmp68 + P(1, 9) * _tmp65 - P(13, 9) * _tmp86 - P(14, 9) * _tmp82 - + P(15, 9) * _tmp78 + P(2, 9) * _tmp91 + P(3, 9) * _tmp75 + P(4, 9) + _tmp159 * dt; + _p_new(5, 9) = P(0, 9) * _tmp118 + P(1, 9) * _tmp129 - P(13, 9) * _tmp121 - P(14, 9) * _tmp126 - + P(15, 9) * _tmp124 + P(2, 9) * _tmp116 + P(3, 9) * _tmp120 + P(5, 9) + + _tmp160 * dt; + _p_new(6, 9) = P(0, 9) * _tmp148 + P(1, 9) * _tmp150 - P(13, 9) * _tmp151 - P(14, 9) * _tmp152 - + P(15, 9) * _tmp153 + P(2, 9) * _tmp149 + P(3, 9) * _tmp147 + P(6, 9) + + _tmp164 * dt; _p_new(7, 9) = P(4, 9) * dt + P(7, 9) + dt * (P(4, 6) * dt + P(7, 6)); _p_new(8, 9) = P(5, 9) * dt + P(8, 9) + dt * (P(5, 6) * dt + P(8, 6)); _p_new(9, 9) = P(6, 9) * dt + P(9, 9) + dt * (P(6, 6) * dt + P(9, 6)); @@ -635,18 +635,18 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(22, 9) = 0; _p_new(23, 9) = 0; _p_new(0, 10) = _tmp13; - _p_new(1, 10) = _tmp33; - _p_new(2, 10) = _tmp45; - _p_new(3, 10) = _tmp55; - _p_new(4, 10) = P(0, 10) * _tmp69 + P(1, 10) * _tmp66 - P(13, 10) * _tmp81 - - P(14, 10) * _tmp78 - P(15, 10) * _tmp76 + P(2, 10) * _tmp86 + - P(3, 10) * _tmp74 + P(4, 10); - _p_new(5, 10) = P(0, 10) * _tmp119 + P(1, 10) * _tmp128 - P(13, 10) * _tmp122 - - P(14, 10) * _tmp125 - P(15, 10) * _tmp123 + P(2, 10) * _tmp117 + - P(3, 10) * _tmp121 + P(5, 10); - _p_new(6, 10) = P(0, 10) * _tmp150 + P(1, 10) * _tmp152 - P(13, 10) * _tmp153 - - P(14, 10) * _tmp154 - P(15, 10) * _tmp155 + P(2, 10) * _tmp151 + - P(3, 10) * _tmp149 + P(6, 10); + _p_new(1, 10) = _tmp36; + _p_new(2, 10) = _tmp46; + _p_new(3, 10) = _tmp54; + _p_new(4, 10) = P(0, 10) * _tmp68 + P(1, 10) * _tmp65 - P(13, 10) * _tmp86 - + P(14, 10) * _tmp82 - P(15, 10) * _tmp78 + P(2, 10) * _tmp91 + + P(3, 10) * _tmp75 + P(4, 10); + _p_new(5, 10) = P(0, 10) * _tmp118 + P(1, 10) * _tmp129 - P(13, 10) * _tmp121 - + P(14, 10) * _tmp126 - P(15, 10) * _tmp124 + P(2, 10) * _tmp116 + + P(3, 10) * _tmp120 + P(5, 10); + _p_new(6, 10) = P(0, 10) * _tmp148 + P(1, 10) * _tmp150 - P(13, 10) * _tmp151 - + P(14, 10) * _tmp152 - P(15, 10) * _tmp153 + P(2, 10) * _tmp149 + + P(3, 10) * _tmp147 + P(6, 10); _p_new(7, 10) = P(4, 10) * dt + P(7, 10); _p_new(8, 10) = P(5, 10) * dt + P(8, 10); _p_new(9, 10) = P(6, 10) * dt + P(9, 10); @@ -665,18 +665,18 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(22, 10) = 0; _p_new(23, 10) = 0; _p_new(0, 11) = _tmp12; - _p_new(1, 11) = _tmp32; - _p_new(2, 11) = _tmp44; - _p_new(3, 11) = _tmp54; - _p_new(4, 11) = P(0, 11) * _tmp69 + P(1, 11) * _tmp66 - P(13, 11) * _tmp81 - - P(14, 11) * _tmp78 - P(15, 11) * _tmp76 + P(2, 11) * _tmp86 + - P(3, 11) * _tmp74 + P(4, 11); - _p_new(5, 11) = P(0, 11) * _tmp119 + P(1, 11) * _tmp128 - P(13, 11) * _tmp122 - - P(14, 11) * _tmp125 - P(15, 11) * _tmp123 + P(2, 11) * _tmp117 + - P(3, 11) * _tmp121 + P(5, 11); - _p_new(6, 11) = P(0, 11) * _tmp150 + P(1, 11) * _tmp152 - P(13, 11) * _tmp153 - - P(14, 11) * _tmp154 - P(15, 11) * _tmp155 + P(2, 11) * _tmp151 + - P(3, 11) * _tmp149 + P(6, 11); + _p_new(1, 11) = _tmp35; + _p_new(2, 11) = _tmp45; + _p_new(3, 11) = _tmp53; + _p_new(4, 11) = P(0, 11) * _tmp68 + P(1, 11) * _tmp65 - P(13, 11) * _tmp86 - + P(14, 11) * _tmp82 - P(15, 11) * _tmp78 + P(2, 11) * _tmp91 + + P(3, 11) * _tmp75 + P(4, 11); + _p_new(5, 11) = P(0, 11) * _tmp118 + P(1, 11) * _tmp129 - P(13, 11) * _tmp121 - + P(14, 11) * _tmp126 - P(15, 11) * _tmp124 + P(2, 11) * _tmp116 + + P(3, 11) * _tmp120 + P(5, 11); + _p_new(6, 11) = P(0, 11) * _tmp148 + P(1, 11) * _tmp150 - P(13, 11) * _tmp151 - + P(14, 11) * _tmp152 - P(15, 11) * _tmp153 + P(2, 11) * _tmp149 + + P(3, 11) * _tmp147 + P(6, 11); _p_new(7, 11) = P(4, 11) * dt + P(7, 11); _p_new(8, 11) = P(5, 11) * dt + P(8, 11); _p_new(9, 11) = P(6, 11) * dt + P(9, 11); @@ -695,18 +695,18 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(22, 11) = 0; _p_new(23, 11) = 0; _p_new(0, 12) = _tmp14; - _p_new(1, 12) = _tmp31; - _p_new(2, 12) = _tmp43; - _p_new(3, 12) = _tmp53; - _p_new(4, 12) = P(0, 12) * _tmp69 + P(1, 12) * _tmp66 - P(13, 12) * _tmp81 - - P(14, 12) * _tmp78 - P(15, 12) * _tmp76 + P(2, 12) * _tmp86 + - P(3, 12) * _tmp74 + P(4, 12); - _p_new(5, 12) = P(0, 12) * _tmp119 + P(1, 12) * _tmp128 - P(13, 12) * _tmp122 - - P(14, 12) * _tmp125 - P(15, 12) * _tmp123 + P(2, 12) * _tmp117 + - P(3, 12) * _tmp121 + P(5, 12); - _p_new(6, 12) = P(0, 12) * _tmp150 + P(1, 12) * _tmp152 - P(13, 12) * _tmp153 - - P(14, 12) * _tmp154 - P(15, 12) * _tmp155 + P(2, 12) * _tmp151 + - P(3, 12) * _tmp149 + P(6, 12); + _p_new(1, 12) = _tmp34; + _p_new(2, 12) = _tmp44; + _p_new(3, 12) = _tmp52; + _p_new(4, 12) = P(0, 12) * _tmp68 + P(1, 12) * _tmp65 - P(13, 12) * _tmp86 - + P(14, 12) * _tmp82 - P(15, 12) * _tmp78 + P(2, 12) * _tmp91 + + P(3, 12) * _tmp75 + P(4, 12); + _p_new(5, 12) = P(0, 12) * _tmp118 + P(1, 12) * _tmp129 - P(13, 12) * _tmp121 - + P(14, 12) * _tmp126 - P(15, 12) * _tmp124 + P(2, 12) * _tmp116 + + P(3, 12) * _tmp120 + P(5, 12); + _p_new(6, 12) = P(0, 12) * _tmp148 + P(1, 12) * _tmp150 - P(13, 12) * _tmp151 - + P(14, 12) * _tmp152 - P(15, 12) * _tmp153 + P(2, 12) * _tmp149 + + P(3, 12) * _tmp147 + P(6, 12); _p_new(7, 12) = P(4, 12) * dt + P(7, 12); _p_new(8, 12) = P(5, 12) * dt + P(8, 12); _p_new(9, 12) = P(6, 12) * dt + P(9, 12); @@ -724,13 +724,13 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 12) = 0; _p_new(22, 12) = 0; _p_new(23, 12) = 0; - _p_new(0, 13) = _tmp82; - _p_new(1, 13) = _tmp88; - _p_new(2, 13) = _tmp94; - _p_new(3, 13) = _tmp97; - _p_new(4, 13) = _tmp101; - _p_new(5, 13) = _tmp143; - _p_new(6, 13) = _tmp166; + _p_new(0, 13) = _tmp87; + _p_new(1, 13) = _tmp94; + _p_new(2, 13) = _tmp101; + _p_new(3, 13) = _tmp103; + _p_new(4, 13) = _tmp107; + _p_new(5, 13) = _tmp141; + _p_new(6, 13) = _tmp161; _p_new(7, 13) = P(4, 13) * dt + P(7, 13); _p_new(8, 13) = P(5, 13) * dt + P(8, 13); _p_new(9, 13) = P(6, 13) * dt + P(9, 13); @@ -748,13 +748,13 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 13) = 0; _p_new(22, 13) = 0; _p_new(23, 13) = 0; - _p_new(0, 14) = _tmp77; - _p_new(1, 14) = _tmp90; - _p_new(2, 14) = _tmp93; - _p_new(3, 14) = _tmp98; - _p_new(4, 14) = _tmp102; - _p_new(5, 14) = _tmp145; - _p_new(6, 14) = _tmp167; + _p_new(0, 14) = _tmp83; + _p_new(1, 14) = _tmp96; + _p_new(2, 14) = _tmp100; + _p_new(3, 14) = _tmp104; + _p_new(4, 14) = _tmp108; + _p_new(5, 14) = _tmp143; + _p_new(6, 14) = _tmp162; _p_new(7, 14) = P(4, 14) * dt + P(7, 14); _p_new(8, 14) = P(5, 14) * dt + P(8, 14); _p_new(9, 14) = P(6, 14) * dt + P(9, 14); @@ -772,13 +772,13 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 14) = 0; _p_new(22, 14) = 0; _p_new(23, 14) = 0; - _p_new(0, 15) = _tmp75; - _p_new(1, 15) = _tmp89; - _p_new(2, 15) = _tmp92; - _p_new(3, 15) = _tmp99; - _p_new(4, 15) = _tmp103; - _p_new(5, 15) = _tmp144; - _p_new(6, 15) = _tmp168; + _p_new(0, 15) = _tmp79; + _p_new(1, 15) = _tmp95; + _p_new(2, 15) = _tmp99; + _p_new(3, 15) = _tmp105; + _p_new(4, 15) = _tmp109; + _p_new(5, 15) = _tmp142; + _p_new(6, 15) = _tmp163; _p_new(7, 15) = P(4, 15) * dt + P(7, 15); _p_new(8, 15) = P(5, 15) * dt + P(8, 15); _p_new(9, 15) = P(6, 15) * dt + P(9, 15); @@ -798,21 +798,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(23, 15) = 0; _p_new(0, 16) = P(0, 16) + P(1, 16) * _tmp8 + P(10, 16) * _tmp11 + P(11, 16) * _tmp10 + P(12, 16) * _tmp9 + P(2, 16) * _tmp5 + P(3, 16) * _tmp2; - _p_new(1, 16) = P(0, 16) * _tmp30 + P(1, 16) - P(10, 16) * _tmp27 + P(11, 16) * _tmp9 - - P(12, 16) * _tmp10 + P(2, 16) * _tmp29 + P(3, 16) * _tmp5; - _p_new(2, 16) = P(0, 16) * _tmp41 + P(1, 16) * _tmp2 - P(10, 16) * _tmp9 - P(11, 16) * _tmp27 + - P(12, 16) * _tmp11 + P(2, 16) + P(3, 16) * _tmp30; - _p_new(3, 16) = P(0, 16) * _tmp29 + P(1, 16) * _tmp41 + P(10, 16) * _tmp10 - - P(11, 16) * _tmp11 - P(12, 16) * _tmp27 + P(2, 16) * _tmp8 + P(3, 16); - _p_new(4, 16) = P(0, 16) * _tmp69 + P(1, 16) * _tmp66 - P(13, 16) * _tmp81 - - P(14, 16) * _tmp78 - P(15, 16) * _tmp76 + P(2, 16) * _tmp86 + - P(3, 16) * _tmp74 + P(4, 16); - _p_new(5, 16) = P(0, 16) * _tmp119 + P(1, 16) * _tmp128 - P(13, 16) * _tmp122 - - P(14, 16) * _tmp125 - P(15, 16) * _tmp123 + P(2, 16) * _tmp117 + - P(3, 16) * _tmp121 + P(5, 16); - _p_new(6, 16) = P(0, 16) * _tmp150 + P(1, 16) * _tmp152 - P(13, 16) * _tmp153 - - P(14, 16) * _tmp154 - P(15, 16) * _tmp155 + P(2, 16) * _tmp151 + - P(3, 16) * _tmp149 + P(6, 16); + _p_new(1, 16) = P(0, 16) * _tmp33 + P(1, 16) - P(10, 16) * _tmp30 + P(11, 16) * _tmp9 - + P(12, 16) * _tmp10 + P(2, 16) * _tmp32 + P(3, 16) * _tmp5; + _p_new(2, 16) = P(0, 16) * _tmp43 + P(1, 16) * _tmp2 - P(10, 16) * _tmp9 - P(11, 16) * _tmp30 + + P(12, 16) * _tmp11 + P(2, 16) + P(3, 16) * _tmp33; + _p_new(3, 16) = P(0, 16) * _tmp32 + P(1, 16) * _tmp43 + P(10, 16) * _tmp10 - + P(11, 16) * _tmp11 - P(12, 16) * _tmp30 + P(2, 16) * _tmp8 + P(3, 16); + _p_new(4, 16) = P(0, 16) * _tmp68 + P(1, 16) * _tmp65 - P(13, 16) * _tmp86 - + P(14, 16) * _tmp82 - P(15, 16) * _tmp78 + P(2, 16) * _tmp91 + + P(3, 16) * _tmp75 + P(4, 16); + _p_new(5, 16) = P(0, 16) * _tmp118 + P(1, 16) * _tmp129 - P(13, 16) * _tmp121 - + P(14, 16) * _tmp126 - P(15, 16) * _tmp124 + P(2, 16) * _tmp116 + + P(3, 16) * _tmp120 + P(5, 16); + _p_new(6, 16) = P(0, 16) * _tmp148 + P(1, 16) * _tmp150 - P(13, 16) * _tmp151 - + P(14, 16) * _tmp152 - P(15, 16) * _tmp153 + P(2, 16) * _tmp149 + + P(3, 16) * _tmp147 + P(6, 16); _p_new(7, 16) = P(4, 16) * dt + P(7, 16); _p_new(8, 16) = P(5, 16) * dt + P(8, 16); _p_new(9, 16) = P(6, 16) * dt + P(9, 16); @@ -832,21 +832,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(23, 16) = 0; _p_new(0, 17) = P(0, 17) + P(1, 17) * _tmp8 + P(10, 17) * _tmp11 + P(11, 17) * _tmp10 + P(12, 17) * _tmp9 + P(2, 17) * _tmp5 + P(3, 17) * _tmp2; - _p_new(1, 17) = P(0, 17) * _tmp30 + P(1, 17) - P(10, 17) * _tmp27 + P(11, 17) * _tmp9 - - P(12, 17) * _tmp10 + P(2, 17) * _tmp29 + P(3, 17) * _tmp5; - _p_new(2, 17) = P(0, 17) * _tmp41 + P(1, 17) * _tmp2 - P(10, 17) * _tmp9 - P(11, 17) * _tmp27 + - P(12, 17) * _tmp11 + P(2, 17) + P(3, 17) * _tmp30; - _p_new(3, 17) = P(0, 17) * _tmp29 + P(1, 17) * _tmp41 + P(10, 17) * _tmp10 - - P(11, 17) * _tmp11 - P(12, 17) * _tmp27 + P(2, 17) * _tmp8 + P(3, 17); - _p_new(4, 17) = P(0, 17) * _tmp69 + P(1, 17) * _tmp66 - P(13, 17) * _tmp81 - - P(14, 17) * _tmp78 - P(15, 17) * _tmp76 + P(2, 17) * _tmp86 + - P(3, 17) * _tmp74 + P(4, 17); - _p_new(5, 17) = P(0, 17) * _tmp119 + P(1, 17) * _tmp128 - P(13, 17) * _tmp122 - - P(14, 17) * _tmp125 - P(15, 17) * _tmp123 + P(2, 17) * _tmp117 + - P(3, 17) * _tmp121 + P(5, 17); - _p_new(6, 17) = P(0, 17) * _tmp150 + P(1, 17) * _tmp152 - P(13, 17) * _tmp153 - - P(14, 17) * _tmp154 - P(15, 17) * _tmp155 + P(2, 17) * _tmp151 + - P(3, 17) * _tmp149 + P(6, 17); + _p_new(1, 17) = P(0, 17) * _tmp33 + P(1, 17) - P(10, 17) * _tmp30 + P(11, 17) * _tmp9 - + P(12, 17) * _tmp10 + P(2, 17) * _tmp32 + P(3, 17) * _tmp5; + _p_new(2, 17) = P(0, 17) * _tmp43 + P(1, 17) * _tmp2 - P(10, 17) * _tmp9 - P(11, 17) * _tmp30 + + P(12, 17) * _tmp11 + P(2, 17) + P(3, 17) * _tmp33; + _p_new(3, 17) = P(0, 17) * _tmp32 + P(1, 17) * _tmp43 + P(10, 17) * _tmp10 - + P(11, 17) * _tmp11 - P(12, 17) * _tmp30 + P(2, 17) * _tmp8 + P(3, 17); + _p_new(4, 17) = P(0, 17) * _tmp68 + P(1, 17) * _tmp65 - P(13, 17) * _tmp86 - + P(14, 17) * _tmp82 - P(15, 17) * _tmp78 + P(2, 17) * _tmp91 + + P(3, 17) * _tmp75 + P(4, 17); + _p_new(5, 17) = P(0, 17) * _tmp118 + P(1, 17) * _tmp129 - P(13, 17) * _tmp121 - + P(14, 17) * _tmp126 - P(15, 17) * _tmp124 + P(2, 17) * _tmp116 + + P(3, 17) * _tmp120 + P(5, 17); + _p_new(6, 17) = P(0, 17) * _tmp148 + P(1, 17) * _tmp150 - P(13, 17) * _tmp151 - + P(14, 17) * _tmp152 - P(15, 17) * _tmp153 + P(2, 17) * _tmp149 + + P(3, 17) * _tmp147 + P(6, 17); _p_new(7, 17) = P(4, 17) * dt + P(7, 17); _p_new(8, 17) = P(5, 17) * dt + P(8, 17); _p_new(9, 17) = P(6, 17) * dt + P(9, 17); @@ -866,21 +866,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(23, 17) = 0; _p_new(0, 18) = P(0, 18) + P(1, 18) * _tmp8 + P(10, 18) * _tmp11 + P(11, 18) * _tmp10 + P(12, 18) * _tmp9 + P(2, 18) * _tmp5 + P(3, 18) * _tmp2; - _p_new(1, 18) = P(0, 18) * _tmp30 + P(1, 18) - P(10, 18) * _tmp27 + P(11, 18) * _tmp9 - - P(12, 18) * _tmp10 + P(2, 18) * _tmp29 + P(3, 18) * _tmp5; - _p_new(2, 18) = P(0, 18) * _tmp41 + P(1, 18) * _tmp2 - P(10, 18) * _tmp9 - P(11, 18) * _tmp27 + - P(12, 18) * _tmp11 + P(2, 18) + P(3, 18) * _tmp30; - _p_new(3, 18) = P(0, 18) * _tmp29 + P(1, 18) * _tmp41 + P(10, 18) * _tmp10 - - P(11, 18) * _tmp11 - P(12, 18) * _tmp27 + P(2, 18) * _tmp8 + P(3, 18); - _p_new(4, 18) = P(0, 18) * _tmp69 + P(1, 18) * _tmp66 - P(13, 18) * _tmp81 - - P(14, 18) * _tmp78 - P(15, 18) * _tmp76 + P(2, 18) * _tmp86 + - P(3, 18) * _tmp74 + P(4, 18); - _p_new(5, 18) = P(0, 18) * _tmp119 + P(1, 18) * _tmp128 - P(13, 18) * _tmp122 - - P(14, 18) * _tmp125 - P(15, 18) * _tmp123 + P(2, 18) * _tmp117 + - P(3, 18) * _tmp121 + P(5, 18); - _p_new(6, 18) = P(0, 18) * _tmp150 + P(1, 18) * _tmp152 - P(13, 18) * _tmp153 - - P(14, 18) * _tmp154 - P(15, 18) * _tmp155 + P(2, 18) * _tmp151 + - P(3, 18) * _tmp149 + P(6, 18); + _p_new(1, 18) = P(0, 18) * _tmp33 + P(1, 18) - P(10, 18) * _tmp30 + P(11, 18) * _tmp9 - + P(12, 18) * _tmp10 + P(2, 18) * _tmp32 + P(3, 18) * _tmp5; + _p_new(2, 18) = P(0, 18) * _tmp43 + P(1, 18) * _tmp2 - P(10, 18) * _tmp9 - P(11, 18) * _tmp30 + + P(12, 18) * _tmp11 + P(2, 18) + P(3, 18) * _tmp33; + _p_new(3, 18) = P(0, 18) * _tmp32 + P(1, 18) * _tmp43 + P(10, 18) * _tmp10 - + P(11, 18) * _tmp11 - P(12, 18) * _tmp30 + P(2, 18) * _tmp8 + P(3, 18); + _p_new(4, 18) = P(0, 18) * _tmp68 + P(1, 18) * _tmp65 - P(13, 18) * _tmp86 - + P(14, 18) * _tmp82 - P(15, 18) * _tmp78 + P(2, 18) * _tmp91 + + P(3, 18) * _tmp75 + P(4, 18); + _p_new(5, 18) = P(0, 18) * _tmp118 + P(1, 18) * _tmp129 - P(13, 18) * _tmp121 - + P(14, 18) * _tmp126 - P(15, 18) * _tmp124 + P(2, 18) * _tmp116 + + P(3, 18) * _tmp120 + P(5, 18); + _p_new(6, 18) = P(0, 18) * _tmp148 + P(1, 18) * _tmp150 - P(13, 18) * _tmp151 - + P(14, 18) * _tmp152 - P(15, 18) * _tmp153 + P(2, 18) * _tmp149 + + P(3, 18) * _tmp147 + P(6, 18); _p_new(7, 18) = P(4, 18) * dt + P(7, 18); _p_new(8, 18) = P(5, 18) * dt + P(8, 18); _p_new(9, 18) = P(6, 18) * dt + P(9, 18); @@ -900,21 +900,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(23, 18) = 0; _p_new(0, 19) = P(0, 19) + P(1, 19) * _tmp8 + P(10, 19) * _tmp11 + P(11, 19) * _tmp10 + P(12, 19) * _tmp9 + P(2, 19) * _tmp5 + P(3, 19) * _tmp2; - _p_new(1, 19) = P(0, 19) * _tmp30 + P(1, 19) - P(10, 19) * _tmp27 + P(11, 19) * _tmp9 - - P(12, 19) * _tmp10 + P(2, 19) * _tmp29 + P(3, 19) * _tmp5; - _p_new(2, 19) = P(0, 19) * _tmp41 + P(1, 19) * _tmp2 - P(10, 19) * _tmp9 - P(11, 19) * _tmp27 + - P(12, 19) * _tmp11 + P(2, 19) + P(3, 19) * _tmp30; - _p_new(3, 19) = P(0, 19) * _tmp29 + P(1, 19) * _tmp41 + P(10, 19) * _tmp10 - - P(11, 19) * _tmp11 - P(12, 19) * _tmp27 + P(2, 19) * _tmp8 + P(3, 19); - _p_new(4, 19) = P(0, 19) * _tmp69 + P(1, 19) * _tmp66 - P(13, 19) * _tmp81 - - P(14, 19) * _tmp78 - P(15, 19) * _tmp76 + P(2, 19) * _tmp86 + - P(3, 19) * _tmp74 + P(4, 19); - _p_new(5, 19) = P(0, 19) * _tmp119 + P(1, 19) * _tmp128 - P(13, 19) * _tmp122 - - P(14, 19) * _tmp125 - P(15, 19) * _tmp123 + P(2, 19) * _tmp117 + - P(3, 19) * _tmp121 + P(5, 19); - _p_new(6, 19) = P(0, 19) * _tmp150 + P(1, 19) * _tmp152 - P(13, 19) * _tmp153 - - P(14, 19) * _tmp154 - P(15, 19) * _tmp155 + P(2, 19) * _tmp151 + - P(3, 19) * _tmp149 + P(6, 19); + _p_new(1, 19) = P(0, 19) * _tmp33 + P(1, 19) - P(10, 19) * _tmp30 + P(11, 19) * _tmp9 - + P(12, 19) * _tmp10 + P(2, 19) * _tmp32 + P(3, 19) * _tmp5; + _p_new(2, 19) = P(0, 19) * _tmp43 + P(1, 19) * _tmp2 - P(10, 19) * _tmp9 - P(11, 19) * _tmp30 + + P(12, 19) * _tmp11 + P(2, 19) + P(3, 19) * _tmp33; + _p_new(3, 19) = P(0, 19) * _tmp32 + P(1, 19) * _tmp43 + P(10, 19) * _tmp10 - + P(11, 19) * _tmp11 - P(12, 19) * _tmp30 + P(2, 19) * _tmp8 + P(3, 19); + _p_new(4, 19) = P(0, 19) * _tmp68 + P(1, 19) * _tmp65 - P(13, 19) * _tmp86 - + P(14, 19) * _tmp82 - P(15, 19) * _tmp78 + P(2, 19) * _tmp91 + + P(3, 19) * _tmp75 + P(4, 19); + _p_new(5, 19) = P(0, 19) * _tmp118 + P(1, 19) * _tmp129 - P(13, 19) * _tmp121 - + P(14, 19) * _tmp126 - P(15, 19) * _tmp124 + P(2, 19) * _tmp116 + + P(3, 19) * _tmp120 + P(5, 19); + _p_new(6, 19) = P(0, 19) * _tmp148 + P(1, 19) * _tmp150 - P(13, 19) * _tmp151 - + P(14, 19) * _tmp152 - P(15, 19) * _tmp153 + P(2, 19) * _tmp149 + + P(3, 19) * _tmp147 + P(6, 19); _p_new(7, 19) = P(4, 19) * dt + P(7, 19); _p_new(8, 19) = P(5, 19) * dt + P(8, 19); _p_new(9, 19) = P(6, 19) * dt + P(9, 19); @@ -934,21 +934,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(23, 19) = 0; _p_new(0, 20) = P(0, 20) + P(1, 20) * _tmp8 + P(10, 20) * _tmp11 + P(11, 20) * _tmp10 + P(12, 20) * _tmp9 + P(2, 20) * _tmp5 + P(3, 20) * _tmp2; - _p_new(1, 20) = P(0, 20) * _tmp30 + P(1, 20) - P(10, 20) * _tmp27 + P(11, 20) * _tmp9 - - P(12, 20) * _tmp10 + P(2, 20) * _tmp29 + P(3, 20) * _tmp5; - _p_new(2, 20) = P(0, 20) * _tmp41 + P(1, 20) * _tmp2 - P(10, 20) * _tmp9 - P(11, 20) * _tmp27 + - P(12, 20) * _tmp11 + P(2, 20) + P(3, 20) * _tmp30; - _p_new(3, 20) = P(0, 20) * _tmp29 + P(1, 20) * _tmp41 + P(10, 20) * _tmp10 - - P(11, 20) * _tmp11 - P(12, 20) * _tmp27 + P(2, 20) * _tmp8 + P(3, 20); - _p_new(4, 20) = P(0, 20) * _tmp69 + P(1, 20) * _tmp66 - P(13, 20) * _tmp81 - - P(14, 20) * _tmp78 - P(15, 20) * _tmp76 + P(2, 20) * _tmp86 + - P(3, 20) * _tmp74 + P(4, 20); - _p_new(5, 20) = P(0, 20) * _tmp119 + P(1, 20) * _tmp128 - P(13, 20) * _tmp122 - - P(14, 20) * _tmp125 - P(15, 20) * _tmp123 + P(2, 20) * _tmp117 + - P(3, 20) * _tmp121 + P(5, 20); - _p_new(6, 20) = P(0, 20) * _tmp150 + P(1, 20) * _tmp152 - P(13, 20) * _tmp153 - - P(14, 20) * _tmp154 - P(15, 20) * _tmp155 + P(2, 20) * _tmp151 + - P(3, 20) * _tmp149 + P(6, 20); + _p_new(1, 20) = P(0, 20) * _tmp33 + P(1, 20) - P(10, 20) * _tmp30 + P(11, 20) * _tmp9 - + P(12, 20) * _tmp10 + P(2, 20) * _tmp32 + P(3, 20) * _tmp5; + _p_new(2, 20) = P(0, 20) * _tmp43 + P(1, 20) * _tmp2 - P(10, 20) * _tmp9 - P(11, 20) * _tmp30 + + P(12, 20) * _tmp11 + P(2, 20) + P(3, 20) * _tmp33; + _p_new(3, 20) = P(0, 20) * _tmp32 + P(1, 20) * _tmp43 + P(10, 20) * _tmp10 - + P(11, 20) * _tmp11 - P(12, 20) * _tmp30 + P(2, 20) * _tmp8 + P(3, 20); + _p_new(4, 20) = P(0, 20) * _tmp68 + P(1, 20) * _tmp65 - P(13, 20) * _tmp86 - + P(14, 20) * _tmp82 - P(15, 20) * _tmp78 + P(2, 20) * _tmp91 + + P(3, 20) * _tmp75 + P(4, 20); + _p_new(5, 20) = P(0, 20) * _tmp118 + P(1, 20) * _tmp129 - P(13, 20) * _tmp121 - + P(14, 20) * _tmp126 - P(15, 20) * _tmp124 + P(2, 20) * _tmp116 + + P(3, 20) * _tmp120 + P(5, 20); + _p_new(6, 20) = P(0, 20) * _tmp148 + P(1, 20) * _tmp150 - P(13, 20) * _tmp151 - + P(14, 20) * _tmp152 - P(15, 20) * _tmp153 + P(2, 20) * _tmp149 + + P(3, 20) * _tmp147 + P(6, 20); _p_new(7, 20) = P(4, 20) * dt + P(7, 20); _p_new(8, 20) = P(5, 20) * dt + P(8, 20); _p_new(9, 20) = P(6, 20) * dt + P(9, 20); @@ -968,21 +968,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(23, 20) = 0; _p_new(0, 21) = P(0, 21) + P(1, 21) * _tmp8 + P(10, 21) * _tmp11 + P(11, 21) * _tmp10 + P(12, 21) * _tmp9 + P(2, 21) * _tmp5 + P(3, 21) * _tmp2; - _p_new(1, 21) = P(0, 21) * _tmp30 + P(1, 21) - P(10, 21) * _tmp27 + P(11, 21) * _tmp9 - - P(12, 21) * _tmp10 + P(2, 21) * _tmp29 + P(3, 21) * _tmp5; - _p_new(2, 21) = P(0, 21) * _tmp41 + P(1, 21) * _tmp2 - P(10, 21) * _tmp9 - P(11, 21) * _tmp27 + - P(12, 21) * _tmp11 + P(2, 21) + P(3, 21) * _tmp30; - _p_new(3, 21) = P(0, 21) * _tmp29 + P(1, 21) * _tmp41 + P(10, 21) * _tmp10 - - P(11, 21) * _tmp11 - P(12, 21) * _tmp27 + P(2, 21) * _tmp8 + P(3, 21); - _p_new(4, 21) = P(0, 21) * _tmp69 + P(1, 21) * _tmp66 - P(13, 21) * _tmp81 - - P(14, 21) * _tmp78 - P(15, 21) * _tmp76 + P(2, 21) * _tmp86 + - P(3, 21) * _tmp74 + P(4, 21); - _p_new(5, 21) = P(0, 21) * _tmp119 + P(1, 21) * _tmp128 - P(13, 21) * _tmp122 - - P(14, 21) * _tmp125 - P(15, 21) * _tmp123 + P(2, 21) * _tmp117 + - P(3, 21) * _tmp121 + P(5, 21); - _p_new(6, 21) = P(0, 21) * _tmp150 + P(1, 21) * _tmp152 - P(13, 21) * _tmp153 - - P(14, 21) * _tmp154 - P(15, 21) * _tmp155 + P(2, 21) * _tmp151 + - P(3, 21) * _tmp149 + P(6, 21); + _p_new(1, 21) = P(0, 21) * _tmp33 + P(1, 21) - P(10, 21) * _tmp30 + P(11, 21) * _tmp9 - + P(12, 21) * _tmp10 + P(2, 21) * _tmp32 + P(3, 21) * _tmp5; + _p_new(2, 21) = P(0, 21) * _tmp43 + P(1, 21) * _tmp2 - P(10, 21) * _tmp9 - P(11, 21) * _tmp30 + + P(12, 21) * _tmp11 + P(2, 21) + P(3, 21) * _tmp33; + _p_new(3, 21) = P(0, 21) * _tmp32 + P(1, 21) * _tmp43 + P(10, 21) * _tmp10 - + P(11, 21) * _tmp11 - P(12, 21) * _tmp30 + P(2, 21) * _tmp8 + P(3, 21); + _p_new(4, 21) = P(0, 21) * _tmp68 + P(1, 21) * _tmp65 - P(13, 21) * _tmp86 - + P(14, 21) * _tmp82 - P(15, 21) * _tmp78 + P(2, 21) * _tmp91 + + P(3, 21) * _tmp75 + P(4, 21); + _p_new(5, 21) = P(0, 21) * _tmp118 + P(1, 21) * _tmp129 - P(13, 21) * _tmp121 - + P(14, 21) * _tmp126 - P(15, 21) * _tmp124 + P(2, 21) * _tmp116 + + P(3, 21) * _tmp120 + P(5, 21); + _p_new(6, 21) = P(0, 21) * _tmp148 + P(1, 21) * _tmp150 - P(13, 21) * _tmp151 - + P(14, 21) * _tmp152 - P(15, 21) * _tmp153 + P(2, 21) * _tmp149 + + P(3, 21) * _tmp147 + P(6, 21); _p_new(7, 21) = P(4, 21) * dt + P(7, 21); _p_new(8, 21) = P(5, 21) * dt + P(8, 21); _p_new(9, 21) = P(6, 21) * dt + P(9, 21); @@ -1002,21 +1002,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(23, 21) = 0; _p_new(0, 22) = P(0, 22) + P(1, 22) * _tmp8 + P(10, 22) * _tmp11 + P(11, 22) * _tmp10 + P(12, 22) * _tmp9 + P(2, 22) * _tmp5 + P(3, 22) * _tmp2; - _p_new(1, 22) = P(0, 22) * _tmp30 + P(1, 22) - P(10, 22) * _tmp27 + P(11, 22) * _tmp9 - - P(12, 22) * _tmp10 + P(2, 22) * _tmp29 + P(3, 22) * _tmp5; - _p_new(2, 22) = P(0, 22) * _tmp41 + P(1, 22) * _tmp2 - P(10, 22) * _tmp9 - P(11, 22) * _tmp27 + - P(12, 22) * _tmp11 + P(2, 22) + P(3, 22) * _tmp30; - _p_new(3, 22) = P(0, 22) * _tmp29 + P(1, 22) * _tmp41 + P(10, 22) * _tmp10 - - P(11, 22) * _tmp11 - P(12, 22) * _tmp27 + P(2, 22) * _tmp8 + P(3, 22); - _p_new(4, 22) = P(0, 22) * _tmp69 + P(1, 22) * _tmp66 - P(13, 22) * _tmp81 - - P(14, 22) * _tmp78 - P(15, 22) * _tmp76 + P(2, 22) * _tmp86 + - P(3, 22) * _tmp74 + P(4, 22); - _p_new(5, 22) = P(0, 22) * _tmp119 + P(1, 22) * _tmp128 - P(13, 22) * _tmp122 - - P(14, 22) * _tmp125 - P(15, 22) * _tmp123 + P(2, 22) * _tmp117 + - P(3, 22) * _tmp121 + P(5, 22); - _p_new(6, 22) = P(0, 22) * _tmp150 + P(1, 22) * _tmp152 - P(13, 22) * _tmp153 - - P(14, 22) * _tmp154 - P(15, 22) * _tmp155 + P(2, 22) * _tmp151 + - P(3, 22) * _tmp149 + P(6, 22); + _p_new(1, 22) = P(0, 22) * _tmp33 + P(1, 22) - P(10, 22) * _tmp30 + P(11, 22) * _tmp9 - + P(12, 22) * _tmp10 + P(2, 22) * _tmp32 + P(3, 22) * _tmp5; + _p_new(2, 22) = P(0, 22) * _tmp43 + P(1, 22) * _tmp2 - P(10, 22) * _tmp9 - P(11, 22) * _tmp30 + + P(12, 22) * _tmp11 + P(2, 22) + P(3, 22) * _tmp33; + _p_new(3, 22) = P(0, 22) * _tmp32 + P(1, 22) * _tmp43 + P(10, 22) * _tmp10 - + P(11, 22) * _tmp11 - P(12, 22) * _tmp30 + P(2, 22) * _tmp8 + P(3, 22); + _p_new(4, 22) = P(0, 22) * _tmp68 + P(1, 22) * _tmp65 - P(13, 22) * _tmp86 - + P(14, 22) * _tmp82 - P(15, 22) * _tmp78 + P(2, 22) * _tmp91 + + P(3, 22) * _tmp75 + P(4, 22); + _p_new(5, 22) = P(0, 22) * _tmp118 + P(1, 22) * _tmp129 - P(13, 22) * _tmp121 - + P(14, 22) * _tmp126 - P(15, 22) * _tmp124 + P(2, 22) * _tmp116 + + P(3, 22) * _tmp120 + P(5, 22); + _p_new(6, 22) = P(0, 22) * _tmp148 + P(1, 22) * _tmp150 - P(13, 22) * _tmp151 - + P(14, 22) * _tmp152 - P(15, 22) * _tmp153 + P(2, 22) * _tmp149 + + P(3, 22) * _tmp147 + P(6, 22); _p_new(7, 22) = P(4, 22) * dt + P(7, 22); _p_new(8, 22) = P(5, 22) * dt + P(8, 22); _p_new(9, 22) = P(6, 22) * dt + P(9, 22); @@ -1036,21 +1036,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(23, 22) = 0; _p_new(0, 23) = P(0, 23) + P(1, 23) * _tmp8 + P(10, 23) * _tmp11 + P(11, 23) * _tmp10 + P(12, 23) * _tmp9 + P(2, 23) * _tmp5 + P(3, 23) * _tmp2; - _p_new(1, 23) = P(0, 23) * _tmp30 + P(1, 23) - P(10, 23) * _tmp27 + P(11, 23) * _tmp9 - - P(12, 23) * _tmp10 + P(2, 23) * _tmp29 + P(3, 23) * _tmp5; - _p_new(2, 23) = P(0, 23) * _tmp41 + P(1, 23) * _tmp2 - P(10, 23) * _tmp9 - P(11, 23) * _tmp27 + - P(12, 23) * _tmp11 + P(2, 23) + P(3, 23) * _tmp30; - _p_new(3, 23) = P(0, 23) * _tmp29 + P(1, 23) * _tmp41 + P(10, 23) * _tmp10 - - P(11, 23) * _tmp11 - P(12, 23) * _tmp27 + P(2, 23) * _tmp8 + P(3, 23); - _p_new(4, 23) = P(0, 23) * _tmp69 + P(1, 23) * _tmp66 - P(13, 23) * _tmp81 - - P(14, 23) * _tmp78 - P(15, 23) * _tmp76 + P(2, 23) * _tmp86 + - P(3, 23) * _tmp74 + P(4, 23); - _p_new(5, 23) = P(0, 23) * _tmp119 + P(1, 23) * _tmp128 - P(13, 23) * _tmp122 - - P(14, 23) * _tmp125 - P(15, 23) * _tmp123 + P(2, 23) * _tmp117 + - P(3, 23) * _tmp121 + P(5, 23); - _p_new(6, 23) = P(0, 23) * _tmp150 + P(1, 23) * _tmp152 - P(13, 23) * _tmp153 - - P(14, 23) * _tmp154 - P(15, 23) * _tmp155 + P(2, 23) * _tmp151 + - P(3, 23) * _tmp149 + P(6, 23); + _p_new(1, 23) = P(0, 23) * _tmp33 + P(1, 23) - P(10, 23) * _tmp30 + P(11, 23) * _tmp9 - + P(12, 23) * _tmp10 + P(2, 23) * _tmp32 + P(3, 23) * _tmp5; + _p_new(2, 23) = P(0, 23) * _tmp43 + P(1, 23) * _tmp2 - P(10, 23) * _tmp9 - P(11, 23) * _tmp30 + + P(12, 23) * _tmp11 + P(2, 23) + P(3, 23) * _tmp33; + _p_new(3, 23) = P(0, 23) * _tmp32 + P(1, 23) * _tmp43 + P(10, 23) * _tmp10 - + P(11, 23) * _tmp11 - P(12, 23) * _tmp30 + P(2, 23) * _tmp8 + P(3, 23); + _p_new(4, 23) = P(0, 23) * _tmp68 + P(1, 23) * _tmp65 - P(13, 23) * _tmp86 - + P(14, 23) * _tmp82 - P(15, 23) * _tmp78 + P(2, 23) * _tmp91 + + P(3, 23) * _tmp75 + P(4, 23); + _p_new(5, 23) = P(0, 23) * _tmp118 + P(1, 23) * _tmp129 - P(13, 23) * _tmp121 - + P(14, 23) * _tmp126 - P(15, 23) * _tmp124 + P(2, 23) * _tmp116 + + P(3, 23) * _tmp120 + P(5, 23); + _p_new(6, 23) = P(0, 23) * _tmp148 + P(1, 23) * _tmp150 - P(13, 23) * _tmp151 - + P(14, 23) * _tmp152 - P(15, 23) * _tmp153 + P(2, 23) * _tmp149 + + P(3, 23) * _tmp147 + P(6, 23); _p_new(7, 23) = P(4, 23) * dt + P(7, 23); _p_new(8, 23) = P(5, 23) * dt + P(8, 23); _p_new(9, 23) = P(6, 23) * dt + P(9, 23);