diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 28572f69de..51b17931c2 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -207,13 +207,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) } // predict the covariance - SquareMatrixState nextP; - // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states - sym::PredictCovariance(_state.vector(), P, + P = sym::PredictCovariance(_state.vector(), P, imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var, - imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var, - &nextP); + imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var); // Construct the process noise variance diagonal for those states with a stationary process model // These are kinematic states and their error growth is controlled separately by the IMU noise variances @@ -225,10 +222,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) const unsigned i = State::gyro_bias.idx + index; if (!_gyro_bias_inhibit[index]) { - nextP(i, i) += gyro_bias_process_noise; + P(i, i) += gyro_bias_process_noise; } else { - nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index)); + P.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index)); } } @@ -239,10 +236,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) const unsigned i = State::accel_bias.idx + index; if (!_accel_bias_inhibit[index]) { - nextP(i, i) += accel_bias_process_noise; + P(i, i) += accel_bias_process_noise; } else { - nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index)); + P.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index)); } } @@ -256,7 +253,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) for (unsigned index = 0; index < State::mag_I.dof; index++) { unsigned i = State::mag_I.idx + index; - nextP(i, i) += mag_I_process_noise; + P(i, i) += mag_I_process_noise; } } @@ -268,23 +265,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) for (unsigned index = 0; index < State::mag_B.dof; index++) { unsigned i = State::mag_B.idx + index; - nextP(i, i) += mag_B_process_noise; - } - } - - } else { - // keep previous covariance - for (unsigned i = 0; i < State::mag_I.dof; i++) { - unsigned row = State::mag_I.idx + i; - for (unsigned col = 0; col < State::size; col++) { - nextP(row, col) = nextP(col, row) = P(row, col); - } - } - - for (unsigned i = 0; i < State::mag_B.dof; i++) { - unsigned row = State::mag_B.idx + i; - for (unsigned col = 0; col < State::size; col++) { - nextP(row, col) = nextP(col, row) = P(row, col); + P(i, i) += mag_B_process_noise; } } } @@ -301,16 +282,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) for (unsigned index = 0; index < State::wind_vel.dof; index++) { unsigned i = State::wind_vel.idx + index; - nextP(i, i) += wind_vel_process_noise; - } - } - - } else { - // keep previous covariance - for (unsigned i = 0; i < State::wind_vel.dof; i++) { - unsigned row = State::wind_vel.idx + i; - for (unsigned col = 0; col < State::size; col++) { - nextP(row, col) = nextP(col, row) = P(row, col); + P(i, i) += wind_vel_process_noise; } } } @@ -319,10 +291,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // covariance matrix is symmetrical, so copy upper half to lower half for (unsigned row = 0; row < State::size; row++) { for (unsigned column = 0 ; column < row; column++) { - P(row, column) = P(column, row) = nextP(column, row); + P(row, column) = P(column, row); } - - P(row, row) = nextP(row, row); } // fix gross errors in the covariance matrix and ensure rows and diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 0981e92f0b..4ab40828b8 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -612,7 +612,7 @@ def rot_var_ned_to_lower_triangular_quat_cov( return q_var.lower_triangle() print("Derive EKF2 equations...") -generate_px4_function(predict_covariance, output_names=["P_new"]) +generate_px4_function(predict_covariance, output_names=None) if not args.disable_mag: generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "innov_var", "H"]) 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 ee0ee8e1d7..3637c654a3 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 @@ -26,16 +26,16 @@ namespace sym { * d_ang_var: Scalar * * Outputs: - * P_new: Matrix24_24 + * res: Matrix24_24 */ template -void PredictCovariance(const matrix::Matrix& state, - const matrix::Matrix& P, - const matrix::Matrix& d_vel, const Scalar d_vel_dt, - const matrix::Matrix& d_vel_var, - const matrix::Matrix& d_ang, const Scalar d_ang_dt, - const Scalar d_ang_var, - matrix::Matrix* const P_new = nullptr) { +matrix::Matrix PredictCovariance(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& d_vel, + const Scalar d_vel_dt, + const matrix::Matrix& d_vel_var, + const matrix::Matrix& d_ang, + const Scalar d_ang_dt, const Scalar d_ang_var) { // Total ops: 2882 // Input arrays @@ -314,769 +314,757 @@ void PredictCovariance(const matrix::Matrix& state, const Scalar _tmp172 = P(11, 8) * _tmp1; // Output terms (1) - if (P_new != nullptr) { - matrix::Matrix& _p_new = (*P_new); + matrix::Matrix _res; - _p_new(0, 0) = _tmp10 * _tmp13 + _tmp11 * _tmp15 + _tmp12 * _tmp14 + _tmp16 * _tmp3 + - _tmp17 * _tmp6 + _tmp19 * _tmp9 + _tmp22 + _tmp23 + _tmp28; - _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) = _tmp11 * _tmp14 - _tmp12 * _tmp15 - _tmp13 * _tmp29 + _tmp16 * _tmp32 + - _tmp17 * _tmp3 + _tmp19 + _tmp23 * _tmp31 - _tmp30 * state(0, 0); - _p_new(1, 1) = _tmp11 * _tmp35 - _tmp12 * _tmp33 + _tmp25 - _tmp29 * _tmp34 + _tmp3 * _tmp37 + - _tmp31 * _tmp38 + _tmp32 * _tmp36 + _tmp39 + _tmp41; - _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) = _tmp10 * _tmp15 - _tmp11 * _tmp13 - _tmp14 * _tmp29 + _tmp16 + _tmp17 * _tmp31 + - _tmp19 * _tmp6 + _tmp23 * _tmp43 - _tmp42 * state(2, 0); - _p_new(1, 2) = _tmp10 * _tmp33 - _tmp11 * _tmp34 - _tmp29 * _tmp35 - _tmp30 * state(2, 0) + - _tmp31 * _tmp37 + _tmp36 + _tmp38 * _tmp43 + _tmp39 * _tmp6; - _p_new(2, 2) = _tmp10 * _tmp49 - _tmp11 * _tmp48 + _tmp28 - _tmp29 * _tmp47 + _tmp31 * _tmp46 + - _tmp40 + _tmp43 * _tmp44 + _tmp45 * _tmp6 + _tmp50; - _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 * _tmp14 + _tmp12 * _tmp13 - _tmp15 * _tmp29 + _tmp16 * _tmp9 + _tmp17 + - _tmp19 * _tmp43 + _tmp23 * _tmp32 - _tmp42 * state(3, 0); - _p_new(1, 3) = -_tmp10 * _tmp35 + _tmp12 * _tmp34 - _tmp29 * _tmp33 - _tmp30 * state(3, 0) + - _tmp32 * _tmp38 + _tmp36 * _tmp9 + _tmp37 + _tmp39 * _tmp43; - _p_new(2, 3) = -_tmp10 * _tmp47 + _tmp12 * _tmp48 - _tmp21 * state(2, 0) * state(3, 0) - - _tmp29 * _tmp49 + _tmp32 * _tmp44 + _tmp43 * _tmp45 + _tmp46 + _tmp50 * _tmp9; - _p_new(3, 3) = -_tmp10 * _tmp54 + _tmp12 * _tmp56 + _tmp27 - _tmp29 * _tmp55 + _tmp32 * _tmp53 + - _tmp41 + _tmp43 * _tmp52 + _tmp51 * _tmp9 + _tmp57; - _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) = _tmp16 * _tmp90 + _tmp17 * _tmp93 + _tmp19 * _tmp85 + _tmp23 * _tmp64 - - _tmp65 * _tmp69 - _tmp70 * _tmp75 - _tmp76 * _tmp81 + _tmp95; - _p_new(1, 4) = _tmp36 * _tmp90 + _tmp37 * _tmp93 + _tmp38 * _tmp64 + _tmp39 * _tmp85 - - _tmp69 * _tmp96 - _tmp75 * _tmp98 - _tmp81 * _tmp97 + _tmp99; - _p_new(2, 4) = -_tmp100 * _tmp69 - _tmp101 * _tmp81 - _tmp102 * _tmp75 + _tmp103 + - _tmp44 * _tmp64 + _tmp45 * _tmp85 + _tmp46 * _tmp93 + _tmp50 * _tmp90; - _p_new(3, 4) = -_tmp104 * _tmp69 - _tmp105 * _tmp81 - _tmp106 * _tmp75 + _tmp107 + - _tmp51 * _tmp90 + _tmp52 * _tmp85 + _tmp53 * _tmp64 + _tmp57 * _tmp93; - _p_new(4, 4) = -_tmp108 * _tmp69 - _tmp109 * _tmp81 - _tmp110 * _tmp75 + _tmp111 * _tmp85 + - _tmp112 * _tmp64 + _tmp113 * _tmp93 + _tmp114 * _tmp90 + _tmp115 + - std::pow(_tmp68, Scalar(2)) * d_vel_var(0, 0) + - std::pow(_tmp74, Scalar(2)) * d_vel_var(2, 0) + - std::pow(_tmp80, Scalar(2)) * d_vel_var(1, 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) = -_tmp119 * _tmp70 - _tmp122 * _tmp76 - _tmp124 * _tmp65 + _tmp127 * _tmp19 + - _tmp129 * _tmp17 + _tmp131 * _tmp23 + _tmp133 * _tmp16 + _tmp134; - _p_new(1, 5) = -_tmp119 * _tmp98 - _tmp122 * _tmp97 - _tmp124 * _tmp96 + _tmp127 * _tmp39 + - _tmp129 * _tmp37 + _tmp131 * _tmp38 + _tmp133 * _tmp36 + _tmp135; - _p_new(2, 5) = -_tmp100 * _tmp124 - _tmp101 * _tmp122 - _tmp102 * _tmp119 + _tmp127 * _tmp45 + - _tmp129 * _tmp46 + _tmp131 * _tmp44 + _tmp133 * _tmp50 + _tmp136; - _p_new(3, 5) = -_tmp104 * _tmp124 - _tmp105 * _tmp122 - _tmp106 * _tmp119 + _tmp127 * _tmp52 + - _tmp129 * _tmp57 + _tmp131 * _tmp53 + _tmp133 * _tmp51 + _tmp137; - _p_new(4, 5) = -_tmp108 * _tmp124 - _tmp109 * _tmp122 - _tmp110 * _tmp119 + _tmp111 * _tmp127 + - _tmp112 * _tmp131 + _tmp113 * _tmp129 + _tmp114 * _tmp133 + - _tmp118 * _tmp74 * d_vel_var(2, 0) + _tmp121 * _tmp139 + _tmp138 * _tmp68 + - _tmp140; - _p_new(5, 5) = std::pow(_tmp118, Scalar(2)) * d_vel_var(2, 0) - _tmp119 * _tmp143 + - std::pow(_tmp121, Scalar(2)) * d_vel_var(1, 0) - _tmp122 * _tmp142 + - std::pow(_tmp123, Scalar(2)) * d_vel_var(0, 0) - _tmp124 * _tmp141 + - _tmp127 * _tmp147 + _tmp129 * _tmp146 + _tmp131 * _tmp145 + _tmp133 * _tmp144 + - _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) = -_tmp150 * _tmp76 - _tmp152 * _tmp70 - _tmp154 * _tmp65 + _tmp156 * _tmp19 + - _tmp157 * _tmp16 + _tmp158 * _tmp23 + _tmp159 * _tmp17 + _tmp161; - _p_new(1, 6) = -_tmp150 * _tmp97 - _tmp152 * _tmp98 - _tmp154 * _tmp96 + _tmp156 * _tmp39 + - _tmp157 * _tmp36 + _tmp158 * _tmp38 + _tmp159 * _tmp37 + _tmp162; - _p_new(2, 6) = -_tmp100 * _tmp154 - _tmp101 * _tmp150 - _tmp102 * _tmp152 + _tmp156 * _tmp45 + - _tmp157 * _tmp50 + _tmp158 * _tmp44 + _tmp159 * _tmp46 + _tmp163; - _p_new(3, 6) = -_tmp104 * _tmp154 - _tmp105 * _tmp150 - _tmp106 * _tmp152 + _tmp156 * _tmp52 + - _tmp157 * _tmp51 + _tmp158 * _tmp53 + _tmp159 * _tmp57 + _tmp164; - _p_new(4, 6) = -_tmp108 * _tmp154 - _tmp109 * _tmp150 - _tmp110 * _tmp152 + _tmp111 * _tmp156 + - _tmp112 * _tmp158 + _tmp113 * _tmp159 + _tmp114 * _tmp157 + _tmp139 * _tmp149 + - _tmp153 * _tmp68 * d_vel_var(0, 0) + _tmp165 * _tmp74 + _tmp166; - _p_new(5, 6) = _tmp118 * _tmp165 + _tmp121 * _tmp149 * d_vel_var(1, 0) + _tmp138 * _tmp153 - - _tmp141 * _tmp154 - _tmp142 * _tmp150 - _tmp143 * _tmp152 + _tmp144 * _tmp157 + - _tmp145 * _tmp158 + _tmp146 * _tmp159 + _tmp147 * _tmp156 + _tmp167; - _p_new(6, 6) = - std::pow(_tmp149, Scalar(2)) * d_vel_var(1, 0) - _tmp150 * _tmp169 + - std::pow(_tmp151, Scalar(2)) * d_vel_var(2, 0) - _tmp152 * _tmp170 + - std::pow(_tmp153, Scalar(2)) * d_vel_var(0, 0) - _tmp154 * _tmp168 + - _tmp156 * (P(0, 1) * _tmp158 + P(1, 1) * _tmp156 - P(13, 1) * _tmp154 - P(14, 1) * _tmp150 - - P(15, 1) * _tmp152 + P(2, 1) * _tmp157 + P(3, 1) * _tmp159 + P(6, 1)) + - _tmp157 * (P(0, 2) * _tmp158 + P(1, 2) * _tmp156 - P(13, 2) * _tmp154 - P(14, 2) * _tmp150 - - P(15, 2) * _tmp152 + P(2, 2) * _tmp157 + P(3, 2) * _tmp159 + P(6, 2)) + - _tmp158 * (P(0, 0) * _tmp158 + P(1, 0) * _tmp156 - P(13, 0) * _tmp154 - P(14, 0) * _tmp150 - - P(15, 0) * _tmp152 + P(2, 0) * _tmp157 + P(3, 0) * _tmp159 + P(6, 0)) + - _tmp159 * (P(0, 3) * _tmp158 + P(1, 3) * _tmp156 - P(13, 3) * _tmp154 - P(14, 3) * _tmp150 - - P(15, 3) * _tmp152 + P(2, 3) * _tmp157 + P(3, 3) * _tmp159 + P(6, 3)) + - _tmp171; - _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) * _tmp9 + P(10, 7) * _tmp10 + P(11, 7) * _tmp12 + - P(12, 7) * _tmp11 + P(2, 7) * _tmp3 + P(3, 7) * _tmp6 + _tmp95 * d_vel_dt; - _p_new(1, 7) = P(0, 7) * _tmp31 + P(1, 7) - P(10, 7) * _tmp29 + P(11, 7) * _tmp11 - - P(12, 7) * _tmp12 + P(2, 7) * _tmp32 + P(3, 7) * _tmp3 + _tmp99 * d_vel_dt; - _p_new(2, 7) = P(0, 7) * _tmp43 + P(1, 7) * _tmp6 - P(10, 7) * _tmp11 - P(11, 7) * _tmp29 + - P(12, 7) * _tmp10 + P(2, 7) + P(3, 7) * _tmp31 + _tmp103 * d_vel_dt; - _p_new(3, 7) = P(0, 7) * _tmp32 + P(1, 7) * _tmp43 + P(10, 7) * _tmp12 - P(11, 7) * _tmp10 - - P(12, 7) * _tmp29 + P(2, 7) * _tmp9 + P(3, 7) + _tmp107 * d_vel_dt; - _p_new(4, 7) = P(0, 7) * _tmp64 + P(1, 7) * _tmp85 - P(13, 7) * _tmp69 - P(14, 7) * _tmp81 - - P(15, 7) * _tmp75 + P(2, 7) * _tmp90 + P(3, 7) * _tmp93 + P(4, 7) + - _tmp115 * d_vel_dt; - _p_new(5, 7) = P(0, 7) * _tmp131 + P(1, 7) * _tmp127 - P(13, 7) * _tmp124 - P(14, 7) * _tmp122 - - P(15, 7) * _tmp119 + P(2, 7) * _tmp133 + P(3, 7) * _tmp129 + P(5, 7) + - d_vel_dt * (P(0, 4) * _tmp131 + P(1, 4) * _tmp127 - P(13, 4) * _tmp124 - - P(14, 4) * _tmp122 - P(15, 4) * _tmp119 + P(2, 4) * _tmp133 + - P(3, 4) * _tmp129 + P(5, 4)); - _p_new(6, 7) = P(0, 7) * _tmp158 + P(1, 7) * _tmp156 - P(13, 7) * _tmp154 - P(14, 7) * _tmp150 - - P(15, 7) * _tmp152 + P(2, 7) * _tmp157 + P(3, 7) * _tmp159 + P(6, 7) + - d_vel_dt * (P(0, 4) * _tmp158 + P(1, 4) * _tmp156 - P(13, 4) * _tmp154 - - P(14, 4) * _tmp150 - P(15, 4) * _tmp152 + P(2, 4) * _tmp157 + - P(3, 4) * _tmp159 + P(6, 4)); - _p_new(7, 7) = P(4, 7) * d_vel_dt + P(7, 7) + d_vel_dt * (P(4, 4) * d_vel_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) * _tmp9 + P(10, 8) * _tmp10 + P(12, 8) * _tmp11 + - P(2, 8) * _tmp3 + P(3, 8) * _tmp6 + _tmp134 * d_vel_dt + _tmp172 * state(2, 0); - _p_new(1, 8) = P(0, 8) * _tmp31 + P(1, 8) - P(10, 8) * _tmp29 - P(12, 8) * _tmp12 + - P(2, 8) * _tmp32 + P(3, 8) * _tmp3 + _tmp135 * d_vel_dt + _tmp172 * state(3, 0); - _p_new(2, 8) = P(0, 8) * _tmp43 + P(1, 8) * _tmp6 - P(10, 8) * _tmp11 + P(12, 8) * _tmp10 + - P(2, 8) + P(3, 8) * _tmp31 + _tmp136 * d_vel_dt - _tmp172 * state(0, 0); - _p_new(3, 8) = P(0, 8) * _tmp32 + P(1, 8) * _tmp43 + P(10, 8) * _tmp12 - P(12, 8) * _tmp29 + - P(2, 8) * _tmp9 + P(3, 8) + _tmp137 * d_vel_dt - _tmp172 * state(1, 0); - _p_new(4, 8) = P(0, 8) * _tmp64 + P(1, 8) * _tmp85 - P(13, 8) * _tmp69 - P(14, 8) * _tmp81 - - P(15, 8) * _tmp75 + P(2, 8) * _tmp90 + P(3, 8) * _tmp93 + P(4, 8) + - _tmp140 * d_vel_dt; - _p_new(5, 8) = P(0, 8) * _tmp131 + P(1, 8) * _tmp127 - P(13, 8) * _tmp124 - P(14, 8) * _tmp122 - - P(15, 8) * _tmp119 + P(2, 8) * _tmp133 + P(3, 8) * _tmp129 + P(5, 8) + - _tmp148 * d_vel_dt; - _p_new(6, 8) = P(0, 8) * _tmp158 + P(1, 8) * _tmp156 - P(13, 8) * _tmp154 - P(14, 8) * _tmp150 - - P(15, 8) * _tmp152 + P(2, 8) * _tmp157 + P(3, 8) * _tmp159 + P(6, 8) + - d_vel_dt * (P(0, 5) * _tmp158 + P(1, 5) * _tmp156 - P(13, 5) * _tmp154 - - P(14, 5) * _tmp150 - P(15, 5) * _tmp152 + P(2, 5) * _tmp157 + - P(3, 5) * _tmp159 + P(6, 5)); - _p_new(7, 8) = P(4, 8) * d_vel_dt + P(7, 8) + d_vel_dt * (P(4, 5) * d_vel_dt + P(7, 5)); - _p_new(8, 8) = P(5, 8) * d_vel_dt + P(8, 8) + d_vel_dt * (P(5, 5) * d_vel_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) * _tmp9 + P(10, 9) * _tmp10 + P(11, 9) * _tmp12 + - P(12, 9) * _tmp11 + P(2, 9) * _tmp3 + P(3, 9) * _tmp6 + _tmp161 * d_vel_dt; - _p_new(1, 9) = P(0, 9) * _tmp31 + P(1, 9) - P(10, 9) * _tmp29 + P(11, 9) * _tmp11 - - P(12, 9) * _tmp12 + P(2, 9) * _tmp32 + P(3, 9) * _tmp3 + _tmp162 * d_vel_dt; - _p_new(2, 9) = P(0, 9) * _tmp43 + P(1, 9) * _tmp6 - P(10, 9) * _tmp11 - P(11, 9) * _tmp29 + - P(12, 9) * _tmp10 + P(2, 9) + P(3, 9) * _tmp31 + _tmp163 * d_vel_dt; - _p_new(3, 9) = P(0, 9) * _tmp32 + P(1, 9) * _tmp43 + P(10, 9) * _tmp12 - P(11, 9) * _tmp10 - - P(12, 9) * _tmp29 + P(2, 9) * _tmp9 + P(3, 9) + _tmp164 * d_vel_dt; - _p_new(4, 9) = P(0, 9) * _tmp64 + P(1, 9) * _tmp85 - P(13, 9) * _tmp69 - P(14, 9) * _tmp81 - - P(15, 9) * _tmp75 + P(2, 9) * _tmp90 + P(3, 9) * _tmp93 + P(4, 9) + - _tmp166 * d_vel_dt; - _p_new(5, 9) = P(0, 9) * _tmp131 + P(1, 9) * _tmp127 - P(13, 9) * _tmp124 - P(14, 9) * _tmp122 - - P(15, 9) * _tmp119 + P(2, 9) * _tmp133 + P(3, 9) * _tmp129 + P(5, 9) + - _tmp167 * d_vel_dt; - _p_new(6, 9) = P(0, 9) * _tmp158 + P(1, 9) * _tmp156 - P(13, 9) * _tmp154 - P(14, 9) * _tmp150 - - P(15, 9) * _tmp152 + P(2, 9) * _tmp157 + P(3, 9) * _tmp159 + P(6, 9) + - _tmp171 * d_vel_dt; - _p_new(7, 9) = P(4, 9) * d_vel_dt + P(7, 9) + d_vel_dt * (P(4, 6) * d_vel_dt + P(7, 6)); - _p_new(8, 9) = P(5, 9) * d_vel_dt + P(8, 9) + d_vel_dt * (P(5, 6) * d_vel_dt + P(8, 6)); - _p_new(9, 9) = P(6, 9) * d_vel_dt + P(9, 9) + d_vel_dt * (P(6, 6) * d_vel_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) = _tmp34; - _p_new(2, 10) = _tmp48; - _p_new(3, 10) = _tmp56; - _p_new(4, 10) = P(0, 10) * _tmp64 + P(1, 10) * _tmp85 - P(13, 10) * _tmp69 - - P(14, 10) * _tmp81 - P(15, 10) * _tmp75 + P(2, 10) * _tmp90 + - P(3, 10) * _tmp93 + P(4, 10); - _p_new(5, 10) = P(0, 10) * _tmp131 + P(1, 10) * _tmp127 - P(13, 10) * _tmp124 - - P(14, 10) * _tmp122 - P(15, 10) * _tmp119 + P(2, 10) * _tmp133 + - P(3, 10) * _tmp129 + P(5, 10); - _p_new(6, 10) = P(0, 10) * _tmp158 + P(1, 10) * _tmp156 - P(13, 10) * _tmp154 - - P(14, 10) * _tmp150 - P(15, 10) * _tmp152 + P(2, 10) * _tmp157 + - P(3, 10) * _tmp159 + P(6, 10); - _p_new(7, 10) = P(4, 10) * d_vel_dt + P(7, 10); - _p_new(8, 10) = P(5, 10) * d_vel_dt + P(8, 10); - _p_new(9, 10) = P(6, 10) * d_vel_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) = _tmp14; - _p_new(1, 11) = _tmp35; - _p_new(2, 11) = _tmp47; - _p_new(3, 11) = _tmp54; - _p_new(4, 11) = P(0, 11) * _tmp64 + P(1, 11) * _tmp85 - P(13, 11) * _tmp69 - - P(14, 11) * _tmp81 - P(15, 11) * _tmp75 + P(2, 11) * _tmp90 + - P(3, 11) * _tmp93 + P(4, 11); - _p_new(5, 11) = P(0, 11) * _tmp131 + P(1, 11) * _tmp127 - P(13, 11) * _tmp124 - - P(14, 11) * _tmp122 - P(15, 11) * _tmp119 + P(2, 11) * _tmp133 + - P(3, 11) * _tmp129 + P(5, 11); - _p_new(6, 11) = P(0, 11) * _tmp158 + P(1, 11) * _tmp156 - P(13, 11) * _tmp154 - - P(14, 11) * _tmp150 - P(15, 11) * _tmp152 + P(2, 11) * _tmp157 + - P(3, 11) * _tmp159 + P(6, 11); - _p_new(7, 11) = P(4, 11) * d_vel_dt + P(7, 11); - _p_new(8, 11) = P(5, 11) * d_vel_dt + P(8, 11); - _p_new(9, 11) = P(6, 11) * d_vel_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) = _tmp15; - _p_new(1, 12) = _tmp33; - _p_new(2, 12) = _tmp49; - _p_new(3, 12) = _tmp55; - _p_new(4, 12) = P(0, 12) * _tmp64 + P(1, 12) * _tmp85 - P(13, 12) * _tmp69 - - P(14, 12) * _tmp81 - P(15, 12) * _tmp75 + P(2, 12) * _tmp90 + - P(3, 12) * _tmp93 + P(4, 12); - _p_new(5, 12) = P(0, 12) * _tmp131 + P(1, 12) * _tmp127 - P(13, 12) * _tmp124 - - P(14, 12) * _tmp122 - P(15, 12) * _tmp119 + P(2, 12) * _tmp133 + - P(3, 12) * _tmp129 + P(5, 12); - _p_new(6, 12) = P(0, 12) * _tmp158 + P(1, 12) * _tmp156 - P(13, 12) * _tmp154 - - P(14, 12) * _tmp150 - P(15, 12) * _tmp152 + P(2, 12) * _tmp157 + - P(3, 12) * _tmp159 + P(6, 12); - _p_new(7, 12) = P(4, 12) * d_vel_dt + P(7, 12); - _p_new(8, 12) = P(5, 12) * d_vel_dt + P(8, 12); - _p_new(9, 12) = P(6, 12) * d_vel_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) = _tmp65; - _p_new(1, 13) = _tmp96; - _p_new(2, 13) = _tmp100; - _p_new(3, 13) = _tmp104; - _p_new(4, 13) = _tmp108; - _p_new(5, 13) = _tmp141; - _p_new(6, 13) = _tmp168; - _p_new(7, 13) = P(4, 13) * d_vel_dt + P(7, 13); - _p_new(8, 13) = P(5, 13) * d_vel_dt + P(8, 13); - _p_new(9, 13) = P(6, 13) * d_vel_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) = _tmp76; - _p_new(1, 14) = _tmp97; - _p_new(2, 14) = _tmp101; - _p_new(3, 14) = _tmp105; - _p_new(4, 14) = _tmp109; - _p_new(5, 14) = _tmp142; - _p_new(6, 14) = _tmp169; - _p_new(7, 14) = P(4, 14) * d_vel_dt + P(7, 14); - _p_new(8, 14) = P(5, 14) * d_vel_dt + P(8, 14); - _p_new(9, 14) = P(6, 14) * d_vel_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) = _tmp70; - _p_new(1, 15) = _tmp98; - _p_new(2, 15) = _tmp102; - _p_new(3, 15) = _tmp106; - _p_new(4, 15) = _tmp110; - _p_new(5, 15) = _tmp143; - _p_new(6, 15) = _tmp170; - _p_new(7, 15) = P(4, 15) * d_vel_dt + P(7, 15); - _p_new(8, 15) = P(5, 15) * d_vel_dt + P(8, 15); - _p_new(9, 15) = P(6, 15) * d_vel_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) * _tmp9 + P(10, 16) * _tmp10 + P(11, 16) * _tmp12 + - P(12, 16) * _tmp11 + P(2, 16) * _tmp3 + P(3, 16) * _tmp6; - _p_new(1, 16) = P(0, 16) * _tmp31 + P(1, 16) - P(10, 16) * _tmp29 + P(11, 16) * _tmp11 - - P(12, 16) * _tmp12 + P(2, 16) * _tmp32 + P(3, 16) * _tmp3; - _p_new(2, 16) = P(0, 16) * _tmp43 + P(1, 16) * _tmp6 - P(10, 16) * _tmp11 - P(11, 16) * _tmp29 + - P(12, 16) * _tmp10 + P(2, 16) + P(3, 16) * _tmp31; - _p_new(3, 16) = P(0, 16) * _tmp32 + P(1, 16) * _tmp43 + P(10, 16) * _tmp12 - - P(11, 16) * _tmp10 - P(12, 16) * _tmp29 + P(2, 16) * _tmp9 + P(3, 16); - _p_new(4, 16) = P(0, 16) * _tmp64 + P(1, 16) * _tmp85 - P(13, 16) * _tmp69 - - P(14, 16) * _tmp81 - P(15, 16) * _tmp75 + P(2, 16) * _tmp90 + - P(3, 16) * _tmp93 + P(4, 16); - _p_new(5, 16) = P(0, 16) * _tmp131 + P(1, 16) * _tmp127 - P(13, 16) * _tmp124 - - P(14, 16) * _tmp122 - P(15, 16) * _tmp119 + P(2, 16) * _tmp133 + - P(3, 16) * _tmp129 + P(5, 16); - _p_new(6, 16) = P(0, 16) * _tmp158 + P(1, 16) * _tmp156 - P(13, 16) * _tmp154 - - P(14, 16) * _tmp150 - P(15, 16) * _tmp152 + P(2, 16) * _tmp157 + - P(3, 16) * _tmp159 + P(6, 16); - _p_new(7, 16) = P(4, 16) * d_vel_dt + P(7, 16); - _p_new(8, 16) = P(5, 16) * d_vel_dt + P(8, 16); - _p_new(9, 16) = P(6, 16) * d_vel_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) * _tmp9 + P(10, 17) * _tmp10 + P(11, 17) * _tmp12 + - P(12, 17) * _tmp11 + P(2, 17) * _tmp3 + P(3, 17) * _tmp6; - _p_new(1, 17) = P(0, 17) * _tmp31 + P(1, 17) - P(10, 17) * _tmp29 + P(11, 17) * _tmp11 - - P(12, 17) * _tmp12 + P(2, 17) * _tmp32 + P(3, 17) * _tmp3; - _p_new(2, 17) = P(0, 17) * _tmp43 + P(1, 17) * _tmp6 - P(10, 17) * _tmp11 - P(11, 17) * _tmp29 + - P(12, 17) * _tmp10 + P(2, 17) + P(3, 17) * _tmp31; - _p_new(3, 17) = P(0, 17) * _tmp32 + P(1, 17) * _tmp43 + P(10, 17) * _tmp12 - - P(11, 17) * _tmp10 - P(12, 17) * _tmp29 + P(2, 17) * _tmp9 + P(3, 17); - _p_new(4, 17) = P(0, 17) * _tmp64 + P(1, 17) * _tmp85 - P(13, 17) * _tmp69 - - P(14, 17) * _tmp81 - P(15, 17) * _tmp75 + P(2, 17) * _tmp90 + - P(3, 17) * _tmp93 + P(4, 17); - _p_new(5, 17) = P(0, 17) * _tmp131 + P(1, 17) * _tmp127 - P(13, 17) * _tmp124 - - P(14, 17) * _tmp122 - P(15, 17) * _tmp119 + P(2, 17) * _tmp133 + - P(3, 17) * _tmp129 + P(5, 17); - _p_new(6, 17) = P(0, 17) * _tmp158 + P(1, 17) * _tmp156 - P(13, 17) * _tmp154 - - P(14, 17) * _tmp150 - P(15, 17) * _tmp152 + P(2, 17) * _tmp157 + - P(3, 17) * _tmp159 + P(6, 17); - _p_new(7, 17) = P(4, 17) * d_vel_dt + P(7, 17); - _p_new(8, 17) = P(5, 17) * d_vel_dt + P(8, 17); - _p_new(9, 17) = P(6, 17) * d_vel_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) * _tmp9 + P(10, 18) * _tmp10 + P(11, 18) * _tmp12 + - P(12, 18) * _tmp11 + P(2, 18) * _tmp3 + P(3, 18) * _tmp6; - _p_new(1, 18) = P(0, 18) * _tmp31 + P(1, 18) - P(10, 18) * _tmp29 + P(11, 18) * _tmp11 - - P(12, 18) * _tmp12 + P(2, 18) * _tmp32 + P(3, 18) * _tmp3; - _p_new(2, 18) = P(0, 18) * _tmp43 + P(1, 18) * _tmp6 - P(10, 18) * _tmp11 - P(11, 18) * _tmp29 + - P(12, 18) * _tmp10 + P(2, 18) + P(3, 18) * _tmp31; - _p_new(3, 18) = P(0, 18) * _tmp32 + P(1, 18) * _tmp43 + P(10, 18) * _tmp12 - - P(11, 18) * _tmp10 - P(12, 18) * _tmp29 + P(2, 18) * _tmp9 + P(3, 18); - _p_new(4, 18) = P(0, 18) * _tmp64 + P(1, 18) * _tmp85 - P(13, 18) * _tmp69 - - P(14, 18) * _tmp81 - P(15, 18) * _tmp75 + P(2, 18) * _tmp90 + - P(3, 18) * _tmp93 + P(4, 18); - _p_new(5, 18) = P(0, 18) * _tmp131 + P(1, 18) * _tmp127 - P(13, 18) * _tmp124 - - P(14, 18) * _tmp122 - P(15, 18) * _tmp119 + P(2, 18) * _tmp133 + - P(3, 18) * _tmp129 + P(5, 18); - _p_new(6, 18) = P(0, 18) * _tmp158 + P(1, 18) * _tmp156 - P(13, 18) * _tmp154 - - P(14, 18) * _tmp150 - P(15, 18) * _tmp152 + P(2, 18) * _tmp157 + - P(3, 18) * _tmp159 + P(6, 18); - _p_new(7, 18) = P(4, 18) * d_vel_dt + P(7, 18); - _p_new(8, 18) = P(5, 18) * d_vel_dt + P(8, 18); - _p_new(9, 18) = P(6, 18) * d_vel_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) * _tmp9 + P(10, 19) * _tmp10 + P(11, 19) * _tmp12 + - P(12, 19) * _tmp11 + P(2, 19) * _tmp3 + P(3, 19) * _tmp6; - _p_new(1, 19) = P(0, 19) * _tmp31 + P(1, 19) - P(10, 19) * _tmp29 + P(11, 19) * _tmp11 - - P(12, 19) * _tmp12 + P(2, 19) * _tmp32 + P(3, 19) * _tmp3; - _p_new(2, 19) = P(0, 19) * _tmp43 + P(1, 19) * _tmp6 - P(10, 19) * _tmp11 - P(11, 19) * _tmp29 + - P(12, 19) * _tmp10 + P(2, 19) + P(3, 19) * _tmp31; - _p_new(3, 19) = P(0, 19) * _tmp32 + P(1, 19) * _tmp43 + P(10, 19) * _tmp12 - - P(11, 19) * _tmp10 - P(12, 19) * _tmp29 + P(2, 19) * _tmp9 + P(3, 19); - _p_new(4, 19) = P(0, 19) * _tmp64 + P(1, 19) * _tmp85 - P(13, 19) * _tmp69 - - P(14, 19) * _tmp81 - P(15, 19) * _tmp75 + P(2, 19) * _tmp90 + - P(3, 19) * _tmp93 + P(4, 19); - _p_new(5, 19) = P(0, 19) * _tmp131 + P(1, 19) * _tmp127 - P(13, 19) * _tmp124 - - P(14, 19) * _tmp122 - P(15, 19) * _tmp119 + P(2, 19) * _tmp133 + - P(3, 19) * _tmp129 + P(5, 19); - _p_new(6, 19) = P(0, 19) * _tmp158 + P(1, 19) * _tmp156 - P(13, 19) * _tmp154 - - P(14, 19) * _tmp150 - P(15, 19) * _tmp152 + P(2, 19) * _tmp157 + - P(3, 19) * _tmp159 + P(6, 19); - _p_new(7, 19) = P(4, 19) * d_vel_dt + P(7, 19); - _p_new(8, 19) = P(5, 19) * d_vel_dt + P(8, 19); - _p_new(9, 19) = P(6, 19) * d_vel_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) * _tmp9 + P(10, 20) * _tmp10 + P(11, 20) * _tmp12 + - P(12, 20) * _tmp11 + P(2, 20) * _tmp3 + P(3, 20) * _tmp6; - _p_new(1, 20) = P(0, 20) * _tmp31 + P(1, 20) - P(10, 20) * _tmp29 + P(11, 20) * _tmp11 - - P(12, 20) * _tmp12 + P(2, 20) * _tmp32 + P(3, 20) * _tmp3; - _p_new(2, 20) = P(0, 20) * _tmp43 + P(1, 20) * _tmp6 - P(10, 20) * _tmp11 - P(11, 20) * _tmp29 + - P(12, 20) * _tmp10 + P(2, 20) + P(3, 20) * _tmp31; - _p_new(3, 20) = P(0, 20) * _tmp32 + P(1, 20) * _tmp43 + P(10, 20) * _tmp12 - - P(11, 20) * _tmp10 - P(12, 20) * _tmp29 + P(2, 20) * _tmp9 + P(3, 20); - _p_new(4, 20) = P(0, 20) * _tmp64 + P(1, 20) * _tmp85 - P(13, 20) * _tmp69 - - P(14, 20) * _tmp81 - P(15, 20) * _tmp75 + P(2, 20) * _tmp90 + - P(3, 20) * _tmp93 + P(4, 20); - _p_new(5, 20) = P(0, 20) * _tmp131 + P(1, 20) * _tmp127 - P(13, 20) * _tmp124 - - P(14, 20) * _tmp122 - P(15, 20) * _tmp119 + P(2, 20) * _tmp133 + - P(3, 20) * _tmp129 + P(5, 20); - _p_new(6, 20) = P(0, 20) * _tmp158 + P(1, 20) * _tmp156 - P(13, 20) * _tmp154 - - P(14, 20) * _tmp150 - P(15, 20) * _tmp152 + P(2, 20) * _tmp157 + - P(3, 20) * _tmp159 + P(6, 20); - _p_new(7, 20) = P(4, 20) * d_vel_dt + P(7, 20); - _p_new(8, 20) = P(5, 20) * d_vel_dt + P(8, 20); - _p_new(9, 20) = P(6, 20) * d_vel_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) * _tmp9 + P(10, 21) * _tmp10 + P(11, 21) * _tmp12 + - P(12, 21) * _tmp11 + P(2, 21) * _tmp3 + P(3, 21) * _tmp6; - _p_new(1, 21) = P(0, 21) * _tmp31 + P(1, 21) - P(10, 21) * _tmp29 + P(11, 21) * _tmp11 - - P(12, 21) * _tmp12 + P(2, 21) * _tmp32 + P(3, 21) * _tmp3; - _p_new(2, 21) = P(0, 21) * _tmp43 + P(1, 21) * _tmp6 - P(10, 21) * _tmp11 - P(11, 21) * _tmp29 + - P(12, 21) * _tmp10 + P(2, 21) + P(3, 21) * _tmp31; - _p_new(3, 21) = P(0, 21) * _tmp32 + P(1, 21) * _tmp43 + P(10, 21) * _tmp12 - - P(11, 21) * _tmp10 - P(12, 21) * _tmp29 + P(2, 21) * _tmp9 + P(3, 21); - _p_new(4, 21) = P(0, 21) * _tmp64 + P(1, 21) * _tmp85 - P(13, 21) * _tmp69 - - P(14, 21) * _tmp81 - P(15, 21) * _tmp75 + P(2, 21) * _tmp90 + - P(3, 21) * _tmp93 + P(4, 21); - _p_new(5, 21) = P(0, 21) * _tmp131 + P(1, 21) * _tmp127 - P(13, 21) * _tmp124 - - P(14, 21) * _tmp122 - P(15, 21) * _tmp119 + P(2, 21) * _tmp133 + - P(3, 21) * _tmp129 + P(5, 21); - _p_new(6, 21) = P(0, 21) * _tmp158 + P(1, 21) * _tmp156 - P(13, 21) * _tmp154 - - P(14, 21) * _tmp150 - P(15, 21) * _tmp152 + P(2, 21) * _tmp157 + - P(3, 21) * _tmp159 + P(6, 21); - _p_new(7, 21) = P(4, 21) * d_vel_dt + P(7, 21); - _p_new(8, 21) = P(5, 21) * d_vel_dt + P(8, 21); - _p_new(9, 21) = P(6, 21) * d_vel_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) * _tmp9 + P(10, 22) * _tmp10 + P(11, 22) * _tmp12 + - P(12, 22) * _tmp11 + P(2, 22) * _tmp3 + P(3, 22) * _tmp6; - _p_new(1, 22) = P(0, 22) * _tmp31 + P(1, 22) - P(10, 22) * _tmp29 + P(11, 22) * _tmp11 - - P(12, 22) * _tmp12 + P(2, 22) * _tmp32 + P(3, 22) * _tmp3; - _p_new(2, 22) = P(0, 22) * _tmp43 + P(1, 22) * _tmp6 - P(10, 22) * _tmp11 - P(11, 22) * _tmp29 + - P(12, 22) * _tmp10 + P(2, 22) + P(3, 22) * _tmp31; - _p_new(3, 22) = P(0, 22) * _tmp32 + P(1, 22) * _tmp43 + P(10, 22) * _tmp12 - - P(11, 22) * _tmp10 - P(12, 22) * _tmp29 + P(2, 22) * _tmp9 + P(3, 22); - _p_new(4, 22) = P(0, 22) * _tmp64 + P(1, 22) * _tmp85 - P(13, 22) * _tmp69 - - P(14, 22) * _tmp81 - P(15, 22) * _tmp75 + P(2, 22) * _tmp90 + - P(3, 22) * _tmp93 + P(4, 22); - _p_new(5, 22) = P(0, 22) * _tmp131 + P(1, 22) * _tmp127 - P(13, 22) * _tmp124 - - P(14, 22) * _tmp122 - P(15, 22) * _tmp119 + P(2, 22) * _tmp133 + - P(3, 22) * _tmp129 + P(5, 22); - _p_new(6, 22) = P(0, 22) * _tmp158 + P(1, 22) * _tmp156 - P(13, 22) * _tmp154 - - P(14, 22) * _tmp150 - P(15, 22) * _tmp152 + P(2, 22) * _tmp157 + - P(3, 22) * _tmp159 + P(6, 22); - _p_new(7, 22) = P(4, 22) * d_vel_dt + P(7, 22); - _p_new(8, 22) = P(5, 22) * d_vel_dt + P(8, 22); - _p_new(9, 22) = P(6, 22) * d_vel_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) * _tmp9 + P(10, 23) * _tmp10 + P(11, 23) * _tmp12 + - P(12, 23) * _tmp11 + P(2, 23) * _tmp3 + P(3, 23) * _tmp6; - _p_new(1, 23) = P(0, 23) * _tmp31 + P(1, 23) - P(10, 23) * _tmp29 + P(11, 23) * _tmp11 - - P(12, 23) * _tmp12 + P(2, 23) * _tmp32 + P(3, 23) * _tmp3; - _p_new(2, 23) = P(0, 23) * _tmp43 + P(1, 23) * _tmp6 - P(10, 23) * _tmp11 - P(11, 23) * _tmp29 + - P(12, 23) * _tmp10 + P(2, 23) + P(3, 23) * _tmp31; - _p_new(3, 23) = P(0, 23) * _tmp32 + P(1, 23) * _tmp43 + P(10, 23) * _tmp12 - - P(11, 23) * _tmp10 - P(12, 23) * _tmp29 + P(2, 23) * _tmp9 + P(3, 23); - _p_new(4, 23) = P(0, 23) * _tmp64 + P(1, 23) * _tmp85 - P(13, 23) * _tmp69 - - P(14, 23) * _tmp81 - P(15, 23) * _tmp75 + P(2, 23) * _tmp90 + - P(3, 23) * _tmp93 + P(4, 23); - _p_new(5, 23) = P(0, 23) * _tmp131 + P(1, 23) * _tmp127 - P(13, 23) * _tmp124 - - P(14, 23) * _tmp122 - P(15, 23) * _tmp119 + P(2, 23) * _tmp133 + - P(3, 23) * _tmp129 + P(5, 23); - _p_new(6, 23) = P(0, 23) * _tmp158 + P(1, 23) * _tmp156 - P(13, 23) * _tmp154 - - P(14, 23) * _tmp150 - P(15, 23) * _tmp152 + P(2, 23) * _tmp157 + - P(3, 23) * _tmp159 + P(6, 23); - _p_new(7, 23) = P(4, 23) * d_vel_dt + P(7, 23); - _p_new(8, 23) = P(5, 23) * d_vel_dt + P(8, 23); - _p_new(9, 23) = P(6, 23) * d_vel_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); - } + _res(0, 0) = _tmp10 * _tmp13 + _tmp11 * _tmp15 + _tmp12 * _tmp14 + _tmp16 * _tmp3 + + _tmp17 * _tmp6 + _tmp19 * _tmp9 + _tmp22 + _tmp23 + _tmp28; + _res(1, 0) = 0; + _res(2, 0) = 0; + _res(3, 0) = 0; + _res(4, 0) = 0; + _res(5, 0) = 0; + _res(6, 0) = 0; + _res(7, 0) = 0; + _res(8, 0) = 0; + _res(9, 0) = 0; + _res(10, 0) = 0; + _res(11, 0) = 0; + _res(12, 0) = 0; + _res(13, 0) = 0; + _res(14, 0) = 0; + _res(15, 0) = 0; + _res(16, 0) = 0; + _res(17, 0) = 0; + _res(18, 0) = 0; + _res(19, 0) = 0; + _res(20, 0) = 0; + _res(21, 0) = 0; + _res(22, 0) = 0; + _res(23, 0) = 0; + _res(0, 1) = _tmp11 * _tmp14 - _tmp12 * _tmp15 - _tmp13 * _tmp29 + _tmp16 * _tmp32 + + _tmp17 * _tmp3 + _tmp19 + _tmp23 * _tmp31 - _tmp30 * state(0, 0); + _res(1, 1) = _tmp11 * _tmp35 - _tmp12 * _tmp33 + _tmp25 - _tmp29 * _tmp34 + _tmp3 * _tmp37 + + _tmp31 * _tmp38 + _tmp32 * _tmp36 + _tmp39 + _tmp41; + _res(2, 1) = 0; + _res(3, 1) = 0; + _res(4, 1) = 0; + _res(5, 1) = 0; + _res(6, 1) = 0; + _res(7, 1) = 0; + _res(8, 1) = 0; + _res(9, 1) = 0; + _res(10, 1) = 0; + _res(11, 1) = 0; + _res(12, 1) = 0; + _res(13, 1) = 0; + _res(14, 1) = 0; + _res(15, 1) = 0; + _res(16, 1) = 0; + _res(17, 1) = 0; + _res(18, 1) = 0; + _res(19, 1) = 0; + _res(20, 1) = 0; + _res(21, 1) = 0; + _res(22, 1) = 0; + _res(23, 1) = 0; + _res(0, 2) = _tmp10 * _tmp15 - _tmp11 * _tmp13 - _tmp14 * _tmp29 + _tmp16 + _tmp17 * _tmp31 + + _tmp19 * _tmp6 + _tmp23 * _tmp43 - _tmp42 * state(2, 0); + _res(1, 2) = _tmp10 * _tmp33 - _tmp11 * _tmp34 - _tmp29 * _tmp35 - _tmp30 * state(2, 0) + + _tmp31 * _tmp37 + _tmp36 + _tmp38 * _tmp43 + _tmp39 * _tmp6; + _res(2, 2) = _tmp10 * _tmp49 - _tmp11 * _tmp48 + _tmp28 - _tmp29 * _tmp47 + _tmp31 * _tmp46 + + _tmp40 + _tmp43 * _tmp44 + _tmp45 * _tmp6 + _tmp50; + _res(3, 2) = 0; + _res(4, 2) = 0; + _res(5, 2) = 0; + _res(6, 2) = 0; + _res(7, 2) = 0; + _res(8, 2) = 0; + _res(9, 2) = 0; + _res(10, 2) = 0; + _res(11, 2) = 0; + _res(12, 2) = 0; + _res(13, 2) = 0; + _res(14, 2) = 0; + _res(15, 2) = 0; + _res(16, 2) = 0; + _res(17, 2) = 0; + _res(18, 2) = 0; + _res(19, 2) = 0; + _res(20, 2) = 0; + _res(21, 2) = 0; + _res(22, 2) = 0; + _res(23, 2) = 0; + _res(0, 3) = -_tmp10 * _tmp14 + _tmp12 * _tmp13 - _tmp15 * _tmp29 + _tmp16 * _tmp9 + _tmp17 + + _tmp19 * _tmp43 + _tmp23 * _tmp32 - _tmp42 * state(3, 0); + _res(1, 3) = -_tmp10 * _tmp35 + _tmp12 * _tmp34 - _tmp29 * _tmp33 - _tmp30 * state(3, 0) + + _tmp32 * _tmp38 + _tmp36 * _tmp9 + _tmp37 + _tmp39 * _tmp43; + _res(2, 3) = -_tmp10 * _tmp47 + _tmp12 * _tmp48 - _tmp21 * state(2, 0) * state(3, 0) - + _tmp29 * _tmp49 + _tmp32 * _tmp44 + _tmp43 * _tmp45 + _tmp46 + _tmp50 * _tmp9; + _res(3, 3) = -_tmp10 * _tmp54 + _tmp12 * _tmp56 + _tmp27 - _tmp29 * _tmp55 + _tmp32 * _tmp53 + + _tmp41 + _tmp43 * _tmp52 + _tmp51 * _tmp9 + _tmp57; + _res(4, 3) = 0; + _res(5, 3) = 0; + _res(6, 3) = 0; + _res(7, 3) = 0; + _res(8, 3) = 0; + _res(9, 3) = 0; + _res(10, 3) = 0; + _res(11, 3) = 0; + _res(12, 3) = 0; + _res(13, 3) = 0; + _res(14, 3) = 0; + _res(15, 3) = 0; + _res(16, 3) = 0; + _res(17, 3) = 0; + _res(18, 3) = 0; + _res(19, 3) = 0; + _res(20, 3) = 0; + _res(21, 3) = 0; + _res(22, 3) = 0; + _res(23, 3) = 0; + _res(0, 4) = _tmp16 * _tmp90 + _tmp17 * _tmp93 + _tmp19 * _tmp85 + _tmp23 * _tmp64 - + _tmp65 * _tmp69 - _tmp70 * _tmp75 - _tmp76 * _tmp81 + _tmp95; + _res(1, 4) = _tmp36 * _tmp90 + _tmp37 * _tmp93 + _tmp38 * _tmp64 + _tmp39 * _tmp85 - + _tmp69 * _tmp96 - _tmp75 * _tmp98 - _tmp81 * _tmp97 + _tmp99; + _res(2, 4) = -_tmp100 * _tmp69 - _tmp101 * _tmp81 - _tmp102 * _tmp75 + _tmp103 + _tmp44 * _tmp64 + + _tmp45 * _tmp85 + _tmp46 * _tmp93 + _tmp50 * _tmp90; + _res(3, 4) = -_tmp104 * _tmp69 - _tmp105 * _tmp81 - _tmp106 * _tmp75 + _tmp107 + _tmp51 * _tmp90 + + _tmp52 * _tmp85 + _tmp53 * _tmp64 + _tmp57 * _tmp93; + _res(4, 4) = -_tmp108 * _tmp69 - _tmp109 * _tmp81 - _tmp110 * _tmp75 + _tmp111 * _tmp85 + + _tmp112 * _tmp64 + _tmp113 * _tmp93 + _tmp114 * _tmp90 + _tmp115 + + std::pow(_tmp68, Scalar(2)) * d_vel_var(0, 0) + + std::pow(_tmp74, Scalar(2)) * d_vel_var(2, 0) + + std::pow(_tmp80, Scalar(2)) * d_vel_var(1, 0); + _res(5, 4) = 0; + _res(6, 4) = 0; + _res(7, 4) = 0; + _res(8, 4) = 0; + _res(9, 4) = 0; + _res(10, 4) = 0; + _res(11, 4) = 0; + _res(12, 4) = 0; + _res(13, 4) = 0; + _res(14, 4) = 0; + _res(15, 4) = 0; + _res(16, 4) = 0; + _res(17, 4) = 0; + _res(18, 4) = 0; + _res(19, 4) = 0; + _res(20, 4) = 0; + _res(21, 4) = 0; + _res(22, 4) = 0; + _res(23, 4) = 0; + _res(0, 5) = -_tmp119 * _tmp70 - _tmp122 * _tmp76 - _tmp124 * _tmp65 + _tmp127 * _tmp19 + + _tmp129 * _tmp17 + _tmp131 * _tmp23 + _tmp133 * _tmp16 + _tmp134; + _res(1, 5) = -_tmp119 * _tmp98 - _tmp122 * _tmp97 - _tmp124 * _tmp96 + _tmp127 * _tmp39 + + _tmp129 * _tmp37 + _tmp131 * _tmp38 + _tmp133 * _tmp36 + _tmp135; + _res(2, 5) = -_tmp100 * _tmp124 - _tmp101 * _tmp122 - _tmp102 * _tmp119 + _tmp127 * _tmp45 + + _tmp129 * _tmp46 + _tmp131 * _tmp44 + _tmp133 * _tmp50 + _tmp136; + _res(3, 5) = -_tmp104 * _tmp124 - _tmp105 * _tmp122 - _tmp106 * _tmp119 + _tmp127 * _tmp52 + + _tmp129 * _tmp57 + _tmp131 * _tmp53 + _tmp133 * _tmp51 + _tmp137; + _res(4, 5) = -_tmp108 * _tmp124 - _tmp109 * _tmp122 - _tmp110 * _tmp119 + _tmp111 * _tmp127 + + _tmp112 * _tmp131 + _tmp113 * _tmp129 + _tmp114 * _tmp133 + + _tmp118 * _tmp74 * d_vel_var(2, 0) + _tmp121 * _tmp139 + _tmp138 * _tmp68 + _tmp140; + _res(5, 5) = std::pow(_tmp118, Scalar(2)) * d_vel_var(2, 0) - _tmp119 * _tmp143 + + std::pow(_tmp121, Scalar(2)) * d_vel_var(1, 0) - _tmp122 * _tmp142 + + std::pow(_tmp123, Scalar(2)) * d_vel_var(0, 0) - _tmp124 * _tmp141 + + _tmp127 * _tmp147 + _tmp129 * _tmp146 + _tmp131 * _tmp145 + _tmp133 * _tmp144 + + _tmp148; + _res(6, 5) = 0; + _res(7, 5) = 0; + _res(8, 5) = 0; + _res(9, 5) = 0; + _res(10, 5) = 0; + _res(11, 5) = 0; + _res(12, 5) = 0; + _res(13, 5) = 0; + _res(14, 5) = 0; + _res(15, 5) = 0; + _res(16, 5) = 0; + _res(17, 5) = 0; + _res(18, 5) = 0; + _res(19, 5) = 0; + _res(20, 5) = 0; + _res(21, 5) = 0; + _res(22, 5) = 0; + _res(23, 5) = 0; + _res(0, 6) = -_tmp150 * _tmp76 - _tmp152 * _tmp70 - _tmp154 * _tmp65 + _tmp156 * _tmp19 + + _tmp157 * _tmp16 + _tmp158 * _tmp23 + _tmp159 * _tmp17 + _tmp161; + _res(1, 6) = -_tmp150 * _tmp97 - _tmp152 * _tmp98 - _tmp154 * _tmp96 + _tmp156 * _tmp39 + + _tmp157 * _tmp36 + _tmp158 * _tmp38 + _tmp159 * _tmp37 + _tmp162; + _res(2, 6) = -_tmp100 * _tmp154 - _tmp101 * _tmp150 - _tmp102 * _tmp152 + _tmp156 * _tmp45 + + _tmp157 * _tmp50 + _tmp158 * _tmp44 + _tmp159 * _tmp46 + _tmp163; + _res(3, 6) = -_tmp104 * _tmp154 - _tmp105 * _tmp150 - _tmp106 * _tmp152 + _tmp156 * _tmp52 + + _tmp157 * _tmp51 + _tmp158 * _tmp53 + _tmp159 * _tmp57 + _tmp164; + _res(4, 6) = -_tmp108 * _tmp154 - _tmp109 * _tmp150 - _tmp110 * _tmp152 + _tmp111 * _tmp156 + + _tmp112 * _tmp158 + _tmp113 * _tmp159 + _tmp114 * _tmp157 + _tmp139 * _tmp149 + + _tmp153 * _tmp68 * d_vel_var(0, 0) + _tmp165 * _tmp74 + _tmp166; + _res(5, 6) = _tmp118 * _tmp165 + _tmp121 * _tmp149 * d_vel_var(1, 0) + _tmp138 * _tmp153 - + _tmp141 * _tmp154 - _tmp142 * _tmp150 - _tmp143 * _tmp152 + _tmp144 * _tmp157 + + _tmp145 * _tmp158 + _tmp146 * _tmp159 + _tmp147 * _tmp156 + _tmp167; + _res(6, 6) = + std::pow(_tmp149, Scalar(2)) * d_vel_var(1, 0) - _tmp150 * _tmp169 + + std::pow(_tmp151, Scalar(2)) * d_vel_var(2, 0) - _tmp152 * _tmp170 + + std::pow(_tmp153, Scalar(2)) * d_vel_var(0, 0) - _tmp154 * _tmp168 + + _tmp156 * (P(0, 1) * _tmp158 + P(1, 1) * _tmp156 - P(13, 1) * _tmp154 - P(14, 1) * _tmp150 - + P(15, 1) * _tmp152 + P(2, 1) * _tmp157 + P(3, 1) * _tmp159 + P(6, 1)) + + _tmp157 * (P(0, 2) * _tmp158 + P(1, 2) * _tmp156 - P(13, 2) * _tmp154 - P(14, 2) * _tmp150 - + P(15, 2) * _tmp152 + P(2, 2) * _tmp157 + P(3, 2) * _tmp159 + P(6, 2)) + + _tmp158 * (P(0, 0) * _tmp158 + P(1, 0) * _tmp156 - P(13, 0) * _tmp154 - P(14, 0) * _tmp150 - + P(15, 0) * _tmp152 + P(2, 0) * _tmp157 + P(3, 0) * _tmp159 + P(6, 0)) + + _tmp159 * (P(0, 3) * _tmp158 + P(1, 3) * _tmp156 - P(13, 3) * _tmp154 - P(14, 3) * _tmp150 - + P(15, 3) * _tmp152 + P(2, 3) * _tmp157 + P(3, 3) * _tmp159 + P(6, 3)) + + _tmp171; + _res(7, 6) = 0; + _res(8, 6) = 0; + _res(9, 6) = 0; + _res(10, 6) = 0; + _res(11, 6) = 0; + _res(12, 6) = 0; + _res(13, 6) = 0; + _res(14, 6) = 0; + _res(15, 6) = 0; + _res(16, 6) = 0; + _res(17, 6) = 0; + _res(18, 6) = 0; + _res(19, 6) = 0; + _res(20, 6) = 0; + _res(21, 6) = 0; + _res(22, 6) = 0; + _res(23, 6) = 0; + _res(0, 7) = P(0, 7) + P(1, 7) * _tmp9 + P(10, 7) * _tmp10 + P(11, 7) * _tmp12 + + P(12, 7) * _tmp11 + P(2, 7) * _tmp3 + P(3, 7) * _tmp6 + _tmp95 * d_vel_dt; + _res(1, 7) = P(0, 7) * _tmp31 + P(1, 7) - P(10, 7) * _tmp29 + P(11, 7) * _tmp11 - + P(12, 7) * _tmp12 + P(2, 7) * _tmp32 + P(3, 7) * _tmp3 + _tmp99 * d_vel_dt; + _res(2, 7) = P(0, 7) * _tmp43 + P(1, 7) * _tmp6 - P(10, 7) * _tmp11 - P(11, 7) * _tmp29 + + P(12, 7) * _tmp10 + P(2, 7) + P(3, 7) * _tmp31 + _tmp103 * d_vel_dt; + _res(3, 7) = P(0, 7) * _tmp32 + P(1, 7) * _tmp43 + P(10, 7) * _tmp12 - P(11, 7) * _tmp10 - + P(12, 7) * _tmp29 + P(2, 7) * _tmp9 + P(3, 7) + _tmp107 * d_vel_dt; + _res(4, 7) = P(0, 7) * _tmp64 + P(1, 7) * _tmp85 - P(13, 7) * _tmp69 - P(14, 7) * _tmp81 - + P(15, 7) * _tmp75 + P(2, 7) * _tmp90 + P(3, 7) * _tmp93 + P(4, 7) + + _tmp115 * d_vel_dt; + _res(5, 7) = + P(0, 7) * _tmp131 + P(1, 7) * _tmp127 - P(13, 7) * _tmp124 - P(14, 7) * _tmp122 - + P(15, 7) * _tmp119 + P(2, 7) * _tmp133 + P(3, 7) * _tmp129 + P(5, 7) + + d_vel_dt * (P(0, 4) * _tmp131 + P(1, 4) * _tmp127 - P(13, 4) * _tmp124 - P(14, 4) * _tmp122 - + P(15, 4) * _tmp119 + P(2, 4) * _tmp133 + P(3, 4) * _tmp129 + P(5, 4)); + _res(6, 7) = + P(0, 7) * _tmp158 + P(1, 7) * _tmp156 - P(13, 7) * _tmp154 - P(14, 7) * _tmp150 - + P(15, 7) * _tmp152 + P(2, 7) * _tmp157 + P(3, 7) * _tmp159 + P(6, 7) + + d_vel_dt * (P(0, 4) * _tmp158 + P(1, 4) * _tmp156 - P(13, 4) * _tmp154 - P(14, 4) * _tmp150 - + P(15, 4) * _tmp152 + P(2, 4) * _tmp157 + P(3, 4) * _tmp159 + P(6, 4)); + _res(7, 7) = P(4, 7) * d_vel_dt + P(7, 7) + d_vel_dt * (P(4, 4) * d_vel_dt + P(7, 4)); + _res(8, 7) = 0; + _res(9, 7) = 0; + _res(10, 7) = 0; + _res(11, 7) = 0; + _res(12, 7) = 0; + _res(13, 7) = 0; + _res(14, 7) = 0; + _res(15, 7) = 0; + _res(16, 7) = 0; + _res(17, 7) = 0; + _res(18, 7) = 0; + _res(19, 7) = 0; + _res(20, 7) = 0; + _res(21, 7) = 0; + _res(22, 7) = 0; + _res(23, 7) = 0; + _res(0, 8) = P(0, 8) + P(1, 8) * _tmp9 + P(10, 8) * _tmp10 + P(12, 8) * _tmp11 + P(2, 8) * _tmp3 + + P(3, 8) * _tmp6 + _tmp134 * d_vel_dt + _tmp172 * state(2, 0); + _res(1, 8) = P(0, 8) * _tmp31 + P(1, 8) - P(10, 8) * _tmp29 - P(12, 8) * _tmp12 + + P(2, 8) * _tmp32 + P(3, 8) * _tmp3 + _tmp135 * d_vel_dt + _tmp172 * state(3, 0); + _res(2, 8) = P(0, 8) * _tmp43 + P(1, 8) * _tmp6 - P(10, 8) * _tmp11 + P(12, 8) * _tmp10 + + P(2, 8) + P(3, 8) * _tmp31 + _tmp136 * d_vel_dt - _tmp172 * state(0, 0); + _res(3, 8) = P(0, 8) * _tmp32 + P(1, 8) * _tmp43 + P(10, 8) * _tmp12 - P(12, 8) * _tmp29 + + P(2, 8) * _tmp9 + P(3, 8) + _tmp137 * d_vel_dt - _tmp172 * state(1, 0); + _res(4, 8) = P(0, 8) * _tmp64 + P(1, 8) * _tmp85 - P(13, 8) * _tmp69 - P(14, 8) * _tmp81 - + P(15, 8) * _tmp75 + P(2, 8) * _tmp90 + P(3, 8) * _tmp93 + P(4, 8) + + _tmp140 * d_vel_dt; + _res(5, 8) = P(0, 8) * _tmp131 + P(1, 8) * _tmp127 - P(13, 8) * _tmp124 - P(14, 8) * _tmp122 - + P(15, 8) * _tmp119 + P(2, 8) * _tmp133 + P(3, 8) * _tmp129 + P(5, 8) + + _tmp148 * d_vel_dt; + _res(6, 8) = + P(0, 8) * _tmp158 + P(1, 8) * _tmp156 - P(13, 8) * _tmp154 - P(14, 8) * _tmp150 - + P(15, 8) * _tmp152 + P(2, 8) * _tmp157 + P(3, 8) * _tmp159 + P(6, 8) + + d_vel_dt * (P(0, 5) * _tmp158 + P(1, 5) * _tmp156 - P(13, 5) * _tmp154 - P(14, 5) * _tmp150 - + P(15, 5) * _tmp152 + P(2, 5) * _tmp157 + P(3, 5) * _tmp159 + P(6, 5)); + _res(7, 8) = P(4, 8) * d_vel_dt + P(7, 8) + d_vel_dt * (P(4, 5) * d_vel_dt + P(7, 5)); + _res(8, 8) = P(5, 8) * d_vel_dt + P(8, 8) + d_vel_dt * (P(5, 5) * d_vel_dt + P(8, 5)); + _res(9, 8) = 0; + _res(10, 8) = 0; + _res(11, 8) = 0; + _res(12, 8) = 0; + _res(13, 8) = 0; + _res(14, 8) = 0; + _res(15, 8) = 0; + _res(16, 8) = 0; + _res(17, 8) = 0; + _res(18, 8) = 0; + _res(19, 8) = 0; + _res(20, 8) = 0; + _res(21, 8) = 0; + _res(22, 8) = 0; + _res(23, 8) = 0; + _res(0, 9) = P(0, 9) + P(1, 9) * _tmp9 + P(10, 9) * _tmp10 + P(11, 9) * _tmp12 + + P(12, 9) * _tmp11 + P(2, 9) * _tmp3 + P(3, 9) * _tmp6 + _tmp161 * d_vel_dt; + _res(1, 9) = P(0, 9) * _tmp31 + P(1, 9) - P(10, 9) * _tmp29 + P(11, 9) * _tmp11 - + P(12, 9) * _tmp12 + P(2, 9) * _tmp32 + P(3, 9) * _tmp3 + _tmp162 * d_vel_dt; + _res(2, 9) = P(0, 9) * _tmp43 + P(1, 9) * _tmp6 - P(10, 9) * _tmp11 - P(11, 9) * _tmp29 + + P(12, 9) * _tmp10 + P(2, 9) + P(3, 9) * _tmp31 + _tmp163 * d_vel_dt; + _res(3, 9) = P(0, 9) * _tmp32 + P(1, 9) * _tmp43 + P(10, 9) * _tmp12 - P(11, 9) * _tmp10 - + P(12, 9) * _tmp29 + P(2, 9) * _tmp9 + P(3, 9) + _tmp164 * d_vel_dt; + _res(4, 9) = P(0, 9) * _tmp64 + P(1, 9) * _tmp85 - P(13, 9) * _tmp69 - P(14, 9) * _tmp81 - + P(15, 9) * _tmp75 + P(2, 9) * _tmp90 + P(3, 9) * _tmp93 + P(4, 9) + + _tmp166 * d_vel_dt; + _res(5, 9) = P(0, 9) * _tmp131 + P(1, 9) * _tmp127 - P(13, 9) * _tmp124 - P(14, 9) * _tmp122 - + P(15, 9) * _tmp119 + P(2, 9) * _tmp133 + P(3, 9) * _tmp129 + P(5, 9) + + _tmp167 * d_vel_dt; + _res(6, 9) = P(0, 9) * _tmp158 + P(1, 9) * _tmp156 - P(13, 9) * _tmp154 - P(14, 9) * _tmp150 - + P(15, 9) * _tmp152 + P(2, 9) * _tmp157 + P(3, 9) * _tmp159 + P(6, 9) + + _tmp171 * d_vel_dt; + _res(7, 9) = P(4, 9) * d_vel_dt + P(7, 9) + d_vel_dt * (P(4, 6) * d_vel_dt + P(7, 6)); + _res(8, 9) = P(5, 9) * d_vel_dt + P(8, 9) + d_vel_dt * (P(5, 6) * d_vel_dt + P(8, 6)); + _res(9, 9) = P(6, 9) * d_vel_dt + P(9, 9) + d_vel_dt * (P(6, 6) * d_vel_dt + P(9, 6)); + _res(10, 9) = 0; + _res(11, 9) = 0; + _res(12, 9) = 0; + _res(13, 9) = 0; + _res(14, 9) = 0; + _res(15, 9) = 0; + _res(16, 9) = 0; + _res(17, 9) = 0; + _res(18, 9) = 0; + _res(19, 9) = 0; + _res(20, 9) = 0; + _res(21, 9) = 0; + _res(22, 9) = 0; + _res(23, 9) = 0; + _res(0, 10) = _tmp13; + _res(1, 10) = _tmp34; + _res(2, 10) = _tmp48; + _res(3, 10) = _tmp56; + _res(4, 10) = P(0, 10) * _tmp64 + P(1, 10) * _tmp85 - P(13, 10) * _tmp69 - P(14, 10) * _tmp81 - + P(15, 10) * _tmp75 + P(2, 10) * _tmp90 + P(3, 10) * _tmp93 + P(4, 10); + _res(5, 10) = P(0, 10) * _tmp131 + P(1, 10) * _tmp127 - P(13, 10) * _tmp124 - + P(14, 10) * _tmp122 - P(15, 10) * _tmp119 + P(2, 10) * _tmp133 + + P(3, 10) * _tmp129 + P(5, 10); + _res(6, 10) = P(0, 10) * _tmp158 + P(1, 10) * _tmp156 - P(13, 10) * _tmp154 - + P(14, 10) * _tmp150 - P(15, 10) * _tmp152 + P(2, 10) * _tmp157 + + P(3, 10) * _tmp159 + P(6, 10); + _res(7, 10) = P(4, 10) * d_vel_dt + P(7, 10); + _res(8, 10) = P(5, 10) * d_vel_dt + P(8, 10); + _res(9, 10) = P(6, 10) * d_vel_dt + P(9, 10); + _res(10, 10) = P(10, 10); + _res(11, 10) = 0; + _res(12, 10) = 0; + _res(13, 10) = 0; + _res(14, 10) = 0; + _res(15, 10) = 0; + _res(16, 10) = 0; + _res(17, 10) = 0; + _res(18, 10) = 0; + _res(19, 10) = 0; + _res(20, 10) = 0; + _res(21, 10) = 0; + _res(22, 10) = 0; + _res(23, 10) = 0; + _res(0, 11) = _tmp14; + _res(1, 11) = _tmp35; + _res(2, 11) = _tmp47; + _res(3, 11) = _tmp54; + _res(4, 11) = P(0, 11) * _tmp64 + P(1, 11) * _tmp85 - P(13, 11) * _tmp69 - P(14, 11) * _tmp81 - + P(15, 11) * _tmp75 + P(2, 11) * _tmp90 + P(3, 11) * _tmp93 + P(4, 11); + _res(5, 11) = P(0, 11) * _tmp131 + P(1, 11) * _tmp127 - P(13, 11) * _tmp124 - + P(14, 11) * _tmp122 - P(15, 11) * _tmp119 + P(2, 11) * _tmp133 + + P(3, 11) * _tmp129 + P(5, 11); + _res(6, 11) = P(0, 11) * _tmp158 + P(1, 11) * _tmp156 - P(13, 11) * _tmp154 - + P(14, 11) * _tmp150 - P(15, 11) * _tmp152 + P(2, 11) * _tmp157 + + P(3, 11) * _tmp159 + P(6, 11); + _res(7, 11) = P(4, 11) * d_vel_dt + P(7, 11); + _res(8, 11) = P(5, 11) * d_vel_dt + P(8, 11); + _res(9, 11) = P(6, 11) * d_vel_dt + P(9, 11); + _res(10, 11) = P(10, 11); + _res(11, 11) = P(11, 11); + _res(12, 11) = 0; + _res(13, 11) = 0; + _res(14, 11) = 0; + _res(15, 11) = 0; + _res(16, 11) = 0; + _res(17, 11) = 0; + _res(18, 11) = 0; + _res(19, 11) = 0; + _res(20, 11) = 0; + _res(21, 11) = 0; + _res(22, 11) = 0; + _res(23, 11) = 0; + _res(0, 12) = _tmp15; + _res(1, 12) = _tmp33; + _res(2, 12) = _tmp49; + _res(3, 12) = _tmp55; + _res(4, 12) = P(0, 12) * _tmp64 + P(1, 12) * _tmp85 - P(13, 12) * _tmp69 - P(14, 12) * _tmp81 - + P(15, 12) * _tmp75 + P(2, 12) * _tmp90 + P(3, 12) * _tmp93 + P(4, 12); + _res(5, 12) = P(0, 12) * _tmp131 + P(1, 12) * _tmp127 - P(13, 12) * _tmp124 - + P(14, 12) * _tmp122 - P(15, 12) * _tmp119 + P(2, 12) * _tmp133 + + P(3, 12) * _tmp129 + P(5, 12); + _res(6, 12) = P(0, 12) * _tmp158 + P(1, 12) * _tmp156 - P(13, 12) * _tmp154 - + P(14, 12) * _tmp150 - P(15, 12) * _tmp152 + P(2, 12) * _tmp157 + + P(3, 12) * _tmp159 + P(6, 12); + _res(7, 12) = P(4, 12) * d_vel_dt + P(7, 12); + _res(8, 12) = P(5, 12) * d_vel_dt + P(8, 12); + _res(9, 12) = P(6, 12) * d_vel_dt + P(9, 12); + _res(10, 12) = P(10, 12); + _res(11, 12) = P(11, 12); + _res(12, 12) = P(12, 12); + _res(13, 12) = 0; + _res(14, 12) = 0; + _res(15, 12) = 0; + _res(16, 12) = 0; + _res(17, 12) = 0; + _res(18, 12) = 0; + _res(19, 12) = 0; + _res(20, 12) = 0; + _res(21, 12) = 0; + _res(22, 12) = 0; + _res(23, 12) = 0; + _res(0, 13) = _tmp65; + _res(1, 13) = _tmp96; + _res(2, 13) = _tmp100; + _res(3, 13) = _tmp104; + _res(4, 13) = _tmp108; + _res(5, 13) = _tmp141; + _res(6, 13) = _tmp168; + _res(7, 13) = P(4, 13) * d_vel_dt + P(7, 13); + _res(8, 13) = P(5, 13) * d_vel_dt + P(8, 13); + _res(9, 13) = P(6, 13) * d_vel_dt + P(9, 13); + _res(10, 13) = P(10, 13); + _res(11, 13) = P(11, 13); + _res(12, 13) = P(12, 13); + _res(13, 13) = P(13, 13); + _res(14, 13) = 0; + _res(15, 13) = 0; + _res(16, 13) = 0; + _res(17, 13) = 0; + _res(18, 13) = 0; + _res(19, 13) = 0; + _res(20, 13) = 0; + _res(21, 13) = 0; + _res(22, 13) = 0; + _res(23, 13) = 0; + _res(0, 14) = _tmp76; + _res(1, 14) = _tmp97; + _res(2, 14) = _tmp101; + _res(3, 14) = _tmp105; + _res(4, 14) = _tmp109; + _res(5, 14) = _tmp142; + _res(6, 14) = _tmp169; + _res(7, 14) = P(4, 14) * d_vel_dt + P(7, 14); + _res(8, 14) = P(5, 14) * d_vel_dt + P(8, 14); + _res(9, 14) = P(6, 14) * d_vel_dt + P(9, 14); + _res(10, 14) = P(10, 14); + _res(11, 14) = P(11, 14); + _res(12, 14) = P(12, 14); + _res(13, 14) = P(13, 14); + _res(14, 14) = P(14, 14); + _res(15, 14) = 0; + _res(16, 14) = 0; + _res(17, 14) = 0; + _res(18, 14) = 0; + _res(19, 14) = 0; + _res(20, 14) = 0; + _res(21, 14) = 0; + _res(22, 14) = 0; + _res(23, 14) = 0; + _res(0, 15) = _tmp70; + _res(1, 15) = _tmp98; + _res(2, 15) = _tmp102; + _res(3, 15) = _tmp106; + _res(4, 15) = _tmp110; + _res(5, 15) = _tmp143; + _res(6, 15) = _tmp170; + _res(7, 15) = P(4, 15) * d_vel_dt + P(7, 15); + _res(8, 15) = P(5, 15) * d_vel_dt + P(8, 15); + _res(9, 15) = P(6, 15) * d_vel_dt + P(9, 15); + _res(10, 15) = P(10, 15); + _res(11, 15) = P(11, 15); + _res(12, 15) = P(12, 15); + _res(13, 15) = P(13, 15); + _res(14, 15) = P(14, 15); + _res(15, 15) = P(15, 15); + _res(16, 15) = 0; + _res(17, 15) = 0; + _res(18, 15) = 0; + _res(19, 15) = 0; + _res(20, 15) = 0; + _res(21, 15) = 0; + _res(22, 15) = 0; + _res(23, 15) = 0; + _res(0, 16) = P(0, 16) + P(1, 16) * _tmp9 + P(10, 16) * _tmp10 + P(11, 16) * _tmp12 + + P(12, 16) * _tmp11 + P(2, 16) * _tmp3 + P(3, 16) * _tmp6; + _res(1, 16) = P(0, 16) * _tmp31 + P(1, 16) - P(10, 16) * _tmp29 + P(11, 16) * _tmp11 - + P(12, 16) * _tmp12 + P(2, 16) * _tmp32 + P(3, 16) * _tmp3; + _res(2, 16) = P(0, 16) * _tmp43 + P(1, 16) * _tmp6 - P(10, 16) * _tmp11 - P(11, 16) * _tmp29 + + P(12, 16) * _tmp10 + P(2, 16) + P(3, 16) * _tmp31; + _res(3, 16) = P(0, 16) * _tmp32 + P(1, 16) * _tmp43 + P(10, 16) * _tmp12 - P(11, 16) * _tmp10 - + P(12, 16) * _tmp29 + P(2, 16) * _tmp9 + P(3, 16); + _res(4, 16) = P(0, 16) * _tmp64 + P(1, 16) * _tmp85 - P(13, 16) * _tmp69 - P(14, 16) * _tmp81 - + P(15, 16) * _tmp75 + P(2, 16) * _tmp90 + P(3, 16) * _tmp93 + P(4, 16); + _res(5, 16) = P(0, 16) * _tmp131 + P(1, 16) * _tmp127 - P(13, 16) * _tmp124 - + P(14, 16) * _tmp122 - P(15, 16) * _tmp119 + P(2, 16) * _tmp133 + + P(3, 16) * _tmp129 + P(5, 16); + _res(6, 16) = P(0, 16) * _tmp158 + P(1, 16) * _tmp156 - P(13, 16) * _tmp154 - + P(14, 16) * _tmp150 - P(15, 16) * _tmp152 + P(2, 16) * _tmp157 + + P(3, 16) * _tmp159 + P(6, 16); + _res(7, 16) = P(4, 16) * d_vel_dt + P(7, 16); + _res(8, 16) = P(5, 16) * d_vel_dt + P(8, 16); + _res(9, 16) = P(6, 16) * d_vel_dt + P(9, 16); + _res(10, 16) = P(10, 16); + _res(11, 16) = P(11, 16); + _res(12, 16) = P(12, 16); + _res(13, 16) = P(13, 16); + _res(14, 16) = P(14, 16); + _res(15, 16) = P(15, 16); + _res(16, 16) = P(16, 16); + _res(17, 16) = 0; + _res(18, 16) = 0; + _res(19, 16) = 0; + _res(20, 16) = 0; + _res(21, 16) = 0; + _res(22, 16) = 0; + _res(23, 16) = 0; + _res(0, 17) = P(0, 17) + P(1, 17) * _tmp9 + P(10, 17) * _tmp10 + P(11, 17) * _tmp12 + + P(12, 17) * _tmp11 + P(2, 17) * _tmp3 + P(3, 17) * _tmp6; + _res(1, 17) = P(0, 17) * _tmp31 + P(1, 17) - P(10, 17) * _tmp29 + P(11, 17) * _tmp11 - + P(12, 17) * _tmp12 + P(2, 17) * _tmp32 + P(3, 17) * _tmp3; + _res(2, 17) = P(0, 17) * _tmp43 + P(1, 17) * _tmp6 - P(10, 17) * _tmp11 - P(11, 17) * _tmp29 + + P(12, 17) * _tmp10 + P(2, 17) + P(3, 17) * _tmp31; + _res(3, 17) = P(0, 17) * _tmp32 + P(1, 17) * _tmp43 + P(10, 17) * _tmp12 - P(11, 17) * _tmp10 - + P(12, 17) * _tmp29 + P(2, 17) * _tmp9 + P(3, 17); + _res(4, 17) = P(0, 17) * _tmp64 + P(1, 17) * _tmp85 - P(13, 17) * _tmp69 - P(14, 17) * _tmp81 - + P(15, 17) * _tmp75 + P(2, 17) * _tmp90 + P(3, 17) * _tmp93 + P(4, 17); + _res(5, 17) = P(0, 17) * _tmp131 + P(1, 17) * _tmp127 - P(13, 17) * _tmp124 - + P(14, 17) * _tmp122 - P(15, 17) * _tmp119 + P(2, 17) * _tmp133 + + P(3, 17) * _tmp129 + P(5, 17); + _res(6, 17) = P(0, 17) * _tmp158 + P(1, 17) * _tmp156 - P(13, 17) * _tmp154 - + P(14, 17) * _tmp150 - P(15, 17) * _tmp152 + P(2, 17) * _tmp157 + + P(3, 17) * _tmp159 + P(6, 17); + _res(7, 17) = P(4, 17) * d_vel_dt + P(7, 17); + _res(8, 17) = P(5, 17) * d_vel_dt + P(8, 17); + _res(9, 17) = P(6, 17) * d_vel_dt + P(9, 17); + _res(10, 17) = P(10, 17); + _res(11, 17) = P(11, 17); + _res(12, 17) = P(12, 17); + _res(13, 17) = P(13, 17); + _res(14, 17) = P(14, 17); + _res(15, 17) = P(15, 17); + _res(16, 17) = P(16, 17); + _res(17, 17) = P(17, 17); + _res(18, 17) = 0; + _res(19, 17) = 0; + _res(20, 17) = 0; + _res(21, 17) = 0; + _res(22, 17) = 0; + _res(23, 17) = 0; + _res(0, 18) = P(0, 18) + P(1, 18) * _tmp9 + P(10, 18) * _tmp10 + P(11, 18) * _tmp12 + + P(12, 18) * _tmp11 + P(2, 18) * _tmp3 + P(3, 18) * _tmp6; + _res(1, 18) = P(0, 18) * _tmp31 + P(1, 18) - P(10, 18) * _tmp29 + P(11, 18) * _tmp11 - + P(12, 18) * _tmp12 + P(2, 18) * _tmp32 + P(3, 18) * _tmp3; + _res(2, 18) = P(0, 18) * _tmp43 + P(1, 18) * _tmp6 - P(10, 18) * _tmp11 - P(11, 18) * _tmp29 + + P(12, 18) * _tmp10 + P(2, 18) + P(3, 18) * _tmp31; + _res(3, 18) = P(0, 18) * _tmp32 + P(1, 18) * _tmp43 + P(10, 18) * _tmp12 - P(11, 18) * _tmp10 - + P(12, 18) * _tmp29 + P(2, 18) * _tmp9 + P(3, 18); + _res(4, 18) = P(0, 18) * _tmp64 + P(1, 18) * _tmp85 - P(13, 18) * _tmp69 - P(14, 18) * _tmp81 - + P(15, 18) * _tmp75 + P(2, 18) * _tmp90 + P(3, 18) * _tmp93 + P(4, 18); + _res(5, 18) = P(0, 18) * _tmp131 + P(1, 18) * _tmp127 - P(13, 18) * _tmp124 - + P(14, 18) * _tmp122 - P(15, 18) * _tmp119 + P(2, 18) * _tmp133 + + P(3, 18) * _tmp129 + P(5, 18); + _res(6, 18) = P(0, 18) * _tmp158 + P(1, 18) * _tmp156 - P(13, 18) * _tmp154 - + P(14, 18) * _tmp150 - P(15, 18) * _tmp152 + P(2, 18) * _tmp157 + + P(3, 18) * _tmp159 + P(6, 18); + _res(7, 18) = P(4, 18) * d_vel_dt + P(7, 18); + _res(8, 18) = P(5, 18) * d_vel_dt + P(8, 18); + _res(9, 18) = P(6, 18) * d_vel_dt + P(9, 18); + _res(10, 18) = P(10, 18); + _res(11, 18) = P(11, 18); + _res(12, 18) = P(12, 18); + _res(13, 18) = P(13, 18); + _res(14, 18) = P(14, 18); + _res(15, 18) = P(15, 18); + _res(16, 18) = P(16, 18); + _res(17, 18) = P(17, 18); + _res(18, 18) = P(18, 18); + _res(19, 18) = 0; + _res(20, 18) = 0; + _res(21, 18) = 0; + _res(22, 18) = 0; + _res(23, 18) = 0; + _res(0, 19) = P(0, 19) + P(1, 19) * _tmp9 + P(10, 19) * _tmp10 + P(11, 19) * _tmp12 + + P(12, 19) * _tmp11 + P(2, 19) * _tmp3 + P(3, 19) * _tmp6; + _res(1, 19) = P(0, 19) * _tmp31 + P(1, 19) - P(10, 19) * _tmp29 + P(11, 19) * _tmp11 - + P(12, 19) * _tmp12 + P(2, 19) * _tmp32 + P(3, 19) * _tmp3; + _res(2, 19) = P(0, 19) * _tmp43 + P(1, 19) * _tmp6 - P(10, 19) * _tmp11 - P(11, 19) * _tmp29 + + P(12, 19) * _tmp10 + P(2, 19) + P(3, 19) * _tmp31; + _res(3, 19) = P(0, 19) * _tmp32 + P(1, 19) * _tmp43 + P(10, 19) * _tmp12 - P(11, 19) * _tmp10 - + P(12, 19) * _tmp29 + P(2, 19) * _tmp9 + P(3, 19); + _res(4, 19) = P(0, 19) * _tmp64 + P(1, 19) * _tmp85 - P(13, 19) * _tmp69 - P(14, 19) * _tmp81 - + P(15, 19) * _tmp75 + P(2, 19) * _tmp90 + P(3, 19) * _tmp93 + P(4, 19); + _res(5, 19) = P(0, 19) * _tmp131 + P(1, 19) * _tmp127 - P(13, 19) * _tmp124 - + P(14, 19) * _tmp122 - P(15, 19) * _tmp119 + P(2, 19) * _tmp133 + + P(3, 19) * _tmp129 + P(5, 19); + _res(6, 19) = P(0, 19) * _tmp158 + P(1, 19) * _tmp156 - P(13, 19) * _tmp154 - + P(14, 19) * _tmp150 - P(15, 19) * _tmp152 + P(2, 19) * _tmp157 + + P(3, 19) * _tmp159 + P(6, 19); + _res(7, 19) = P(4, 19) * d_vel_dt + P(7, 19); + _res(8, 19) = P(5, 19) * d_vel_dt + P(8, 19); + _res(9, 19) = P(6, 19) * d_vel_dt + P(9, 19); + _res(10, 19) = P(10, 19); + _res(11, 19) = P(11, 19); + _res(12, 19) = P(12, 19); + _res(13, 19) = P(13, 19); + _res(14, 19) = P(14, 19); + _res(15, 19) = P(15, 19); + _res(16, 19) = P(16, 19); + _res(17, 19) = P(17, 19); + _res(18, 19) = P(18, 19); + _res(19, 19) = P(19, 19); + _res(20, 19) = 0; + _res(21, 19) = 0; + _res(22, 19) = 0; + _res(23, 19) = 0; + _res(0, 20) = P(0, 20) + P(1, 20) * _tmp9 + P(10, 20) * _tmp10 + P(11, 20) * _tmp12 + + P(12, 20) * _tmp11 + P(2, 20) * _tmp3 + P(3, 20) * _tmp6; + _res(1, 20) = P(0, 20) * _tmp31 + P(1, 20) - P(10, 20) * _tmp29 + P(11, 20) * _tmp11 - + P(12, 20) * _tmp12 + P(2, 20) * _tmp32 + P(3, 20) * _tmp3; + _res(2, 20) = P(0, 20) * _tmp43 + P(1, 20) * _tmp6 - P(10, 20) * _tmp11 - P(11, 20) * _tmp29 + + P(12, 20) * _tmp10 + P(2, 20) + P(3, 20) * _tmp31; + _res(3, 20) = P(0, 20) * _tmp32 + P(1, 20) * _tmp43 + P(10, 20) * _tmp12 - P(11, 20) * _tmp10 - + P(12, 20) * _tmp29 + P(2, 20) * _tmp9 + P(3, 20); + _res(4, 20) = P(0, 20) * _tmp64 + P(1, 20) * _tmp85 - P(13, 20) * _tmp69 - P(14, 20) * _tmp81 - + P(15, 20) * _tmp75 + P(2, 20) * _tmp90 + P(3, 20) * _tmp93 + P(4, 20); + _res(5, 20) = P(0, 20) * _tmp131 + P(1, 20) * _tmp127 - P(13, 20) * _tmp124 - + P(14, 20) * _tmp122 - P(15, 20) * _tmp119 + P(2, 20) * _tmp133 + + P(3, 20) * _tmp129 + P(5, 20); + _res(6, 20) = P(0, 20) * _tmp158 + P(1, 20) * _tmp156 - P(13, 20) * _tmp154 - + P(14, 20) * _tmp150 - P(15, 20) * _tmp152 + P(2, 20) * _tmp157 + + P(3, 20) * _tmp159 + P(6, 20); + _res(7, 20) = P(4, 20) * d_vel_dt + P(7, 20); + _res(8, 20) = P(5, 20) * d_vel_dt + P(8, 20); + _res(9, 20) = P(6, 20) * d_vel_dt + P(9, 20); + _res(10, 20) = P(10, 20); + _res(11, 20) = P(11, 20); + _res(12, 20) = P(12, 20); + _res(13, 20) = P(13, 20); + _res(14, 20) = P(14, 20); + _res(15, 20) = P(15, 20); + _res(16, 20) = P(16, 20); + _res(17, 20) = P(17, 20); + _res(18, 20) = P(18, 20); + _res(19, 20) = P(19, 20); + _res(20, 20) = P(20, 20); + _res(21, 20) = 0; + _res(22, 20) = 0; + _res(23, 20) = 0; + _res(0, 21) = P(0, 21) + P(1, 21) * _tmp9 + P(10, 21) * _tmp10 + P(11, 21) * _tmp12 + + P(12, 21) * _tmp11 + P(2, 21) * _tmp3 + P(3, 21) * _tmp6; + _res(1, 21) = P(0, 21) * _tmp31 + P(1, 21) - P(10, 21) * _tmp29 + P(11, 21) * _tmp11 - + P(12, 21) * _tmp12 + P(2, 21) * _tmp32 + P(3, 21) * _tmp3; + _res(2, 21) = P(0, 21) * _tmp43 + P(1, 21) * _tmp6 - P(10, 21) * _tmp11 - P(11, 21) * _tmp29 + + P(12, 21) * _tmp10 + P(2, 21) + P(3, 21) * _tmp31; + _res(3, 21) = P(0, 21) * _tmp32 + P(1, 21) * _tmp43 + P(10, 21) * _tmp12 - P(11, 21) * _tmp10 - + P(12, 21) * _tmp29 + P(2, 21) * _tmp9 + P(3, 21); + _res(4, 21) = P(0, 21) * _tmp64 + P(1, 21) * _tmp85 - P(13, 21) * _tmp69 - P(14, 21) * _tmp81 - + P(15, 21) * _tmp75 + P(2, 21) * _tmp90 + P(3, 21) * _tmp93 + P(4, 21); + _res(5, 21) = P(0, 21) * _tmp131 + P(1, 21) * _tmp127 - P(13, 21) * _tmp124 - + P(14, 21) * _tmp122 - P(15, 21) * _tmp119 + P(2, 21) * _tmp133 + + P(3, 21) * _tmp129 + P(5, 21); + _res(6, 21) = P(0, 21) * _tmp158 + P(1, 21) * _tmp156 - P(13, 21) * _tmp154 - + P(14, 21) * _tmp150 - P(15, 21) * _tmp152 + P(2, 21) * _tmp157 + + P(3, 21) * _tmp159 + P(6, 21); + _res(7, 21) = P(4, 21) * d_vel_dt + P(7, 21); + _res(8, 21) = P(5, 21) * d_vel_dt + P(8, 21); + _res(9, 21) = P(6, 21) * d_vel_dt + P(9, 21); + _res(10, 21) = P(10, 21); + _res(11, 21) = P(11, 21); + _res(12, 21) = P(12, 21); + _res(13, 21) = P(13, 21); + _res(14, 21) = P(14, 21); + _res(15, 21) = P(15, 21); + _res(16, 21) = P(16, 21); + _res(17, 21) = P(17, 21); + _res(18, 21) = P(18, 21); + _res(19, 21) = P(19, 21); + _res(20, 21) = P(20, 21); + _res(21, 21) = P(21, 21); + _res(22, 21) = 0; + _res(23, 21) = 0; + _res(0, 22) = P(0, 22) + P(1, 22) * _tmp9 + P(10, 22) * _tmp10 + P(11, 22) * _tmp12 + + P(12, 22) * _tmp11 + P(2, 22) * _tmp3 + P(3, 22) * _tmp6; + _res(1, 22) = P(0, 22) * _tmp31 + P(1, 22) - P(10, 22) * _tmp29 + P(11, 22) * _tmp11 - + P(12, 22) * _tmp12 + P(2, 22) * _tmp32 + P(3, 22) * _tmp3; + _res(2, 22) = P(0, 22) * _tmp43 + P(1, 22) * _tmp6 - P(10, 22) * _tmp11 - P(11, 22) * _tmp29 + + P(12, 22) * _tmp10 + P(2, 22) + P(3, 22) * _tmp31; + _res(3, 22) = P(0, 22) * _tmp32 + P(1, 22) * _tmp43 + P(10, 22) * _tmp12 - P(11, 22) * _tmp10 - + P(12, 22) * _tmp29 + P(2, 22) * _tmp9 + P(3, 22); + _res(4, 22) = P(0, 22) * _tmp64 + P(1, 22) * _tmp85 - P(13, 22) * _tmp69 - P(14, 22) * _tmp81 - + P(15, 22) * _tmp75 + P(2, 22) * _tmp90 + P(3, 22) * _tmp93 + P(4, 22); + _res(5, 22) = P(0, 22) * _tmp131 + P(1, 22) * _tmp127 - P(13, 22) * _tmp124 - + P(14, 22) * _tmp122 - P(15, 22) * _tmp119 + P(2, 22) * _tmp133 + + P(3, 22) * _tmp129 + P(5, 22); + _res(6, 22) = P(0, 22) * _tmp158 + P(1, 22) * _tmp156 - P(13, 22) * _tmp154 - + P(14, 22) * _tmp150 - P(15, 22) * _tmp152 + P(2, 22) * _tmp157 + + P(3, 22) * _tmp159 + P(6, 22); + _res(7, 22) = P(4, 22) * d_vel_dt + P(7, 22); + _res(8, 22) = P(5, 22) * d_vel_dt + P(8, 22); + _res(9, 22) = P(6, 22) * d_vel_dt + P(9, 22); + _res(10, 22) = P(10, 22); + _res(11, 22) = P(11, 22); + _res(12, 22) = P(12, 22); + _res(13, 22) = P(13, 22); + _res(14, 22) = P(14, 22); + _res(15, 22) = P(15, 22); + _res(16, 22) = P(16, 22); + _res(17, 22) = P(17, 22); + _res(18, 22) = P(18, 22); + _res(19, 22) = P(19, 22); + _res(20, 22) = P(20, 22); + _res(21, 22) = P(21, 22); + _res(22, 22) = P(22, 22); + _res(23, 22) = 0; + _res(0, 23) = P(0, 23) + P(1, 23) * _tmp9 + P(10, 23) * _tmp10 + P(11, 23) * _tmp12 + + P(12, 23) * _tmp11 + P(2, 23) * _tmp3 + P(3, 23) * _tmp6; + _res(1, 23) = P(0, 23) * _tmp31 + P(1, 23) - P(10, 23) * _tmp29 + P(11, 23) * _tmp11 - + P(12, 23) * _tmp12 + P(2, 23) * _tmp32 + P(3, 23) * _tmp3; + _res(2, 23) = P(0, 23) * _tmp43 + P(1, 23) * _tmp6 - P(10, 23) * _tmp11 - P(11, 23) * _tmp29 + + P(12, 23) * _tmp10 + P(2, 23) + P(3, 23) * _tmp31; + _res(3, 23) = P(0, 23) * _tmp32 + P(1, 23) * _tmp43 + P(10, 23) * _tmp12 - P(11, 23) * _tmp10 - + P(12, 23) * _tmp29 + P(2, 23) * _tmp9 + P(3, 23); + _res(4, 23) = P(0, 23) * _tmp64 + P(1, 23) * _tmp85 - P(13, 23) * _tmp69 - P(14, 23) * _tmp81 - + P(15, 23) * _tmp75 + P(2, 23) * _tmp90 + P(3, 23) * _tmp93 + P(4, 23); + _res(5, 23) = P(0, 23) * _tmp131 + P(1, 23) * _tmp127 - P(13, 23) * _tmp124 - + P(14, 23) * _tmp122 - P(15, 23) * _tmp119 + P(2, 23) * _tmp133 + + P(3, 23) * _tmp129 + P(5, 23); + _res(6, 23) = P(0, 23) * _tmp158 + P(1, 23) * _tmp156 - P(13, 23) * _tmp154 - + P(14, 23) * _tmp150 - P(15, 23) * _tmp152 + P(2, 23) * _tmp157 + + P(3, 23) * _tmp159 + P(6, 23); + _res(7, 23) = P(4, 23) * d_vel_dt + P(7, 23); + _res(8, 23) = P(5, 23) * d_vel_dt + P(8, 23); + _res(9, 23) = P(6, 23) * d_vel_dt + P(9, 23); + _res(10, 23) = P(10, 23); + _res(11, 23) = P(11, 23); + _res(12, 23) = P(12, 23); + _res(13, 23) = P(13, 23); + _res(14, 23) = P(14, 23); + _res(15, 23) = P(15, 23); + _res(16, 23) = P(16, 23); + _res(17, 23) = P(17, 23); + _res(18, 23) = P(18, 23); + _res(19, 23) = P(19, 23); + _res(20, 23) = P(20, 23); + _res(21, 23) = P(21, 23); + _res(22, 23) = P(22, 23); + _res(23, 23) = P(23, 23); + + return _res; } // NOLINT(readability/fn_size) // NOLINTNEXTLINE(readability/fn_size)