From a4e511b90e6c45048c431f37b0299be21235daec Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 10 Oct 2022 16:45:00 +0200 Subject: [PATCH] ekf2: migrate covariance prediction to SymForce --- src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/covariance.cpp | 686 +---------- .../EKF/python/ekf_derivation/derivation.py | 49 + .../python/ekf_derivation/derivation_utils.py | 34 +- .../generated/predict_covariance.h | 1075 +++++++++++++++++ 5 files changed, 1197 insertions(+), 648 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 7d818b3bdb..74ed0b6494 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -48,6 +48,7 @@ if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0) ${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h ${EKF_DERIVATION_DIR}/generated/compute_sideslip_h_and_k.h ${EKF_DERIVATION_DIR}/generated/compute_sideslip_innov_and_innov_var.h + ${EKF_DERIVATION_DIR}/generated/covariance_prediction.h ) add_custom_command( diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index b80cacd636..9e310a0d57 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -42,6 +42,7 @@ */ #include "ekf.h" +#include "python/ekf_derivation/generated/predict_covariance.h" #include "utils.hpp" #include @@ -99,28 +100,6 @@ void Ekf::initialiseCovariance() void Ekf::predictCovariance() { - // assign intermediate state variables - const float q0 = _state.quat_nominal(0); - const float q1 = _state.quat_nominal(1); - const float q2 = _state.quat_nominal(2); - const float q3 = _state.quat_nominal(3); - - const float dax = _imu_sample_delayed.delta_ang(0); - const float day = _imu_sample_delayed.delta_ang(1); - const float daz = _imu_sample_delayed.delta_ang(2); - - const float dvx = _imu_sample_delayed.delta_vel(0); - const float dvy = _imu_sample_delayed.delta_vel(1); - const float dvz = _imu_sample_delayed.delta_vel(2); - - const float dax_b = _state.delta_ang_bias(0); - const float day_b = _state.delta_ang_bias(1); - const float daz_b = _state.delta_ang_bias(2); - - const float dvx_b = _state.delta_vel_bias(0); - const float dvy_b = _state.delta_vel_bias(1); - const float dvz_b = _state.delta_vel_bias(2); - // 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; @@ -234,362 +213,27 @@ void Ekf::predictCovariance() // 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 daxVar = sq(dt * gyro_noise); - const float dayVar = daxVar; - const float dazVar = daxVar; + const float d_ang_var = sq(dt * gyro_noise); float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); - if (_fault_status.flags.bad_acc_vertical) { - // Increase accelerometer process noise if bad accel data is detected. Measurement errors due to - // vibration induced clipping commonly reach an equivalent 0.5g offset. - accel_noise = BADACC_BIAS_PNOISE; - } + Vector3f d_vel_var; - float dvxVar, dvyVar, dvzVar; - dvxVar = dvyVar = dvzVar = sq(dt * accel_noise); + for (int i = 0; i <= 2; i++) { + if (_fault_status.flags.bad_acc_vertical || _imu_sample_delayed.delta_vel_clipping[i]) { + // Increase accelerometer process noise if bad accel data is detected + d_vel_var(i) = sq(dt * BADACC_BIAS_PNOISE); - // Accelerometer Clipping - // delta velocity X: increase process noise if sample contained any X axis clipping - if (_imu_sample_delayed.delta_vel_clipping[0]) { - dvxVar = sq(dt * BADACC_BIAS_PNOISE); - } - - // delta velocity Y: increase process noise if sample contained any Y axis clipping - if (_imu_sample_delayed.delta_vel_clipping[1]) { - dvyVar = sq(dt * BADACC_BIAS_PNOISE); - } - - // delta velocity Z: increase process noise if sample contained any Z axis clipping - if (_imu_sample_delayed.delta_vel_clipping[2]) { - dvzVar = sq(dt * BADACC_BIAS_PNOISE); + } else { + d_vel_var(i) = sq(dt * accel_noise); + } } // predict the covariance - // equations generated using EKF/python/ekf_derivation/main.py - - // intermediate calculations - const float PS0 = ecl::powf(q1, 2); - const float PS1 = 0.25F*daxVar; - const float PS2 = ecl::powf(q2, 2); - const float PS3 = 0.25F*dayVar; - const float PS4 = ecl::powf(q3, 2); - const float PS5 = 0.25F*dazVar; - const float PS6 = 0.5F*q1; - const float PS7 = 0.5F*q2; - const float PS8 = P(10,11)*PS7; - const float PS9 = 0.5F*q3; - const float PS10 = P(10,12)*PS9; - const float PS11 = 0.5F*dax - 0.5F*dax_b; - const float PS12 = 0.5F*day - 0.5F*day_b; - const float PS13 = 0.5F*daz - 0.5F*daz_b; - const float PS14 = P(0,10) - P(1,10)*PS11 + P(10,10)*PS6 - P(2,10)*PS12 - P(3,10)*PS13 + PS10 + PS8; - const float PS15 = P(10,11)*PS6; - const float PS16 = P(11,12)*PS9; - const float PS17 = P(0,11) - P(1,11)*PS11 + P(11,11)*PS7 - P(2,11)*PS12 - P(3,11)*PS13 + PS15 + PS16; - const float PS18 = P(10,12)*PS6; - const float PS19 = P(11,12)*PS7; - const float PS20 = P(0,12) - P(1,12)*PS11 + P(12,12)*PS9 - P(2,12)*PS12 - P(3,12)*PS13 + PS18 + PS19; - const float PS21 = P(1,2)*PS12; - const float PS22 = -P(1,3)*PS13; - const float PS23 = P(0,1) - P(1,1)*PS11 + P(1,10)*PS6 + P(1,11)*PS7 + P(1,12)*PS9 - PS21 + PS22; - const float PS24 = -P(1,2)*PS11; - const float PS25 = P(2,3)*PS13; - const float PS26 = P(0,2) + P(2,10)*PS6 + P(2,11)*PS7 + P(2,12)*PS9 - P(2,2)*PS12 + PS24 - PS25; - const float PS27 = P(1,3)*PS11; - const float PS28 = -P(2,3)*PS12; - const float PS29 = P(0,3) + P(3,10)*PS6 + P(3,11)*PS7 + P(3,12)*PS9 - P(3,3)*PS13 - PS27 + PS28; - const float PS30 = P(0,1)*PS11; - const float PS31 = P(0,2)*PS12; - const float PS32 = P(0,3)*PS13; - const float PS33 = P(0,0) + P(0,10)*PS6 + P(0,11)*PS7 + P(0,12)*PS9 - PS30 - PS31 - PS32; - const float PS34 = 0.5F*q0; - const float PS35 = q2*q3; - const float PS36 = q0*q1; - const float PS37 = q1*q3; - const float PS38 = q0*q2; - const float PS39 = q1*q2; - const float PS40 = q0*q3; - const float PS41 = 2*PS2; - const float PS42 = 2*PS4 - 1; - const float PS43 = PS41 + PS42; - const float PS44 = P(0,13) - P(1,13)*PS11 + P(10,13)*PS6 + P(11,13)*PS7 + P(12,13)*PS9 - P(2,13)*PS12 - P(3,13)*PS13; - const float PS45 = PS37 + PS38; - const float PS46 = P(0,15) - P(1,15)*PS11 + P(10,15)*PS6 + P(11,15)*PS7 + P(12,15)*PS9 - P(2,15)*PS12 - P(3,15)*PS13; - const float PS47 = 2*PS46; - const float PS48 = dvy - dvy_b; - const float PS49 = PS48*q0; - const float PS50 = dvz - dvz_b; - const float PS51 = PS50*q1; - const float PS52 = dvx - dvx_b; - const float PS53 = PS52*q3; - const float PS54 = PS49 - PS51 + 2*PS53; - const float PS55 = 2*PS29; - const float PS56 = -PS39 + PS40; - const float PS57 = P(0,14) - P(1,14)*PS11 + P(10,14)*PS6 + P(11,14)*PS7 + P(12,14)*PS9 - P(2,14)*PS12 - P(3,14)*PS13; - const float PS58 = 2*PS57; - const float PS59 = PS48*q2; - const float PS60 = PS50*q3; - const float PS61 = PS59 + PS60; - const float PS62 = 2*PS23; - const float PS63 = PS50*q2; - const float PS64 = PS48*q3; - const float PS65 = -PS64; - const float PS66 = PS63 + PS65; - const float PS67 = 2*PS33; - const float PS68 = PS50*q0; - const float PS69 = PS48*q1; - const float PS70 = PS52*q2; - const float PS71 = PS68 + PS69 - 2*PS70; - const float PS72 = 2*PS26; - const float PS73 = P(0,4) - P(1,4)*PS11 - P(2,4)*PS12 - P(3,4)*PS13 + P(4,10)*PS6 + P(4,11)*PS7 + P(4,12)*PS9; - const float PS74 = 2*PS0; - const float PS75 = PS42 + PS74; - const float PS76 = PS39 + PS40; - const float PS77 = 2*PS44; - const float PS78 = PS51 - PS53; - const float PS79 = -PS70; - const float PS80 = PS68 + 2*PS69 + PS79; - const float PS81 = -PS35 + PS36; - const float PS82 = PS52*q1; - const float PS83 = PS60 + PS82; - const float PS84 = PS52*q0; - const float PS85 = PS63 - 2*PS64 + PS84; - const float PS86 = P(0,5) - P(1,5)*PS11 - P(2,5)*PS12 - P(3,5)*PS13 + P(5,10)*PS6 + P(5,11)*PS7 + P(5,12)*PS9; - const float PS87 = PS41 + PS74 - 1; - const float PS88 = PS35 + PS36; - const float PS89 = 2*PS63 + PS65 + PS84; - const float PS90 = -PS37 + PS38; - const float PS91 = PS59 + PS82; - const float PS92 = PS69 + PS79; - const float PS93 = PS49 - 2*PS51 + PS53; - const float PS94 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9; - const float PS95 = ecl::powf(q0, 2); - const float PS96 = -P(10,11)*PS34; - const float PS97 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS96; - const float PS98 = P(0,2)*PS13; - const float PS99 = P(0,3)*PS12; - const float PS100 = P(0,0)*PS11 + P(0,1) - P(0,10)*PS34 + P(0,11)*PS9 - P(0,12)*PS7 + PS98 - PS99; - const float PS101 = P(0,2)*PS11; - const float PS102 = P(1,2) - P(2,10)*PS34 + P(2,11)*PS9 - P(2,12)*PS7 + P(2,2)*PS13 + PS101 + PS28; - const float PS103 = P(10,11)*PS9; - const float PS104 = P(10,12)*PS7; - const float PS105 = P(0,10)*PS11 + P(1,10) - P(10,10)*PS34 + P(2,10)*PS13 - P(3,10)*PS12 + PS103 - PS104; - const float PS106 = -P(10,12)*PS34; - const float PS107 = P(0,12)*PS11 + P(1,12) - P(12,12)*PS7 + P(2,12)*PS13 - P(3,12)*PS12 + PS106 + PS16; - const float PS108 = P(0,3)*PS11; - const float PS109 = P(1,3) - P(3,10)*PS34 + P(3,11)*PS9 - P(3,12)*PS7 - P(3,3)*PS12 + PS108 + PS25; - const float PS110 = P(1,2)*PS13; - const float PS111 = P(1,3)*PS12; - const float PS112 = P(1,1) - P(1,10)*PS34 + P(1,11)*PS9 - P(1,12)*PS7 + PS110 - PS111 + PS30; - const float PS113 = P(0,13)*PS11 + P(1,13) - P(10,13)*PS34 + P(11,13)*PS9 - P(12,13)*PS7 + P(2,13)*PS13 - P(3,13)*PS12; - const float PS114 = P(0,15)*PS11 + P(1,15) - P(10,15)*PS34 + P(11,15)*PS9 - P(12,15)*PS7 + P(2,15)*PS13 - P(3,15)*PS12; - const float PS115 = 2*PS114; - const float PS116 = 2*PS109; - const float PS117 = P(0,14)*PS11 + P(1,14) - P(10,14)*PS34 + P(11,14)*PS9 - P(12,14)*PS7 + P(2,14)*PS13 - P(3,14)*PS12; - const float PS118 = 2*PS117; - const float PS119 = 2*PS112; - const float PS120 = 2*PS100; - const float PS121 = 2*PS102; - const float PS122 = P(0,4)*PS11 + P(1,4) + P(2,4)*PS13 - P(3,4)*PS12 - P(4,10)*PS34 + P(4,11)*PS9 - P(4,12)*PS7; - const float PS123 = 2*PS113; - const float PS124 = P(0,5)*PS11 + P(1,5) + P(2,5)*PS13 - P(3,5)*PS12 - P(5,10)*PS34 + P(5,11)*PS9 - P(5,12)*PS7; - const float PS125 = P(0,6)*PS11 + P(1,6) + P(2,6)*PS13 - P(3,6)*PS12 - P(6,10)*PS34 + P(6,11)*PS9 - P(6,12)*PS7; - const float PS126 = -P(11,12)*PS34; - const float PS127 = P(0,12)*PS12 - P(1,12)*PS13 + P(12,12)*PS6 + P(2,12) + P(3,12)*PS11 - PS10 + PS126; - const float PS128 = P(2,3) - P(3,10)*PS9 - P(3,11)*PS34 + P(3,12)*PS6 + P(3,3)*PS11 + PS22 + PS99; - const float PS129 = P(0,1)*PS13; - const float PS130 = P(0,0)*PS12 - P(0,10)*PS9 - P(0,11)*PS34 + P(0,12)*PS6 + P(0,2) + PS108 - PS129; - const float PS131 = P(11,12)*PS6; - const float PS132 = P(0,11)*PS12 - P(1,11)*PS13 - P(11,11)*PS34 + P(2,11) + P(3,11)*PS11 - PS103 + PS131; - const float PS133 = P(0,10)*PS12 - P(1,10)*PS13 - P(10,10)*PS9 + P(2,10) + P(3,10)*PS11 + PS18 + PS96; - const float PS134 = P(0,1)*PS12; - const float PS135 = -P(1,1)*PS13 - P(1,10)*PS9 - P(1,11)*PS34 + P(1,12)*PS6 + P(1,2) + PS134 + PS27; - const float PS136 = P(2,3)*PS11; - const float PS137 = -P(2,10)*PS9 - P(2,11)*PS34 + P(2,12)*PS6 + P(2,2) - PS110 + PS136 + PS31; - const float PS138 = P(0,13)*PS12 - P(1,13)*PS13 - P(10,13)*PS9 - P(11,13)*PS34 + P(12,13)*PS6 + P(2,13) + P(3,13)*PS11; - const float PS139 = P(0,15)*PS12 - P(1,15)*PS13 - P(10,15)*PS9 - P(11,15)*PS34 + P(12,15)*PS6 + P(2,15) + P(3,15)*PS11; - const float PS140 = 2*PS139; - const float PS141 = 2*PS128; - const float PS142 = P(0,14)*PS12 - P(1,14)*PS13 - P(10,14)*PS9 - P(11,14)*PS34 + P(12,14)*PS6 + P(2,14) + P(3,14)*PS11; - const float PS143 = 2*PS142; - const float PS144 = 2*PS135; - const float PS145 = 2*PS130; - const float PS146 = 2*PS137; - const float PS147 = P(0,4)*PS12 - P(1,4)*PS13 + P(2,4) + P(3,4)*PS11 - P(4,10)*PS9 - P(4,11)*PS34 + P(4,12)*PS6; - const float PS148 = 2*PS138; - const float PS149 = P(0,5)*PS12 - P(1,5)*PS13 + P(2,5) + P(3,5)*PS11 - P(5,10)*PS9 - P(5,11)*PS34 + P(5,12)*PS6; - const float PS150 = P(0,6)*PS12 - P(1,6)*PS13 + P(2,6) + P(3,6)*PS11 - P(6,10)*PS9 - P(6,11)*PS34 + P(6,12)*PS6; - const float PS151 = P(0,10)*PS13 + P(1,10)*PS12 + P(10,10)*PS7 - P(2,10)*PS11 + P(3,10) + PS106 - PS15; - const float PS152 = P(1,1)*PS12 + P(1,10)*PS7 - P(1,11)*PS6 - P(1,12)*PS34 + P(1,3) + PS129 + PS24; - const float PS153 = P(0,0)*PS13 + P(0,10)*PS7 - P(0,11)*PS6 - P(0,12)*PS34 + P(0,3) - PS101 + PS134; - const float PS154 = P(0,12)*PS13 + P(1,12)*PS12 - P(12,12)*PS34 - P(2,12)*PS11 + P(3,12) + PS104 - PS131; - const float PS155 = P(0,11)*PS13 + P(1,11)*PS12 - P(11,11)*PS6 - P(2,11)*PS11 + P(3,11) + PS126 + PS8; - const float PS156 = P(2,10)*PS7 - P(2,11)*PS6 - P(2,12)*PS34 - P(2,2)*PS11 + P(2,3) + PS21 + PS98; - const float PS157 = P(3,10)*PS7 - P(3,11)*PS6 - P(3,12)*PS34 + P(3,3) + PS111 - PS136 + PS32; - const float PS158 = P(0,13)*PS13 + P(1,13)*PS12 + P(10,13)*PS7 - P(11,13)*PS6 - P(12,13)*PS34 - P(2,13)*PS11 + P(3,13); - const float PS159 = P(0,15)*PS13 + P(1,15)*PS12 + P(10,15)*PS7 - P(11,15)*PS6 - P(12,15)*PS34 - P(2,15)*PS11 + P(3,15); - const float PS160 = 2*PS159; - const float PS161 = 2*PS157; - const float PS162 = P(0,14)*PS13 + P(1,14)*PS12 + P(10,14)*PS7 - P(11,14)*PS6 - P(12,14)*PS34 - P(2,14)*PS11 + P(3,14); - const float PS163 = 2*PS162; - const float PS164 = 2*PS152; - const float PS165 = 2*PS153; - const float PS166 = 2*PS156; - const float PS167 = P(0,4)*PS13 + P(1,4)*PS12 - P(2,4)*PS11 + P(3,4) + P(4,10)*PS7 - P(4,11)*PS6 - P(4,12)*PS34; - const float PS168 = 2*PS158; - const float PS169 = P(0,5)*PS13 + P(1,5)*PS12 - P(2,5)*PS11 + P(3,5) + P(5,10)*PS7 - P(5,11)*PS6 - P(5,12)*PS34; - const float PS170 = P(0,6)*PS13 + P(1,6)*PS12 - P(2,6)*PS11 + P(3,6) + P(6,10)*PS7 - P(6,11)*PS6 - P(6,12)*PS34; - const float PS171 = 2*PS45; - const float PS172 = 2*PS56; - const float PS173 = 2*PS61; - const float PS174 = 2*PS66; - const float PS175 = 2*PS71; - const float PS176 = 2*PS54; - const float PS177 = P(0,13)*PS174 + P(1,13)*PS173 + P(13,13)*PS43 + P(13,14)*PS172 - P(13,15)*PS171 + P(2,13)*PS175 - P(3,13)*PS176 + P(4,13); - const float PS178 = P(0,15)*PS174 + P(1,15)*PS173 + P(13,15)*PS43 + P(14,15)*PS172 - P(15,15)*PS171 + P(2,15)*PS175 - P(3,15)*PS176 + P(4,15); - const float PS179 = P(0,3)*PS174 + P(1,3)*PS173 + P(2,3)*PS175 + P(3,13)*PS43 + P(3,14)*PS172 - P(3,15)*PS171 - P(3,3)*PS176 + P(3,4); - const float PS180 = P(0,14)*PS174 + P(1,14)*PS173 + P(13,14)*PS43 + P(14,14)*PS172 - P(14,15)*PS171 + P(2,14)*PS175 - P(3,14)*PS176 + P(4,14); - const float PS181 = P(0,1)*PS174 + P(1,1)*PS173 + P(1,13)*PS43 + P(1,14)*PS172 - P(1,15)*PS171 + P(1,2)*PS175 - P(1,3)*PS176 + P(1,4); - const float PS182 = P(0,0)*PS174 + P(0,1)*PS173 + P(0,13)*PS43 + P(0,14)*PS172 - P(0,15)*PS171 + P(0,2)*PS175 - P(0,3)*PS176 + P(0,4); - const float PS183 = P(0,2)*PS174 + P(1,2)*PS173 + P(2,13)*PS43 + P(2,14)*PS172 - P(2,15)*PS171 + P(2,2)*PS175 - P(2,3)*PS176 + P(2,4); - const float PS184 = 4*dvyVar; - const float PS185 = 4*dvzVar; - const float PS186 = P(0,4)*PS174 + P(1,4)*PS173 + P(2,4)*PS175 - P(3,4)*PS176 + P(4,13)*PS43 + P(4,14)*PS172 - P(4,15)*PS171 + P(4,4); - const float PS187 = 2*PS177; - const float PS188 = 2*PS182; - const float PS189 = 2*PS181; - const float PS190 = 2*PS81; - const float PS191 = 2*PS183; - const float PS192 = 2*PS179; - const float PS193 = 2*PS76; - const float PS194 = PS43*dvxVar; - const float PS195 = PS75*dvyVar; - const float PS196 = P(0,5)*PS174 + P(1,5)*PS173 + P(2,5)*PS175 - P(3,5)*PS176 + P(4,5) + P(5,13)*PS43 + P(5,14)*PS172 - P(5,15)*PS171; - const float PS197 = 2*PS88; - const float PS198 = PS87*dvzVar; - const float PS199 = 2*PS90; - const float PS200 = P(0,6)*PS174 + P(1,6)*PS173 + P(2,6)*PS175 - P(3,6)*PS176 + P(4,6) + P(6,13)*PS43 + P(6,14)*PS172 - P(6,15)*PS171; - const float PS201 = 2*PS83; - const float PS202 = 2*PS78; - const float PS203 = 2*PS85; - const float PS204 = 2*PS80; - const float PS205 = -P(0,14)*PS202 - P(1,14)*PS204 - P(13,14)*PS193 + P(14,14)*PS75 + P(14,15)*PS190 + P(2,14)*PS201 + P(3,14)*PS203 + P(5,14); - const float PS206 = -P(0,13)*PS202 - P(1,13)*PS204 - P(13,13)*PS193 + P(13,14)*PS75 + P(13,15)*PS190 + P(2,13)*PS201 + P(3,13)*PS203 + P(5,13); - const float PS207 = -P(0,0)*PS202 - P(0,1)*PS204 - P(0,13)*PS193 + P(0,14)*PS75 + P(0,15)*PS190 + P(0,2)*PS201 + P(0,3)*PS203 + P(0,5); - const float PS208 = -P(0,1)*PS202 - P(1,1)*PS204 - P(1,13)*PS193 + P(1,14)*PS75 + P(1,15)*PS190 + P(1,2)*PS201 + P(1,3)*PS203 + P(1,5); - const float PS209 = -P(0,15)*PS202 - P(1,15)*PS204 - P(13,15)*PS193 + P(14,15)*PS75 + P(15,15)*PS190 + P(2,15)*PS201 + P(3,15)*PS203 + P(5,15); - const float PS210 = -P(0,2)*PS202 - P(1,2)*PS204 - P(2,13)*PS193 + P(2,14)*PS75 + P(2,15)*PS190 + P(2,2)*PS201 + P(2,3)*PS203 + P(2,5); - const float PS211 = -P(0,3)*PS202 - P(1,3)*PS204 + P(2,3)*PS201 - P(3,13)*PS193 + P(3,14)*PS75 + P(3,15)*PS190 + P(3,3)*PS203 + P(3,5); - const float PS212 = 4*dvxVar; - const float PS213 = -P(0,5)*PS202 - P(1,5)*PS204 + P(2,5)*PS201 + P(3,5)*PS203 - P(5,13)*PS193 + P(5,14)*PS75 + P(5,15)*PS190 + P(5,5); - const float PS214 = 2*PS89; - const float PS215 = 2*PS91; - const float PS216 = 2*PS92; - const float PS217 = 2*PS93; - const float PS218 = -P(0,6)*PS202 - P(1,6)*PS204 + P(2,6)*PS201 + P(3,6)*PS203 + P(5,6) - P(6,13)*PS193 + P(6,14)*PS75 + P(6,15)*PS190; - const float PS219 = P(0,15)*PS216 + P(1,15)*PS217 + P(13,15)*PS199 - P(14,15)*PS197 + P(15,15)*PS87 - P(2,15)*PS214 + P(3,15)*PS215 + P(6,15); - const float PS220 = P(0,14)*PS216 + P(1,14)*PS217 + P(13,14)*PS199 - P(14,14)*PS197 + P(14,15)*PS87 - P(2,14)*PS214 + P(3,14)*PS215 + P(6,14); - const float PS221 = P(0,13)*PS216 + P(1,13)*PS217 + P(13,13)*PS199 - P(13,14)*PS197 + P(13,15)*PS87 - P(2,13)*PS214 + P(3,13)*PS215 + P(6,13); - const float PS222 = P(0,6)*PS216 + P(1,6)*PS217 - P(2,6)*PS214 + P(3,6)*PS215 + P(6,13)*PS199 - P(6,14)*PS197 + P(6,15)*PS87 + P(6,6); - - - // covariance update SquareMatrix24f nextP; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states - - nextP(0,0) = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5; - nextP(0,1) = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5; - nextP(1,1) = PS1*PS95 + PS100*PS11 + PS102*PS13 - PS105*PS34 - PS107*PS7 - PS109*PS12 + PS112 + PS2*PS5 + PS3*PS4 + PS9*PS97; - nextP(0,2) = -PS1*PS37 + PS11*PS29 + PS12*PS33 - PS13*PS23 - PS14*PS9 - PS17*PS34 + PS20*PS6 + PS26 - PS3*PS38 + PS37*PS5; - nextP(1,2) = PS1*PS40 + PS100*PS12 + PS102 - PS105*PS9 + PS107*PS6 + PS109*PS11 - PS112*PS13 - PS3*PS40 - PS34*PS97 - PS39*PS5; - nextP(2,2) = PS0*PS5 + PS1*PS4 + PS11*PS128 + PS12*PS130 + PS127*PS6 - PS13*PS135 - PS132*PS34 - PS133*PS9 + PS137 + PS3*PS95; - nextP(0,3) = PS1*PS39 - PS11*PS26 + PS12*PS23 + PS13*PS33 + PS14*PS7 - PS17*PS6 - PS20*PS34 + PS29 - PS3*PS39 - PS40*PS5; - nextP(1,3) = -PS1*PS38 + PS100*PS13 - PS102*PS11 + PS105*PS7 - PS107*PS34 + PS109 + PS112*PS12 - PS3*PS37 + PS38*PS5 - PS6*PS97; - nextP(2,3) = -PS1*PS35 - PS11*PS137 + PS12*PS135 - PS127*PS34 + PS128 + PS13*PS130 - PS132*PS6 + PS133*PS7 + PS3*PS36 - PS36*PS5; - nextP(3,3) = PS0*PS3 + PS1*PS2 - PS11*PS156 + PS12*PS152 + PS13*PS153 + PS151*PS7 - PS154*PS34 - PS155*PS6 + PS157 + PS5*PS95; - nextP(0,4) = PS43*PS44 - PS45*PS47 - PS54*PS55 + PS56*PS58 + PS61*PS62 + PS66*PS67 + PS71*PS72 + PS73; - nextP(1,4) = PS113*PS43 - PS115*PS45 - PS116*PS54 + PS118*PS56 + PS119*PS61 + PS120*PS66 + PS121*PS71 + PS122; - nextP(2,4) = PS138*PS43 - PS140*PS45 - PS141*PS54 + PS143*PS56 + PS144*PS61 + PS145*PS66 + PS146*PS71 + PS147; - nextP(3,4) = PS158*PS43 - PS160*PS45 - PS161*PS54 + PS163*PS56 + PS164*PS61 + PS165*PS66 + PS166*PS71 + PS167; - nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*ecl::powf(PS56, 2) + PS185*ecl::powf(PS45, 2) + PS186 + ecl::powf(PS43, 2)*dvxVar; - nextP(0,5) = PS47*PS81 + PS55*PS85 + PS57*PS75 - PS62*PS80 - PS67*PS78 + PS72*PS83 - PS76*PS77 + PS86; - nextP(1,5) = PS115*PS81 + PS116*PS85 + PS117*PS75 - PS119*PS80 - PS120*PS78 + PS121*PS83 - PS123*PS76 + PS124; - nextP(2,5) = PS140*PS81 + PS141*PS85 + PS142*PS75 - PS144*PS80 - PS145*PS78 + PS146*PS83 - PS148*PS76 + PS149; - nextP(3,5) = PS160*PS81 + PS161*PS85 + PS162*PS75 - PS164*PS80 - PS165*PS78 + PS166*PS83 - PS168*PS76 + PS169; - nextP(4,5) = PS172*PS195 + PS178*PS190 + PS180*PS75 - PS185*PS45*PS81 - PS187*PS76 - PS188*PS78 - PS189*PS80 + PS191*PS83 + PS192*PS85 - PS193*PS194 + PS196; - nextP(5,5) = PS185*ecl::powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*ecl::powf(PS76, 2) + PS213 + ecl::powf(PS75, 2)*dvyVar; - nextP(0,6) = PS46*PS87 + PS55*PS91 - PS58*PS88 + PS62*PS93 + PS67*PS92 - PS72*PS89 + PS77*PS90 + PS94; - nextP(1,6) = PS114*PS87 + PS116*PS91 - PS118*PS88 + PS119*PS93 + PS120*PS92 - PS121*PS89 + PS123*PS90 + PS125; - nextP(2,6) = PS139*PS87 + PS141*PS91 - PS143*PS88 + PS144*PS93 + PS145*PS92 - PS146*PS89 + PS148*PS90 + PS150; - nextP(3,6) = PS159*PS87 + PS161*PS91 - PS163*PS88 + PS164*PS93 + PS165*PS92 - PS166*PS89 + PS168*PS90 + PS170; - nextP(4,6) = -PS171*PS198 + PS178*PS87 - PS180*PS197 - PS184*PS56*PS88 + PS187*PS90 + PS188*PS92 + PS189*PS93 - PS191*PS89 + PS192*PS91 + PS194*PS199 + PS200; - nextP(5,6) = PS190*PS198 - PS195*PS197 - PS197*PS205 + PS199*PS206 + PS207*PS216 + PS208*PS217 + PS209*PS87 - PS210*PS214 + PS211*PS215 - PS212*PS76*PS90 + PS218; - nextP(6,6) = PS184*ecl::powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*ecl::powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + ecl::powf(PS87, 2)*dvzVar; - nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS73*dt; - nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS122*dt; - nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS147*dt; - nextP(3,7) = P(0,7)*PS13 + P(1,7)*PS12 - P(2,7)*PS11 + P(3,7) + P(7,10)*PS7 - P(7,11)*PS6 - P(7,12)*PS34 + PS167*dt; - nextP(4,7) = P(0,7)*PS174 + P(1,7)*PS173 + P(2,7)*PS175 - P(3,7)*PS176 + P(4,7) + P(7,13)*PS43 + P(7,14)*PS172 - P(7,15)*PS171 + PS186*dt; - nextP(5,7) = -P(0,7)*PS202 - P(1,7)*PS204 + P(2,7)*PS201 + P(3,7)*PS203 + P(5,7) - P(7,13)*PS193 + P(7,14)*PS75 + P(7,15)*PS190 + dt*(-P(0,4)*PS202 - P(1,4)*PS204 + P(2,4)*PS201 + P(3,4)*PS203 - P(4,13)*PS193 + P(4,14)*PS75 + P(4,15)*PS190 + P(4,5)); - nextP(6,7) = P(0,7)*PS216 + P(1,7)*PS217 - P(2,7)*PS214 + P(3,7)*PS215 + P(6,7) + P(7,13)*PS199 - P(7,14)*PS197 + P(7,15)*PS87 + dt*(P(0,4)*PS216 + P(1,4)*PS217 - P(2,4)*PS214 + P(3,4)*PS215 + P(4,13)*PS199 - P(4,14)*PS197 + P(4,15)*PS87 + P(4,6)); - nextP(7,7) = P(4,7)*dt + P(7,7) + dt*(P(4,4)*dt + P(4,7)); - nextP(0,8) = P(0,8) - P(1,8)*PS11 - P(2,8)*PS12 - P(3,8)*PS13 + P(8,10)*PS6 + P(8,11)*PS7 + P(8,12)*PS9 + PS86*dt; - nextP(1,8) = P(0,8)*PS11 + P(1,8) + P(2,8)*PS13 - P(3,8)*PS12 - P(8,10)*PS34 + P(8,11)*PS9 - P(8,12)*PS7 + PS124*dt; - nextP(2,8) = P(0,8)*PS12 - P(1,8)*PS13 + P(2,8) + P(3,8)*PS11 - P(8,10)*PS9 - P(8,11)*PS34 + P(8,12)*PS6 + PS149*dt; - nextP(3,8) = P(0,8)*PS13 + P(1,8)*PS12 - P(2,8)*PS11 + P(3,8) + P(8,10)*PS7 - P(8,11)*PS6 - P(8,12)*PS34 + PS169*dt; - nextP(4,8) = P(0,8)*PS174 + P(1,8)*PS173 + P(2,8)*PS175 - P(3,8)*PS176 + P(4,8) + P(8,13)*PS43 + P(8,14)*PS172 - P(8,15)*PS171 + PS196*dt; - nextP(5,8) = -P(0,8)*PS202 - P(1,8)*PS204 + P(2,8)*PS201 + P(3,8)*PS203 + P(5,8) - P(8,13)*PS193 + P(8,14)*PS75 + P(8,15)*PS190 + PS213*dt; - nextP(6,8) = P(0,8)*PS216 + P(1,8)*PS217 - P(2,8)*PS214 + P(3,8)*PS215 + P(6,8) + P(8,13)*PS199 - P(8,14)*PS197 + P(8,15)*PS87 + dt*(P(0,5)*PS216 + P(1,5)*PS217 - P(2,5)*PS214 + P(3,5)*PS215 + P(5,13)*PS199 - P(5,14)*PS197 + P(5,15)*PS87 + P(5,6)); - nextP(7,8) = P(4,8)*dt + P(7,8) + dt*(P(4,5)*dt + P(5,7)); - nextP(8,8) = P(5,8)*dt + P(8,8) + dt*(P(5,5)*dt + P(5,8)); - nextP(0,9) = P(0,9) - P(1,9)*PS11 - P(2,9)*PS12 - P(3,9)*PS13 + P(9,10)*PS6 + P(9,11)*PS7 + P(9,12)*PS9 + PS94*dt; - nextP(1,9) = P(0,9)*PS11 + P(1,9) + P(2,9)*PS13 - P(3,9)*PS12 - P(9,10)*PS34 + P(9,11)*PS9 - P(9,12)*PS7 + PS125*dt; - nextP(2,9) = P(0,9)*PS12 - P(1,9)*PS13 + P(2,9) + P(3,9)*PS11 - P(9,10)*PS9 - P(9,11)*PS34 + P(9,12)*PS6 + PS150*dt; - nextP(3,9) = P(0,9)*PS13 + P(1,9)*PS12 - P(2,9)*PS11 + P(3,9) + P(9,10)*PS7 - P(9,11)*PS6 - P(9,12)*PS34 + PS170*dt; - nextP(4,9) = P(0,9)*PS174 + P(1,9)*PS173 + P(2,9)*PS175 - P(3,9)*PS176 + P(4,9) + P(9,13)*PS43 + P(9,14)*PS172 - P(9,15)*PS171 + PS200*dt; - nextP(5,9) = -P(0,9)*PS202 - P(1,9)*PS204 + P(2,9)*PS201 + P(3,9)*PS203 + P(5,9) - P(9,13)*PS193 + P(9,14)*PS75 + P(9,15)*PS190 + PS218*dt; - nextP(6,9) = P(0,9)*PS216 + P(1,9)*PS217 - P(2,9)*PS214 + P(3,9)*PS215 + P(6,9) + P(9,13)*PS199 - P(9,14)*PS197 + P(9,15)*PS87 + PS222*dt; - nextP(7,9) = P(4,9)*dt + P(7,9) + dt*(P(4,6)*dt + P(6,7)); - nextP(8,9) = P(5,9)*dt + P(8,9) + dt*(P(5,6)*dt + P(6,8)); - nextP(9,9) = P(6,9)*dt + P(9,9) + dt*(P(6,6)*dt + P(6,9)); - nextP(0,10) = PS14; - nextP(1,10) = PS105; - nextP(2,10) = PS133; - nextP(3,10) = PS151; - nextP(4,10) = P(0,10)*PS174 + P(1,10)*PS173 + P(10,13)*PS43 + P(10,14)*PS172 - P(10,15)*PS171 + P(2,10)*PS175 - P(3,10)*PS176 + P(4,10); - nextP(5,10) = -P(0,10)*PS202 - P(1,10)*PS204 - P(10,13)*PS193 + P(10,14)*PS75 + P(10,15)*PS190 + P(2,10)*PS201 + P(3,10)*PS203 + P(5,10); - nextP(6,10) = P(0,10)*PS216 + P(1,10)*PS217 + P(10,13)*PS199 - P(10,14)*PS197 + P(10,15)*PS87 - P(2,10)*PS214 + P(3,10)*PS215 + P(6,10); - nextP(7,10) = P(4,10)*dt + P(7,10); - nextP(8,10) = P(5,10)*dt + P(8,10); - nextP(9,10) = P(6,10)*dt + P(9,10); - nextP(10,10) = P(10,10); - nextP(0,11) = PS17; - nextP(1,11) = PS97; - nextP(2,11) = PS132; - nextP(3,11) = PS155; - nextP(4,11) = P(0,11)*PS174 + P(1,11)*PS173 + P(11,13)*PS43 + P(11,14)*PS172 - P(11,15)*PS171 + P(2,11)*PS175 - P(3,11)*PS176 + P(4,11); - nextP(5,11) = -P(0,11)*PS202 - P(1,11)*PS204 - P(11,13)*PS193 + P(11,14)*PS75 + P(11,15)*PS190 + P(2,11)*PS201 + P(3,11)*PS203 + P(5,11); - nextP(6,11) = P(0,11)*PS216 + P(1,11)*PS217 + P(11,13)*PS199 - P(11,14)*PS197 + P(11,15)*PS87 - P(2,11)*PS214 + P(3,11)*PS215 + P(6,11); - nextP(7,11) = P(4,11)*dt + P(7,11); - nextP(8,11) = P(5,11)*dt + P(8,11); - nextP(9,11) = P(6,11)*dt + P(9,11); - nextP(10,11) = P(10,11); - nextP(11,11) = P(11,11); - nextP(0,12) = PS20; - nextP(1,12) = PS107; - nextP(2,12) = PS127; - nextP(3,12) = PS154; - nextP(4,12) = P(0,12)*PS174 + P(1,12)*PS173 + P(12,13)*PS43 + P(12,14)*PS172 - P(12,15)*PS171 + P(2,12)*PS175 - P(3,12)*PS176 + P(4,12); - nextP(5,12) = -P(0,12)*PS202 - P(1,12)*PS204 - P(12,13)*PS193 + P(12,14)*PS75 + P(12,15)*PS190 + P(2,12)*PS201 + P(3,12)*PS203 + P(5,12); - nextP(6,12) = P(0,12)*PS216 + P(1,12)*PS217 + P(12,13)*PS199 - P(12,14)*PS197 + P(12,15)*PS87 - P(2,12)*PS214 + P(3,12)*PS215 + P(6,12); - nextP(7,12) = P(4,12)*dt + P(7,12); - nextP(8,12) = P(5,12)*dt + P(8,12); - nextP(9,12) = P(6,12)*dt + P(9,12); - nextP(10,12) = P(10,12); - nextP(11,12) = P(11,12); - nextP(12,12) = P(12,12); + sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, _imu_sample_delayed.delta_vel, d_vel_var, _imu_sample_delayed.delta_ang, d_ang_var, dt, &nextP); // process noise contribution for delta angle states can be very small compared to // the variances, therefore use algorithm to minimise numerical error @@ -598,281 +242,24 @@ void Ekf::predictCovariance() nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index)); } - if (!_accel_bias_inhibit[0]) { - // calculate variances and upper diagonal covariances for IMU X axis delta velocity bias state - nextP(0,13) = PS44; - nextP(1,13) = PS113; - nextP(2,13) = PS138; - nextP(3,13) = PS158; - nextP(4,13) = PS177; - nextP(5,13) = PS206; - nextP(6,13) = PS221; - nextP(7,13) = P(4,13)*dt + P(7,13); - nextP(8,13) = P(5,13)*dt + P(8,13); - nextP(9,13) = P(6,13)*dt + P(9,13); - nextP(10,13) = P(10,13); - nextP(11,13) = P(11,13); - nextP(12,13) = P(12,13); - nextP(13,13) = P(13,13); + for (int i = 13; i <= 15; i++) { + const int index = i - 13; - // add process noise that is not from the IMU - // process noise contribution for delta velocity states can be very small compared to - // the variances, therefore use algorithm to minimise numerical error - nextP(13, 13) = kahanSummation(nextP(13, 13), process_noise(13), _delta_vel_bias_var_accum(0)); + if (!_accel_bias_inhibit[index]) { + // add process noise that is not from the IMU + // process noise contribution for delta velocity states can be very small compared to + // the variances, therefore use algorithm to minimise numerical error + nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_vel_bias_var_accum(index)); - } else { - nextP.uncorrelateCovarianceSetVariance<1>(13, _prev_dvel_bias_var(0)); - _delta_vel_bias_var_accum(0) = 0.f; - - } - - if (!_accel_bias_inhibit[1]) { - // calculate variances and upper diagonal covariances for IMU Y axis delta velocity bias state - - nextP(0,14) = PS57; - nextP(1,14) = PS117; - nextP(2,14) = PS142; - nextP(3,14) = PS162; - nextP(4,14) = PS180; - nextP(5,14) = PS205; - nextP(6,14) = PS220; - nextP(7,14) = P(4,14)*dt + P(7,14); - nextP(8,14) = P(5,14)*dt + P(8,14); - nextP(9,14) = P(6,14)*dt + P(9,14); - nextP(10,14) = P(10,14); - nextP(11,14) = P(11,14); - nextP(12,14) = P(12,14); - nextP(13,14) = P(13,14); - nextP(14,14) = P(14,14); - - // add process noise that is not from the IMU - // process noise contribution for delta velocity states can be very small compared to - // the variances, therefore use algorithm to minimise numerical error - nextP(14, 14) = kahanSummation(nextP(14, 14), process_noise(14), _delta_vel_bias_var_accum(1)); - - } else { - nextP.uncorrelateCovarianceSetVariance<1>(14, _prev_dvel_bias_var(1)); - _delta_vel_bias_var_accum(1) = 0.f; - - } - - if (!_accel_bias_inhibit[2]) { - // calculate variances and upper diagonal covariances for IMU Z axis delta velocity bias state - nextP(0,15) = PS46; - nextP(1,15) = PS114; - nextP(2,15) = PS139; - nextP(3,15) = PS159; - nextP(4,15) = PS178; - nextP(5,15) = PS209; - nextP(6,15) = PS219; - nextP(7,15) = P(4,15)*dt + P(7,15); - nextP(8,15) = P(5,15)*dt + P(8,15); - nextP(9,15) = P(6,15)*dt + P(9,15); - nextP(10,15) = P(10,15); - nextP(11,15) = P(11,15); - nextP(12,15) = P(12,15); - nextP(13,15) = P(13,15); - nextP(14,15) = P(14,15); - nextP(15,15) = P(15,15); - - // add process noise that is not from the IMU - // process noise contribution for delta velocity states can be very small compared to - // the variances, therefore use algorithm to minimise numerical error - nextP(15, 15) = kahanSummation(nextP(15, 15), process_noise(15), _delta_vel_bias_var_accum(2)); - - } else { - nextP.uncorrelateCovarianceSetVariance<1>(15, _prev_dvel_bias_var(2)); - _delta_vel_bias_var_accum(2) = 0.f; - } - - // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion - if (_control_status.flags.mag_3D) { - // calculate variances and upper diagonal covariances for earth and body magnetic field states - - - nextP(0,16) = P(0,16) - P(1,16)*PS11 + P(10,16)*PS6 + P(11,16)*PS7 + P(12,16)*PS9 - P(2,16)*PS12 - P(3,16)*PS13; - nextP(1,16) = P(0,16)*PS11 + P(1,16) - P(10,16)*PS34 + P(11,16)*PS9 - P(12,16)*PS7 + P(2,16)*PS13 - P(3,16)*PS12; - nextP(2,16) = P(0,16)*PS12 - P(1,16)*PS13 - P(10,16)*PS9 - P(11,16)*PS34 + P(12,16)*PS6 + P(2,16) + P(3,16)*PS11; - nextP(3,16) = P(0,16)*PS13 + P(1,16)*PS12 + P(10,16)*PS7 - P(11,16)*PS6 - P(12,16)*PS34 - P(2,16)*PS11 + P(3,16); - nextP(4,16) = P(0,16)*PS174 + P(1,16)*PS173 + P(13,16)*PS43 + P(14,16)*PS172 - P(15,16)*PS171 + P(2,16)*PS175 - P(3,16)*PS176 + P(4,16); - nextP(5,16) = -P(0,16)*PS202 - P(1,16)*PS204 - P(13,16)*PS193 + P(14,16)*PS75 + P(15,16)*PS190 + P(2,16)*PS201 + P(3,16)*PS203 + P(5,16); - nextP(6,16) = P(0,16)*PS216 + P(1,16)*PS217 + P(13,16)*PS199 - P(14,16)*PS197 + P(15,16)*PS87 - P(2,16)*PS214 + P(3,16)*PS215 + P(6,16); - nextP(7,16) = P(4,16)*dt + P(7,16); - nextP(8,16) = P(5,16)*dt + P(8,16); - nextP(9,16) = P(6,16)*dt + P(9,16); - nextP(10,16) = P(10,16); - nextP(11,16) = P(11,16); - nextP(12,16) = P(12,16); - nextP(13,16) = P(13,16); - nextP(14,16) = P(14,16); - nextP(15,16) = P(15,16); - nextP(16,16) = P(16,16); - nextP(0,17) = P(0,17) - P(1,17)*PS11 + P(10,17)*PS6 + P(11,17)*PS7 + P(12,17)*PS9 - P(2,17)*PS12 - P(3,17)*PS13; - nextP(1,17) = P(0,17)*PS11 + P(1,17) - P(10,17)*PS34 + P(11,17)*PS9 - P(12,17)*PS7 + P(2,17)*PS13 - P(3,17)*PS12; - nextP(2,17) = P(0,17)*PS12 - P(1,17)*PS13 - P(10,17)*PS9 - P(11,17)*PS34 + P(12,17)*PS6 + P(2,17) + P(3,17)*PS11; - nextP(3,17) = P(0,17)*PS13 + P(1,17)*PS12 + P(10,17)*PS7 - P(11,17)*PS6 - P(12,17)*PS34 - P(2,17)*PS11 + P(3,17); - nextP(4,17) = P(0,17)*PS174 + P(1,17)*PS173 + P(13,17)*PS43 + P(14,17)*PS172 - P(15,17)*PS171 + P(2,17)*PS175 - P(3,17)*PS176 + P(4,17); - nextP(5,17) = -P(0,17)*PS202 - P(1,17)*PS204 - P(13,17)*PS193 + P(14,17)*PS75 + P(15,17)*PS190 + P(2,17)*PS201 + P(3,17)*PS203 + P(5,17); - nextP(6,17) = P(0,17)*PS216 + P(1,17)*PS217 + P(13,17)*PS199 - P(14,17)*PS197 + P(15,17)*PS87 - P(2,17)*PS214 + P(3,17)*PS215 + P(6,17); - nextP(7,17) = P(4,17)*dt + P(7,17); - nextP(8,17) = P(5,17)*dt + P(8,17); - nextP(9,17) = P(6,17)*dt + P(9,17); - nextP(10,17) = P(10,17); - nextP(11,17) = P(11,17); - nextP(12,17) = P(12,17); - nextP(13,17) = P(13,17); - nextP(14,17) = P(14,17); - nextP(15,17) = P(15,17); - nextP(16,17) = P(16,17); - nextP(17,17) = P(17,17); - nextP(0,18) = P(0,18) - P(1,18)*PS11 + P(10,18)*PS6 + P(11,18)*PS7 + P(12,18)*PS9 - P(2,18)*PS12 - P(3,18)*PS13; - nextP(1,18) = P(0,18)*PS11 + P(1,18) - P(10,18)*PS34 + P(11,18)*PS9 - P(12,18)*PS7 + P(2,18)*PS13 - P(3,18)*PS12; - nextP(2,18) = P(0,18)*PS12 - P(1,18)*PS13 - P(10,18)*PS9 - P(11,18)*PS34 + P(12,18)*PS6 + P(2,18) + P(3,18)*PS11; - nextP(3,18) = P(0,18)*PS13 + P(1,18)*PS12 + P(10,18)*PS7 - P(11,18)*PS6 - P(12,18)*PS34 - P(2,18)*PS11 + P(3,18); - nextP(4,18) = P(0,18)*PS174 + P(1,18)*PS173 + P(13,18)*PS43 + P(14,18)*PS172 - P(15,18)*PS171 + P(2,18)*PS175 - P(3,18)*PS176 + P(4,18); - nextP(5,18) = -P(0,18)*PS202 - P(1,18)*PS204 - P(13,18)*PS193 + P(14,18)*PS75 + P(15,18)*PS190 + P(2,18)*PS201 + P(3,18)*PS203 + P(5,18); - nextP(6,18) = P(0,18)*PS216 + P(1,18)*PS217 + P(13,18)*PS199 - P(14,18)*PS197 + P(15,18)*PS87 - P(2,18)*PS214 + P(3,18)*PS215 + P(6,18); - nextP(7,18) = P(4,18)*dt + P(7,18); - nextP(8,18) = P(5,18)*dt + P(8,18); - nextP(9,18) = P(6,18)*dt + P(9,18); - nextP(10,18) = P(10,18); - nextP(11,18) = P(11,18); - nextP(12,18) = P(12,18); - nextP(13,18) = P(13,18); - nextP(14,18) = P(14,18); - nextP(15,18) = P(15,18); - nextP(16,18) = P(16,18); - nextP(17,18) = P(17,18); - nextP(18,18) = P(18,18); - nextP(0,19) = P(0,19) - P(1,19)*PS11 + P(10,19)*PS6 + P(11,19)*PS7 + P(12,19)*PS9 - P(2,19)*PS12 - P(3,19)*PS13; - nextP(1,19) = P(0,19)*PS11 + P(1,19) - P(10,19)*PS34 + P(11,19)*PS9 - P(12,19)*PS7 + P(2,19)*PS13 - P(3,19)*PS12; - nextP(2,19) = P(0,19)*PS12 - P(1,19)*PS13 - P(10,19)*PS9 - P(11,19)*PS34 + P(12,19)*PS6 + P(2,19) + P(3,19)*PS11; - nextP(3,19) = P(0,19)*PS13 + P(1,19)*PS12 + P(10,19)*PS7 - P(11,19)*PS6 - P(12,19)*PS34 - P(2,19)*PS11 + P(3,19); - nextP(4,19) = P(0,19)*PS174 + P(1,19)*PS173 + P(13,19)*PS43 + P(14,19)*PS172 - P(15,19)*PS171 + P(2,19)*PS175 - P(3,19)*PS176 + P(4,19); - nextP(5,19) = -P(0,19)*PS202 - P(1,19)*PS204 - P(13,19)*PS193 + P(14,19)*PS75 + P(15,19)*PS190 + P(2,19)*PS201 + P(3,19)*PS203 + P(5,19); - nextP(6,19) = P(0,19)*PS216 + P(1,19)*PS217 + P(13,19)*PS199 - P(14,19)*PS197 + P(15,19)*PS87 - P(2,19)*PS214 + P(3,19)*PS215 + P(6,19); - nextP(7,19) = P(4,19)*dt + P(7,19); - nextP(8,19) = P(5,19)*dt + P(8,19); - nextP(9,19) = P(6,19)*dt + P(9,19); - nextP(10,19) = P(10,19); - nextP(11,19) = P(11,19); - nextP(12,19) = P(12,19); - nextP(13,19) = P(13,19); - nextP(14,19) = P(14,19); - nextP(15,19) = P(15,19); - nextP(16,19) = P(16,19); - nextP(17,19) = P(17,19); - nextP(18,19) = P(18,19); - nextP(19,19) = P(19,19); - nextP(0,20) = P(0,20) - P(1,20)*PS11 + P(10,20)*PS6 + P(11,20)*PS7 + P(12,20)*PS9 - P(2,20)*PS12 - P(3,20)*PS13; - nextP(1,20) = P(0,20)*PS11 + P(1,20) - P(10,20)*PS34 + P(11,20)*PS9 - P(12,20)*PS7 + P(2,20)*PS13 - P(3,20)*PS12; - nextP(2,20) = P(0,20)*PS12 - P(1,20)*PS13 - P(10,20)*PS9 - P(11,20)*PS34 + P(12,20)*PS6 + P(2,20) + P(3,20)*PS11; - nextP(3,20) = P(0,20)*PS13 + P(1,20)*PS12 + P(10,20)*PS7 - P(11,20)*PS6 - P(12,20)*PS34 - P(2,20)*PS11 + P(3,20); - nextP(4,20) = P(0,20)*PS174 + P(1,20)*PS173 + P(13,20)*PS43 + P(14,20)*PS172 - P(15,20)*PS171 + P(2,20)*PS175 - P(3,20)*PS176 + P(4,20); - nextP(5,20) = -P(0,20)*PS202 - P(1,20)*PS204 - P(13,20)*PS193 + P(14,20)*PS75 + P(15,20)*PS190 + P(2,20)*PS201 + P(3,20)*PS203 + P(5,20); - nextP(6,20) = P(0,20)*PS216 + P(1,20)*PS217 + P(13,20)*PS199 - P(14,20)*PS197 + P(15,20)*PS87 - P(2,20)*PS214 + P(3,20)*PS215 + P(6,20); - nextP(7,20) = P(4,20)*dt + P(7,20); - nextP(8,20) = P(5,20)*dt + P(8,20); - nextP(9,20) = P(6,20)*dt + P(9,20); - nextP(10,20) = P(10,20); - nextP(11,20) = P(11,20); - nextP(12,20) = P(12,20); - nextP(13,20) = P(13,20); - nextP(14,20) = P(14,20); - nextP(15,20) = P(15,20); - nextP(16,20) = P(16,20); - nextP(17,20) = P(17,20); - nextP(18,20) = P(18,20); - nextP(19,20) = P(19,20); - nextP(20,20) = P(20,20); - nextP(0,21) = P(0,21) - P(1,21)*PS11 + P(10,21)*PS6 + P(11,21)*PS7 + P(12,21)*PS9 - P(2,21)*PS12 - P(3,21)*PS13; - nextP(1,21) = P(0,21)*PS11 + P(1,21) - P(10,21)*PS34 + P(11,21)*PS9 - P(12,21)*PS7 + P(2,21)*PS13 - P(3,21)*PS12; - nextP(2,21) = P(0,21)*PS12 - P(1,21)*PS13 - P(10,21)*PS9 - P(11,21)*PS34 + P(12,21)*PS6 + P(2,21) + P(3,21)*PS11; - nextP(3,21) = P(0,21)*PS13 + P(1,21)*PS12 + P(10,21)*PS7 - P(11,21)*PS6 - P(12,21)*PS34 - P(2,21)*PS11 + P(3,21); - nextP(4,21) = P(0,21)*PS174 + P(1,21)*PS173 + P(13,21)*PS43 + P(14,21)*PS172 - P(15,21)*PS171 + P(2,21)*PS175 - P(3,21)*PS176 + P(4,21); - nextP(5,21) = -P(0,21)*PS202 - P(1,21)*PS204 - P(13,21)*PS193 + P(14,21)*PS75 + P(15,21)*PS190 + P(2,21)*PS201 + P(3,21)*PS203 + P(5,21); - nextP(6,21) = P(0,21)*PS216 + P(1,21)*PS217 + P(13,21)*PS199 - P(14,21)*PS197 + P(15,21)*PS87 - P(2,21)*PS214 + P(3,21)*PS215 + P(6,21); - nextP(7,21) = P(4,21)*dt + P(7,21); - nextP(8,21) = P(5,21)*dt + P(8,21); - nextP(9,21) = P(6,21)*dt + P(9,21); - nextP(10,21) = P(10,21); - nextP(11,21) = P(11,21); - nextP(12,21) = P(12,21); - nextP(13,21) = P(13,21); - nextP(14,21) = P(14,21); - nextP(15,21) = P(15,21); - nextP(16,21) = P(16,21); - nextP(17,21) = P(17,21); - nextP(18,21) = P(18,21); - nextP(19,21) = P(19,21); - nextP(20,21) = P(20,21); - nextP(21,21) = P(21,21); - - // add process noise that is not from the IMU - for (unsigned i = 16; i <= 21; i++) { - nextP(i, i) += process_noise(i); + } else { + nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_dvel_bias_var(index)); + _delta_vel_bias_var_accum(index) = 0.f; } - } - // Don't do covariance prediction on wind states unless we are using them - if (_control_status.flags.wind) { - - // calculate variances and upper diagonal covariances for wind states - - nextP(0,22) = P(0,22) - P(1,22)*PS11 + P(10,22)*PS6 + P(11,22)*PS7 + P(12,22)*PS9 - P(2,22)*PS12 - P(3,22)*PS13; - nextP(1,22) = P(0,22)*PS11 + P(1,22) - P(10,22)*PS34 + P(11,22)*PS9 - P(12,22)*PS7 + P(2,22)*PS13 - P(3,22)*PS12; - nextP(2,22) = P(0,22)*PS12 - P(1,22)*PS13 - P(10,22)*PS9 - P(11,22)*PS34 + P(12,22)*PS6 + P(2,22) + P(3,22)*PS11; - nextP(3,22) = P(0,22)*PS13 + P(1,22)*PS12 + P(10,22)*PS7 - P(11,22)*PS6 - P(12,22)*PS34 - P(2,22)*PS11 + P(3,22); - nextP(4,22) = P(0,22)*PS174 + P(1,22)*PS173 + P(13,22)*PS43 + P(14,22)*PS172 - P(15,22)*PS171 + P(2,22)*PS175 - P(3,22)*PS176 + P(4,22); - nextP(5,22) = -P(0,22)*PS202 - P(1,22)*PS204 - P(13,22)*PS193 + P(14,22)*PS75 + P(15,22)*PS190 + P(2,22)*PS201 + P(3,22)*PS203 + P(5,22); - nextP(6,22) = P(0,22)*PS216 + P(1,22)*PS217 + P(13,22)*PS199 - P(14,22)*PS197 + P(15,22)*PS87 - P(2,22)*PS214 + P(3,22)*PS215 + P(6,22); - nextP(7,22) = P(4,22)*dt + P(7,22); - nextP(8,22) = P(5,22)*dt + P(8,22); - nextP(9,22) = P(6,22)*dt + P(9,22); - nextP(10,22) = P(10,22); - nextP(11,22) = P(11,22); - nextP(12,22) = P(12,22); - nextP(13,22) = P(13,22); - nextP(14,22) = P(14,22); - nextP(15,22) = P(15,22); - nextP(16,22) = P(16,22); - nextP(17,22) = P(17,22); - nextP(18,22) = P(18,22); - nextP(19,22) = P(19,22); - nextP(20,22) = P(20,22); - nextP(21,22) = P(21,22); - nextP(22,22) = P(22,22); - nextP(0,23) = P(0,23) - P(1,23)*PS11 + P(10,23)*PS6 + P(11,23)*PS7 + P(12,23)*PS9 - P(2,23)*PS12 - P(3,23)*PS13; - nextP(1,23) = P(0,23)*PS11 + P(1,23) - P(10,23)*PS34 + P(11,23)*PS9 - P(12,23)*PS7 + P(2,23)*PS13 - P(3,23)*PS12; - nextP(2,23) = P(0,23)*PS12 - P(1,23)*PS13 - P(10,23)*PS9 - P(11,23)*PS34 + P(12,23)*PS6 + P(2,23) + P(3,23)*PS11; - nextP(3,23) = P(0,23)*PS13 + P(1,23)*PS12 + P(10,23)*PS7 - P(11,23)*PS6 - P(12,23)*PS34 - P(2,23)*PS11 + P(3,23); - nextP(4,23) = P(0,23)*PS174 + P(1,23)*PS173 + P(13,23)*PS43 + P(14,23)*PS172 - P(15,23)*PS171 + P(2,23)*PS175 - P(3,23)*PS176 + P(4,23); - nextP(5,23) = -P(0,23)*PS202 - P(1,23)*PS204 - P(13,23)*PS193 + P(14,23)*PS75 + P(15,23)*PS190 + P(2,23)*PS201 + P(3,23)*PS203 + P(5,23); - nextP(6,23) = P(0,23)*PS216 + P(1,23)*PS217 + P(13,23)*PS199 - P(14,23)*PS197 + P(15,23)*PS87 - P(2,23)*PS214 + P(3,23)*PS215 + P(6,23); - nextP(7,23) = P(4,23)*dt + P(7,23); - nextP(8,23) = P(5,23)*dt + P(8,23); - nextP(9,23) = P(6,23)*dt + P(9,23); - nextP(10,23) = P(10,23); - nextP(11,23) = P(11,23); - nextP(12,23) = P(12,23); - nextP(13,23) = P(13,23); - nextP(14,23) = P(14,23); - nextP(15,23) = P(15,23); - nextP(16,23) = P(16,23); - nextP(17,23) = P(17,23); - nextP(18,23) = P(18,23); - nextP(19,23) = P(19,23); - nextP(20,23) = P(20,23); - nextP(21,23) = P(21,23); - nextP(22,23) = P(22,23); - nextP(23,23) = P(23,23); - - // add process noise that is not from the IMU - for (unsigned i = 22; i <= 23; i++) { - nextP(i, i) += process_noise(i); - } - + // add process noise that is not from the IMU + for (unsigned i = 16; i <= 23; i++) { + nextP(i, i) += process_noise(i); } // stop position covariance growth if our total position variance reaches 100m @@ -887,15 +274,32 @@ void Ekf::predictCovariance() } // covariance matrix is symmetrical, so copy upper half to lower half - for (unsigned row = 1; row < _k_num_states; row++) { + for (unsigned row = 0; row <= 15; row++) { for (unsigned column = 0 ; column < row; column++) { P(row, column) = P(column, row) = nextP(column, row); } + + P(row, row) = nextP(row, row); } - // copy variances (diagonals) - for (unsigned i = 0; i < _k_num_states; i++) { - P(i, i) = nextP(i, i); + if (_control_status.flags.mag_3D) { + for (unsigned row = 16; row <= 21; row++) { + for (unsigned column = 0 ; column < row; column++) { + P(row, column) = P(column, row) = nextP(column, row); + } + + P(row, row) = nextP(row, row); + } + } + + if (_control_status.flags.wind) { + for (unsigned row = 22; row <= 23; row++) { + for (unsigned column = 0 ; column < row; column++) { + P(row, column) = P(column, row) = nextP(column, row); + } + + P(row, row) = nextP(row, row); + } } // fix gross errors in the covariance matrix and ensure rows and diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index e6bc963a51..1350c356fd 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -69,6 +69,53 @@ class VState(sf.Matrix): class MState(sf.Matrix): SHAPE = (State.n_states, State.n_states) +def predict_covariance( + state: VState, + P: MState, + d_vel: sf.V3, + d_vel_var: sf.V3, + d_ang: sf.V3, + d_ang_var: sf.Scalar, + dt: sf.Scalar +): + g = sf.Symbol("g") # does not appear in the jacobians + + d_vel_b = sf.V3(state[State.d_vel_bx], state[State.d_vel_by], state[State.d_vel_bz]) + d_vel_true = d_vel - d_vel_b + + d_ang_b = sf.V3(state[State.d_ang_bx], state[State.d_ang_by], state[State.d_ang_bz]) + d_ang_true = d_ang - d_ang_b + + q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_earth = quat_to_rot_simplified(q) + v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) + p = sf.V3(state[State.px], state[State.py], state[State.pz]) + + q_new = quat_mult(q, sf.V4(1, 0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2])) + v_new = v + R_to_earth * d_vel_true + sf.V3(0 ,0 ,g) * dt + p_new = p + v * dt + + # Predicted state vector at time t + dt + state_new = VState.block_matrix([[q_new], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]]) + + # State propagation jacobian + A = state_new.jacobian(state) + G = state_new.jacobian(sf.V6.block_matrix([[d_vel], [d_ang]])) + + # Covariance propagation + var_u = sf.Matrix.diag([d_vel_var[0], d_vel_var[1], d_vel_var[2], d_ang_var, d_ang_var, d_ang_var]) + P_new = A * P * A.T + G * var_u * G.T + + # Generate the equations for the lower triangular matrix and the diagonal only + # Since the matrix is symmetric, the upper triangle does not need to be derived + # and can simply be copied in the implementation + for index in range(State.n_states): + for j in range(State.n_states): + if index > j: + P_new[index,j] = 0 + + return P_new + def compute_airspeed_innov_and_innov_var( state: VState, P: MState, @@ -148,8 +195,10 @@ def compute_sideslip_h_and_k( return (H.T, K) +print("Derive EKF2 equations...") generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) +generate_px4_function(predict_covariance, output_names=["P_new"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py index 68ee60931e..882014ca84 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -34,9 +34,7 @@ Description: Common functions used for the derivation of most estimators """ -from symforce import symbolic as sm -from symforce import geo -from symforce import typing as T +import symforce.symbolic as sf # q: quaternion describing rotation from frame 1 to frame 2 # returns a rotation matrix derived form q which describes the same @@ -47,17 +45,40 @@ def quat_to_rot(q): q2 = q[2] q3 = q[3] - Rot = geo.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], + Rot = sf.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) return Rot -def sign_no_zero(x) -> T.Scalar: +def quat_to_rot_simplified(q): + q0 = q[0] + q1 = q[1] + q2 = q[2] + q3 = q[3] + + # Use the simplified formula for unit quaternion to rotation matrix + # as it produces a simpler and more stable EKF derivation given + # the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1 + Rot = sf.Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], + [2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)], + [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]]) + + return Rot + +def quat_mult(p,q): + r = sf.Matrix([p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3], + p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2], + p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1], + p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0]]) + + return r + +def sign_no_zero(x) -> sf.Scalar: """ Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero """ - return 2 * sm.Min(sm.sign(x), 0) + 1 + return 2 * sf.Min(sf.sign(x), 0) + 1 def add_epsilon_sign(expr, var, eps): # Avoids a singularity at 0 while keeping the derivative correct @@ -76,7 +97,6 @@ def generate_px4_function(function_name, output_names): output_dir="generated", skip_directory_nesting=True) - print("Files generated in {}:\n".format(metadata.output_dir)) for f in metadata.generated_files: print(" |- {}".format(os.path.relpath(f, metadata.output_dir))) 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 new file mode 100644 index 0000000000..11e2ac3c9e --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h @@ -0,0 +1,1075 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: predict_covariance + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * d_vel: Matrix31 + * d_vel_var: Matrix31 + * d_ang: Matrix31 + * d_ang_var: Scalar + * dt: Scalar + * + * Outputs: + * P_new: Matrix24_24 + */ +template +void PredictCovariance(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& d_vel, + const matrix::Matrix& d_vel_var, + const matrix::Matrix& d_ang, const Scalar d_ang_var, + const Scalar dt, matrix::Matrix* const P_new = nullptr) { + // Total ops: 2887 + + // Input arrays + + // Intermediate terms (171) + const Scalar _tmp0 = Scalar(0.5) * d_ang(2, 0); + const Scalar _tmp1 = Scalar(0.5) * state(12, 0); + const Scalar _tmp2 = -_tmp0 + _tmp1; + const Scalar _tmp3 = Scalar(0.5) * d_ang(1, 0); + const Scalar _tmp4 = Scalar(0.5) * state(11, 0); + const Scalar _tmp5 = -_tmp3 + _tmp4; + const Scalar _tmp6 = Scalar(0.5) * d_ang(0, 0); + const Scalar _tmp7 = Scalar(0.5) * state(10, 0); + const Scalar _tmp8 = -_tmp6 + _tmp7; + const Scalar _tmp9 = Scalar(0.5) * state(3, 0); + const Scalar _tmp10 = Scalar(0.5) * state(2, 0); + const Scalar _tmp11 = Scalar(0.5) * state(1, 0); + const Scalar _tmp12 = P(0, 11) + P(1, 11) * _tmp8 + P(10, 11) * _tmp11 + P(11, 11) * _tmp10 + + P(12, 11) * _tmp9 + P(2, 11) * _tmp5 + P(3, 11) * _tmp2; + const Scalar _tmp13 = P(0, 10) + P(1, 10) * _tmp8 + P(10, 10) * _tmp11 + P(11, 10) * _tmp10 + + P(12, 10) * _tmp9 + P(2, 10) * _tmp5 + P(3, 10) * _tmp2; + const Scalar _tmp14 = P(0, 12) + P(1, 12) * _tmp8 + P(10, 12) * _tmp11 + P(11, 12) * _tmp10 + + P(12, 12) * _tmp9 + P(2, 12) * _tmp5 + P(3, 12) * _tmp2; + const Scalar _tmp15 = P(0, 2) + P(1, 2) * _tmp8 + P(10, 2) * _tmp11 + P(11, 2) * _tmp10 + + P(12, 2) * _tmp9 + P(2, 2) * _tmp5 + P(3, 2) * _tmp2; + const Scalar _tmp16 = P(0, 3) + P(1, 3) * _tmp8 + P(10, 3) * _tmp11 + P(11, 3) * _tmp10 + + P(12, 3) * _tmp9 + P(2, 3) * _tmp5 + P(3, 3) * _tmp2; + const Scalar _tmp17 = P(0, 1) + P(1, 1) * _tmp8 + P(10, 1) * _tmp11 + P(11, 1) * _tmp10 + + P(12, 1) * _tmp9 + P(2, 1) * _tmp5 + P(3, 1) * _tmp2; + const Scalar _tmp18 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp19 = Scalar(0.25) * d_ang_var; + const Scalar _tmp20 = _tmp18 * _tmp19; + const Scalar _tmp21 = P(0, 0) + P(1, 0) * _tmp8 + P(10, 0) * _tmp11 + P(11, 0) * _tmp10 + + P(12, 0) * _tmp9 + P(2, 0) * _tmp5 + P(3, 0) * _tmp2; + const Scalar _tmp22 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp23 = _tmp19 * _tmp22; + const Scalar _tmp24 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp25 = _tmp19 * _tmp24; + const Scalar _tmp26 = _tmp23 + _tmp25; + const Scalar _tmp27 = Scalar(0.5) * state(0, 0); + const Scalar _tmp28 = state(0, 0) * state(1, 0); + const Scalar _tmp29 = _tmp0 - _tmp1; + const Scalar _tmp30 = _tmp6 - _tmp7; + const Scalar _tmp31 = P(0, 12) * _tmp30 + P(1, 12) - P(10, 12) * _tmp27 + P(11, 12) * _tmp9 - + P(12, 12) * _tmp10 + P(2, 12) * _tmp29 + P(3, 12) * _tmp5; + const Scalar _tmp32 = P(0, 11) * _tmp30 + P(1, 11) - P(10, 11) * _tmp27 + P(11, 11) * _tmp9 - + P(12, 11) * _tmp10 + P(2, 11) * _tmp29 + P(3, 11) * _tmp5; + const Scalar _tmp33 = P(0, 10) * _tmp30 + P(1, 10) - P(10, 10) * _tmp27 + P(11, 10) * _tmp9 - + P(12, 10) * _tmp10 + P(2, 10) * _tmp29 + P(3, 10) * _tmp5; + const Scalar _tmp34 = P(0, 3) * _tmp30 + P(1, 3) - P(10, 3) * _tmp27 + P(11, 3) * _tmp9 - + P(12, 3) * _tmp10 + P(2, 3) * _tmp29 + P(3, 3) * _tmp5; + const Scalar _tmp35 = P(0, 2) * _tmp30 + P(1, 2) - P(10, 2) * _tmp27 + P(11, 2) * _tmp9 - + P(12, 2) * _tmp10 + P(2, 2) * _tmp29 + P(3, 2) * _tmp5; + const Scalar _tmp36 = P(0, 0) * _tmp30 + P(1, 0) - P(10, 0) * _tmp27 + P(11, 0) * _tmp9 - + P(12, 0) * _tmp10 + P(2, 0) * _tmp29 + P(3, 0) * _tmp5; + const Scalar _tmp37 = P(0, 1) * _tmp30 + P(1, 1) - P(10, 1) * _tmp27 + P(11, 1) * _tmp9 - + P(12, 1) * _tmp10 + P(2, 1) * _tmp29 + P(3, 1) * _tmp5; + const Scalar _tmp38 = _tmp19 * std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp39 = _tmp20 + _tmp38; + const Scalar _tmp40 = state(0, 0) * state(2, 0); + const Scalar _tmp41 = _tmp3 - _tmp4; + const Scalar _tmp42 = state(1, 0) * state(2, 0); + const Scalar _tmp43 = P(0, 12) * _tmp41 + P(1, 12) * _tmp2 - P(10, 12) * _tmp9 - + P(11, 12) * _tmp27 + P(12, 12) * _tmp11 + P(2, 12) + P(3, 12) * _tmp30; + const Scalar _tmp44 = P(0, 11) * _tmp41 + P(1, 11) * _tmp2 - P(10, 11) * _tmp9 - + P(11, 11) * _tmp27 + P(12, 11) * _tmp11 + P(2, 11) + P(3, 11) * _tmp30; + const Scalar _tmp45 = P(0, 10) * _tmp41 + P(1, 10) * _tmp2 - P(10, 10) * _tmp9 - + P(11, 10) * _tmp27 + P(12, 10) * _tmp11 + P(2, 10) + P(3, 10) * _tmp30; + const Scalar _tmp46 = P(0, 0) * _tmp41 + P(1, 0) * _tmp2 - P(10, 0) * _tmp9 - P(11, 0) * _tmp27 + + P(12, 0) * _tmp11 + P(2, 0) + P(3, 0) * _tmp30; + const Scalar _tmp47 = P(0, 1) * _tmp41 + P(1, 1) * _tmp2 - P(10, 1) * _tmp9 - P(11, 1) * _tmp27 + + P(12, 1) * _tmp11 + P(2, 1) + P(3, 1) * _tmp30; + const Scalar _tmp48 = P(0, 3) * _tmp41 + P(1, 3) * _tmp2 - P(10, 3) * _tmp9 - P(11, 3) * _tmp27 + + P(12, 3) * _tmp11 + P(2, 3) + P(3, 3) * _tmp30; + const Scalar _tmp49 = P(0, 2) * _tmp41 + P(1, 2) * _tmp2 - P(10, 2) * _tmp9 - P(11, 2) * _tmp27 + + P(12, 2) * _tmp11 + P(2, 2) + P(3, 2) * _tmp30; + const Scalar _tmp50 = state(0, 0) * state(3, 0); + const Scalar _tmp51 = state(1, 0) * state(3, 0); + const Scalar _tmp52 = state(2, 0) * state(3, 0); + const Scalar _tmp53 = P(0, 12) * _tmp29 + P(1, 12) * _tmp41 + P(10, 12) * _tmp10 - + P(11, 12) * _tmp11 - P(12, 12) * _tmp27 + P(2, 12) * _tmp8 + P(3, 12); + const Scalar _tmp54 = P(0, 11) * _tmp29 + P(1, 11) * _tmp41 + P(10, 11) * _tmp10 - + P(11, 11) * _tmp11 - P(12, 11) * _tmp27 + P(2, 11) * _tmp8 + P(3, 11); + const Scalar _tmp55 = P(0, 10) * _tmp29 + P(1, 10) * _tmp41 + P(10, 10) * _tmp10 - + P(11, 10) * _tmp11 - P(12, 10) * _tmp27 + P(2, 10) * _tmp8 + P(3, 10); + const Scalar _tmp56 = P(0, 2) * _tmp29 + P(1, 2) * _tmp41 + P(10, 2) * _tmp10 - + P(11, 2) * _tmp11 - P(12, 2) * _tmp27 + P(2, 2) * _tmp8 + P(3, 2); + const Scalar _tmp57 = P(0, 0) * _tmp29 + P(1, 0) * _tmp41 + P(10, 0) * _tmp10 - + P(11, 0) * _tmp11 - P(12, 0) * _tmp27 + P(2, 0) * _tmp8 + P(3, 0); + const Scalar _tmp58 = P(0, 1) * _tmp29 + P(1, 1) * _tmp41 + P(10, 1) * _tmp10 - + P(11, 1) * _tmp11 - P(12, 1) * _tmp27 + P(2, 1) * _tmp8 + P(3, 1); + const Scalar _tmp59 = P(0, 3) * _tmp29 + P(1, 3) * _tmp41 + P(10, 3) * _tmp10 - + P(11, 3) * _tmp11 - P(12, 3) * _tmp27 + P(2, 3) * _tmp8 + P(3, 3); + const Scalar _tmp60 = d_vel(1, 0) - state(14, 0); + const Scalar _tmp61 = 2 * _tmp60; + const Scalar _tmp62 = _tmp61 * state(2, 0); + const Scalar _tmp63 = d_vel(2, 0) - state(15, 0); + const Scalar _tmp64 = 2 * _tmp63; + const Scalar _tmp65 = _tmp64 * state(3, 0); + const Scalar _tmp66 = _tmp62 + _tmp65; + const Scalar _tmp67 = _tmp64 * state(2, 0); + const Scalar _tmp68 = _tmp61 * state(3, 0); + const Scalar _tmp69 = _tmp67 - _tmp68; + const Scalar _tmp70 = _tmp64 * state(1, 0); + const Scalar _tmp71 = _tmp61 * state(0, 0); + const Scalar _tmp72 = d_vel(0, 0) - state(13, 0); + const Scalar _tmp73 = 4 * state(3, 0); + const Scalar _tmp74 = _tmp70 - _tmp71 - _tmp72 * _tmp73; + const Scalar _tmp75 = P(0, 15) + P(1, 15) * _tmp8 + P(10, 15) * _tmp11 + P(11, 15) * _tmp10 + + P(12, 15) * _tmp9 + P(2, 15) * _tmp5 + P(3, 15) * _tmp2; + const Scalar _tmp76 = 2 * _tmp40 + 2 * _tmp51; + const Scalar _tmp77 = P(0, 14) + P(1, 14) * _tmp8 + P(10, 14) * _tmp11 + P(11, 14) * _tmp10 + + P(12, 14) * _tmp9 + P(2, 14) * _tmp5 + P(3, 14) * _tmp2; + const Scalar _tmp78 = 2 * _tmp42 - 2 * _tmp50; + const Scalar _tmp79 = -2 * _tmp18; + const Scalar _tmp80 = -2 * _tmp22; + const Scalar _tmp81 = _tmp79 + _tmp80 + 1; + const Scalar _tmp82 = P(0, 13) + P(1, 13) * _tmp8 + P(10, 13) * _tmp11 + P(11, 13) * _tmp10 + + P(12, 13) * _tmp9 + P(2, 13) * _tmp5 + P(3, 13) * _tmp2; + const Scalar _tmp83 = _tmp61 * state(1, 0); + const Scalar _tmp84 = _tmp64 * state(0, 0); + const Scalar _tmp85 = 4 * state(2, 0); + const Scalar _tmp86 = -_tmp72 * _tmp85 + _tmp83 + _tmp84; + const Scalar _tmp87 = P(0, 4) + P(1, 4) * _tmp8 + P(10, 4) * _tmp11 + P(11, 4) * _tmp10 + + P(12, 4) * _tmp9 + P(2, 4) * _tmp5 + P(3, 4) * _tmp2; + const Scalar _tmp88 = P(0, 13) * _tmp30 + P(1, 13) - P(10, 13) * _tmp27 + P(11, 13) * _tmp9 - + P(12, 13) * _tmp10 + P(2, 13) * _tmp29 + P(3, 13) * _tmp5; + const Scalar _tmp89 = P(0, 15) * _tmp30 + P(1, 15) - P(10, 15) * _tmp27 + P(11, 15) * _tmp9 - + P(12, 15) * _tmp10 + P(2, 15) * _tmp29 + P(3, 15) * _tmp5; + const Scalar _tmp90 = P(0, 14) * _tmp30 + P(1, 14) - P(10, 14) * _tmp27 + P(11, 14) * _tmp9 - + P(12, 14) * _tmp10 + P(2, 14) * _tmp29 + P(3, 14) * _tmp5; + const Scalar _tmp91 = P(0, 4) * _tmp30 + P(1, 4) - P(10, 4) * _tmp27 + P(11, 4) * _tmp9 - + P(12, 4) * _tmp10 + P(2, 4) * _tmp29 + P(3, 4) * _tmp5; + const Scalar _tmp92 = P(0, 15) * _tmp41 + P(1, 15) * _tmp2 - P(10, 15) * _tmp9 - + P(11, 15) * _tmp27 + P(12, 15) * _tmp11 + P(2, 15) + P(3, 15) * _tmp30; + const Scalar _tmp93 = P(0, 14) * _tmp41 + P(1, 14) * _tmp2 - P(10, 14) * _tmp9 - + P(11, 14) * _tmp27 + P(12, 14) * _tmp11 + P(2, 14) + P(3, 14) * _tmp30; + const Scalar _tmp94 = P(0, 13) * _tmp41 + P(1, 13) * _tmp2 - P(10, 13) * _tmp9 - + P(11, 13) * _tmp27 + P(12, 13) * _tmp11 + P(2, 13) + P(3, 13) * _tmp30; + const Scalar _tmp95 = Scalar(0.5) * P(11, 4); + const Scalar _tmp96 = P(0, 4) * _tmp41 + P(1, 4) * _tmp2 - P(10, 4) * _tmp9 + P(12, 4) * _tmp11 + + P(2, 4) + P(3, 4) * _tmp30 - _tmp95 * state(0, 0); + const Scalar _tmp97 = P(0, 13) * _tmp29 + P(1, 13) * _tmp41 + P(10, 13) * _tmp10 - + P(11, 13) * _tmp11 - P(12, 13) * _tmp27 + P(2, 13) * _tmp8 + P(3, 13); + const Scalar _tmp98 = P(0, 14) * _tmp29 + P(1, 14) * _tmp41 + P(10, 14) * _tmp10 - + P(11, 14) * _tmp11 - P(12, 14) * _tmp27 + P(2, 14) * _tmp8 + P(3, 14); + const Scalar _tmp99 = P(0, 15) * _tmp29 + P(1, 15) * _tmp41 + P(10, 15) * _tmp10 - + P(11, 15) * _tmp11 - P(12, 15) * _tmp27 + P(2, 15) * _tmp8 + P(3, 15); + const Scalar _tmp100 = P(0, 4) * _tmp29 + P(1, 4) * _tmp41 + P(10, 4) * _tmp10 - + P(12, 4) * _tmp27 + P(2, 4) * _tmp8 + P(3, 4) - _tmp95 * state(1, 0); + const Scalar _tmp101 = P(0, 13) * _tmp69 + P(1, 13) * _tmp66 - P(13, 13) * _tmp81 - + P(14, 13) * _tmp78 - P(15, 13) * _tmp76 + P(2, 13) * _tmp86 + + P(3, 13) * _tmp74 + P(4, 13); + const Scalar _tmp102 = P(0, 14) * _tmp69 + P(1, 14) * _tmp66 - P(13, 14) * _tmp81 - + P(14, 14) * _tmp78 - P(15, 14) * _tmp76 + P(2, 14) * _tmp86 + + P(3, 14) * _tmp74 + P(4, 14); + const Scalar _tmp103 = P(0, 15) * _tmp69 + P(1, 15) * _tmp66 - P(13, 15) * _tmp81 - + P(14, 15) * _tmp78 - P(15, 15) * _tmp76 + P(2, 15) * _tmp86 + + P(3, 15) * _tmp74 + P(4, 15); + const Scalar _tmp104 = 2 * _tmp40; + const Scalar _tmp105 = 2 * _tmp51; + const Scalar _tmp106 = _tmp104 + _tmp105; + const Scalar _tmp107 = 2 * _tmp50; + const Scalar _tmp108 = 2 * _tmp42; + const Scalar _tmp109 = -_tmp107 + _tmp108; + const Scalar _tmp110 = P(0, 0) * _tmp69 + P(1, 0) * _tmp66 - P(13, 0) * _tmp81 - + P(14, 0) * _tmp78 - P(15, 0) * _tmp76 + P(2, 0) * _tmp86 + + P(3, 0) * _tmp74 + P(4, 0); + const Scalar _tmp111 = P(0, 1) * _tmp69 + P(1, 1) * _tmp66 - P(13, 1) * _tmp81 - + P(14, 1) * _tmp78 - P(15, 1) * _tmp76 + P(2, 1) * _tmp86 + + P(3, 1) * _tmp74 + P(4, 1); + const Scalar _tmp112 = P(0, 2) * _tmp69 + P(1, 2) * _tmp66 - P(13, 2) * _tmp81 - + P(14, 2) * _tmp78 - P(15, 2) * _tmp76 + P(2, 2) * _tmp86 + + P(3, 2) * _tmp74 + P(4, 2); + const Scalar _tmp113 = P(0, 3) * _tmp69 + P(1, 3) * _tmp66 - P(13, 3) * _tmp81 - + P(14, 3) * _tmp78 - P(15, 3) * _tmp76 + P(2, 3) * _tmp86 + + P(3, 3) * _tmp74 + P(4, 3); + const Scalar _tmp114 = P(0, 4) * _tmp69 + P(1, 4) * _tmp66 - P(13, 4) * _tmp81 - + P(14, 4) * _tmp78 - P(15, 4) * _tmp76 + P(2, 4) * _tmp86 + + P(3, 4) * _tmp74 + P(4, 4); + const Scalar _tmp115 = 2 * _tmp72; + const Scalar _tmp116 = _tmp115 * state(1, 0); + const Scalar _tmp117 = _tmp116 + _tmp65; + const Scalar _tmp118 = _tmp115 * state(3, 0); + const Scalar _tmp119 = _tmp118 - _tmp70; + const Scalar _tmp120 = _tmp115 * state(0, 0); + const Scalar _tmp121 = _tmp120 - _tmp60 * _tmp73 + _tmp67; + const Scalar _tmp122 = 2 * _tmp42 + 2 * _tmp50; + const Scalar _tmp123 = -2 * _tmp28 + 2 * _tmp52; + const Scalar _tmp124 = 1 - 2 * _tmp24; + const Scalar _tmp125 = _tmp124 + _tmp79; + const Scalar _tmp126 = 4 * state(1, 0); + const Scalar _tmp127 = _tmp115 * state(2, 0); + const Scalar _tmp128 = -_tmp126 * _tmp60 + _tmp127 - _tmp84; + const Scalar _tmp129 = P(0, 5) + P(1, 5) * _tmp8 + P(10, 5) * _tmp11 + P(11, 5) * _tmp10 + + P(12, 5) * _tmp9 + P(2, 5) * _tmp5 + P(3, 5) * _tmp2; + const Scalar _tmp130 = P(0, 5) * _tmp30 + P(1, 5) - P(10, 5) * _tmp27 + P(11, 5) * _tmp9 - + P(12, 5) * _tmp10 + P(2, 5) * _tmp29 + P(3, 5) * _tmp5; + const Scalar _tmp131 = Scalar(0.5) * P(11, 5); + const Scalar _tmp132 = P(0, 5) * _tmp41 + P(1, 5) * _tmp2 - P(10, 5) * _tmp9 + P(12, 5) * _tmp11 + + P(2, 5) + P(3, 5) * _tmp30 - _tmp131 * state(0, 0); + const Scalar _tmp133 = P(0, 5) * _tmp29 + P(1, 5) * _tmp41 + P(10, 5) * _tmp10 - + P(12, 5) * _tmp27 + P(2, 5) * _tmp8 + P(3, 5) - _tmp131 * state(1, 0); + const Scalar _tmp134 = _tmp107 + _tmp108; + const Scalar _tmp135 = _tmp81 * d_vel_var(0, 0); + const Scalar _tmp136 = 2 * _tmp52; + const Scalar _tmp137 = 2 * _tmp28; + const Scalar _tmp138 = _tmp136 - _tmp137; + const Scalar _tmp139 = _tmp138 * d_vel_var(2, 0); + const Scalar _tmp140 = P(0, 5) * _tmp69 + P(1, 5) * _tmp66 - P(13, 5) * _tmp81 - + P(14, 5) * _tmp78 - P(15, 5) * _tmp76 + P(2, 5) * _tmp86 + + P(3, 5) * _tmp74 + P(4, 5); + const Scalar _tmp141 = P(0, 3) * _tmp119 + P(1, 3) * _tmp128 - P(13, 3) * _tmp122 - + P(14, 3) * _tmp125 - P(15, 3) * _tmp123 + P(2, 3) * _tmp117 + + P(3, 3) * _tmp121 + P(5, 3); + const Scalar _tmp142 = P(0, 1) * _tmp119 + P(1, 1) * _tmp128 - P(13, 1) * _tmp122 - + P(14, 1) * _tmp125 - P(15, 1) * _tmp123 + P(2, 1) * _tmp117 + + P(3, 1) * _tmp121 + P(5, 1); + const Scalar _tmp143 = P(0, 13) * _tmp119 + P(1, 13) * _tmp128 - P(13, 13) * _tmp122 - + P(14, 13) * _tmp125 - P(15, 13) * _tmp123 + P(2, 13) * _tmp117 + + P(3, 13) * _tmp121 + P(5, 13); + const Scalar _tmp144 = P(0, 15) * _tmp119 + P(1, 15) * _tmp128 - P(13, 15) * _tmp122 - + P(14, 15) * _tmp125 - P(15, 15) * _tmp123 + P(2, 15) * _tmp117 + + P(3, 15) * _tmp121 + P(5, 15); + const Scalar _tmp145 = P(0, 14) * _tmp119 + P(1, 14) * _tmp128 - P(13, 14) * _tmp122 - + P(14, 14) * _tmp125 - P(15, 14) * _tmp123 + P(2, 14) * _tmp117 + + P(3, 14) * _tmp121 + P(5, 14); + const Scalar _tmp146 = P(0, 0) * _tmp119 + P(1, 0) * _tmp128 - P(13, 0) * _tmp122 - + P(14, 0) * _tmp125 - P(15, 0) * _tmp123 + P(2, 0) * _tmp117 + + P(3, 0) * _tmp121 + P(5, 0); + const Scalar _tmp147 = P(0, 2) * _tmp119 + P(1, 2) * _tmp128 - P(13, 2) * _tmp122 - + P(14, 2) * _tmp125 - P(15, 2) * _tmp123 + P(2, 2) * _tmp117 + + P(3, 2) * _tmp121 + P(5, 2); + const Scalar _tmp148 = P(0, 5) * _tmp119 + P(1, 5) * _tmp128 - P(13, 5) * _tmp122 - + P(14, 5) * _tmp125 - P(15, 5) * _tmp123 + P(2, 5) * _tmp117 + + P(3, 5) * _tmp121 + P(5, 5); + const Scalar _tmp149 = _tmp116 + _tmp62; + const Scalar _tmp150 = -_tmp127 + _tmp83; + const Scalar _tmp151 = -_tmp120 - _tmp63 * _tmp85 + _tmp68; + const Scalar _tmp152 = _tmp118 - _tmp126 * _tmp63 + _tmp71; + const Scalar _tmp153 = -2 * _tmp40 + 2 * _tmp51; + const Scalar _tmp154 = 2 * _tmp28 + 2 * _tmp52; + const Scalar _tmp155 = _tmp124 + _tmp80; + const Scalar _tmp156 = P(0, 6) + P(1, 6) * _tmp8 + P(10, 6) * _tmp11 + P(11, 6) * _tmp10 + + P(12, 6) * _tmp9 + P(2, 6) * _tmp5 + P(3, 6) * _tmp2; + const Scalar _tmp157 = P(0, 6) * _tmp30 + P(1, 6) - P(10, 6) * _tmp27 + P(11, 6) * _tmp9 - + P(12, 6) * _tmp10 + P(2, 6) * _tmp29 + P(3, 6) * _tmp5; + const Scalar _tmp158 = Scalar(0.5) * P(11, 6); + const Scalar _tmp159 = P(0, 6) * _tmp41 + P(1, 6) * _tmp2 - P(10, 6) * _tmp9 + P(12, 6) * _tmp11 + + P(2, 6) + P(3, 6) * _tmp30 - _tmp158 * state(0, 0); + const Scalar _tmp160 = P(0, 6) * _tmp29 + P(1, 6) * _tmp41 + P(10, 6) * _tmp10 - + P(12, 6) * _tmp27 + P(2, 6) * _tmp8 + P(3, 6) - _tmp158 * state(1, 0); + const Scalar _tmp161 = -_tmp104 + _tmp105; + const Scalar _tmp162 = _tmp136 + _tmp137; + const Scalar _tmp163 = _tmp162 * d_vel_var(1, 0); + const Scalar _tmp164 = P(0, 6) * _tmp69 + P(1, 6) * _tmp66 - P(13, 6) * _tmp81 - + P(14, 6) * _tmp78 - P(15, 6) * _tmp76 + P(2, 6) * _tmp86 + + P(3, 6) * _tmp74 + P(4, 6); + const Scalar _tmp165 = P(0, 6) * _tmp119 + P(1, 6) * _tmp128 - P(13, 6) * _tmp122 - + P(14, 6) * _tmp125 - P(15, 6) * _tmp123 + P(2, 6) * _tmp117 + + P(3, 6) * _tmp121 + P(5, 6); + const Scalar _tmp166 = P(0, 13) * _tmp150 + P(1, 13) * _tmp152 - P(13, 13) * _tmp153 - + P(14, 13) * _tmp154 - P(15, 13) * _tmp155 + P(2, 13) * _tmp151 + + P(3, 13) * _tmp149 + P(6, 13); + const Scalar _tmp167 = P(0, 14) * _tmp150 + P(1, 14) * _tmp152 - P(13, 14) * _tmp153 - + P(14, 14) * _tmp154 - P(15, 14) * _tmp155 + P(2, 14) * _tmp151 + + P(3, 14) * _tmp149 + P(6, 14); + const Scalar _tmp168 = P(0, 15) * _tmp150 + P(1, 15) * _tmp152 - P(13, 15) * _tmp153 - + P(14, 15) * _tmp154 - P(15, 15) * _tmp155 + P(2, 15) * _tmp151 + + P(3, 15) * _tmp149 + P(6, 15); + const Scalar _tmp169 = P(0, 6) * _tmp150 + P(1, 6) * _tmp152 - P(13, 6) * _tmp153 - + P(14, 6) * _tmp154 - P(15, 6) * _tmp155 + P(2, 6) * _tmp151 + + P(3, 6) * _tmp149 + P(6, 6); + const Scalar _tmp170 = Scalar(0.5) * P(11, 8); + + // Output terms (1) + if (P_new != nullptr) { + matrix::Matrix& _P_new = (*P_new); + + _P_new(0, 0) = _tmp10 * _tmp12 + _tmp11 * _tmp13 + _tmp14 * _tmp9 + _tmp15 * _tmp5 + + _tmp16 * _tmp2 + _tmp17 * _tmp8 + _tmp20 + _tmp21 + _tmp26; + _P_new(1, 0) = 0; + _P_new(2, 0) = 0; + _P_new(3, 0) = 0; + _P_new(4, 0) = 0; + _P_new(5, 0) = 0; + _P_new(6, 0) = 0; + _P_new(7, 0) = 0; + _P_new(8, 0) = 0; + _P_new(9, 0) = 0; + _P_new(10, 0) = 0; + _P_new(11, 0) = 0; + _P_new(12, 0) = 0; + _P_new(13, 0) = 0; + _P_new(14, 0) = 0; + _P_new(15, 0) = 0; + _P_new(16, 0) = 0; + _P_new(17, 0) = 0; + _P_new(18, 0) = 0; + _P_new(19, 0) = 0; + _P_new(20, 0) = 0; + _P_new(21, 0) = 0; + _P_new(22, 0) = 0; + _P_new(23, 0) = 0; + _P_new(0, 1) = -_tmp10 * _tmp14 + _tmp12 * _tmp9 - _tmp13 * _tmp27 + _tmp15 * _tmp29 + + _tmp16 * _tmp5 + _tmp17 - _tmp19 * _tmp28 + _tmp21 * _tmp30; + _P_new(1, 1) = -_tmp10 * _tmp31 + _tmp23 - _tmp27 * _tmp33 + _tmp29 * _tmp35 + _tmp30 * _tmp36 + + _tmp32 * _tmp9 + _tmp34 * _tmp5 + _tmp37 + _tmp39; + _P_new(2, 1) = 0; + _P_new(3, 1) = 0; + _P_new(4, 1) = 0; + _P_new(5, 1) = 0; + _P_new(6, 1) = 0; + _P_new(7, 1) = 0; + _P_new(8, 1) = 0; + _P_new(9, 1) = 0; + _P_new(10, 1) = 0; + _P_new(11, 1) = 0; + _P_new(12, 1) = 0; + _P_new(13, 1) = 0; + _P_new(14, 1) = 0; + _P_new(15, 1) = 0; + _P_new(16, 1) = 0; + _P_new(17, 1) = 0; + _P_new(18, 1) = 0; + _P_new(19, 1) = 0; + _P_new(20, 1) = 0; + _P_new(21, 1) = 0; + _P_new(22, 1) = 0; + _P_new(23, 1) = 0; + _P_new(0, 2) = _tmp11 * _tmp14 - _tmp12 * _tmp27 - _tmp13 * _tmp9 + _tmp15 + _tmp16 * _tmp30 + + _tmp17 * _tmp2 - _tmp19 * _tmp40 + _tmp21 * _tmp41; + _P_new(1, 2) = _tmp11 * _tmp31 - _tmp19 * _tmp42 + _tmp2 * _tmp37 - _tmp27 * _tmp32 + + _tmp30 * _tmp34 - _tmp33 * _tmp9 + _tmp35 + _tmp36 * _tmp41; + _P_new(2, 2) = _tmp11 * _tmp43 + _tmp2 * _tmp47 + _tmp25 - _tmp27 * _tmp44 + _tmp30 * _tmp48 + + _tmp39 + _tmp41 * _tmp46 - _tmp45 * _tmp9 + _tmp49; + _P_new(3, 2) = 0; + _P_new(4, 2) = 0; + _P_new(5, 2) = 0; + _P_new(6, 2) = 0; + _P_new(7, 2) = 0; + _P_new(8, 2) = 0; + _P_new(9, 2) = 0; + _P_new(10, 2) = 0; + _P_new(11, 2) = 0; + _P_new(12, 2) = 0; + _P_new(13, 2) = 0; + _P_new(14, 2) = 0; + _P_new(15, 2) = 0; + _P_new(16, 2) = 0; + _P_new(17, 2) = 0; + _P_new(18, 2) = 0; + _P_new(19, 2) = 0; + _P_new(20, 2) = 0; + _P_new(21, 2) = 0; + _P_new(22, 2) = 0; + _P_new(23, 2) = 0; + _P_new(0, 3) = _tmp10 * _tmp13 - _tmp11 * _tmp12 - _tmp14 * _tmp27 + _tmp15 * _tmp8 + _tmp16 + + _tmp17 * _tmp41 - _tmp19 * _tmp50 + _tmp21 * _tmp29; + _P_new(1, 3) = _tmp10 * _tmp33 - _tmp11 * _tmp32 - _tmp19 * _tmp51 - _tmp27 * _tmp31 + + _tmp29 * _tmp36 + _tmp34 + _tmp35 * _tmp8 + _tmp37 * _tmp41; + _P_new(2, 3) = _tmp10 * _tmp45 - _tmp11 * _tmp44 - _tmp19 * _tmp52 - _tmp27 * _tmp43 + + _tmp29 * _tmp46 + _tmp41 * _tmp47 + _tmp48 + _tmp49 * _tmp8; + _P_new(3, 3) = _tmp10 * _tmp55 - _tmp11 * _tmp54 + _tmp26 - _tmp27 * _tmp53 + _tmp29 * _tmp57 + + _tmp38 + _tmp41 * _tmp58 + _tmp56 * _tmp8 + _tmp59; + _P_new(4, 3) = 0; + _P_new(5, 3) = 0; + _P_new(6, 3) = 0; + _P_new(7, 3) = 0; + _P_new(8, 3) = 0; + _P_new(9, 3) = 0; + _P_new(10, 3) = 0; + _P_new(11, 3) = 0; + _P_new(12, 3) = 0; + _P_new(13, 3) = 0; + _P_new(14, 3) = 0; + _P_new(15, 3) = 0; + _P_new(16, 3) = 0; + _P_new(17, 3) = 0; + _P_new(18, 3) = 0; + _P_new(19, 3) = 0; + _P_new(20, 3) = 0; + _P_new(21, 3) = 0; + _P_new(22, 3) = 0; + _P_new(23, 3) = 0; + _P_new(0, 4) = _tmp15 * _tmp86 + _tmp16 * _tmp74 + _tmp17 * _tmp66 + _tmp21 * _tmp69 - + _tmp75 * _tmp76 - _tmp77 * _tmp78 - _tmp81 * _tmp82 + _tmp87; + _P_new(1, 4) = _tmp34 * _tmp74 + _tmp35 * _tmp86 + _tmp36 * _tmp69 + _tmp37 * _tmp66 - + _tmp76 * _tmp89 - _tmp78 * _tmp90 - _tmp81 * _tmp88 + _tmp91; + _P_new(2, 4) = _tmp46 * _tmp69 + _tmp47 * _tmp66 + _tmp48 * _tmp74 + _tmp49 * _tmp86 - + _tmp76 * _tmp92 - _tmp78 * _tmp93 - _tmp81 * _tmp94 + _tmp96; + _P_new(3, 4) = _tmp100 + _tmp56 * _tmp86 + _tmp57 * _tmp69 + _tmp58 * _tmp66 + _tmp59 * _tmp74 - + _tmp76 * _tmp99 - _tmp78 * _tmp98 - _tmp81 * _tmp97; + _P_new(4, 4) = -_tmp101 * _tmp81 - _tmp102 * _tmp78 - _tmp103 * _tmp76 + + std::pow(_tmp106, Scalar(2)) * d_vel_var(2, 0) + + std::pow(_tmp109, Scalar(2)) * d_vel_var(1, 0) + _tmp110 * _tmp69 + + _tmp111 * _tmp66 + _tmp112 * _tmp86 + _tmp113 * _tmp74 + _tmp114 + + std::pow(_tmp81, Scalar(2)) * d_vel_var(0, 0); + _P_new(5, 4) = 0; + _P_new(6, 4) = 0; + _P_new(7, 4) = 0; + _P_new(8, 4) = 0; + _P_new(9, 4) = 0; + _P_new(10, 4) = 0; + _P_new(11, 4) = 0; + _P_new(12, 4) = 0; + _P_new(13, 4) = 0; + _P_new(14, 4) = 0; + _P_new(15, 4) = 0; + _P_new(16, 4) = 0; + _P_new(17, 4) = 0; + _P_new(18, 4) = 0; + _P_new(19, 4) = 0; + _P_new(20, 4) = 0; + _P_new(21, 4) = 0; + _P_new(22, 4) = 0; + _P_new(23, 4) = 0; + _P_new(0, 5) = _tmp117 * _tmp15 + _tmp119 * _tmp21 + _tmp121 * _tmp16 - _tmp122 * _tmp82 - + _tmp123 * _tmp75 - _tmp125 * _tmp77 + _tmp128 * _tmp17 + _tmp129; + _P_new(1, 5) = _tmp117 * _tmp35 + _tmp119 * _tmp36 + _tmp121 * _tmp34 - _tmp122 * _tmp88 - + _tmp123 * _tmp89 - _tmp125 * _tmp90 + _tmp128 * _tmp37 + _tmp130; + _P_new(2, 5) = _tmp117 * _tmp49 + _tmp119 * _tmp46 + _tmp121 * _tmp48 - _tmp122 * _tmp94 - + _tmp123 * _tmp92 - _tmp125 * _tmp93 + _tmp128 * _tmp47 + _tmp132; + _P_new(3, 5) = _tmp117 * _tmp56 + _tmp119 * _tmp57 + _tmp121 * _tmp59 - _tmp122 * _tmp97 - + _tmp123 * _tmp99 - _tmp125 * _tmp98 + _tmp128 * _tmp58 + _tmp133; + _P_new(4, 5) = -_tmp101 * _tmp122 - _tmp102 * _tmp125 - _tmp103 * _tmp123 + _tmp106 * _tmp139 + + _tmp109 * _tmp125 * d_vel_var(1, 0) + _tmp110 * _tmp119 + _tmp111 * _tmp128 + + _tmp112 * _tmp117 + _tmp113 * _tmp121 + _tmp134 * _tmp135 + _tmp140; + _P_new(5, 5) = _tmp117 * _tmp147 + _tmp119 * _tmp146 + _tmp121 * _tmp141 - _tmp122 * _tmp143 - + _tmp123 * _tmp144 + std::pow(_tmp125, Scalar(2)) * d_vel_var(1, 0) - + _tmp125 * _tmp145 + _tmp128 * _tmp142 + + std::pow(_tmp134, Scalar(2)) * d_vel_var(0, 0) + + std::pow(_tmp138, Scalar(2)) * d_vel_var(2, 0) + _tmp148; + _P_new(6, 5) = 0; + _P_new(7, 5) = 0; + _P_new(8, 5) = 0; + _P_new(9, 5) = 0; + _P_new(10, 5) = 0; + _P_new(11, 5) = 0; + _P_new(12, 5) = 0; + _P_new(13, 5) = 0; + _P_new(14, 5) = 0; + _P_new(15, 5) = 0; + _P_new(16, 5) = 0; + _P_new(17, 5) = 0; + _P_new(18, 5) = 0; + _P_new(19, 5) = 0; + _P_new(20, 5) = 0; + _P_new(21, 5) = 0; + _P_new(22, 5) = 0; + _P_new(23, 5) = 0; + _P_new(0, 6) = _tmp149 * _tmp16 + _tmp15 * _tmp151 + _tmp150 * _tmp21 + _tmp152 * _tmp17 - + _tmp153 * _tmp82 - _tmp154 * _tmp77 - _tmp155 * _tmp75 + _tmp156; + _P_new(1, 6) = _tmp149 * _tmp34 + _tmp150 * _tmp36 + _tmp151 * _tmp35 + _tmp152 * _tmp37 - + _tmp153 * _tmp88 - _tmp154 * _tmp90 - _tmp155 * _tmp89 + _tmp157; + _P_new(2, 6) = _tmp149 * _tmp48 + _tmp150 * _tmp46 + _tmp151 * _tmp49 + _tmp152 * _tmp47 - + _tmp153 * _tmp94 - _tmp154 * _tmp93 - _tmp155 * _tmp92 + _tmp159; + _P_new(3, 6) = _tmp149 * _tmp59 + _tmp150 * _tmp57 + _tmp151 * _tmp56 + _tmp152 * _tmp58 - + _tmp153 * _tmp97 - _tmp154 * _tmp98 - _tmp155 * _tmp99 + _tmp160; + _P_new(4, 6) = -_tmp101 * _tmp153 - _tmp102 * _tmp154 - _tmp103 * _tmp155 + + _tmp106 * _tmp155 * d_vel_var(2, 0) + _tmp109 * _tmp163 + _tmp110 * _tmp150 + + _tmp111 * _tmp152 + _tmp112 * _tmp151 + _tmp113 * _tmp149 + _tmp135 * _tmp161 + + _tmp164; + _P_new(5, 6) = _tmp125 * _tmp163 + _tmp134 * _tmp161 * d_vel_var(0, 0) + _tmp139 * _tmp155 + + _tmp141 * _tmp149 + _tmp142 * _tmp152 - _tmp143 * _tmp153 - _tmp144 * _tmp155 - + _tmp145 * _tmp154 + _tmp146 * _tmp150 + _tmp147 * _tmp151 + _tmp165; + _P_new(6, 6) = + _tmp149 * (P(0, 3) * _tmp150 + P(1, 3) * _tmp152 - P(13, 3) * _tmp153 - P(14, 3) * _tmp154 - + P(15, 3) * _tmp155 + P(2, 3) * _tmp151 + P(3, 3) * _tmp149 + P(6, 3)) + + _tmp150 * (P(0, 0) * _tmp150 + P(1, 0) * _tmp152 - P(13, 0) * _tmp153 - P(14, 0) * _tmp154 - + P(15, 0) * _tmp155 + P(2, 0) * _tmp151 + P(3, 0) * _tmp149 + P(6, 0)) + + _tmp151 * (P(0, 2) * _tmp150 + P(1, 2) * _tmp152 - P(13, 2) * _tmp153 - P(14, 2) * _tmp154 - + P(15, 2) * _tmp155 + P(2, 2) * _tmp151 + P(3, 2) * _tmp149 + P(6, 2)) + + _tmp152 * (P(0, 1) * _tmp150 + P(1, 1) * _tmp152 - P(13, 1) * _tmp153 - P(14, 1) * _tmp154 - + P(15, 1) * _tmp155 + P(2, 1) * _tmp151 + P(3, 1) * _tmp149 + P(6, 1)) - + _tmp153 * _tmp166 - _tmp154 * _tmp167 + std::pow(_tmp155, Scalar(2)) * d_vel_var(2, 0) - + _tmp155 * _tmp168 + std::pow(_tmp161, Scalar(2)) * d_vel_var(0, 0) + + std::pow(_tmp162, Scalar(2)) * d_vel_var(1, 0) + _tmp169; + _P_new(7, 6) = 0; + _P_new(8, 6) = 0; + _P_new(9, 6) = 0; + _P_new(10, 6) = 0; + _P_new(11, 6) = 0; + _P_new(12, 6) = 0; + _P_new(13, 6) = 0; + _P_new(14, 6) = 0; + _P_new(15, 6) = 0; + _P_new(16, 6) = 0; + _P_new(17, 6) = 0; + _P_new(18, 6) = 0; + _P_new(19, 6) = 0; + _P_new(20, 6) = 0; + _P_new(21, 6) = 0; + _P_new(22, 6) = 0; + _P_new(23, 6) = 0; + _P_new(0, 7) = P(0, 7) + P(1, 7) * _tmp8 + P(10, 7) * _tmp11 + P(11, 7) * _tmp10 + + P(12, 7) * _tmp9 + P(2, 7) * _tmp5 + P(3, 7) * _tmp2 + _tmp87 * dt; + _P_new(1, 7) = P(0, 7) * _tmp30 + P(1, 7) - P(10, 7) * _tmp27 + P(11, 7) * _tmp9 - + P(12, 7) * _tmp10 + P(2, 7) * _tmp29 + P(3, 7) * _tmp5 + _tmp91 * dt; + _P_new(2, 7) = P(0, 7) * _tmp41 + P(1, 7) * _tmp2 - P(10, 7) * _tmp9 - P(11, 7) * _tmp27 + + P(12, 7) * _tmp11 + P(2, 7) + P(3, 7) * _tmp30 + _tmp96 * dt; + _P_new(3, 7) = P(0, 7) * _tmp29 + P(1, 7) * _tmp41 + P(10, 7) * _tmp10 - P(11, 7) * _tmp11 - + P(12, 7) * _tmp27 + P(2, 7) * _tmp8 + P(3, 7) + _tmp100 * dt; + _P_new(4, 7) = P(0, 7) * _tmp69 + P(1, 7) * _tmp66 - P(13, 7) * _tmp81 - P(14, 7) * _tmp78 - + P(15, 7) * _tmp76 + P(2, 7) * _tmp86 + P(3, 7) * _tmp74 + P(4, 7) + _tmp114 * dt; + _P_new(5, 7) = + P(0, 7) * _tmp119 + P(1, 7) * _tmp128 - P(13, 7) * _tmp122 - P(14, 7) * _tmp125 - + P(15, 7) * _tmp123 + P(2, 7) * _tmp117 + P(3, 7) * _tmp121 + P(5, 7) + + dt * (P(0, 4) * _tmp119 + P(1, 4) * _tmp128 - P(13, 4) * _tmp122 - P(14, 4) * _tmp125 - + P(15, 4) * _tmp123 + P(2, 4) * _tmp117 + P(3, 4) * _tmp121 + P(5, 4)); + _P_new(6, 7) = + P(0, 7) * _tmp150 + P(1, 7) * _tmp152 - P(13, 7) * _tmp153 - P(14, 7) * _tmp154 - + P(15, 7) * _tmp155 + P(2, 7) * _tmp151 + P(3, 7) * _tmp149 + P(6, 7) + + dt * (P(0, 4) * _tmp150 + P(1, 4) * _tmp152 - P(13, 4) * _tmp153 - P(14, 4) * _tmp154 - + P(15, 4) * _tmp155 + P(2, 4) * _tmp151 + P(3, 4) * _tmp149 + P(6, 4)); + _P_new(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4)); + _P_new(8, 7) = 0; + _P_new(9, 7) = 0; + _P_new(10, 7) = 0; + _P_new(11, 7) = 0; + _P_new(12, 7) = 0; + _P_new(13, 7) = 0; + _P_new(14, 7) = 0; + _P_new(15, 7) = 0; + _P_new(16, 7) = 0; + _P_new(17, 7) = 0; + _P_new(18, 7) = 0; + _P_new(19, 7) = 0; + _P_new(20, 7) = 0; + _P_new(21, 7) = 0; + _P_new(22, 7) = 0; + _P_new(23, 7) = 0; + _P_new(0, 8) = P(0, 8) + P(1, 8) * _tmp8 + P(10, 8) * _tmp11 + P(11, 8) * _tmp10 + + P(12, 8) * _tmp9 + P(2, 8) * _tmp5 + P(3, 8) * _tmp2 + _tmp129 * dt; + _P_new(1, 8) = P(0, 8) * _tmp30 + P(1, 8) - P(10, 8) * _tmp27 + P(11, 8) * _tmp9 - + P(12, 8) * _tmp10 + P(2, 8) * _tmp29 + P(3, 8) * _tmp5 + _tmp130 * dt; + _P_new(2, 8) = P(0, 8) * _tmp41 + P(1, 8) * _tmp2 - P(10, 8) * _tmp9 + P(12, 8) * _tmp11 + + P(2, 8) + P(3, 8) * _tmp30 + _tmp132 * dt - _tmp170 * state(0, 0); + _P_new(3, 8) = P(0, 8) * _tmp29 + P(1, 8) * _tmp41 + P(10, 8) * _tmp10 - P(12, 8) * _tmp27 + + P(2, 8) * _tmp8 + P(3, 8) + _tmp133 * dt - _tmp170 * state(1, 0); + _P_new(4, 8) = P(0, 8) * _tmp69 + P(1, 8) * _tmp66 - P(13, 8) * _tmp81 - P(14, 8) * _tmp78 - + P(15, 8) * _tmp76 + P(2, 8) * _tmp86 + P(3, 8) * _tmp74 + P(4, 8) + _tmp140 * dt; + _P_new(5, 8) = P(0, 8) * _tmp119 + P(1, 8) * _tmp128 - P(13, 8) * _tmp122 - P(14, 8) * _tmp125 - + P(15, 8) * _tmp123 + P(2, 8) * _tmp117 + P(3, 8) * _tmp121 + P(5, 8) + + _tmp148 * dt; + _P_new(6, 8) = + P(0, 8) * _tmp150 + P(1, 8) * _tmp152 - P(13, 8) * _tmp153 - P(14, 8) * _tmp154 - + P(15, 8) * _tmp155 + P(2, 8) * _tmp151 + P(3, 8) * _tmp149 + P(6, 8) + + dt * (P(0, 5) * _tmp150 + P(1, 5) * _tmp152 - P(13, 5) * _tmp153 - P(14, 5) * _tmp154 - + P(15, 5) * _tmp155 + P(2, 5) * _tmp151 + P(3, 5) * _tmp149 + P(6, 5)); + _P_new(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5)); + _P_new(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5)); + _P_new(9, 8) = 0; + _P_new(10, 8) = 0; + _P_new(11, 8) = 0; + _P_new(12, 8) = 0; + _P_new(13, 8) = 0; + _P_new(14, 8) = 0; + _P_new(15, 8) = 0; + _P_new(16, 8) = 0; + _P_new(17, 8) = 0; + _P_new(18, 8) = 0; + _P_new(19, 8) = 0; + _P_new(20, 8) = 0; + _P_new(21, 8) = 0; + _P_new(22, 8) = 0; + _P_new(23, 8) = 0; + _P_new(0, 9) = P(0, 9) + P(1, 9) * _tmp8 + P(10, 9) * _tmp11 + P(11, 9) * _tmp10 + + P(12, 9) * _tmp9 + P(2, 9) * _tmp5 + P(3, 9) * _tmp2 + _tmp156 * dt; + _P_new(1, 9) = P(0, 9) * _tmp30 + P(1, 9) - P(10, 9) * _tmp27 + P(11, 9) * _tmp9 - + P(12, 9) * _tmp10 + P(2, 9) * _tmp29 + P(3, 9) * _tmp5 + _tmp157 * dt; + _P_new(2, 9) = P(0, 9) * _tmp41 + P(1, 9) * _tmp2 - P(10, 9) * _tmp9 - P(11, 9) * _tmp27 + + P(12, 9) * _tmp11 + P(2, 9) + P(3, 9) * _tmp30 + _tmp159 * dt; + _P_new(3, 9) = P(0, 9) * _tmp29 + P(1, 9) * _tmp41 + P(10, 9) * _tmp10 - P(11, 9) * _tmp11 - + P(12, 9) * _tmp27 + P(2, 9) * _tmp8 + P(3, 9) + _tmp160 * dt; + _P_new(4, 9) = P(0, 9) * _tmp69 + P(1, 9) * _tmp66 - P(13, 9) * _tmp81 - P(14, 9) * _tmp78 - + P(15, 9) * _tmp76 + P(2, 9) * _tmp86 + P(3, 9) * _tmp74 + P(4, 9) + _tmp164 * dt; + _P_new(5, 9) = P(0, 9) * _tmp119 + P(1, 9) * _tmp128 - P(13, 9) * _tmp122 - P(14, 9) * _tmp125 - + P(15, 9) * _tmp123 + P(2, 9) * _tmp117 + P(3, 9) * _tmp121 + P(5, 9) + + _tmp165 * dt; + _P_new(6, 9) = P(0, 9) * _tmp150 + P(1, 9) * _tmp152 - P(13, 9) * _tmp153 - P(14, 9) * _tmp154 - + P(15, 9) * _tmp155 + P(2, 9) * _tmp151 + P(3, 9) * _tmp149 + P(6, 9) + + _tmp169 * dt; + _P_new(7, 9) = P(4, 9) * dt + P(7, 9) + dt * (P(4, 6) * dt + P(7, 6)); + _P_new(8, 9) = P(5, 9) * dt + P(8, 9) + dt * (P(5, 6) * dt + P(8, 6)); + _P_new(9, 9) = P(6, 9) * dt + P(9, 9) + dt * (P(6, 6) * dt + P(9, 6)); + _P_new(10, 9) = 0; + _P_new(11, 9) = 0; + _P_new(12, 9) = 0; + _P_new(13, 9) = 0; + _P_new(14, 9) = 0; + _P_new(15, 9) = 0; + _P_new(16, 9) = 0; + _P_new(17, 9) = 0; + _P_new(18, 9) = 0; + _P_new(19, 9) = 0; + _P_new(20, 9) = 0; + _P_new(21, 9) = 0; + _P_new(22, 9) = 0; + _P_new(23, 9) = 0; + _P_new(0, 10) = _tmp13; + _P_new(1, 10) = _tmp33; + _P_new(2, 10) = _tmp45; + _P_new(3, 10) = _tmp55; + _P_new(4, 10) = P(0, 10) * _tmp69 + P(1, 10) * _tmp66 - P(13, 10) * _tmp81 - + P(14, 10) * _tmp78 - P(15, 10) * _tmp76 + P(2, 10) * _tmp86 + + P(3, 10) * _tmp74 + P(4, 10); + _P_new(5, 10) = P(0, 10) * _tmp119 + P(1, 10) * _tmp128 - P(13, 10) * _tmp122 - + P(14, 10) * _tmp125 - P(15, 10) * _tmp123 + P(2, 10) * _tmp117 + + P(3, 10) * _tmp121 + P(5, 10); + _P_new(6, 10) = P(0, 10) * _tmp150 + P(1, 10) * _tmp152 - P(13, 10) * _tmp153 - + P(14, 10) * _tmp154 - P(15, 10) * _tmp155 + P(2, 10) * _tmp151 + + P(3, 10) * _tmp149 + P(6, 10); + _P_new(7, 10) = P(4, 10) * dt + P(7, 10); + _P_new(8, 10) = P(5, 10) * dt + P(8, 10); + _P_new(9, 10) = P(6, 10) * dt + P(9, 10); + _P_new(10, 10) = P(10, 10); + _P_new(11, 10) = 0; + _P_new(12, 10) = 0; + _P_new(13, 10) = 0; + _P_new(14, 10) = 0; + _P_new(15, 10) = 0; + _P_new(16, 10) = 0; + _P_new(17, 10) = 0; + _P_new(18, 10) = 0; + _P_new(19, 10) = 0; + _P_new(20, 10) = 0; + _P_new(21, 10) = 0; + _P_new(22, 10) = 0; + _P_new(23, 10) = 0; + _P_new(0, 11) = _tmp12; + _P_new(1, 11) = _tmp32; + _P_new(2, 11) = _tmp44; + _P_new(3, 11) = _tmp54; + _P_new(4, 11) = P(0, 11) * _tmp69 + P(1, 11) * _tmp66 - P(13, 11) * _tmp81 - + P(14, 11) * _tmp78 - P(15, 11) * _tmp76 + P(2, 11) * _tmp86 + + P(3, 11) * _tmp74 + P(4, 11); + _P_new(5, 11) = P(0, 11) * _tmp119 + P(1, 11) * _tmp128 - P(13, 11) * _tmp122 - + P(14, 11) * _tmp125 - P(15, 11) * _tmp123 + P(2, 11) * _tmp117 + + P(3, 11) * _tmp121 + P(5, 11); + _P_new(6, 11) = P(0, 11) * _tmp150 + P(1, 11) * _tmp152 - P(13, 11) * _tmp153 - + P(14, 11) * _tmp154 - P(15, 11) * _tmp155 + P(2, 11) * _tmp151 + + P(3, 11) * _tmp149 + P(6, 11); + _P_new(7, 11) = P(4, 11) * dt + P(7, 11); + _P_new(8, 11) = P(5, 11) * dt + P(8, 11); + _P_new(9, 11) = P(6, 11) * dt + P(9, 11); + _P_new(10, 11) = P(10, 11); + _P_new(11, 11) = P(11, 11); + _P_new(12, 11) = 0; + _P_new(13, 11) = 0; + _P_new(14, 11) = 0; + _P_new(15, 11) = 0; + _P_new(16, 11) = 0; + _P_new(17, 11) = 0; + _P_new(18, 11) = 0; + _P_new(19, 11) = 0; + _P_new(20, 11) = 0; + _P_new(21, 11) = 0; + _P_new(22, 11) = 0; + _P_new(23, 11) = 0; + _P_new(0, 12) = _tmp14; + _P_new(1, 12) = _tmp31; + _P_new(2, 12) = _tmp43; + _P_new(3, 12) = _tmp53; + _P_new(4, 12) = P(0, 12) * _tmp69 + P(1, 12) * _tmp66 - P(13, 12) * _tmp81 - + P(14, 12) * _tmp78 - P(15, 12) * _tmp76 + P(2, 12) * _tmp86 + + P(3, 12) * _tmp74 + P(4, 12); + _P_new(5, 12) = P(0, 12) * _tmp119 + P(1, 12) * _tmp128 - P(13, 12) * _tmp122 - + P(14, 12) * _tmp125 - P(15, 12) * _tmp123 + P(2, 12) * _tmp117 + + P(3, 12) * _tmp121 + P(5, 12); + _P_new(6, 12) = P(0, 12) * _tmp150 + P(1, 12) * _tmp152 - P(13, 12) * _tmp153 - + P(14, 12) * _tmp154 - P(15, 12) * _tmp155 + P(2, 12) * _tmp151 + + P(3, 12) * _tmp149 + P(6, 12); + _P_new(7, 12) = P(4, 12) * dt + P(7, 12); + _P_new(8, 12) = P(5, 12) * dt + P(8, 12); + _P_new(9, 12) = P(6, 12) * dt + P(9, 12); + _P_new(10, 12) = P(10, 12); + _P_new(11, 12) = P(11, 12); + _P_new(12, 12) = P(12, 12); + _P_new(13, 12) = 0; + _P_new(14, 12) = 0; + _P_new(15, 12) = 0; + _P_new(16, 12) = 0; + _P_new(17, 12) = 0; + _P_new(18, 12) = 0; + _P_new(19, 12) = 0; + _P_new(20, 12) = 0; + _P_new(21, 12) = 0; + _P_new(22, 12) = 0; + _P_new(23, 12) = 0; + _P_new(0, 13) = _tmp82; + _P_new(1, 13) = _tmp88; + _P_new(2, 13) = _tmp94; + _P_new(3, 13) = _tmp97; + _P_new(4, 13) = _tmp101; + _P_new(5, 13) = _tmp143; + _P_new(6, 13) = _tmp166; + _P_new(7, 13) = P(4, 13) * dt + P(7, 13); + _P_new(8, 13) = P(5, 13) * dt + P(8, 13); + _P_new(9, 13) = P(6, 13) * dt + P(9, 13); + _P_new(10, 13) = P(10, 13); + _P_new(11, 13) = P(11, 13); + _P_new(12, 13) = P(12, 13); + _P_new(13, 13) = P(13, 13); + _P_new(14, 13) = 0; + _P_new(15, 13) = 0; + _P_new(16, 13) = 0; + _P_new(17, 13) = 0; + _P_new(18, 13) = 0; + _P_new(19, 13) = 0; + _P_new(20, 13) = 0; + _P_new(21, 13) = 0; + _P_new(22, 13) = 0; + _P_new(23, 13) = 0; + _P_new(0, 14) = _tmp77; + _P_new(1, 14) = _tmp90; + _P_new(2, 14) = _tmp93; + _P_new(3, 14) = _tmp98; + _P_new(4, 14) = _tmp102; + _P_new(5, 14) = _tmp145; + _P_new(6, 14) = _tmp167; + _P_new(7, 14) = P(4, 14) * dt + P(7, 14); + _P_new(8, 14) = P(5, 14) * dt + P(8, 14); + _P_new(9, 14) = P(6, 14) * dt + P(9, 14); + _P_new(10, 14) = P(10, 14); + _P_new(11, 14) = P(11, 14); + _P_new(12, 14) = P(12, 14); + _P_new(13, 14) = P(13, 14); + _P_new(14, 14) = P(14, 14); + _P_new(15, 14) = 0; + _P_new(16, 14) = 0; + _P_new(17, 14) = 0; + _P_new(18, 14) = 0; + _P_new(19, 14) = 0; + _P_new(20, 14) = 0; + _P_new(21, 14) = 0; + _P_new(22, 14) = 0; + _P_new(23, 14) = 0; + _P_new(0, 15) = _tmp75; + _P_new(1, 15) = _tmp89; + _P_new(2, 15) = _tmp92; + _P_new(3, 15) = _tmp99; + _P_new(4, 15) = _tmp103; + _P_new(5, 15) = _tmp144; + _P_new(6, 15) = _tmp168; + _P_new(7, 15) = P(4, 15) * dt + P(7, 15); + _P_new(8, 15) = P(5, 15) * dt + P(8, 15); + _P_new(9, 15) = P(6, 15) * dt + P(9, 15); + _P_new(10, 15) = P(10, 15); + _P_new(11, 15) = P(11, 15); + _P_new(12, 15) = P(12, 15); + _P_new(13, 15) = P(13, 15); + _P_new(14, 15) = P(14, 15); + _P_new(15, 15) = P(15, 15); + _P_new(16, 15) = 0; + _P_new(17, 15) = 0; + _P_new(18, 15) = 0; + _P_new(19, 15) = 0; + _P_new(20, 15) = 0; + _P_new(21, 15) = 0; + _P_new(22, 15) = 0; + _P_new(23, 15) = 0; + _P_new(0, 16) = P(0, 16) + P(1, 16) * _tmp8 + P(10, 16) * _tmp11 + P(11, 16) * _tmp10 + + P(12, 16) * _tmp9 + P(2, 16) * _tmp5 + P(3, 16) * _tmp2; + _P_new(1, 16) = P(0, 16) * _tmp30 + P(1, 16) - P(10, 16) * _tmp27 + P(11, 16) * _tmp9 - + P(12, 16) * _tmp10 + P(2, 16) * _tmp29 + P(3, 16) * _tmp5; + _P_new(2, 16) = P(0, 16) * _tmp41 + P(1, 16) * _tmp2 - P(10, 16) * _tmp9 - P(11, 16) * _tmp27 + + P(12, 16) * _tmp11 + P(2, 16) + P(3, 16) * _tmp30; + _P_new(3, 16) = P(0, 16) * _tmp29 + P(1, 16) * _tmp41 + P(10, 16) * _tmp10 - + P(11, 16) * _tmp11 - P(12, 16) * _tmp27 + P(2, 16) * _tmp8 + P(3, 16); + _P_new(4, 16) = P(0, 16) * _tmp69 + P(1, 16) * _tmp66 - P(13, 16) * _tmp81 - + P(14, 16) * _tmp78 - P(15, 16) * _tmp76 + P(2, 16) * _tmp86 + + P(3, 16) * _tmp74 + P(4, 16); + _P_new(5, 16) = P(0, 16) * _tmp119 + P(1, 16) * _tmp128 - P(13, 16) * _tmp122 - + P(14, 16) * _tmp125 - P(15, 16) * _tmp123 + P(2, 16) * _tmp117 + + P(3, 16) * _tmp121 + P(5, 16); + _P_new(6, 16) = P(0, 16) * _tmp150 + P(1, 16) * _tmp152 - P(13, 16) * _tmp153 - + P(14, 16) * _tmp154 - P(15, 16) * _tmp155 + P(2, 16) * _tmp151 + + P(3, 16) * _tmp149 + P(6, 16); + _P_new(7, 16) = P(4, 16) * dt + P(7, 16); + _P_new(8, 16) = P(5, 16) * dt + P(8, 16); + _P_new(9, 16) = P(6, 16) * dt + P(9, 16); + _P_new(10, 16) = P(10, 16); + _P_new(11, 16) = P(11, 16); + _P_new(12, 16) = P(12, 16); + _P_new(13, 16) = P(13, 16); + _P_new(14, 16) = P(14, 16); + _P_new(15, 16) = P(15, 16); + _P_new(16, 16) = P(16, 16); + _P_new(17, 16) = 0; + _P_new(18, 16) = 0; + _P_new(19, 16) = 0; + _P_new(20, 16) = 0; + _P_new(21, 16) = 0; + _P_new(22, 16) = 0; + _P_new(23, 16) = 0; + _P_new(0, 17) = P(0, 17) + P(1, 17) * _tmp8 + P(10, 17) * _tmp11 + P(11, 17) * _tmp10 + + P(12, 17) * _tmp9 + P(2, 17) * _tmp5 + P(3, 17) * _tmp2; + _P_new(1, 17) = P(0, 17) * _tmp30 + P(1, 17) - P(10, 17) * _tmp27 + P(11, 17) * _tmp9 - + P(12, 17) * _tmp10 + P(2, 17) * _tmp29 + P(3, 17) * _tmp5; + _P_new(2, 17) = P(0, 17) * _tmp41 + P(1, 17) * _tmp2 - P(10, 17) * _tmp9 - P(11, 17) * _tmp27 + + P(12, 17) * _tmp11 + P(2, 17) + P(3, 17) * _tmp30; + _P_new(3, 17) = P(0, 17) * _tmp29 + P(1, 17) * _tmp41 + P(10, 17) * _tmp10 - + P(11, 17) * _tmp11 - P(12, 17) * _tmp27 + P(2, 17) * _tmp8 + P(3, 17); + _P_new(4, 17) = P(0, 17) * _tmp69 + P(1, 17) * _tmp66 - P(13, 17) * _tmp81 - + P(14, 17) * _tmp78 - P(15, 17) * _tmp76 + P(2, 17) * _tmp86 + + P(3, 17) * _tmp74 + P(4, 17); + _P_new(5, 17) = P(0, 17) * _tmp119 + P(1, 17) * _tmp128 - P(13, 17) * _tmp122 - + P(14, 17) * _tmp125 - P(15, 17) * _tmp123 + P(2, 17) * _tmp117 + + P(3, 17) * _tmp121 + P(5, 17); + _P_new(6, 17) = P(0, 17) * _tmp150 + P(1, 17) * _tmp152 - P(13, 17) * _tmp153 - + P(14, 17) * _tmp154 - P(15, 17) * _tmp155 + P(2, 17) * _tmp151 + + P(3, 17) * _tmp149 + P(6, 17); + _P_new(7, 17) = P(4, 17) * dt + P(7, 17); + _P_new(8, 17) = P(5, 17) * dt + P(8, 17); + _P_new(9, 17) = P(6, 17) * dt + P(9, 17); + _P_new(10, 17) = P(10, 17); + _P_new(11, 17) = P(11, 17); + _P_new(12, 17) = P(12, 17); + _P_new(13, 17) = P(13, 17); + _P_new(14, 17) = P(14, 17); + _P_new(15, 17) = P(15, 17); + _P_new(16, 17) = P(16, 17); + _P_new(17, 17) = P(17, 17); + _P_new(18, 17) = 0; + _P_new(19, 17) = 0; + _P_new(20, 17) = 0; + _P_new(21, 17) = 0; + _P_new(22, 17) = 0; + _P_new(23, 17) = 0; + _P_new(0, 18) = P(0, 18) + P(1, 18) * _tmp8 + P(10, 18) * _tmp11 + P(11, 18) * _tmp10 + + P(12, 18) * _tmp9 + P(2, 18) * _tmp5 + P(3, 18) * _tmp2; + _P_new(1, 18) = P(0, 18) * _tmp30 + P(1, 18) - P(10, 18) * _tmp27 + P(11, 18) * _tmp9 - + P(12, 18) * _tmp10 + P(2, 18) * _tmp29 + P(3, 18) * _tmp5; + _P_new(2, 18) = P(0, 18) * _tmp41 + P(1, 18) * _tmp2 - P(10, 18) * _tmp9 - P(11, 18) * _tmp27 + + P(12, 18) * _tmp11 + P(2, 18) + P(3, 18) * _tmp30; + _P_new(3, 18) = P(0, 18) * _tmp29 + P(1, 18) * _tmp41 + P(10, 18) * _tmp10 - + P(11, 18) * _tmp11 - P(12, 18) * _tmp27 + P(2, 18) * _tmp8 + P(3, 18); + _P_new(4, 18) = P(0, 18) * _tmp69 + P(1, 18) * _tmp66 - P(13, 18) * _tmp81 - + P(14, 18) * _tmp78 - P(15, 18) * _tmp76 + P(2, 18) * _tmp86 + + P(3, 18) * _tmp74 + P(4, 18); + _P_new(5, 18) = P(0, 18) * _tmp119 + P(1, 18) * _tmp128 - P(13, 18) * _tmp122 - + P(14, 18) * _tmp125 - P(15, 18) * _tmp123 + P(2, 18) * _tmp117 + + P(3, 18) * _tmp121 + P(5, 18); + _P_new(6, 18) = P(0, 18) * _tmp150 + P(1, 18) * _tmp152 - P(13, 18) * _tmp153 - + P(14, 18) * _tmp154 - P(15, 18) * _tmp155 + P(2, 18) * _tmp151 + + P(3, 18) * _tmp149 + P(6, 18); + _P_new(7, 18) = P(4, 18) * dt + P(7, 18); + _P_new(8, 18) = P(5, 18) * dt + P(8, 18); + _P_new(9, 18) = P(6, 18) * dt + P(9, 18); + _P_new(10, 18) = P(10, 18); + _P_new(11, 18) = P(11, 18); + _P_new(12, 18) = P(12, 18); + _P_new(13, 18) = P(13, 18); + _P_new(14, 18) = P(14, 18); + _P_new(15, 18) = P(15, 18); + _P_new(16, 18) = P(16, 18); + _P_new(17, 18) = P(17, 18); + _P_new(18, 18) = P(18, 18); + _P_new(19, 18) = 0; + _P_new(20, 18) = 0; + _P_new(21, 18) = 0; + _P_new(22, 18) = 0; + _P_new(23, 18) = 0; + _P_new(0, 19) = P(0, 19) + P(1, 19) * _tmp8 + P(10, 19) * _tmp11 + P(11, 19) * _tmp10 + + P(12, 19) * _tmp9 + P(2, 19) * _tmp5 + P(3, 19) * _tmp2; + _P_new(1, 19) = P(0, 19) * _tmp30 + P(1, 19) - P(10, 19) * _tmp27 + P(11, 19) * _tmp9 - + P(12, 19) * _tmp10 + P(2, 19) * _tmp29 + P(3, 19) * _tmp5; + _P_new(2, 19) = P(0, 19) * _tmp41 + P(1, 19) * _tmp2 - P(10, 19) * _tmp9 - P(11, 19) * _tmp27 + + P(12, 19) * _tmp11 + P(2, 19) + P(3, 19) * _tmp30; + _P_new(3, 19) = P(0, 19) * _tmp29 + P(1, 19) * _tmp41 + P(10, 19) * _tmp10 - + P(11, 19) * _tmp11 - P(12, 19) * _tmp27 + P(2, 19) * _tmp8 + P(3, 19); + _P_new(4, 19) = P(0, 19) * _tmp69 + P(1, 19) * _tmp66 - P(13, 19) * _tmp81 - + P(14, 19) * _tmp78 - P(15, 19) * _tmp76 + P(2, 19) * _tmp86 + + P(3, 19) * _tmp74 + P(4, 19); + _P_new(5, 19) = P(0, 19) * _tmp119 + P(1, 19) * _tmp128 - P(13, 19) * _tmp122 - + P(14, 19) * _tmp125 - P(15, 19) * _tmp123 + P(2, 19) * _tmp117 + + P(3, 19) * _tmp121 + P(5, 19); + _P_new(6, 19) = P(0, 19) * _tmp150 + P(1, 19) * _tmp152 - P(13, 19) * _tmp153 - + P(14, 19) * _tmp154 - P(15, 19) * _tmp155 + P(2, 19) * _tmp151 + + P(3, 19) * _tmp149 + P(6, 19); + _P_new(7, 19) = P(4, 19) * dt + P(7, 19); + _P_new(8, 19) = P(5, 19) * dt + P(8, 19); + _P_new(9, 19) = P(6, 19) * dt + P(9, 19); + _P_new(10, 19) = P(10, 19); + _P_new(11, 19) = P(11, 19); + _P_new(12, 19) = P(12, 19); + _P_new(13, 19) = P(13, 19); + _P_new(14, 19) = P(14, 19); + _P_new(15, 19) = P(15, 19); + _P_new(16, 19) = P(16, 19); + _P_new(17, 19) = P(17, 19); + _P_new(18, 19) = P(18, 19); + _P_new(19, 19) = P(19, 19); + _P_new(20, 19) = 0; + _P_new(21, 19) = 0; + _P_new(22, 19) = 0; + _P_new(23, 19) = 0; + _P_new(0, 20) = P(0, 20) + P(1, 20) * _tmp8 + P(10, 20) * _tmp11 + P(11, 20) * _tmp10 + + P(12, 20) * _tmp9 + P(2, 20) * _tmp5 + P(3, 20) * _tmp2; + _P_new(1, 20) = P(0, 20) * _tmp30 + P(1, 20) - P(10, 20) * _tmp27 + P(11, 20) * _tmp9 - + P(12, 20) * _tmp10 + P(2, 20) * _tmp29 + P(3, 20) * _tmp5; + _P_new(2, 20) = P(0, 20) * _tmp41 + P(1, 20) * _tmp2 - P(10, 20) * _tmp9 - P(11, 20) * _tmp27 + + P(12, 20) * _tmp11 + P(2, 20) + P(3, 20) * _tmp30; + _P_new(3, 20) = P(0, 20) * _tmp29 + P(1, 20) * _tmp41 + P(10, 20) * _tmp10 - + P(11, 20) * _tmp11 - P(12, 20) * _tmp27 + P(2, 20) * _tmp8 + P(3, 20); + _P_new(4, 20) = P(0, 20) * _tmp69 + P(1, 20) * _tmp66 - P(13, 20) * _tmp81 - + P(14, 20) * _tmp78 - P(15, 20) * _tmp76 + P(2, 20) * _tmp86 + + P(3, 20) * _tmp74 + P(4, 20); + _P_new(5, 20) = P(0, 20) * _tmp119 + P(1, 20) * _tmp128 - P(13, 20) * _tmp122 - + P(14, 20) * _tmp125 - P(15, 20) * _tmp123 + P(2, 20) * _tmp117 + + P(3, 20) * _tmp121 + P(5, 20); + _P_new(6, 20) = P(0, 20) * _tmp150 + P(1, 20) * _tmp152 - P(13, 20) * _tmp153 - + P(14, 20) * _tmp154 - P(15, 20) * _tmp155 + P(2, 20) * _tmp151 + + P(3, 20) * _tmp149 + P(6, 20); + _P_new(7, 20) = P(4, 20) * dt + P(7, 20); + _P_new(8, 20) = P(5, 20) * dt + P(8, 20); + _P_new(9, 20) = P(6, 20) * dt + P(9, 20); + _P_new(10, 20) = P(10, 20); + _P_new(11, 20) = P(11, 20); + _P_new(12, 20) = P(12, 20); + _P_new(13, 20) = P(13, 20); + _P_new(14, 20) = P(14, 20); + _P_new(15, 20) = P(15, 20); + _P_new(16, 20) = P(16, 20); + _P_new(17, 20) = P(17, 20); + _P_new(18, 20) = P(18, 20); + _P_new(19, 20) = P(19, 20); + _P_new(20, 20) = P(20, 20); + _P_new(21, 20) = 0; + _P_new(22, 20) = 0; + _P_new(23, 20) = 0; + _P_new(0, 21) = P(0, 21) + P(1, 21) * _tmp8 + P(10, 21) * _tmp11 + P(11, 21) * _tmp10 + + P(12, 21) * _tmp9 + P(2, 21) * _tmp5 + P(3, 21) * _tmp2; + _P_new(1, 21) = P(0, 21) * _tmp30 + P(1, 21) - P(10, 21) * _tmp27 + P(11, 21) * _tmp9 - + P(12, 21) * _tmp10 + P(2, 21) * _tmp29 + P(3, 21) * _tmp5; + _P_new(2, 21) = P(0, 21) * _tmp41 + P(1, 21) * _tmp2 - P(10, 21) * _tmp9 - P(11, 21) * _tmp27 + + P(12, 21) * _tmp11 + P(2, 21) + P(3, 21) * _tmp30; + _P_new(3, 21) = P(0, 21) * _tmp29 + P(1, 21) * _tmp41 + P(10, 21) * _tmp10 - + P(11, 21) * _tmp11 - P(12, 21) * _tmp27 + P(2, 21) * _tmp8 + P(3, 21); + _P_new(4, 21) = P(0, 21) * _tmp69 + P(1, 21) * _tmp66 - P(13, 21) * _tmp81 - + P(14, 21) * _tmp78 - P(15, 21) * _tmp76 + P(2, 21) * _tmp86 + + P(3, 21) * _tmp74 + P(4, 21); + _P_new(5, 21) = P(0, 21) * _tmp119 + P(1, 21) * _tmp128 - P(13, 21) * _tmp122 - + P(14, 21) * _tmp125 - P(15, 21) * _tmp123 + P(2, 21) * _tmp117 + + P(3, 21) * _tmp121 + P(5, 21); + _P_new(6, 21) = P(0, 21) * _tmp150 + P(1, 21) * _tmp152 - P(13, 21) * _tmp153 - + P(14, 21) * _tmp154 - P(15, 21) * _tmp155 + P(2, 21) * _tmp151 + + P(3, 21) * _tmp149 + P(6, 21); + _P_new(7, 21) = P(4, 21) * dt + P(7, 21); + _P_new(8, 21) = P(5, 21) * dt + P(8, 21); + _P_new(9, 21) = P(6, 21) * dt + P(9, 21); + _P_new(10, 21) = P(10, 21); + _P_new(11, 21) = P(11, 21); + _P_new(12, 21) = P(12, 21); + _P_new(13, 21) = P(13, 21); + _P_new(14, 21) = P(14, 21); + _P_new(15, 21) = P(15, 21); + _P_new(16, 21) = P(16, 21); + _P_new(17, 21) = P(17, 21); + _P_new(18, 21) = P(18, 21); + _P_new(19, 21) = P(19, 21); + _P_new(20, 21) = P(20, 21); + _P_new(21, 21) = P(21, 21); + _P_new(22, 21) = 0; + _P_new(23, 21) = 0; + _P_new(0, 22) = P(0, 22) + P(1, 22) * _tmp8 + P(10, 22) * _tmp11 + P(11, 22) * _tmp10 + + P(12, 22) * _tmp9 + P(2, 22) * _tmp5 + P(3, 22) * _tmp2; + _P_new(1, 22) = P(0, 22) * _tmp30 + P(1, 22) - P(10, 22) * _tmp27 + P(11, 22) * _tmp9 - + P(12, 22) * _tmp10 + P(2, 22) * _tmp29 + P(3, 22) * _tmp5; + _P_new(2, 22) = P(0, 22) * _tmp41 + P(1, 22) * _tmp2 - P(10, 22) * _tmp9 - P(11, 22) * _tmp27 + + P(12, 22) * _tmp11 + P(2, 22) + P(3, 22) * _tmp30; + _P_new(3, 22) = P(0, 22) * _tmp29 + P(1, 22) * _tmp41 + P(10, 22) * _tmp10 - + P(11, 22) * _tmp11 - P(12, 22) * _tmp27 + P(2, 22) * _tmp8 + P(3, 22); + _P_new(4, 22) = P(0, 22) * _tmp69 + P(1, 22) * _tmp66 - P(13, 22) * _tmp81 - + P(14, 22) * _tmp78 - P(15, 22) * _tmp76 + P(2, 22) * _tmp86 + + P(3, 22) * _tmp74 + P(4, 22); + _P_new(5, 22) = P(0, 22) * _tmp119 + P(1, 22) * _tmp128 - P(13, 22) * _tmp122 - + P(14, 22) * _tmp125 - P(15, 22) * _tmp123 + P(2, 22) * _tmp117 + + P(3, 22) * _tmp121 + P(5, 22); + _P_new(6, 22) = P(0, 22) * _tmp150 + P(1, 22) * _tmp152 - P(13, 22) * _tmp153 - + P(14, 22) * _tmp154 - P(15, 22) * _tmp155 + P(2, 22) * _tmp151 + + P(3, 22) * _tmp149 + P(6, 22); + _P_new(7, 22) = P(4, 22) * dt + P(7, 22); + _P_new(8, 22) = P(5, 22) * dt + P(8, 22); + _P_new(9, 22) = P(6, 22) * dt + P(9, 22); + _P_new(10, 22) = P(10, 22); + _P_new(11, 22) = P(11, 22); + _P_new(12, 22) = P(12, 22); + _P_new(13, 22) = P(13, 22); + _P_new(14, 22) = P(14, 22); + _P_new(15, 22) = P(15, 22); + _P_new(16, 22) = P(16, 22); + _P_new(17, 22) = P(17, 22); + _P_new(18, 22) = P(18, 22); + _P_new(19, 22) = P(19, 22); + _P_new(20, 22) = P(20, 22); + _P_new(21, 22) = P(21, 22); + _P_new(22, 22) = P(22, 22); + _P_new(23, 22) = 0; + _P_new(0, 23) = P(0, 23) + P(1, 23) * _tmp8 + P(10, 23) * _tmp11 + P(11, 23) * _tmp10 + + P(12, 23) * _tmp9 + P(2, 23) * _tmp5 + P(3, 23) * _tmp2; + _P_new(1, 23) = P(0, 23) * _tmp30 + P(1, 23) - P(10, 23) * _tmp27 + P(11, 23) * _tmp9 - + P(12, 23) * _tmp10 + P(2, 23) * _tmp29 + P(3, 23) * _tmp5; + _P_new(2, 23) = P(0, 23) * _tmp41 + P(1, 23) * _tmp2 - P(10, 23) * _tmp9 - P(11, 23) * _tmp27 + + P(12, 23) * _tmp11 + P(2, 23) + P(3, 23) * _tmp30; + _P_new(3, 23) = P(0, 23) * _tmp29 + P(1, 23) * _tmp41 + P(10, 23) * _tmp10 - + P(11, 23) * _tmp11 - P(12, 23) * _tmp27 + P(2, 23) * _tmp8 + P(3, 23); + _P_new(4, 23) = P(0, 23) * _tmp69 + P(1, 23) * _tmp66 - P(13, 23) * _tmp81 - + P(14, 23) * _tmp78 - P(15, 23) * _tmp76 + P(2, 23) * _tmp86 + + P(3, 23) * _tmp74 + P(4, 23); + _P_new(5, 23) = P(0, 23) * _tmp119 + P(1, 23) * _tmp128 - P(13, 23) * _tmp122 - + P(14, 23) * _tmp125 - P(15, 23) * _tmp123 + P(2, 23) * _tmp117 + + P(3, 23) * _tmp121 + P(5, 23); + _P_new(6, 23) = P(0, 23) * _tmp150 + P(1, 23) * _tmp152 - P(13, 23) * _tmp153 - + P(14, 23) * _tmp154 - P(15, 23) * _tmp155 + P(2, 23) * _tmp151 + + P(3, 23) * _tmp149 + P(6, 23); + _P_new(7, 23) = P(4, 23) * dt + P(7, 23); + _P_new(8, 23) = P(5, 23) * dt + P(8, 23); + _P_new(9, 23) = P(6, 23) * dt + P(9, 23); + _P_new(10, 23) = P(10, 23); + _P_new(11, 23) = P(11, 23); + _P_new(12, 23) = P(12, 23); + _P_new(13, 23) = P(13, 23); + _P_new(14, 23) = P(14, 23); + _P_new(15, 23) = P(15, 23); + _P_new(16, 23) = P(16, 23); + _P_new(17, 23) = P(17, 23); + _P_new(18, 23) = P(18, 23); + _P_new(19, 23) = P(19, 23); + _P_new(20, 23) = P(20, 23); + _P_new(21, 23) = P(21, 23); + _P_new(22, 23) = P(22, 23); + _P_new(23, 23) = P(23, 23); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym