diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py old mode 100644 new mode 100755 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py old mode 100644 new mode 100755 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 42970f10c8..963a139f55 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -36,6 +36,8 @@ Description: 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 @@ -107,6 +109,10 @@ def generate_px4_function(function_name, output_names): line = line.replace("std::min", "math::min") line = line.replace("Eigen", "matrix") line = line.replace("matrix/Dense", "matrix/math.hpp") + + # don't allow underscore + uppercase identifier naming (always reserved for any use) + line = re.sub(r'_([A-Z])', lambda x: '_' + x.group(1).lower(), line) + print(line, end='') def generate_python_function(function_name, output_names): diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h index 0bf15dd0cc..42d82b0e78 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h @@ -47,67 +47,67 @@ void ComputeAirspeedHAndK(const matrix::Matrix& state, // Output terms (2) if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _h.setZero(); - _H(4, 0) = _tmp3; - _H(5, 0) = _tmp4; - _H(6, 0) = _tmp5; - _H(22, 0) = -_tmp3; - _H(23, 0) = -_tmp4; + _h(4, 0) = _tmp3; + _h(5, 0) = _tmp4; + _h(6, 0) = _tmp5; + _h(22, 0) = -_tmp3; + _h(23, 0) = -_tmp4; } if (K != nullptr) { - matrix::Matrix& _K = (*K); + matrix::Matrix& _k = (*K); - _K(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 + + _k(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 + P(0, 6) * _tmp5); - _K(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 + + _k(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 + P(1, 6) * _tmp5); - _K(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 + + _k(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 + P(2, 6) * _tmp5); - _K(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 + + _k(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 + P(3, 6) * _tmp5); - _K(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 + + _k(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 + P(4, 6) * _tmp5); - _K(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 + + _k(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 + P(5, 6) * _tmp5); - _K(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 + + _k(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 + P(6, 6) * _tmp5); - _K(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 + + _k(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 + P(7, 6) * _tmp5); - _K(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 + + _k(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 + P(8, 6) * _tmp5); - _K(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 + + _k(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 + P(9, 6) * _tmp5); - _K(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 + + _k(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 + P(10, 5) * _tmp4 + P(10, 6) * _tmp5); - _K(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 + + _k(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 + P(11, 5) * _tmp4 + P(11, 6) * _tmp5); - _K(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 + + _k(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 + P(12, 5) * _tmp4 + P(12, 6) * _tmp5); - _K(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 + + _k(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 + P(13, 5) * _tmp4 + P(13, 6) * _tmp5); - _K(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 + + _k(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 + P(14, 5) * _tmp4 + P(14, 6) * _tmp5); - _K(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 + + _k(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 + P(15, 5) * _tmp4 + P(15, 6) * _tmp5); - _K(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 + + _k(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 + P(16, 5) * _tmp4 + P(16, 6) * _tmp5); - _K(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 + + _k(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 + P(17, 5) * _tmp4 + P(17, 6) * _tmp5); - _K(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 + + _k(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 + P(18, 5) * _tmp4 + P(18, 6) * _tmp5); - _K(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 + + _k(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 + P(19, 5) * _tmp4 + P(19, 6) * _tmp5); - _K(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 + + _k(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 + P(20, 5) * _tmp4 + P(20, 6) * _tmp5); - _K(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 + + _k(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 + P(21, 5) * _tmp4 + P(21, 6) * _tmp5); - _K(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 + + _k(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 + P(22, 5) * _tmp4 + P(22, 6) * _tmp5); - _K(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 + + _k(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 + P(23, 5) * _tmp4 + P(23, 6) * _tmp5); } } // NOLINT(readability/fn_size) 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 bdff5c8fd9..bcff0c6179 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 @@ -167,14 +167,14 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix& state, } if (K != nullptr) { - matrix::Matrix& _K = (*K); + matrix::Matrix& _k = (*K); - _K.setZero(); + _k.setZero(); - _K(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 + + _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 + + _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); } 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 c8407d00e1..8cbabba2f2 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 @@ -168,14 +168,14 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix& state, } if (K != nullptr) { - matrix::Matrix& _K = (*K); + matrix::Matrix& _k = (*K); - _K.setZero(); + _k.setZero(); - _K(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 + + _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 + + _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); } 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 56f3d87737..6ef629a65a 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 @@ -109,17 +109,17 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _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) = _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; } } // 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 70737d1220..c8ffe911b4 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 @@ -77,17 +77,17 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _h.setZero(); - _H(0, 0) = -_tmp10; - _H(1, 0) = -_tmp11; - _H(2, 0) = -_tmp12; - _H(3, 0) = -_tmp8; - _H(4, 0) = -_tmp1; - _H(5, 0) = -_tmp4; - _H(6, 0) = -_tmp5; + _h(0, 0) = -_tmp10; + _h(1, 0) = -_tmp11; + _h(2, 0) = -_tmp12; + _h(3, 0) = -_tmp8; + _h(4, 0) = -_tmp1; + _h(5, 0) = -_tmp4; + _h(6, 0) = -_tmp5; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h index be89340b42..0836399f9d 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h @@ -88,14 +88,14 @@ void ComputeGnssYawInnonInnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _h.setZero(); - _H(0, 0) = _tmp26; - _H(1, 0) = _tmp25; - _H(2, 0) = _tmp27; - _H(3, 0) = _tmp19; + _h(0, 0) = _tmp26; + _h(1, 0) = _tmp25; + _h(2, 0) = _tmp27; + _h(3, 0) = _tmp19; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h index b75b801c0a..4fd26103e4 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h @@ -61,12 +61,12 @@ void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix& } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _h.setZero(); - _H(16, 0) = -_tmp2; - _H(17, 0) = _tmp3; + _h(16, 0) = -_tmp2; + _h(17, 0) = _tmp3; } } // 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 a657636194..a838bb1886 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 @@ -158,18 +158,18 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, } if (Hx != nullptr) { - matrix::Matrix& _Hx = (*Hx); + matrix::Matrix& _hx = (*Hx); - _Hx.setZero(); + _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(19, 0) = 1; + _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(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 0e97debcd7..083ae8d006 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 @@ -72,18 +72,18 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _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(20, 0) = 1; + _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(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 1cac4d6876..90c896b946 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 @@ -72,18 +72,18 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _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(21, 0) = 1; + _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(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 ee1c190df4..2f00cb2b6e 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 @@ -84,94 +84,94 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, // Output terms (2) if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _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) = _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; } if (K != nullptr) { - matrix::Matrix& _K = (*K); + matrix::Matrix& _k = (*K); - _K(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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 + + _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); } 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 2ef1864a97..30c08f9c25 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 @@ -60,14 +60,14 @@ void ComputeYaw312InnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _h.setZero(); - _H(0, 0) = _tmp10; - _H(1, 0) = _tmp7; - _H(2, 0) = _tmp8; - _H(3, 0) = _tmp9; + _h(0, 0) = _tmp10; + _h(1, 0) = _tmp7; + _h(2, 0) = _tmp8; + _h(3, 0) = _tmp9; } } // 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 e5b76ed267..862b67c7c4 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 @@ -60,14 +60,14 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix& sta } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _h.setZero(); - _H(0, 0) = -_tmp10; - _H(1, 0) = -_tmp8; - _H(2, 0) = -_tmp7; - _H(3, 0) = -_tmp9; + _h(0, 0) = -_tmp10; + _h(1, 0) = -_tmp8; + _h(2, 0) = -_tmp7; + _h(3, 0) = -_tmp9; } } // 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 eb2caad5c4..066b5166da 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 @@ -60,14 +60,14 @@ void ComputeYaw321InnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _h.setZero(); - _H(0, 0) = _tmp9; - _H(1, 0) = _tmp8; - _H(2, 0) = _tmp7; - _H(3, 0) = _tmp10; + _h(0, 0) = _tmp9; + _h(1, 0) = _tmp8; + _h(2, 0) = _tmp7; + _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_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h index 240d725470..d29921a775 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 @@ -60,14 +60,14 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix& sta } if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H.setZero(); + _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) = -_tmp10; + _h(3, 0) = -_tmp7; } } // 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 11e2ac3c9e..dfe663070b 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 @@ -311,196 +311,196 @@ void PredictCovariance(const matrix::Matrix& state, // Output terms (1) if (P_new != nullptr) { - matrix::Matrix& _P_new = (*P_new); + matrix::Matrix& _p_new = (*P_new); - _P_new(0, 0) = _tmp10 * _tmp12 + _tmp11 * _tmp13 + _tmp14 * _tmp9 + _tmp15 * _tmp5 + + _p_new(0, 0) = _tmp10 * _tmp12 + _tmp11 * _tmp13 + _tmp14 * _tmp9 + _tmp15 * _tmp5 + _tmp16 * _tmp2 + _tmp17 * _tmp8 + _tmp20 + _tmp21 + _tmp26; - _P_new(1, 0) = 0; - _P_new(2, 0) = 0; - _P_new(3, 0) = 0; - _P_new(4, 0) = 0; - _P_new(5, 0) = 0; - _P_new(6, 0) = 0; - _P_new(7, 0) = 0; - _P_new(8, 0) = 0; - _P_new(9, 0) = 0; - _P_new(10, 0) = 0; - _P_new(11, 0) = 0; - _P_new(12, 0) = 0; - _P_new(13, 0) = 0; - _P_new(14, 0) = 0; - _P_new(15, 0) = 0; - _P_new(16, 0) = 0; - _P_new(17, 0) = 0; - _P_new(18, 0) = 0; - _P_new(19, 0) = 0; - _P_new(20, 0) = 0; - _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 + + _p_new(1, 0) = 0; + _p_new(2, 0) = 0; + _p_new(3, 0) = 0; + _p_new(4, 0) = 0; + _p_new(5, 0) = 0; + _p_new(6, 0) = 0; + _p_new(7, 0) = 0; + _p_new(8, 0) = 0; + _p_new(9, 0) = 0; + _p_new(10, 0) = 0; + _p_new(11, 0) = 0; + _p_new(12, 0) = 0; + _p_new(13, 0) = 0; + _p_new(14, 0) = 0; + _p_new(15, 0) = 0; + _p_new(16, 0) = 0; + _p_new(17, 0) = 0; + _p_new(18, 0) = 0; + _p_new(19, 0) = 0; + _p_new(20, 0) = 0; + _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 + + _p_new(1, 1) = -_tmp10 * _tmp31 + _tmp23 - _tmp27 * _tmp33 + _tmp29 * _tmp35 + _tmp30 * _tmp36 + _tmp32 * _tmp9 + _tmp34 * _tmp5 + _tmp37 + _tmp39; - _P_new(2, 1) = 0; - _P_new(3, 1) = 0; - _P_new(4, 1) = 0; - _P_new(5, 1) = 0; - _P_new(6, 1) = 0; - _P_new(7, 1) = 0; - _P_new(8, 1) = 0; - _P_new(9, 1) = 0; - _P_new(10, 1) = 0; - _P_new(11, 1) = 0; - _P_new(12, 1) = 0; - _P_new(13, 1) = 0; - _P_new(14, 1) = 0; - _P_new(15, 1) = 0; - _P_new(16, 1) = 0; - _P_new(17, 1) = 0; - _P_new(18, 1) = 0; - _P_new(19, 1) = 0; - _P_new(20, 1) = 0; - _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 + + _p_new(2, 1) = 0; + _p_new(3, 1) = 0; + _p_new(4, 1) = 0; + _p_new(5, 1) = 0; + _p_new(6, 1) = 0; + _p_new(7, 1) = 0; + _p_new(8, 1) = 0; + _p_new(9, 1) = 0; + _p_new(10, 1) = 0; + _p_new(11, 1) = 0; + _p_new(12, 1) = 0; + _p_new(13, 1) = 0; + _p_new(14, 1) = 0; + _p_new(15, 1) = 0; + _p_new(16, 1) = 0; + _p_new(17, 1) = 0; + _p_new(18, 1) = 0; + _p_new(19, 1) = 0; + _p_new(20, 1) = 0; + _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 + + _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 + + _p_new(2, 2) = _tmp11 * _tmp43 + _tmp2 * _tmp47 + _tmp25 - _tmp27 * _tmp44 + _tmp30 * _tmp48 + _tmp39 + _tmp41 * _tmp46 - _tmp45 * _tmp9 + _tmp49; - _P_new(3, 2) = 0; - _P_new(4, 2) = 0; - _P_new(5, 2) = 0; - _P_new(6, 2) = 0; - _P_new(7, 2) = 0; - _P_new(8, 2) = 0; - _P_new(9, 2) = 0; - _P_new(10, 2) = 0; - _P_new(11, 2) = 0; - _P_new(12, 2) = 0; - _P_new(13, 2) = 0; - _P_new(14, 2) = 0; - _P_new(15, 2) = 0; - _P_new(16, 2) = 0; - _P_new(17, 2) = 0; - _P_new(18, 2) = 0; - _P_new(19, 2) = 0; - _P_new(20, 2) = 0; - _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 + + _p_new(3, 2) = 0; + _p_new(4, 2) = 0; + _p_new(5, 2) = 0; + _p_new(6, 2) = 0; + _p_new(7, 2) = 0; + _p_new(8, 2) = 0; + _p_new(9, 2) = 0; + _p_new(10, 2) = 0; + _p_new(11, 2) = 0; + _p_new(12, 2) = 0; + _p_new(13, 2) = 0; + _p_new(14, 2) = 0; + _p_new(15, 2) = 0; + _p_new(16, 2) = 0; + _p_new(17, 2) = 0; + _p_new(18, 2) = 0; + _p_new(19, 2) = 0; + _p_new(20, 2) = 0; + _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 + + _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 + + _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 + + _p_new(3, 3) = _tmp10 * _tmp55 - _tmp11 * _tmp54 + _tmp26 - _tmp27 * _tmp53 + _tmp29 * _tmp57 + _tmp38 + _tmp41 * _tmp58 + _tmp56 * _tmp8 + _tmp59; - _P_new(4, 3) = 0; - _P_new(5, 3) = 0; - _P_new(6, 3) = 0; - _P_new(7, 3) = 0; - _P_new(8, 3) = 0; - _P_new(9, 3) = 0; - _P_new(10, 3) = 0; - _P_new(11, 3) = 0; - _P_new(12, 3) = 0; - _P_new(13, 3) = 0; - _P_new(14, 3) = 0; - _P_new(15, 3) = 0; - _P_new(16, 3) = 0; - _P_new(17, 3) = 0; - _P_new(18, 3) = 0; - _P_new(19, 3) = 0; - _P_new(20, 3) = 0; - _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 - + _p_new(4, 3) = 0; + _p_new(5, 3) = 0; + _p_new(6, 3) = 0; + _p_new(7, 3) = 0; + _p_new(8, 3) = 0; + _p_new(9, 3) = 0; + _p_new(10, 3) = 0; + _p_new(11, 3) = 0; + _p_new(12, 3) = 0; + _p_new(13, 3) = 0; + _p_new(14, 3) = 0; + _p_new(15, 3) = 0; + _p_new(16, 3) = 0; + _p_new(17, 3) = 0; + _p_new(18, 3) = 0; + _p_new(19, 3) = 0; + _p_new(20, 3) = 0; + _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 - + _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 - + _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 - + _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 + + _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(5, 4) = 0; - _P_new(6, 4) = 0; - _P_new(7, 4) = 0; - _P_new(8, 4) = 0; - _P_new(9, 4) = 0; - _P_new(10, 4) = 0; - _P_new(11, 4) = 0; - _P_new(12, 4) = 0; - _P_new(13, 4) = 0; - _P_new(14, 4) = 0; - _P_new(15, 4) = 0; - _P_new(16, 4) = 0; - _P_new(17, 4) = 0; - _P_new(18, 4) = 0; - _P_new(19, 4) = 0; - _P_new(20, 4) = 0; - _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 - + _p_new(5, 4) = 0; + _p_new(6, 4) = 0; + _p_new(7, 4) = 0; + _p_new(8, 4) = 0; + _p_new(9, 4) = 0; + _p_new(10, 4) = 0; + _p_new(11, 4) = 0; + _p_new(12, 4) = 0; + _p_new(13, 4) = 0; + _p_new(14, 4) = 0; + _p_new(15, 4) = 0; + _p_new(16, 4) = 0; + _p_new(17, 4) = 0; + _p_new(18, 4) = 0; + _p_new(19, 4) = 0; + _p_new(20, 4) = 0; + _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 - + _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 - + _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 - + _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 + + _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 - + _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(6, 5) = 0; - _P_new(7, 5) = 0; - _P_new(8, 5) = 0; - _P_new(9, 5) = 0; - _P_new(10, 5) = 0; - _P_new(11, 5) = 0; - _P_new(12, 5) = 0; - _P_new(13, 5) = 0; - _P_new(14, 5) = 0; - _P_new(15, 5) = 0; - _P_new(16, 5) = 0; - _P_new(17, 5) = 0; - _P_new(18, 5) = 0; - _P_new(19, 5) = 0; - _P_new(20, 5) = 0; - _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 - + _p_new(6, 5) = 0; + _p_new(7, 5) = 0; + _p_new(8, 5) = 0; + _p_new(9, 5) = 0; + _p_new(10, 5) = 0; + _p_new(11, 5) = 0; + _p_new(12, 5) = 0; + _p_new(13, 5) = 0; + _p_new(14, 5) = 0; + _p_new(15, 5) = 0; + _p_new(16, 5) = 0; + _p_new(17, 5) = 0; + _p_new(18, 5) = 0; + _p_new(19, 5) = 0; + _p_new(20, 5) = 0; + _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 - + _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 - + _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 - + _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 + + _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 + + _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(6, 6) = + _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 - @@ -512,562 +512,562 @@ void PredictCovariance(const matrix::Matrix& state, _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; - _P_new(7, 6) = 0; - _P_new(8, 6) = 0; - _P_new(9, 6) = 0; - _P_new(10, 6) = 0; - _P_new(11, 6) = 0; - _P_new(12, 6) = 0; - _P_new(13, 6) = 0; - _P_new(14, 6) = 0; - _P_new(15, 6) = 0; - _P_new(16, 6) = 0; - _P_new(17, 6) = 0; - _P_new(18, 6) = 0; - _P_new(19, 6) = 0; - _P_new(20, 6) = 0; - _P_new(21, 6) = 0; - _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_new(7, 6) = 0; + _p_new(8, 6) = 0; + _p_new(9, 6) = 0; + _p_new(10, 6) = 0; + _p_new(11, 6) = 0; + _p_new(12, 6) = 0; + _p_new(13, 6) = 0; + _p_new(14, 6) = 0; + _p_new(15, 6) = 0; + _p_new(16, 6) = 0; + _p_new(17, 6) = 0; + _p_new(18, 6) = 0; + _p_new(19, 6) = 0; + _p_new(20, 6) = 0; + _p_new(21, 6) = 0; + _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_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_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_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_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_new(5, 7) = + _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_new(6, 7) = + _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_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; - _P_new(10, 7) = 0; - _P_new(11, 7) = 0; - _P_new(12, 7) = 0; - _P_new(13, 7) = 0; - _P_new(14, 7) = 0; - _P_new(15, 7) = 0; - _P_new(16, 7) = 0; - _P_new(17, 7) = 0; - _P_new(18, 7) = 0; - _P_new(19, 7) = 0; - _P_new(20, 7) = 0; - _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_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; + _p_new(10, 7) = 0; + _p_new(11, 7) = 0; + _p_new(12, 7) = 0; + _p_new(13, 7) = 0; + _p_new(14, 7) = 0; + _p_new(15, 7) = 0; + _p_new(16, 7) = 0; + _p_new(17, 7) = 0; + _p_new(18, 7) = 0; + _p_new(19, 7) = 0; + _p_new(20, 7) = 0; + _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_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_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_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_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_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(6, 8) = + _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_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; - _P_new(10, 8) = 0; - _P_new(11, 8) = 0; - _P_new(12, 8) = 0; - _P_new(13, 8) = 0; - _P_new(14, 8) = 0; - _P_new(15, 8) = 0; - _P_new(16, 8) = 0; - _P_new(17, 8) = 0; - _P_new(18, 8) = 0; - _P_new(19, 8) = 0; - _P_new(20, 8) = 0; - _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_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; + _p_new(10, 8) = 0; + _p_new(11, 8) = 0; + _p_new(12, 8) = 0; + _p_new(13, 8) = 0; + _p_new(14, 8) = 0; + _p_new(15, 8) = 0; + _p_new(16, 8) = 0; + _p_new(17, 8) = 0; + _p_new(18, 8) = 0; + _p_new(19, 8) = 0; + _p_new(20, 8) = 0; + _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_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_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_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_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_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_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(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)); - _P_new(10, 9) = 0; - _P_new(11, 9) = 0; - _P_new(12, 9) = 0; - _P_new(13, 9) = 0; - _P_new(14, 9) = 0; - _P_new(15, 9) = 0; - _P_new(16, 9) = 0; - _P_new(17, 9) = 0; - _P_new(18, 9) = 0; - _P_new(19, 9) = 0; - _P_new(20, 9) = 0; - _P_new(21, 9) = 0; - _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_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)); + _p_new(10, 9) = 0; + _p_new(11, 9) = 0; + _p_new(12, 9) = 0; + _p_new(13, 9) = 0; + _p_new(14, 9) = 0; + _p_new(15, 9) = 0; + _p_new(16, 9) = 0; + _p_new(17, 9) = 0; + _p_new(18, 9) = 0; + _p_new(19, 9) = 0; + _p_new(20, 9) = 0; + _p_new(21, 9) = 0; + _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_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_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(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); - _P_new(10, 10) = P(10, 10); - _P_new(11, 10) = 0; - _P_new(12, 10) = 0; - _P_new(13, 10) = 0; - _P_new(14, 10) = 0; - _P_new(15, 10) = 0; - _P_new(16, 10) = 0; - _P_new(17, 10) = 0; - _P_new(18, 10) = 0; - _P_new(19, 10) = 0; - _P_new(20, 10) = 0; - _P_new(21, 10) = 0; - _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_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); + _p_new(10, 10) = P(10, 10); + _p_new(11, 10) = 0; + _p_new(12, 10) = 0; + _p_new(13, 10) = 0; + _p_new(14, 10) = 0; + _p_new(15, 10) = 0; + _p_new(16, 10) = 0; + _p_new(17, 10) = 0; + _p_new(18, 10) = 0; + _p_new(19, 10) = 0; + _p_new(20, 10) = 0; + _p_new(21, 10) = 0; + _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_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_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(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); - _P_new(10, 11) = P(10, 11); - _P_new(11, 11) = P(11, 11); - _P_new(12, 11) = 0; - _P_new(13, 11) = 0; - _P_new(14, 11) = 0; - _P_new(15, 11) = 0; - _P_new(16, 11) = 0; - _P_new(17, 11) = 0; - _P_new(18, 11) = 0; - _P_new(19, 11) = 0; - _P_new(20, 11) = 0; - _P_new(21, 11) = 0; - _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_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); + _p_new(10, 11) = P(10, 11); + _p_new(11, 11) = P(11, 11); + _p_new(12, 11) = 0; + _p_new(13, 11) = 0; + _p_new(14, 11) = 0; + _p_new(15, 11) = 0; + _p_new(16, 11) = 0; + _p_new(17, 11) = 0; + _p_new(18, 11) = 0; + _p_new(19, 11) = 0; + _p_new(20, 11) = 0; + _p_new(21, 11) = 0; + _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_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_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(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); - _P_new(10, 12) = P(10, 12); - _P_new(11, 12) = P(11, 12); - _P_new(12, 12) = P(12, 12); - _P_new(13, 12) = 0; - _P_new(14, 12) = 0; - _P_new(15, 12) = 0; - _P_new(16, 12) = 0; - _P_new(17, 12) = 0; - _P_new(18, 12) = 0; - _P_new(19, 12) = 0; - _P_new(20, 12) = 0; - _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(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); - _P_new(10, 13) = P(10, 13); - _P_new(11, 13) = P(11, 13); - _P_new(12, 13) = P(12, 13); - _P_new(13, 13) = P(13, 13); - _P_new(14, 13) = 0; - _P_new(15, 13) = 0; - _P_new(16, 13) = 0; - _P_new(17, 13) = 0; - _P_new(18, 13) = 0; - _P_new(19, 13) = 0; - _P_new(20, 13) = 0; - _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(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); - _P_new(10, 14) = P(10, 14); - _P_new(11, 14) = P(11, 14); - _P_new(12, 14) = P(12, 14); - _P_new(13, 14) = P(13, 14); - _P_new(14, 14) = P(14, 14); - _P_new(15, 14) = 0; - _P_new(16, 14) = 0; - _P_new(17, 14) = 0; - _P_new(18, 14) = 0; - _P_new(19, 14) = 0; - _P_new(20, 14) = 0; - _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(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); - _P_new(10, 15) = P(10, 15); - _P_new(11, 15) = P(11, 15); - _P_new(12, 15) = P(12, 15); - _P_new(13, 15) = P(13, 15); - _P_new(14, 15) = P(14, 15); - _P_new(15, 15) = P(15, 15); - _P_new(16, 15) = 0; - _P_new(17, 15) = 0; - _P_new(18, 15) = 0; - _P_new(19, 15) = 0; - _P_new(20, 15) = 0; - _P_new(21, 15) = 0; - _P_new(22, 15) = 0; - _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_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); + _p_new(10, 12) = P(10, 12); + _p_new(11, 12) = P(11, 12); + _p_new(12, 12) = P(12, 12); + _p_new(13, 12) = 0; + _p_new(14, 12) = 0; + _p_new(15, 12) = 0; + _p_new(16, 12) = 0; + _p_new(17, 12) = 0; + _p_new(18, 12) = 0; + _p_new(19, 12) = 0; + _p_new(20, 12) = 0; + _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(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); + _p_new(10, 13) = P(10, 13); + _p_new(11, 13) = P(11, 13); + _p_new(12, 13) = P(12, 13); + _p_new(13, 13) = P(13, 13); + _p_new(14, 13) = 0; + _p_new(15, 13) = 0; + _p_new(16, 13) = 0; + _p_new(17, 13) = 0; + _p_new(18, 13) = 0; + _p_new(19, 13) = 0; + _p_new(20, 13) = 0; + _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(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); + _p_new(10, 14) = P(10, 14); + _p_new(11, 14) = P(11, 14); + _p_new(12, 14) = P(12, 14); + _p_new(13, 14) = P(13, 14); + _p_new(14, 14) = P(14, 14); + _p_new(15, 14) = 0; + _p_new(16, 14) = 0; + _p_new(17, 14) = 0; + _p_new(18, 14) = 0; + _p_new(19, 14) = 0; + _p_new(20, 14) = 0; + _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(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); + _p_new(10, 15) = P(10, 15); + _p_new(11, 15) = P(11, 15); + _p_new(12, 15) = P(12, 15); + _p_new(13, 15) = P(13, 15); + _p_new(14, 15) = P(14, 15); + _p_new(15, 15) = P(15, 15); + _p_new(16, 15) = 0; + _p_new(17, 15) = 0; + _p_new(18, 15) = 0; + _p_new(19, 15) = 0; + _p_new(20, 15) = 0; + _p_new(21, 15) = 0; + _p_new(22, 15) = 0; + _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_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_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_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_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_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_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(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); - _P_new(10, 16) = P(10, 16); - _P_new(11, 16) = P(11, 16); - _P_new(12, 16) = P(12, 16); - _P_new(13, 16) = P(13, 16); - _P_new(14, 16) = P(14, 16); - _P_new(15, 16) = P(15, 16); - _P_new(16, 16) = P(16, 16); - _P_new(17, 16) = 0; - _P_new(18, 16) = 0; - _P_new(19, 16) = 0; - _P_new(20, 16) = 0; - _P_new(21, 16) = 0; - _P_new(22, 16) = 0; - _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_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); + _p_new(10, 16) = P(10, 16); + _p_new(11, 16) = P(11, 16); + _p_new(12, 16) = P(12, 16); + _p_new(13, 16) = P(13, 16); + _p_new(14, 16) = P(14, 16); + _p_new(15, 16) = P(15, 16); + _p_new(16, 16) = P(16, 16); + _p_new(17, 16) = 0; + _p_new(18, 16) = 0; + _p_new(19, 16) = 0; + _p_new(20, 16) = 0; + _p_new(21, 16) = 0; + _p_new(22, 16) = 0; + _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_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_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_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_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_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_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(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); - _P_new(10, 17) = P(10, 17); - _P_new(11, 17) = P(11, 17); - _P_new(12, 17) = P(12, 17); - _P_new(13, 17) = P(13, 17); - _P_new(14, 17) = P(14, 17); - _P_new(15, 17) = P(15, 17); - _P_new(16, 17) = P(16, 17); - _P_new(17, 17) = P(17, 17); - _P_new(18, 17) = 0; - _P_new(19, 17) = 0; - _P_new(20, 17) = 0; - _P_new(21, 17) = 0; - _P_new(22, 17) = 0; - _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_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); + _p_new(10, 17) = P(10, 17); + _p_new(11, 17) = P(11, 17); + _p_new(12, 17) = P(12, 17); + _p_new(13, 17) = P(13, 17); + _p_new(14, 17) = P(14, 17); + _p_new(15, 17) = P(15, 17); + _p_new(16, 17) = P(16, 17); + _p_new(17, 17) = P(17, 17); + _p_new(18, 17) = 0; + _p_new(19, 17) = 0; + _p_new(20, 17) = 0; + _p_new(21, 17) = 0; + _p_new(22, 17) = 0; + _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_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_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_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_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_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_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(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); - _P_new(10, 18) = P(10, 18); - _P_new(11, 18) = P(11, 18); - _P_new(12, 18) = P(12, 18); - _P_new(13, 18) = P(13, 18); - _P_new(14, 18) = P(14, 18); - _P_new(15, 18) = P(15, 18); - _P_new(16, 18) = P(16, 18); - _P_new(17, 18) = P(17, 18); - _P_new(18, 18) = P(18, 18); - _P_new(19, 18) = 0; - _P_new(20, 18) = 0; - _P_new(21, 18) = 0; - _P_new(22, 18) = 0; - _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_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); + _p_new(10, 18) = P(10, 18); + _p_new(11, 18) = P(11, 18); + _p_new(12, 18) = P(12, 18); + _p_new(13, 18) = P(13, 18); + _p_new(14, 18) = P(14, 18); + _p_new(15, 18) = P(15, 18); + _p_new(16, 18) = P(16, 18); + _p_new(17, 18) = P(17, 18); + _p_new(18, 18) = P(18, 18); + _p_new(19, 18) = 0; + _p_new(20, 18) = 0; + _p_new(21, 18) = 0; + _p_new(22, 18) = 0; + _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_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_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_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_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_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_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(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); - _P_new(10, 19) = P(10, 19); - _P_new(11, 19) = P(11, 19); - _P_new(12, 19) = P(12, 19); - _P_new(13, 19) = P(13, 19); - _P_new(14, 19) = P(14, 19); - _P_new(15, 19) = P(15, 19); - _P_new(16, 19) = P(16, 19); - _P_new(17, 19) = P(17, 19); - _P_new(18, 19) = P(18, 19); - _P_new(19, 19) = P(19, 19); - _P_new(20, 19) = 0; - _P_new(21, 19) = 0; - _P_new(22, 19) = 0; - _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_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); + _p_new(10, 19) = P(10, 19); + _p_new(11, 19) = P(11, 19); + _p_new(12, 19) = P(12, 19); + _p_new(13, 19) = P(13, 19); + _p_new(14, 19) = P(14, 19); + _p_new(15, 19) = P(15, 19); + _p_new(16, 19) = P(16, 19); + _p_new(17, 19) = P(17, 19); + _p_new(18, 19) = P(18, 19); + _p_new(19, 19) = P(19, 19); + _p_new(20, 19) = 0; + _p_new(21, 19) = 0; + _p_new(22, 19) = 0; + _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_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_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_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_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_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_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(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); - _P_new(10, 20) = P(10, 20); - _P_new(11, 20) = P(11, 20); - _P_new(12, 20) = P(12, 20); - _P_new(13, 20) = P(13, 20); - _P_new(14, 20) = P(14, 20); - _P_new(15, 20) = P(15, 20); - _P_new(16, 20) = P(16, 20); - _P_new(17, 20) = P(17, 20); - _P_new(18, 20) = P(18, 20); - _P_new(19, 20) = P(19, 20); - _P_new(20, 20) = P(20, 20); - _P_new(21, 20) = 0; - _P_new(22, 20) = 0; - _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_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); + _p_new(10, 20) = P(10, 20); + _p_new(11, 20) = P(11, 20); + _p_new(12, 20) = P(12, 20); + _p_new(13, 20) = P(13, 20); + _p_new(14, 20) = P(14, 20); + _p_new(15, 20) = P(15, 20); + _p_new(16, 20) = P(16, 20); + _p_new(17, 20) = P(17, 20); + _p_new(18, 20) = P(18, 20); + _p_new(19, 20) = P(19, 20); + _p_new(20, 20) = P(20, 20); + _p_new(21, 20) = 0; + _p_new(22, 20) = 0; + _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_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_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_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_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_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_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(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); - _P_new(10, 21) = P(10, 21); - _P_new(11, 21) = P(11, 21); - _P_new(12, 21) = P(12, 21); - _P_new(13, 21) = P(13, 21); - _P_new(14, 21) = P(14, 21); - _P_new(15, 21) = P(15, 21); - _P_new(16, 21) = P(16, 21); - _P_new(17, 21) = P(17, 21); - _P_new(18, 21) = P(18, 21); - _P_new(19, 21) = P(19, 21); - _P_new(20, 21) = P(20, 21); - _P_new(21, 21) = P(21, 21); - _P_new(22, 21) = 0; - _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_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); + _p_new(10, 21) = P(10, 21); + _p_new(11, 21) = P(11, 21); + _p_new(12, 21) = P(12, 21); + _p_new(13, 21) = P(13, 21); + _p_new(14, 21) = P(14, 21); + _p_new(15, 21) = P(15, 21); + _p_new(16, 21) = P(16, 21); + _p_new(17, 21) = P(17, 21); + _p_new(18, 21) = P(18, 21); + _p_new(19, 21) = P(19, 21); + _p_new(20, 21) = P(20, 21); + _p_new(21, 21) = P(21, 21); + _p_new(22, 21) = 0; + _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_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_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_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_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_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_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(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); - _P_new(10, 22) = P(10, 22); - _P_new(11, 22) = P(11, 22); - _P_new(12, 22) = P(12, 22); - _P_new(13, 22) = P(13, 22); - _P_new(14, 22) = P(14, 22); - _P_new(15, 22) = P(15, 22); - _P_new(16, 22) = P(16, 22); - _P_new(17, 22) = P(17, 22); - _P_new(18, 22) = P(18, 22); - _P_new(19, 22) = P(19, 22); - _P_new(20, 22) = P(20, 22); - _P_new(21, 22) = P(21, 22); - _P_new(22, 22) = P(22, 22); - _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_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); + _p_new(10, 22) = P(10, 22); + _p_new(11, 22) = P(11, 22); + _p_new(12, 22) = P(12, 22); + _p_new(13, 22) = P(13, 22); + _p_new(14, 22) = P(14, 22); + _p_new(15, 22) = P(15, 22); + _p_new(16, 22) = P(16, 22); + _p_new(17, 22) = P(17, 22); + _p_new(18, 22) = P(18, 22); + _p_new(19, 22) = P(19, 22); + _p_new(20, 22) = P(20, 22); + _p_new(21, 22) = P(21, 22); + _p_new(22, 22) = P(22, 22); + _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_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_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_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_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_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_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(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); - _P_new(10, 23) = P(10, 23); - _P_new(11, 23) = P(11, 23); - _P_new(12, 23) = P(12, 23); - _P_new(13, 23) = P(13, 23); - _P_new(14, 23) = P(14, 23); - _P_new(15, 23) = P(15, 23); - _P_new(16, 23) = P(16, 23); - _P_new(17, 23) = P(17, 23); - _P_new(18, 23) = P(18, 23); - _P_new(19, 23) = P(19, 23); - _P_new(20, 23) = P(20, 23); - _P_new(21, 23) = P(21, 23); - _P_new(22, 23) = P(22, 23); - _P_new(23, 23) = P(23, 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); + _p_new(10, 23) = P(10, 23); + _p_new(11, 23) = P(11, 23); + _p_new(12, 23) = P(12, 23); + _p_new(13, 23) = P(13, 23); + _p_new(14, 23) = P(14, 23); + _p_new(15, 23) = P(15, 23); + _p_new(16, 23) = P(16, 23); + _p_new(17, 23) = P(17, 23); + _p_new(18, 23) = P(18, 23); + _p_new(19, 23) = P(19, 23); + _p_new(20, 23) = P(20, 23); + _p_new(21, 23) = P(21, 23); + _p_new(22, 23) = P(22, 23); + _p_new(23, 23) = P(23, 23); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h index 505802a863..b2af651030 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h @@ -56,9 +56,9 @@ void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P } if (H != nullptr) { - Scalar& _H = (*H); + Scalar& _h = (*H); - _H = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2)); + _h = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2)); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h index 57058233e4..0bf7cb85f0 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h @@ -55,9 +55,9 @@ void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P, } if (H != nullptr) { - Scalar& _H = (*H); + Scalar& _h = (*H); - _H = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2)); + _h = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2)); } } // NOLINT(readability/fn_size)