From 016db84d69930b7b36b819c2db170090a665554d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 13 Sep 2023 09:58:56 -0400 Subject: [PATCH] ekf2: covariance update use IMU sample dt (#22032) - usually the delta angle and delta velocity dt is the same, but they can be slightly different --- src/modules/ekf2/EKF/covariance.cpp | 18 +- .../EKF/python/ekf_derivation/derivation.py | 15 +- .../generated/predict_covariance.h | 1280 +++++++++-------- .../test/change_indication/ekf_gsf_reset.csv | 14 +- .../ekf2/test/change_indication/iris_gps.csv | 8 +- 5 files changed, 672 insertions(+), 663 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 45d66797f6..81fb7563f6 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -98,15 +98,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) { // Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values const float dt = _dt_ekf_avg; - const float dt_inv = 1.f / dt; // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); const float beta = 1.0f - alpha; - _ang_rate_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt); - _accel_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_vel.norm(), beta * _accel_magnitude_filt); - _accel_vec_filt = alpha * dt_inv * imu_delayed.delta_vel + beta * _accel_vec_filt; + _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt); + _accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt); + _accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt; const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim; @@ -216,7 +215,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // assign IMU noise variances // inputs to the system are 3 delta angles and 3 delta velocities float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); - const float d_ang_var = sq(dt * gyro_noise); + const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise); float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); @@ -225,10 +224,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) for (int i = 0; i <= 2; 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(dt * BADACC_BIAS_PNOISE); + d_vel_var(i) = sq(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE); } else { - d_vel_var(i) = sq(dt * accel_noise); + d_vel_var(i) = sq(imu_delayed.delta_vel_dt * accel_noise); } } @@ -236,7 +235,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) SquareMatrix24f nextP; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states - sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, imu_delayed.delta_vel, d_vel_var, imu_delayed.delta_ang, d_ang_var, dt, &nextP); + sym::PredictCovariance(getStateAtFusionHorizonAsVector(), 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); // compute noise variance for stationary processes Vector24f process_noise; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 13262ba988..3fdef210c2 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Copyright (c) 2022 PX4 Development Team @@ -82,19 +82,20 @@ def predict_covariance( state: VState, P: MState, d_vel: sf.V3, + d_vel_dt: sf.Scalar, d_vel_var: sf.V3, d_ang: sf.V3, - d_ang_var: sf.Scalar, - dt: sf.Scalar + d_ang_dt: sf.Scalar, + d_ang_var: sf.Scalar ): g = sf.Symbol("g") # does not appear in the jacobians accel_b = sf.V3(state[State.accel_bx], state[State.accel_by], state[State.accel_bz]) - d_vel_b = accel_b * dt + d_vel_b = accel_b * d_vel_dt d_vel_true = d_vel - d_vel_b gyro_b = sf.V3(state[State.gyro_bx], state[State.gyro_by], state[State.gyro_bz]) - d_ang_b = gyro_b * dt + d_ang_b = gyro_b * d_ang_dt d_ang_true = d_ang - d_ang_b q = state_to_quat(state) @@ -103,8 +104,8 @@ def predict_covariance( p = sf.V3(state[State.px], state[State.py], state[State.pz]) q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1) - v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * dt - p_new = p + v * dt + v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * d_vel_dt + p_new = p + v * d_vel_dt # Predicted state vector at time t + dt state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.gyro_bx:State.n_states])]]) 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 a24f06b065..ee0ee8e1d7 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 @@ -19,10 +19,11 @@ namespace sym { * state: Matrix24_1 * P: Matrix24_24 * d_vel: Matrix31 + * d_vel_dt: Scalar * d_vel_var: Matrix31 * d_ang: Matrix31 + * d_ang_dt: Scalar * d_ang_var: Scalar - * dt: Scalar * * Outputs: * P_new: Matrix24_24 @@ -30,294 +31,294 @@ namespace sym { template void PredictCovariance(const matrix::Matrix& state, const matrix::Matrix& P, - const matrix::Matrix& d_vel, + 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_var, - const Scalar dt, matrix::Matrix* const P_new = nullptr) { + const matrix::Matrix& d_ang, const Scalar d_ang_dt, + const Scalar d_ang_var, + matrix::Matrix* const P_new = nullptr) { // Total ops: 2882 // Input arrays - // Intermediate terms (174) - const Scalar _tmp0 = Scalar(0.5) * d_ang(0, 0); - const Scalar _tmp1 = Scalar(0.5) * dt; - const Scalar _tmp2 = _tmp1 * state(10, 0); + // Intermediate terms (173) + const Scalar _tmp0 = Scalar(0.5) * d_ang(1, 0); + const Scalar _tmp1 = Scalar(0.5) * d_ang_dt; + const Scalar _tmp2 = _tmp1 * state(11, 0); const Scalar _tmp3 = -_tmp0 + _tmp2; const Scalar _tmp4 = Scalar(0.5) * d_ang(2, 0); const Scalar _tmp5 = _tmp1 * state(12, 0); const Scalar _tmp6 = -_tmp4 + _tmp5; - const Scalar _tmp7 = Scalar(0.5) * d_ang(1, 0); - const Scalar _tmp8 = _tmp1 * state(11, 0); + const Scalar _tmp7 = Scalar(0.5) * d_ang(0, 0); + const Scalar _tmp8 = _tmp1 * state(10, 0); const Scalar _tmp9 = -_tmp7 + _tmp8; - const Scalar _tmp10 = _tmp1 * state(2, 0); + const Scalar _tmp10 = _tmp1 * state(1, 0); const Scalar _tmp11 = _tmp1 * state(3, 0); - const Scalar _tmp12 = _tmp1 * state(1, 0); - const Scalar _tmp13 = P(0, 12) + P(1, 12) * _tmp3 + P(10, 12) * _tmp12 + P(11, 12) * _tmp10 + - P(12, 12) * _tmp11 + P(2, 12) * _tmp9 + P(3, 12) * _tmp6; - const Scalar _tmp14 = P(0, 11) + P(1, 11) * _tmp3 + P(10, 11) * _tmp12 + P(11, 11) * _tmp10 + - P(12, 11) * _tmp11 + P(2, 11) * _tmp9 + P(3, 11) * _tmp6; - const Scalar _tmp15 = P(0, 10) + P(1, 10) * _tmp3 + P(10, 10) * _tmp12 + P(11, 10) * _tmp10 + - P(12, 10) * _tmp11 + P(2, 10) * _tmp9 + P(3, 10) * _tmp6; - const Scalar _tmp16 = P(0, 2) + P(1, 2) * _tmp3 + P(10, 2) * _tmp12 + P(11, 2) * _tmp10 + - P(12, 2) * _tmp11 + P(2, 2) * _tmp9 + P(3, 2) * _tmp6; - const Scalar _tmp17 = P(0, 1) + P(1, 1) * _tmp3 + P(10, 1) * _tmp12 + P(11, 1) * _tmp10 + - P(12, 1) * _tmp11 + P(2, 1) * _tmp9 + P(3, 1) * _tmp6; - const Scalar _tmp18 = P(0, 3) + P(1, 3) * _tmp3 + P(10, 3) * _tmp12 + P(11, 3) * _tmp10 + - P(12, 3) * _tmp11 + P(2, 3) * _tmp9 + P(3, 3) * _tmp6; - const Scalar _tmp19 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp20 = Scalar(0.25) * d_ang_var; - const Scalar _tmp21 = _tmp19 * _tmp20; - const Scalar _tmp22 = P(0, 0) + P(1, 0) * _tmp3 + P(10, 0) * _tmp12 + P(11, 0) * _tmp10 + - P(12, 0) * _tmp11 + P(2, 0) * _tmp9 + P(3, 0) * _tmp6; - const Scalar _tmp23 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp24 = _tmp20 * _tmp23; - const Scalar _tmp25 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp26 = _tmp20 * _tmp25; - const Scalar _tmp27 = _tmp24 + _tmp26; - const Scalar _tmp28 = _tmp20 * state(1, 0); + const Scalar _tmp12 = _tmp1 * state(2, 0); + const Scalar _tmp13 = P(0, 10) + P(1, 10) * _tmp9 + P(10, 10) * _tmp10 + P(11, 10) * _tmp12 + + P(12, 10) * _tmp11 + P(2, 10) * _tmp3 + P(3, 10) * _tmp6; + const Scalar _tmp14 = P(0, 11) + P(1, 11) * _tmp9 + P(10, 11) * _tmp10 + P(11, 11) * _tmp12 + + P(12, 11) * _tmp11 + P(2, 11) * _tmp3 + P(3, 11) * _tmp6; + const Scalar _tmp15 = P(0, 12) + P(1, 12) * _tmp9 + P(10, 12) * _tmp10 + P(11, 12) * _tmp12 + + P(12, 12) * _tmp11 + P(2, 12) * _tmp3 + P(3, 12) * _tmp6; + const Scalar _tmp16 = P(0, 2) + P(1, 2) * _tmp9 + P(10, 2) * _tmp10 + P(11, 2) * _tmp12 + + P(12, 2) * _tmp11 + P(2, 2) * _tmp3 + P(3, 2) * _tmp6; + const Scalar _tmp17 = P(0, 3) + P(1, 3) * _tmp9 + P(10, 3) * _tmp10 + P(11, 3) * _tmp12 + + P(12, 3) * _tmp11 + P(2, 3) * _tmp3 + P(3, 3) * _tmp6; + const Scalar _tmp18 = P(11, 1) * _tmp1; + const Scalar _tmp19 = P(0, 1) + P(1, 1) * _tmp9 + P(10, 1) * _tmp10 + P(12, 1) * _tmp11 + + P(2, 1) * _tmp3 + P(3, 1) * _tmp6 + _tmp18 * state(2, 0); + const Scalar _tmp20 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp21 = Scalar(0.25) * d_ang_var; + const Scalar _tmp22 = _tmp20 * _tmp21; + const Scalar _tmp23 = P(0, 0) + P(1, 0) * _tmp9 + P(10, 0) * _tmp10 + P(11, 0) * _tmp12 + + P(12, 0) * _tmp11 + P(2, 0) * _tmp3 + P(3, 0) * _tmp6; + const Scalar _tmp24 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp25 = _tmp21 * _tmp24; + const Scalar _tmp26 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp27 = _tmp21 * _tmp26; + const Scalar _tmp28 = _tmp25 + _tmp27; const Scalar _tmp29 = _tmp1 * state(0, 0); - const Scalar _tmp30 = _tmp0 - _tmp2; - const Scalar _tmp31 = _tmp4 - _tmp5; - const Scalar _tmp32 = P(0, 10) * _tmp30 + P(1, 10) - P(10, 10) * _tmp29 + P(11, 10) * _tmp11 - - P(12, 10) * _tmp10 + P(2, 10) * _tmp31 + P(3, 10) * _tmp9; - const Scalar _tmp33 = P(0, 11) * _tmp30 + P(1, 11) - P(10, 11) * _tmp29 + P(11, 11) * _tmp11 - - P(12, 11) * _tmp10 + P(2, 11) * _tmp31 + P(3, 11) * _tmp9; - const Scalar _tmp34 = P(0, 12) * _tmp30 + P(1, 12) - P(10, 12) * _tmp29 + P(11, 12) * _tmp11 - - P(12, 12) * _tmp10 + P(2, 12) * _tmp31 + P(3, 12) * _tmp9; - const Scalar _tmp35 = P(0, 0) * _tmp30 + P(1, 0) - P(10, 0) * _tmp29 + P(11, 0) * _tmp11 - - P(12, 0) * _tmp10 + P(2, 0) * _tmp31 + P(3, 0) * _tmp9; - const Scalar _tmp36 = P(0, 3) * _tmp30 + P(1, 3) - P(10, 3) * _tmp29 + P(11, 3) * _tmp11 - - P(12, 3) * _tmp10 + P(2, 3) * _tmp31 + P(3, 3) * _tmp9; - const Scalar _tmp37 = P(0, 2) * _tmp30 + P(1, 2) - P(10, 2) * _tmp29 + P(11, 2) * _tmp11 - - P(12, 2) * _tmp10 + P(2, 2) * _tmp31 + P(3, 2) * _tmp9; - const Scalar _tmp38 = _tmp20 * std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp39 = P(0, 1) * _tmp30 + P(1, 1) - P(10, 1) * _tmp29 + P(11, 1) * _tmp11 - - P(12, 1) * _tmp10 + P(2, 1) * _tmp31 + P(3, 1) * _tmp9; - const Scalar _tmp40 = _tmp20 * state(0, 0); - const Scalar _tmp41 = _tmp7 - _tmp8; - const Scalar _tmp42 = P(0, 11) * _tmp41 + P(1, 11) * _tmp6 - P(10, 11) * _tmp11 - - P(11, 11) * _tmp29 + P(12, 11) * _tmp12 + P(2, 11) + P(3, 11) * _tmp30; - const Scalar _tmp43 = P(0, 10) * _tmp41 + P(1, 10) * _tmp6 - P(10, 10) * _tmp11 - - P(11, 10) * _tmp29 + P(12, 10) * _tmp12 + P(2, 10) + P(3, 10) * _tmp30; - const Scalar _tmp44 = P(0, 12) * _tmp41 + P(1, 12) * _tmp6 - P(10, 12) * _tmp11 - - P(11, 12) * _tmp29 + P(12, 12) * _tmp12 + P(2, 12) + P(3, 12) * _tmp30; - const Scalar _tmp45 = P(0, 3) * _tmp41 + P(1, 3) * _tmp6 - P(10, 3) * _tmp11 - P(11, 3) * _tmp29 + - P(12, 3) * _tmp12 + P(2, 3) + P(3, 3) * _tmp30; - const Scalar _tmp46 = P(0, 1) * _tmp41 + P(1, 1) * _tmp6 - P(10, 1) * _tmp11 - P(11, 1) * _tmp29 + - P(12, 1) * _tmp12 + P(2, 1) + P(3, 1) * _tmp30; - const Scalar _tmp47 = P(0, 0) * _tmp41 + P(1, 0) * _tmp6 - P(10, 0) * _tmp11 - P(11, 0) * _tmp29 + - P(12, 0) * _tmp12 + P(2, 0) + P(3, 0) * _tmp30; - const Scalar _tmp48 = P(0, 2) * _tmp41 + P(1, 2) * _tmp6 - P(10, 2) * _tmp11 - P(11, 2) * _tmp29 + - P(12, 2) * _tmp12 + P(2, 2) + P(3, 2) * _tmp30; - const Scalar _tmp49 = _tmp21 + _tmp38; - const Scalar _tmp50 = P(0, 10) * _tmp31 + P(1, 10) * _tmp41 + P(10, 10) * _tmp10 - - P(11, 10) * _tmp12 - P(12, 10) * _tmp29 + P(2, 10) * _tmp3 + P(3, 10); - const Scalar _tmp51 = P(0, 11) * _tmp31 + P(1, 11) * _tmp41 + P(10, 11) * _tmp10 - - P(11, 11) * _tmp12 - P(12, 11) * _tmp29 + P(2, 11) * _tmp3 + P(3, 11); - const Scalar _tmp52 = P(0, 12) * _tmp31 + P(1, 12) * _tmp41 + P(10, 12) * _tmp10 - - P(11, 12) * _tmp12 - P(12, 12) * _tmp29 + P(2, 12) * _tmp3 + P(3, 12); - const Scalar _tmp53 = P(0, 2) * _tmp31 + P(1, 2) * _tmp41 + P(10, 2) * _tmp10 - - P(11, 2) * _tmp12 - P(12, 2) * _tmp29 + P(2, 2) * _tmp3 + P(3, 2); - const Scalar _tmp54 = P(0, 1) * _tmp31 + P(1, 1) * _tmp41 + P(10, 1) * _tmp10 - - P(11, 1) * _tmp12 - P(12, 1) * _tmp29 + P(2, 1) * _tmp3 + P(3, 1); - const Scalar _tmp55 = P(0, 0) * _tmp31 + P(1, 0) * _tmp41 + P(10, 0) * _tmp10 - - P(11, 0) * _tmp12 - P(12, 0) * _tmp29 + P(2, 0) * _tmp3 + P(3, 0); - const Scalar _tmp56 = P(0, 3) * _tmp31 + P(1, 3) * _tmp41 + P(10, 3) * _tmp10 - - P(11, 3) * _tmp12 - P(12, 3) * _tmp29 + P(2, 3) * _tmp3 + P(3, 3); - const Scalar _tmp57 = P(0, 13) + P(1, 13) * _tmp3 + P(10, 13) * _tmp12 + P(11, 13) * _tmp10 + - P(12, 13) * _tmp11 + P(2, 13) * _tmp9 + P(3, 13) * _tmp6; - const Scalar _tmp58 = -2 * _tmp23; - const Scalar _tmp59 = 1 - 2 * _tmp25; - const Scalar _tmp60 = _tmp58 + _tmp59; - const Scalar _tmp61 = _tmp60 * dt; - const Scalar _tmp62 = P(0, 15) + P(1, 15) * _tmp3 + P(10, 15) * _tmp12 + P(11, 15) * _tmp10 + - P(12, 15) * _tmp11 + P(2, 15) * _tmp9 + P(3, 15) * _tmp6; - const Scalar _tmp63 = 2 * state(0, 0); - const Scalar _tmp64 = _tmp63 * state(2, 0); - const Scalar _tmp65 = 2 * state(1, 0); - const Scalar _tmp66 = _tmp65 * state(3, 0); - const Scalar _tmp67 = _tmp64 + _tmp66; - const Scalar _tmp68 = _tmp67 * dt; - const Scalar _tmp69 = P(0, 14) + P(1, 14) * _tmp3 + P(10, 14) * _tmp12 + P(11, 14) * _tmp10 + - P(12, 14) * _tmp11 + P(2, 14) * _tmp9 + P(3, 14) * _tmp6; - const Scalar _tmp70 = _tmp63 * state(3, 0); - const Scalar _tmp71 = _tmp65 * state(2, 0); - const Scalar _tmp72 = -_tmp70 + _tmp71; - const Scalar _tmp73 = _tmp72 * dt; - const Scalar _tmp74 = d_vel(1, 0) - dt * state(14, 0); - const Scalar _tmp75 = 2 * state(3, 0); - const Scalar _tmp76 = _tmp74 * _tmp75; - const Scalar _tmp77 = d_vel(2, 0) - dt * state(15, 0); - const Scalar _tmp78 = 2 * state(2, 0); - const Scalar _tmp79 = _tmp77 * _tmp78; - const Scalar _tmp80 = -_tmp76 + _tmp79; - const Scalar _tmp81 = _tmp74 * _tmp78; - const Scalar _tmp82 = _tmp75 * _tmp77; - const Scalar _tmp83 = _tmp81 + _tmp82; - const Scalar _tmp84 = _tmp63 * _tmp74; - const Scalar _tmp85 = d_vel(0, 0) - dt * state(13, 0); - const Scalar _tmp86 = 4 * _tmp85; - const Scalar _tmp87 = _tmp65 * _tmp77; - const Scalar _tmp88 = -_tmp84 - _tmp86 * state(3, 0) + _tmp87; - const Scalar _tmp89 = _tmp65 * _tmp74; - const Scalar _tmp90 = _tmp63 * _tmp77; - const Scalar _tmp91 = -_tmp86 * state(2, 0) + _tmp89 + _tmp90; - const Scalar _tmp92 = P(0, 4) + P(1, 4) * _tmp3 + P(10, 4) * _tmp12 + P(11, 4) * _tmp10 + - P(12, 4) * _tmp11 + P(2, 4) * _tmp9 + P(3, 4) * _tmp6; - const Scalar _tmp93 = P(0, 13) * _tmp30 + P(1, 13) - P(10, 13) * _tmp29 + P(11, 13) * _tmp11 - - P(12, 13) * _tmp10 + P(2, 13) * _tmp31 + P(3, 13) * _tmp9; - const Scalar _tmp94 = P(0, 15) * _tmp30 + P(1, 15) - P(10, 15) * _tmp29 + P(11, 15) * _tmp11 - - P(12, 15) * _tmp10 + P(2, 15) * _tmp31 + P(3, 15) * _tmp9; - const Scalar _tmp95 = P(0, 14) * _tmp30 + P(1, 14) - P(10, 14) * _tmp29 + P(11, 14) * _tmp11 - - P(12, 14) * _tmp10 + P(2, 14) * _tmp31 + P(3, 14) * _tmp9; - const Scalar _tmp96 = P(0, 4) * _tmp30 + P(1, 4) - P(10, 4) * _tmp29 + P(11, 4) * _tmp11 - - P(12, 4) * _tmp10 + P(2, 4) * _tmp31 + P(3, 4) * _tmp9; - const Scalar _tmp97 = P(0, 15) * _tmp41 + P(1, 15) * _tmp6 - P(10, 15) * _tmp11 - - P(11, 15) * _tmp29 + P(12, 15) * _tmp12 + P(2, 15) + P(3, 15) * _tmp30; - const Scalar _tmp98 = P(0, 14) * _tmp41 + P(1, 14) * _tmp6 - P(10, 14) * _tmp11 - - P(11, 14) * _tmp29 + P(12, 14) * _tmp12 + P(2, 14) + P(3, 14) * _tmp30; - const Scalar _tmp99 = _tmp98 * dt; - const Scalar _tmp100 = P(0, 13) * _tmp41 + P(1, 13) * _tmp6 - P(10, 13) * _tmp11 - - P(11, 13) * _tmp29 + P(12, 13) * _tmp12 + P(2, 13) + P(3, 13) * _tmp30; - const Scalar _tmp101 = _tmp100 * dt; - const Scalar _tmp102 = P(0, 4) * _tmp41 + P(1, 4) * _tmp6 - P(10, 4) * _tmp11 - - P(11, 4) * _tmp29 + P(12, 4) * _tmp12 + P(2, 4) + P(3, 4) * _tmp30; - const Scalar _tmp103 = P(0, 15) * _tmp31 + P(1, 15) * _tmp41 + P(10, 15) * _tmp10 - - P(11, 15) * _tmp12 - P(12, 15) * _tmp29 + P(2, 15) * _tmp3 + P(3, 15); - const Scalar _tmp104 = P(0, 14) * _tmp31 + P(1, 14) * _tmp41 + P(10, 14) * _tmp10 - - P(11, 14) * _tmp12 - P(12, 14) * _tmp29 + P(2, 14) * _tmp3 + P(3, 14); - const Scalar _tmp105 = P(0, 13) * _tmp31 + P(1, 13) * _tmp41 + P(10, 13) * _tmp10 - - P(11, 13) * _tmp12 - P(12, 13) * _tmp29 + P(2, 13) * _tmp3 + P(3, 13); - const Scalar _tmp106 = P(0, 4) * _tmp31 + P(1, 4) * _tmp41 + P(10, 4) * _tmp10 - - P(11, 4) * _tmp12 - P(12, 4) * _tmp29 + P(2, 4) * _tmp3 + P(3, 4); - const Scalar _tmp107 = P(0, 1) * _tmp80 + P(1, 1) * _tmp83 - P(13, 1) * _tmp61 - - P(14, 1) * _tmp73 - P(15, 1) * _tmp68 + P(2, 1) * _tmp91 + - P(3, 1) * _tmp88 + P(4, 1); - const Scalar _tmp108 = P(0, 0) * _tmp80 + P(1, 0) * _tmp83 - P(13, 0) * _tmp61 - - P(14, 0) * _tmp73 - P(15, 0) * _tmp68 + P(2, 0) * _tmp91 + - P(3, 0) * _tmp88 + P(4, 0); - const Scalar _tmp109 = P(0, 13) * _tmp80 + P(1, 13) * _tmp83 - P(13, 13) * _tmp61 - - P(14, 13) * _tmp73 - P(15, 13) * _tmp68 + P(2, 13) * _tmp91 + - P(3, 13) * _tmp88 + P(4, 13); - const Scalar _tmp110 = P(0, 15) * _tmp80 + P(1, 15) * _tmp83 - P(13, 15) * _tmp61 - - P(14, 15) * _tmp73 - P(15, 15) * _tmp68 + P(2, 15) * _tmp91 + - P(3, 15) * _tmp88 + P(4, 15); - const Scalar _tmp111 = P(0, 14) * _tmp80 + P(1, 14) * _tmp83 - P(13, 14) * _tmp61 - - P(14, 14) * _tmp73 - P(15, 14) * _tmp68 + P(2, 14) * _tmp91 + - P(3, 14) * _tmp88 + P(4, 14); - const Scalar _tmp112 = P(0, 3) * _tmp80 + P(1, 3) * _tmp83 - P(13, 3) * _tmp61 - - P(14, 3) * _tmp73 - P(15, 3) * _tmp68 + P(2, 3) * _tmp91 + - P(3, 3) * _tmp88 + P(4, 3); - const Scalar _tmp113 = P(0, 2) * _tmp80 + P(1, 2) * _tmp83 - P(13, 2) * _tmp61 - - P(14, 2) * _tmp73 - P(15, 2) * _tmp68 + P(2, 2) * _tmp91 + - P(3, 2) * _tmp88 + P(4, 2); - const Scalar _tmp114 = P(0, 4) * _tmp80 + P(1, 4) * _tmp83 - P(13, 4) * _tmp61 - - P(14, 4) * _tmp73 - P(15, 4) * _tmp68 + P(2, 4) * _tmp91 + - P(3, 4) * _tmp88 + P(4, 4); - const Scalar _tmp115 = -2 * _tmp19; - const Scalar _tmp116 = _tmp115 + _tmp58 + 1; - const Scalar _tmp117 = _tmp116 * dt; - const Scalar _tmp118 = _tmp70 + _tmp71; - const Scalar _tmp119 = _tmp118 * dt; - const Scalar _tmp120 = _tmp65 * _tmp85; - const Scalar _tmp121 = _tmp120 + _tmp82; - const Scalar _tmp122 = _tmp75 * _tmp85; - const Scalar _tmp123 = _tmp122 - _tmp87; - const Scalar _tmp124 = 4 * _tmp74; - const Scalar _tmp125 = _tmp78 * _tmp85; - const Scalar _tmp126 = -_tmp124 * state(1, 0) + _tmp125 - _tmp90; - const Scalar _tmp127 = _tmp63 * _tmp85; - const Scalar _tmp128 = -_tmp124 * state(3, 0) + _tmp127 + _tmp79; - const Scalar _tmp129 = _tmp75 * state(2, 0); - const Scalar _tmp130 = _tmp63 * state(1, 0); - const Scalar _tmp131 = _tmp129 - _tmp130; - const Scalar _tmp132 = _tmp131 * dt; - const Scalar _tmp133 = P(0, 5) + P(1, 5) * _tmp3 + P(10, 5) * _tmp12 + P(11, 5) * _tmp10 + - P(12, 5) * _tmp11 + P(2, 5) * _tmp9 + P(3, 5) * _tmp6; - const Scalar _tmp134 = P(0, 5) * _tmp30 + P(1, 5) - P(10, 5) * _tmp29 + P(11, 5) * _tmp11 - - P(12, 5) * _tmp10 + P(2, 5) * _tmp31 + P(3, 5) * _tmp9; - const Scalar _tmp135 = P(0, 5) * _tmp41 + P(1, 5) * _tmp6 - P(10, 5) * _tmp11 - - P(11, 5) * _tmp29 + P(12, 5) * _tmp12 + P(2, 5) + P(3, 5) * _tmp30; - const Scalar _tmp136 = P(0, 5) * _tmp31 + P(1, 5) * _tmp41 + P(10, 5) * _tmp10 - - P(11, 5) * _tmp12 - P(12, 5) * _tmp29 + P(2, 5) * _tmp3 + P(3, 5); - const Scalar _tmp137 = _tmp118 * d_vel_var(0, 0); - const Scalar _tmp138 = _tmp72 * d_vel_var(1, 0); - const Scalar _tmp139 = _tmp67 * d_vel_var(2, 0); - const Scalar _tmp140 = P(0, 5) * _tmp80 + P(1, 5) * _tmp83 - P(13, 5) * _tmp61 - - P(14, 5) * _tmp73 - P(15, 5) * _tmp68 + P(2, 5) * _tmp91 + - P(3, 5) * _tmp88 + P(4, 5); - const Scalar _tmp141 = P(0, 0) * _tmp123 + P(1, 0) * _tmp126 - P(13, 0) * _tmp119 - - P(14, 0) * _tmp117 - P(15, 0) * _tmp132 + P(2, 0) * _tmp121 + - P(3, 0) * _tmp128 + P(5, 0); - const Scalar _tmp142 = P(0, 2) * _tmp123 + P(1, 2) * _tmp126 - P(13, 2) * _tmp119 - - P(14, 2) * _tmp117 - P(15, 2) * _tmp132 + P(2, 2) * _tmp121 + - P(3, 2) * _tmp128 + P(5, 2); - const Scalar _tmp143 = P(0, 14) * _tmp123 + P(1, 14) * _tmp126 - P(13, 14) * _tmp119 - - P(14, 14) * _tmp117 - P(15, 14) * _tmp132 + P(2, 14) * _tmp121 + - P(3, 14) * _tmp128 + P(5, 14); - const Scalar _tmp144 = P(0, 15) * _tmp123 + P(1, 15) * _tmp126 - P(13, 15) * _tmp119 - - P(14, 15) * _tmp117 - P(15, 15) * _tmp132 + P(2, 15) * _tmp121 + - P(3, 15) * _tmp128 + P(5, 15); - const Scalar _tmp145 = P(0, 3) * _tmp123 + P(1, 3) * _tmp126 - P(13, 3) * _tmp119 - - P(14, 3) * _tmp117 - P(15, 3) * _tmp132 + P(2, 3) * _tmp121 + - P(3, 3) * _tmp128 + P(5, 3); - const Scalar _tmp146 = P(0, 1) * _tmp123 + P(1, 1) * _tmp126 - P(13, 1) * _tmp119 - - P(14, 1) * _tmp117 - P(15, 1) * _tmp132 + P(2, 1) * _tmp121 + - P(3, 1) * _tmp128 + P(5, 1); - const Scalar _tmp147 = P(0, 13) * _tmp123 + P(1, 13) * _tmp126 - P(13, 13) * _tmp119 - - P(14, 13) * _tmp117 - P(15, 13) * _tmp132 + P(2, 13) * _tmp121 + - P(3, 13) * _tmp128 + P(5, 13); - const Scalar _tmp148 = _tmp147 * dt; - const Scalar _tmp149 = P(0, 5) * _tmp123 + P(1, 5) * _tmp126 - P(13, 5) * _tmp119 - - P(14, 5) * _tmp117 - P(15, 5) * _tmp132 + P(2, 5) * _tmp121 + - P(3, 5) * _tmp128 + P(5, 5); - const Scalar _tmp150 = _tmp115 + _tmp59; - const Scalar _tmp151 = _tmp150 * dt; - const Scalar _tmp152 = -_tmp64 + _tmp66; - const Scalar _tmp153 = _tmp152 * dt; - const Scalar _tmp154 = -_tmp125 + _tmp89; - const Scalar _tmp155 = _tmp120 + _tmp81; - const Scalar _tmp156 = 4 * _tmp77; - const Scalar _tmp157 = _tmp122 - _tmp156 * state(1, 0) + _tmp84; - const Scalar _tmp158 = -_tmp127 - _tmp156 * state(2, 0) + _tmp76; - const Scalar _tmp159 = _tmp129 + _tmp130; - const Scalar _tmp160 = _tmp159 * dt; - const Scalar _tmp161 = P(11, 6) * _tmp1; - const Scalar _tmp162 = P(0, 6) + P(1, 6) * _tmp3 + P(10, 6) * _tmp12 + P(12, 6) * _tmp11 + - P(2, 6) * _tmp9 + P(3, 6) * _tmp6 + _tmp161 * state(2, 0); - const Scalar _tmp163 = P(0, 6) * _tmp30 + P(1, 6) - P(10, 6) * _tmp29 - P(12, 6) * _tmp10 + - P(2, 6) * _tmp31 + P(3, 6) * _tmp9 + _tmp161 * state(3, 0); - const Scalar _tmp164 = P(0, 6) * _tmp41 + P(1, 6) * _tmp6 - P(10, 6) * _tmp11 + - P(12, 6) * _tmp12 + P(2, 6) + P(3, 6) * _tmp30 - _tmp161 * state(0, 0); - const Scalar _tmp165 = P(0, 6) * _tmp31 + P(1, 6) * _tmp41 + P(10, 6) * _tmp10 - - P(11, 6) * _tmp12 - P(12, 6) * _tmp29 + P(2, 6) * _tmp3 + P(3, 6); - const Scalar _tmp166 = P(0, 6) * _tmp80 + P(1, 6) * _tmp83 - P(13, 6) * _tmp61 - - P(14, 6) * _tmp73 - P(15, 6) * _tmp68 + P(2, 6) * _tmp91 + - P(3, 6) * _tmp88 + P(4, 6); - const Scalar _tmp167 = P(0, 6) * _tmp123 + P(1, 6) * _tmp126 - P(13, 6) * _tmp119 - - P(14, 6) * _tmp117 - P(15, 6) * _tmp132 + P(2, 6) * _tmp121 + - P(3, 6) * _tmp128 + P(5, 6); - const Scalar _tmp168 = P(0, 13) * _tmp154 + P(1, 13) * _tmp157 - P(13, 13) * _tmp153 - - P(14, 13) * _tmp160 - P(15, 13) * _tmp151 + P(2, 13) * _tmp158 + - P(3, 13) * _tmp155 + P(6, 13); - const Scalar _tmp169 = P(0, 15) * _tmp154 + P(1, 15) * _tmp157 - P(13, 15) * _tmp153 - - P(14, 15) * _tmp160 - P(15, 15) * _tmp151 + P(2, 15) * _tmp158 + - P(3, 15) * _tmp155 + P(6, 15); - const Scalar _tmp170 = P(0, 14) * _tmp154 + P(1, 14) * _tmp157 - P(13, 14) * _tmp153 - - P(14, 14) * _tmp160 - P(15, 14) * _tmp151 + P(2, 14) * _tmp158 + - P(3, 14) * _tmp155 + P(6, 14); - const Scalar _tmp171 = P(0, 6) * _tmp154 + P(1, 6) * _tmp157 - P(13, 6) * _tmp153 - - P(14, 6) * _tmp160 - P(15, 6) * _tmp151 + P(2, 6) * _tmp158 + - P(3, 6) * _tmp155 + P(6, 6); + const Scalar _tmp30 = _tmp21 * state(1, 0); + const Scalar _tmp31 = _tmp7 - _tmp8; + const Scalar _tmp32 = _tmp4 - _tmp5; + const Scalar _tmp33 = P(0, 12) * _tmp31 + P(1, 12) - P(10, 12) * _tmp29 + P(11, 12) * _tmp11 - + P(12, 12) * _tmp12 + P(2, 12) * _tmp32 + P(3, 12) * _tmp3; + const Scalar _tmp34 = P(0, 10) * _tmp31 + P(1, 10) - P(10, 10) * _tmp29 + P(11, 10) * _tmp11 - + P(12, 10) * _tmp12 + P(2, 10) * _tmp32 + P(3, 10) * _tmp3; + const Scalar _tmp35 = P(0, 11) * _tmp31 + P(1, 11) - P(10, 11) * _tmp29 + P(11, 11) * _tmp11 - + P(12, 11) * _tmp12 + P(2, 11) * _tmp32 + P(3, 11) * _tmp3; + const Scalar _tmp36 = P(0, 2) * _tmp31 + P(1, 2) - P(10, 2) * _tmp29 + P(11, 2) * _tmp11 - + P(12, 2) * _tmp12 + P(2, 2) * _tmp32 + P(3, 2) * _tmp3; + const Scalar _tmp37 = P(0, 3) * _tmp31 + P(1, 3) - P(10, 3) * _tmp29 + P(11, 3) * _tmp11 - + P(12, 3) * _tmp12 + P(2, 3) * _tmp32 + P(3, 3) * _tmp3; + const Scalar _tmp38 = P(0, 0) * _tmp31 + P(1, 0) - P(10, 0) * _tmp29 + P(11, 0) * _tmp11 - + P(12, 0) * _tmp12 + P(2, 0) * _tmp32 + P(3, 0) * _tmp3; + const Scalar _tmp39 = P(0, 1) * _tmp31 + P(1, 1) - P(10, 1) * _tmp29 - P(12, 1) * _tmp12 + + P(2, 1) * _tmp32 + P(3, 1) * _tmp3 + _tmp18 * state(3, 0); + const Scalar _tmp40 = _tmp21 * std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp41 = _tmp22 + _tmp40; + const Scalar _tmp42 = _tmp21 * state(0, 0); + const Scalar _tmp43 = _tmp0 - _tmp2; + const Scalar _tmp44 = P(0, 0) * _tmp43 + P(1, 0) * _tmp6 - P(10, 0) * _tmp11 - P(11, 0) * _tmp29 + + P(12, 0) * _tmp10 + P(2, 0) + P(3, 0) * _tmp31; + const Scalar _tmp45 = P(0, 1) * _tmp43 + P(1, 1) * _tmp6 - P(10, 1) * _tmp11 + P(12, 1) * _tmp10 + + P(2, 1) + P(3, 1) * _tmp31 - _tmp18 * state(0, 0); + const Scalar _tmp46 = P(0, 3) * _tmp43 + P(1, 3) * _tmp6 - P(10, 3) * _tmp11 - P(11, 3) * _tmp29 + + P(12, 3) * _tmp10 + P(2, 3) + P(3, 3) * _tmp31; + const Scalar _tmp47 = P(0, 11) * _tmp43 + P(1, 11) * _tmp6 - P(10, 11) * _tmp11 - + P(11, 11) * _tmp29 + P(12, 11) * _tmp10 + P(2, 11) + P(3, 11) * _tmp31; + const Scalar _tmp48 = P(0, 10) * _tmp43 + P(1, 10) * _tmp6 - P(10, 10) * _tmp11 - + P(11, 10) * _tmp29 + P(12, 10) * _tmp10 + P(2, 10) + P(3, 10) * _tmp31; + const Scalar _tmp49 = P(0, 12) * _tmp43 + P(1, 12) * _tmp6 - P(10, 12) * _tmp11 - + P(11, 12) * _tmp29 + P(12, 12) * _tmp10 + P(2, 12) + P(3, 12) * _tmp31; + const Scalar _tmp50 = P(0, 2) * _tmp43 + P(1, 2) * _tmp6 - P(10, 2) * _tmp11 - P(11, 2) * _tmp29 + + P(12, 2) * _tmp10 + P(2, 2) + P(3, 2) * _tmp31; + const Scalar _tmp51 = P(0, 2) * _tmp32 + P(1, 2) * _tmp43 + P(10, 2) * _tmp12 - + P(11, 2) * _tmp10 - P(12, 2) * _tmp29 + P(2, 2) * _tmp9 + P(3, 2); + const Scalar _tmp52 = P(0, 1) * _tmp32 + P(1, 1) * _tmp43 + P(10, 1) * _tmp12 - + P(11, 1) * _tmp10 - P(12, 1) * _tmp29 + P(2, 1) * _tmp9 + P(3, 1); + const Scalar _tmp53 = P(0, 0) * _tmp32 + P(1, 0) * _tmp43 + P(10, 0) * _tmp12 - + P(11, 0) * _tmp10 - P(12, 0) * _tmp29 + P(2, 0) * _tmp9 + P(3, 0); + const Scalar _tmp54 = P(0, 11) * _tmp32 + P(1, 11) * _tmp43 + P(10, 11) * _tmp12 - + P(11, 11) * _tmp10 - P(12, 11) * _tmp29 + P(2, 11) * _tmp9 + P(3, 11); + const Scalar _tmp55 = P(0, 12) * _tmp32 + P(1, 12) * _tmp43 + P(10, 12) * _tmp12 - + P(11, 12) * _tmp10 - P(12, 12) * _tmp29 + P(2, 12) * _tmp9 + P(3, 12); + const Scalar _tmp56 = P(0, 10) * _tmp32 + P(1, 10) * _tmp43 + P(10, 10) * _tmp12 - + P(11, 10) * _tmp10 - P(12, 10) * _tmp29 + P(2, 10) * _tmp9 + P(3, 10); + const Scalar _tmp57 = P(0, 3) * _tmp32 + P(1, 3) * _tmp43 + P(10, 3) * _tmp12 - + P(11, 3) * _tmp10 - P(12, 3) * _tmp29 + P(2, 3) * _tmp9 + P(3, 3); + const Scalar _tmp58 = d_vel(2, 0) - d_vel_dt * state(15, 0); + const Scalar _tmp59 = 2 * state(2, 0); + const Scalar _tmp60 = _tmp58 * _tmp59; + const Scalar _tmp61 = d_vel(1, 0) - d_vel_dt * state(14, 0); + const Scalar _tmp62 = 2 * _tmp61; + const Scalar _tmp63 = _tmp62 * state(3, 0); + const Scalar _tmp64 = _tmp60 - _tmp63; + const Scalar _tmp65 = P(0, 13) + P(1, 13) * _tmp9 + P(10, 13) * _tmp10 + P(11, 13) * _tmp12 + + P(12, 13) * _tmp11 + P(2, 13) * _tmp3 + P(3, 13) * _tmp6; + const Scalar _tmp66 = -2 * _tmp20; + const Scalar _tmp67 = 1 - 2 * _tmp24; + const Scalar _tmp68 = _tmp66 + _tmp67; + const Scalar _tmp69 = _tmp68 * d_vel_dt; + const Scalar _tmp70 = P(0, 15) + P(1, 15) * _tmp9 + P(10, 15) * _tmp10 + P(11, 15) * _tmp12 + + P(12, 15) * _tmp11 + P(2, 15) * _tmp3 + P(3, 15) * _tmp6; + const Scalar _tmp71 = _tmp59 * state(0, 0); + const Scalar _tmp72 = 2 * state(1, 0); + const Scalar _tmp73 = _tmp72 * state(3, 0); + const Scalar _tmp74 = _tmp71 + _tmp73; + const Scalar _tmp75 = _tmp74 * d_vel_dt; + const Scalar _tmp76 = P(0, 14) + P(1, 14) * _tmp9 + P(10, 14) * _tmp10 + P(11, 14) * _tmp12 + + P(12, 14) * _tmp11 + P(2, 14) * _tmp3 + P(3, 14) * _tmp6; + const Scalar _tmp77 = 2 * state(0, 0); + const Scalar _tmp78 = _tmp77 * state(3, 0); + const Scalar _tmp79 = _tmp59 * state(1, 0); + const Scalar _tmp80 = -_tmp78 + _tmp79; + const Scalar _tmp81 = _tmp80 * d_vel_dt; + const Scalar _tmp82 = 2 * _tmp58; + const Scalar _tmp83 = _tmp82 * state(3, 0); + const Scalar _tmp84 = _tmp59 * _tmp61; + const Scalar _tmp85 = _tmp83 + _tmp84; + const Scalar _tmp86 = d_vel(0, 0) - d_vel_dt * state(13, 0); + const Scalar _tmp87 = 4 * _tmp86; + const Scalar _tmp88 = _tmp82 * state(0, 0); + const Scalar _tmp89 = _tmp62 * state(1, 0); + const Scalar _tmp90 = -_tmp87 * state(2, 0) + _tmp88 + _tmp89; + const Scalar _tmp91 = _tmp82 * state(1, 0); + const Scalar _tmp92 = _tmp62 * state(0, 0); + const Scalar _tmp93 = -_tmp87 * state(3, 0) + _tmp91 - _tmp92; + const Scalar _tmp94 = P(11, 4) * _tmp1; + const Scalar _tmp95 = P(0, 4) + P(1, 4) * _tmp9 + P(10, 4) * _tmp10 + P(12, 4) * _tmp11 + + P(2, 4) * _tmp3 + P(3, 4) * _tmp6 + _tmp94 * state(2, 0); + const Scalar _tmp96 = P(0, 13) * _tmp31 + P(1, 13) - P(10, 13) * _tmp29 + P(11, 13) * _tmp11 - + P(12, 13) * _tmp12 + P(2, 13) * _tmp32 + P(3, 13) * _tmp3; + const Scalar _tmp97 = P(0, 14) * _tmp31 + P(1, 14) - P(10, 14) * _tmp29 + P(11, 14) * _tmp11 - + P(12, 14) * _tmp12 + P(2, 14) * _tmp32 + P(3, 14) * _tmp3; + const Scalar _tmp98 = P(0, 15) * _tmp31 + P(1, 15) - P(10, 15) * _tmp29 + P(11, 15) * _tmp11 - + P(12, 15) * _tmp12 + P(2, 15) * _tmp32 + P(3, 15) * _tmp3; + const Scalar _tmp99 = P(0, 4) * _tmp31 + P(1, 4) - P(10, 4) * _tmp29 - P(12, 4) * _tmp12 + + P(2, 4) * _tmp32 + P(3, 4) * _tmp3 + _tmp94 * state(3, 0); + const Scalar _tmp100 = P(0, 13) * _tmp43 + P(1, 13) * _tmp6 - P(10, 13) * _tmp11 - + P(11, 13) * _tmp29 + P(12, 13) * _tmp10 + P(2, 13) + P(3, 13) * _tmp31; + const Scalar _tmp101 = P(0, 14) * _tmp43 + P(1, 14) * _tmp6 - P(10, 14) * _tmp11 - + P(11, 14) * _tmp29 + P(12, 14) * _tmp10 + P(2, 14) + P(3, 14) * _tmp31; + const Scalar _tmp102 = P(0, 15) * _tmp43 + P(1, 15) * _tmp6 - P(10, 15) * _tmp11 - + P(11, 15) * _tmp29 + P(12, 15) * _tmp10 + P(2, 15) + P(3, 15) * _tmp31; + const Scalar _tmp103 = P(0, 4) * _tmp43 + P(1, 4) * _tmp6 - P(10, 4) * _tmp11 + + P(12, 4) * _tmp10 + P(2, 4) + P(3, 4) * _tmp31 - _tmp94 * state(0, 0); + const Scalar _tmp104 = P(0, 13) * _tmp32 + P(1, 13) * _tmp43 + P(10, 13) * _tmp12 - + P(11, 13) * _tmp10 - P(12, 13) * _tmp29 + P(2, 13) * _tmp9 + P(3, 13); + const Scalar _tmp105 = P(0, 14) * _tmp32 + P(1, 14) * _tmp43 + P(10, 14) * _tmp12 - + P(11, 14) * _tmp10 - P(12, 14) * _tmp29 + P(2, 14) * _tmp9 + P(3, 14); + const Scalar _tmp106 = P(0, 15) * _tmp32 + P(1, 15) * _tmp43 + P(10, 15) * _tmp12 - + P(11, 15) * _tmp10 - P(12, 15) * _tmp29 + P(2, 15) * _tmp9 + P(3, 15); + const Scalar _tmp107 = P(0, 4) * _tmp32 + P(1, 4) * _tmp43 + P(10, 4) * _tmp12 - + P(12, 4) * _tmp29 + P(2, 4) * _tmp9 + P(3, 4) - _tmp94 * state(1, 0); + const Scalar _tmp108 = P(0, 13) * _tmp64 + P(1, 13) * _tmp85 - P(13, 13) * _tmp69 - + P(14, 13) * _tmp81 - P(15, 13) * _tmp75 + P(2, 13) * _tmp90 + + P(3, 13) * _tmp93 + P(4, 13); + const Scalar _tmp109 = P(0, 14) * _tmp64 + P(1, 14) * _tmp85 - P(13, 14) * _tmp69 - + P(14, 14) * _tmp81 - P(15, 14) * _tmp75 + P(2, 14) * _tmp90 + + P(3, 14) * _tmp93 + P(4, 14); + const Scalar _tmp110 = P(0, 15) * _tmp64 + P(1, 15) * _tmp85 - P(13, 15) * _tmp69 - + P(14, 15) * _tmp81 - P(15, 15) * _tmp75 + P(2, 15) * _tmp90 + + P(3, 15) * _tmp93 + P(4, 15); + const Scalar _tmp111 = P(0, 1) * _tmp64 + P(1, 1) * _tmp85 - P(13, 1) * _tmp69 - + P(14, 1) * _tmp81 - P(15, 1) * _tmp75 + P(2, 1) * _tmp90 + + P(3, 1) * _tmp93 + P(4, 1); + const Scalar _tmp112 = P(0, 0) * _tmp64 + P(1, 0) * _tmp85 - P(13, 0) * _tmp69 - + P(14, 0) * _tmp81 - P(15, 0) * _tmp75 + P(2, 0) * _tmp90 + + P(3, 0) * _tmp93 + P(4, 0); + const Scalar _tmp113 = P(0, 3) * _tmp64 + P(1, 3) * _tmp85 - P(13, 3) * _tmp69 - + P(14, 3) * _tmp81 - P(15, 3) * _tmp75 + P(2, 3) * _tmp90 + + P(3, 3) * _tmp93 + P(4, 3); + const Scalar _tmp114 = P(0, 2) * _tmp64 + P(1, 2) * _tmp85 - P(13, 2) * _tmp69 - + P(14, 2) * _tmp81 - P(15, 2) * _tmp75 + P(2, 2) * _tmp90 + + P(3, 2) * _tmp93 + P(4, 2); + const Scalar _tmp115 = P(0, 4) * _tmp64 + P(1, 4) * _tmp85 - P(13, 4) * _tmp69 - + P(14, 4) * _tmp81 - P(15, 4) * _tmp75 + P(2, 4) * _tmp90 + + P(3, 4) * _tmp93 + P(4, 4); + const Scalar _tmp116 = _tmp59 * state(3, 0); + const Scalar _tmp117 = _tmp72 * state(0, 0); + const Scalar _tmp118 = _tmp116 - _tmp117; + const Scalar _tmp119 = _tmp118 * d_vel_dt; + const Scalar _tmp120 = -2 * _tmp26; + const Scalar _tmp121 = _tmp120 + _tmp67; + const Scalar _tmp122 = _tmp121 * d_vel_dt; + const Scalar _tmp123 = _tmp78 + _tmp79; + const Scalar _tmp124 = _tmp123 * d_vel_dt; + const Scalar _tmp125 = _tmp59 * _tmp86; + const Scalar _tmp126 = 4 * _tmp61; + const Scalar _tmp127 = _tmp125 - _tmp126 * state(1, 0) - _tmp88; + const Scalar _tmp128 = _tmp77 * _tmp86; + const Scalar _tmp129 = -_tmp126 * state(3, 0) + _tmp128 + _tmp60; + const Scalar _tmp130 = 2 * _tmp86 * state(3, 0); + const Scalar _tmp131 = _tmp130 - _tmp91; + const Scalar _tmp132 = _tmp72 * _tmp86; + const Scalar _tmp133 = _tmp132 + _tmp83; + const Scalar _tmp134 = P(0, 5) + P(1, 5) * _tmp9 + P(10, 5) * _tmp10 + P(11, 5) * _tmp12 + + P(12, 5) * _tmp11 + P(2, 5) * _tmp3 + P(3, 5) * _tmp6; + const Scalar _tmp135 = P(0, 5) * _tmp31 + P(1, 5) - P(10, 5) * _tmp29 + P(11, 5) * _tmp11 - + P(12, 5) * _tmp12 + P(2, 5) * _tmp32 + P(3, 5) * _tmp3; + const Scalar _tmp136 = P(0, 5) * _tmp43 + P(1, 5) * _tmp6 - P(10, 5) * _tmp11 - + P(11, 5) * _tmp29 + P(12, 5) * _tmp10 + P(2, 5) + P(3, 5) * _tmp31; + const Scalar _tmp137 = P(0, 5) * _tmp32 + P(1, 5) * _tmp43 + P(10, 5) * _tmp12 - + P(11, 5) * _tmp10 - P(12, 5) * _tmp29 + P(2, 5) * _tmp9 + P(3, 5); + const Scalar _tmp138 = _tmp123 * d_vel_var(0, 0); + const Scalar _tmp139 = _tmp80 * d_vel_var(1, 0); + const Scalar _tmp140 = P(0, 5) * _tmp64 + P(1, 5) * _tmp85 - P(13, 5) * _tmp69 - + P(14, 5) * _tmp81 - P(15, 5) * _tmp75 + P(2, 5) * _tmp90 + + P(3, 5) * _tmp93 + P(4, 5); + const Scalar _tmp141 = P(0, 13) * _tmp131 + P(1, 13) * _tmp127 - P(13, 13) * _tmp124 - + P(14, 13) * _tmp122 - P(15, 13) * _tmp119 + P(2, 13) * _tmp133 + + P(3, 13) * _tmp129 + P(5, 13); + const Scalar _tmp142 = P(0, 14) * _tmp131 + P(1, 14) * _tmp127 - P(13, 14) * _tmp124 - + P(14, 14) * _tmp122 - P(15, 14) * _tmp119 + P(2, 14) * _tmp133 + + P(3, 14) * _tmp129 + P(5, 14); + const Scalar _tmp143 = P(0, 15) * _tmp131 + P(1, 15) * _tmp127 - P(13, 15) * _tmp124 - + P(14, 15) * _tmp122 - P(15, 15) * _tmp119 + P(2, 15) * _tmp133 + + P(3, 15) * _tmp129 + P(5, 15); + const Scalar _tmp144 = P(0, 2) * _tmp131 + P(1, 2) * _tmp127 - P(13, 2) * _tmp124 - + P(14, 2) * _tmp122 - P(15, 2) * _tmp119 + P(2, 2) * _tmp133 + + P(3, 2) * _tmp129 + P(5, 2); + const Scalar _tmp145 = P(0, 0) * _tmp131 + P(1, 0) * _tmp127 - P(13, 0) * _tmp124 - + P(14, 0) * _tmp122 - P(15, 0) * _tmp119 + P(2, 0) * _tmp133 + + P(3, 0) * _tmp129 + P(5, 0); + const Scalar _tmp146 = P(0, 3) * _tmp131 + P(1, 3) * _tmp127 - P(13, 3) * _tmp124 - + P(14, 3) * _tmp122 - P(15, 3) * _tmp119 + P(2, 3) * _tmp133 + + P(3, 3) * _tmp129 + P(5, 3); + const Scalar _tmp147 = P(0, 1) * _tmp131 + P(1, 1) * _tmp127 - P(13, 1) * _tmp124 - + P(14, 1) * _tmp122 - P(15, 1) * _tmp119 + P(2, 1) * _tmp133 + + P(3, 1) * _tmp129 + P(5, 1); + const Scalar _tmp148 = P(0, 5) * _tmp131 + P(1, 5) * _tmp127 - P(13, 5) * _tmp124 - + P(14, 5) * _tmp122 - P(15, 5) * _tmp119 + P(2, 5) * _tmp133 + + P(3, 5) * _tmp129 + P(5, 5); + const Scalar _tmp149 = _tmp116 + _tmp117; + const Scalar _tmp150 = _tmp149 * d_vel_dt; + const Scalar _tmp151 = _tmp120 + _tmp66 + 1; + const Scalar _tmp152 = _tmp151 * d_vel_dt; + const Scalar _tmp153 = -_tmp71 + _tmp73; + const Scalar _tmp154 = _tmp153 * d_vel_dt; + const Scalar _tmp155 = 4 * _tmp58; + const Scalar _tmp156 = _tmp130 - _tmp155 * state(1, 0) + _tmp92; + const Scalar _tmp157 = -_tmp128 - _tmp155 * state(2, 0) + _tmp63; + const Scalar _tmp158 = -_tmp125 + _tmp89; + const Scalar _tmp159 = _tmp132 + _tmp84; + const Scalar _tmp160 = P(11, 6) * _tmp1; + const Scalar _tmp161 = P(0, 6) + P(1, 6) * _tmp9 + P(10, 6) * _tmp10 + P(12, 6) * _tmp11 + + P(2, 6) * _tmp3 + P(3, 6) * _tmp6 + _tmp160 * state(2, 0); + const Scalar _tmp162 = P(0, 6) * _tmp31 + P(1, 6) - P(10, 6) * _tmp29 - P(12, 6) * _tmp12 + + P(2, 6) * _tmp32 + P(3, 6) * _tmp3 + _tmp160 * state(3, 0); + const Scalar _tmp163 = P(0, 6) * _tmp43 + P(1, 6) * _tmp6 - P(10, 6) * _tmp11 + + P(12, 6) * _tmp10 + P(2, 6) + P(3, 6) * _tmp31 - _tmp160 * state(0, 0); + const Scalar _tmp164 = P(0, 6) * _tmp32 + P(1, 6) * _tmp43 + P(10, 6) * _tmp12 - + P(11, 6) * _tmp10 - P(12, 6) * _tmp29 + P(2, 6) * _tmp9 + P(3, 6); + const Scalar _tmp165 = _tmp151 * d_vel_var(2, 0); + const Scalar _tmp166 = P(0, 6) * _tmp64 + P(1, 6) * _tmp85 - P(13, 6) * _tmp69 - + P(14, 6) * _tmp81 - P(15, 6) * _tmp75 + P(2, 6) * _tmp90 + + P(3, 6) * _tmp93 + P(4, 6); + const Scalar _tmp167 = P(0, 6) * _tmp131 + P(1, 6) * _tmp127 - P(13, 6) * _tmp124 - + P(14, 6) * _tmp122 - P(15, 6) * _tmp119 + P(2, 6) * _tmp133 + + P(3, 6) * _tmp129 + P(5, 6); + const Scalar _tmp168 = P(0, 13) * _tmp158 + P(1, 13) * _tmp156 - P(13, 13) * _tmp154 - + P(14, 13) * _tmp150 - P(15, 13) * _tmp152 + P(2, 13) * _tmp157 + + P(3, 13) * _tmp159 + P(6, 13); + const Scalar _tmp169 = P(0, 14) * _tmp158 + P(1, 14) * _tmp156 - P(13, 14) * _tmp154 - + P(14, 14) * _tmp150 - P(15, 14) * _tmp152 + P(2, 14) * _tmp157 + + P(3, 14) * _tmp159 + P(6, 14); + const Scalar _tmp170 = P(0, 15) * _tmp158 + P(1, 15) * _tmp156 - P(13, 15) * _tmp154 - + P(14, 15) * _tmp150 - P(15, 15) * _tmp152 + P(2, 15) * _tmp157 + + P(3, 15) * _tmp159 + P(6, 15); + const Scalar _tmp171 = P(0, 6) * _tmp158 + P(1, 6) * _tmp156 - P(13, 6) * _tmp154 - + P(14, 6) * _tmp150 - P(15, 6) * _tmp152 + P(2, 6) * _tmp157 + + P(3, 6) * _tmp159 + P(6, 6); const Scalar _tmp172 = P(11, 8) * _tmp1; - const Scalar _tmp173 = P(11, 9) * _tmp1; // Output terms (1) if (P_new != nullptr) { matrix::Matrix& _p_new = (*P_new); - _p_new(0, 0) = _tmp10 * _tmp14 + _tmp11 * _tmp13 + _tmp12 * _tmp15 + _tmp16 * _tmp9 + - _tmp17 * _tmp3 + _tmp18 * _tmp6 + _tmp21 + _tmp22 + _tmp27; + _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; @@ -341,10 +342,10 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 0) = 0; _p_new(22, 0) = 0; _p_new(23, 0) = 0; - _p_new(0, 1) = -_tmp10 * _tmp13 + _tmp11 * _tmp14 - _tmp15 * _tmp29 + _tmp16 * _tmp31 + _tmp17 + - _tmp18 * _tmp9 + _tmp22 * _tmp30 - _tmp28 * state(0, 0); - _p_new(1, 1) = -_tmp10 * _tmp34 + _tmp11 * _tmp33 + _tmp27 - _tmp29 * _tmp32 + _tmp30 * _tmp35 + - _tmp31 * _tmp37 + _tmp36 * _tmp9 + _tmp38 + _tmp39; + _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; @@ -367,12 +368,12 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 1) = 0; _p_new(22, 1) = 0; _p_new(23, 1) = 0; - _p_new(0, 2) = -_tmp11 * _tmp15 + _tmp12 * _tmp13 - _tmp14 * _tmp29 + _tmp16 + _tmp17 * _tmp6 + - _tmp18 * _tmp30 + _tmp22 * _tmp41 - _tmp40 * state(2, 0); - _p_new(1, 2) = -_tmp11 * _tmp32 + _tmp12 * _tmp34 - _tmp28 * state(2, 0) - _tmp29 * _tmp33 + - _tmp30 * _tmp36 + _tmp35 * _tmp41 + _tmp37 + _tmp39 * _tmp6; - _p_new(2, 2) = -_tmp11 * _tmp43 + _tmp12 * _tmp44 + _tmp24 - _tmp29 * _tmp42 + _tmp30 * _tmp45 + - _tmp41 * _tmp47 + _tmp46 * _tmp6 + _tmp48 + _tmp49; + _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; @@ -394,14 +395,14 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 2) = 0; _p_new(22, 2) = 0; _p_new(23, 2) = 0; - _p_new(0, 3) = _tmp10 * _tmp15 - _tmp12 * _tmp14 - _tmp13 * _tmp29 + _tmp16 * _tmp3 + - _tmp17 * _tmp41 + _tmp18 + _tmp22 * _tmp31 - _tmp40 * state(3, 0); - _p_new(1, 3) = _tmp10 * _tmp32 - _tmp12 * _tmp33 - _tmp28 * state(3, 0) - _tmp29 * _tmp34 + - _tmp3 * _tmp37 + _tmp31 * _tmp35 + _tmp36 + _tmp39 * _tmp41; - _p_new(2, 3) = _tmp10 * _tmp43 - _tmp12 * _tmp42 - _tmp20 * state(2, 0) * state(3, 0) - - _tmp29 * _tmp44 + _tmp3 * _tmp48 + _tmp31 * _tmp47 + _tmp41 * _tmp46 + _tmp45; - _p_new(3, 3) = _tmp10 * _tmp50 - _tmp12 * _tmp51 + _tmp26 - _tmp29 * _tmp52 + _tmp3 * _tmp53 + - _tmp31 * _tmp55 + _tmp41 * _tmp54 + _tmp49 + _tmp56; + _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; @@ -422,19 +423,19 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 3) = 0; _p_new(22, 3) = 0; _p_new(23, 3) = 0; - _p_new(0, 4) = _tmp16 * _tmp91 + _tmp17 * _tmp83 + _tmp18 * _tmp88 + _tmp22 * _tmp80 - - _tmp57 * _tmp61 - _tmp62 * _tmp68 - _tmp69 * _tmp73 + _tmp92; - _p_new(1, 4) = _tmp35 * _tmp80 + _tmp36 * _tmp88 + _tmp37 * _tmp91 + _tmp39 * _tmp83 - - _tmp61 * _tmp93 - _tmp68 * _tmp94 - _tmp73 * _tmp95 + _tmp96; - _p_new(2, 4) = -_tmp101 * _tmp60 + _tmp102 + _tmp45 * _tmp88 + _tmp46 * _tmp83 + - _tmp47 * _tmp80 + _tmp48 * _tmp91 - _tmp68 * _tmp97 - _tmp72 * _tmp99; - _p_new(3, 4) = -_tmp103 * _tmp68 - _tmp104 * _tmp73 - _tmp105 * _tmp61 + _tmp106 + - _tmp53 * _tmp91 + _tmp54 * _tmp83 + _tmp55 * _tmp80 + _tmp56 * _tmp88; - _p_new(4, 4) = _tmp107 * _tmp83 + _tmp108 * _tmp80 - _tmp109 * _tmp61 - _tmp110 * _tmp68 - - _tmp111 * _tmp73 + _tmp112 * _tmp88 + _tmp113 * _tmp91 + _tmp114 + - std::pow(_tmp60, Scalar(2)) * d_vel_var(0, 0) + - std::pow(_tmp67, Scalar(2)) * d_vel_var(2, 0) + - std::pow(_tmp72, Scalar(2)) * d_vel_var(1, 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; @@ -454,21 +455,23 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 4) = 0; _p_new(22, 4) = 0; _p_new(23, 4) = 0; - _p_new(0, 5) = -_tmp117 * _tmp69 - _tmp119 * _tmp57 + _tmp121 * _tmp16 + _tmp123 * _tmp22 + - _tmp126 * _tmp17 + _tmp128 * _tmp18 - _tmp132 * _tmp62 + _tmp133; - _p_new(1, 5) = -_tmp117 * _tmp95 - _tmp119 * _tmp93 + _tmp121 * _tmp37 + _tmp123 * _tmp35 + - _tmp126 * _tmp39 + _tmp128 * _tmp36 - _tmp132 * _tmp94 + _tmp134; - _p_new(2, 5) = -_tmp101 * _tmp118 - _tmp116 * _tmp99 + _tmp121 * _tmp48 + _tmp123 * _tmp47 + - _tmp126 * _tmp46 + _tmp128 * _tmp45 - _tmp132 * _tmp97 + _tmp135; - _p_new(3, 5) = -_tmp103 * _tmp132 - _tmp104 * _tmp117 - _tmp105 * _tmp119 + _tmp121 * _tmp53 + - _tmp123 * _tmp55 + _tmp126 * _tmp54 + _tmp128 * _tmp56 + _tmp136; - _p_new(4, 5) = _tmp107 * _tmp126 + _tmp108 * _tmp123 - _tmp109 * _tmp119 - _tmp110 * _tmp132 - - _tmp111 * _tmp117 + _tmp112 * _tmp128 + _tmp113 * _tmp121 + _tmp116 * _tmp138 + - _tmp131 * _tmp139 + _tmp137 * _tmp60 + _tmp140; - _p_new(5, 5) = std::pow(_tmp116, Scalar(2)) * d_vel_var(1, 0) - _tmp117 * _tmp143 + - std::pow(_tmp118, Scalar(2)) * d_vel_var(0, 0) - _tmp118 * _tmp148 + - _tmp121 * _tmp142 + _tmp123 * _tmp141 + _tmp126 * _tmp146 + _tmp128 * _tmp145 + - std::pow(_tmp131, Scalar(2)) * d_vel_var(2, 0) - _tmp132 * _tmp144 + _tmp149; + _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; @@ -487,33 +490,33 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 5) = 0; _p_new(22, 5) = 0; _p_new(23, 5) = 0; - _p_new(0, 6) = -_tmp151 * _tmp62 - _tmp153 * _tmp57 + _tmp154 * _tmp22 + _tmp155 * _tmp18 + - _tmp157 * _tmp17 + _tmp158 * _tmp16 - _tmp160 * _tmp69 + _tmp162; - _p_new(1, 6) = -_tmp151 * _tmp94 - _tmp153 * _tmp93 + _tmp154 * _tmp35 + _tmp155 * _tmp36 + - _tmp157 * _tmp39 + _tmp158 * _tmp37 - _tmp160 * _tmp95 + _tmp163; - _p_new(2, 6) = -_tmp101 * _tmp152 - _tmp151 * _tmp97 + _tmp154 * _tmp47 + _tmp155 * _tmp45 + - _tmp157 * _tmp46 + _tmp158 * _tmp48 - _tmp159 * _tmp99 + _tmp164; - _p_new(3, 6) = -_tmp103 * _tmp151 - _tmp104 * _tmp160 - _tmp105 * _tmp153 + _tmp154 * _tmp55 + - _tmp155 * _tmp56 + _tmp157 * _tmp54 + _tmp158 * _tmp53 + _tmp165; - _p_new(4, 6) = _tmp107 * _tmp157 + _tmp108 * _tmp154 - _tmp109 * _tmp153 - _tmp110 * _tmp151 - - _tmp111 * _tmp160 + _tmp112 * _tmp155 + _tmp113 * _tmp158 + _tmp138 * _tmp159 + - _tmp139 * _tmp150 + _tmp152 * _tmp60 * d_vel_var(0, 0) + _tmp166; - _p_new(5, 6) = _tmp116 * _tmp159 * d_vel_var(1, 0) + _tmp131 * _tmp150 * d_vel_var(2, 0) + - _tmp137 * _tmp152 + _tmp141 * _tmp154 + _tmp142 * _tmp158 - _tmp143 * _tmp160 - - _tmp144 * _tmp151 + _tmp145 * _tmp155 + _tmp146 * _tmp157 - _tmp148 * _tmp152 + - _tmp167; + _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(_tmp150, Scalar(2)) * d_vel_var(2, 0) - _tmp151 * _tmp169 + - std::pow(_tmp152, Scalar(2)) * d_vel_var(0, 0) - _tmp153 * _tmp168 + - _tmp154 * (P(0, 0) * _tmp154 + P(1, 0) * _tmp157 - P(13, 0) * _tmp153 - P(14, 0) * _tmp160 - - P(15, 0) * _tmp151 + P(2, 0) * _tmp158 + P(3, 0) * _tmp155 + P(6, 0)) + - _tmp155 * (P(0, 3) * _tmp154 + P(1, 3) * _tmp157 - P(13, 3) * _tmp153 - P(14, 3) * _tmp160 - - P(15, 3) * _tmp151 + P(2, 3) * _tmp158 + P(3, 3) * _tmp155 + P(6, 3)) + - _tmp157 * (P(0, 1) * _tmp154 + P(1, 1) * _tmp157 - P(13, 1) * _tmp153 - P(14, 1) * _tmp160 - - P(15, 1) * _tmp151 + P(2, 1) * _tmp158 + P(3, 1) * _tmp155 + P(6, 1)) + - _tmp158 * (P(0, 2) * _tmp154 + P(1, 2) * _tmp157 - P(13, 2) * _tmp153 - P(14, 2) * _tmp160 - - P(15, 2) * _tmp151 + P(2, 2) * _tmp158 + P(3, 2) * _tmp155 + P(6, 2)) + - std::pow(_tmp159, Scalar(2)) * d_vel_var(1, 0) - _tmp160 * _tmp170 + _tmp171; + 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; @@ -531,27 +534,28 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 7) * _tmp12 + P(11, 7) * _tmp10 + - P(12, 7) * _tmp11 + P(2, 7) * _tmp9 + P(3, 7) * _tmp6 + _tmp92 * dt; - _p_new(1, 7) = P(0, 7) * _tmp30 + P(1, 7) - P(10, 7) * _tmp29 + P(11, 7) * _tmp11 - - P(12, 7) * _tmp10 + P(2, 7) * _tmp31 + P(3, 7) * _tmp9 + _tmp96 * dt; - _p_new(2, 7) = P(0, 7) * _tmp41 + P(1, 7) * _tmp6 - P(10, 7) * _tmp11 - P(11, 7) * _tmp29 + - P(12, 7) * _tmp12 + P(2, 7) + P(3, 7) * _tmp30 + _tmp102 * dt; - _p_new(3, 7) = P(0, 7) * _tmp31 + P(1, 7) * _tmp41 + P(10, 7) * _tmp10 - P(11, 7) * _tmp12 - - P(12, 7) * _tmp29 + P(2, 7) * _tmp3 + P(3, 7) + _tmp106 * dt; - _p_new(4, 7) = P(0, 7) * _tmp80 + P(1, 7) * _tmp83 - P(13, 7) * _tmp61 - P(14, 7) * _tmp73 - - P(15, 7) * _tmp68 + P(2, 7) * _tmp91 + P(3, 7) * _tmp88 + P(4, 7) + _tmp114 * dt; - _p_new(5, 7) = - P(0, 7) * _tmp123 + P(1, 7) * _tmp126 - P(13, 7) * _tmp119 - P(14, 7) * _tmp117 - - P(15, 7) * _tmp132 + P(2, 7) * _tmp121 + P(3, 7) * _tmp128 + P(5, 7) + - dt * (P(0, 4) * _tmp123 + P(1, 4) * _tmp126 - P(13, 4) * _tmp119 - P(14, 4) * _tmp117 - - P(15, 4) * _tmp132 + P(2, 4) * _tmp121 + P(3, 4) * _tmp128 + P(5, 4)); - _p_new(6, 7) = - P(0, 7) * _tmp154 + P(1, 7) * _tmp157 - P(13, 7) * _tmp153 - P(14, 7) * _tmp160 - - P(15, 7) * _tmp151 + P(2, 7) * _tmp158 + P(3, 7) * _tmp155 + P(6, 7) + - dt * (P(0, 4) * _tmp154 + P(1, 4) * _tmp157 - P(13, 4) * _tmp153 - P(14, 4) * _tmp160 - - P(15, 4) * _tmp151 + P(2, 4) * _tmp158 + P(3, 4) * _tmp155 + P(6, 4)); - _p_new(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4)); + _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; @@ -568,26 +572,27 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 7) = 0; _p_new(22, 7) = 0; _p_new(23, 7) = 0; - _p_new(0, 8) = P(0, 8) + P(1, 8) * _tmp3 + P(10, 8) * _tmp12 + P(12, 8) * _tmp11 + - P(2, 8) * _tmp9 + P(3, 8) * _tmp6 + _tmp133 * dt + _tmp172 * state(2, 0); - _p_new(1, 8) = P(0, 8) * _tmp30 + P(1, 8) - P(10, 8) * _tmp29 - P(12, 8) * _tmp10 + - P(2, 8) * _tmp31 + P(3, 8) * _tmp9 + _tmp134 * dt + _tmp172 * state(3, 0); - _p_new(2, 8) = P(0, 8) * _tmp41 + P(1, 8) * _tmp6 - P(10, 8) * _tmp11 + P(12, 8) * _tmp12 + - P(2, 8) + P(3, 8) * _tmp30 + _tmp135 * dt - _tmp172 * state(0, 0); - _p_new(3, 8) = P(0, 8) * _tmp31 + P(1, 8) * _tmp41 + P(10, 8) * _tmp10 - P(11, 8) * _tmp12 - - P(12, 8) * _tmp29 + P(2, 8) * _tmp3 + P(3, 8) + _tmp136 * dt; - _p_new(4, 8) = P(0, 8) * _tmp80 + P(1, 8) * _tmp83 - P(13, 8) * _tmp61 - P(14, 8) * _tmp73 - - P(15, 8) * _tmp68 + P(2, 8) * _tmp91 + P(3, 8) * _tmp88 + P(4, 8) + _tmp140 * dt; - _p_new(5, 8) = P(0, 8) * _tmp123 + P(1, 8) * _tmp126 - P(13, 8) * _tmp119 - P(14, 8) * _tmp117 - - P(15, 8) * _tmp132 + P(2, 8) * _tmp121 + P(3, 8) * _tmp128 + P(5, 8) + - _tmp149 * dt; - _p_new(6, 8) = - P(0, 8) * _tmp154 + P(1, 8) * _tmp157 - P(13, 8) * _tmp153 - P(14, 8) * _tmp160 - - P(15, 8) * _tmp151 + P(2, 8) * _tmp158 + P(3, 8) * _tmp155 + P(6, 8) + - dt * (P(0, 5) * _tmp154 + P(1, 5) * _tmp157 - P(13, 5) * _tmp153 - P(14, 5) * _tmp160 - - P(15, 5) * _tmp151 + P(2, 5) * _tmp158 + P(3, 5) * _tmp155 + P(6, 5)); - _p_new(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5)); - _p_new(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5)); + _p_new(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; @@ -603,25 +608,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 8) = 0; _p_new(22, 8) = 0; _p_new(23, 8) = 0; - _p_new(0, 9) = P(0, 9) + P(1, 9) * _tmp3 + P(10, 9) * _tmp12 + P(12, 9) * _tmp11 + - P(2, 9) * _tmp9 + P(3, 9) * _tmp6 + _tmp162 * dt + _tmp173 * state(2, 0); - _p_new(1, 9) = P(0, 9) * _tmp30 + P(1, 9) - P(10, 9) * _tmp29 - P(12, 9) * _tmp10 + - P(2, 9) * _tmp31 + P(3, 9) * _tmp9 + _tmp163 * dt + _tmp173 * state(3, 0); - _p_new(2, 9) = P(0, 9) * _tmp41 + P(1, 9) * _tmp6 - P(10, 9) * _tmp11 + P(12, 9) * _tmp12 + - P(2, 9) + P(3, 9) * _tmp30 + _tmp164 * dt - _tmp173 * state(0, 0); - _p_new(3, 9) = P(0, 9) * _tmp31 + P(1, 9) * _tmp41 + P(10, 9) * _tmp10 - P(11, 9) * _tmp12 - - P(12, 9) * _tmp29 + P(2, 9) * _tmp3 + P(3, 9) + _tmp165 * dt; - _p_new(4, 9) = P(0, 9) * _tmp80 + P(1, 9) * _tmp83 - P(13, 9) * _tmp61 - P(14, 9) * _tmp73 - - P(15, 9) * _tmp68 + P(2, 9) * _tmp91 + P(3, 9) * _tmp88 + P(4, 9) + _tmp166 * dt; - _p_new(5, 9) = P(0, 9) * _tmp123 + P(1, 9) * _tmp126 - P(13, 9) * _tmp119 - P(14, 9) * _tmp117 - - P(15, 9) * _tmp132 + P(2, 9) * _tmp121 + P(3, 9) * _tmp128 + P(5, 9) + - _tmp167 * dt; - _p_new(6, 9) = P(0, 9) * _tmp154 + P(1, 9) * _tmp157 - P(13, 9) * _tmp153 - P(14, 9) * _tmp160 - - P(15, 9) * _tmp151 + P(2, 9) * _tmp158 + P(3, 9) * _tmp155 + P(6, 9) + - _tmp171 * dt; - _p_new(7, 9) = P(4, 9) * dt + P(7, 9) + dt * (P(4, 6) * dt + P(7, 6)); - _p_new(8, 9) = P(5, 9) * dt + P(8, 9) + dt * (P(5, 6) * dt + P(8, 6)); - _p_new(9, 9) = P(6, 9) * dt + P(9, 9) + dt * (P(6, 6) * dt + P(9, 6)); + _p_new(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; @@ -636,22 +642,22 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 9) = 0; _p_new(22, 9) = 0; _p_new(23, 9) = 0; - _p_new(0, 10) = _tmp15; - _p_new(1, 10) = _tmp32; - _p_new(2, 10) = _tmp43; - _p_new(3, 10) = _tmp50; - _p_new(4, 10) = P(0, 10) * _tmp80 + P(1, 10) * _tmp83 - P(13, 10) * _tmp61 - - P(14, 10) * _tmp73 - P(15, 10) * _tmp68 + P(2, 10) * _tmp91 + - P(3, 10) * _tmp88 + P(4, 10); - _p_new(5, 10) = P(0, 10) * _tmp123 + P(1, 10) * _tmp126 - P(13, 10) * _tmp119 - - P(14, 10) * _tmp117 - P(15, 10) * _tmp132 + P(2, 10) * _tmp121 + - P(3, 10) * _tmp128 + P(5, 10); - _p_new(6, 10) = P(0, 10) * _tmp154 + P(1, 10) * _tmp157 - P(13, 10) * _tmp153 - - P(14, 10) * _tmp160 - P(15, 10) * _tmp151 + P(2, 10) * _tmp158 + - P(3, 10) * _tmp155 + P(6, 10); - _p_new(7, 10) = P(4, 10) * dt + P(7, 10); - _p_new(8, 10) = P(5, 10) * dt + P(8, 10); - _p_new(9, 10) = P(6, 10) * dt + P(9, 10); + _p_new(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; @@ -667,21 +673,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(22, 10) = 0; _p_new(23, 10) = 0; _p_new(0, 11) = _tmp14; - _p_new(1, 11) = _tmp33; - _p_new(2, 11) = _tmp42; - _p_new(3, 11) = _tmp51; - _p_new(4, 11) = P(0, 11) * _tmp80 + P(1, 11) * _tmp83 - P(13, 11) * _tmp61 - - P(14, 11) * _tmp73 - P(15, 11) * _tmp68 + P(2, 11) * _tmp91 + - P(3, 11) * _tmp88 + P(4, 11); - _p_new(5, 11) = P(0, 11) * _tmp123 + P(1, 11) * _tmp126 - P(13, 11) * _tmp119 - - P(14, 11) * _tmp117 - P(15, 11) * _tmp132 + P(2, 11) * _tmp121 + - P(3, 11) * _tmp128 + P(5, 11); - _p_new(6, 11) = P(0, 11) * _tmp154 + P(1, 11) * _tmp157 - P(13, 11) * _tmp153 - - P(14, 11) * _tmp160 - P(15, 11) * _tmp151 + P(2, 11) * _tmp158 + - P(3, 11) * _tmp155 + P(6, 11); - _p_new(7, 11) = P(4, 11) * dt + P(7, 11); - _p_new(8, 11) = P(5, 11) * dt + P(8, 11); - _p_new(9, 11) = P(6, 11) * dt + P(9, 11); + _p_new(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; @@ -696,22 +702,22 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 11) = 0; _p_new(22, 11) = 0; _p_new(23, 11) = 0; - _p_new(0, 12) = _tmp13; - _p_new(1, 12) = _tmp34; - _p_new(2, 12) = _tmp44; - _p_new(3, 12) = _tmp52; - _p_new(4, 12) = P(0, 12) * _tmp80 + P(1, 12) * _tmp83 - P(13, 12) * _tmp61 - - P(14, 12) * _tmp73 - P(15, 12) * _tmp68 + P(2, 12) * _tmp91 + - P(3, 12) * _tmp88 + P(4, 12); - _p_new(5, 12) = P(0, 12) * _tmp123 + P(1, 12) * _tmp126 - P(13, 12) * _tmp119 - - P(14, 12) * _tmp117 - P(15, 12) * _tmp132 + P(2, 12) * _tmp121 + - P(3, 12) * _tmp128 + P(5, 12); - _p_new(6, 12) = P(0, 12) * _tmp154 + P(1, 12) * _tmp157 - P(13, 12) * _tmp153 - - P(14, 12) * _tmp160 - P(15, 12) * _tmp151 + P(2, 12) * _tmp158 + - P(3, 12) * _tmp155 + P(6, 12); - _p_new(7, 12) = P(4, 12) * dt + P(7, 12); - _p_new(8, 12) = P(5, 12) * dt + P(8, 12); - _p_new(9, 12) = P(6, 12) * dt + P(9, 12); + _p_new(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); @@ -726,16 +732,16 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 12) = 0; _p_new(22, 12) = 0; _p_new(23, 12) = 0; - _p_new(0, 13) = _tmp57; - _p_new(1, 13) = _tmp93; + _p_new(0, 13) = _tmp65; + _p_new(1, 13) = _tmp96; _p_new(2, 13) = _tmp100; - _p_new(3, 13) = _tmp105; - _p_new(4, 13) = _tmp109; - _p_new(5, 13) = _tmp147; + _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) * dt + P(7, 13); - _p_new(8, 13) = P(5, 13) * dt + P(8, 13); - _p_new(9, 13) = P(6, 13) * dt + P(9, 13); + _p_new(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); @@ -750,16 +756,16 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 13) = 0; _p_new(22, 13) = 0; _p_new(23, 13) = 0; - _p_new(0, 14) = _tmp69; - _p_new(1, 14) = _tmp95; - _p_new(2, 14) = _tmp98; - _p_new(3, 14) = _tmp104; - _p_new(4, 14) = _tmp111; - _p_new(5, 14) = _tmp143; - _p_new(6, 14) = _tmp170; - _p_new(7, 14) = P(4, 14) * dt + P(7, 14); - _p_new(8, 14) = P(5, 14) * dt + P(8, 14); - _p_new(9, 14) = P(6, 14) * dt + P(9, 14); + _p_new(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); @@ -774,16 +780,16 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 14) = 0; _p_new(22, 14) = 0; _p_new(23, 14) = 0; - _p_new(0, 15) = _tmp62; - _p_new(1, 15) = _tmp94; - _p_new(2, 15) = _tmp97; - _p_new(3, 15) = _tmp103; + _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) = _tmp144; - _p_new(6, 15) = _tmp169; - _p_new(7, 15) = P(4, 15) * dt + P(7, 15); - _p_new(8, 15) = P(5, 15) * dt + P(8, 15); - _p_new(9, 15) = P(6, 15) * dt + P(9, 15); + _p_new(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); @@ -798,26 +804,26 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 16) * _tmp12 + P(11, 16) * _tmp10 + - P(12, 16) * _tmp11 + P(2, 16) * _tmp9 + P(3, 16) * _tmp6; - _p_new(1, 16) = P(0, 16) * _tmp30 + P(1, 16) - P(10, 16) * _tmp29 + P(11, 16) * _tmp11 - - P(12, 16) * _tmp10 + P(2, 16) * _tmp31 + P(3, 16) * _tmp9; - _p_new(2, 16) = P(0, 16) * _tmp41 + P(1, 16) * _tmp6 - P(10, 16) * _tmp11 - P(11, 16) * _tmp29 + - P(12, 16) * _tmp12 + P(2, 16) + P(3, 16) * _tmp30; - _p_new(3, 16) = P(0, 16) * _tmp31 + P(1, 16) * _tmp41 + P(10, 16) * _tmp10 - - P(11, 16) * _tmp12 - P(12, 16) * _tmp29 + P(2, 16) * _tmp3 + P(3, 16); - _p_new(4, 16) = P(0, 16) * _tmp80 + P(1, 16) * _tmp83 - P(13, 16) * _tmp61 - - P(14, 16) * _tmp73 - P(15, 16) * _tmp68 + P(2, 16) * _tmp91 + - P(3, 16) * _tmp88 + P(4, 16); - _p_new(5, 16) = P(0, 16) * _tmp123 + P(1, 16) * _tmp126 - P(13, 16) * _tmp119 - - P(14, 16) * _tmp117 - P(15, 16) * _tmp132 + P(2, 16) * _tmp121 + - P(3, 16) * _tmp128 + P(5, 16); - _p_new(6, 16) = P(0, 16) * _tmp154 + P(1, 16) * _tmp157 - P(13, 16) * _tmp153 - - P(14, 16) * _tmp160 - P(15, 16) * _tmp151 + P(2, 16) * _tmp158 + - P(3, 16) * _tmp155 + P(6, 16); - _p_new(7, 16) = P(4, 16) * dt + P(7, 16); - _p_new(8, 16) = P(5, 16) * dt + P(8, 16); - _p_new(9, 16) = P(6, 16) * dt + P(9, 16); + _p_new(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); @@ -832,26 +838,26 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 17) * _tmp12 + P(11, 17) * _tmp10 + - P(12, 17) * _tmp11 + P(2, 17) * _tmp9 + P(3, 17) * _tmp6; - _p_new(1, 17) = P(0, 17) * _tmp30 + P(1, 17) - P(10, 17) * _tmp29 + P(11, 17) * _tmp11 - - P(12, 17) * _tmp10 + P(2, 17) * _tmp31 + P(3, 17) * _tmp9; - _p_new(2, 17) = P(0, 17) * _tmp41 + P(1, 17) * _tmp6 - P(10, 17) * _tmp11 - P(11, 17) * _tmp29 + - P(12, 17) * _tmp12 + P(2, 17) + P(3, 17) * _tmp30; - _p_new(3, 17) = P(0, 17) * _tmp31 + P(1, 17) * _tmp41 + P(10, 17) * _tmp10 - - P(11, 17) * _tmp12 - P(12, 17) * _tmp29 + P(2, 17) * _tmp3 + P(3, 17); - _p_new(4, 17) = P(0, 17) * _tmp80 + P(1, 17) * _tmp83 - P(13, 17) * _tmp61 - - P(14, 17) * _tmp73 - P(15, 17) * _tmp68 + P(2, 17) * _tmp91 + - P(3, 17) * _tmp88 + P(4, 17); - _p_new(5, 17) = P(0, 17) * _tmp123 + P(1, 17) * _tmp126 - P(13, 17) * _tmp119 - - P(14, 17) * _tmp117 - P(15, 17) * _tmp132 + P(2, 17) * _tmp121 + - P(3, 17) * _tmp128 + P(5, 17); - _p_new(6, 17) = P(0, 17) * _tmp154 + P(1, 17) * _tmp157 - P(13, 17) * _tmp153 - - P(14, 17) * _tmp160 - P(15, 17) * _tmp151 + P(2, 17) * _tmp158 + - P(3, 17) * _tmp155 + P(6, 17); - _p_new(7, 17) = P(4, 17) * dt + P(7, 17); - _p_new(8, 17) = P(5, 17) * dt + P(8, 17); - _p_new(9, 17) = P(6, 17) * dt + P(9, 17); + _p_new(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); @@ -866,26 +872,26 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 18) * _tmp12 + P(11, 18) * _tmp10 + - P(12, 18) * _tmp11 + P(2, 18) * _tmp9 + P(3, 18) * _tmp6; - _p_new(1, 18) = P(0, 18) * _tmp30 + P(1, 18) - P(10, 18) * _tmp29 + P(11, 18) * _tmp11 - - P(12, 18) * _tmp10 + P(2, 18) * _tmp31 + P(3, 18) * _tmp9; - _p_new(2, 18) = P(0, 18) * _tmp41 + P(1, 18) * _tmp6 - P(10, 18) * _tmp11 - P(11, 18) * _tmp29 + - P(12, 18) * _tmp12 + P(2, 18) + P(3, 18) * _tmp30; - _p_new(3, 18) = P(0, 18) * _tmp31 + P(1, 18) * _tmp41 + P(10, 18) * _tmp10 - - P(11, 18) * _tmp12 - P(12, 18) * _tmp29 + P(2, 18) * _tmp3 + P(3, 18); - _p_new(4, 18) = P(0, 18) * _tmp80 + P(1, 18) * _tmp83 - P(13, 18) * _tmp61 - - P(14, 18) * _tmp73 - P(15, 18) * _tmp68 + P(2, 18) * _tmp91 + - P(3, 18) * _tmp88 + P(4, 18); - _p_new(5, 18) = P(0, 18) * _tmp123 + P(1, 18) * _tmp126 - P(13, 18) * _tmp119 - - P(14, 18) * _tmp117 - P(15, 18) * _tmp132 + P(2, 18) * _tmp121 + - P(3, 18) * _tmp128 + P(5, 18); - _p_new(6, 18) = P(0, 18) * _tmp154 + P(1, 18) * _tmp157 - P(13, 18) * _tmp153 - - P(14, 18) * _tmp160 - P(15, 18) * _tmp151 + P(2, 18) * _tmp158 + - P(3, 18) * _tmp155 + P(6, 18); - _p_new(7, 18) = P(4, 18) * dt + P(7, 18); - _p_new(8, 18) = P(5, 18) * dt + P(8, 18); - _p_new(9, 18) = P(6, 18) * dt + P(9, 18); + _p_new(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); @@ -900,26 +906,26 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 19) * _tmp12 + P(11, 19) * _tmp10 + - P(12, 19) * _tmp11 + P(2, 19) * _tmp9 + P(3, 19) * _tmp6; - _p_new(1, 19) = P(0, 19) * _tmp30 + P(1, 19) - P(10, 19) * _tmp29 + P(11, 19) * _tmp11 - - P(12, 19) * _tmp10 + P(2, 19) * _tmp31 + P(3, 19) * _tmp9; - _p_new(2, 19) = P(0, 19) * _tmp41 + P(1, 19) * _tmp6 - P(10, 19) * _tmp11 - P(11, 19) * _tmp29 + - P(12, 19) * _tmp12 + P(2, 19) + P(3, 19) * _tmp30; - _p_new(3, 19) = P(0, 19) * _tmp31 + P(1, 19) * _tmp41 + P(10, 19) * _tmp10 - - P(11, 19) * _tmp12 - P(12, 19) * _tmp29 + P(2, 19) * _tmp3 + P(3, 19); - _p_new(4, 19) = P(0, 19) * _tmp80 + P(1, 19) * _tmp83 - P(13, 19) * _tmp61 - - P(14, 19) * _tmp73 - P(15, 19) * _tmp68 + P(2, 19) * _tmp91 + - P(3, 19) * _tmp88 + P(4, 19); - _p_new(5, 19) = P(0, 19) * _tmp123 + P(1, 19) * _tmp126 - P(13, 19) * _tmp119 - - P(14, 19) * _tmp117 - P(15, 19) * _tmp132 + P(2, 19) * _tmp121 + - P(3, 19) * _tmp128 + P(5, 19); - _p_new(6, 19) = P(0, 19) * _tmp154 + P(1, 19) * _tmp157 - P(13, 19) * _tmp153 - - P(14, 19) * _tmp160 - P(15, 19) * _tmp151 + P(2, 19) * _tmp158 + - P(3, 19) * _tmp155 + P(6, 19); - _p_new(7, 19) = P(4, 19) * dt + P(7, 19); - _p_new(8, 19) = P(5, 19) * dt + P(8, 19); - _p_new(9, 19) = P(6, 19) * dt + P(9, 19); + _p_new(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); @@ -934,26 +940,26 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 20) * _tmp12 + P(11, 20) * _tmp10 + - P(12, 20) * _tmp11 + P(2, 20) * _tmp9 + P(3, 20) * _tmp6; - _p_new(1, 20) = P(0, 20) * _tmp30 + P(1, 20) - P(10, 20) * _tmp29 + P(11, 20) * _tmp11 - - P(12, 20) * _tmp10 + P(2, 20) * _tmp31 + P(3, 20) * _tmp9; - _p_new(2, 20) = P(0, 20) * _tmp41 + P(1, 20) * _tmp6 - P(10, 20) * _tmp11 - P(11, 20) * _tmp29 + - P(12, 20) * _tmp12 + P(2, 20) + P(3, 20) * _tmp30; - _p_new(3, 20) = P(0, 20) * _tmp31 + P(1, 20) * _tmp41 + P(10, 20) * _tmp10 - - P(11, 20) * _tmp12 - P(12, 20) * _tmp29 + P(2, 20) * _tmp3 + P(3, 20); - _p_new(4, 20) = P(0, 20) * _tmp80 + P(1, 20) * _tmp83 - P(13, 20) * _tmp61 - - P(14, 20) * _tmp73 - P(15, 20) * _tmp68 + P(2, 20) * _tmp91 + - P(3, 20) * _tmp88 + P(4, 20); - _p_new(5, 20) = P(0, 20) * _tmp123 + P(1, 20) * _tmp126 - P(13, 20) * _tmp119 - - P(14, 20) * _tmp117 - P(15, 20) * _tmp132 + P(2, 20) * _tmp121 + - P(3, 20) * _tmp128 + P(5, 20); - _p_new(6, 20) = P(0, 20) * _tmp154 + P(1, 20) * _tmp157 - P(13, 20) * _tmp153 - - P(14, 20) * _tmp160 - P(15, 20) * _tmp151 + P(2, 20) * _tmp158 + - P(3, 20) * _tmp155 + P(6, 20); - _p_new(7, 20) = P(4, 20) * dt + P(7, 20); - _p_new(8, 20) = P(5, 20) * dt + P(8, 20); - _p_new(9, 20) = P(6, 20) * dt + P(9, 20); + _p_new(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); @@ -968,26 +974,26 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 21) * _tmp12 + P(11, 21) * _tmp10 + - P(12, 21) * _tmp11 + P(2, 21) * _tmp9 + P(3, 21) * _tmp6; - _p_new(1, 21) = P(0, 21) * _tmp30 + P(1, 21) - P(10, 21) * _tmp29 + P(11, 21) * _tmp11 - - P(12, 21) * _tmp10 + P(2, 21) * _tmp31 + P(3, 21) * _tmp9; - _p_new(2, 21) = P(0, 21) * _tmp41 + P(1, 21) * _tmp6 - P(10, 21) * _tmp11 - P(11, 21) * _tmp29 + - P(12, 21) * _tmp12 + P(2, 21) + P(3, 21) * _tmp30; - _p_new(3, 21) = P(0, 21) * _tmp31 + P(1, 21) * _tmp41 + P(10, 21) * _tmp10 - - P(11, 21) * _tmp12 - P(12, 21) * _tmp29 + P(2, 21) * _tmp3 + P(3, 21); - _p_new(4, 21) = P(0, 21) * _tmp80 + P(1, 21) * _tmp83 - P(13, 21) * _tmp61 - - P(14, 21) * _tmp73 - P(15, 21) * _tmp68 + P(2, 21) * _tmp91 + - P(3, 21) * _tmp88 + P(4, 21); - _p_new(5, 21) = P(0, 21) * _tmp123 + P(1, 21) * _tmp126 - P(13, 21) * _tmp119 - - P(14, 21) * _tmp117 - P(15, 21) * _tmp132 + P(2, 21) * _tmp121 + - P(3, 21) * _tmp128 + P(5, 21); - _p_new(6, 21) = P(0, 21) * _tmp154 + P(1, 21) * _tmp157 - P(13, 21) * _tmp153 - - P(14, 21) * _tmp160 - P(15, 21) * _tmp151 + P(2, 21) * _tmp158 + - P(3, 21) * _tmp155 + P(6, 21); - _p_new(7, 21) = P(4, 21) * dt + P(7, 21); - _p_new(8, 21) = P(5, 21) * dt + P(8, 21); - _p_new(9, 21) = P(6, 21) * dt + P(9, 21); + _p_new(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); @@ -1002,26 +1008,26 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 22) * _tmp12 + P(11, 22) * _tmp10 + - P(12, 22) * _tmp11 + P(2, 22) * _tmp9 + P(3, 22) * _tmp6; - _p_new(1, 22) = P(0, 22) * _tmp30 + P(1, 22) - P(10, 22) * _tmp29 + P(11, 22) * _tmp11 - - P(12, 22) * _tmp10 + P(2, 22) * _tmp31 + P(3, 22) * _tmp9; - _p_new(2, 22) = P(0, 22) * _tmp41 + P(1, 22) * _tmp6 - P(10, 22) * _tmp11 - P(11, 22) * _tmp29 + - P(12, 22) * _tmp12 + P(2, 22) + P(3, 22) * _tmp30; - _p_new(3, 22) = P(0, 22) * _tmp31 + P(1, 22) * _tmp41 + P(10, 22) * _tmp10 - - P(11, 22) * _tmp12 - P(12, 22) * _tmp29 + P(2, 22) * _tmp3 + P(3, 22); - _p_new(4, 22) = P(0, 22) * _tmp80 + P(1, 22) * _tmp83 - P(13, 22) * _tmp61 - - P(14, 22) * _tmp73 - P(15, 22) * _tmp68 + P(2, 22) * _tmp91 + - P(3, 22) * _tmp88 + P(4, 22); - _p_new(5, 22) = P(0, 22) * _tmp123 + P(1, 22) * _tmp126 - P(13, 22) * _tmp119 - - P(14, 22) * _tmp117 - P(15, 22) * _tmp132 + P(2, 22) * _tmp121 + - P(3, 22) * _tmp128 + P(5, 22); - _p_new(6, 22) = P(0, 22) * _tmp154 + P(1, 22) * _tmp157 - P(13, 22) * _tmp153 - - P(14, 22) * _tmp160 - P(15, 22) * _tmp151 + P(2, 22) * _tmp158 + - P(3, 22) * _tmp155 + P(6, 22); - _p_new(7, 22) = P(4, 22) * dt + P(7, 22); - _p_new(8, 22) = P(5, 22) * dt + P(8, 22); - _p_new(9, 22) = P(6, 22) * dt + P(9, 22); + _p_new(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); @@ -1036,26 +1042,26 @@ void PredictCovariance(const matrix::Matrix& state, _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) * _tmp3 + P(10, 23) * _tmp12 + P(11, 23) * _tmp10 + - P(12, 23) * _tmp11 + P(2, 23) * _tmp9 + P(3, 23) * _tmp6; - _p_new(1, 23) = P(0, 23) * _tmp30 + P(1, 23) - P(10, 23) * _tmp29 + P(11, 23) * _tmp11 - - P(12, 23) * _tmp10 + P(2, 23) * _tmp31 + P(3, 23) * _tmp9; - _p_new(2, 23) = P(0, 23) * _tmp41 + P(1, 23) * _tmp6 - P(10, 23) * _tmp11 - P(11, 23) * _tmp29 + - P(12, 23) * _tmp12 + P(2, 23) + P(3, 23) * _tmp30; - _p_new(3, 23) = P(0, 23) * _tmp31 + P(1, 23) * _tmp41 + P(10, 23) * _tmp10 - - P(11, 23) * _tmp12 - P(12, 23) * _tmp29 + P(2, 23) * _tmp3 + P(3, 23); - _p_new(4, 23) = P(0, 23) * _tmp80 + P(1, 23) * _tmp83 - P(13, 23) * _tmp61 - - P(14, 23) * _tmp73 - P(15, 23) * _tmp68 + P(2, 23) * _tmp91 + - P(3, 23) * _tmp88 + P(4, 23); - _p_new(5, 23) = P(0, 23) * _tmp123 + P(1, 23) * _tmp126 - P(13, 23) * _tmp119 - - P(14, 23) * _tmp117 - P(15, 23) * _tmp132 + P(2, 23) * _tmp121 + - P(3, 23) * _tmp128 + P(5, 23); - _p_new(6, 23) = P(0, 23) * _tmp154 + P(1, 23) * _tmp157 - P(13, 23) * _tmp153 - - P(14, 23) * _tmp160 - P(15, 23) * _tmp151 + P(2, 23) * _tmp158 + - P(3, 23) * _tmp155 + P(6, 23); - _p_new(7, 23) = P(4, 23) * dt + P(7, 23); - _p_new(8, 23) = P(5, 23) * dt + P(8, 23); - _p_new(9, 23) = P(6, 23) * dt + P(9, 23); + _p_new(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); diff --git a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv index 21eeff723a..4c93faf6f4 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -215,9 +215,9 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 21290000,0.71,0.00047,-0.012,0.71,-0.004,-0.018,0.016,0.0034,-0.0055,-3.7e+02,-0.0014,-0.006,4.1e-05,-0.00073,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.9e-05,9.8e-05,0.0023,0.024,0.024,0.0086,0.048,0.048,0.039,1.5e-06,1.5e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21390000,0.71,0.00051,-0.012,0.71,-0.0048,-0.017,0.016,0.0028,-0.0034,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.00035,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.7e-05,9.6e-05,0.0023,0.022,0.022,0.0085,0.043,0.043,0.039,1.4e-06,1.4e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21490000,0.71,0.00052,-0.012,0.71,-0.0053,-0.018,0.015,0.0023,-0.0052,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.00036,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.7e-05,9.7e-05,0.0023,0.023,0.023,0.0085,0.048,0.048,0.038,1.4e-06,1.4e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21590000,0.71,0.00055,-0.012,0.71,-0.0058,-0.015,0.015,0.0019,-0.0032,-3.7e+02,-0.0014,-0.006,4.2e-05,-5.5e-06,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.6e-05,9.5e-05,0.0023,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-06,1.3e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21690000,0.71,0.00056,-0.012,0.71,-0.0057,-0.017,0.017,0.0013,-0.0048,-3.7e+02,-0.0014,-0.006,4.2e-05,-6.7e-06,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.6e-05,9.5e-05,0.0023,0.023,0.023,0.0084,0.048,0.048,0.038,1.3e-06,1.3e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21790000,0.71,0.00057,-0.012,0.71,-0.0063,-0.011,0.015,7.2e-05,-0.00077,-3.7e+02,-0.0014,-0.0059,4.2e-05,0.00052,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.5e-05,9.4e-05,0.0023,0.021,0.021,0.0082,0.042,0.042,0.038,1.2e-06,1.2e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21590000,0.71,0.00055,-0.012,0.71,-0.0058,-0.015,0.015,0.0019,-0.0032,-3.7e+02,-0.0014,-0.006,4.2e-05,-5.4e-06,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.6e-05,9.5e-05,0.0023,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-06,1.3e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21690000,0.71,0.00056,-0.012,0.71,-0.0057,-0.017,0.017,0.0013,-0.0048,-3.7e+02,-0.0014,-0.006,4.2e-05,-6.6e-06,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.6e-05,9.5e-05,0.0023,0.023,0.023,0.0084,0.048,0.048,0.038,1.3e-06,1.3e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21790000,0.71,0.00057,-0.012,0.71,-0.0063,-0.011,0.015,7.1e-05,-0.00077,-3.7e+02,-0.0014,-0.0059,4.2e-05,0.00052,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.5e-05,9.4e-05,0.0023,0.021,0.021,0.0082,0.042,0.042,0.038,1.2e-06,1.2e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21890000,0.71,0.00058,-0.012,0.71,-0.0063,-0.012,0.016,-0.00057,-0.0019,-3.7e+02,-0.0014,-0.0059,4.2e-05,0.00051,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.5e-05,9.4e-05,0.0023,0.022,0.022,0.0082,0.047,0.047,0.038,1.2e-06,1.3e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21990000,0.71,0.00063,-0.012,0.71,-0.0068,-0.0091,0.016,-0.0015,0.0015,-3.7e+02,-0.0014,-0.0059,4.3e-05,0.00095,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.4e-05,9.3e-05,0.0023,0.02,0.02,0.0081,0.042,0.042,0.038,1.2e-06,1.2e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 22090000,0.71,0.00064,-0.012,0.71,-0.0072,-0.0083,0.015,-0.0022,0.0006,-3.7e+02,-0.0014,-0.0059,4.3e-05,0.00095,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.4e-05,9.3e-05,0.0023,0.022,0.022,0.0081,0.047,0.047,0.038,1.2e-06,1.2e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 @@ -242,7 +242,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 23990000,0.71,0.00098,-0.01,0.71,-0.11,-0.04,-0.25,-0.034,-0.0083,-3.7e+02,-0.0014,-0.0059,4.3e-05,0.0021,0.0059,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.7e-05,8.6e-05,0.0024,0.017,0.017,0.0077,0.041,0.041,0.035,6.7e-07,6.7e-07,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 24090000,0.7,0.0023,-0.0088,0.71,-0.11,-0.04,-0.3,-0.045,-0.012,-3.7e+02,-0.0014,-0.0059,4.3e-05,0.0021,0.0059,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.7e-05,8.7e-05,0.0024,0.018,0.019,0.0078,0.045,0.045,0.035,6.7e-07,6.7e-07,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 24190000,0.7,0.0033,-0.0065,0.71,-0.11,-0.041,-0.35,-0.046,-0.014,-3.7e+02,-0.0014,-0.0059,4.2e-05,0.0024,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.6e-05,8.6e-05,0.0024,0.017,0.017,0.0077,0.04,0.04,0.035,6.4e-07,6.4e-07,3.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24290000,0.7,0.0038,-0.0057,0.71,-0.12,-0.045,-0.41,-0.058,-0.019,-3.7e+02,-0.0014,-0.0059,4.2e-05,0.0024,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.6e-05,8.6e-05,0.0024,0.018,0.018,0.0078,0.045,0.045,0.036,6.5e-07,6.4e-07,3.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24290000,0.7,0.0038,-0.0057,0.71,-0.12,-0.045,-0.41,-0.058,-0.019,-3.7e+02,-0.0014,-0.0059,4.2e-05,0.0024,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.6e-05,8.6e-05,0.0024,0.018,0.018,0.0078,0.045,0.045,0.036,6.4e-07,6.4e-07,3.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 24390000,0.7,0.0039,-0.0059,0.71,-0.13,-0.052,-0.46,-0.064,-0.03,-3.7e+02,-0.0014,-0.0059,4.1e-05,0.002,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.6e-05,8.5e-05,0.0024,0.017,0.017,0.0078,0.04,0.04,0.035,6.2e-07,6.2e-07,3.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 24490000,0.7,0.0047,-0.0018,0.71,-0.14,-0.058,-0.51,-0.077,-0.036,-3.7e+02,-0.0014,-0.0059,4.1e-05,0.002,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.6e-05,8.6e-05,0.0024,0.018,0.018,0.0078,0.045,0.045,0.035,6.2e-07,6.2e-07,3.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 24590000,0.7,0.0052,0.0019,0.71,-0.16,-0.069,-0.56,-0.081,-0.045,-3.7e+02,-0.0013,-0.0059,4e-05,0.002,0.0042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.5e-05,8.5e-05,0.0024,0.017,0.017,0.0078,0.04,0.04,0.035,5.9e-07,5.9e-07,3.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 @@ -290,7 +290,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 28790000,0.73,-0.0015,0.00012,0.69,-2.6,-1.2,0.97,-5.9,-2.9,-3.7e+02,-0.00081,-0.006,0.0002,-0.0027,-0.023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00087,8.2e-05,7.8e-05,0.00087,0.025,0.03,0.0089,0.13,0.15,0.036,3.8e-07,4.4e-07,3.1e-06,0.028,0.028,0.0005,0.0025,0.00022,0.0025,0.0025,0.0025,0.0025,0,0 28890000,0.73,-0.0015,0.00035,0.69,-2.5,-1.2,0.96,-6.1,-3.1,-3.7e+02,-0.00081,-0.0059,0.0002,-0.0032,-0.022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00087,8.3e-05,7.8e-05,0.00087,0.026,0.03,0.009,0.14,0.16,0.036,3.8e-07,4.4e-07,3.1e-06,0.028,0.028,0.0005,0.0025,0.00021,0.0025,0.0025,0.0025,0.0025,0,0 28990000,0.73,-0.0014,0.0008,0.68,-2.5,-1.1,0.95,-6.5,-3.2,-3.7e+02,-0.00089,-0.006,0.00021,-0.0028,-0.03,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00086,8.3e-05,7.8e-05,0.00086,0.025,0.027,0.009,0.14,0.16,0.036,3.8e-07,4.4e-07,3.1e-06,0.028,0.028,0.0005,0.0025,0.0002,0.0025,0.0025,0.0025,0.0025,0,0 -29090000,0.73,-0.0013,0.0012,0.68,-2.4,-1.1,0.94,-6.7,-3.3,-3.7e+02,-0.00089,-0.006,0.00021,-0.0033,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00086,8.3e-05,7.8e-05,0.00086,0.026,0.027,0.009,0.15,0.17,0.036,3.8e-07,4.4e-07,3.1e-06,0.028,0.028,0.0005,0.0025,0.00019,0.0025,0.0025,0.0025,0.0025,0,0 +29090000,0.73,-0.0013,0.0012,0.68,-2.4,-1.1,0.94,-6.7,-3.3,-3.7e+02,-0.00089,-0.006,0.00021,-0.0032,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00086,8.3e-05,7.8e-05,0.00086,0.026,0.027,0.009,0.15,0.17,0.036,3.8e-07,4.4e-07,3.1e-06,0.028,0.028,0.0005,0.0025,0.00019,0.0025,0.0025,0.0025,0.0025,0,0 29190000,0.73,-0.0011,0.0016,0.68,-2.4,-1.1,0.93,-7,-3.4,-3.7e+02,-0.00094,-0.006,0.00022,-0.0035,-0.031,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00085,8.3e-05,7.8e-05,0.00085,0.025,0.025,0.009,0.15,0.17,0.036,3.7e-07,4.4e-07,3.1e-06,0.028,0.028,0.0005,0.0025,0.00018,0.0025,0.0025,0.0025,0.0025,0,0 29290000,0.73,-0.00069,0.0025,0.68,-2.3,-1.1,0.96,-7.3,-3.5,-3.7e+02,-0.00093,-0.006,0.00022,-0.0041,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00085,8.3e-05,7.8e-05,0.00085,0.026,0.026,0.0091,0.16,0.18,0.036,3.7e-07,4.4e-07,3.1e-06,0.028,0.028,0.0005,0.0025,0.00018,0.0025,0.0025,0.0025,0.0025,0,0 29390000,0.73,-0.00011,0.0039,0.68,-2.3,-1.1,0.97,-7.6,-3.6,-3.7e+02,-0.001,-0.006,0.00022,-0.0037,-0.033,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00085,8.3e-05,7.8e-05,0.00085,0.024,0.025,0.009,0.16,0.18,0.036,3.7e-07,4.3e-07,3.1e-06,0.028,0.027,0.0005,0.0025,0.00017,0.0025,0.0025,0.0025,0.0025,0,0 @@ -322,7 +322,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 31990000,0.73,4.9e-05,-0.00054,0.69,-1.6,-0.92,0.71,-13,-6.1,-3.7e+02,-0.0014,-0.0058,1.1e-05,-0.016,-0.0092,-0.097,0.37,0.0037,0.025,0,0,0,0,0,0.00073,8.4e-05,7.5e-05,0.00073,0.025,0.031,0.0086,0.28,0.31,0.036,3e-07,3.2e-07,3e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 32090000,0.73,-0.00026,-0.0012,0.69,-1.5,-0.91,0.72,-13,-6.2,-3.7e+02,-0.0014,-0.0058,1e-05,-0.016,-0.0077,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00073,8.5e-05,7.6e-05,0.00073,0.027,0.033,0.0087,0.3,0.32,0.037,3.1e-07,3.3e-07,3e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 32190000,0.72,-0.00037,-0.0021,0.69,-1.5,-0.9,0.72,-13,-6.3,-3.7e+02,-0.0014,-0.0058,-6.8e-06,-0.017,-0.0056,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00071,8.4e-05,7.5e-05,0.00071,0.025,0.031,0.0086,0.29,0.32,0.036,3e-07,3.2e-07,3e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32290000,0.72,-0.00062,-0.0028,0.69,-1.5,-0.89,0.71,-14,-6.3,-3.7e+02,-0.0014,-0.0058,-7.1e-06,-0.017,-0.004,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00071,8.4e-05,7.5e-05,0.00071,0.027,0.033,0.0086,0.3,0.33,0.037,3e-07,3.2e-07,3e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32290000,0.72,-0.00062,-0.0028,0.69,-1.5,-0.89,0.71,-14,-6.3,-3.7e+02,-0.0014,-0.0058,-7.1e-06,-0.017,-0.004,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00071,8.5e-05,7.5e-05,0.00071,0.027,0.033,0.0086,0.3,0.33,0.037,3e-07,3.2e-07,3e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 32390000,0.72,-0.00072,-0.0035,0.69,-1.5,-0.87,0.71,-14,-6.4,-3.7e+02,-0.0014,-0.0057,-1.9e-05,-0.017,-0.0032,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00069,8.4e-05,7.5e-05,0.00069,0.025,0.031,0.0085,0.3,0.33,0.037,3e-07,3.1e-07,3e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 32490000,0.72,-0.00085,-0.0037,0.69,-1.4,-0.87,0.72,-14,-6.5,-3.7e+02,-0.0014,-0.0057,-1.9e-05,-0.018,-0.0021,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00069,8.4e-05,7.5e-05,0.00069,0.027,0.033,0.0085,0.31,0.34,0.037,3e-07,3.1e-07,3e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 32590000,0.72,-0.00075,-0.004,0.69,-1.4,-0.85,0.72,-14,-6.6,-3.7e+02,-0.0014,-0.0057,-3.1e-05,-0.018,-0.0012,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00067,8.4e-05,7.5e-05,0.00067,0.025,0.031,0.0084,0.31,0.34,0.036,2.9e-07,3e-07,3e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 @@ -334,7 +334,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 33190000,0.72,0.0031,-0.0031,0.7,-1.2,-0.79,0.64,-15,-7,-3.7e+02,-0.0015,-0.0057,-6.3e-05,-0.019,0.0041,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00063,8.2e-05,7.4e-05,0.00061,0.025,0.031,0.0083,0.34,0.37,0.036,2.8e-07,2.9e-07,3e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 33290000,0.67,0.015,-0.0024,0.75,-1.2,-0.77,0.62,-15,-7.1,-3.7e+02,-0.0015,-0.0057,-6.3e-05,-0.019,0.0045,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00071,8.2e-05,7.4e-05,0.00053,0.026,0.032,0.0083,0.35,0.38,0.036,2.8e-07,2.9e-07,3e-06,0.027,0.025,0.0005,0.0025,9.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 33390000,0.56,0.013,-0.0027,0.83,-1.2,-0.75,0.81,-15,-7.2,-3.7e+02,-0.0015,-0.0057,-6.9e-05,-0.019,0.0055,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00084,8e-05,7.5e-05,0.00037,0.025,0.03,0.0083,0.35,0.38,0.036,2.8e-07,2.8e-07,3e-06,0.027,0.025,0.0005,0.0025,9e-05,0.0025,0.0025,0.0025,0.0025,0,0 -33490000,0.43,0.0065,-5.7e-05,0.9,-1.2,-0.75,0.83,-15,-7.3,-3.7e+02,-0.0015,-0.0057,-6.9e-05,-0.019,0.0056,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.001,7.9e-05,7.7e-05,0.00021,0.026,0.032,0.0081,0.36,0.39,0.036,2.8e-07,2.9e-07,3e-06,0.027,0.025,0.0005,0.0025,8.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33490000,0.43,0.0065,-5.8e-05,0.9,-1.2,-0.75,0.83,-15,-7.3,-3.7e+02,-0.0015,-0.0057,-6.9e-05,-0.019,0.0056,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.001,7.9e-05,7.7e-05,0.00021,0.026,0.032,0.0081,0.36,0.39,0.036,2.8e-07,2.9e-07,3e-06,0.027,0.025,0.0005,0.0025,8.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 33590000,0.27,0.00058,-0.0025,0.96,-1.2,-0.74,0.79,-15,-7.3,-3.7e+02,-0.0015,-0.0057,-7.8e-05,-0.019,0.0056,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0011,7.7e-05,7.9e-05,8.7e-05,0.026,0.031,0.0078,0.36,0.39,0.036,2.8e-07,2.8e-07,3e-06,0.027,0.025,0.0005,0.0025,8.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 33690000,0.1,-0.0029,-0.0054,0.99,-1.1,-0.74,0.8,-15,-7.4,-3.7e+02,-0.0015,-0.0057,-7.8e-05,-0.019,0.0056,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.5e-05,8.1e-05,2.1e-05,0.028,0.034,0.0076,0.37,0.4,0.036,2.8e-07,2.8e-07,3e-06,0.027,0.025,0.0005,0.0025,8.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 33790000,-0.067,-0.0044,-0.0073,1,-1.1,-0.72,0.78,-16,-7.5,-3.7e+02,-0.0015,-0.0057,-8.9e-05,-0.019,0.0056,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0011,7.3e-05,8e-05,2e-05,0.028,0.034,0.0074,0.36,0.4,0.036,2.7e-07,2.8e-07,3e-06,0.027,0.025,0.0005,0.0025,8.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index fd20001fc8..091296fcdc 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -214,7 +214,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 21190000,0.98,0.0017,-0.0059,0.19,-0.049,0.05,-0.5,-0.0013,0.0035,-0.078,-0.0014,-0.0059,8.3e-06,0.015,0.0012,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.6e-05,9.4e-05,0.0045,0.023,0.023,0.0083,0.043,0.043,0.036,1.6e-06,1.6e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21290000,0.98,-0.0005,-0.0072,0.19,-0.049,0.054,-0.63,-0.0062,0.0087,-0.14,-0.0014,-0.0059,8.3e-06,0.015,0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.6e-05,9.5e-05,0.0045,0.026,0.026,0.0083,0.048,0.048,0.036,1.6e-06,1.6e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21390000,0.98,-0.002,-0.0079,0.19,-0.045,0.05,-0.75,-0.0051,0.011,-0.2,-0.0014,-0.0059,8.3e-06,0.015,0.00077,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.4e-05,9.2e-05,0.0045,0.023,0.023,0.0082,0.043,0.043,0.036,1.4e-06,1.4e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0093,0.016,-0.29,-0.0014,-0.0059,8.3e-06,0.015,0.00067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.5e-05,9.3e-05,0.0045,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-06,1.4e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0094,0.016,-0.29,-0.0014,-0.0059,8.3e-06,0.015,0.00067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.5e-05,9.3e-05,0.0045,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-06,1.4e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21590000,0.98,-0.0033,-0.0083,0.19,-0.032,0.043,-1,-0.008,0.016,-0.38,-0.0014,-0.0059,8.2e-06,0.015,0.00063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.2e-05,9.1e-05,0.0045,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-06,1.3e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21690000,0.98,-0.0036,-0.0082,0.19,-0.029,0.039,-1.1,-0.011,0.021,-0.49,-0.0014,-0.0059,8.2e-06,0.015,0.00046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.3e-05,9.1e-05,0.0045,0.025,0.025,0.0083,0.048,0.048,0.036,1.3e-06,1.3e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 21790000,0.98,-0.004,-0.0084,0.19,-0.021,0.032,-1.3,-0.0038,0.018,-0.61,-0.0014,-0.0058,8.2e-06,0.016,0.00027,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9e-05,8.9e-05,0.0045,0.023,0.023,0.0082,0.043,0.043,0.036,1.2e-06,1.2e-06,3.1e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 @@ -249,7 +249,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 24690000,0.98,-0.011,-0.015,0.19,0.059,-0.061,0.064,0.17,-0.12,-3.6,-0.0014,-0.0058,8.6e-06,0.019,-0.0037,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00017,7.9e-05,7.7e-05,0.0046,0.016,0.016,0.0081,0.044,0.044,0.036,5.9e-07,5.9e-07,3.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 24790000,0.98,-0.011,-0.014,0.19,0.056,-0.059,0.056,0.17,-0.11,-3.6,-0.0015,-0.0058,8.9e-06,0.019,-0.0043,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00017,7.9e-05,7.7e-05,0.0046,0.015,0.015,0.008,0.039,0.039,0.035,5.7e-07,5.7e-07,3.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 24890000,0.98,-0.011,-0.013,0.19,0.054,-0.062,0.045,0.18,-0.12,-3.6,-0.0015,-0.0058,8.9e-06,0.019,-0.0043,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00017,7.9e-05,7.8e-05,0.0046,0.016,0.016,0.008,0.043,0.043,0.035,5.7e-07,5.7e-07,3.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24990000,0.98,-0.011,-0.013,0.19,0.046,-0.058,0.038,0.18,-0.11,-3.6,-0.0015,-0.0058,9.2e-06,0.019,-0.005,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00017,7.9e-05,7.7e-05,0.0046,0.015,0.015,0.008,0.039,0.039,0.035,5.5e-07,5.5e-07,3.2e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +24990000,0.98,-0.011,-0.013,0.19,0.046,-0.058,0.038,0.18,-0.11,-3.6,-0.0015,-0.0058,9.2e-06,0.019,-0.005,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00017,7.9e-05,7.7e-05,0.0046,0.015,0.015,0.008,0.039,0.039,0.035,5.5e-07,5.5e-07,3.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 25090000,0.98,-0.011,-0.013,0.19,0.042,-0.057,0.035,0.18,-0.12,-3.6,-0.0015,-0.0058,9.2e-06,0.019,-0.005,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00017,7.9e-05,7.8e-05,0.0046,0.016,0.016,0.0081,0.043,0.043,0.035,5.5e-07,5.5e-07,3.2e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 25190000,0.98,-0.011,-0.013,0.19,0.037,-0.05,0.035,0.18,-0.11,-3.6,-0.0015,-0.0058,9.6e-06,0.018,-0.0055,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00017,7.9e-05,7.8e-05,0.0046,0.015,0.015,0.008,0.039,0.039,0.035,5.3e-07,5.3e-07,3.2e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 25290000,0.98,-0.011,-0.012,0.19,0.032,-0.052,0.029,0.18,-0.11,-3.6,-0.0015,-0.0058,9.6e-06,0.018,-0.0055,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8e-05,7.8e-05,0.0046,0.016,0.016,0.0081,0.043,0.043,0.036,5.3e-07,5.3e-07,3.2e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 @@ -319,8 +319,8 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 31690000,0.98,-0.006,-0.015,0.18,-0.02,0.0055,0.76,-0.04,0.039,-0.44,-0.0014,-0.0057,2.6e-05,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.3e-05,8.1e-05,0.0047,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 31790000,0.98,-0.0062,-0.015,0.18,-0.011,0.0029,0.76,-0.028,0.037,-0.36,-0.0014,-0.0057,2.6e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.3e-05,8e-05,0.0047,0.012,0.012,0.008,0.037,0.037,0.035,2.9e-07,2.9e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 31890000,0.98,-0.0059,-0.015,0.18,-0.0081,0.00064,0.76,-0.029,0.038,-0.3,-0.0014,-0.0057,2.6e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.3e-05,8e-05,0.0047,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31990000,0.98,-0.0062,-0.015,0.18,-0.00016,-2.2e-05,0.75,-0.017,0.034,-0.23,-0.0014,-0.0057,2.7e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.2e-05,8e-05,0.0047,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32090000,0.98,-0.0065,-0.014,0.18,-0.0007,-0.0034,0.76,-0.017,0.034,-0.16,-0.0014,-0.0057,2.7e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.3e-05,8e-05,0.0047,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-07,2.8e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31990000,0.98,-0.0062,-0.015,0.18,-0.00016,-2.6e-05,0.75,-0.017,0.034,-0.23,-0.0014,-0.0057,2.7e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.2e-05,8e-05,0.0047,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32090000,0.98,-0.0065,-0.014,0.18,-0.00071,-0.0034,0.76,-0.017,0.034,-0.16,-0.0014,-0.0057,2.7e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.3e-05,8e-05,0.0047,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-07,2.8e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 32190000,0.98,-0.0067,-0.015,0.18,0.0046,-0.0066,0.76,-0.006,0.033,-0.092,-0.0014,-0.0057,2.7e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.0047,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 32290000,0.98,-0.0066,-0.015,0.18,0.0062,-0.0094,0.75,-0.0055,0.032,-0.024,-0.0014,-0.0057,2.7e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.0047,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-07,2.8e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 32390000,0.98,-0.0068,-0.015,0.18,0.012,-0.011,0.75,0.0057,0.03,0.051,-0.0013,-0.0057,2.7e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.0048,0.012,0.012,0.008,0.037,0.037,0.035,2.8e-07,2.8e-07,3.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0