diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 6983754c5f..aec4082cc1 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -108,27 +108,28 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // delta angle noise variance float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f); - const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise); + const float gyro_var = sq(gyro_noise); // delta velocity noise variance float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f); - Vector3f d_vel_var; + Vector3f accel_var; for (unsigned i = 0; i < 3; i++) { if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) { // Increase accelerometer process noise if bad accel data is detected - d_vel_var(i) = sq(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE); + accel_var(i) = sq(BADACC_BIAS_PNOISE); } else { - d_vel_var(i) = sq(imu_delayed.delta_vel_dt * accel_noise); + accel_var(i) = sq(accel_noise); } } // predict the covariance // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states 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); + imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var, + imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var, + 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt)); // 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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 67e74569bc..641e7f290d 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -31,6 +31,10 @@ File: derivation.py Description: + Derivation of an error-state EKF based on + Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017). + The derivation is directly done in discrete-time as this allows us to define the desired type of discretization + for each element while defining the equations (easier than a continuous-time derivation followed by a block-wise discretization). """ import argparse @@ -42,6 +46,8 @@ import symforce.symbolic as sf from symforce import typing as T from symforce import ops from symforce.values import Values + +import sympy as sp from derivation_utils import * # Initialize parser @@ -108,12 +114,11 @@ def vstate_to_state(v: VState): def predict_covariance( state: VState, P: MTangent, - d_vel: sf.V3, - d_vel_dt: sf.Scalar, - d_vel_var: sf.V3, - d_ang: sf.V3, - d_ang_dt: sf.Scalar, - d_ang_var: sf.Scalar + accel: sf.V3, + accel_var: sf.V3, + gyro: sf.V3, + gyro_var: sf.Scalar, + dt: sf.Scalar ) -> MTangent: state = vstate_to_state(state) @@ -143,37 +148,37 @@ def predict_covariance( for key in state.keys(): if key == "quat_nominal": # Create true quaternion using small angle approximation of the error rotation - state_t["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(0.5 * state_error["theta"]), w=1)) + state_t["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(state_error["theta"] / 2), w=1)) else: state_t[key] = state[key] + state_error[key] noise = Values( - d_vel = sf.V3.symbolic("a_n"), - d_ang = sf.V3.symbolic("w_n"), + accel = sf.V3.symbolic("a_n"), + gyro = sf.V3.symbolic("w_n"), ) input_t = Values( - d_vel = d_vel - state_t["accel_bias"] * d_vel_dt - noise["d_vel"], - d_ang = d_ang - state_t["gyro_bias"] * d_ang_dt - noise["d_ang"] + accel = accel - state_t["accel_bias"] - noise["accel"], + gyro = gyro - state_t["gyro_bias"] - noise["gyro"] ) R_t = state_t["quat_nominal"] state_t_pred = state_t.copy() - state_t_pred["quat_nominal"] = state_t["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(0.5 * input_t["d_ang"]), w=1)) - state_t_pred["vel"] = state_t["vel"] + R_t * input_t["d_vel"] + sf.V3(0, 0, g) * d_vel_dt - state_t_pred["pos"] = state_t["pos"] + state_t["vel"] * d_vel_dt + state_t_pred["quat_nominal"] = state_t["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(input_t["gyro"] * dt / 2), w=1)) + state_t_pred["vel"] = state_t["vel"] + (R_t * input_t["accel"] + sf.V3(0, 0, g)) * dt + state_t_pred["pos"] = state_t["pos"] + state_t["vel"] * dt # Nominal state kinematics input = Values( - d_vel = d_vel - state["accel_bias"] * d_vel_dt, - d_ang = d_ang - state["gyro_bias"] * d_ang_dt + accel = accel - state["accel_bias"], + gyro = gyro - state["gyro_bias"] ) R = state["quat_nominal"] state_pred = state.copy() - state_pred["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(0.5 * input["d_ang"]), w=1)) - state_pred["vel"] = state["vel"] + R * input["d_vel"] + sf.V3(0, 0, g) * d_vel_dt - state_pred["pos"] = state["pos"] + state["vel"] * d_vel_dt + state_pred["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(input["gyro"] * dt / 2), w=1)) + state_pred["vel"] = state["vel"] + (R * input["accel"] + sf.V3(0, 0, g)) * dt + state_pred["pos"] = state["pos"] + state["vel"] * dt # Error state kinematics state_error_pred = Values() @@ -184,6 +189,12 @@ def predict_covariance( else: state_error_pred[key] = state_t_pred[key] - state_pred[key] + # Simplify angular error state prediction + for i in range(state_error_pred["theta"].storage_dim()): + state_error_pred["theta"][i] = sp.expand(state_error_pred["theta"][i]).subs(dt**2, 0) # do not consider dt**2 effects in the derivation + q_est = sf.Quaternion.from_storage(state["quat_nominal"].to_storage()) + state_error_pred["theta"][i] = sp.factor(state_error_pred["theta"][i]).subs(q_est.w**2 + q_est.x**2 + q_est.y**2 + q_est.z**2, 1) # unit norm quaternion + zero_state_error = {state_error[key]: state_error[key].zero() for key in state_error.keys()} zero_noise = {noise[key]: noise[key].zero() for key in noise.keys()} @@ -192,7 +203,7 @@ def predict_covariance( G = VTangent(state_error_pred.to_storage()).jacobian(noise).subs(zero_state_error).subs(zero_noise) # Covariance propagation - var_u = sf.Matrix.diag([d_vel_var[0], d_vel_var[1], d_vel_var[2], d_ang_var, d_ang_var, d_ang_var]) + var_u = sf.Matrix.diag([accel_var[0], accel_var[1], accel_var[2], gyro_var, gyro_var, gyro_var]) P_new = A * P * A.T + G * var_u * G.T # Generate the equations for the upper triangular matrix and the diagonal only 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 d5ab87e38e..338ee277c0 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 @@ -18,12 +18,11 @@ namespace sym { * Args: * state: Matrix24_1 * P: Matrix23_23 - * d_vel: Matrix31 - * d_vel_dt: Scalar - * d_vel_var: Matrix31 - * d_ang: Matrix31 - * d_ang_dt: Scalar - * d_ang_var: Scalar + * accel: Matrix31 + * accel_var: Matrix31 + * gyro: Matrix31 + * gyro_var: Scalar + * dt: Scalar * * Outputs: * res: Matrix23_23 @@ -31,330 +30,176 @@ namespace sym { template 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: 2445 + const matrix::Matrix& accel, + const matrix::Matrix& accel_var, + const matrix::Matrix& gyro, + const Scalar gyro_var, const Scalar dt) { + // Total ops: 1793 // Input arrays - // Intermediate terms (247) - const Scalar _tmp0 = d_ang(1, 0) - d_ang_dt * state(11, 0); - const Scalar _tmp1 = Scalar(0.5) * state(2, 0); - const Scalar _tmp2 = d_ang(0, 0) - d_ang_dt * state(10, 0); - const Scalar _tmp3 = Scalar(0.5) * state(1, 0); - const Scalar _tmp4 = d_ang(2, 0) - d_ang_dt * state(12, 0); - const Scalar _tmp5 = Scalar(0.5) * state(3, 0); - const Scalar _tmp6 = -_tmp0 * _tmp1 - _tmp2 * _tmp3 - _tmp4 * _tmp5 + state(0, 0); - const Scalar _tmp7 = Scalar(1.0) * _tmp6; - const Scalar _tmp8 = _tmp7 * state(0, 0); - const Scalar _tmp9 = Scalar(0.5) * state(0, 0); - const Scalar _tmp10 = _tmp0 * _tmp3 - _tmp1 * _tmp2 + _tmp4 * _tmp9 + state(3, 0); - const Scalar _tmp11 = Scalar(1.0) * _tmp10; - const Scalar _tmp12 = _tmp11 * state(3, 0); - const Scalar _tmp13 = _tmp0 * _tmp9 + _tmp2 * _tmp5 - _tmp3 * _tmp4 + state(2, 0); - const Scalar _tmp14 = Scalar(1.0) * state(2, 0); - const Scalar _tmp15 = _tmp13 * _tmp14; - const Scalar _tmp16 = -_tmp0 * _tmp5 + _tmp1 * _tmp4 + _tmp2 * _tmp9 + state(1, 0); - const Scalar _tmp17 = Scalar(1.0) * _tmp16; - const Scalar _tmp18 = _tmp17 * state(1, 0); - const Scalar _tmp19 = - -_tmp12 * d_ang_dt - _tmp15 * d_ang_dt - _tmp18 * d_ang_dt - _tmp8 * d_ang_dt; - const Scalar _tmp20 = -_tmp5; - const Scalar _tmp21 = Scalar(0.25) * _tmp4; - const Scalar _tmp22 = _tmp21 * state(0, 0); - const Scalar _tmp23 = Scalar(0.25) * _tmp0; - const Scalar _tmp24 = _tmp23 * state(1, 0); - const Scalar _tmp25 = Scalar(0.25) * _tmp2; - const Scalar _tmp26 = -_tmp25 * state(2, 0); - const Scalar _tmp27 = -_tmp24 + _tmp26; - const Scalar _tmp28 = _tmp20 + _tmp22 + _tmp27; - const Scalar _tmp29 = 2 * _tmp6; - const Scalar _tmp30 = _tmp25 * state(0, 0); + // Intermediate terms (134) + const Scalar _tmp0 = dt * gyro(1, 0); + const Scalar _tmp1 = dt * state(11, 0); + const Scalar _tmp2 = -_tmp0 + _tmp1; + const Scalar _tmp3 = dt * gyro(2, 0); + const Scalar _tmp4 = dt * state(12, 0); + const Scalar _tmp5 = _tmp3 - _tmp4; + const Scalar _tmp6 = P(0, 2) + P(1, 2) * _tmp5 + P(2, 2) * _tmp2 - P(9, 2) * dt; + const Scalar _tmp7 = P(0, 1) + P(1, 1) * _tmp5 + P(2, 1) * _tmp2 - P(9, 1) * dt; + const Scalar _tmp8 = P(0, 9) + P(1, 9) * _tmp5 + P(2, 9) * _tmp2 - P(9, 9) * dt; + const Scalar _tmp9 = std::pow(dt, Scalar(2)); + const Scalar _tmp10 = _tmp9 * gyro_var; + const Scalar _tmp11 = P(0, 0) + P(1, 0) * _tmp5 + P(2, 0) * _tmp2 - P(9, 0) * dt; + const Scalar _tmp12 = P(0, 10) + P(1, 10) * _tmp5 + P(2, 10) * _tmp2 - P(9, 10) * dt; + const Scalar _tmp13 = -_tmp3 + _tmp4; + const Scalar _tmp14 = dt * gyro(0, 0); + const Scalar _tmp15 = dt * state(10, 0); + const Scalar _tmp16 = _tmp14 - _tmp15; + const Scalar _tmp17 = P(0, 10) * _tmp13 + P(1, 10) - P(10, 10) * dt + P(2, 10) * _tmp16; + const Scalar _tmp18 = P(0, 0) * _tmp13 + P(1, 0) - P(10, 0) * dt + P(2, 0) * _tmp16; + const Scalar _tmp19 = P(0, 2) * _tmp13 + P(1, 2) - P(10, 2) * dt + P(2, 2) * _tmp16; + const Scalar _tmp20 = P(0, 1) * _tmp13 + P(1, 1) - P(10, 1) * dt + P(2, 1) * _tmp16; + const Scalar _tmp21 = P(0, 11) + P(1, 11) * _tmp5 + P(2, 11) * _tmp2 - P(9, 11) * dt; + const Scalar _tmp22 = _tmp0 - _tmp1; + const Scalar _tmp23 = -_tmp14 + _tmp15; + const Scalar _tmp24 = P(0, 11) * _tmp13 + P(1, 11) - P(10, 11) * dt + P(2, 11) * _tmp16; + const Scalar _tmp25 = P(0, 11) * _tmp22 + P(1, 11) * _tmp23 - P(11, 11) * dt + P(2, 11); + const Scalar _tmp26 = P(0, 1) * _tmp22 + P(1, 1) * _tmp23 - P(11, 1) * dt + P(2, 1); + const Scalar _tmp27 = P(0, 0) * _tmp22 + P(1, 0) * _tmp23 - P(11, 0) * dt + P(2, 0); + const Scalar _tmp28 = P(0, 2) * _tmp22 + P(1, 2) * _tmp23 - P(11, 2) * dt + P(2, 2); + const Scalar _tmp29 = 2 * state(0, 0); + const Scalar _tmp30 = _tmp29 * state(3, 0); const Scalar _tmp31 = -_tmp30; - const Scalar _tmp32 = -_tmp23 * state(3, 0); - const Scalar _tmp33 = _tmp21 * state(2, 0); - const Scalar _tmp34 = _tmp32 - _tmp33; - const Scalar _tmp35 = _tmp3 + _tmp31 + _tmp34; - const Scalar _tmp36 = 2 * _tmp13; - const Scalar _tmp37 = _tmp23 * state(2, 0); - const Scalar _tmp38 = _tmp25 * state(1, 0); - const Scalar _tmp39 = _tmp21 * state(3, 0); - const Scalar _tmp40 = -_tmp37 + _tmp38 + _tmp39 + _tmp9; - const Scalar _tmp41 = 2 * _tmp10; - const Scalar _tmp42 = _tmp23 * state(0, 0); - const Scalar _tmp43 = -_tmp42; - const Scalar _tmp44 = _tmp25 * state(3, 0); - const Scalar _tmp45 = -_tmp21 * state(1, 0); - const Scalar _tmp46 = -_tmp1 + _tmp45; - const Scalar _tmp47 = _tmp43 + _tmp44 + _tmp46; - const Scalar _tmp48 = 2 * _tmp16; - const Scalar _tmp49 = _tmp28 * _tmp29 - _tmp35 * _tmp36 + _tmp40 * _tmp41 - _tmp47 * _tmp48; - const Scalar _tmp50 = -_tmp44; - const Scalar _tmp51 = _tmp1 + _tmp43 + _tmp45 + _tmp50; - const Scalar _tmp52 = _tmp37 + _tmp9; - const Scalar _tmp53 = _tmp38 - _tmp39 + _tmp52; - const Scalar _tmp54 = -_tmp3; - const Scalar _tmp55 = _tmp30 + _tmp34 + _tmp54; - const Scalar _tmp56 = -_tmp22; - const Scalar _tmp57 = _tmp20 + _tmp24 + _tmp26 + _tmp56; - const Scalar _tmp58 = _tmp29 * _tmp51 - _tmp36 * _tmp53 + _tmp41 * _tmp55 - _tmp48 * _tmp57; - const Scalar _tmp59 = -_tmp38 + _tmp39 + _tmp52; - const Scalar _tmp60 = _tmp42 + _tmp46 + _tmp50; - const Scalar _tmp61 = _tmp27 + _tmp5 + _tmp56; - const Scalar _tmp62 = _tmp31 + _tmp32 + _tmp33 + _tmp54; - const Scalar _tmp63 = _tmp29 * _tmp59 - _tmp36 * _tmp60 + _tmp41 * _tmp61 - _tmp48 * _tmp62; - const Scalar _tmp64 = _tmp14 * _tmp6; - const Scalar _tmp65 = _tmp64 * d_ang_dt; - const Scalar _tmp66 = _tmp11 * state(1, 0); - const Scalar _tmp67 = _tmp66 * d_ang_dt; - const Scalar _tmp68 = Scalar(1.0) * _tmp13; - const Scalar _tmp69 = _tmp68 * state(0, 0); - const Scalar _tmp70 = _tmp69 * d_ang_dt; - const Scalar _tmp71 = _tmp17 * state(3, 0); - const Scalar _tmp72 = _tmp71 * d_ang_dt; - const Scalar _tmp73 = -_tmp65 + _tmp67 + _tmp70 - _tmp72; - const Scalar _tmp74 = _tmp7 * state(3, 0); - const Scalar _tmp75 = _tmp74 * d_ang_dt; - const Scalar _tmp76 = _tmp14 * _tmp16; - const Scalar _tmp77 = _tmp76 * d_ang_dt; - const Scalar _tmp78 = _tmp11 * state(0, 0); - const Scalar _tmp79 = _tmp78 * d_ang_dt; - const Scalar _tmp80 = _tmp68 * state(1, 0); - const Scalar _tmp81 = _tmp80 * d_ang_dt; - const Scalar _tmp82 = _tmp75 - _tmp77 - _tmp79 + _tmp81; - const Scalar _tmp83 = P(0, 9) * _tmp63 + P(1, 9) * _tmp49 + P(10, 9) * _tmp82 + - P(11, 9) * _tmp73 + P(2, 9) * _tmp58 + P(9, 9) * _tmp19; - const Scalar _tmp84 = -_tmp64 + _tmp66 + _tmp69 - _tmp71; - const Scalar _tmp85 = _tmp74 - _tmp76 - _tmp78 + _tmp80; - const Scalar _tmp86 = -_tmp12 - _tmp15 - _tmp18 - _tmp8; - const Scalar _tmp87 = std::pow(_tmp86, Scalar(2)) * d_ang_var; - const Scalar _tmp88 = P(0, 0) * _tmp63 + P(1, 0) * _tmp49 + P(10, 0) * _tmp82 + - P(11, 0) * _tmp73 + P(2, 0) * _tmp58 + P(9, 0) * _tmp19; - const Scalar _tmp89 = P(0, 1) * _tmp63 + P(1, 1) * _tmp49 + P(10, 1) * _tmp82 + - P(11, 1) * _tmp73 + P(2, 1) * _tmp58 + P(9, 1) * _tmp19; - const Scalar _tmp90 = P(0, 2) * _tmp63 + P(1, 2) * _tmp49 + P(10, 2) * _tmp82 + - P(11, 2) * _tmp73 + P(2, 2) * _tmp58 + P(9, 2) * _tmp19; - const Scalar _tmp91 = P(0, 11) * _tmp63 + P(1, 11) * _tmp49 + P(10, 11) * _tmp82 + - P(11, 11) * _tmp73 + P(2, 11) * _tmp58 + P(9, 11) * _tmp19; - const Scalar _tmp92 = P(0, 10) * _tmp63 + P(1, 10) * _tmp49 + P(10, 10) * _tmp82 + - P(11, 10) * _tmp73 + P(2, 10) * _tmp58 + P(9, 10) * _tmp19; - const Scalar _tmp93 = _tmp17 * state(0, 0); - const Scalar _tmp94 = _tmp10 * _tmp14; - const Scalar _tmp95 = _tmp68 * state(3, 0); - const Scalar _tmp96 = _tmp7 * state(1, 0); - const Scalar _tmp97 = -_tmp93 + _tmp94 - _tmp95 + _tmp96; - const Scalar _tmp98 = _tmp97 * d_ang_var; - const Scalar _tmp99 = _tmp86 * d_ang_var; - const Scalar _tmp100 = -_tmp74 + _tmp76 + _tmp78 - _tmp80; - const Scalar _tmp101 = -_tmp75 + _tmp77 + _tmp79 - _tmp81; - const Scalar _tmp102 = -_tmp28 * _tmp41 + _tmp29 * _tmp40 + _tmp35 * _tmp48 - _tmp36 * _tmp47; - const Scalar _tmp103 = _tmp29 * _tmp55 - _tmp36 * _tmp57 - _tmp41 * _tmp51 + _tmp48 * _tmp53; - const Scalar _tmp104 = 2 * _tmp62; - const Scalar _tmp105 = -_tmp104 * _tmp13 + _tmp29 * _tmp61 - _tmp41 * _tmp59 + _tmp48 * _tmp60; - const Scalar _tmp106 = _tmp96 * d_ang_dt; - const Scalar _tmp107 = _tmp95 * d_ang_dt; - const Scalar _tmp108 = _tmp94 * d_ang_dt; - const Scalar _tmp109 = _tmp93 * d_ang_dt; - const Scalar _tmp110 = _tmp106 - _tmp107 + _tmp108 - _tmp109; - const Scalar _tmp111 = P(0, 9) * _tmp105 + P(1, 9) * _tmp102 + P(10, 9) * _tmp19 + - P(11, 9) * _tmp110 + P(2, 9) * _tmp103 + P(9, 9) * _tmp101; - const Scalar _tmp112 = P(0, 2) * _tmp105 + P(1, 2) * _tmp102 + P(10, 2) * _tmp19 + - P(11, 2) * _tmp110 + P(2, 2) * _tmp103 + P(9, 2) * _tmp101; - const Scalar _tmp113 = P(0, 1) * _tmp105 + P(1, 1) * _tmp102 + P(10, 1) * _tmp19 + - P(11, 1) * _tmp110 + P(2, 1) * _tmp103 + P(9, 1) * _tmp101; - const Scalar _tmp114 = P(0, 0) * _tmp105 + P(1, 0) * _tmp102 + P(10, 0) * _tmp19 + - P(11, 0) * _tmp110 + P(2, 0) * _tmp103 + P(9, 0) * _tmp101; - const Scalar _tmp115 = P(0, 10) * _tmp105 + P(1, 10) * _tmp102 + P(10, 10) * _tmp19 + - P(11, 10) * _tmp110 + P(2, 10) * _tmp103 + P(9, 10) * _tmp101; - const Scalar _tmp116 = P(0, 11) * _tmp105 + P(1, 11) * _tmp102 + P(10, 11) * _tmp19 + - P(11, 11) * _tmp110 + P(2, 11) * _tmp103 + P(9, 11) * _tmp101; - const Scalar _tmp117 = _tmp93 - _tmp94 + _tmp95 - _tmp96; - const Scalar _tmp118 = _tmp117 * d_ang_var; - const Scalar _tmp119 = _tmp64 - _tmp66 - _tmp69 + _tmp71; - const Scalar _tmp120 = _tmp119 * d_ang_var; - const Scalar _tmp121 = _tmp65 - _tmp67 - _tmp70 + _tmp72; - const Scalar _tmp122 = _tmp29 * _tmp53 + _tmp36 * _tmp51 - _tmp41 * _tmp57 - _tmp48 * _tmp55; - const Scalar _tmp123 = _tmp28 * _tmp36 + _tmp29 * _tmp35 - _tmp40 * _tmp48 - _tmp41 * _tmp47; - const Scalar _tmp124 = -_tmp10 * _tmp104 + _tmp29 * _tmp60 + _tmp36 * _tmp59 - _tmp48 * _tmp61; - const Scalar _tmp125 = -_tmp106 + _tmp107 - _tmp108 + _tmp109; - const Scalar _tmp126 = P(0, 2) * _tmp124 + P(1, 2) * _tmp123 + P(10, 2) * _tmp125 + - P(11, 2) * _tmp19 + P(2, 2) * _tmp122 + P(9, 2) * _tmp121; - const Scalar _tmp127 = P(0, 9) * _tmp124 + P(1, 9) * _tmp123 + P(10, 9) * _tmp125 + - P(11, 9) * _tmp19 + P(2, 9) * _tmp122 + P(9, 9) * _tmp121; - const Scalar _tmp128 = P(0, 11) * _tmp124 + P(1, 11) * _tmp123 + P(10, 11) * _tmp125 + - P(11, 11) * _tmp19 + P(2, 11) * _tmp122 + P(9, 11) * _tmp121; - const Scalar _tmp129 = P(0, 10) * _tmp124 + P(1, 10) * _tmp123 + P(10, 10) * _tmp125 + - P(11, 10) * _tmp19 + P(2, 10) * _tmp122 + P(9, 10) * _tmp121; - const Scalar _tmp130 = P(0, 0) * _tmp124 + P(1, 0) * _tmp123 + P(10, 0) * _tmp125 + - P(11, 0) * _tmp19 + P(2, 0) * _tmp122 + P(9, 0) * _tmp121; - const Scalar _tmp131 = P(0, 1) * _tmp124 + P(1, 1) * _tmp123 + P(10, 1) * _tmp125 + - P(11, 1) * _tmp19 + P(2, 1) * _tmp122 + P(9, 1) * _tmp121; - const Scalar _tmp132 = P(0, 13) * _tmp63 + P(1, 13) * _tmp49 + P(10, 13) * _tmp82 + - P(11, 13) * _tmp73 + P(2, 13) * _tmp58 + P(9, 13) * _tmp19; - const Scalar _tmp133 = 2 * state(0, 0); - const Scalar _tmp134 = _tmp133 * state(3, 0); - const Scalar _tmp135 = 2 * state(2, 0); - const Scalar _tmp136 = _tmp135 * state(1, 0); - const Scalar _tmp137 = -_tmp134 + _tmp136; - const Scalar _tmp138 = _tmp137 * d_vel_dt; - const Scalar _tmp139 = P(0, 14) * _tmp63 + P(1, 14) * _tmp49 + P(10, 14) * _tmp82 + - P(11, 14) * _tmp73 + P(2, 14) * _tmp58 + P(9, 14) * _tmp19; - const Scalar _tmp140 = _tmp135 * state(0, 0); - const Scalar _tmp141 = state(1, 0) * state(3, 0); - const Scalar _tmp142 = 2 * _tmp141; - const Scalar _tmp143 = _tmp140 + _tmp142; - const Scalar _tmp144 = _tmp143 * d_vel_dt; - const Scalar _tmp145 = Scalar(2.0) * state(0, 0); - const Scalar _tmp146 = _tmp145 * state(2, 0); - const Scalar _tmp147 = Scalar(2.0) * _tmp141; - const Scalar _tmp148 = d_vel(1, 0) - d_vel_dt * state(14, 0); - const Scalar _tmp149 = d_vel(2, 0) - d_vel_dt * state(15, 0); - const Scalar _tmp150 = _tmp145 * state(3, 0); - const Scalar _tmp151 = Scalar(2.0) * state(2, 0); - const Scalar _tmp152 = _tmp151 * state(1, 0); - const Scalar _tmp153 = -_tmp152; - const Scalar _tmp154 = _tmp148 * (_tmp146 + _tmp147) + _tmp149 * (_tmp150 + _tmp153); - const Scalar _tmp155 = Scalar(1.0) * std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp156 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp157 = Scalar(1.0) * _tmp156; - const Scalar _tmp158 = -_tmp157; - const Scalar _tmp159 = _tmp155 + _tmp158; - const Scalar _tmp160 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp161 = Scalar(1.0) * _tmp160; - const Scalar _tmp162 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp163 = Scalar(1.0) * _tmp162; - const Scalar _tmp164 = -_tmp163; - const Scalar _tmp165 = _tmp161 + _tmp164; - const Scalar _tmp166 = -_tmp146; - const Scalar _tmp167 = -_tmp147; - const Scalar _tmp168 = d_vel(0, 0) - d_vel_dt * state(13, 0); - const Scalar _tmp169 = _tmp149 * (_tmp159 + _tmp165) + _tmp168 * (_tmp166 + _tmp167); - const Scalar _tmp170 = -_tmp150; - const Scalar _tmp171 = -_tmp161; - const Scalar _tmp172 = _tmp163 + _tmp171; - const Scalar _tmp173 = -_tmp155; - const Scalar _tmp174 = _tmp157 + _tmp173; - const Scalar _tmp175 = _tmp148 * (_tmp172 + _tmp174) + _tmp168 * (_tmp152 + _tmp170); - const Scalar _tmp176 = P(0, 12) * _tmp63 + P(1, 12) * _tmp49 + P(10, 12) * _tmp82 + - P(11, 12) * _tmp73 + P(2, 12) * _tmp58 + P(9, 12) * _tmp19; - const Scalar _tmp177 = -2 * _tmp162; - const Scalar _tmp178 = 1 - 2 * _tmp156; - const Scalar _tmp179 = _tmp177 + _tmp178; - const Scalar _tmp180 = _tmp179 * d_vel_dt; - const Scalar _tmp181 = P(0, 3) * _tmp63 + P(1, 3) * _tmp49 + P(10, 3) * _tmp82 + - P(11, 3) * _tmp73 + P(2, 3) * _tmp58 + P(9, 3) * _tmp19; - const Scalar _tmp182 = P(0, 12) * _tmp105 + P(1, 12) * _tmp102 + P(10, 12) * _tmp19 + - P(11, 12) * _tmp110 + P(2, 12) * _tmp103 + P(9, 12) * _tmp101; - const Scalar _tmp183 = P(0, 14) * _tmp105 + P(1, 14) * _tmp102 + P(10, 14) * _tmp19 + - P(11, 14) * _tmp110 + P(2, 14) * _tmp103 + P(9, 14) * _tmp101; - const Scalar _tmp184 = P(0, 13) * _tmp105 + P(1, 13) * _tmp102 + P(10, 13) * _tmp19 + - P(11, 13) * _tmp110 + P(2, 13) * _tmp103 + P(9, 13) * _tmp101; - const Scalar _tmp185 = P(0, 3) * _tmp105 + P(1, 3) * _tmp102 + P(10, 3) * _tmp19 + - P(11, 3) * _tmp110 + P(2, 3) * _tmp103 + P(9, 3) * _tmp101; - const Scalar _tmp186 = P(0, 14) * _tmp124 + P(1, 14) * _tmp123 + P(10, 14) * _tmp125 + - P(11, 14) * _tmp19 + P(2, 14) * _tmp122 + P(9, 14) * _tmp121; - const Scalar _tmp187 = P(0, 13) * _tmp124 + P(1, 13) * _tmp123 + P(10, 13) * _tmp125 + - P(11, 13) * _tmp19 + P(2, 13) * _tmp122 + P(9, 13) * _tmp121; - const Scalar _tmp188 = P(0, 12) * _tmp124 + P(1, 12) * _tmp123 + P(10, 12) * _tmp125 + - P(11, 12) * _tmp19 + P(2, 12) * _tmp122 + P(9, 12) * _tmp121; - const Scalar _tmp189 = P(0, 3) * _tmp124 + P(1, 3) * _tmp123 + P(10, 3) * _tmp125 + - P(11, 3) * _tmp19 + P(2, 3) * _tmp122 + P(9, 3) * _tmp121; - const Scalar _tmp190 = P(0, 2) * _tmp154 + P(1, 2) * _tmp169 - P(12, 2) * _tmp180 - - P(13, 2) * _tmp138 - P(14, 2) * _tmp144 + P(2, 2) * _tmp175 + P(3, 2); - const Scalar _tmp191 = P(0, 1) * _tmp154 + P(1, 1) * _tmp169 - P(12, 1) * _tmp180 - - P(13, 1) * _tmp138 - P(14, 1) * _tmp144 + P(2, 1) * _tmp175 + P(3, 1); - const Scalar _tmp192 = P(0, 0) * _tmp154 + P(1, 0) * _tmp169 - P(12, 0) * _tmp180 - - P(13, 0) * _tmp138 - P(14, 0) * _tmp144 + P(2, 0) * _tmp175 + P(3, 0); - const Scalar _tmp193 = P(0, 12) * _tmp154 + P(1, 12) * _tmp169 - P(12, 12) * _tmp180 - - P(13, 12) * _tmp138 - P(14, 12) * _tmp144 + P(2, 12) * _tmp175 + P(3, 12); - const Scalar _tmp194 = P(0, 13) * _tmp154 + P(1, 13) * _tmp169 - P(12, 13) * _tmp180 - - P(13, 13) * _tmp138 - P(14, 13) * _tmp144 + P(2, 13) * _tmp175 + P(3, 13); - const Scalar _tmp195 = _tmp194 * d_vel_dt; - const Scalar _tmp196 = P(0, 14) * _tmp154 + P(1, 14) * _tmp169 - P(12, 14) * _tmp180 - - P(13, 14) * _tmp138 - P(14, 14) * _tmp144 + P(2, 14) * _tmp175 + P(3, 14); - const Scalar _tmp197 = P(0, 3) * _tmp154 + P(1, 3) * _tmp169 - P(12, 3) * _tmp180 - - P(13, 3) * _tmp138 - P(14, 3) * _tmp144 + P(2, 3) * _tmp175 + P(3, 3); - const Scalar _tmp198 = _tmp135 * state(3, 0); - const Scalar _tmp199 = _tmp133 * state(1, 0); - const Scalar _tmp200 = _tmp198 - _tmp199; - const Scalar _tmp201 = _tmp200 * d_vel_dt; - const Scalar _tmp202 = _tmp151 * state(3, 0); - const Scalar _tmp203 = _tmp145 * state(1, 0); - const Scalar _tmp204 = -_tmp203; - const Scalar _tmp205 = - _tmp148 * (_tmp202 + _tmp204) + _tmp149 * (_tmp158 + _tmp161 + _tmp163 + _tmp173); - const Scalar _tmp206 = _tmp134 + _tmp136; - const Scalar _tmp207 = _tmp206 * d_vel_dt; - const Scalar _tmp208 = - _tmp148 * (_tmp153 + _tmp170) + _tmp168 * (_tmp155 + _tmp157 + _tmp164 + _tmp171); - const Scalar _tmp209 = -_tmp202; - const Scalar _tmp210 = _tmp149 * (_tmp150 + _tmp152) + _tmp168 * (_tmp203 + _tmp209); - const Scalar _tmp211 = -2 * _tmp160; - const Scalar _tmp212 = _tmp177 + _tmp211 + 1; - const Scalar _tmp213 = _tmp212 * d_vel_dt; - const Scalar _tmp214 = P(0, 4) * _tmp63 + P(1, 4) * _tmp49 + P(10, 4) * _tmp82 + - P(11, 4) * _tmp73 + P(2, 4) * _tmp58 + P(9, 4) * _tmp19; - const Scalar _tmp215 = P(0, 4) * _tmp105 + P(1, 4) * _tmp102 + P(10, 4) * _tmp19 + - P(11, 4) * _tmp110 + P(2, 4) * _tmp103 + P(9, 4) * _tmp101; - const Scalar _tmp216 = P(0, 4) * _tmp124 + P(1, 4) * _tmp123 + P(10, 4) * _tmp125 + - P(11, 4) * _tmp19 + P(2, 4) * _tmp122 + P(9, 4) * _tmp121; - const Scalar _tmp217 = _tmp212 * d_vel_var(1, 0); - const Scalar _tmp218 = _tmp193 * d_vel_dt; - const Scalar _tmp219 = P(0, 4) * _tmp154 + P(1, 4) * _tmp169 - P(12, 4) * _tmp180 - - P(13, 4) * _tmp138 - P(14, 4) * _tmp144 + P(2, 4) * _tmp175 + P(3, 4); - const Scalar _tmp220 = P(0, 1) * _tmp205 + P(1, 1) * _tmp210 - P(12, 1) * _tmp207 - - P(13, 1) * _tmp213 - P(14, 1) * _tmp201 + P(2, 1) * _tmp208 + P(4, 1); - const Scalar _tmp221 = P(0, 12) * _tmp205 + P(1, 12) * _tmp210 - P(12, 12) * _tmp207 - - P(13, 12) * _tmp213 - P(14, 12) * _tmp201 + P(2, 12) * _tmp208 + P(4, 12); - const Scalar _tmp222 = P(0, 13) * _tmp205 + P(1, 13) * _tmp210 - P(12, 13) * _tmp207 - - P(13, 13) * _tmp213 - P(14, 13) * _tmp201 + P(2, 13) * _tmp208 + P(4, 13); - const Scalar _tmp223 = P(0, 2) * _tmp205 + P(1, 2) * _tmp210 - P(12, 2) * _tmp207 - - P(13, 2) * _tmp213 - P(14, 2) * _tmp201 + P(2, 2) * _tmp208 + P(4, 2); - const Scalar _tmp224 = P(0, 14) * _tmp205 + P(1, 14) * _tmp210 - P(12, 14) * _tmp207 - - P(13, 14) * _tmp213 - P(14, 14) * _tmp201 + P(2, 14) * _tmp208 + P(4, 14); - const Scalar _tmp225 = P(0, 0) * _tmp205 + P(1, 0) * _tmp210 - P(12, 0) * _tmp207 - - P(13, 0) * _tmp213 - P(14, 0) * _tmp201 + P(2, 0) * _tmp208 + P(4, 0); - const Scalar _tmp226 = P(0, 4) * _tmp205 + P(1, 4) * _tmp210 - P(12, 4) * _tmp207 - - P(13, 4) * _tmp213 - P(14, 4) * _tmp201 + P(2, 4) * _tmp208 + P(4, 4); - const Scalar _tmp227 = _tmp198 + _tmp199; - const Scalar _tmp228 = _tmp227 * d_vel_dt; - const Scalar _tmp229 = -_tmp140 + _tmp142; - const Scalar _tmp230 = _tmp229 * d_vel_dt; - const Scalar _tmp231 = _tmp148 * (_tmp146 + _tmp167) + _tmp168 * (_tmp202 + _tmp203); - const Scalar _tmp232 = _tmp149 * (_tmp147 + _tmp166) + _tmp168 * (_tmp165 + _tmp174); - const Scalar _tmp233 = _tmp148 * (_tmp159 + _tmp172) + _tmp149 * (_tmp204 + _tmp209); - const Scalar _tmp234 = _tmp178 + _tmp211; - const Scalar _tmp235 = _tmp234 * d_vel_dt; - const Scalar _tmp236 = P(0, 5) * _tmp63 + P(1, 5) * _tmp49 + P(10, 5) * _tmp82 + - P(11, 5) * _tmp73 + P(2, 5) * _tmp58 + P(9, 5) * _tmp19; - const Scalar _tmp237 = P(0, 5) * _tmp105 + P(1, 5) * _tmp102 + P(10, 5) * _tmp19 + - P(11, 5) * _tmp110 + P(2, 5) * _tmp103 + P(9, 5) * _tmp101; - const Scalar _tmp238 = P(0, 5) * _tmp124 + P(1, 5) * _tmp123 + P(10, 5) * _tmp125 + - P(11, 5) * _tmp19 + P(2, 5) * _tmp122 + P(9, 5) * _tmp121; - const Scalar _tmp239 = _tmp229 * d_vel_var(0, 0); - const Scalar _tmp240 = _tmp234 * d_vel_var(2, 0); - const Scalar _tmp241 = P(0, 5) * _tmp154 + P(1, 5) * _tmp169 - P(12, 5) * _tmp180 - - P(13, 5) * _tmp138 - P(14, 5) * _tmp144 + P(2, 5) * _tmp175 + P(3, 5); - const Scalar _tmp242 = P(0, 5) * _tmp205 + P(1, 5) * _tmp210 - P(12, 5) * _tmp207 - - P(13, 5) * _tmp213 - P(14, 5) * _tmp201 + P(2, 5) * _tmp208 + P(4, 5); - const Scalar _tmp243 = P(0, 14) * _tmp233 + P(1, 14) * _tmp232 - P(12, 14) * _tmp230 - - P(13, 14) * _tmp228 - P(14, 14) * _tmp235 + P(2, 14) * _tmp231 + P(5, 14); - const Scalar _tmp244 = P(0, 12) * _tmp233 + P(1, 12) * _tmp232 - P(12, 12) * _tmp230 - - P(13, 12) * _tmp228 - P(14, 12) * _tmp235 + P(2, 12) * _tmp231 + P(5, 12); - const Scalar _tmp245 = P(0, 13) * _tmp233 + P(1, 13) * _tmp232 - P(12, 13) * _tmp230 - - P(13, 13) * _tmp228 - P(14, 13) * _tmp235 + P(2, 13) * _tmp231 + P(5, 13); - const Scalar _tmp246 = P(0, 5) * _tmp233 + P(1, 5) * _tmp232 - P(12, 5) * _tmp230 - - P(13, 5) * _tmp228 - P(14, 5) * _tmp235 + P(2, 5) * _tmp231 + P(5, 5); + const Scalar _tmp32 = 2 * state(1, 0); + const Scalar _tmp33 = _tmp32 * state(2, 0); + const Scalar _tmp34 = _tmp31 + _tmp33; + const Scalar _tmp35 = accel(0, 0) - state(13, 0); + const Scalar _tmp36 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp37 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp38 = -_tmp37; + const Scalar _tmp39 = _tmp36 + _tmp38; + const Scalar _tmp40 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp41 = -_tmp40; + const Scalar _tmp42 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp43 = _tmp41 + _tmp42; + const Scalar _tmp44 = accel(1, 0) - state(14, 0); + const Scalar _tmp45 = dt * (_tmp34 * _tmp35 + _tmp44 * (_tmp39 + _tmp43)); + const Scalar _tmp46 = P(0, 12) + P(1, 12) * _tmp5 + P(2, 12) * _tmp2 - P(9, 12) * dt; + const Scalar _tmp47 = -2 * _tmp42; + const Scalar _tmp48 = -2 * _tmp36; + const Scalar _tmp49 = _tmp47 + _tmp48 + 1; + const Scalar _tmp50 = _tmp49 * dt; + const Scalar _tmp51 = _tmp29 * state(2, 0); + const Scalar _tmp52 = -_tmp51; + const Scalar _tmp53 = _tmp32 * state(3, 0); + const Scalar _tmp54 = -_tmp53; + const Scalar _tmp55 = -_tmp42; + const Scalar _tmp56 = _tmp40 + _tmp55; + const Scalar _tmp57 = -_tmp36; + const Scalar _tmp58 = _tmp37 + _tmp57; + const Scalar _tmp59 = accel(2, 0) - state(15, 0); + const Scalar _tmp60 = dt * (_tmp35 * (_tmp52 + _tmp54) + _tmp59 * (_tmp56 + _tmp58)); + const Scalar _tmp61 = _tmp51 + _tmp53; + const Scalar _tmp62 = -_tmp33; + const Scalar _tmp63 = dt * (_tmp44 * _tmp61 + _tmp59 * (_tmp30 + _tmp62)); + const Scalar _tmp64 = P(0, 13) + P(1, 13) * _tmp5 + P(2, 13) * _tmp2 - P(9, 13) * dt; + const Scalar _tmp65 = _tmp34 * dt; + const Scalar _tmp66 = P(0, 14) + P(1, 14) * _tmp5 + P(2, 14) * _tmp2 - P(9, 14) * dt; + const Scalar _tmp67 = _tmp61 * dt; + const Scalar _tmp68 = P(0, 3) + P(1, 3) * _tmp5 + P(2, 3) * _tmp2 - P(9, 3) * dt; + const Scalar _tmp69 = P(0, 13) * _tmp13 + P(1, 13) - P(10, 13) * dt + P(2, 13) * _tmp16; + const Scalar _tmp70 = P(0, 14) * _tmp13 + P(1, 14) - P(10, 14) * dt + P(2, 14) * _tmp16; + const Scalar _tmp71 = P(0, 12) * _tmp13 + P(1, 12) - P(10, 12) * dt + P(2, 12) * _tmp16; + const Scalar _tmp72 = P(0, 3) * _tmp13 + P(1, 3) - P(10, 3) * dt + P(2, 3) * _tmp16; + const Scalar _tmp73 = P(0, 14) * _tmp22 + P(1, 14) * _tmp23 - P(11, 14) * dt + P(2, 14); + const Scalar _tmp74 = P(0, 13) * _tmp22 + P(1, 13) * _tmp23 - P(11, 13) * dt + P(2, 13); + const Scalar _tmp75 = P(0, 12) * _tmp22 + P(1, 12) * _tmp23 - P(11, 12) * dt + P(2, 12); + const Scalar _tmp76 = P(0, 3) * _tmp22 + P(1, 3) * _tmp23 - P(11, 3) * dt + P(2, 3); + const Scalar _tmp77 = P(0, 1) * _tmp63 + P(1, 1) * _tmp60 - P(12, 1) * _tmp50 - + P(13, 1) * _tmp65 - P(14, 1) * _tmp67 + P(2, 1) * _tmp45 + P(3, 1); + const Scalar _tmp78 = P(0, 14) * _tmp63 + P(1, 14) * _tmp60 - P(12, 14) * _tmp50 - + P(13, 14) * _tmp65 - P(14, 14) * _tmp67 + P(2, 14) * _tmp45 + P(3, 14); + const Scalar _tmp79 = P(0, 13) * _tmp63 + P(1, 13) * _tmp60 - P(12, 13) * _tmp50 - + P(13, 13) * _tmp65 - P(14, 13) * _tmp67 + P(2, 13) * _tmp45 + P(3, 13); + const Scalar _tmp80 = P(0, 0) * _tmp63 + P(1, 0) * _tmp60 - P(12, 0) * _tmp50 - + P(13, 0) * _tmp65 - P(14, 0) * _tmp67 + P(2, 0) * _tmp45 + P(3, 0); + const Scalar _tmp81 = _tmp9 * accel_var(0, 0); + const Scalar _tmp82 = P(0, 2) * _tmp63 + P(1, 2) * _tmp60 - P(12, 2) * _tmp50 - + P(13, 2) * _tmp65 - P(14, 2) * _tmp67 + P(2, 2) * _tmp45 + P(3, 2); + const Scalar _tmp83 = _tmp9 * accel_var(1, 0); + const Scalar _tmp84 = _tmp9 * accel_var(2, 0); + const Scalar _tmp85 = P(0, 12) * _tmp63 + P(1, 12) * _tmp60 - P(12, 12) * _tmp50 - + P(13, 12) * _tmp65 - P(14, 12) * _tmp67 + P(2, 12) * _tmp45 + P(3, 12); + const Scalar _tmp86 = P(0, 3) * _tmp63 + P(1, 3) * _tmp60 - P(12, 3) * _tmp50 - + P(13, 3) * _tmp65 - P(14, 3) * _tmp67 + P(2, 3) * _tmp45 + P(3, 3); + const Scalar _tmp87 = 2 * state(2, 0) * state(3, 0); + const Scalar _tmp88 = _tmp29 * state(1, 0); + const Scalar _tmp89 = -_tmp88; + const Scalar _tmp90 = _tmp87 + _tmp89; + const Scalar _tmp91 = dt * (_tmp44 * _tmp90 + _tmp59 * (_tmp43 + _tmp58)); + const Scalar _tmp92 = dt * (_tmp35 * (_tmp39 + _tmp56) + _tmp44 * (_tmp31 + _tmp62)); + const Scalar _tmp93 = 1 - 2 * _tmp37; + const Scalar _tmp94 = _tmp47 + _tmp93; + const Scalar _tmp95 = _tmp94 * dt; + const Scalar _tmp96 = _tmp90 * dt; + const Scalar _tmp97 = _tmp30 + _tmp33; + const Scalar _tmp98 = -_tmp87; + const Scalar _tmp99 = dt * (_tmp35 * (_tmp88 + _tmp98) + _tmp59 * _tmp97); + const Scalar _tmp100 = _tmp97 * dt; + const Scalar _tmp101 = P(0, 4) + P(1, 4) * _tmp5 + P(2, 4) * _tmp2 - P(9, 4) * dt; + const Scalar _tmp102 = P(0, 4) * _tmp13 + P(1, 4) - P(10, 4) * dt + P(2, 4) * _tmp16; + const Scalar _tmp103 = _tmp74 * dt; + const Scalar _tmp104 = P(0, 4) * _tmp22 + P(1, 4) * _tmp23 - P(11, 4) * dt + P(2, 4); + const Scalar _tmp105 = _tmp81 * _tmp97; + const Scalar _tmp106 = _tmp84 * _tmp90; + const Scalar _tmp107 = P(0, 4) * _tmp63 + P(1, 4) * _tmp60 - P(12, 4) * _tmp50 - + P(13, 4) * _tmp65 - P(14, 4) * _tmp67 + P(2, 4) * _tmp45 + P(3, 4); + const Scalar _tmp108 = P(0, 14) * _tmp91 + P(1, 14) * _tmp99 - P(12, 14) * _tmp100 - + P(13, 14) * _tmp95 - P(14, 14) * _tmp96 + P(2, 14) * _tmp92 + P(4, 14); + const Scalar _tmp109 = P(0, 0) * _tmp91 + P(1, 0) * _tmp99 - P(12, 0) * _tmp100 - + P(13, 0) * _tmp95 - P(14, 0) * _tmp96 + P(2, 0) * _tmp92 + P(4, 0); + const Scalar _tmp110 = P(0, 13) * _tmp91 + P(1, 13) * _tmp99 - P(12, 13) * _tmp100 - + P(13, 13) * _tmp95 - P(14, 13) * _tmp96 + P(2, 13) * _tmp92 + P(4, 13); + const Scalar _tmp111 = P(0, 12) * _tmp91 + P(1, 12) * _tmp99 - P(12, 12) * _tmp100 - + P(13, 12) * _tmp95 - P(14, 12) * _tmp96 + P(2, 12) * _tmp92 + P(4, 12); + const Scalar _tmp112 = P(0, 1) * _tmp91 + P(1, 1) * _tmp99 - P(12, 1) * _tmp100 - + P(13, 1) * _tmp95 - P(14, 1) * _tmp96 + P(2, 1) * _tmp92 + P(4, 1); + const Scalar _tmp113 = P(0, 2) * _tmp91 + P(1, 2) * _tmp99 - P(12, 2) * _tmp100 - + P(13, 2) * _tmp95 - P(14, 2) * _tmp96 + P(2, 2) * _tmp92 + P(4, 2); + const Scalar _tmp114 = P(0, 4) * _tmp91 + P(1, 4) * _tmp99 - P(12, 4) * _tmp100 - + P(13, 4) * _tmp95 - P(14, 4) * _tmp96 + P(2, 4) * _tmp92 + P(4, 4); + const Scalar _tmp115 = + dt * (_tmp44 * (_tmp38 + _tmp40 + _tmp42 + _tmp57) + _tmp59 * (_tmp89 + _tmp98)); + const Scalar _tmp116 = _tmp48 + _tmp93; + const Scalar _tmp117 = _tmp116 * dt; + const Scalar _tmp118 = _tmp87 + _tmp88; + const Scalar _tmp119 = _tmp118 * dt; + const Scalar _tmp120 = dt * (_tmp118 * _tmp35 + _tmp44 * (_tmp51 + _tmp54)); + const Scalar _tmp121 = _tmp52 + _tmp53; + const Scalar _tmp122 = dt * (_tmp121 * _tmp59 + _tmp35 * (_tmp36 + _tmp37 + _tmp41 + _tmp55)); + const Scalar _tmp123 = _tmp121 * dt; + const Scalar _tmp124 = P(0, 5) + P(1, 5) * _tmp5 + P(2, 5) * _tmp2 - P(9, 5) * dt; + const Scalar _tmp125 = P(0, 5) * _tmp13 + P(1, 5) - P(10, 5) * dt + P(2, 5) * _tmp16; + const Scalar _tmp126 = P(0, 5) * _tmp22 + P(1, 5) * _tmp23 - P(11, 5) * dt + P(2, 5); + const Scalar _tmp127 = _tmp118 * _tmp83; + const Scalar _tmp128 = P(0, 5) * _tmp63 + P(1, 5) * _tmp60 - P(12, 5) * _tmp50 - + P(13, 5) * _tmp65 - P(14, 5) * _tmp67 + P(2, 5) * _tmp45 + P(3, 5); + const Scalar _tmp129 = P(0, 5) * _tmp91 + P(1, 5) * _tmp99 - P(12, 5) * _tmp100 - + P(13, 5) * _tmp95 - P(14, 5) * _tmp96 + P(2, 5) * _tmp92 + P(4, 5); + const Scalar _tmp130 = P(0, 13) * _tmp115 + P(1, 13) * _tmp122 - P(12, 13) * _tmp123 - + P(13, 13) * _tmp119 - P(14, 13) * _tmp117 + P(2, 13) * _tmp120 + P(5, 13); + const Scalar _tmp131 = P(0, 14) * _tmp115 + P(1, 14) * _tmp122 - P(12, 14) * _tmp123 - + P(13, 14) * _tmp119 - P(14, 14) * _tmp117 + P(2, 14) * _tmp120 + P(5, 14); + const Scalar _tmp132 = P(0, 12) * _tmp115 + P(1, 12) * _tmp122 - P(12, 12) * _tmp123 - + P(13, 12) * _tmp119 - P(14, 12) * _tmp117 + P(2, 12) * _tmp120 + P(5, 12); + const Scalar _tmp133 = P(0, 5) * _tmp115 + P(1, 5) * _tmp122 - P(12, 5) * _tmp123 - + P(13, 5) * _tmp119 - P(14, 5) * _tmp117 + P(2, 5) * _tmp120 + P(5, 5); // Output terms (1) matrix::Matrix _res; - _res(0, 0) = _tmp19 * _tmp83 + _tmp49 * _tmp89 + _tmp58 * _tmp90 + _tmp63 * _tmp88 + - _tmp73 * _tmp91 + _tmp82 * _tmp92 + std::pow(_tmp84, Scalar(2)) * d_ang_var + - std::pow(_tmp85, Scalar(2)) * d_ang_var + _tmp87; + _res(0, 0) = _tmp10 + _tmp11 + _tmp2 * _tmp6 + _tmp5 * _tmp7 - _tmp8 * dt; _res(1, 0) = 0; _res(2, 0) = 0; _res(3, 0) = 0; @@ -377,12 +222,8 @@ matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix