From 78ce46f00777f07d24c31bdc5e2edb203be2c0f4 Mon Sep 17 00:00:00 2001 From: Kamil Ritz Date: Sat, 19 Dec 2020 11:34:18 +0100 Subject: [PATCH] Removing Matlab derivation --- EKF/documentation/readme.txt | 4 +- .../Inertial Nav EKF/Airspeed Fusion.txt | 50 -- .../Covariance Prediction.txt | 367 --------- .../Magnetic Declination Fusion.txt | 67 -- .../Inertial Nav EKF/Magnetometer Fusion.txt | 142 ---- .../generated/Inertial Nav EKF/Notes.txt | 22 - .../Inertial Nav EKF/Optical Flow Fusion.txt | 267 ------- .../Inertial Nav EKF/Sideslip Fusion.txt | 69 -- .../Simple Magnetometer Fusion.txt | 42 - .../Inertial Nav EKF/Yaw Angle Fusion.txt | 67 -- EKF/matlab/generated/README.md | 5 - .../scripts/Inertial Nav EKF/Airspeed.mat | Bin 8710 -> 0 bytes .../scripts/Inertial Nav EKF/C_code24.txt | 686 ---------------- .../scripts/Inertial Nav EKF/C_code4.txt | 35 - .../C_code_use_matrix_lib24.txt | 686 ---------------- .../scripts/Inertial Nav EKF/ConvertCToC.m | 58 -- .../scripts/Inertial Nav EKF/ConvertToC.m | 213 ----- .../scripts/Inertial Nav EKF/ConvertToM.m | 46 -- .../DeriveYawResetEquations.m | 106 --- EKF/matlab/scripts/Inertial Nav EKF/Drag.mat | Bin 18384 -> 0 bytes .../scripts/Inertial Nav EKF/EulToQuat.m | 23 - .../GenerateNavFilterEquations.m | 519 ------------ EKF/matlab/scripts/Inertial Nav EKF/H_LOSX.c | 8 - EKF/matlab/scripts/Inertial Nav EKF/H_LOSY.c | 8 - EKF/matlab/scripts/Inertial Nav EKF/H_VELX.c | 7 - EKF/matlab/scripts/Inertial Nav EKF/H_VELY.c | 7 - EKF/matlab/scripts/Inertial Nav EKF/H_VELZ.c | 7 - EKF/matlab/scripts/Inertial Nav EKF/K_LOSX.c | 117 --- EKF/matlab/scripts/Inertial Nav EKF/K_LOSY.c | 117 --- EKF/matlab/scripts/Inertial Nav EKF/K_VELX.c | 116 --- EKF/matlab/scripts/Inertial Nav EKF/K_VELY.c | 116 --- EKF/matlab/scripts/Inertial Nav EKF/K_VELZ.c | 116 --- EKF/matlab/scripts/Inertial Nav EKF/LOSX.c | 160 ---- EKF/matlab/scripts/Inertial Nav EKF/LOSY.c | 162 ---- .../scripts/Inertial Nav EKF/M_code24.txt | 686 ---------------- .../scripts/Inertial Nav EKF/Magnetometer.mat | Bin 19484 -> 0 bytes .../scripts/Inertial Nav EKF/NormQuat.m | 5 - .../Inertial Nav EKF/OptimiseAlgebra.m | 29 - .../scripts/Inertial Nav EKF/Q_airdata.c | 89 --- .../scripts/Inertial Nav EKF/Quat2Tbn.m | 14 - .../scripts/Inertial Nav EKF/QuatDivide.m | 16 - .../QuatErrTransferEquations.m | 62 -- .../scripts/Inertial Nav EKF/QuatMult.m | 5 - .../scripts/Inertial Nav EKF/QuatToEul.m | 9 - .../scripts/Inertial Nav EKF/RotToQuat.m | 10 - .../scripts/Inertial Nav EKF/SaveScriptCode.m | 666 ---------------- .../scripts/Inertial Nav EKF/Sideslip.mat | Bin 9809 -> 0 bytes .../StateAndCovariancePrediction.mat | Bin 135809 -> 0 bytes .../Inertial Nav EKF/StatePrediction.mat | Bin 106832 -> 0 bytes .../Inertial Nav EKF/SymbolicOutput24.mat | Bin 184296 -> 0 bytes .../Inertial Nav EKF/SymbolicOutput24.txt | 742 ------------------ EKF/matlab/scripts/Inertial Nav EKF/Tbn_312.c | 15 - EKF/matlab/scripts/Inertial Nav EKF/Tbn_321.c | 15 - .../scripts/Inertial Nav EKF/Tbn_quat.c | 17 - .../scripts/Inertial Nav EKF/YawCovariance.c | 42 - .../scripts/Inertial Nav EKF/calcH_YAW312.c | 38 - .../scripts/Inertial Nav EKF/calcH_YAW321.c | 42 - .../scripts/Inertial Nav EKF/calcH_YAWGPS.c | 38 - .../scripts/Inertial Nav EKF/calcMAGD.c | 46 -- .../Inertial Nav EKF/derive_air_data_errors.m | 97 --- .../scripts/Inertial Nav EKF/fix_c_code.m | 92 --- .../scripts/Inertial Nav EKF/polar2cart_cov.m | 8 - .../scripts/Inertial Nav EKF/quat2yaw312.m | 29 - .../scripts/Inertial Nav EKF/quat2yaw321.m | 29 - .../scripts/Inertial Nav EKF/quatCovMat.c | 59 -- .../scripts/Inertial Nav EKF/rotVarVec.c | 19 - EKF/matlab/scripts/Inertial Nav EKF/temp1.mat | Bin 1043 -> 0 bytes EKF/matlab/scripts/Inertial Nav EKF/temp2.mat | Bin 2434 -> 0 bytes EKF/matlab/scripts/Inertial Nav EKF/temp3.mat | Bin 2443 -> 0 bytes .../test_output_filter_gains.m | 35 - .../scripts/Inertial Nav EKF/wind_cov.py | 20 - .../scripts/Inertial Nav EKF/yaw_input_312.c | 2 - .../scripts/Inertial Nav EKF/yaw_input_321.c | 2 - 73 files changed, 2 insertions(+), 7391 deletions(-) delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Notes.txt delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt delete mode 100644 EKF/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt delete mode 100644 EKF/matlab/generated/README.md delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Airspeed.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/C_code24.txt delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/C_code4.txt delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/C_code_use_matrix_lib24.txt delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/ConvertCToC.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/ConvertToC.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/ConvertToM.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/DeriveYawResetEquations.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Drag.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/EulToQuat.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/H_LOSX.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/H_LOSY.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/H_VELX.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/H_VELY.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/H_VELZ.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/K_LOSX.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/K_LOSY.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/K_VELX.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/K_VELY.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/K_VELZ.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/LOSX.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/LOSY.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/M_code24.txt delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Magnetometer.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/NormQuat.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Q_airdata.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Quat2Tbn.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/QuatDivide.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/QuatMult.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/QuatToEul.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/RotToQuat.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Sideslip.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/StatePrediction.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Tbn_312.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Tbn_321.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/Tbn_quat.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/calcH_YAWGPS.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/calcMAGD.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/derive_air_data_errors.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/fix_c_code.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/quat2yaw312.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/quat2yaw321.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/quatCovMat.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/rotVarVec.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/temp1.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/temp2.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/temp3.mat delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/test_output_filter_gains.m delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/yaw_input_312.c delete mode 100644 EKF/matlab/scripts/Inertial Nav EKF/yaw_input_321.c diff --git a/EKF/documentation/readme.txt b/EKF/documentation/readme.txt index a76e0c3d8e..0bb5a83f80 100644 --- a/EKF/documentation/readme.txt +++ b/EKF/documentation/readme.txt @@ -1,7 +1,7 @@ -The EKF uses a range of techniques acquired from several years of experience including an original method to handle the delayed time horizon problem. +The EKF uses a range of techniques acquired from several years of experience including an original method to handle the delayed time horizon problem. A list of references I have found useful has been included. -- The Matlab script used to derive the autocoded expressions in the EKF can be found here: https://github.com/PX4/ecl/blob/master/EKF/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m +- The python script used to derive the autogenerated expressions in the EKF can be found here: https://github.com/PX4/ecl/blob/master/EKF/python/ekf_derivation/main.py - A working Matlab model of the filter capable of replaying flight data can be found here: https://github.com/PX4/ecl/tree/master/EKF/matlab/EKF_replay diff --git a/EKF/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt b/EKF/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt deleted file mode 100644 index 8cc6b0c703..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt +++ /dev/null @@ -1,50 +0,0 @@ -// Auto code for fusion of true airspeed - -// Calculate the observation jacobian - -// intermediate variable from algebraic optimisation -float SH_TAS[3]; -SH_TAS[0] = 1.0f/v_tas_pred; -SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f; -SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f; - -// observation jacobian -float H_TAS[24]; -H_TAS[4] = SH_TAS[2]; -H_TAS[5] = SH_TAS[1]; -H_TAS[6] = vd*SH_TAS[0]; -H_TAS[22] = -SH_TAS[2]; -H_TAS[23] = -SH_TAS[1]; - -// calculate the Kalman gain matrix - -// intermediate variables - note SK_TAS[0] is 1/(innovation variance) -float SK_TAS[2]; -SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); -SK_TAS[1] = SH_TAS[1]; - -float Kfusion[24]; -Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); -Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); -Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); -Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]); -Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]); -Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]); -Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]); -Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]); -Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); -Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); -Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); -Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); -Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); -Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]); -Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]); -Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]); -Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]); -Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]); -Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]); -Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]); -Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]); -Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]); -Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); -Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); diff --git a/EKF/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt b/EKF/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt deleted file mode 100644 index 348ab0827f..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt +++ /dev/null @@ -1,367 +0,0 @@ -// Auto code for covariance prediction - -// Intermediate expressions obtained from algebraic optimisation -float SF[21] = {}; -SF[0] = dvz - dvz_b; -SF[1] = dvy - dvy_b; -SF[2] = dvx - dvx_b; -SF[3] = 2.0f*q1*SF[2] + 2.0f*q2*SF[1] + 2.0f*q3*SF[0]; -SF[4] = 2.0f*q0*SF[1] - 2.0f*q1*SF[0] + 2.0f*q3*SF[2]; -SF[5] = 2.0f*q0*SF[2] + 2.0f*q2*SF[0] - 2.0f*q3*SF[1]; -SF[6] = day*0.5f - day_b*0.5f; -SF[7] = daz*0.5f - daz_b*0.5f; -SF[8] = dax*0.5f - dax_b*0.5f; -SF[9] = dax_b*0.5f - dax*0.5f; -SF[10] = daz_b*0.5f - daz*0.5f; -SF[11] = day_b*0.5f - day*0.5f; -SF[12] = 2.0f*q1*SF[1]; -SF[13] = 2.0f*q0*SF[0]; -SF[14] = q1*0.5f; -SF[15] = q2*0.5f; -SF[16] = q3*0.5f; -SF[17] = sq(q3); -SF[18] = sq(q2); -SF[19] = sq(q1); -SF[20] = sq(q0); - -float SG[8] = {}; -SG[0] = q0*0.5f; -SG[1] = sq(q3); -SG[2] = sq(q2); -SG[3] = sq(q1); -SG[4] = sq(q0); -SG[5] = 2.0f*q2*q3; -SG[6] = 2.0f*q1*q3; -SG[7] = 2.0f*q1*q2; - -float SQ[11] = {}; -SQ[0] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[7] + 2.0f*q0*q3); -SQ[1] = dvzVar*(SG[6] + 2.0f*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[7] - 2.0f*q0*q3); -SQ[2] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[6] + 2.0f*q0*q2) - dvyVar*(SG[7] - 2.0f*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2.0f*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); -SQ[3] = (dayVar*q1*SG[0])*0.5f - (dazVar*q1*SG[0])*0.5f - (daxVar*q2*q3)*0.25f; -SQ[4] = (dazVar*q2*SG[0])*0.5f - (daxVar*q2*SG[0])*0.5f - (dayVar*q1*q3)*0.25f; -SQ[5] = (daxVar*q3*SG[0])*0.5f - (dayVar*q3*SG[0])*0.5f - (dazVar*q1*q2)*0.25f; -SQ[6] = (daxVar*q1*q2)*0.25f - (dazVar*q3*SG[0])*0.5f - (dayVar*q1*q2)*0.25f; -SQ[7] = (dazVar*q1*q3)*0.25f - (daxVar*q1*q3)*0.25f - (dayVar*q2*SG[0])*0.5f; -SQ[8] = (dayVar*q2*q3)*0.25f - (daxVar*q1*SG[0])*0.5f - (dazVar*q2*q3)*0.25f; -SQ[9] = sq(SG[0]); -SQ[10] = sq(q1); - -float SPP[11] = {}; -SPP[0] = SF[12] + SF[13] - 2.0f*q2*SF[2]; -SPP[1] = SF[17] - SF[18] - SF[19] + SF[20]; -SPP[2] = SF[17] - SF[18] + SF[19] - SF[20]; -SPP[3] = SF[17] + SF[18] - SF[19] - SF[20]; -SPP[4] = 2.0f*q0*q2 - 2.0f*q1*q3; -SPP[5] = 2.0f*q0*q1 - 2.0f*q2*q3; -SPP[6] = 2.0f*q0*q3 - 2.0f*q1*q2; -SPP[7] = 2.0f*q0*q1 + 2.0f*q2*q3; -SPP[8] = 2.0f*q0*q3 + 2.0f*q1*q2; -SPP[9] = 2.0f*q0*q2 + 2.0f*q1*q3; -SPP[10] = SF[16]; - -// Calculate uppder diagonal elements of the predicted covariance matrix -// Use symmetry to assign value to lower diagonal -// Note: this matrix does not include the process noise for stationary states, it only includes the effect of noise on the inertial measurements. -// Process noise for stationary states must be added later. -float nextP[24][24]; -nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])*0.25f + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))*0.25f + (dazVar*sq(q3))*0.25f; -nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))*0.5f; -nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)*0.5f + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))*0.25f + (dazVar*sq(q2))*0.25f - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))*0.5f; -nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))*0.5f; -nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)*0.5f + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))*0.5f; -nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])*0.25f - (P[11][2]*q0)*0.5f + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))*0.25f - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))*0.5f; -nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))*0.5f; -nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)*0.5f + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))*0.5f; -nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)*0.5f + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))*0.5f; -nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])*0.25f + dazVar*SQ[9] - (P[12][3]*q0)*0.5f + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))*0.25f - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))*0.5f; -nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); -nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)*0.5f + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); -nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)*0.5f + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); -nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)*0.5f + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); -nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2.0f*q0*q3) + dvzVar*sq(SG[6] + 2.0f*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]); -nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); -nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)*0.5f + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); -nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)*0.5f + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); -nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)*0.5f + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); -nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); -nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2.0f*q0*q3) + dvzVar*sq(SG[5] - 2.0f*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]); -nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); -nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)*0.5f + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); -nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)*0.5f + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); -nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)*0.5f + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); -nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); -nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]); -nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2.0f*q0*q2) + dvyVar*sq(SG[5] + 2.0f*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]); -nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]); -nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)*0.5f + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2); -nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)*0.5f + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2); -nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)*0.5f + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2); -nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]); -nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]); -nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]); -nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); -nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]); -nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)*0.5f + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2); -nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)*0.5f + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2); -nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)*0.5f + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2); -nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]); -nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]); -nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]); -nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); -nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); -nextP[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]); -nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)*0.5f + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2); -nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)*0.5f + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2); -nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)*0.5f + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2); -nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]); -nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]); -nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]); -nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); -nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); -nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); -nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]; -nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)*0.5f; -nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)*0.5f; -nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)*0.5f; -nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9]; -nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5]; -nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1]; -nextP[7][10] = P[7][10] + P[4][10]*dt; -nextP[8][10] = P[8][10] + P[5][10]*dt; -nextP[9][10] = P[9][10] + P[6][10]*dt; -nextP[10][10] = P[10][10]; -nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]; -nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)*0.5f; -nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)*0.5f; -nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)*0.5f; -nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9]; -nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5]; -nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1]; -nextP[7][11] = P[7][11] + P[4][11]*dt; -nextP[8][11] = P[8][11] + P[5][11]*dt; -nextP[9][11] = P[9][11] + P[6][11]*dt; -nextP[10][11] = P[10][11]; -nextP[11][11] = P[11][11]; -nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]; -nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)*0.5f; -nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)*0.5f; -nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)*0.5f; -nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9]; -nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5]; -nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1]; -nextP[7][12] = P[7][12] + P[4][12]*dt; -nextP[8][12] = P[8][12] + P[5][12]*dt; -nextP[9][12] = P[9][12] + P[6][12]*dt; -nextP[10][12] = P[10][12]; -nextP[11][12] = P[11][12]; -nextP[12][12] = P[12][12]; -nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]; -nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)*0.5f; -nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)*0.5f; -nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)*0.5f; -nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]; -nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]; -nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]; -nextP[7][13] = P[7][13] + P[4][13]*dt; -nextP[8][13] = P[8][13] + P[5][13]*dt; -nextP[9][13] = P[9][13] + P[6][13]*dt; -nextP[10][13] = P[10][13]; -nextP[11][13] = P[11][13]; -nextP[12][13] = P[12][13]; -nextP[13][13] = P[13][13]; -nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]; -nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)*0.5f; -nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)*0.5f; -nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)*0.5f; -nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]; -nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]; -nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]; -nextP[7][14] = P[7][14] + P[4][14]*dt; -nextP[8][14] = P[8][14] + P[5][14]*dt; -nextP[9][14] = P[9][14] + P[6][14]*dt; -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]; -nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]; -nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)*0.5f; -nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)*0.5f; -nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)*0.5f; -nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]; -nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]; -nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]; -nextP[7][15] = P[7][15] + P[4][15]*dt; -nextP[8][15] = P[8][15] + P[5][15]*dt; -nextP[9][15] = P[9][15] + P[6][15]*dt; -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]; -nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10]; -nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)*0.5f; -nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)*0.5f; -nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)*0.5f; -nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9]; -nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5]; -nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1]; -nextP[7][16] = P[7][16] + P[4][16]*dt; -nextP[8][16] = P[8][16] + P[5][16]*dt; -nextP[9][16] = P[9][16] + P[6][16]*dt; -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]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10]; -nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)*0.5f; -nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)*0.5f; -nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)*0.5f; -nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9]; -nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5]; -nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1]; -nextP[7][17] = P[7][17] + P[4][17]*dt; -nextP[8][17] = P[8][17] + P[5][17]*dt; -nextP[9][17] = P[9][17] + P[6][17]*dt; -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]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10]; -nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)*0.5f; -nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)*0.5f; -nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)*0.5f; -nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9]; -nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5]; -nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1]; -nextP[7][18] = P[7][18] + P[4][18]*dt; -nextP[8][18] = P[8][18] + P[5][18]*dt; -nextP[9][18] = P[9][18] + P[6][18]*dt; -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]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10]; -nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)*0.5f; -nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)*0.5f; -nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)*0.5f; -nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9]; -nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5]; -nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1]; -nextP[7][19] = P[7][19] + P[4][19]*dt; -nextP[8][19] = P[8][19] + P[5][19]*dt; -nextP[9][19] = P[9][19] + P[6][19]*dt; -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]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10]; -nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)*0.5f; -nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)*0.5f; -nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)*0.5f; -nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9]; -nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5]; -nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1]; -nextP[7][20] = P[7][20] + P[4][20]*dt; -nextP[8][20] = P[8][20] + P[5][20]*dt; -nextP[9][20] = P[9][20] + P[6][20]*dt; -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]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10]; -nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)*0.5f; -nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)*0.5f; -nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)*0.5f; -nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9]; -nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5]; -nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1]; -nextP[7][21] = P[7][21] + P[4][21]*dt; -nextP[8][21] = P[8][21] + P[5][21]*dt; -nextP[9][21] = P[9][21] + P[6][21]*dt; -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]; -nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10]; -nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)*0.5f; -nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)*0.5f; -nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)*0.5f; -nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9]; -nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5]; -nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1]; -nextP[7][22] = P[7][22] + P[4][22]*dt; -nextP[8][22] = P[8][22] + P[5][22]*dt; -nextP[9][22] = P[9][22] + P[6][22]*dt; -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]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10]; -nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)*0.5f; -nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)*0.5f; -nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)*0.5f; -nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9]; -nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5]; -nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1]; -nextP[7][23] = P[7][23] + P[4][23]*dt; -nextP[8][23] = P[8][23] + P[5][23]*dt; -nextP[9][23] = P[9][23] + P[6][23]*dt; -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]; diff --git a/EKF/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt b/EKF/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt deleted file mode 100644 index f3bd513e24..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt +++ /dev/null @@ -1,67 +0,0 @@ -/* -Autocode for fusion of a magnetic declination estimate where the innovation is given by - -innovation = atanf(magMeasEarthFrameEast/magMeasEarthFrameNorth) - declinationAngle; - -magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to earth frame. -declinationAngle is the estimated declination as that location - -This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements -(e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors -can cause the magnetic declination of the earth field estimates to slowly rotate. - -Divide by zero protection and protection against a badly conditioned covariance matrix must be included. -*/ - -// Calculate intermediate variable -float t2 = magE*magE; -float t3 = magN*magN; -float t4 = t2+t3; -float t5 = P[16][16]*t2; -float t6 = P[17][17]*t3; -float t7 = t2*t2; -float t8 = R_DECL*t7; -float t9 = t3*t3; -float t10 = R_DECL*t9; -float t11 = R_DECL*t2*t3*2.0f; -float t14 = P[16][17]*magE*magN; -float t15 = P[17][16]*magE*magN; -float t12 = t5+t6+t8+t10+t11-t14-t15; -float t13 = 1.0f / t12; -float t16 = magE; -float t17 = magN; -float t18 = t16*t16; -float t19 = t17*t17; -float t20 = t18+t19; -float t21 = 1.0f/t20; - -// Calculate the observation Jacobian -// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost -H_DECL[16] = -t16*t21; -H_DECL[17] = t17*t21; - -// Calculate the Kalman gains -Kfusion[0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN); -Kfusion[1] = -t4*t13*(P[1][16]*magE-P[1][17]*magN); -Kfusion[2] = -t4*t13*(P[2][16]*magE-P[2][17]*magN); -Kfusion[3] = -t4*t13*(P[3][16]*magE-P[3][17]*magN); -Kfusion[4] = -t4*t13*(P[4][16]*magE-P[4][17]*magN); -Kfusion[5] = -t4*t13*(P[5][16]*magE-P[5][17]*magN); -Kfusion[6] = -t4*t13*(P[6][16]*magE-P[6][17]*magN); -Kfusion[7] = -t4*t13*(P[7][16]*magE-P[7][17]*magN); -Kfusion[8] = -t4*t13*(P[8][16]*magE-P[8][17]*magN); -Kfusion[9] = -t4*t13*(P[9][16]*magE-P[9][17]*magN); -Kfusion[10] = -t4*t13*(P[10][16]*magE-P[10][17]*magN); -Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN); -Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN); -Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN); -Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN); -Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN); -Kfusion[16] = -t4*t13*(P[16][16]*magE-P[16][17]*magN); -Kfusion[17] = -t4*t13*(P[17][16]*magE-P[17][17]*magN); -Kfusion[18] = -t4*t13*(P[18][16]*magE-P[18][17]*magN); -Kfusion[19] = -t4*t13*(P[19][16]*magE-P[19][17]*magN); -Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN); -Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN); -Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); -Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); diff --git a/EKF/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt b/EKF/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt deleted file mode 100644 index 9df90c509f..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt +++ /dev/null @@ -1,142 +0,0 @@ -// Auto code for fusion of XYZ magnetometer vector measurement -// Sequential fusion of each axis is used (assumes uncorrrelated observation errors) - -// common expressions for XYZ axis observation jacobians -SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1; -SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2; -SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3; -SH_MAG[3] = sq(q3); -SH_MAG[4] = sq(q2); -SH_MAG[5] = sq(q1); -SH_MAG[6] = sq(q0); -SH_MAG[7] = 2.0f*magN*q0; -SH_MAG[8] = 2.0f*magE*q3; - -// X axis observation jacobians -H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; -H_MAG[1] = SH_MAG[0]; -H_MAG[2] = -SH_MAG[1]; -H_MAG[3] = SH_MAG[2]; -H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; -H_MAG[17] = 2.0f*q0*q3 + 2.0f*q1*q2; -H_MAG[18] = 2.0f*q1*q3 - 2.0f*q0*q2; -H_MAG[19] = 1.0f; - -// common expressions for X axis Kalman gains -SK_MX[0] = 1.0f/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2.0f*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2.0f*q1*q2) - P[18][17]*(2*q0*q2 - 2.0f*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q2 - 2.0f*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2.0f*q1*q2) - P[18][18]*(2*q0*q2 - 2.0f*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2.0f*q1*q2) - P[18][0]*(2*q0*q2 - 2.0f*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[17][19]*(2*q0*q3 + 2.0f*q1*q2) - P[18][19]*(2*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2.0f*q1*q2) - P[18][1]*(2*q0*q2 - 2.0f*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2.0f*q1*q2) - P[18][2]*(2*q0*q2 - 2.0f*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2.0f*q1*q2) - P[18][3]*(2*q0*q2 - 2.0f*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2.0f*q1*q2) - P[18][16]*(2*q0*q2 - 2.0f*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); -SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; -SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; -SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3; -SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2; - -// X axis Kalman gains -Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); -Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); -Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); -Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]); -Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]); -Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]); -Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]); -Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]); -Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]); -Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]); -Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]); -Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]); -Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]); -Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]); -Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]); -Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]); -Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]); -Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]); -Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]); -Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]); -Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]); -Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]); -Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); -Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); - -// Y axis observation Jacobians -H_MAG[0] = SH_MAG[2]; -H_MAG[1] = SH_MAG[1]; -H_MAG[2] = SH_MAG[0]; -H_MAG[3] = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7]; -H_MAG[16] = 2.0f*q1*q2 - 2.0f*q0*q3; -H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; -H_MAG[18] = 2.0f*q0*q1 + 2.0f*q2*q3; -H_MAG[20] = 1; - -// Common expressions for Y axis Kalamn gains -SK_MY[0] = 1.0f/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2.0f*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2.0f*q1*q2) + P[18][16]*(2*q0*q1 + 2.0f*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2*q0*q1 + 2.0f*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2.0f*q1*q2) + P[18][18]*(2*q0*q1 + 2.0f*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2.0f*q1*q2) + P[18][3]*(2*q0*q1 + 2.0f*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[16][20]*(2*q0*q3 - 2.0f*q1*q2) + P[18][20]*(2*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2.0f*q1*q2) + P[18][0]*(2*q0*q1 + 2.0f*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2.0f*q1*q2) + P[18][1]*(2*q0*q1 + 2.0f*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2.0f*q1*q2) + P[18][2]*(2*q0*q1 + 2.0f*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2.0f*q1*q2) + P[18][17]*(2*q0*q1 + 2.0f*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); -SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; -SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; -SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2; -SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3; - -// Y axis Kalmna gains -Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); -Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); -Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); -Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); -Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); -Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); -Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); -Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); -Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); -Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); -Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); -Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); -Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); -Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); -Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); -Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); -Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); -Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); -Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); -Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); -Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); -Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); -Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); -Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); - -// Z axis observation Jacobians -H_MAG[0] = SH_MAG[1]; -H_MAG[1] = -SH_MAG[2]; -H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; -H_MAG[3] = SH_MAG[0]; -H_MAG[16] = 2.0f*q0*q2 + 2.0f*q1*q3; -H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1; -H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; -H_MAG[21] = 1; - -// Common expressions for Z axis Kalman gains -SK_MZ[0] = 1.0f/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2.0f*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2.0f*q1*q3) - P[17][16]*(2*q0*q1 - 2.0f*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2.0f*q1*q3) - P[17][17]*(2*q0*q1 - 2.0f*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2.0f*q1*q3) - P[17][2]*(2*q0*q1 - 2.0f*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[16][21]*(2*q0*q2 + 2.0f*q1*q3) - P[17][21]*(2*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2.0f*q1*q3) - P[17][0]*(2*q0*q1 - 2.0f*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2.0f*q1*q3) - P[17][1]*(2*q0*q1 - 2.0f*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2.0f*q1*q3) - P[17][3]*(2*q0*q1 - 2.0f*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2.0f*q1*q3) - P[17][18]*(2*q0*q1 - 2.0f*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); -SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; -SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; -SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3; -SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3; - -// Z axis Kalman gains -Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]); -Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]); -Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]); -Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]); -Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]); -Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]); -Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]); -Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]); -Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]); -Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]); -Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]); -Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]); -Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]); -Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]); -Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]); -Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]); -Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]); -Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]); -Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]); -Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]); -Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]); -Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]); -Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); -Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); diff --git a/EKF/matlab/generated/Inertial Nav EKF/Notes.txt b/EKF/matlab/generated/Inertial Nav EKF/Notes.txt deleted file mode 100644 index 0e0e06616f..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Notes.txt +++ /dev/null @@ -1,22 +0,0 @@ -NOTES FOR FUSION OF MEASUREMENTS USING AUTOCODE FRAGMENTS - -The auto-code for the fusion of the various measurements provides in most cases the observation Jacobian and Kalman gain matrix for that observation. -Where no Kalman gain is provided, it will need to be calculated using the usual K = P*transpose(H)/(H*P*transpose(H) + R) where: - -K = Kalman Gain matrix -H = observation partial derivative matrix (observaton Jacobian) -R = observation variance -(H*P*transpose(H) + R) is the innovation variance (always a scalar) - -When the observation is a vector, it is always assumed that the errors in the vectors are uncorelated to each other and the observations are fused -sequentially, so no matrix inversion is required. - -It is important that maximum useage of the sparsity in the H matrix be taken advantage of. If a sparse math library is available it could be used, -otherwise the matrix operations should need unrolled with inclusion of conditional statements to improve efficiency. Examples of this technique can -be found in the existing att_pos_ekf_estimator library - -NOTES FOR COVARIANCE PREDICTION - -Only expression for the upper diagonal is provided. The values will need to be copied across to the lower diagnal elements assuming symmetry -Process noise for time invariant states, eg Gyro bias, has not been included in the auto-code. The process noise variance for these states will -need to be added in a separate operation. diff --git a/EKF/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt b/EKF/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt deleted file mode 100644 index 6c9aed1462..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt +++ /dev/null @@ -1,267 +0,0 @@ -// Auto code for fusion of line of sight rate massurements from a optical flow camera aligned with the Z body axis -// Conversion from Matlab symbolic toolbox objects to c-code made using custom scripts and auto-coder from the InertialNav repo -// Observations are body modtion compensated optica flow rates about the X and Y body axis -// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated) - -float H_LOS[2][24]; // Optical flow observation Jacobians -float Kfusion[24][2]; // Optical flow Kalman gains - -// calculate X axis observation Jacobian -float t2 = 1.0f / range; -H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); -H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); -H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); -H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); -H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f); -H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3); -H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f); - -// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains -float t3 = q1*vd*2.0f; -float t4 = q0*ve*2.0f; -float t11 = q3*vn*2.0f; -float t5 = t3+t4-t11; -float t6 = q0*q3*2.0f; -float t29 = q1*q2*2.0f; -float t7 = t6-t29; -float t8 = q0*q1*2.0f; -float t9 = q2*q3*2.0f; -float t10 = t8+t9; -float t12 = P[0][0]*t2*t5; -float t13 = q0*vd*2.0f; -float t14 = q2*vn*2.0f; -float t28 = q1*ve*2.0f; -float t15 = t13+t14-t28; -float t16 = q3*vd*2.0f; -float t17 = q2*ve*2.0f; -float t18 = q1*vn*2.0f; -float t19 = t16+t17+t18; -float t20 = q3*ve*2.0f; -float t21 = q0*vn*2.0f; -float t30 = q2*vd*2.0f; -float t22 = t20+t21-t30; -float t23 = q0*q0; -float t24 = q1*q1; -float t25 = q2*q2; -float t26 = q3*q3; -float t27 = t23-t24+t25-t26; -float t31 = P[1][1]*t2*t15; -float t32 = P[6][0]*t2*t10; -float t33 = P[1][0]*t2*t15; -float t34 = P[2][0]*t2*t19; -float t35 = P[5][0]*t2*t27; -float t79 = P[4][0]*t2*t7; -float t80 = P[3][0]*t2*t22; -float t36 = t12+t32+t33+t34+t35-t79-t80; -float t37 = t2*t5*t36; -float t38 = P[6][1]*t2*t10; -float t39 = P[0][1]*t2*t5; -float t40 = P[2][1]*t2*t19; -float t41 = P[5][1]*t2*t27; -float t81 = P[4][1]*t2*t7; -float t82 = P[3][1]*t2*t22; -float t42 = t31+t38+t39+t40+t41-t81-t82; -float t43 = t2*t15*t42; -float t44 = P[6][2]*t2*t10; -float t45 = P[0][2]*t2*t5; -float t46 = P[1][2]*t2*t15; -float t47 = P[2][2]*t2*t19; -float t48 = P[5][2]*t2*t27; -float t83 = P[4][2]*t2*t7; -float t84 = P[3][2]*t2*t22; -float t49 = t44+t45+t46+t47+t48-t83-t84; -float t50 = t2*t19*t49; -float t51 = P[6][3]*t2*t10; -float t52 = P[0][3]*t2*t5; -float t53 = P[1][3]*t2*t15; -float t54 = P[2][3]*t2*t19; -float t55 = P[5][3]*t2*t27; -float t85 = P[4][3]*t2*t7; -float t86 = P[3][3]*t2*t22; -float t56 = t51+t52+t53+t54+t55-t85-t86; -float t57 = P[6][5]*t2*t10; -float t58 = P[0][5]*t2*t5; -float t59 = P[1][5]*t2*t15; -float t60 = P[2][5]*t2*t19; -float t61 = P[5][5]*t2*t27; -float t88 = P[4][5]*t2*t7; -float t89 = P[3][5]*t2*t22; -float t62 = t57+t58+t59+t60+t61-t88-t89; -float t63 = t2*t27*t62; -float t64 = P[6][4]*t2*t10; -float t65 = P[0][4]*t2*t5; -float t66 = P[1][4]*t2*t15; -float t67 = P[2][4]*t2*t19; -float t68 = P[5][4]*t2*t27; -float t90 = P[4][4]*t2*t7; -float t91 = P[3][4]*t2*t22; -float t69 = t64+t65+t66+t67+t68-t90-t91; -float t70 = P[6][6]*t2*t10; -float t71 = P[0][6]*t2*t5; -float t72 = P[1][6]*t2*t15; -float t73 = P[2][6]*t2*t19; -float t74 = P[5][6]*t2*t27; -float t93 = P[4][6]*t2*t7; -float t94 = P[3][6]*t2*t22; -float t75 = t70+t71+t72+t73+t74-t93-t94; -float t76 = t2*t10*t75; -float t87 = t2*t22*t56; -float t92 = t2*t7*t69; -float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; -float t78 = 1.0f / t77; - -// calculate Kalman gains for X-axis observation -Kfusion[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27); -Kfusion[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27); -Kfusion[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27); -Kfusion[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27); -Kfusion[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27); -Kfusion[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22); -Kfusion[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27); -Kfusion[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27); -Kfusion[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27); -Kfusion[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27); -Kfusion[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27); -Kfusion[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27); -Kfusion[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27); -Kfusion[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27); -Kfusion[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27); -Kfusion[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27); -Kfusion[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27); -Kfusion[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27); -Kfusion[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27); -Kfusion[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27); -Kfusion[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27); -Kfusion[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27); -Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); -Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); - -// calculate Y axis observation Jacobian -float t2 = 1.0f / range; -H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); -H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); -H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); -H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); -H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3); -H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f); -H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f); - -// calculate intermediate variables for the Y axis observaton innovation variance and Kalman gains -float t3 = q3*ve*2.0f; -float t4 = q0*vn*2.0f; -float t11 = q2*vd*2.0f; -float t5 = t3+t4-t11; -float t6 = q0*q3*2.0f; -float t7 = q1*q2*2.0f; -float t8 = t6+t7; -float t9 = q0*q2*2.0f; -float t28 = q1*q3*2.0f; -float t10 = t9-t28; -float t12 = P[0][0]*t2*t5; -float t13 = q3*vd*2.0f; -float t14 = q2*ve*2.0f; -float t15 = q1*vn*2.0f; -float t16 = t13+t14+t15; -float t17 = q0*vd*2.0f; -float t18 = q2*vn*2.0f; -float t29 = q1*ve*2.0f; -float t19 = t17+t18-t29; -float t20 = q1*vd*2.0f; -float t21 = q0*ve*2.0f; -float t30 = q3*vn*2.0f; -float t22 = t20+t21-t30; -float t23 = q0*q0; -float t24 = q1*q1; -float t25 = q2*q2; -float t26 = q3*q3; -float t27 = t23+t24-t25-t26; -float t31 = P[1][1]*t2*t16; -float t32 = P[5][0]*t2*t8; -float t33 = P[1][0]*t2*t16; -float t34 = P[3][0]*t2*t22; -float t35 = P[4][0]*t2*t27; -float t80 = P[6][0]*t2*t10; -float t81 = P[2][0]*t2*t19; -float t36 = t12+t32+t33+t34+t35-t80-t81; -float t37 = t2*t5*t36; -float t38 = P[5][1]*t2*t8; -float t39 = P[0][1]*t2*t5; -float t40 = P[3][1]*t2*t22; -float t41 = P[4][1]*t2*t27; -float t82 = P[6][1]*t2*t10; -float t83 = P[2][1]*t2*t19; -float t42 = t31+t38+t39+t40+t41-t82-t83; -float t43 = t2*t16*t42; -float t44 = P[5][2]*t2*t8; -float t45 = P[0][2]*t2*t5; -float t46 = P[1][2]*t2*t16; -float t47 = P[3][2]*t2*t22; -float t48 = P[4][2]*t2*t27; -float t79 = P[2][2]*t2*t19; -float t84 = P[6][2]*t2*t10; -float t49 = t44+t45+t46+t47+t48-t79-t84; -float t50 = P[5][3]*t2*t8; -float t51 = P[0][3]*t2*t5; -float t52 = P[1][3]*t2*t16; -float t53 = P[3][3]*t2*t22; -float t54 = P[4][3]*t2*t27; -float t86 = P[6][3]*t2*t10; -float t87 = P[2][3]*t2*t19; -float t55 = t50+t51+t52+t53+t54-t86-t87; -float t56 = t2*t22*t55; -float t57 = P[5][4]*t2*t8; -float t58 = P[0][4]*t2*t5; -float t59 = P[1][4]*t2*t16; -float t60 = P[3][4]*t2*t22; -float t61 = P[4][4]*t2*t27; -float t88 = P[6][4]*t2*t10; -float t89 = P[2][4]*t2*t19; -float t62 = t57+t58+t59+t60+t61-t88-t89; -float t63 = t2*t27*t62; -float t64 = P[5][5]*t2*t8; -float t65 = P[0][5]*t2*t5; -float t66 = P[1][5]*t2*t16; -float t67 = P[3][5]*t2*t22; -float t68 = P[4][5]*t2*t27; -float t90 = P[6][5]*t2*t10; -float t91 = P[2][5]*t2*t19; -float t69 = t64+t65+t66+t67+t68-t90-t91; -float t70 = t2*t8*t69; -float t71 = P[5][6]*t2*t8; -float t72 = P[0][6]*t2*t5; -float t73 = P[1][6]*t2*t16; -float t74 = P[3][6]*t2*t22; -float t75 = P[4][6]*t2*t27; -float t92 = P[6][6]*t2*t10; -float t93 = P[2][6]*t2*t19; -float t76 = t71+t72+t73+t74+t75-t92-t93; -float t85 = t2*t19*t49; -float t94 = t2*t10*t76; -float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; -float t78 = 1.0f / t77; - -// calculate Kalman gains for Y-axis observation -Kfusion[0][1] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27); -Kfusion[1][1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27); -Kfusion[2][1] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27); -Kfusion[3][1] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27); -Kfusion[4][1] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22); -Kfusion[5][1] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27); -Kfusion[6][1] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27); -Kfusion[7][1] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27); -Kfusion[8][1] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27); -Kfusion[9][1] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27); -Kfusion[10][1] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27); -Kfusion[11][1] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27); -Kfusion[12][1] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27); -Kfusion[13][1] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27); -Kfusion[14][1] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27); -Kfusion[15][1] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27); -Kfusion[16][1] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27); -Kfusion[17][1] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27); -Kfusion[18][1] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27); -Kfusion[19][1] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27); -Kfusion[20][1] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27); -Kfusion[21][1] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27); -Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); -Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); diff --git a/EKF/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt b/EKF/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt deleted file mode 100644 index 3f645fb7fe..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt +++ /dev/null @@ -1,69 +0,0 @@ -// Auto code for fusion of symthetic sideslip - -// Calculate the observation jacobian - -// intermediate variable from algebraic optimisation -float SH_BETA[13]; -SH_BETA = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + (ve - vwe)*(2.0f*q0*q3 + 2.0f*q1*q2); -SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - (vn - vwn)*(2.0f*q0*q3 - 2.0f*q1*q2); -SH_BETA[2] = vn - vwn; -SH_BETA[3] = ve - vwe; -SH_BETA[4] = 1.0f/sq(SH_BETA); -SH_BETA[5] = 1.0f/SH_BETA; -SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); -SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_BETA[8] = 2.0f*q0*SH_BETA[3] - 2.0f*q3*SH_BETA[2] + 2.0f*q1*vd; -SH_BETA[9] = 2.0f*q0*SH_BETA[2] + 2.0f*q3*SH_BETA[3] - 2.0f*q2*vd; -SH_BETA[10] = 2.0f*q2*SH_BETA[2] - 2.0f*q1*SH_BETA[3] + 2.0f*q0*vd; -SH_BETA[11] = 2.0f*q1*SH_BETA[2] + 2.0f*q2*SH_BETA[3] + 2.0f*q3*vd; -SH_BETA[12] = 2.0f*q0*q3; - -float H_BETA[24]; -H_BETA[0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; -H_BETA[1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; -H_BETA[2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; -H_BETA[3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -H_BETA[4] = - SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -H_BETA[5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); -H_BETA[6] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); -H_BETA[22] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -H_BETA[23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2) - SH_BETA[6]; - -// calculate the Kalman gain matrix - -// intermediate variables - note SK_BETA[0] is 1.0f/(innovation variance) -float SK_BETA[8]; -SK_BETA[0] = 1.0f/(R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3)))); -SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); -SK_BETA[3] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); -SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; -SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; -SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; -SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; - -float Kfusion[24]; -Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]); -Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]); -Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]); -Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]); -Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]); -Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]); -Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]); -Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]); -Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]); -Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]); -Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]); -Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]); -Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]); -Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]); -Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]); -Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]); -Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]); -Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]); -Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]); -Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][22]*SK_BETA[1] - P[19][23]*SK_BETA[2]); -Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]); -Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]); -Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); -Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); diff --git a/EKF/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt b/EKF/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt deleted file mode 100644 index fc4e7ac0ea..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt +++ /dev/null @@ -1,42 +0,0 @@ -/* -Observation jacobian for fusion of the horizontal components of magnetometer measurements. - -innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained -by rotating the measured magnetometer readings from body into earth axes. - -This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use - -Divide by zero protection hs been added -*/ - -// calculate intermediate variables for observation jacobian -float t9 = q0*q3; -float t10 = q1*q2; -float t2 = t9+t10; -float t3 = q0*q0; -float t4 = q1*q1; -float t5 = q2*q2; -float t6 = q3*q3; -float t7 = t3+t4-t5-t6; -float t8 = t7*t7; -if (t8 > 1e-6f) { - t8 = 1.0f/t8; -} else { - return; -} -float t11 = t2*t2; -float t12 = t8*t11*4.0f; -float t13 = t12+1.0f; -float t14; -if (fabsf(t13) > 1e-6f) { - t14 = 1.0f/t13; -} else { - return; -} - -// calculate observation jacobian -float H_DECL[4] = {}; -H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f; -H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f; -H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f; -H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; diff --git a/EKF/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt b/EKF/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt deleted file mode 100644 index 9b0c46ea13..0000000000 --- a/EKF/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt +++ /dev/null @@ -1,67 +0,0 @@ -/* -Code fragments for fusion of an Euler yaw measurement from a 321 sequence. -*/ - -// calculate observation jacobian when we are observing the first rotation in a 321 sequence -float t9 = q0*q3; -float t10 = q1*q2; -float t2 = t9+t10; -float t3 = q0*q0; -float t4 = q1*q1; -float t5 = q2*q2; -float t6 = q3*q3; -float t7 = t3+t4-t5-t6; -float t8 = t7*t7; -if (t8 > 1e-6f) { - t8 = 1.0f/t8; -} else { - return; -} -float t11 = t2*t2; -float t12 = t8*t11*4.0f; -float t13 = t12+1.0f; -float t14; -if (fabsf(t13) > 1e-6f) { - t14 = 1.0f/t13; -} else { - return; -} - -H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f; -H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f; -H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f; -H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; - -/* -Code fragments for fusion of an Euler yaw measurement from a 312 sequence. -*/ - -// calculate observaton jacobian when we are observing a rotation in a 312 sequence -float t9 = q0*q3; -float t10 = q1*q2; -float t2 = t9-t10; -float t3 = q0*q0; -float t4 = q1*q1; -float t5 = q2*q2; -float t6 = q3*q3; -float t7 = t3-t4+t5-t6; -float t8 = t7*t7; -if (t8 > 1e-6f) { - t8 = 1.0f/t8; -} else { - return; -} -float t11 = t2*t2; -float t12 = t8*t11*4.0f; -float t13 = t12+1.0f; -float t14; -if (fabsf(t13) > 1e-6f) { - t14 = 1.0f/t13; -} else { - return; -} - -H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f; -H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f; -H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f; -H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f; diff --git a/EKF/matlab/generated/README.md b/EKF/matlab/generated/README.md deleted file mode 100644 index d9df3f66e5..0000000000 --- a/EKF/matlab/generated/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# Generated Code - -These directories contains code generated using the derivation script files that has been 'cleaned up'. - -Inertia Nav EKF: contains code generated by GenerateNavFilterEquations.m \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Airspeed.mat b/EKF/matlab/scripts/Inertial Nav EKF/Airspeed.mat deleted file mode 100644 index fd834d9025b51a66a7c7752e9f550fe83b84b657..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8710 zcmb7|RZtv&vS6_w!5xCT1a}MW?hFuIg1a*?xDD>^1PSgAfgpoJaQEQugY3PvzwcG; zc2#$E_s6MI{n;uK+R74=l)P*llqwS1tk$*;RxFh24rU(K&TfuE|Jg|?@bR!vO1W8? zd01IeIy(tbYI!j4Oj&g%>Q&s|4EkdM^{E3v2QkH76F@IJ zk)V$|(Cz5f`&HM&W#B9bxZn5o_wuD+)%icelg1)w4g?+jfD07+;0Ia1*WGko_q{)F zB=jvGRUf~1PJuo`J>TC6p%fo2AEyP5pt+A*nt?Xf5JBP*^Y;wUvsK_d(kYe-*f4y~5@BL^sPN5=kb!HLFKIJ(%W5Cm@(L|r) znVh4b9-HJ7x)Xe}KE5i9CW8)BU@^%F@UCytWQWB^o~n+OIzMrt+V#NB_&IU?o zM-XzeVn)VQVjYvm#vV#aF>v=WfY|apRDV&KN^>P`bK%fuNR;7llR!@Gj4qrBNVjwT zqxt6<9#|Bjp^;!+L&@?=hH^EoX@QQ+Z!J?297lC^^t4@zPYT>9h_-TpV2!e84TURk z_@(18F81^jdvQa_iN3 zOqCb{qSB0AsnmFS9F*u6txW0bvW#Olr)W`$bwlpjaOLERXmYG!<@QMP!YHW~z>0`U zOYRZ}Ochwe)_FB91KxK#g9h_piKKj+B>wivwG0@y`OLl=Vy8vm2Jm}>OCxglRc&j= zr}3xntwwYt_O7s1{AoARZh9-G4}U(Fs|}v7_sQnJQ|Kf)opn zR2u+R@8vBZc;ju?rHnfWPG#Nw1_~XPFL!BB{EQu-jTY9nMO2F|xw6`{`=HD@d;tK%qHUJdd3O# z3~X)3IXMKB_iP#DFSz8YVP=dhj@*nA6SUL8f&$N{C~@J@HUMuPpyq<@nZ?qH z7w^m00$yP9pq(kV*DE)b2&=H>XEV+oElL5E%2mx$EGF*7GR-KQ$+pbe?yoRQ#uZl zoq-;hF1~6t3q1!Owc`$7xJ-X~p7M>$qw3dlmEFT)rr{qUESv#S3A~KMZDeFO z_EybXk9P}^(8{ecG~`3-iB_DoTqiMNT!s`HpQALrL^4bHIi#PNTOOf>(u%kE9v}Ud zy#gjz&;45rcvV^aQx3bYU7`eFttnH%}K1C4^BA=b+#_xT~W{ZK!dVD?&f-)sbzh| zm(^Hjgm`?<&sTxw8sSQd8wHFB1E0M86N&ia1uoxff9OaP9dOq^rCYk1mR@(qrp~0> zNc(otqG#fi{~`Kbtg&AS6RflW^ecwzlraW!I93V@^0T(<%-~r}k+Srj3Qy70z2DGT zVLtGm8pX&YuO0h}Euynhtxq7oVP`D$A!r#*lf8I|SyKfvYs3ag2V2RE{D&kg)KwaPS0d=z zdMZ*iEyS8ci7Sef)zrT&^`Rw(vc-S8k3v~-t-JQ1wxzp9Pm~Q838OU>1ZXtZr!F&T({(+YxTx8XI2L3;u~?KbKyaF0m?6+6;@6ZCwSb5|MgaggmI|P%Gx6p zBO7uvy}V})FZBpQ_M}f`^lRz(#E*E~6O*qmRmHb*N;fCW-rEqNe9qqAVxO1LQjcEK`J)(Iy@#FFoXyC+^#YA~SKG{F!h)avj-KgTIvFVj2nw zT7P@D&kR6*+wg#^#erYdpd`L7ch36@*G&g*qVFdx68!8`t^Qr}`vw2iJn1qH0OD=! zO2;y7n14^}ItKW@L|-HN%p&?orKqKOo~6j%iJwxK_>!>O4mMrh-pg|11*&o;A76Rr zC%PP4vx8V1^yLv#!p}GAd1y z;NVdAY%z;@LX<_+g=}|?+y?LsO2|t3?25`K9Z)&w99tQQxo(hb&N!dUX_Qh1;SS=O zSt~04vEDc+(tQMz`S;PD{@btGnbaDR6qnq`VdDxdVg4<1ZHbw$DN*Liy>pjwm{C#~ zD;d)=HY#N`Wp+$o2|nSSvhr1{q&sy55@5MhT(vv1_;g*}9_d(*t_q*##oCnd_B5vMTs6|{Xu8?=4@O{4?)8jy^2yFR~8gA09% zu1?>d=w+&%COOd0Ad<>$i%7M`gI&axc2vd%I0$dms3m%n5N5+L{IC+0fzjL4l>S#9 zeodbYz&cVRJ{Q`^g%RG@qTL^8PNsb3nPARz^a3Cm`*-T}uiV-!KYk4v!*jiv1ZO{X zg{)`GusP4Bd}$d*l<+)|KSeZ^FM6=E3WXXH?2ZP9&Q-X4DkOC%zPaD-YyFc2PE6Dp`oSvIO&rkGc_W`;30M+9^2D#I-GlZHj`xlevfYQ9Y9wnb`!wsmJdgj2tLF2{%N@_`XH!uk__ z)YpOwsLsY{_wj|3NfT-ma`)%ri#G#l?FfM(Dj2$;5XPBjUlf6NQ84}sUuzcm*Jm&m@gRA^ECZ-?&<7Gt6L^D0lLXTT} zS-p0dT8w>EgZAthp`=-)sF22B+=#yhJOSZVwu99_$S5PUuS!t;Mwwr)q#ZC1hD0g zGYmO+W-EQXvyTki8D@yc-=qmZBJt$DG&4g$=+n|BGI$;nYM;Og%+Z{0e?z6tQkcvz}-2KWXll~|$N zU(y(Krjb(JOZ!QiOu9F%3uj^G))Q25m2yymXTFTX8ZrRne@E7*5I-+yq+0204`ecU zPX{-IV@~qJo4dbqi)nvVA(TEVeg9%q&wVVXZk%;3MKRk0l-gd?EV?7-X@8Tnm zLrS`o7Ag5Bm}+%B1dK3pii+7COe-MCqSICRY5J?)-$t1z!RY#>LL= zXXDMfxd#rUYH@2V`5gI(#x2OP6rSlPfw7mQs}RloOc>eIo*mlC{;RU8u2v00lj$>I zp!nAZ?uVCq&@a%d+6UkJ%htzr!u68ci^+gV55^^T@2hpdxZt0;Qc-if(#SbZX{Fl0A zKoa+@vgGhv?zfHO4Tva&`GdenpP(>n#z?)ec(Nyz(EeNwbGYRKKNU?=@4fgwZlpj` zD!2}iLFJA#;c?l2^;`JK-E{i={cx{)FS0IK-j$WZ{|=naPS}dC%a(X5O(^-P{v~ZOo}SpMDfsli!wO@n z@ZP1&=2g>kQ!%Zb@}b{GH@^#2+;B%{%i}@NOv`VR;6sz)#osnVQ^mjJT-`)QGatc` zI&78iU;~Q>sf($v>MO_?F_&HO7xesXYZ)d#UzPu8@a{Zk$a_RG-MskP>U$Pnvl;(- zaVr7Q$ARGR2~p#Lsj>IetjoB`l+KycHTvBny$^1vik!(rSvwZ9-P0F6e}}bPY4-~} zYfc=-Bb{9CZIN^LNxfiYd93)EYVDv2C7v|eT|bcE|1OzRzUI7%_`@RP{oQi^7e zt_kO{Rw+?AoYM=Ddf01Tp~s&-gv^{$eINzpmyI0BB1$-)dd=KZ3H7?&S69ZHx-)qW zQo
h@DLrPF6s$*a!-$RkIMH>fpGy&yT+e#*&an23$a4eQOD-Moe^Yh^)=v{v>> z$W}(eHTcm-r(Ay@1v8wr&1K3c?BAZ%W9BUZi(j_v4Cb9+!z~M%{af<#OD?-T}K()RAP2&5@q zxh8g~L+8Xw&5$F4oi1s7%P5kiF3O(7FbA6*CijNyfGf z8H0~5;nm6^ijGk&VI3reKSCI~9Uvp6_pY_7Chm{VA=OaJb_R5KmX>UUTc^LdXu;Gu zZ4`C5pVw&`mK}Aek=Vfv?@7p7Or=Njd{t|I}`hYq+9VZx~O7F9qnw9BfMoj+ z+__m}m^@g9VYT7mNyMtrd31UImUwvXLHc2UO%w9_Knu|(TdUaxMQpxa=n^ZEl^zIc z@#N%3JJk z2`OH&5N_ufW5)BJhjo&GbIm~n!BswwKKJLtF8GyIeV(Er+NUnGKNAaU{C_#au^2^vAXBQ)LU6+NURp|r3SqGR67fRHop|w z=aze?>89nw)^6n6U*U~`mw!C0ryJd}Sefj)Pd4&(rM}jyc~Z&$l&Yz45m=?)aZV`q z|Fn8)uF?o9w%zh_G@p#o4=oz!*{tPS)W98a**)&mE4^B$$NIOHIF^p|9CGH!)my-= zSFu2RYT}>APkQm9IXn164<5)EU)n}RVnS^-+%sLRprpRVl{kU4SSn9!dvsJNh=W-l z(HXhq%UGm*JMoFSo4GMJ0`|d8Atv>_dC-Z$A5@6eKW-7z0iZeg)x#n$Q*Nvy$A#A% z9|$#-@NdtbBttw%JDCk`)g_^)m-==B`t%HrZtM7@rE$I{j zew6Ue&J!3BqnJ49a+M7R%(`$VboGZ=s%=R&{kxisWArB&z;X9@&11;26MB6YU0Rir zPZdbXM!jONRg3Kk}&K81_48j%^8S(sxemm#}b7k zxE%CU>$)j}V_xAHbW5f)<=bpHA`hIV)XN)m(TZiQKNSs^p;1>uy|64;9*wm=5;Fp` z0P*at17yso`FfR;UlZEEv7sLIa`Y&K(c%O?J6gOdI^Vf%+^nYXE+KdaZcn*9c6f^@ z#~Or7nDPsI739^(dhK!R8>0SmbhY~PbhlJlU0r4-@7WE*VE^raK^yFG%ZB{9rkx*9 z!A&4K^#STu-`%tCKXz2u7hQ_b`7s|}_AKJmomhANh_W@dMMV(?5%X93x~9um3#jVi z^;(B#OO%*#jS}$;$oNY`E5+B-oY9GD8Qzl4;HFpw`>{j>Xx{H!_uG)dwJebEKWGo5 z^7ggnE3nDx73x^pdCpr?`s#2h+B#;jwmEg*bPjFsBsbjoVR+eaU>NP|5KYKN7WYu& z-<6@<8xg}hNU9%1UK6&N+-ln;Pe9S&XNA7)tq2(B@8TtZxbiBL#;#LdxeVYa@k;dy zc>V4b$mmcTB49s5Cp}wOzHov=W2a#0KW)Ztyfiub{LZ+$mfqWuU-LHyt3);sX%E)5 z)M`zpkSywF4qIz}s3%NH!_4QJTz>b#s2~(XzHG(Tg)m;T=GWk79F7aC;!lHGV#FCsLd~|)y z92-_H;eDCD?MY1T*P)p*i+ykBBYlCu*XO;xy>hVt;OovENMyvDqHp!lWY>A>dYU5e zzThE){knb05%B)%F4Ww@i!PJ$c4t9N#{NT^FqgXk-FnBXnuHWYlBkeA`F^phLjmAg zbj$mmF-@b-mc4N|m1nsp?ZCcKMJtZp+_6>BwHMyti{ z)gV#d^<#g-EarT8v!MZEh>IgzDTnF|ug*8|O9%S7x$=<$SL$y?0ePI2USrsPks<*^NlTn4BQ2q<)5j#is zzjNdVu5!(e&uNSW0*LxxIB-U$B$qbp!l(x1=BNQ3l%yFSrrSRnr4fzSdwjeo8|Mfe z!k3gjEN-sfyfiiqZ4YsS^SEf>bJ}*JZ!NcRK?|XZXQU`0479F9Ka?_)AE1IAG?i3| zrT^))F~=Ox$m5YSjMUYOIW%orL)=CHb8#G*6}6D$3+gGnb+_okO>L6&k8N`KRRqpL z(jm2Ad$2IY$jOtffFnwy9nrWS%rmRvz-G2KLL-de0Dmpqu$ANLF}KpitvM*9p2!MP z9a7C57y2Po5=*VNkVf3Of-kDltgBTN;B$^`7t5W3=X*3Q$1{LxFQ-6bASA|1+uac! z+$}tf9`2i|4ss%GE&Ul%@#$W%-7nuB%A9W8=p2p?uKQ=vq6i)>EsMcWw)tvc)2@8- zkP0*8zuHb8NG4F=hhk7xhmr2_ClD#hSv+AwTRc`aC*5Y#@wX&Z#yFSi&=HxW=#bGI z?2u`vPvWHd`tKd;DJ}Cwe>r|*HZI{6N&ZI{H%s5?Ykb*N6-DZft4d};?pqG!y*2pH zQCtQRIZquL9QCq)laof}Fx<5?#+{>v6@rGw6M#Oarz$-&G= z)5eY0Q8{__1Mzc${*H@@78WnEC+OeO@4;JG>m-LLO1OxPe)tm3!?zfO12Ap3zE46s z$<5h#yHX0qc%E)ZR>%dn`Q>$}+`KYXr=@9zzo<>)W`~#QbhhTdDPCTMA`AP($Y-fC zpD19ED4bUHB&Y5t#o~+Ed^%&+pAHzExmze9GjHHrQId zO)q_M>0u=I!lSiDwOGpgA-I`rJnZO3!#l$}9!m=IE3R3ML$O?YSc%r4@a5&)^sfyn zMyc^YdbVL?!`4{)$X9S8YbFETnmX%Dn}C-{r5T!(%OiahHNJ*0&elPmO5DECu82N+ ztpCnYmMb`!RWm9b1r1Y|M*cOBt19u@lB-8w$vWQJCpIrGgkee&zr#@v5i(2P2IW9G z4fb!Ut&?z=#F0p53E`wp$fd*j{(_=>FUM5TYNi^^q}Cn#^}qs_v0>S?{pg^h;pI(8b+pZv`sp$1DU_8A->LZYD;g3WB5M` z#@-J{dd0;`$jd(tIyS-Te6f@&{*(3#rMfb2${`RX@_Pk=(-hlG6Z@BH|NBoy+3)Z} zCc50LS?82{hQYvJOsfDa3YjzYhCvb3g%(42=P)abwJ+BMA)iG6*nbzm)PV+uXttXX z`ISv>l)oA!es^JH?KeDKkS_q&z1N&fP}3{fQ2r^bKGB+`&Cj6UGJR=K>yS8Zs$W*+ z{|kLC&>A#~^67X1-yo|Cqz6e_wxObU$`}7tm9!9qYduojvo|rM#jlaZo)pwrzKNfV zlUsP|n1-mk`{!T!gaupRj{ZiKxBQ0>Xo01acDmdZWH11`g)b+C)|`A2##Y0{45u2qyoUdKSU?k@ zn!XdI%-vU%dBVzkI+w)7{r>Fm*NvdWL(P+t>jO8J6(~ zX<>b1DSv_kX%AELxYuS#4LT40czS9xem(5F+if#B49xl1&#HTJ2FaNOzRKlYzF$6r xKA1luw;t{kmMwx%1vIPYp^~EgXjNe*n3w;tT)) diff --git a/EKF/matlab/scripts/Inertial Nav EKF/C_code24.txt b/EKF/matlab/scripts/Inertial Nav EKF/C_code24.txt deleted file mode 100644 index 7c6f9d1b5f..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/C_code24.txt +++ /dev/null @@ -1,686 +0,0 @@ -float SF[21][1]; -SF[0] = dvz - dvz_b; -SF[1] = dvy - dvy_b; -SF[2] = dvx - dvx_b; -SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0]; -SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2]; -SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1]; -SF[6] = day/2 - day_b/2; -SF[7] = daz/2 - daz_b/2; -SF[8] = dax/2 - dax_b/2; -SF[9] = dax_b/2 - dax/2; -SF[10] = daz_b/2 - daz/2; -SF[11] = day_b/2 - day/2; -SF[12] = 2*q1*SF[1]; -SF[13] = 2*q0*SF[0]; -SF[14] = q1/2; -SF[15] = q2/2; -SF[16] = q3/2; -SF[17] = sq(q3); -SF[18] = sq(q2); -SF[19] = sq(q1); -SF[20] = sq(q0); -float SG[8][1]; -SG[0] = q0/2; -SG[1] = sq(q3); -SG[2] = sq(q2); -SG[3] = sq(q1); -SG[4] = sq(q0); -SG[5] = 2*q2*q3; -SG[6] = 2*q1*q3; -SG[7] = 2*q1*q2; -float SQ[11][1]; -SQ[0] = dvzVar*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); -SQ[1] = dvzVar*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); -SQ[2] = dvzVar*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyVar*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); -SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4; -SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4; -SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4; -SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4; -SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2; -SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4; -SQ[9] = sq(SG[0]); -SQ[10] = sq(q1); -float SPP[11][1]; -SPP[0] = SF[12] + SF[13] - 2*q2*SF[2]; -SPP[1] = SF[17] - SF[18] - SF[19] + SF[20]; -SPP[2] = SF[17] - SF[18] + SF[19] - SF[20]; -SPP[3] = SF[17] + SF[18] - SF[19] - SF[20]; -SPP[4] = 2*q0*q2 - 2*q1*q3; -SPP[5] = 2*q0*q1 - 2*q2*q3; -SPP[6] = 2*q0*q3 - 2*q1*q2; -SPP[7] = 2*q0*q1 + 2*q2*q3; -SPP[8] = 2*q0*q3 + 2*q1*q2; -SPP[9] = 2*q0*q2 + 2*q1*q3; -SPP[10] = SF[16]; -float nextP[24][24]; -nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4; -nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))/2; -nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)/2 + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))/4 + (dazVar*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))/2; -nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))/2; -nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))/2; -nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])/4 - (P[11][2]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))/2; -nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))/2; -nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2 + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))/2; -nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2 + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))/2; -nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])/4 + dazVar*SQ[9] - (P[12][3]*q0)/2 + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))/2; -nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); -nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); -nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); -nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); -nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2*q0*q3) + dvzVar*sq(SG[6] + 2*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]); -nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); -nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); -nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); -nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); -nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); -nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2*q0*q3) + dvzVar*sq(SG[5] - 2*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]); -nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); -nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2 + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); -nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2 + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); -nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2 + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); -nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); -nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]); -nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2*q0*q2) + dvyVar*sq(SG[5] + 2*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]); -nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]); -nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2); -nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2); -nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2); -nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]); -nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]); -nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]); -nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); -nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]); -nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2); -nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2); -nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2); -nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]); -nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]); -nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]); -nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); -nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); -nextP[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]); -nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2); -nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2); -nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2); -nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]); -nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]); -nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]); -nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); -nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); -nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); -nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]; -nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2; -nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2; -nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2; -nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9]; -nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5]; -nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1]; -nextP[7][10] = P[7][10] + P[4][10]*dt; -nextP[8][10] = P[8][10] + P[5][10]*dt; -nextP[9][10] = P[9][10] + P[6][10]*dt; -nextP[10][10] = P[10][10]; -nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]; -nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2; -nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2; -nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2; -nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9]; -nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5]; -nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1]; -nextP[7][11] = P[7][11] + P[4][11]*dt; -nextP[8][11] = P[8][11] + P[5][11]*dt; -nextP[9][11] = P[9][11] + P[6][11]*dt; -nextP[10][11] = P[10][11]; -nextP[11][11] = P[11][11]; -nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]; -nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2; -nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2; -nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2; -nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9]; -nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5]; -nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1]; -nextP[7][12] = P[7][12] + P[4][12]*dt; -nextP[8][12] = P[8][12] + P[5][12]*dt; -nextP[9][12] = P[9][12] + P[6][12]*dt; -nextP[10][12] = P[10][12]; -nextP[11][12] = P[11][12]; -nextP[12][12] = P[12][12]; -nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]; -nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2; -nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2; -nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2; -nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]; -nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]; -nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]; -nextP[7][13] = P[7][13] + P[4][13]*dt; -nextP[8][13] = P[8][13] + P[5][13]*dt; -nextP[9][13] = P[9][13] + P[6][13]*dt; -nextP[10][13] = P[10][13]; -nextP[11][13] = P[11][13]; -nextP[12][13] = P[12][13]; -nextP[13][13] = P[13][13]; -nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]; -nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2; -nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2; -nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2; -nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]; -nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]; -nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]; -nextP[7][14] = P[7][14] + P[4][14]*dt; -nextP[8][14] = P[8][14] + P[5][14]*dt; -nextP[9][14] = P[9][14] + P[6][14]*dt; -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]; -nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]; -nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2; -nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2; -nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2; -nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]; -nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]; -nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]; -nextP[7][15] = P[7][15] + P[4][15]*dt; -nextP[8][15] = P[8][15] + P[5][15]*dt; -nextP[9][15] = P[9][15] + P[6][15]*dt; -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]; -nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10]; -nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)/2; -nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)/2; -nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)/2; -nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9]; -nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5]; -nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1]; -nextP[7][16] = P[7][16] + P[4][16]*dt; -nextP[8][16] = P[8][16] + P[5][16]*dt; -nextP[9][16] = P[9][16] + P[6][16]*dt; -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]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10]; -nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)/2; -nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)/2; -nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)/2; -nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9]; -nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5]; -nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1]; -nextP[7][17] = P[7][17] + P[4][17]*dt; -nextP[8][17] = P[8][17] + P[5][17]*dt; -nextP[9][17] = P[9][17] + P[6][17]*dt; -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]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10]; -nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)/2; -nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)/2; -nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)/2; -nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9]; -nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5]; -nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1]; -nextP[7][18] = P[7][18] + P[4][18]*dt; -nextP[8][18] = P[8][18] + P[5][18]*dt; -nextP[9][18] = P[9][18] + P[6][18]*dt; -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]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10]; -nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)/2; -nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)/2; -nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)/2; -nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9]; -nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5]; -nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1]; -nextP[7][19] = P[7][19] + P[4][19]*dt; -nextP[8][19] = P[8][19] + P[5][19]*dt; -nextP[9][19] = P[9][19] + P[6][19]*dt; -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]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10]; -nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)/2; -nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)/2; -nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)/2; -nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9]; -nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5]; -nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1]; -nextP[7][20] = P[7][20] + P[4][20]*dt; -nextP[8][20] = P[8][20] + P[5][20]*dt; -nextP[9][20] = P[9][20] + P[6][20]*dt; -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]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10]; -nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)/2; -nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)/2; -nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)/2; -nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9]; -nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5]; -nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1]; -nextP[7][21] = P[7][21] + P[4][21]*dt; -nextP[8][21] = P[8][21] + P[5][21]*dt; -nextP[9][21] = P[9][21] + P[6][21]*dt; -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]; -nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10]; -nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)/2; -nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)/2; -nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)/2; -nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9]; -nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5]; -nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1]; -nextP[7][22] = P[7][22] + P[4][22]*dt; -nextP[8][22] = P[8][22] + P[5][22]*dt; -nextP[9][22] = P[9][22] + P[6][22]*dt; -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]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10]; -nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)/2; -nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)/2; -nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)/2; -nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9]; -nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5]; -nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1]; -nextP[7][23] = P[7][23] + P[4][23]*dt; -nextP[8][23] = P[8][23] + P[5][23]*dt; -nextP[9][23] = P[9][23] + P[6][23]*dt; -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]; -float SH_TAS[3][1]; -SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); -SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; -SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; -float H_TAS[1][24]; -H_TAS[0][4] = SH_TAS[2]; -H_TAS[0][5] = SH_TAS[1]; -H_TAS[0][6] = vd*SH_TAS[0]; -H_TAS[0][22] = -SH_TAS[2]; -H_TAS[0][23] = -SH_TAS[1]; -float SK_TAS[2][1]; -SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); -SK_TAS[1] = SH_TAS[1]; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); -Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); -Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); -Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]); -Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]); -Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]); -Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]); -Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]); -Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); -Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); -Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); -Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); -Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); -Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]); -Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]); -Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]); -Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]); -Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]); -Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]); -Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]); -Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]); -Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]); -Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); -Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); -float SH_BETA[13][1]; -SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); -SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2); -SH_BETA[2] = vn - vwn; -SH_BETA[3] = ve - vwe; -SH_BETA[4] = 1/sq(SH_BETA[0]); -SH_BETA[5] = 1/SH_BETA[0]; -SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); -SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_BETA[8] = 2*q0*SH_BETA[3] - 2*q3*SH_BETA[2] + 2*q1*vd; -SH_BETA[9] = 2*q0*SH_BETA[2] + 2*q3*SH_BETA[3] - 2*q2*vd; -SH_BETA[10] = 2*q2*SH_BETA[2] - 2*q1*SH_BETA[3] + 2*q0*vd; -SH_BETA[11] = 2*q1*SH_BETA[2] + 2*q2*SH_BETA[3] + 2*q3*vd; -SH_BETA[12] = 2*q0*q3; -float H_BETA[1][24]; -H_BETA[0][0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; -H_BETA[0][1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; -H_BETA[0][2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; -H_BETA[0][3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -H_BETA[0][4] = - SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -H_BETA[0][5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2); -H_BETA[0][6] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3); -H_BETA[0][22] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -H_BETA[0][23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6]; -float SK_BETA[8][1]; -SK_BETA[0] = 1/(R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3)))); -SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2); -SK_BETA[3] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3); -SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; -SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; -SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; -SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]); -Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]); -Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]); -Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]); -Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]); -Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]); -Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]); -Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]); -Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]); -Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]); -Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]); -Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]); -Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]); -Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]); -Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]); -Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]); -Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]); -Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]); -Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]); -Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][22]*SK_BETA[1] - P[19][23]*SK_BETA[2]); -Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]); -Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]); -Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); -Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); -float SH_MAG[9][1]; -SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; -SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; -SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; -SH_MAG[3] = sq(q3); -SH_MAG[4] = sq(q2); -SH_MAG[5] = sq(q1); -SH_MAG[6] = sq(q0); -SH_MAG[7] = 2*magN*q0; -SH_MAG[8] = 2*magE*q3; -float H_MAG[1][24]; -H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -H_MAG[1] = SH_MAG[0]; -H_MAG[2] = -SH_MAG[1]; -H_MAG[3] = SH_MAG[2]; -H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; -H_MAG[17] = 2*q0*q3 + 2*q1*q2; -H_MAG[18] = 2*q1*q3 - 2*q0*q2; -H_MAG[19] = 1; -float SK_MX[5][1]; -SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); -SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; -SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -SK_MX[3] = 2*q0*q2 - 2*q1*q3; -SK_MX[4] = 2*q0*q3 + 2*q1*q2; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); -Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); -Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); -Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]); -Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]); -Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]); -Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]); -Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]); -Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]); -Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]); -Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]); -Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]); -Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]); -Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]); -Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]); -Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]); -Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]); -Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]); -Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]); -Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]); -Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]); -Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]); -Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); -Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); -float H_MAG[1][24]; -H_MAG[0] = SH_MAG[2]; -H_MAG[1] = SH_MAG[1]; -H_MAG[2] = SH_MAG[0]; -H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; -H_MAG[16] = 2*q1*q2 - 2*q0*q3; -H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; -H_MAG[18] = 2*q0*q1 + 2*q2*q3; -H_MAG[20] = 1; -float SK_MY[5][1]; -SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); -SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; -SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -SK_MY[3] = 2*q0*q3 - 2*q1*q2; -SK_MY[4] = 2*q0*q1 + 2*q2*q3; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); -Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); -Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); -Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); -Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); -Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); -Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); -Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); -Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); -Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); -Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); -Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); -Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); -Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); -Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); -Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); -Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); -Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); -Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); -Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); -Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); -Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); -Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); -Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); -float H_MAG[1][24]; -H_MAG[0] = SH_MAG[1]; -H_MAG[1] = -SH_MAG[2]; -H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -H_MAG[3] = SH_MAG[0]; -H_MAG[16] = 2*q0*q2 + 2*q1*q3; -H_MAG[17] = 2*q2*q3 - 2*q0*q1; -H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; -H_MAG[21] = 1; -float SK_MZ[5][1]; -SK_MZ[0] = 1/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); -SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; -SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -SK_MZ[3] = 2*q0*q1 - 2*q2*q3; -SK_MZ[4] = 2*q0*q2 + 2*q1*q3; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]); -Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]); -Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]); -Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]); -Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]); -Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]); -Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]); -Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]); -Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]); -Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]); -Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]); -Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]); -Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]); -Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]); -Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]); -Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]); -Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]); -Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]); -Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]); -Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]); -Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]); -Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]); -Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); -Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); -float SH_ACCX[4][1]; -SH_ACCX[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_ACCX[1] = vn - vwn; -SH_ACCX[2] = ve - vwe; -SH_ACCX[3] = 2*q0*q3 + 2*q1*q2; -float H_ACCX[1][24]; -H_ACCX[0][0] = -Kaccx*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd); -H_ACCX[0][1] = -Kaccx*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd); -H_ACCX[0][2] = Kaccx*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd); -H_ACCX[0][3] = -Kaccx*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd); -H_ACCX[0][4] = -Kaccx*SH_ACCX[0]; -H_ACCX[0][5] = -Kaccx*SH_ACCX[3]; -H_ACCX[0][6] = Kaccx*(2*q0*q2 - 2*q1*q3); -H_ACCX[0][22] = Kaccx*SH_ACCX[0]; -H_ACCX[0][23] = Kaccx*SH_ACCX[3]; -float SK_ACCX[7][1]; -SK_ACCX[0] = 1/(R_ACC + Kaccx*SH_ACCX[0]*(Kaccx*P[4][4]*SH_ACCX[0] + Kaccx*P[5][4]*SH_ACCX[3] - Kaccx*P[22][4]*SH_ACCX[0] - Kaccx*P[23][4]*SH_ACCX[3] - Kaccx*P[6][4]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][4]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][4]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][4]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][4]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*SH_ACCX[3]*(Kaccx*P[4][5]*SH_ACCX[0] + Kaccx*P[5][5]*SH_ACCX[3] - Kaccx*P[22][5]*SH_ACCX[0] - Kaccx*P[23][5]*SH_ACCX[3] - Kaccx*P[6][5]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][5]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][5]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][5]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][5]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*SH_ACCX[0]*(Kaccx*P[4][22]*SH_ACCX[0] + Kaccx*P[5][22]*SH_ACCX[3] - Kaccx*P[22][22]*SH_ACCX[0] - Kaccx*P[23][22]*SH_ACCX[3] - Kaccx*P[6][22]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][22]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][22]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][22]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][22]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*SH_ACCX[3]*(Kaccx*P[4][23]*SH_ACCX[0] + Kaccx*P[5][23]*SH_ACCX[3] - Kaccx*P[22][23]*SH_ACCX[0] - Kaccx*P[23][23]*SH_ACCX[3] - Kaccx*P[6][23]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][23]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][23]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][23]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][23]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*P[4][6]*SH_ACCX[0] + Kaccx*P[5][6]*SH_ACCX[3] - Kaccx*P[22][6]*SH_ACCX[0] - Kaccx*P[23][6]*SH_ACCX[3] - Kaccx*P[6][6]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][6]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][6]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][6]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][6]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd)*(Kaccx*P[4][0]*SH_ACCX[0] + Kaccx*P[5][0]*SH_ACCX[3] - Kaccx*P[22][0]*SH_ACCX[0] - Kaccx*P[23][0]*SH_ACCX[3] - Kaccx*P[6][0]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][0]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][0]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][0]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][0]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd)*(Kaccx*P[4][1]*SH_ACCX[0] + Kaccx*P[5][1]*SH_ACCX[3] - Kaccx*P[22][1]*SH_ACCX[0] - Kaccx*P[23][1]*SH_ACCX[3] - Kaccx*P[6][1]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][1]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][1]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][1]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][1]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd)*(Kaccx*P[4][2]*SH_ACCX[0] + Kaccx*P[5][2]*SH_ACCX[3] - Kaccx*P[22][2]*SH_ACCX[0] - Kaccx*P[23][2]*SH_ACCX[3] - Kaccx*P[6][2]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][2]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][2]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][2]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][2]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)*(Kaccx*P[4][3]*SH_ACCX[0] + Kaccx*P[5][3]*SH_ACCX[3] - Kaccx*P[22][3]*SH_ACCX[0] - Kaccx*P[23][3]*SH_ACCX[3] - Kaccx*P[6][3]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][3]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][3]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][3]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][3]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd))); -SK_ACCX[1] = 2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd; -SK_ACCX[2] = 2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd; -SK_ACCX[3] = 2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd; -SK_ACCX[4] = 2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd; -SK_ACCX[5] = 2*q0*q2 - 2*q1*q3; -SK_ACCX[6] = SH_ACCX[3]; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = -SK_ACCX[0]*(Kaccx*P[0][4]*SH_ACCX[0] - Kaccx*P[0][22]*SH_ACCX[0] + Kaccx*P[0][0]*SK_ACCX[3] - Kaccx*P[0][2]*SK_ACCX[2] + Kaccx*P[0][3]*SK_ACCX[1] + Kaccx*P[0][1]*SK_ACCX[4] + Kaccx*P[0][5]*SK_ACCX[6] - Kaccx*P[0][6]*SK_ACCX[5] - Kaccx*P[0][23]*SK_ACCX[6]); -Kfusion[1] = -SK_ACCX[0]*(Kaccx*P[1][4]*SH_ACCX[0] - Kaccx*P[1][22]*SH_ACCX[0] + Kaccx*P[1][0]*SK_ACCX[3] - Kaccx*P[1][2]*SK_ACCX[2] + Kaccx*P[1][3]*SK_ACCX[1] + Kaccx*P[1][1]*SK_ACCX[4] + Kaccx*P[1][5]*SK_ACCX[6] - Kaccx*P[1][6]*SK_ACCX[5] - Kaccx*P[1][23]*SK_ACCX[6]); -Kfusion[2] = -SK_ACCX[0]*(Kaccx*P[2][4]*SH_ACCX[0] - Kaccx*P[2][22]*SH_ACCX[0] + Kaccx*P[2][0]*SK_ACCX[3] - Kaccx*P[2][2]*SK_ACCX[2] + Kaccx*P[2][3]*SK_ACCX[1] + Kaccx*P[2][1]*SK_ACCX[4] + Kaccx*P[2][5]*SK_ACCX[6] - Kaccx*P[2][6]*SK_ACCX[5] - Kaccx*P[2][23]*SK_ACCX[6]); -Kfusion[3] = -SK_ACCX[0]*(Kaccx*P[3][4]*SH_ACCX[0] - Kaccx*P[3][22]*SH_ACCX[0] + Kaccx*P[3][0]*SK_ACCX[3] - Kaccx*P[3][2]*SK_ACCX[2] + Kaccx*P[3][3]*SK_ACCX[1] + Kaccx*P[3][1]*SK_ACCX[4] + Kaccx*P[3][5]*SK_ACCX[6] - Kaccx*P[3][6]*SK_ACCX[5] - Kaccx*P[3][23]*SK_ACCX[6]); -Kfusion[4] = -SK_ACCX[0]*(Kaccx*P[4][4]*SH_ACCX[0] - Kaccx*P[4][22]*SH_ACCX[0] + Kaccx*P[4][0]*SK_ACCX[3] - Kaccx*P[4][2]*SK_ACCX[2] + Kaccx*P[4][3]*SK_ACCX[1] + Kaccx*P[4][1]*SK_ACCX[4] + Kaccx*P[4][5]*SK_ACCX[6] - Kaccx*P[4][6]*SK_ACCX[5] - Kaccx*P[4][23]*SK_ACCX[6]); -Kfusion[5] = -SK_ACCX[0]*(Kaccx*P[5][4]*SH_ACCX[0] - Kaccx*P[5][22]*SH_ACCX[0] + Kaccx*P[5][0]*SK_ACCX[3] - Kaccx*P[5][2]*SK_ACCX[2] + Kaccx*P[5][3]*SK_ACCX[1] + Kaccx*P[5][1]*SK_ACCX[4] + Kaccx*P[5][5]*SK_ACCX[6] - Kaccx*P[5][6]*SK_ACCX[5] - Kaccx*P[5][23]*SK_ACCX[6]); -Kfusion[6] = -SK_ACCX[0]*(Kaccx*P[6][4]*SH_ACCX[0] - Kaccx*P[6][22]*SH_ACCX[0] + Kaccx*P[6][0]*SK_ACCX[3] - Kaccx*P[6][2]*SK_ACCX[2] + Kaccx*P[6][3]*SK_ACCX[1] + Kaccx*P[6][1]*SK_ACCX[4] + Kaccx*P[6][5]*SK_ACCX[6] - Kaccx*P[6][6]*SK_ACCX[5] - Kaccx*P[6][23]*SK_ACCX[6]); -Kfusion[7] = -SK_ACCX[0]*(Kaccx*P[7][4]*SH_ACCX[0] - Kaccx*P[7][22]*SH_ACCX[0] + Kaccx*P[7][0]*SK_ACCX[3] - Kaccx*P[7][2]*SK_ACCX[2] + Kaccx*P[7][3]*SK_ACCX[1] + Kaccx*P[7][1]*SK_ACCX[4] + Kaccx*P[7][5]*SK_ACCX[6] - Kaccx*P[7][6]*SK_ACCX[5] - Kaccx*P[7][23]*SK_ACCX[6]); -Kfusion[8] = -SK_ACCX[0]*(Kaccx*P[8][4]*SH_ACCX[0] - Kaccx*P[8][22]*SH_ACCX[0] + Kaccx*P[8][0]*SK_ACCX[3] - Kaccx*P[8][2]*SK_ACCX[2] + Kaccx*P[8][3]*SK_ACCX[1] + Kaccx*P[8][1]*SK_ACCX[4] + Kaccx*P[8][5]*SK_ACCX[6] - Kaccx*P[8][6]*SK_ACCX[5] - Kaccx*P[8][23]*SK_ACCX[6]); -Kfusion[9] = -SK_ACCX[0]*(Kaccx*P[9][4]*SH_ACCX[0] - Kaccx*P[9][22]*SH_ACCX[0] + Kaccx*P[9][0]*SK_ACCX[3] - Kaccx*P[9][2]*SK_ACCX[2] + Kaccx*P[9][3]*SK_ACCX[1] + Kaccx*P[9][1]*SK_ACCX[4] + Kaccx*P[9][5]*SK_ACCX[6] - Kaccx*P[9][6]*SK_ACCX[5] - Kaccx*P[9][23]*SK_ACCX[6]); -Kfusion[10] = -SK_ACCX[0]*(Kaccx*P[10][4]*SH_ACCX[0] - Kaccx*P[10][22]*SH_ACCX[0] + Kaccx*P[10][0]*SK_ACCX[3] - Kaccx*P[10][2]*SK_ACCX[2] + Kaccx*P[10][3]*SK_ACCX[1] + Kaccx*P[10][1]*SK_ACCX[4] + Kaccx*P[10][5]*SK_ACCX[6] - Kaccx*P[10][6]*SK_ACCX[5] - Kaccx*P[10][23]*SK_ACCX[6]); -Kfusion[11] = -SK_ACCX[0]*(Kaccx*P[11][4]*SH_ACCX[0] - Kaccx*P[11][22]*SH_ACCX[0] + Kaccx*P[11][0]*SK_ACCX[3] - Kaccx*P[11][2]*SK_ACCX[2] + Kaccx*P[11][3]*SK_ACCX[1] + Kaccx*P[11][1]*SK_ACCX[4] + Kaccx*P[11][5]*SK_ACCX[6] - Kaccx*P[11][6]*SK_ACCX[5] - Kaccx*P[11][23]*SK_ACCX[6]); -Kfusion[12] = -SK_ACCX[0]*(Kaccx*P[12][4]*SH_ACCX[0] - Kaccx*P[12][22]*SH_ACCX[0] + Kaccx*P[12][0]*SK_ACCX[3] - Kaccx*P[12][2]*SK_ACCX[2] + Kaccx*P[12][3]*SK_ACCX[1] + Kaccx*P[12][1]*SK_ACCX[4] + Kaccx*P[12][5]*SK_ACCX[6] - Kaccx*P[12][6]*SK_ACCX[5] - Kaccx*P[12][23]*SK_ACCX[6]); -Kfusion[13] = -SK_ACCX[0]*(Kaccx*P[13][4]*SH_ACCX[0] - Kaccx*P[13][22]*SH_ACCX[0] + Kaccx*P[13][0]*SK_ACCX[3] - Kaccx*P[13][2]*SK_ACCX[2] + Kaccx*P[13][3]*SK_ACCX[1] + Kaccx*P[13][1]*SK_ACCX[4] + Kaccx*P[13][5]*SK_ACCX[6] - Kaccx*P[13][6]*SK_ACCX[5] - Kaccx*P[13][23]*SK_ACCX[6]); -Kfusion[14] = -SK_ACCX[0]*(Kaccx*P[14][4]*SH_ACCX[0] - Kaccx*P[14][22]*SH_ACCX[0] + Kaccx*P[14][0]*SK_ACCX[3] - Kaccx*P[14][2]*SK_ACCX[2] + Kaccx*P[14][3]*SK_ACCX[1] + Kaccx*P[14][1]*SK_ACCX[4] + Kaccx*P[14][5]*SK_ACCX[6] - Kaccx*P[14][6]*SK_ACCX[5] - Kaccx*P[14][23]*SK_ACCX[6]); -Kfusion[15] = -SK_ACCX[0]*(Kaccx*P[15][4]*SH_ACCX[0] - Kaccx*P[15][22]*SH_ACCX[0] + Kaccx*P[15][0]*SK_ACCX[3] - Kaccx*P[15][2]*SK_ACCX[2] + Kaccx*P[15][3]*SK_ACCX[1] + Kaccx*P[15][1]*SK_ACCX[4] + Kaccx*P[15][5]*SK_ACCX[6] - Kaccx*P[15][6]*SK_ACCX[5] - Kaccx*P[15][23]*SK_ACCX[6]); -Kfusion[16] = -SK_ACCX[0]*(Kaccx*P[16][4]*SH_ACCX[0] - Kaccx*P[16][22]*SH_ACCX[0] + Kaccx*P[16][0]*SK_ACCX[3] - Kaccx*P[16][2]*SK_ACCX[2] + Kaccx*P[16][3]*SK_ACCX[1] + Kaccx*P[16][1]*SK_ACCX[4] + Kaccx*P[16][5]*SK_ACCX[6] - Kaccx*P[16][6]*SK_ACCX[5] - Kaccx*P[16][23]*SK_ACCX[6]); -Kfusion[17] = -SK_ACCX[0]*(Kaccx*P[17][4]*SH_ACCX[0] - Kaccx*P[17][22]*SH_ACCX[0] + Kaccx*P[17][0]*SK_ACCX[3] - Kaccx*P[17][2]*SK_ACCX[2] + Kaccx*P[17][3]*SK_ACCX[1] + Kaccx*P[17][1]*SK_ACCX[4] + Kaccx*P[17][5]*SK_ACCX[6] - Kaccx*P[17][6]*SK_ACCX[5] - Kaccx*P[17][23]*SK_ACCX[6]); -Kfusion[18] = -SK_ACCX[0]*(Kaccx*P[18][4]*SH_ACCX[0] - Kaccx*P[18][22]*SH_ACCX[0] + Kaccx*P[18][0]*SK_ACCX[3] - Kaccx*P[18][2]*SK_ACCX[2] + Kaccx*P[18][3]*SK_ACCX[1] + Kaccx*P[18][1]*SK_ACCX[4] + Kaccx*P[18][5]*SK_ACCX[6] - Kaccx*P[18][6]*SK_ACCX[5] - Kaccx*P[18][23]*SK_ACCX[6]); -Kfusion[19] = -SK_ACCX[0]*(Kaccx*P[19][4]*SH_ACCX[0] - Kaccx*P[19][22]*SH_ACCX[0] + Kaccx*P[19][0]*SK_ACCX[3] - Kaccx*P[19][2]*SK_ACCX[2] + Kaccx*P[19][3]*SK_ACCX[1] + Kaccx*P[19][1]*SK_ACCX[4] + Kaccx*P[19][5]*SK_ACCX[6] - Kaccx*P[19][6]*SK_ACCX[5] - Kaccx*P[19][23]*SK_ACCX[6]); -Kfusion[20] = -SK_ACCX[0]*(Kaccx*P[20][4]*SH_ACCX[0] - Kaccx*P[20][22]*SH_ACCX[0] + Kaccx*P[20][0]*SK_ACCX[3] - Kaccx*P[20][2]*SK_ACCX[2] + Kaccx*P[20][3]*SK_ACCX[1] + Kaccx*P[20][1]*SK_ACCX[4] + Kaccx*P[20][5]*SK_ACCX[6] - Kaccx*P[20][6]*SK_ACCX[5] - Kaccx*P[20][23]*SK_ACCX[6]); -Kfusion[21] = -SK_ACCX[0]*(Kaccx*P[21][4]*SH_ACCX[0] - Kaccx*P[21][22]*SH_ACCX[0] + Kaccx*P[21][0]*SK_ACCX[3] - Kaccx*P[21][2]*SK_ACCX[2] + Kaccx*P[21][3]*SK_ACCX[1] + Kaccx*P[21][1]*SK_ACCX[4] + Kaccx*P[21][5]*SK_ACCX[6] - Kaccx*P[21][6]*SK_ACCX[5] - Kaccx*P[21][23]*SK_ACCX[6]); -Kfusion[22] = -SK_ACCX[0]*(Kaccx*P[22][4]*SH_ACCX[0] - Kaccx*P[22][22]*SH_ACCX[0] + Kaccx*P[22][0]*SK_ACCX[3] - Kaccx*P[22][2]*SK_ACCX[2] + Kaccx*P[22][3]*SK_ACCX[1] + Kaccx*P[22][1]*SK_ACCX[4] + Kaccx*P[22][5]*SK_ACCX[6] - Kaccx*P[22][6]*SK_ACCX[5] - Kaccx*P[22][23]*SK_ACCX[6]); -Kfusion[23] = -SK_ACCX[0]*(Kaccx*P[23][4]*SH_ACCX[0] - Kaccx*P[23][22]*SH_ACCX[0] + Kaccx*P[23][0]*SK_ACCX[3] - Kaccx*P[23][2]*SK_ACCX[2] + Kaccx*P[23][3]*SK_ACCX[1] + Kaccx*P[23][1]*SK_ACCX[4] + Kaccx*P[23][5]*SK_ACCX[6] - Kaccx*P[23][6]*SK_ACCX[5] - Kaccx*P[23][23]*SK_ACCX[6]); -float SH_ACCY[3][1]; -SH_ACCY[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); -SH_ACCY[1] = vn - vwn; -SH_ACCY[2] = ve - vwe; -float H_ACCY[1][24]; -H_ACCY[0][0] = -Kaccy*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd); -H_ACCY[0][1] = -Kaccy*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd); -H_ACCY[0][2] = -Kaccy*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd); -H_ACCY[0][3] = Kaccy*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd); -H_ACCY[0][4] = Kaccy*(2*q0*q3 - 2*q1*q2); -H_ACCY[0][5] = -Kaccy*SH_ACCY[0]; -H_ACCY[0][6] = -Kaccy*(2*q0*q1 + 2*q2*q3); -H_ACCY[0][22] = -2*Kaccy*(q0*q3 - q1*q2); -H_ACCY[0][23] = Kaccy*SH_ACCY[0]; -float SK_ACCY[9][1]; -SK_ACCY[0] = 1/(R_ACC + Kaccy*SH_ACCY[0]*(Kaccy*P[5][5]*SH_ACCY[0] - Kaccy*P[23][5]*SH_ACCY[0] - Kaccy*P[4][5]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][5]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][5]*(q0*q3 - q1*q2) + Kaccy*P[0][5]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][5]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][5]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][5]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*SH_ACCY[0]*(Kaccy*P[5][23]*SH_ACCY[0] - Kaccy*P[23][23]*SH_ACCY[0] - Kaccy*P[4][23]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][23]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][23]*(q0*q3 - q1*q2) + Kaccy*P[0][23]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][23]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][23]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][23]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*P[5][4]*SH_ACCY[0] - Kaccy*P[23][4]*SH_ACCY[0] - Kaccy*P[4][4]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][4]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][4]*(q0*q3 - q1*q2) + Kaccy*P[0][4]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][4]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][4]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][4]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*P[5][6]*SH_ACCY[0] - Kaccy*P[23][6]*SH_ACCY[0] - Kaccy*P[4][6]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][6]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][6]*(q0*q3 - q1*q2) + Kaccy*P[0][6]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][6]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][6]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][6]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*P[5][22]*SH_ACCY[0] - Kaccy*P[23][22]*SH_ACCY[0] - Kaccy*P[4][22]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][22]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][22]*(q0*q3 - q1*q2) + Kaccy*P[0][22]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][22]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][22]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][22]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd)*(Kaccy*P[5][0]*SH_ACCY[0] - Kaccy*P[23][0]*SH_ACCY[0] - Kaccy*P[4][0]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][0]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][0]*(q0*q3 - q1*q2) + Kaccy*P[0][0]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][0]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][0]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][0]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd)*(Kaccy*P[5][1]*SH_ACCY[0] - Kaccy*P[23][1]*SH_ACCY[0] - Kaccy*P[4][1]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][1]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][1]*(q0*q3 - q1*q2) + Kaccy*P[0][1]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][1]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][1]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][1]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd)*(Kaccy*P[5][2]*SH_ACCY[0] - Kaccy*P[23][2]*SH_ACCY[0] - Kaccy*P[4][2]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][2]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][2]*(q0*q3 - q1*q2) + Kaccy*P[0][2]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][2]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][2]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][2]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)*(Kaccy*P[5][3]*SH_ACCY[0] - Kaccy*P[23][3]*SH_ACCY[0] - Kaccy*P[4][3]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][3]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][3]*(q0*q3 - q1*q2) + Kaccy*P[0][3]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][3]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][3]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][3]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd))); -SK_ACCY[1] = 2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd; -SK_ACCY[2] = 2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd; -SK_ACCY[3] = 2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd; -SK_ACCY[4] = 2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd; -SK_ACCY[5] = 2*q0*q3 - 2*q1*q2; -SK_ACCY[6] = q0*q3 - q1*q2; -SK_ACCY[7] = 2*q0*q1 + 2*q2*q3; -SK_ACCY[8] = SH_ACCY[0]; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = -SK_ACCY[0]*(Kaccy*P[0][0]*SK_ACCY[3] + Kaccy*P[0][1]*SK_ACCY[2] - Kaccy*P[0][3]*SK_ACCY[1] + Kaccy*P[0][2]*SK_ACCY[4] - Kaccy*P[0][4]*SK_ACCY[5] + Kaccy*P[0][5]*SK_ACCY[8] + Kaccy*P[0][6]*SK_ACCY[7] + 2*Kaccy*P[0][22]*SK_ACCY[6] - Kaccy*P[0][23]*SK_ACCY[8]); -Kfusion[1] = -SK_ACCY[0]*(Kaccy*P[1][0]*SK_ACCY[3] + Kaccy*P[1][1]*SK_ACCY[2] - Kaccy*P[1][3]*SK_ACCY[1] + Kaccy*P[1][2]*SK_ACCY[4] - Kaccy*P[1][4]*SK_ACCY[5] + Kaccy*P[1][5]*SK_ACCY[8] + Kaccy*P[1][6]*SK_ACCY[7] + 2*Kaccy*P[1][22]*SK_ACCY[6] - Kaccy*P[1][23]*SK_ACCY[8]); -Kfusion[2] = -SK_ACCY[0]*(Kaccy*P[2][0]*SK_ACCY[3] + Kaccy*P[2][1]*SK_ACCY[2] - Kaccy*P[2][3]*SK_ACCY[1] + Kaccy*P[2][2]*SK_ACCY[4] - Kaccy*P[2][4]*SK_ACCY[5] + Kaccy*P[2][5]*SK_ACCY[8] + Kaccy*P[2][6]*SK_ACCY[7] + 2*Kaccy*P[2][22]*SK_ACCY[6] - Kaccy*P[2][23]*SK_ACCY[8]); -Kfusion[3] = -SK_ACCY[0]*(Kaccy*P[3][0]*SK_ACCY[3] + Kaccy*P[3][1]*SK_ACCY[2] - Kaccy*P[3][3]*SK_ACCY[1] + Kaccy*P[3][2]*SK_ACCY[4] - Kaccy*P[3][4]*SK_ACCY[5] + Kaccy*P[3][5]*SK_ACCY[8] + Kaccy*P[3][6]*SK_ACCY[7] + 2*Kaccy*P[3][22]*SK_ACCY[6] - Kaccy*P[3][23]*SK_ACCY[8]); -Kfusion[4] = -SK_ACCY[0]*(Kaccy*P[4][0]*SK_ACCY[3] + Kaccy*P[4][1]*SK_ACCY[2] - Kaccy*P[4][3]*SK_ACCY[1] + Kaccy*P[4][2]*SK_ACCY[4] - Kaccy*P[4][4]*SK_ACCY[5] + Kaccy*P[4][5]*SK_ACCY[8] + Kaccy*P[4][6]*SK_ACCY[7] + 2*Kaccy*P[4][22]*SK_ACCY[6] - Kaccy*P[4][23]*SK_ACCY[8]); -Kfusion[5] = -SK_ACCY[0]*(Kaccy*P[5][0]*SK_ACCY[3] + Kaccy*P[5][1]*SK_ACCY[2] - Kaccy*P[5][3]*SK_ACCY[1] + Kaccy*P[5][2]*SK_ACCY[4] - Kaccy*P[5][4]*SK_ACCY[5] + Kaccy*P[5][5]*SK_ACCY[8] + Kaccy*P[5][6]*SK_ACCY[7] + 2*Kaccy*P[5][22]*SK_ACCY[6] - Kaccy*P[5][23]*SK_ACCY[8]); -Kfusion[6] = -SK_ACCY[0]*(Kaccy*P[6][0]*SK_ACCY[3] + Kaccy*P[6][1]*SK_ACCY[2] - Kaccy*P[6][3]*SK_ACCY[1] + Kaccy*P[6][2]*SK_ACCY[4] - Kaccy*P[6][4]*SK_ACCY[5] + Kaccy*P[6][5]*SK_ACCY[8] + Kaccy*P[6][6]*SK_ACCY[7] + 2*Kaccy*P[6][22]*SK_ACCY[6] - Kaccy*P[6][23]*SK_ACCY[8]); -Kfusion[7] = -SK_ACCY[0]*(Kaccy*P[7][0]*SK_ACCY[3] + Kaccy*P[7][1]*SK_ACCY[2] - Kaccy*P[7][3]*SK_ACCY[1] + Kaccy*P[7][2]*SK_ACCY[4] - Kaccy*P[7][4]*SK_ACCY[5] + Kaccy*P[7][5]*SK_ACCY[8] + Kaccy*P[7][6]*SK_ACCY[7] + 2*Kaccy*P[7][22]*SK_ACCY[6] - Kaccy*P[7][23]*SK_ACCY[8]); -Kfusion[8] = -SK_ACCY[0]*(Kaccy*P[8][0]*SK_ACCY[3] + Kaccy*P[8][1]*SK_ACCY[2] - Kaccy*P[8][3]*SK_ACCY[1] + Kaccy*P[8][2]*SK_ACCY[4] - Kaccy*P[8][4]*SK_ACCY[5] + Kaccy*P[8][5]*SK_ACCY[8] + Kaccy*P[8][6]*SK_ACCY[7] + 2*Kaccy*P[8][22]*SK_ACCY[6] - Kaccy*P[8][23]*SK_ACCY[8]); -Kfusion[9] = -SK_ACCY[0]*(Kaccy*P[9][0]*SK_ACCY[3] + Kaccy*P[9][1]*SK_ACCY[2] - Kaccy*P[9][3]*SK_ACCY[1] + Kaccy*P[9][2]*SK_ACCY[4] - Kaccy*P[9][4]*SK_ACCY[5] + Kaccy*P[9][5]*SK_ACCY[8] + Kaccy*P[9][6]*SK_ACCY[7] + 2*Kaccy*P[9][22]*SK_ACCY[6] - Kaccy*P[9][23]*SK_ACCY[8]); -Kfusion[10] = -SK_ACCY[0]*(Kaccy*P[10][0]*SK_ACCY[3] + Kaccy*P[10][1]*SK_ACCY[2] - Kaccy*P[10][3]*SK_ACCY[1] + Kaccy*P[10][2]*SK_ACCY[4] - Kaccy*P[10][4]*SK_ACCY[5] + Kaccy*P[10][5]*SK_ACCY[8] + Kaccy*P[10][6]*SK_ACCY[7] + 2*Kaccy*P[10][22]*SK_ACCY[6] - Kaccy*P[10][23]*SK_ACCY[8]); -Kfusion[11] = -SK_ACCY[0]*(Kaccy*P[11][0]*SK_ACCY[3] + Kaccy*P[11][1]*SK_ACCY[2] - Kaccy*P[11][3]*SK_ACCY[1] + Kaccy*P[11][2]*SK_ACCY[4] - Kaccy*P[11][4]*SK_ACCY[5] + Kaccy*P[11][5]*SK_ACCY[8] + Kaccy*P[11][6]*SK_ACCY[7] + 2*Kaccy*P[11][22]*SK_ACCY[6] - Kaccy*P[11][23]*SK_ACCY[8]); -Kfusion[12] = -SK_ACCY[0]*(Kaccy*P[12][0]*SK_ACCY[3] + Kaccy*P[12][1]*SK_ACCY[2] - Kaccy*P[12][3]*SK_ACCY[1] + Kaccy*P[12][2]*SK_ACCY[4] - Kaccy*P[12][4]*SK_ACCY[5] + Kaccy*P[12][5]*SK_ACCY[8] + Kaccy*P[12][6]*SK_ACCY[7] + 2*Kaccy*P[12][22]*SK_ACCY[6] - Kaccy*P[12][23]*SK_ACCY[8]); -Kfusion[13] = -SK_ACCY[0]*(Kaccy*P[13][0]*SK_ACCY[3] + Kaccy*P[13][1]*SK_ACCY[2] - Kaccy*P[13][3]*SK_ACCY[1] + Kaccy*P[13][2]*SK_ACCY[4] - Kaccy*P[13][4]*SK_ACCY[5] + Kaccy*P[13][5]*SK_ACCY[8] + Kaccy*P[13][6]*SK_ACCY[7] + 2*Kaccy*P[13][22]*SK_ACCY[6] - Kaccy*P[13][23]*SK_ACCY[8]); -Kfusion[14] = -SK_ACCY[0]*(Kaccy*P[14][0]*SK_ACCY[3] + Kaccy*P[14][1]*SK_ACCY[2] - Kaccy*P[14][3]*SK_ACCY[1] + Kaccy*P[14][2]*SK_ACCY[4] - Kaccy*P[14][4]*SK_ACCY[5] + Kaccy*P[14][5]*SK_ACCY[8] + Kaccy*P[14][6]*SK_ACCY[7] + 2*Kaccy*P[14][22]*SK_ACCY[6] - Kaccy*P[14][23]*SK_ACCY[8]); -Kfusion[15] = -SK_ACCY[0]*(Kaccy*P[15][0]*SK_ACCY[3] + Kaccy*P[15][1]*SK_ACCY[2] - Kaccy*P[15][3]*SK_ACCY[1] + Kaccy*P[15][2]*SK_ACCY[4] - Kaccy*P[15][4]*SK_ACCY[5] + Kaccy*P[15][5]*SK_ACCY[8] + Kaccy*P[15][6]*SK_ACCY[7] + 2*Kaccy*P[15][22]*SK_ACCY[6] - Kaccy*P[15][23]*SK_ACCY[8]); -Kfusion[16] = -SK_ACCY[0]*(Kaccy*P[16][0]*SK_ACCY[3] + Kaccy*P[16][1]*SK_ACCY[2] - Kaccy*P[16][3]*SK_ACCY[1] + Kaccy*P[16][2]*SK_ACCY[4] - Kaccy*P[16][4]*SK_ACCY[5] + Kaccy*P[16][5]*SK_ACCY[8] + Kaccy*P[16][6]*SK_ACCY[7] + 2*Kaccy*P[16][22]*SK_ACCY[6] - Kaccy*P[16][23]*SK_ACCY[8]); -Kfusion[17] = -SK_ACCY[0]*(Kaccy*P[17][0]*SK_ACCY[3] + Kaccy*P[17][1]*SK_ACCY[2] - Kaccy*P[17][3]*SK_ACCY[1] + Kaccy*P[17][2]*SK_ACCY[4] - Kaccy*P[17][4]*SK_ACCY[5] + Kaccy*P[17][5]*SK_ACCY[8] + Kaccy*P[17][6]*SK_ACCY[7] + 2*Kaccy*P[17][22]*SK_ACCY[6] - Kaccy*P[17][23]*SK_ACCY[8]); -Kfusion[18] = -SK_ACCY[0]*(Kaccy*P[18][0]*SK_ACCY[3] + Kaccy*P[18][1]*SK_ACCY[2] - Kaccy*P[18][3]*SK_ACCY[1] + Kaccy*P[18][2]*SK_ACCY[4] - Kaccy*P[18][4]*SK_ACCY[5] + Kaccy*P[18][5]*SK_ACCY[8] + Kaccy*P[18][6]*SK_ACCY[7] + 2*Kaccy*P[18][22]*SK_ACCY[6] - Kaccy*P[18][23]*SK_ACCY[8]); -Kfusion[19] = -SK_ACCY[0]*(Kaccy*P[19][0]*SK_ACCY[3] + Kaccy*P[19][1]*SK_ACCY[2] - Kaccy*P[19][3]*SK_ACCY[1] + Kaccy*P[19][2]*SK_ACCY[4] - Kaccy*P[19][4]*SK_ACCY[5] + Kaccy*P[19][5]*SK_ACCY[8] + Kaccy*P[19][6]*SK_ACCY[7] + 2*Kaccy*P[19][22]*SK_ACCY[6] - Kaccy*P[19][23]*SK_ACCY[8]); -Kfusion[20] = -SK_ACCY[0]*(Kaccy*P[20][0]*SK_ACCY[3] + Kaccy*P[20][1]*SK_ACCY[2] - Kaccy*P[20][3]*SK_ACCY[1] + Kaccy*P[20][2]*SK_ACCY[4] - Kaccy*P[20][4]*SK_ACCY[5] + Kaccy*P[20][5]*SK_ACCY[8] + Kaccy*P[20][6]*SK_ACCY[7] + 2*Kaccy*P[20][22]*SK_ACCY[6] - Kaccy*P[20][23]*SK_ACCY[8]); -Kfusion[21] = -SK_ACCY[0]*(Kaccy*P[21][0]*SK_ACCY[3] + Kaccy*P[21][1]*SK_ACCY[2] - Kaccy*P[21][3]*SK_ACCY[1] + Kaccy*P[21][2]*SK_ACCY[4] - Kaccy*P[21][4]*SK_ACCY[5] + Kaccy*P[21][5]*SK_ACCY[8] + Kaccy*P[21][6]*SK_ACCY[7] + 2*Kaccy*P[21][22]*SK_ACCY[6] - Kaccy*P[21][23]*SK_ACCY[8]); -Kfusion[22] = -SK_ACCY[0]*(Kaccy*P[22][0]*SK_ACCY[3] + Kaccy*P[22][1]*SK_ACCY[2] - Kaccy*P[22][3]*SK_ACCY[1] + Kaccy*P[22][2]*SK_ACCY[4] - Kaccy*P[22][4]*SK_ACCY[5] + Kaccy*P[22][5]*SK_ACCY[8] + Kaccy*P[22][6]*SK_ACCY[7] + 2*Kaccy*P[22][22]*SK_ACCY[6] - Kaccy*P[22][23]*SK_ACCY[8]); -Kfusion[23] = -SK_ACCY[0]*(Kaccy*P[23][0]*SK_ACCY[3] + Kaccy*P[23][1]*SK_ACCY[2] - Kaccy*P[23][3]*SK_ACCY[1] + Kaccy*P[23][2]*SK_ACCY[4] - Kaccy*P[23][4]*SK_ACCY[5] + Kaccy*P[23][5]*SK_ACCY[8] + Kaccy*P[23][6]*SK_ACCY[7] + 2*Kaccy*P[23][22]*SK_ACCY[6] - Kaccy*P[23][23]*SK_ACCY[8]); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/C_code4.txt b/EKF/matlab/scripts/Inertial Nav EKF/C_code4.txt deleted file mode 100644 index 8bef4adc48..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/C_code4.txt +++ /dev/null @@ -1,35 +0,0 @@ -float SF[9][1]; -SF[0] = 0; -SF[1] = 0; -SF[2] = 0; -SF[3] = -SF[2]; -SF[4] = SF[2]; -SF[5] = 0; -SF[6] = 0; -SF[7] = sq(q3); -SF[8] = sq(q2); -float SG[3][1]; -SG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); -SG[1] = 2*q0*q2 - 2*q1*q3; -SG[2] = 2*q0*q1 + 2*q2*q3; -float SQ[4][1]; -SQ[0] = (q1*SG[0])/2 - (q0*SG[2])/2 + (q3*SG[1])/2; -SQ[1] = (q0*SG[1])/2 - (q2*SG[0])/2 + (q3*SG[2])/2; -SQ[2] = (q3*SG[0])/2 - (q1*SG[1])/2 + (q2*SG[2])/2; -SQ[3] = (q0*SG[0])/2 + (q1*SG[2])/2 + (q2*SG[1])/2; -float SPP[4][1]; -SPP[0] = SF[5] - SF[0] + SF[6]; -SPP[1] = SF[0] - SF[5]; -SPP[2] = SF[0] + SF[5] + SF[6]; -SPP[3] = SF[1]; -float nextP[4][4]; -nextP[0][0] = P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1] + SF[3]*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) + SPP[3]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) + SPP[1]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) + daYawVar*sq(SQ[2]); -nextP[0][1] = P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1] - (SF[0] + SF[6])*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) + SF[3]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) - SPP[3]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + daYawVar*SQ[1]*SQ[2]; -nextP[1][1] = P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] + SF[3]*(P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] - P[2][3]*(SF[0] + SF[6])) - SPP[3]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + daYawVar*sq(SQ[1]) - P[2][1]*(SF[0] + SF[6]) - (SF[0] + SF[6])*(P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] - P[2][2]*(SF[0] + SF[6])); -nextP[0][2] = P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1] + SF[4]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + SPP[2]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) - SPP[3]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) + daYawVar*SQ[0]*SQ[2]; -nextP[1][2] = P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] + SF[4]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + SPP[2]*(P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] - P[2][1]*(SF[0] + SF[6])) - SPP[3]*(P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] - P[2][3]*(SF[0] + SF[6])) - P[2][2]*(SF[0] + SF[6]) + daYawVar*SQ[0]*SQ[1]; -nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SPP[2] - P[3][2]*SPP[3] + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SPP[2] - P[3][0]*SPP[3]) + SPP[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SPP[2] - P[3][1]*SPP[3]) - SPP[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SPP[2] - P[3][3]*SPP[3]) + daYawVar*sq(SQ[0]); -nextP[0][3] = P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1] + SF[4]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) + SPP[0]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + SPP[3]*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) - daYawVar*SQ[2]*SQ[3]; -nextP[1][3] = P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] + SF[4]*(P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] - P[2][1]*(SF[0] + SF[6])) + SPP[0]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + SPP[3]*(P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] - P[2][2]*(SF[0] + SF[6])) - P[2][3]*(SF[0] + SF[6]) - daYawVar*SQ[1]*SQ[3]; -nextP[2][3] = P[2][3] + P[0][3]*SF[4] + P[1][3]*SPP[2] - P[3][3]*SPP[3] + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SPP[2] - P[3][1]*SPP[3]) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SPP[2] - P[3][0]*SPP[3]) + SPP[3]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SPP[2] - P[3][2]*SPP[3]) - daYawVar*SQ[0]*SQ[3]; -nextP[3][3] = P[3][3] + P[1][3]*SF[4] + P[0][3]*SPP[0] + P[2][3]*SPP[3] + SF[4]*(P[3][1] + P[1][1]*SF[4] + P[0][1]*SPP[0] + P[2][1]*SPP[3]) + SPP[0]*(P[3][0] + P[1][0]*SF[4] + P[0][0]*SPP[0] + P[2][0]*SPP[3]) + SPP[3]*(P[3][2] + P[1][2]*SF[4] + P[0][2]*SPP[0] + P[2][2]*SPP[3]) + daYawVar*sq(SQ[3]); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/C_code_use_matrix_lib24.txt b/EKF/matlab/scripts/Inertial Nav EKF/C_code_use_matrix_lib24.txt deleted file mode 100644 index fe1faf6652..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/C_code_use_matrix_lib24.txt +++ /dev/null @@ -1,686 +0,0 @@ -float SF[21][1]; -SF[0] = dvz - dvz_b; -SF[1] = dvy - dvy_b; -SF[2] = dvx - dvx_b; -SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0]; -SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2]; -SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1]; -SF[6] = day/2 - day_b/2; -SF[7] = daz/2 - daz_b/2; -SF[8] = dax/2 - dax_b/2; -SF[9] = dax_b/2 - dax/2; -SF[10] = daz_b/2 - daz/2; -SF[11] = day_b/2 - day/2; -SF[12] = 2*q1*SF[1]; -SF[13] = 2*q0*SF[0]; -SF[14] = q1/2; -SF[15] = q2/2; -SF[16] = q3/2; -SF[17] = sq(q3); -SF[18] = sq(q2); -SF[19] = sq(q1); -SF[20] = sq(q0); -float SG[8][1]; -SG[0] = q0/2; -SG[1] = sq(q3); -SG[2] = sq(q2); -SG[3] = sq(q1); -SG[4] = sq(q0); -SG[5] = 2*q2*q3; -SG[6] = 2*q1*q3; -SG[7] = 2*q1*q2; -float SQ[11][1]; -SQ[0] = dvzVar*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); -SQ[1] = dvzVar*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); -SQ[2] = dvzVar*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyVar*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); -SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4; -SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4; -SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4; -SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4; -SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2; -SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4; -SQ[9] = sq(SG[0]); -SQ[10] = sq(q1); -float SPP(11,1); -SPP[0] = SF[12] + SF[13] - 2*q2*SF[2]; -SPP[1] = SF[17] - SF[18] - SF[19] + SF[20]; -SPP[2] = SF[17] - SF[18] + SF[19] - SF[20]; -SPP[3] = SF[17] + SF[18] - SF[19] - SF[20]; -SPP[4] = 2*q0*q2 - 2*q1*q3; -SPP[5] = 2*q0*q1 - 2*q2*q3; -SPP[6] = 2*q0*q3 - 2*q1*q2; -SPP[7] = 2*q0*q1 + 2*q2*q3; -SPP[8] = 2*q0*q3 + 2*q1*q2; -SPP[9] = 2*q0*q2 + 2*q1*q3; -SPP[10] = SF[16]; -float nextP[24][24]; -nextP(0,0) = P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[11]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[10]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SF[14]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) + SF[15]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) + SPP[10]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4; -nextP(0,1) = P(0,1) + SQ[8] + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10] + SF[8]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[7]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[11]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) - SF[15]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) + SPP[10]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) - (q0*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]))/2; -nextP(1,1) = P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] + daxVar*SQ[9] - (P(10,1)*q0)/2 + SF[8]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[7]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SF[11]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) - SF[15]*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)/2) + SPP[10]*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)/2) + (dayVar*sq(q3))/4 + (dazVar*sq(q2))/4 - (q0*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)/2))/2; -nextP(0,2) = P(0,2) + SQ[7] + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10] + SF[6]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[10]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[8]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SF[14]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) - SPP[10]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) - (q0*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]))/2; -nextP(1,2) = P(1,2) + SQ[5] + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2 + SF[6]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[10]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) + SF[8]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) + SF[14]*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)/2) - SPP[10]*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)/2) - (q0*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)/2))/2; -nextP(2,2) = P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])/4 - (P(11,2)*q0)/2 + SF[6]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SF[10]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) + SF[8]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2) + SF[14]*(P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)/2) - SPP[10]*(P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)/2) + (daxVar*sq(q3))/4 - (q0*(P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)/2))/2; -nextP(0,3) = P(0,3) + SQ[6] + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10] + SF[7]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[6]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[9]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[15]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) - SF[14]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) - (q0*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]))/2; -nextP(1,3) = P(1,3) + SQ[4] + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2 + SF[7]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[6]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) + SF[9]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SF[15]*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)/2) - SF[14]*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)/2) - (q0*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)/2))/2; -nextP(2,3) = P(2,3) + SQ[3] + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2 + SF[7]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SF[6]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) + SF[9]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)/2) + SF[15]*(P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)/2) - SF[14]*(P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)/2) - (q0*(P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)/2))/2; -nextP(3,3) = P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] + (dayVar*SQ[10])/4 + dazVar*SQ[9] - (P(12,3)*q0)/2 + SF[7]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)/2) + SF[6]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)/2) + SF[9]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)/2) + SF[15]*(P(3,10) + P(0,10)*SF[7] + P(1,10)*SF[6] + P(2,10)*SF[9] + P(10,10)*SF[15] - P(11,10)*SF[14] - (P(12,10)*q0)/2) - SF[14]*(P(3,11) + P(0,11)*SF[7] + P(1,11)*SF[6] + P(2,11)*SF[9] + P(10,11)*SF[15] - P(11,11)*SF[14] - (P(12,11)*q0)/2) + (daxVar*sq(q2))/4 - (q0*(P(3,12) + P(0,12)*SF[7] + P(1,12)*SF[6] + P(2,12)*SF[9] + P(10,12)*SF[15] - P(11,12)*SF[14] - (P(12,12)*q0)/2))/2; -nextP(0,4) = P(0,4) + P(1,4)*SF[9] + P(2,4)*SF[11] + P(3,4)*SF[10] + P(10,4)*SF[14] + P(11,4)*SF[15] + P(12,4)*SPP[10] + SF[5]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[3]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SF[4]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SPP[0]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SPP[3]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) + SPP[6]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) - SPP[9]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]); -nextP(1,4) = P(1,4) + P(0,4)*SF[8] + P(2,4)*SF[7] + P(3,4)*SF[11] - P(12,4)*SF[15] + P(11,4)*SPP[10] - (P(10,4)*q0)/2 + SF[5]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[3]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) - SF[4]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) + SPP[0]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SPP[3]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)/2) + SPP[6]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)/2) - SPP[9]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)/2); -nextP(2,4) = P(2,4) + P(0,4)*SF[6] + P(1,4)*SF[10] + P(3,4)*SF[8] + P(12,4)*SF[14] - P(10,4)*SPP[10] - (P(11,4)*q0)/2 + SF[5]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SF[3]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) - SF[4]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2) + SPP[0]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)/2) + SPP[3]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)/2) + SPP[6]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)/2) - SPP[9]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)/2); -nextP(3,4) = P(3,4) + P(0,4)*SF[7] + P(1,4)*SF[6] + P(2,4)*SF[9] + P(10,4)*SF[15] - P(11,4)*SF[14] - (P(12,4)*q0)/2 + SF[5]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)/2) + SF[3]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)/2) - SF[4]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)/2) + SPP[0]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)/2) + SPP[3]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)/2) + SPP[6]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)/2) - SPP[9]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)/2); -nextP(4,4) = P(4,4) + P(0,4)*SF[5] + P(1,4)*SF[3] - P(3,4)*SF[4] + P(2,4)*SPP[0] + P(13,4)*SPP[3] + P(14,4)*SPP[6] - P(15,4)*SPP[9] + dvyVar*sq(SG[7] - 2*q0*q3) + dvzVar*sq(SG[6] + 2*q0*q2) + SF[5]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SF[3]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SF[4]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) + SPP[0]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SPP[3]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) + SPP[6]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) - SPP[9]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]); -nextP(0,5) = P(0,5) + P(1,5)*SF[9] + P(2,5)*SF[11] + P(3,5)*SF[10] + P(10,5)*SF[14] + P(11,5)*SF[15] + P(12,5)*SPP[10] + SF[4]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[3]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[5]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) - SPP[0]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SPP[8]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) + SPP[2]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) + SPP[5]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]); -nextP(1,5) = P(1,5) + P(0,5)*SF[8] + P(2,5)*SF[7] + P(3,5)*SF[11] - P(12,5)*SF[15] + P(11,5)*SPP[10] - (P(10,5)*q0)/2 + SF[4]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[3]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SF[5]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) - SPP[0]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) - SPP[8]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)/2) + SPP[2]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)/2) + SPP[5]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)/2); -nextP(2,5) = P(2,5) + P(0,5)*SF[6] + P(1,5)*SF[10] + P(3,5)*SF[8] + P(12,5)*SF[14] - P(10,5)*SPP[10] - (P(11,5)*q0)/2 + SF[4]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SF[3]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)/2) + SF[5]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2) - SPP[0]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) - SPP[8]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)/2) + SPP[2]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)/2) + SPP[5]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)/2); -nextP(3,5) = P(3,5) + P(0,5)*SF[7] + P(1,5)*SF[6] + P(2,5)*SF[9] + P(10,5)*SF[15] - P(11,5)*SF[14] - (P(12,5)*q0)/2 + SF[4]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)/2) + SF[3]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)/2) + SF[5]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)/2) - SPP[0]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)/2) - SPP[8]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)/2) + SPP[2]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)/2) + SPP[5]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)/2); -nextP(4,5) = P(4,5) + SQ[2] + P(0,5)*SF[5] + P(1,5)*SF[3] - P(3,5)*SF[4] + P(2,5)*SPP[0] + P(13,5)*SPP[3] + P(14,5)*SPP[6] - P(15,5)*SPP[9] + SF[4]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SF[3]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SF[5]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) - SPP[0]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SPP[8]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) + SPP[2]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) + SPP[5]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]); -nextP(5,5) = P(5,5) + P(0,5)*SF[4] + P(2,5)*SF[3] + P(3,5)*SF[5] - P(1,5)*SPP[0] - P(13,5)*SPP[8] + P(14,5)*SPP[2] + P(15,5)*SPP[5] + dvxVar*sq(SG[7] + 2*q0*q3) + dvzVar*sq(SG[5] - 2*q0*q1) + SF[4]*(P(5,0) + P(0,0)*SF[4] + P(2,0)*SF[3] + P(3,0)*SF[5] - P(1,0)*SPP[0] - P(13,0)*SPP[8] + P(14,0)*SPP[2] + P(15,0)*SPP[5]) + SF[3]*(P(5,2) + P(0,2)*SF[4] + P(2,2)*SF[3] + P(3,2)*SF[5] - P(1,2)*SPP[0] - P(13,2)*SPP[8] + P(14,2)*SPP[2] + P(15,2)*SPP[5]) + SF[5]*(P(5,3) + P(0,3)*SF[4] + P(2,3)*SF[3] + P(3,3)*SF[5] - P(1,3)*SPP[0] - P(13,3)*SPP[8] + P(14,3)*SPP[2] + P(15,3)*SPP[5]) - SPP[0]*(P(5,1) + P(0,1)*SF[4] + P(2,1)*SF[3] + P(3,1)*SF[5] - P(1,1)*SPP[0] - P(13,1)*SPP[8] + P(14,1)*SPP[2] + P(15,1)*SPP[5]) - SPP[8]*(P(5,13) + P(0,13)*SF[4] + P(2,13)*SF[3] + P(3,13)*SF[5] - P(1,13)*SPP[0] - P(13,13)*SPP[8] + P(14,13)*SPP[2] + P(15,13)*SPP[5]) + SPP[2]*(P(5,14) + P(0,14)*SF[4] + P(2,14)*SF[3] + P(3,14)*SF[5] - P(1,14)*SPP[0] - P(13,14)*SPP[8] + P(14,14)*SPP[2] + P(15,14)*SPP[5]) + SPP[5]*(P(5,15) + P(0,15)*SF[4] + P(2,15)*SF[3] + P(3,15)*SF[5] - P(1,15)*SPP[0] - P(13,15)*SPP[8] + P(14,15)*SPP[2] + P(15,15)*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]); -nextP(0,6) = P(0,6) + P(1,6)*SF[9] + P(2,6)*SF[11] + P(3,6)*SF[10] + P(10,6)*SF[14] + P(11,6)*SF[15] + P(12,6)*SPP[10] + SF[4]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SF[5]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[3]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SPP[0]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SPP[4]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) - SPP[7]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) - SPP[1]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]); -nextP(1,6) = P(1,6) + P(0,6)*SF[8] + P(2,6)*SF[7] + P(3,6)*SF[11] - P(12,6)*SF[15] + P(11,6)*SPP[10] - (P(10,6)*q0)/2 + SF[4]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) - SF[5]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SF[3]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) + SPP[0]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SPP[4]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)/2) - SPP[7]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)/2) - SPP[1]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)/2); -nextP(2,6) = P(2,6) + P(0,6)*SF[6] + P(1,6)*SF[10] + P(3,6)*SF[8] + P(12,6)*SF[14] - P(10,6)*SPP[10] - (P(11,6)*q0)/2 + SF[4]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) - SF[5]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)/2) + SF[3]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2) + SPP[0]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SPP[4]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)/2) - SPP[7]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)/2) - SPP[1]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)/2); -nextP(3,6) = P(3,6) + P(0,6)*SF[7] + P(1,6)*SF[6] + P(2,6)*SF[9] + P(10,6)*SF[15] - P(11,6)*SF[14] - (P(12,6)*q0)/2 + SF[4]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)/2) - SF[5]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)/2) + SF[3]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)/2) + SPP[0]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)/2) + SPP[4]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)/2) - SPP[7]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)/2) - SPP[1]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)/2); -nextP(4,6) = P(4,6) + SQ[1] + P(0,6)*SF[5] + P(1,6)*SF[3] - P(3,6)*SF[4] + P(2,6)*SPP[0] + P(13,6)*SPP[3] + P(14,6)*SPP[6] - P(15,6)*SPP[9] + SF[4]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SF[5]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SF[3]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) + SPP[0]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SPP[4]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) - SPP[7]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) - SPP[1]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]); -nextP(5,6) = P(5,6) + SQ[0] + P(0,6)*SF[4] + P(2,6)*SF[3] + P(3,6)*SF[5] - P(1,6)*SPP[0] - P(13,6)*SPP[8] + P(14,6)*SPP[2] + P(15,6)*SPP[5] + SF[4]*(P(5,1) + P(0,1)*SF[4] + P(2,1)*SF[3] + P(3,1)*SF[5] - P(1,1)*SPP[0] - P(13,1)*SPP[8] + P(14,1)*SPP[2] + P(15,1)*SPP[5]) - SF[5]*(P(5,2) + P(0,2)*SF[4] + P(2,2)*SF[3] + P(3,2)*SF[5] - P(1,2)*SPP[0] - P(13,2)*SPP[8] + P(14,2)*SPP[2] + P(15,2)*SPP[5]) + SF[3]*(P(5,3) + P(0,3)*SF[4] + P(2,3)*SF[3] + P(3,3)*SF[5] - P(1,3)*SPP[0] - P(13,3)*SPP[8] + P(14,3)*SPP[2] + P(15,3)*SPP[5]) + SPP[0]*(P(5,0) + P(0,0)*SF[4] + P(2,0)*SF[3] + P(3,0)*SF[5] - P(1,0)*SPP[0] - P(13,0)*SPP[8] + P(14,0)*SPP[2] + P(15,0)*SPP[5]) + SPP[4]*(P(5,13) + P(0,13)*SF[4] + P(2,13)*SF[3] + P(3,13)*SF[5] - P(1,13)*SPP[0] - P(13,13)*SPP[8] + P(14,13)*SPP[2] + P(15,13)*SPP[5]) - SPP[7]*(P(5,14) + P(0,14)*SF[4] + P(2,14)*SF[3] + P(3,14)*SF[5] - P(1,14)*SPP[0] - P(13,14)*SPP[8] + P(14,14)*SPP[2] + P(15,14)*SPP[5]) - SPP[1]*(P(5,15) + P(0,15)*SF[4] + P(2,15)*SF[3] + P(3,15)*SF[5] - P(1,15)*SPP[0] - P(13,15)*SPP[8] + P(14,15)*SPP[2] + P(15,15)*SPP[5]); -nextP(6,6) = P(6,6) + P(1,6)*SF[4] - P(2,6)*SF[5] + P(3,6)*SF[3] + P(0,6)*SPP[0] + P(13,6)*SPP[4] - P(14,6)*SPP[7] - P(15,6)*SPP[1] + dvxVar*sq(SG[6] - 2*q0*q2) + dvyVar*sq(SG[5] + 2*q0*q1) + SF[4]*(P(6,1) + P(1,1)*SF[4] - P(2,1)*SF[5] + P(3,1)*SF[3] + P(0,1)*SPP[0] + P(13,1)*SPP[4] - P(14,1)*SPP[7] - P(15,1)*SPP[1]) - SF[5]*(P(6,2) + P(1,2)*SF[4] - P(2,2)*SF[5] + P(3,2)*SF[3] + P(0,2)*SPP[0] + P(13,2)*SPP[4] - P(14,2)*SPP[7] - P(15,2)*SPP[1]) + SF[3]*(P(6,3) + P(1,3)*SF[4] - P(2,3)*SF[5] + P(3,3)*SF[3] + P(0,3)*SPP[0] + P(13,3)*SPP[4] - P(14,3)*SPP[7] - P(15,3)*SPP[1]) + SPP[0]*(P(6,0) + P(1,0)*SF[4] - P(2,0)*SF[5] + P(3,0)*SF[3] + P(0,0)*SPP[0] + P(13,0)*SPP[4] - P(14,0)*SPP[7] - P(15,0)*SPP[1]) + SPP[4]*(P(6,13) + P(1,13)*SF[4] - P(2,13)*SF[5] + P(3,13)*SF[3] + P(0,13)*SPP[0] + P(13,13)*SPP[4] - P(14,13)*SPP[7] - P(15,13)*SPP[1]) - SPP[7]*(P(6,14) + P(1,14)*SF[4] - P(2,14)*SF[5] + P(3,14)*SF[3] + P(0,14)*SPP[0] + P(13,14)*SPP[4] - P(14,14)*SPP[7] - P(15,14)*SPP[1]) - SPP[1]*(P(6,15) + P(1,15)*SF[4] - P(2,15)*SF[5] + P(3,15)*SF[3] + P(0,15)*SPP[0] + P(13,15)*SPP[4] - P(14,15)*SPP[7] - P(15,15)*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]); -nextP(0,7) = P(0,7) + P(1,7)*SF[9] + P(2,7)*SF[11] + P(3,7)*SF[10] + P(10,7)*SF[14] + P(11,7)*SF[15] + P(12,7)*SPP[10] + dt*(P(0,4) + P(1,4)*SF[9] + P(2,4)*SF[11] + P(3,4)*SF[10] + P(10,4)*SF[14] + P(11,4)*SF[15] + P(12,4)*SPP[10]); -nextP(1,7) = P(1,7) + P(0,7)*SF[8] + P(2,7)*SF[7] + P(3,7)*SF[11] - P(12,7)*SF[15] + P(11,7)*SPP[10] - (P(10,7)*q0)/2 + dt*(P(1,4) + P(0,4)*SF[8] + P(2,4)*SF[7] + P(3,4)*SF[11] - P(12,4)*SF[15] + P(11,4)*SPP[10] - (P(10,4)*q0)/2); -nextP(2,7) = P(2,7) + P(0,7)*SF[6] + P(1,7)*SF[10] + P(3,7)*SF[8] + P(12,7)*SF[14] - P(10,7)*SPP[10] - (P(11,7)*q0)/2 + dt*(P(2,4) + P(0,4)*SF[6] + P(1,4)*SF[10] + P(3,4)*SF[8] + P(12,4)*SF[14] - P(10,4)*SPP[10] - (P(11,4)*q0)/2); -nextP(3,7) = P(3,7) + P(0,7)*SF[7] + P(1,7)*SF[6] + P(2,7)*SF[9] + P(10,7)*SF[15] - P(11,7)*SF[14] - (P(12,7)*q0)/2 + dt*(P(3,4) + P(0,4)*SF[7] + P(1,4)*SF[6] + P(2,4)*SF[9] + P(10,4)*SF[15] - P(11,4)*SF[14] - (P(12,4)*q0)/2); -nextP(4,7) = P(4,7) + P(0,7)*SF[5] + P(1,7)*SF[3] - P(3,7)*SF[4] + P(2,7)*SPP[0] + P(13,7)*SPP[3] + P(14,7)*SPP[6] - P(15,7)*SPP[9] + dt*(P(4,4) + P(0,4)*SF[5] + P(1,4)*SF[3] - P(3,4)*SF[4] + P(2,4)*SPP[0] + P(13,4)*SPP[3] + P(14,4)*SPP[6] - P(15,4)*SPP[9]); -nextP(5,7) = P(5,7) + P(0,7)*SF[4] + P(2,7)*SF[3] + P(3,7)*SF[5] - P(1,7)*SPP[0] - P(13,7)*SPP[8] + P(14,7)*SPP[2] + P(15,7)*SPP[5] + dt*(P(5,4) + P(0,4)*SF[4] + P(2,4)*SF[3] + P(3,4)*SF[5] - P(1,4)*SPP[0] - P(13,4)*SPP[8] + P(14,4)*SPP[2] + P(15,4)*SPP[5]); -nextP(6,7) = P(6,7) + P(1,7)*SF[4] - P(2,7)*SF[5] + P(3,7)*SF[3] + P(0,7)*SPP[0] + P(13,7)*SPP[4] - P(14,7)*SPP[7] - P(15,7)*SPP[1] + dt*(P(6,4) + P(1,4)*SF[4] - P(2,4)*SF[5] + P(3,4)*SF[3] + P(0,4)*SPP[0] + P(13,4)*SPP[4] - P(14,4)*SPP[7] - P(15,4)*SPP[1]); -nextP(7,7) = P(7,7) + P(4,7)*dt + dt*(P(7,4) + P(4,4)*dt); -nextP(0,8) = P(0,8) + P(1,8)*SF[9] + P(2,8)*SF[11] + P(3,8)*SF[10] + P(10,8)*SF[14] + P(11,8)*SF[15] + P(12,8)*SPP[10] + dt*(P(0,5) + P(1,5)*SF[9] + P(2,5)*SF[11] + P(3,5)*SF[10] + P(10,5)*SF[14] + P(11,5)*SF[15] + P(12,5)*SPP[10]); -nextP(1,8) = P(1,8) + P(0,8)*SF[8] + P(2,8)*SF[7] + P(3,8)*SF[11] - P(12,8)*SF[15] + P(11,8)*SPP[10] - (P(10,8)*q0)/2 + dt*(P(1,5) + P(0,5)*SF[8] + P(2,5)*SF[7] + P(3,5)*SF[11] - P(12,5)*SF[15] + P(11,5)*SPP[10] - (P(10,5)*q0)/2); -nextP(2,8) = P(2,8) + P(0,8)*SF[6] + P(1,8)*SF[10] + P(3,8)*SF[8] + P(12,8)*SF[14] - P(10,8)*SPP[10] - (P(11,8)*q0)/2 + dt*(P(2,5) + P(0,5)*SF[6] + P(1,5)*SF[10] + P(3,5)*SF[8] + P(12,5)*SF[14] - P(10,5)*SPP[10] - (P(11,5)*q0)/2); -nextP(3,8) = P(3,8) + P(0,8)*SF[7] + P(1,8)*SF[6] + P(2,8)*SF[9] + P(10,8)*SF[15] - P(11,8)*SF[14] - (P(12,8)*q0)/2 + dt*(P(3,5) + P(0,5)*SF[7] + P(1,5)*SF[6] + P(2,5)*SF[9] + P(10,5)*SF[15] - P(11,5)*SF[14] - (P(12,5)*q0)/2); -nextP(4,8) = P(4,8) + P(0,8)*SF[5] + P(1,8)*SF[3] - P(3,8)*SF[4] + P(2,8)*SPP[0] + P(13,8)*SPP[3] + P(14,8)*SPP[6] - P(15,8)*SPP[9] + dt*(P(4,5) + P(0,5)*SF[5] + P(1,5)*SF[3] - P(3,5)*SF[4] + P(2,5)*SPP[0] + P(13,5)*SPP[3] + P(14,5)*SPP[6] - P(15,5)*SPP[9]); -nextP(5,8) = P(5,8) + P(0,8)*SF[4] + P(2,8)*SF[3] + P(3,8)*SF[5] - P(1,8)*SPP[0] - P(13,8)*SPP[8] + P(14,8)*SPP[2] + P(15,8)*SPP[5] + dt*(P(5,5) + P(0,5)*SF[4] + P(2,5)*SF[3] + P(3,5)*SF[5] - P(1,5)*SPP[0] - P(13,5)*SPP[8] + P(14,5)*SPP[2] + P(15,5)*SPP[5]); -nextP(6,8) = P(6,8) + P(1,8)*SF[4] - P(2,8)*SF[5] + P(3,8)*SF[3] + P(0,8)*SPP[0] + P(13,8)*SPP[4] - P(14,8)*SPP[7] - P(15,8)*SPP[1] + dt*(P(6,5) + P(1,5)*SF[4] - P(2,5)*SF[5] + P(3,5)*SF[3] + P(0,5)*SPP[0] + P(13,5)*SPP[4] - P(14,5)*SPP[7] - P(15,5)*SPP[1]); -nextP(7,8) = P(7,8) + P(4,8)*dt + dt*(P(7,5) + P(4,5)*dt); -nextP(8,8) = P(8,8) + P(5,8)*dt + dt*(P(8,5) + P(5,5)*dt); -nextP(0,9) = P(0,9) + P(1,9)*SF[9] + P(2,9)*SF[11] + P(3,9)*SF[10] + P(10,9)*SF[14] + P(11,9)*SF[15] + P(12,9)*SPP[10] + dt*(P(0,6) + P(1,6)*SF[9] + P(2,6)*SF[11] + P(3,6)*SF[10] + P(10,6)*SF[14] + P(11,6)*SF[15] + P(12,6)*SPP[10]); -nextP(1,9) = P(1,9) + P(0,9)*SF[8] + P(2,9)*SF[7] + P(3,9)*SF[11] - P(12,9)*SF[15] + P(11,9)*SPP[10] - (P(10,9)*q0)/2 + dt*(P(1,6) + P(0,6)*SF[8] + P(2,6)*SF[7] + P(3,6)*SF[11] - P(12,6)*SF[15] + P(11,6)*SPP[10] - (P(10,6)*q0)/2); -nextP(2,9) = P(2,9) + P(0,9)*SF[6] + P(1,9)*SF[10] + P(3,9)*SF[8] + P(12,9)*SF[14] - P(10,9)*SPP[10] - (P(11,9)*q0)/2 + dt*(P(2,6) + P(0,6)*SF[6] + P(1,6)*SF[10] + P(3,6)*SF[8] + P(12,6)*SF[14] - P(10,6)*SPP[10] - (P(11,6)*q0)/2); -nextP(3,9) = P(3,9) + P(0,9)*SF[7] + P(1,9)*SF[6] + P(2,9)*SF[9] + P(10,9)*SF[15] - P(11,9)*SF[14] - (P(12,9)*q0)/2 + dt*(P(3,6) + P(0,6)*SF[7] + P(1,6)*SF[6] + P(2,6)*SF[9] + P(10,6)*SF[15] - P(11,6)*SF[14] - (P(12,6)*q0)/2); -nextP(4,9) = P(4,9) + P(0,9)*SF[5] + P(1,9)*SF[3] - P(3,9)*SF[4] + P(2,9)*SPP[0] + P(13,9)*SPP[3] + P(14,9)*SPP[6] - P(15,9)*SPP[9] + dt*(P(4,6) + P(0,6)*SF[5] + P(1,6)*SF[3] - P(3,6)*SF[4] + P(2,6)*SPP[0] + P(13,6)*SPP[3] + P(14,6)*SPP[6] - P(15,6)*SPP[9]); -nextP(5,9) = P(5,9) + P(0,9)*SF[4] + P(2,9)*SF[3] + P(3,9)*SF[5] - P(1,9)*SPP[0] - P(13,9)*SPP[8] + P(14,9)*SPP[2] + P(15,9)*SPP[5] + dt*(P(5,6) + P(0,6)*SF[4] + P(2,6)*SF[3] + P(3,6)*SF[5] - P(1,6)*SPP[0] - P(13,6)*SPP[8] + P(14,6)*SPP[2] + P(15,6)*SPP[5]); -nextP(6,9) = P(6,9) + P(1,9)*SF[4] - P(2,9)*SF[5] + P(3,9)*SF[3] + P(0,9)*SPP[0] + P(13,9)*SPP[4] - P(14,9)*SPP[7] - P(15,9)*SPP[1] + dt*(P(6,6) + P(1,6)*SF[4] - P(2,6)*SF[5] + P(3,6)*SF[3] + P(0,6)*SPP[0] + P(13,6)*SPP[4] - P(14,6)*SPP[7] - P(15,6)*SPP[1]); -nextP(7,9) = P(7,9) + P(4,9)*dt + dt*(P(7,6) + P(4,6)*dt); -nextP(8,9) = P(8,9) + P(5,9)*dt + dt*(P(8,6) + P(5,6)*dt); -nextP(9,9) = P(9,9) + P(6,9)*dt + dt*(P(9,6) + P(6,6)*dt); -nextP(0,10) = P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]; -nextP(1,10) = P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)/2; -nextP(2,10) = P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)/2; -nextP(3,10) = P(3,10) + P(0,10)*SF[7] + P(1,10)*SF[6] + P(2,10)*SF[9] + P(10,10)*SF[15] - P(11,10)*SF[14] - (P(12,10)*q0)/2; -nextP(4,10) = P(4,10) + P(0,10)*SF[5] + P(1,10)*SF[3] - P(3,10)*SF[4] + P(2,10)*SPP[0] + P(13,10)*SPP[3] + P(14,10)*SPP[6] - P(15,10)*SPP[9]; -nextP(5,10) = P(5,10) + P(0,10)*SF[4] + P(2,10)*SF[3] + P(3,10)*SF[5] - P(1,10)*SPP[0] - P(13,10)*SPP[8] + P(14,10)*SPP[2] + P(15,10)*SPP[5]; -nextP(6,10) = P(6,10) + P(1,10)*SF[4] - P(2,10)*SF[5] + P(3,10)*SF[3] + P(0,10)*SPP[0] + P(13,10)*SPP[4] - P(14,10)*SPP[7] - P(15,10)*SPP[1]; -nextP(7,10) = P(7,10) + P(4,10)*dt; -nextP(8,10) = P(8,10) + P(5,10)*dt; -nextP(9,10) = P(9,10) + P(6,10)*dt; -nextP(10,10) = P(10,10); -nextP(0,11) = P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]; -nextP(1,11) = P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)/2; -nextP(2,11) = P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)/2; -nextP(3,11) = P(3,11) + P(0,11)*SF[7] + P(1,11)*SF[6] + P(2,11)*SF[9] + P(10,11)*SF[15] - P(11,11)*SF[14] - (P(12,11)*q0)/2; -nextP(4,11) = P(4,11) + P(0,11)*SF[5] + P(1,11)*SF[3] - P(3,11)*SF[4] + P(2,11)*SPP[0] + P(13,11)*SPP[3] + P(14,11)*SPP[6] - P(15,11)*SPP[9]; -nextP(5,11) = P(5,11) + P(0,11)*SF[4] + P(2,11)*SF[3] + P(3,11)*SF[5] - P(1,11)*SPP[0] - P(13,11)*SPP[8] + P(14,11)*SPP[2] + P(15,11)*SPP[5]; -nextP(6,11) = P(6,11) + P(1,11)*SF[4] - P(2,11)*SF[5] + P(3,11)*SF[3] + P(0,11)*SPP[0] + P(13,11)*SPP[4] - P(14,11)*SPP[7] - P(15,11)*SPP[1]; -nextP(7,11) = P(7,11) + P(4,11)*dt; -nextP(8,11) = P(8,11) + P(5,11)*dt; -nextP(9,11) = P(9,11) + P(6,11)*dt; -nextP(10,11) = P(10,11); -nextP(11,11) = P(11,11); -nextP(0,12) = P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]; -nextP(1,12) = P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)/2; -nextP(2,12) = P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)/2; -nextP(3,12) = P(3,12) + P(0,12)*SF[7] + P(1,12)*SF[6] + P(2,12)*SF[9] + P(10,12)*SF[15] - P(11,12)*SF[14] - (P(12,12)*q0)/2; -nextP(4,12) = P(4,12) + P(0,12)*SF[5] + P(1,12)*SF[3] - P(3,12)*SF[4] + P(2,12)*SPP[0] + P(13,12)*SPP[3] + P(14,12)*SPP[6] - P(15,12)*SPP[9]; -nextP(5,12) = P(5,12) + P(0,12)*SF[4] + P(2,12)*SF[3] + P(3,12)*SF[5] - P(1,12)*SPP[0] - P(13,12)*SPP[8] + P(14,12)*SPP[2] + P(15,12)*SPP[5]; -nextP(6,12) = P(6,12) + P(1,12)*SF[4] - P(2,12)*SF[5] + P(3,12)*SF[3] + P(0,12)*SPP[0] + P(13,12)*SPP[4] - P(14,12)*SPP[7] - P(15,12)*SPP[1]; -nextP(7,12) = P(7,12) + P(4,12)*dt; -nextP(8,12) = P(8,12) + P(5,12)*dt; -nextP(9,12) = P(9,12) + P(6,12)*dt; -nextP(10,12) = P(10,12); -nextP(11,12) = P(11,12); -nextP(12,12) = P(12,12); -nextP(0,13) = P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]; -nextP(1,13) = P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)/2; -nextP(2,13) = P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)/2; -nextP(3,13) = P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)/2; -nextP(4,13) = P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]; -nextP(5,13) = P(5,13) + P(0,13)*SF[4] + P(2,13)*SF[3] + P(3,13)*SF[5] - P(1,13)*SPP[0] - P(13,13)*SPP[8] + P(14,13)*SPP[2] + P(15,13)*SPP[5]; -nextP(6,13) = P(6,13) + P(1,13)*SF[4] - P(2,13)*SF[5] + P(3,13)*SF[3] + P(0,13)*SPP[0] + P(13,13)*SPP[4] - P(14,13)*SPP[7] - P(15,13)*SPP[1]; -nextP(7,13) = P(7,13) + P(4,13)*dt; -nextP(8,13) = P(8,13) + P(5,13)*dt; -nextP(9,13) = P(9,13) + P(6,13)*dt; -nextP(10,13) = P(10,13); -nextP(11,13) = P(11,13); -nextP(12,13) = P(12,13); -nextP(13,13) = P(13,13); -nextP(0,14) = P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]; -nextP(1,14) = P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)/2; -nextP(2,14) = P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)/2; -nextP(3,14) = P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)/2; -nextP(4,14) = P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]; -nextP(5,14) = P(5,14) + P(0,14)*SF[4] + P(2,14)*SF[3] + P(3,14)*SF[5] - P(1,14)*SPP[0] - P(13,14)*SPP[8] + P(14,14)*SPP[2] + P(15,14)*SPP[5]; -nextP(6,14) = P(6,14) + P(1,14)*SF[4] - P(2,14)*SF[5] + P(3,14)*SF[3] + P(0,14)*SPP[0] + P(13,14)*SPP[4] - P(14,14)*SPP[7] - P(15,14)*SPP[1]; -nextP(7,14) = P(7,14) + P(4,14)*dt; -nextP(8,14) = P(8,14) + P(5,14)*dt; -nextP(9,14) = P(9,14) + P(6,14)*dt; -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); -nextP(0,15) = P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]; -nextP(1,15) = P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)/2; -nextP(2,15) = P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)/2; -nextP(3,15) = P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)/2; -nextP(4,15) = P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]; -nextP(5,15) = P(5,15) + P(0,15)*SF[4] + P(2,15)*SF[3] + P(3,15)*SF[5] - P(1,15)*SPP[0] - P(13,15)*SPP[8] + P(14,15)*SPP[2] + P(15,15)*SPP[5]; -nextP(6,15) = P(6,15) + P(1,15)*SF[4] - P(2,15)*SF[5] + P(3,15)*SF[3] + P(0,15)*SPP[0] + P(13,15)*SPP[4] - P(14,15)*SPP[7] - P(15,15)*SPP[1]; -nextP(7,15) = P(7,15) + P(4,15)*dt; -nextP(8,15) = P(8,15) + P(5,15)*dt; -nextP(9,15) = P(9,15) + P(6,15)*dt; -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); -nextP(0,16) = P(0,16) + P(1,16)*SF[9] + P(2,16)*SF[11] + P(3,16)*SF[10] + P(10,16)*SF[14] + P(11,16)*SF[15] + P(12,16)*SPP[10]; -nextP(1,16) = P(1,16) + P(0,16)*SF[8] + P(2,16)*SF[7] + P(3,16)*SF[11] - P(12,16)*SF[15] + P(11,16)*SPP[10] - (P(10,16)*q0)/2; -nextP(2,16) = P(2,16) + P(0,16)*SF[6] + P(1,16)*SF[10] + P(3,16)*SF[8] + P(12,16)*SF[14] - P(10,16)*SPP[10] - (P(11,16)*q0)/2; -nextP(3,16) = P(3,16) + P(0,16)*SF[7] + P(1,16)*SF[6] + P(2,16)*SF[9] + P(10,16)*SF[15] - P(11,16)*SF[14] - (P(12,16)*q0)/2; -nextP(4,16) = P(4,16) + P(0,16)*SF[5] + P(1,16)*SF[3] - P(3,16)*SF[4] + P(2,16)*SPP[0] + P(13,16)*SPP[3] + P(14,16)*SPP[6] - P(15,16)*SPP[9]; -nextP(5,16) = P(5,16) + P(0,16)*SF[4] + P(2,16)*SF[3] + P(3,16)*SF[5] - P(1,16)*SPP[0] - P(13,16)*SPP[8] + P(14,16)*SPP[2] + P(15,16)*SPP[5]; -nextP(6,16) = P(6,16) + P(1,16)*SF[4] - P(2,16)*SF[5] + P(3,16)*SF[3] + P(0,16)*SPP[0] + P(13,16)*SPP[4] - P(14,16)*SPP[7] - P(15,16)*SPP[1]; -nextP(7,16) = P(7,16) + P(4,16)*dt; -nextP(8,16) = P(8,16) + P(5,16)*dt; -nextP(9,16) = P(9,16) + P(6,16)*dt; -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)*SF[9] + P(2,17)*SF[11] + P(3,17)*SF[10] + P(10,17)*SF[14] + P(11,17)*SF[15] + P(12,17)*SPP[10]; -nextP(1,17) = P(1,17) + P(0,17)*SF[8] + P(2,17)*SF[7] + P(3,17)*SF[11] - P(12,17)*SF[15] + P(11,17)*SPP[10] - (P(10,17)*q0)/2; -nextP(2,17) = P(2,17) + P(0,17)*SF[6] + P(1,17)*SF[10] + P(3,17)*SF[8] + P(12,17)*SF[14] - P(10,17)*SPP[10] - (P(11,17)*q0)/2; -nextP(3,17) = P(3,17) + P(0,17)*SF[7] + P(1,17)*SF[6] + P(2,17)*SF[9] + P(10,17)*SF[15] - P(11,17)*SF[14] - (P(12,17)*q0)/2; -nextP(4,17) = P(4,17) + P(0,17)*SF[5] + P(1,17)*SF[3] - P(3,17)*SF[4] + P(2,17)*SPP[0] + P(13,17)*SPP[3] + P(14,17)*SPP[6] - P(15,17)*SPP[9]; -nextP(5,17) = P(5,17) + P(0,17)*SF[4] + P(2,17)*SF[3] + P(3,17)*SF[5] - P(1,17)*SPP[0] - P(13,17)*SPP[8] + P(14,17)*SPP[2] + P(15,17)*SPP[5]; -nextP(6,17) = P(6,17) + P(1,17)*SF[4] - P(2,17)*SF[5] + P(3,17)*SF[3] + P(0,17)*SPP[0] + P(13,17)*SPP[4] - P(14,17)*SPP[7] - P(15,17)*SPP[1]; -nextP(7,17) = P(7,17) + P(4,17)*dt; -nextP(8,17) = P(8,17) + P(5,17)*dt; -nextP(9,17) = P(9,17) + P(6,17)*dt; -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)*SF[9] + P(2,18)*SF[11] + P(3,18)*SF[10] + P(10,18)*SF[14] + P(11,18)*SF[15] + P(12,18)*SPP[10]; -nextP(1,18) = P(1,18) + P(0,18)*SF[8] + P(2,18)*SF[7] + P(3,18)*SF[11] - P(12,18)*SF[15] + P(11,18)*SPP[10] - (P(10,18)*q0)/2; -nextP(2,18) = P(2,18) + P(0,18)*SF[6] + P(1,18)*SF[10] + P(3,18)*SF[8] + P(12,18)*SF[14] - P(10,18)*SPP[10] - (P(11,18)*q0)/2; -nextP(3,18) = P(3,18) + P(0,18)*SF[7] + P(1,18)*SF[6] + P(2,18)*SF[9] + P(10,18)*SF[15] - P(11,18)*SF[14] - (P(12,18)*q0)/2; -nextP(4,18) = P(4,18) + P(0,18)*SF[5] + P(1,18)*SF[3] - P(3,18)*SF[4] + P(2,18)*SPP[0] + P(13,18)*SPP[3] + P(14,18)*SPP[6] - P(15,18)*SPP[9]; -nextP(5,18) = P(5,18) + P(0,18)*SF[4] + P(2,18)*SF[3] + P(3,18)*SF[5] - P(1,18)*SPP[0] - P(13,18)*SPP[8] + P(14,18)*SPP[2] + P(15,18)*SPP[5]; -nextP(6,18) = P(6,18) + P(1,18)*SF[4] - P(2,18)*SF[5] + P(3,18)*SF[3] + P(0,18)*SPP[0] + P(13,18)*SPP[4] - P(14,18)*SPP[7] - P(15,18)*SPP[1]; -nextP(7,18) = P(7,18) + P(4,18)*dt; -nextP(8,18) = P(8,18) + P(5,18)*dt; -nextP(9,18) = P(9,18) + P(6,18)*dt; -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)*SF[9] + P(2,19)*SF[11] + P(3,19)*SF[10] + P(10,19)*SF[14] + P(11,19)*SF[15] + P(12,19)*SPP[10]; -nextP(1,19) = P(1,19) + P(0,19)*SF[8] + P(2,19)*SF[7] + P(3,19)*SF[11] - P(12,19)*SF[15] + P(11,19)*SPP[10] - (P(10,19)*q0)/2; -nextP(2,19) = P(2,19) + P(0,19)*SF[6] + P(1,19)*SF[10] + P(3,19)*SF[8] + P(12,19)*SF[14] - P(10,19)*SPP[10] - (P(11,19)*q0)/2; -nextP(3,19) = P(3,19) + P(0,19)*SF[7] + P(1,19)*SF[6] + P(2,19)*SF[9] + P(10,19)*SF[15] - P(11,19)*SF[14] - (P(12,19)*q0)/2; -nextP(4,19) = P(4,19) + P(0,19)*SF[5] + P(1,19)*SF[3] - P(3,19)*SF[4] + P(2,19)*SPP[0] + P(13,19)*SPP[3] + P(14,19)*SPP[6] - P(15,19)*SPP[9]; -nextP(5,19) = P(5,19) + P(0,19)*SF[4] + P(2,19)*SF[3] + P(3,19)*SF[5] - P(1,19)*SPP[0] - P(13,19)*SPP[8] + P(14,19)*SPP[2] + P(15,19)*SPP[5]; -nextP(6,19) = P(6,19) + P(1,19)*SF[4] - P(2,19)*SF[5] + P(3,19)*SF[3] + P(0,19)*SPP[0] + P(13,19)*SPP[4] - P(14,19)*SPP[7] - P(15,19)*SPP[1]; -nextP(7,19) = P(7,19) + P(4,19)*dt; -nextP(8,19) = P(8,19) + P(5,19)*dt; -nextP(9,19) = P(9,19) + P(6,19)*dt; -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)*SF[9] + P(2,20)*SF[11] + P(3,20)*SF[10] + P(10,20)*SF[14] + P(11,20)*SF[15] + P(12,20)*SPP[10]; -nextP(1,20) = P(1,20) + P(0,20)*SF[8] + P(2,20)*SF[7] + P(3,20)*SF[11] - P(12,20)*SF[15] + P(11,20)*SPP[10] - (P(10,20)*q0)/2; -nextP(2,20) = P(2,20) + P(0,20)*SF[6] + P(1,20)*SF[10] + P(3,20)*SF[8] + P(12,20)*SF[14] - P(10,20)*SPP[10] - (P(11,20)*q0)/2; -nextP(3,20) = P(3,20) + P(0,20)*SF[7] + P(1,20)*SF[6] + P(2,20)*SF[9] + P(10,20)*SF[15] - P(11,20)*SF[14] - (P(12,20)*q0)/2; -nextP(4,20) = P(4,20) + P(0,20)*SF[5] + P(1,20)*SF[3] - P(3,20)*SF[4] + P(2,20)*SPP[0] + P(13,20)*SPP[3] + P(14,20)*SPP[6] - P(15,20)*SPP[9]; -nextP(5,20) = P(5,20) + P(0,20)*SF[4] + P(2,20)*SF[3] + P(3,20)*SF[5] - P(1,20)*SPP[0] - P(13,20)*SPP[8] + P(14,20)*SPP[2] + P(15,20)*SPP[5]; -nextP(6,20) = P(6,20) + P(1,20)*SF[4] - P(2,20)*SF[5] + P(3,20)*SF[3] + P(0,20)*SPP[0] + P(13,20)*SPP[4] - P(14,20)*SPP[7] - P(15,20)*SPP[1]; -nextP(7,20) = P(7,20) + P(4,20)*dt; -nextP(8,20) = P(8,20) + P(5,20)*dt; -nextP(9,20) = P(9,20) + P(6,20)*dt; -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)*SF[9] + P(2,21)*SF[11] + P(3,21)*SF[10] + P(10,21)*SF[14] + P(11,21)*SF[15] + P(12,21)*SPP[10]; -nextP(1,21) = P(1,21) + P(0,21)*SF[8] + P(2,21)*SF[7] + P(3,21)*SF[11] - P(12,21)*SF[15] + P(11,21)*SPP[10] - (P(10,21)*q0)/2; -nextP(2,21) = P(2,21) + P(0,21)*SF[6] + P(1,21)*SF[10] + P(3,21)*SF[8] + P(12,21)*SF[14] - P(10,21)*SPP[10] - (P(11,21)*q0)/2; -nextP(3,21) = P(3,21) + P(0,21)*SF[7] + P(1,21)*SF[6] + P(2,21)*SF[9] + P(10,21)*SF[15] - P(11,21)*SF[14] - (P(12,21)*q0)/2; -nextP(4,21) = P(4,21) + P(0,21)*SF[5] + P(1,21)*SF[3] - P(3,21)*SF[4] + P(2,21)*SPP[0] + P(13,21)*SPP[3] + P(14,21)*SPP[6] - P(15,21)*SPP[9]; -nextP(5,21) = P(5,21) + P(0,21)*SF[4] + P(2,21)*SF[3] + P(3,21)*SF[5] - P(1,21)*SPP[0] - P(13,21)*SPP[8] + P(14,21)*SPP[2] + P(15,21)*SPP[5]; -nextP(6,21) = P(6,21) + P(1,21)*SF[4] - P(2,21)*SF[5] + P(3,21)*SF[3] + P(0,21)*SPP[0] + P(13,21)*SPP[4] - P(14,21)*SPP[7] - P(15,21)*SPP[1]; -nextP(7,21) = P(7,21) + P(4,21)*dt; -nextP(8,21) = P(8,21) + P(5,21)*dt; -nextP(9,21) = P(9,21) + P(6,21)*dt; -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); -nextP(0,22) = P(0,22) + P(1,22)*SF[9] + P(2,22)*SF[11] + P(3,22)*SF[10] + P(10,22)*SF[14] + P(11,22)*SF[15] + P(12,22)*SPP[10]; -nextP(1,22) = P(1,22) + P(0,22)*SF[8] + P(2,22)*SF[7] + P(3,22)*SF[11] - P(12,22)*SF[15] + P(11,22)*SPP[10] - (P(10,22)*q0)/2; -nextP(2,22) = P(2,22) + P(0,22)*SF[6] + P(1,22)*SF[10] + P(3,22)*SF[8] + P(12,22)*SF[14] - P(10,22)*SPP[10] - (P(11,22)*q0)/2; -nextP(3,22) = P(3,22) + P(0,22)*SF[7] + P(1,22)*SF[6] + P(2,22)*SF[9] + P(10,22)*SF[15] - P(11,22)*SF[14] - (P(12,22)*q0)/2; -nextP(4,22) = P(4,22) + P(0,22)*SF[5] + P(1,22)*SF[3] - P(3,22)*SF[4] + P(2,22)*SPP[0] + P(13,22)*SPP[3] + P(14,22)*SPP[6] - P(15,22)*SPP[9]; -nextP(5,22) = P(5,22) + P(0,22)*SF[4] + P(2,22)*SF[3] + P(3,22)*SF[5] - P(1,22)*SPP[0] - P(13,22)*SPP[8] + P(14,22)*SPP[2] + P(15,22)*SPP[5]; -nextP(6,22) = P(6,22) + P(1,22)*SF[4] - P(2,22)*SF[5] + P(3,22)*SF[3] + P(0,22)*SPP[0] + P(13,22)*SPP[4] - P(14,22)*SPP[7] - P(15,22)*SPP[1]; -nextP(7,22) = P(7,22) + P(4,22)*dt; -nextP(8,22) = P(8,22) + P(5,22)*dt; -nextP(9,22) = P(9,22) + P(6,22)*dt; -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)*SF[9] + P(2,23)*SF[11] + P(3,23)*SF[10] + P(10,23)*SF[14] + P(11,23)*SF[15] + P(12,23)*SPP[10]; -nextP(1,23) = P(1,23) + P(0,23)*SF[8] + P(2,23)*SF[7] + P(3,23)*SF[11] - P(12,23)*SF[15] + P(11,23)*SPP[10] - (P(10,23)*q0)/2; -nextP(2,23) = P(2,23) + P(0,23)*SF[6] + P(1,23)*SF[10] + P(3,23)*SF[8] + P(12,23)*SF[14] - P(10,23)*SPP[10] - (P(11,23)*q0)/2; -nextP(3,23) = P(3,23) + P(0,23)*SF[7] + P(1,23)*SF[6] + P(2,23)*SF[9] + P(10,23)*SF[15] - P(11,23)*SF[14] - (P(12,23)*q0)/2; -nextP(4,23) = P(4,23) + P(0,23)*SF[5] + P(1,23)*SF[3] - P(3,23)*SF[4] + P(2,23)*SPP[0] + P(13,23)*SPP[3] + P(14,23)*SPP[6] - P(15,23)*SPP[9]; -nextP(5,23) = P(5,23) + P(0,23)*SF[4] + P(2,23)*SF[3] + P(3,23)*SF[5] - P(1,23)*SPP[0] - P(13,23)*SPP[8] + P(14,23)*SPP[2] + P(15,23)*SPP[5]; -nextP(6,23) = P(6,23) + P(1,23)*SF[4] - P(2,23)*SF[5] + P(3,23)*SF[3] + P(0,23)*SPP[0] + P(13,23)*SPP[4] - P(14,23)*SPP[7] - P(15,23)*SPP[1]; -nextP(7,23) = P(7,23) + P(4,23)*dt; -nextP(8,23) = P(8,23) + P(5,23)*dt; -nextP(9,23) = P(9,23) + P(6,23)*dt; -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); -float SH_TAS[3][1]; -SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); -SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; -SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; -float H_TAS[1][24]; -H_TAS[0][4] = SH_TAS[2]; -H_TAS[0][5] = SH_TAS[1]; -H_TAS[0][6] = vd*SH_TAS[0]; -H_TAS[0][22] = -SH_TAS[2]; -H_TAS[0][23] = -SH_TAS[1]; -float SK_TAS[2][1]; -SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0])); -SK_TAS[1] = SH_TAS[1]; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]); -Kfusion[1] = SK_TAS[0]*(P(1,4)*SH_TAS[2] - P(1,22)*SH_TAS[2] + P(1,5)*SK_TAS[1] - P(1,23)*SK_TAS[1] + P(1,6)*vd*SH_TAS[0]); -Kfusion[2] = SK_TAS[0]*(P(2,4)*SH_TAS[2] - P(2,22)*SH_TAS[2] + P(2,5)*SK_TAS[1] - P(2,23)*SK_TAS[1] + P(2,6)*vd*SH_TAS[0]); -Kfusion[3] = SK_TAS[0]*(P(3,4)*SH_TAS[2] - P(3,22)*SH_TAS[2] + P(3,5)*SK_TAS[1] - P(3,23)*SK_TAS[1] + P(3,6)*vd*SH_TAS[0]); -Kfusion[4] = SK_TAS[0]*(P(4,4)*SH_TAS[2] - P(4,22)*SH_TAS[2] + P(4,5)*SK_TAS[1] - P(4,23)*SK_TAS[1] + P(4,6)*vd*SH_TAS[0]); -Kfusion[5] = SK_TAS[0]*(P(5,4)*SH_TAS[2] - P(5,22)*SH_TAS[2] + P(5,5)*SK_TAS[1] - P(5,23)*SK_TAS[1] + P(5,6)*vd*SH_TAS[0]); -Kfusion[6] = SK_TAS[0]*(P(6,4)*SH_TAS[2] - P(6,22)*SH_TAS[2] + P(6,5)*SK_TAS[1] - P(6,23)*SK_TAS[1] + P(6,6)*vd*SH_TAS[0]); -Kfusion[7] = SK_TAS[0]*(P(7,4)*SH_TAS[2] - P(7,22)*SH_TAS[2] + P(7,5)*SK_TAS[1] - P(7,23)*SK_TAS[1] + P(7,6)*vd*SH_TAS[0]); -Kfusion[8] = SK_TAS[0]*(P(8,4)*SH_TAS[2] - P(8,22)*SH_TAS[2] + P(8,5)*SK_TAS[1] - P(8,23)*SK_TAS[1] + P(8,6)*vd*SH_TAS[0]); -Kfusion[9] = SK_TAS[0]*(P(9,4)*SH_TAS[2] - P(9,22)*SH_TAS[2] + P(9,5)*SK_TAS[1] - P(9,23)*SK_TAS[1] + P(9,6)*vd*SH_TAS[0]); -Kfusion[10] = SK_TAS[0]*(P(10,4)*SH_TAS[2] - P(10,22)*SH_TAS[2] + P(10,5)*SK_TAS[1] - P(10,23)*SK_TAS[1] + P(10,6)*vd*SH_TAS[0]); -Kfusion[11] = SK_TAS[0]*(P(11,4)*SH_TAS[2] - P(11,22)*SH_TAS[2] + P(11,5)*SK_TAS[1] - P(11,23)*SK_TAS[1] + P(11,6)*vd*SH_TAS[0]); -Kfusion[12] = SK_TAS[0]*(P(12,4)*SH_TAS[2] - P(12,22)*SH_TAS[2] + P(12,5)*SK_TAS[1] - P(12,23)*SK_TAS[1] + P(12,6)*vd*SH_TAS[0]); -Kfusion[13] = SK_TAS[0]*(P(13,4)*SH_TAS[2] - P(13,22)*SH_TAS[2] + P(13,5)*SK_TAS[1] - P(13,23)*SK_TAS[1] + P(13,6)*vd*SH_TAS[0]); -Kfusion[14] = SK_TAS[0]*(P(14,4)*SH_TAS[2] - P(14,22)*SH_TAS[2] + P(14,5)*SK_TAS[1] - P(14,23)*SK_TAS[1] + P(14,6)*vd*SH_TAS[0]); -Kfusion[15] = SK_TAS[0]*(P(15,4)*SH_TAS[2] - P(15,22)*SH_TAS[2] + P(15,5)*SK_TAS[1] - P(15,23)*SK_TAS[1] + P(15,6)*vd*SH_TAS[0]); -Kfusion[16] = SK_TAS[0]*(P(16,4)*SH_TAS[2] - P(16,22)*SH_TAS[2] + P(16,5)*SK_TAS[1] - P(16,23)*SK_TAS[1] + P(16,6)*vd*SH_TAS[0]); -Kfusion[17] = SK_TAS[0]*(P(17,4)*SH_TAS[2] - P(17,22)*SH_TAS[2] + P(17,5)*SK_TAS[1] - P(17,23)*SK_TAS[1] + P(17,6)*vd*SH_TAS[0]); -Kfusion[18] = SK_TAS[0]*(P(18,4)*SH_TAS[2] - P(18,22)*SH_TAS[2] + P(18,5)*SK_TAS[1] - P(18,23)*SK_TAS[1] + P(18,6)*vd*SH_TAS[0]); -Kfusion[19] = SK_TAS[0]*(P(19,4)*SH_TAS[2] - P(19,22)*SH_TAS[2] + P(19,5)*SK_TAS[1] - P(19,23)*SK_TAS[1] + P(19,6)*vd*SH_TAS[0]); -Kfusion[20] = SK_TAS[0]*(P(20,4)*SH_TAS[2] - P(20,22)*SH_TAS[2] + P(20,5)*SK_TAS[1] - P(20,23)*SK_TAS[1] + P(20,6)*vd*SH_TAS[0]); -Kfusion[21] = SK_TAS[0]*(P(21,4)*SH_TAS[2] - P(21,22)*SH_TAS[2] + P(21,5)*SK_TAS[1] - P(21,23)*SK_TAS[1] + P(21,6)*vd*SH_TAS[0]); -Kfusion[22] = SK_TAS[0]*(P(22,4)*SH_TAS[2] - P(22,22)*SH_TAS[2] + P(22,5)*SK_TAS[1] - P(22,23)*SK_TAS[1] + P(22,6)*vd*SH_TAS[0]); -Kfusion[23] = SK_TAS[0]*(P(23,4)*SH_TAS[2] - P(23,22)*SH_TAS[2] + P(23,5)*SK_TAS[1] - P(23,23)*SK_TAS[1] + P(23,6)*vd*SH_TAS[0]); -float SH_BETA[13][1]; -SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); -SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2); -SH_BETA[2] = vn - vwn; -SH_BETA[3] = ve - vwe; -SH_BETA[4] = 1/sq(SH_BETA[0]); -SH_BETA[5] = 1/SH_BETA[0]; -SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); -SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_BETA[8] = 2*q0*SH_BETA[3] - 2*q3*SH_BETA[2] + 2*q1*vd; -SH_BETA[9] = 2*q0*SH_BETA[2] + 2*q3*SH_BETA[3] - 2*q2*vd; -SH_BETA[10] = 2*q2*SH_BETA[2] - 2*q1*SH_BETA[3] + 2*q0*vd; -SH_BETA[11] = 2*q1*SH_BETA[2] + 2*q2*SH_BETA[3] + 2*q3*vd; -SH_BETA[12] = 2*q0*q3; -float H_BETA[1][24]; -H_BETA[0][0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; -H_BETA[0][1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; -H_BETA[0][2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; -H_BETA[0][3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -H_BETA[0][4] = - SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -H_BETA[0][5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2); -H_BETA[0][6] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3); -H_BETA[0][22] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -H_BETA[0][23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6]; -float SK_BETA[8][1]; -SK_BETA[0] = 1/(R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P(22,4)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,4)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,4)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,4)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,4)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,4)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,4)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,4)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,4)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P(22,22)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,22)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,22)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,22)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,22)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,22)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,22)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,22)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,22)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P(22,5)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,5)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,5)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,5)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,5)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,5)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,5)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,5)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,5)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P(22,23)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,23)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,23)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,23)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,23)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,23)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,23)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,23)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,23)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P(22,0)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,0)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,0)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,0)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,0)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,0)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,0)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,0)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,0)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P(22,1)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,1)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,1)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,1)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,1)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,1)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,1)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,1)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,1)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P(22,2)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,2)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,2)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,2)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,2)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,2)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,2)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,2)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,2)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P(22,3)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,3)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,3)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,3)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,3)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,3)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,3)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,3)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,3)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))*(P(22,6)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,6)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,6)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,6)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,6)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,6)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,6)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,6)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,6)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3)))); -SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; -SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2); -SK_BETA[3] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3); -SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; -SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; -SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; -SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_BETA[0]*(P(0,0)*SK_BETA[5] + P(0,1)*SK_BETA[4] - P(0,4)*SK_BETA[1] + P(0,5)*SK_BETA[2] + P(0,2)*SK_BETA[6] + P(0,6)*SK_BETA[3] - P(0,3)*SK_BETA[7] + P(0,22)*SK_BETA[1] - P(0,23)*SK_BETA[2]); -Kfusion[1] = SK_BETA[0]*(P(1,0)*SK_BETA[5] + P(1,1)*SK_BETA[4] - P(1,4)*SK_BETA[1] + P(1,5)*SK_BETA[2] + P(1,2)*SK_BETA[6] + P(1,6)*SK_BETA[3] - P(1,3)*SK_BETA[7] + P(1,22)*SK_BETA[1] - P(1,23)*SK_BETA[2]); -Kfusion[2] = SK_BETA[0]*(P(2,0)*SK_BETA[5] + P(2,1)*SK_BETA[4] - P(2,4)*SK_BETA[1] + P(2,5)*SK_BETA[2] + P(2,2)*SK_BETA[6] + P(2,6)*SK_BETA[3] - P(2,3)*SK_BETA[7] + P(2,22)*SK_BETA[1] - P(2,23)*SK_BETA[2]); -Kfusion[3] = SK_BETA[0]*(P(3,0)*SK_BETA[5] + P(3,1)*SK_BETA[4] - P(3,4)*SK_BETA[1] + P(3,5)*SK_BETA[2] + P(3,2)*SK_BETA[6] + P(3,6)*SK_BETA[3] - P(3,3)*SK_BETA[7] + P(3,22)*SK_BETA[1] - P(3,23)*SK_BETA[2]); -Kfusion[4] = SK_BETA[0]*(P(4,0)*SK_BETA[5] + P(4,1)*SK_BETA[4] - P(4,4)*SK_BETA[1] + P(4,5)*SK_BETA[2] + P(4,2)*SK_BETA[6] + P(4,6)*SK_BETA[3] - P(4,3)*SK_BETA[7] + P(4,22)*SK_BETA[1] - P(4,23)*SK_BETA[2]); -Kfusion[5] = SK_BETA[0]*(P(5,0)*SK_BETA[5] + P(5,1)*SK_BETA[4] - P(5,4)*SK_BETA[1] + P(5,5)*SK_BETA[2] + P(5,2)*SK_BETA[6] + P(5,6)*SK_BETA[3] - P(5,3)*SK_BETA[7] + P(5,22)*SK_BETA[1] - P(5,23)*SK_BETA[2]); -Kfusion[6] = SK_BETA[0]*(P(6,0)*SK_BETA[5] + P(6,1)*SK_BETA[4] - P(6,4)*SK_BETA[1] + P(6,5)*SK_BETA[2] + P(6,2)*SK_BETA[6] + P(6,6)*SK_BETA[3] - P(6,3)*SK_BETA[7] + P(6,22)*SK_BETA[1] - P(6,23)*SK_BETA[2]); -Kfusion[7] = SK_BETA[0]*(P(7,0)*SK_BETA[5] + P(7,1)*SK_BETA[4] - P(7,4)*SK_BETA[1] + P(7,5)*SK_BETA[2] + P(7,2)*SK_BETA[6] + P(7,6)*SK_BETA[3] - P(7,3)*SK_BETA[7] + P(7,22)*SK_BETA[1] - P(7,23)*SK_BETA[2]); -Kfusion[8] = SK_BETA[0]*(P(8,0)*SK_BETA[5] + P(8,1)*SK_BETA[4] - P(8,4)*SK_BETA[1] + P(8,5)*SK_BETA[2] + P(8,2)*SK_BETA[6] + P(8,6)*SK_BETA[3] - P(8,3)*SK_BETA[7] + P(8,22)*SK_BETA[1] - P(8,23)*SK_BETA[2]); -Kfusion[9] = SK_BETA[0]*(P(9,0)*SK_BETA[5] + P(9,1)*SK_BETA[4] - P(9,4)*SK_BETA[1] + P(9,5)*SK_BETA[2] + P(9,2)*SK_BETA[6] + P(9,6)*SK_BETA[3] - P(9,3)*SK_BETA[7] + P(9,22)*SK_BETA[1] - P(9,23)*SK_BETA[2]); -Kfusion[10] = SK_BETA[0]*(P(10,0)*SK_BETA[5] + P(10,1)*SK_BETA[4] - P(10,4)*SK_BETA[1] + P(10,5)*SK_BETA[2] + P(10,2)*SK_BETA[6] + P(10,6)*SK_BETA[3] - P(10,3)*SK_BETA[7] + P(10,22)*SK_BETA[1] - P(10,23)*SK_BETA[2]); -Kfusion[11] = SK_BETA[0]*(P(11,0)*SK_BETA[5] + P(11,1)*SK_BETA[4] - P(11,4)*SK_BETA[1] + P(11,5)*SK_BETA[2] + P(11,2)*SK_BETA[6] + P(11,6)*SK_BETA[3] - P(11,3)*SK_BETA[7] + P(11,22)*SK_BETA[1] - P(11,23)*SK_BETA[2]); -Kfusion[12] = SK_BETA[0]*(P(12,0)*SK_BETA[5] + P(12,1)*SK_BETA[4] - P(12,4)*SK_BETA[1] + P(12,5)*SK_BETA[2] + P(12,2)*SK_BETA[6] + P(12,6)*SK_BETA[3] - P(12,3)*SK_BETA[7] + P(12,22)*SK_BETA[1] - P(12,23)*SK_BETA[2]); -Kfusion[13] = SK_BETA[0]*(P(13,0)*SK_BETA[5] + P(13,1)*SK_BETA[4] - P(13,4)*SK_BETA[1] + P(13,5)*SK_BETA[2] + P(13,2)*SK_BETA[6] + P(13,6)*SK_BETA[3] - P(13,3)*SK_BETA[7] + P(13,22)*SK_BETA[1] - P(13,23)*SK_BETA[2]); -Kfusion[14] = SK_BETA[0]*(P(14,0)*SK_BETA[5] + P(14,1)*SK_BETA[4] - P(14,4)*SK_BETA[1] + P(14,5)*SK_BETA[2] + P(14,2)*SK_BETA[6] + P(14,6)*SK_BETA[3] - P(14,3)*SK_BETA[7] + P(14,22)*SK_BETA[1] - P(14,23)*SK_BETA[2]); -Kfusion[15] = SK_BETA[0]*(P(15,0)*SK_BETA[5] + P(15,1)*SK_BETA[4] - P(15,4)*SK_BETA[1] + P(15,5)*SK_BETA[2] + P(15,2)*SK_BETA[6] + P(15,6)*SK_BETA[3] - P(15,3)*SK_BETA[7] + P(15,22)*SK_BETA[1] - P(15,23)*SK_BETA[2]); -Kfusion[16] = SK_BETA[0]*(P(16,0)*SK_BETA[5] + P(16,1)*SK_BETA[4] - P(16,4)*SK_BETA[1] + P(16,5)*SK_BETA[2] + P(16,2)*SK_BETA[6] + P(16,6)*SK_BETA[3] - P(16,3)*SK_BETA[7] + P(16,22)*SK_BETA[1] - P(16,23)*SK_BETA[2]); -Kfusion[17] = SK_BETA[0]*(P(17,0)*SK_BETA[5] + P(17,1)*SK_BETA[4] - P(17,4)*SK_BETA[1] + P(17,5)*SK_BETA[2] + P(17,2)*SK_BETA[6] + P(17,6)*SK_BETA[3] - P(17,3)*SK_BETA[7] + P(17,22)*SK_BETA[1] - P(17,23)*SK_BETA[2]); -Kfusion[18] = SK_BETA[0]*(P(18,0)*SK_BETA[5] + P(18,1)*SK_BETA[4] - P(18,4)*SK_BETA[1] + P(18,5)*SK_BETA[2] + P(18,2)*SK_BETA[6] + P(18,6)*SK_BETA[3] - P(18,3)*SK_BETA[7] + P(18,22)*SK_BETA[1] - P(18,23)*SK_BETA[2]); -Kfusion[19] = SK_BETA[0]*(P(19,0)*SK_BETA[5] + P(19,1)*SK_BETA[4] - P(19,4)*SK_BETA[1] + P(19,5)*SK_BETA[2] + P(19,2)*SK_BETA[6] + P(19,6)*SK_BETA[3] - P(19,3)*SK_BETA[7] + P(19,22)*SK_BETA[1] - P(19,23)*SK_BETA[2]); -Kfusion[20] = SK_BETA[0]*(P(20,0)*SK_BETA[5] + P(20,1)*SK_BETA[4] - P(20,4)*SK_BETA[1] + P(20,5)*SK_BETA[2] + P(20,2)*SK_BETA[6] + P(20,6)*SK_BETA[3] - P(20,3)*SK_BETA[7] + P(20,22)*SK_BETA[1] - P(20,23)*SK_BETA[2]); -Kfusion[21] = SK_BETA[0]*(P(21,0)*SK_BETA[5] + P(21,1)*SK_BETA[4] - P(21,4)*SK_BETA[1] + P(21,5)*SK_BETA[2] + P(21,2)*SK_BETA[6] + P(21,6)*SK_BETA[3] - P(21,3)*SK_BETA[7] + P(21,22)*SK_BETA[1] - P(21,23)*SK_BETA[2]); -Kfusion[22] = SK_BETA[0]*(P(22,0)*SK_BETA[5] + P(22,1)*SK_BETA[4] - P(22,4)*SK_BETA[1] + P(22,5)*SK_BETA[2] + P(22,2)*SK_BETA[6] + P(22,6)*SK_BETA[3] - P(22,3)*SK_BETA[7] + P(22,22)*SK_BETA[1] - P(22,23)*SK_BETA[2]); -Kfusion[23] = SK_BETA[0]*(P(23,0)*SK_BETA[5] + P(23,1)*SK_BETA[4] - P(23,4)*SK_BETA[1] + P(23,5)*SK_BETA[2] + P(23,2)*SK_BETA[6] + P(23,6)*SK_BETA[3] - P(23,3)*SK_BETA[7] + P(23,22)*SK_BETA[1] - P(23,23)*SK_BETA[2]); -float SH_MAG[9][1]; -SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; -SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; -SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; -SH_MAG[3] = sq(q3); -SH_MAG[4] = sq(q2); -SH_MAG[5] = sq(q1); -SH_MAG[6] = sq(q0); -SH_MAG[7] = 2*magN*q0; -SH_MAG[8] = 2*magE*q3; -float H_MAG[1][24]; -H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -H_MAG[1] = SH_MAG[0]; -H_MAG[2] = -SH_MAG[1]; -H_MAG[3] = SH_MAG[2]; -H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; -H_MAG[17] = 2*q0*q3 + 2*q1*q2; -H_MAG[18] = 2*q1*q3 - 2*q0*q2; -H_MAG[19] = 1; -float SK_MX[5][1]; -SK_MX[0] = 1/(P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2*q0*q3 + 2*q1*q2) - P(18,17)*(2*q0*q2 - 2*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2*q0*q3 + 2*q1*q2) - P(18,18)*(2*q0*q2 - 2*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2*q0*q3 + 2*q1*q2) - P(18,0)*(2*q0*q2 - 2*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P(17,19)*(2*q0*q3 + 2*q1*q2) - P(18,19)*(2*q0*q2 - 2*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2*q0*q3 + 2*q1*q2) - P(18,1)*(2*q0*q2 - 2*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2*q0*q3 + 2*q1*q2) - P(18,2)*(2*q0*q2 - 2*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2*q0*q3 + 2*q1*q2) - P(18,3)*(2*q0*q2 - 2*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2*q0*q3 + 2*q1*q2) - P(18,16)*(2*q0*q2 - 2*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); -SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; -SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -SK_MX[3] = 2*q0*q2 - 2*q1*q3; -SK_MX[4] = 2*q0*q3 + 2*q1*q2; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_MX[0]*(P(0,19) + P(0,1)*SH_MAG[0] - P(0,2)*SH_MAG[1] + P(0,3)*SH_MAG[2] + P(0,0)*SK_MX[2] - P(0,16)*SK_MX[1] + P(0,17)*SK_MX[4] - P(0,18)*SK_MX[3]); -Kfusion[1] = SK_MX[0]*(P(1,19) + P(1,1)*SH_MAG[0] - P(1,2)*SH_MAG[1] + P(1,3)*SH_MAG[2] + P(1,0)*SK_MX[2] - P(1,16)*SK_MX[1] + P(1,17)*SK_MX[4] - P(1,18)*SK_MX[3]); -Kfusion[2] = SK_MX[0]*(P(2,19) + P(2,1)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(2,3)*SH_MAG[2] + P(2,0)*SK_MX[2] - P(2,16)*SK_MX[1] + P(2,17)*SK_MX[4] - P(2,18)*SK_MX[3]); -Kfusion[3] = SK_MX[0]*(P(3,19) + P(3,1)*SH_MAG[0] - P(3,2)*SH_MAG[1] + P(3,3)*SH_MAG[2] + P(3,0)*SK_MX[2] - P(3,16)*SK_MX[1] + P(3,17)*SK_MX[4] - P(3,18)*SK_MX[3]); -Kfusion[4] = SK_MX[0]*(P(4,19) + P(4,1)*SH_MAG[0] - P(4,2)*SH_MAG[1] + P(4,3)*SH_MAG[2] + P(4,0)*SK_MX[2] - P(4,16)*SK_MX[1] + P(4,17)*SK_MX[4] - P(4,18)*SK_MX[3]); -Kfusion[5] = SK_MX[0]*(P(5,19) + P(5,1)*SH_MAG[0] - P(5,2)*SH_MAG[1] + P(5,3)*SH_MAG[2] + P(5,0)*SK_MX[2] - P(5,16)*SK_MX[1] + P(5,17)*SK_MX[4] - P(5,18)*SK_MX[3]); -Kfusion[6] = SK_MX[0]*(P(6,19) + P(6,1)*SH_MAG[0] - P(6,2)*SH_MAG[1] + P(6,3)*SH_MAG[2] + P(6,0)*SK_MX[2] - P(6,16)*SK_MX[1] + P(6,17)*SK_MX[4] - P(6,18)*SK_MX[3]); -Kfusion[7] = SK_MX[0]*(P(7,19) + P(7,1)*SH_MAG[0] - P(7,2)*SH_MAG[1] + P(7,3)*SH_MAG[2] + P(7,0)*SK_MX[2] - P(7,16)*SK_MX[1] + P(7,17)*SK_MX[4] - P(7,18)*SK_MX[3]); -Kfusion[8] = SK_MX[0]*(P(8,19) + P(8,1)*SH_MAG[0] - P(8,2)*SH_MAG[1] + P(8,3)*SH_MAG[2] + P(8,0)*SK_MX[2] - P(8,16)*SK_MX[1] + P(8,17)*SK_MX[4] - P(8,18)*SK_MX[3]); -Kfusion[9] = SK_MX[0]*(P(9,19) + P(9,1)*SH_MAG[0] - P(9,2)*SH_MAG[1] + P(9,3)*SH_MAG[2] + P(9,0)*SK_MX[2] - P(9,16)*SK_MX[1] + P(9,17)*SK_MX[4] - P(9,18)*SK_MX[3]); -Kfusion[10] = SK_MX[0]*(P(10,19) + P(10,1)*SH_MAG[0] - P(10,2)*SH_MAG[1] + P(10,3)*SH_MAG[2] + P(10,0)*SK_MX[2] - P(10,16)*SK_MX[1] + P(10,17)*SK_MX[4] - P(10,18)*SK_MX[3]); -Kfusion[11] = SK_MX[0]*(P(11,19) + P(11,1)*SH_MAG[0] - P(11,2)*SH_MAG[1] + P(11,3)*SH_MAG[2] + P(11,0)*SK_MX[2] - P(11,16)*SK_MX[1] + P(11,17)*SK_MX[4] - P(11,18)*SK_MX[3]); -Kfusion[12] = SK_MX[0]*(P(12,19) + P(12,1)*SH_MAG[0] - P(12,2)*SH_MAG[1] + P(12,3)*SH_MAG[2] + P(12,0)*SK_MX[2] - P(12,16)*SK_MX[1] + P(12,17)*SK_MX[4] - P(12,18)*SK_MX[3]); -Kfusion[13] = SK_MX[0]*(P(13,19) + P(13,1)*SH_MAG[0] - P(13,2)*SH_MAG[1] + P(13,3)*SH_MAG[2] + P(13,0)*SK_MX[2] - P(13,16)*SK_MX[1] + P(13,17)*SK_MX[4] - P(13,18)*SK_MX[3]); -Kfusion[14] = SK_MX[0]*(P(14,19) + P(14,1)*SH_MAG[0] - P(14,2)*SH_MAG[1] + P(14,3)*SH_MAG[2] + P(14,0)*SK_MX[2] - P(14,16)*SK_MX[1] + P(14,17)*SK_MX[4] - P(14,18)*SK_MX[3]); -Kfusion[15] = SK_MX[0]*(P(15,19) + P(15,1)*SH_MAG[0] - P(15,2)*SH_MAG[1] + P(15,3)*SH_MAG[2] + P(15,0)*SK_MX[2] - P(15,16)*SK_MX[1] + P(15,17)*SK_MX[4] - P(15,18)*SK_MX[3]); -Kfusion[16] = SK_MX[0]*(P(16,19) + P(16,1)*SH_MAG[0] - P(16,2)*SH_MAG[1] + P(16,3)*SH_MAG[2] + P(16,0)*SK_MX[2] - P(16,16)*SK_MX[1] + P(16,17)*SK_MX[4] - P(16,18)*SK_MX[3]); -Kfusion[17] = SK_MX[0]*(P(17,19) + P(17,1)*SH_MAG[0] - P(17,2)*SH_MAG[1] + P(17,3)*SH_MAG[2] + P(17,0)*SK_MX[2] - P(17,16)*SK_MX[1] + P(17,17)*SK_MX[4] - P(17,18)*SK_MX[3]); -Kfusion[18] = SK_MX[0]*(P(18,19) + P(18,1)*SH_MAG[0] - P(18,2)*SH_MAG[1] + P(18,3)*SH_MAG[2] + P(18,0)*SK_MX[2] - P(18,16)*SK_MX[1] + P(18,17)*SK_MX[4] - P(18,18)*SK_MX[3]); -Kfusion[19] = SK_MX[0]*(P(19,19) + P(19,1)*SH_MAG[0] - P(19,2)*SH_MAG[1] + P(19,3)*SH_MAG[2] + P(19,0)*SK_MX[2] - P(19,16)*SK_MX[1] + P(19,17)*SK_MX[4] - P(19,18)*SK_MX[3]); -Kfusion[20] = SK_MX[0]*(P(20,19) + P(20,1)*SH_MAG[0] - P(20,2)*SH_MAG[1] + P(20,3)*SH_MAG[2] + P(20,0)*SK_MX[2] - P(20,16)*SK_MX[1] + P(20,17)*SK_MX[4] - P(20,18)*SK_MX[3]); -Kfusion[21] = SK_MX[0]*(P(21,19) + P(21,1)*SH_MAG[0] - P(21,2)*SH_MAG[1] + P(21,3)*SH_MAG[2] + P(21,0)*SK_MX[2] - P(21,16)*SK_MX[1] + P(21,17)*SK_MX[4] - P(21,18)*SK_MX[3]); -Kfusion[22] = SK_MX[0]*(P(22,19) + P(22,1)*SH_MAG[0] - P(22,2)*SH_MAG[1] + P(22,3)*SH_MAG[2] + P(22,0)*SK_MX[2] - P(22,16)*SK_MX[1] + P(22,17)*SK_MX[4] - P(22,18)*SK_MX[3]); -Kfusion[23] = SK_MX[0]*(P(23,19) + P(23,1)*SH_MAG[0] - P(23,2)*SH_MAG[1] + P(23,3)*SH_MAG[2] + P(23,0)*SK_MX[2] - P(23,16)*SK_MX[1] + P(23,17)*SK_MX[4] - P(23,18)*SK_MX[3]); -float H_MAG[1][24]; -H_MAG[0] = SH_MAG[2]; -H_MAG[1] = SH_MAG[1]; -H_MAG[2] = SH_MAG[0]; -H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; -H_MAG[16] = 2*q1*q2 - 2*q0*q3; -H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; -H_MAG[18] = 2*q0*q1 + 2*q2*q3; -H_MAG[20] = 1; -float SK_MY[5][1]; -SK_MY[0] = 1/(P(20,20) + R_MAG + P(0,20)*SH_MAG[2] + P(1,20)*SH_MAG[1] + P(2,20)*SH_MAG[0] - P(17,20)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P(20,16) + P(0,16)*SH_MAG[2] + P(1,16)*SH_MAG[1] + P(2,16)*SH_MAG[0] - P(17,16)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,16)*(2*q0*q3 - 2*q1*q2) + P(18,16)*(2*q0*q1 + 2*q2*q3) - P(3,16)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P(20,18) + P(0,18)*SH_MAG[2] + P(1,18)*SH_MAG[1] + P(2,18)*SH_MAG[0] - P(17,18)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,18)*(2*q0*q3 - 2*q1*q2) + P(18,18)*(2*q0*q1 + 2*q2*q3) - P(3,18)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P(20,3) + P(0,3)*SH_MAG[2] + P(1,3)*SH_MAG[1] + P(2,3)*SH_MAG[0] - P(17,3)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,3)*(2*q0*q3 - 2*q1*q2) + P(18,3)*(2*q0*q1 + 2*q2*q3) - P(3,3)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P(16,20)*(2*q0*q3 - 2*q1*q2) + P(18,20)*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P(20,0) + P(0,0)*SH_MAG[2] + P(1,0)*SH_MAG[1] + P(2,0)*SH_MAG[0] - P(17,0)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,0)*(2*q0*q3 - 2*q1*q2) + P(18,0)*(2*q0*q1 + 2*q2*q3) - P(3,0)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P(20,1) + P(0,1)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(2,1)*SH_MAG[0] - P(17,1)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,1)*(2*q0*q3 - 2*q1*q2) + P(18,1)*(2*q0*q1 + 2*q2*q3) - P(3,1)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P(20,2) + P(0,2)*SH_MAG[2] + P(1,2)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(17,2)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,2)*(2*q0*q3 - 2*q1*q2) + P(18,2)*(2*q0*q1 + 2*q2*q3) - P(3,2)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P(20,17) + P(0,17)*SH_MAG[2] + P(1,17)*SH_MAG[1] + P(2,17)*SH_MAG[0] - P(17,17)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,17)*(2*q0*q3 - 2*q1*q2) + P(18,17)*(2*q0*q1 + 2*q2*q3) - P(3,17)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P(3,20)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); -SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; -SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -SK_MY[3] = 2*q0*q3 - 2*q1*q2; -SK_MY[4] = 2*q0*q1 + 2*q2*q3; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_MY[0]*(P(0,20) + P(0,0)*SH_MAG[2] + P(0,1)*SH_MAG[1] + P(0,2)*SH_MAG[0] - P(0,3)*SK_MY[2] - P(0,17)*SK_MY[1] - P(0,16)*SK_MY[3] + P(0,18)*SK_MY[4]); -Kfusion[1] = SK_MY[0]*(P(1,20) + P(1,0)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(1,2)*SH_MAG[0] - P(1,3)*SK_MY[2] - P(1,17)*SK_MY[1] - P(1,16)*SK_MY[3] + P(1,18)*SK_MY[4]); -Kfusion[2] = SK_MY[0]*(P(2,20) + P(2,0)*SH_MAG[2] + P(2,1)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(2,3)*SK_MY[2] - P(2,17)*SK_MY[1] - P(2,16)*SK_MY[3] + P(2,18)*SK_MY[4]); -Kfusion[3] = SK_MY[0]*(P(3,20) + P(3,0)*SH_MAG[2] + P(3,1)*SH_MAG[1] + P(3,2)*SH_MAG[0] - P(3,3)*SK_MY[2] - P(3,17)*SK_MY[1] - P(3,16)*SK_MY[3] + P(3,18)*SK_MY[4]); -Kfusion[4] = SK_MY[0]*(P(4,20) + P(4,0)*SH_MAG[2] + P(4,1)*SH_MAG[1] + P(4,2)*SH_MAG[0] - P(4,3)*SK_MY[2] - P(4,17)*SK_MY[1] - P(4,16)*SK_MY[3] + P(4,18)*SK_MY[4]); -Kfusion[5] = SK_MY[0]*(P(5,20) + P(5,0)*SH_MAG[2] + P(5,1)*SH_MAG[1] + P(5,2)*SH_MAG[0] - P(5,3)*SK_MY[2] - P(5,17)*SK_MY[1] - P(5,16)*SK_MY[3] + P(5,18)*SK_MY[4]); -Kfusion[6] = SK_MY[0]*(P(6,20) + P(6,0)*SH_MAG[2] + P(6,1)*SH_MAG[1] + P(6,2)*SH_MAG[0] - P(6,3)*SK_MY[2] - P(6,17)*SK_MY[1] - P(6,16)*SK_MY[3] + P(6,18)*SK_MY[4]); -Kfusion[7] = SK_MY[0]*(P(7,20) + P(7,0)*SH_MAG[2] + P(7,1)*SH_MAG[1] + P(7,2)*SH_MAG[0] - P(7,3)*SK_MY[2] - P(7,17)*SK_MY[1] - P(7,16)*SK_MY[3] + P(7,18)*SK_MY[4]); -Kfusion[8] = SK_MY[0]*(P(8,20) + P(8,0)*SH_MAG[2] + P(8,1)*SH_MAG[1] + P(8,2)*SH_MAG[0] - P(8,3)*SK_MY[2] - P(8,17)*SK_MY[1] - P(8,16)*SK_MY[3] + P(8,18)*SK_MY[4]); -Kfusion[9] = SK_MY[0]*(P(9,20) + P(9,0)*SH_MAG[2] + P(9,1)*SH_MAG[1] + P(9,2)*SH_MAG[0] - P(9,3)*SK_MY[2] - P(9,17)*SK_MY[1] - P(9,16)*SK_MY[3] + P(9,18)*SK_MY[4]); -Kfusion[10] = SK_MY[0]*(P(10,20) + P(10,0)*SH_MAG[2] + P(10,1)*SH_MAG[1] + P(10,2)*SH_MAG[0] - P(10,3)*SK_MY[2] - P(10,17)*SK_MY[1] - P(10,16)*SK_MY[3] + P(10,18)*SK_MY[4]); -Kfusion[11] = SK_MY[0]*(P(11,20) + P(11,0)*SH_MAG[2] + P(11,1)*SH_MAG[1] + P(11,2)*SH_MAG[0] - P(11,3)*SK_MY[2] - P(11,17)*SK_MY[1] - P(11,16)*SK_MY[3] + P(11,18)*SK_MY[4]); -Kfusion[12] = SK_MY[0]*(P(12,20) + P(12,0)*SH_MAG[2] + P(12,1)*SH_MAG[1] + P(12,2)*SH_MAG[0] - P(12,3)*SK_MY[2] - P(12,17)*SK_MY[1] - P(12,16)*SK_MY[3] + P(12,18)*SK_MY[4]); -Kfusion[13] = SK_MY[0]*(P(13,20) + P(13,0)*SH_MAG[2] + P(13,1)*SH_MAG[1] + P(13,2)*SH_MAG[0] - P(13,3)*SK_MY[2] - P(13,17)*SK_MY[1] - P(13,16)*SK_MY[3] + P(13,18)*SK_MY[4]); -Kfusion[14] = SK_MY[0]*(P(14,20) + P(14,0)*SH_MAG[2] + P(14,1)*SH_MAG[1] + P(14,2)*SH_MAG[0] - P(14,3)*SK_MY[2] - P(14,17)*SK_MY[1] - P(14,16)*SK_MY[3] + P(14,18)*SK_MY[4]); -Kfusion[15] = SK_MY[0]*(P(15,20) + P(15,0)*SH_MAG[2] + P(15,1)*SH_MAG[1] + P(15,2)*SH_MAG[0] - P(15,3)*SK_MY[2] - P(15,17)*SK_MY[1] - P(15,16)*SK_MY[3] + P(15,18)*SK_MY[4]); -Kfusion[16] = SK_MY[0]*(P(16,20) + P(16,0)*SH_MAG[2] + P(16,1)*SH_MAG[1] + P(16,2)*SH_MAG[0] - P(16,3)*SK_MY[2] - P(16,17)*SK_MY[1] - P(16,16)*SK_MY[3] + P(16,18)*SK_MY[4]); -Kfusion[17] = SK_MY[0]*(P(17,20) + P(17,0)*SH_MAG[2] + P(17,1)*SH_MAG[1] + P(17,2)*SH_MAG[0] - P(17,3)*SK_MY[2] - P(17,17)*SK_MY[1] - P(17,16)*SK_MY[3] + P(17,18)*SK_MY[4]); -Kfusion[18] = SK_MY[0]*(P(18,20) + P(18,0)*SH_MAG[2] + P(18,1)*SH_MAG[1] + P(18,2)*SH_MAG[0] - P(18,3)*SK_MY[2] - P(18,17)*SK_MY[1] - P(18,16)*SK_MY[3] + P(18,18)*SK_MY[4]); -Kfusion[19] = SK_MY[0]*(P(19,20) + P(19,0)*SH_MAG[2] + P(19,1)*SH_MAG[1] + P(19,2)*SH_MAG[0] - P(19,3)*SK_MY[2] - P(19,17)*SK_MY[1] - P(19,16)*SK_MY[3] + P(19,18)*SK_MY[4]); -Kfusion[20] = SK_MY[0]*(P(20,20) + P(20,0)*SH_MAG[2] + P(20,1)*SH_MAG[1] + P(20,2)*SH_MAG[0] - P(20,3)*SK_MY[2] - P(20,17)*SK_MY[1] - P(20,16)*SK_MY[3] + P(20,18)*SK_MY[4]); -Kfusion[21] = SK_MY[0]*(P(21,20) + P(21,0)*SH_MAG[2] + P(21,1)*SH_MAG[1] + P(21,2)*SH_MAG[0] - P(21,3)*SK_MY[2] - P(21,17)*SK_MY[1] - P(21,16)*SK_MY[3] + P(21,18)*SK_MY[4]); -Kfusion[22] = SK_MY[0]*(P(22,20) + P(22,0)*SH_MAG[2] + P(22,1)*SH_MAG[1] + P(22,2)*SH_MAG[0] - P(22,3)*SK_MY[2] - P(22,17)*SK_MY[1] - P(22,16)*SK_MY[3] + P(22,18)*SK_MY[4]); -Kfusion[23] = SK_MY[0]*(P(23,20) + P(23,0)*SH_MAG[2] + P(23,1)*SH_MAG[1] + P(23,2)*SH_MAG[0] - P(23,3)*SK_MY[2] - P(23,17)*SK_MY[1] - P(23,16)*SK_MY[3] + P(23,18)*SK_MY[4]); -float H_MAG[1][24]; -H_MAG[0] = SH_MAG[1]; -H_MAG[1] = -SH_MAG[2]; -H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -H_MAG[3] = SH_MAG[0]; -H_MAG[16] = 2*q0*q2 + 2*q1*q3; -H_MAG[17] = 2*q2*q3 - 2*q0*q1; -H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; -H_MAG[21] = 1; -float SK_MZ[5][1]; -SK_MZ[0] = 1/(P(21,21) + R_MAG + P(0,21)*SH_MAG[1] - P(1,21)*SH_MAG[2] + P(3,21)*SH_MAG[0] + P(18,21)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2*q1*q3)*(P(21,16) + P(0,16)*SH_MAG[1] - P(1,16)*SH_MAG[2] + P(3,16)*SH_MAG[0] + P(18,16)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,16)*(2*q0*q2 + 2*q1*q3) - P(17,16)*(2*q0*q1 - 2*q2*q3) + P(2,16)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P(21,17) + P(0,17)*SH_MAG[1] - P(1,17)*SH_MAG[2] + P(3,17)*SH_MAG[0] + P(18,17)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,17)*(2*q0*q2 + 2*q1*q3) - P(17,17)*(2*q0*q1 - 2*q2*q3) + P(2,17)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P(21,2) + P(0,2)*SH_MAG[1] - P(1,2)*SH_MAG[2] + P(3,2)*SH_MAG[0] + P(18,2)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,2)*(2*q0*q2 + 2*q1*q3) - P(17,2)*(2*q0*q1 - 2*q2*q3) + P(2,2)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P(16,21)*(2*q0*q2 + 2*q1*q3) - P(17,21)*(2*q0*q1 - 2*q2*q3) + SH_MAG[1]*(P(21,0) + P(0,0)*SH_MAG[1] - P(1,0)*SH_MAG[2] + P(3,0)*SH_MAG[0] + P(18,0)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,0)*(2*q0*q2 + 2*q1*q3) - P(17,0)*(2*q0*q1 - 2*q2*q3) + P(2,0)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[2]*(P(21,1) + P(0,1)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(3,1)*SH_MAG[0] + P(18,1)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,1)*(2*q0*q2 + 2*q1*q3) - P(17,1)*(2*q0*q1 - 2*q2*q3) + P(2,1)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P(21,3) + P(0,3)*SH_MAG[1] - P(1,3)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(18,3)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,3)*(2*q0*q2 + 2*q1*q3) - P(17,3)*(2*q0*q1 - 2*q2*q3) + P(2,3)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P(21,18) + P(0,18)*SH_MAG[1] - P(1,18)*SH_MAG[2] + P(3,18)*SH_MAG[0] + P(18,18)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,18)*(2*q0*q2 + 2*q1*q3) - P(17,18)*(2*q0*q1 - 2*q2*q3) + P(2,18)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P(2,21)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); -SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; -SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; -SK_MZ[3] = 2*q0*q1 - 2*q2*q3; -SK_MZ[4] = 2*q0*q2 + 2*q1*q3; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = SK_MZ[0]*(P(0,21) + P(0,0)*SH_MAG[1] - P(0,1)*SH_MAG[2] + P(0,3)*SH_MAG[0] + P(0,2)*SK_MZ[2] + P(0,18)*SK_MZ[1] + P(0,16)*SK_MZ[4] - P(0,17)*SK_MZ[3]); -Kfusion[1] = SK_MZ[0]*(P(1,21) + P(1,0)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(1,3)*SH_MAG[0] + P(1,2)*SK_MZ[2] + P(1,18)*SK_MZ[1] + P(1,16)*SK_MZ[4] - P(1,17)*SK_MZ[3]); -Kfusion[2] = SK_MZ[0]*(P(2,21) + P(2,0)*SH_MAG[1] - P(2,1)*SH_MAG[2] + P(2,3)*SH_MAG[0] + P(2,2)*SK_MZ[2] + P(2,18)*SK_MZ[1] + P(2,16)*SK_MZ[4] - P(2,17)*SK_MZ[3]); -Kfusion[3] = SK_MZ[0]*(P(3,21) + P(3,0)*SH_MAG[1] - P(3,1)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(3,2)*SK_MZ[2] + P(3,18)*SK_MZ[1] + P(3,16)*SK_MZ[4] - P(3,17)*SK_MZ[3]); -Kfusion[4] = SK_MZ[0]*(P(4,21) + P(4,0)*SH_MAG[1] - P(4,1)*SH_MAG[2] + P(4,3)*SH_MAG[0] + P(4,2)*SK_MZ[2] + P(4,18)*SK_MZ[1] + P(4,16)*SK_MZ[4] - P(4,17)*SK_MZ[3]); -Kfusion[5] = SK_MZ[0]*(P(5,21) + P(5,0)*SH_MAG[1] - P(5,1)*SH_MAG[2] + P(5,3)*SH_MAG[0] + P(5,2)*SK_MZ[2] + P(5,18)*SK_MZ[1] + P(5,16)*SK_MZ[4] - P(5,17)*SK_MZ[3]); -Kfusion[6] = SK_MZ[0]*(P(6,21) + P(6,0)*SH_MAG[1] - P(6,1)*SH_MAG[2] + P(6,3)*SH_MAG[0] + P(6,2)*SK_MZ[2] + P(6,18)*SK_MZ[1] + P(6,16)*SK_MZ[4] - P(6,17)*SK_MZ[3]); -Kfusion[7] = SK_MZ[0]*(P(7,21) + P(7,0)*SH_MAG[1] - P(7,1)*SH_MAG[2] + P(7,3)*SH_MAG[0] + P(7,2)*SK_MZ[2] + P(7,18)*SK_MZ[1] + P(7,16)*SK_MZ[4] - P(7,17)*SK_MZ[3]); -Kfusion[8] = SK_MZ[0]*(P(8,21) + P(8,0)*SH_MAG[1] - P(8,1)*SH_MAG[2] + P(8,3)*SH_MAG[0] + P(8,2)*SK_MZ[2] + P(8,18)*SK_MZ[1] + P(8,16)*SK_MZ[4] - P(8,17)*SK_MZ[3]); -Kfusion[9] = SK_MZ[0]*(P(9,21) + P(9,0)*SH_MAG[1] - P(9,1)*SH_MAG[2] + P(9,3)*SH_MAG[0] + P(9,2)*SK_MZ[2] + P(9,18)*SK_MZ[1] + P(9,16)*SK_MZ[4] - P(9,17)*SK_MZ[3]); -Kfusion[10] = SK_MZ[0]*(P(10,21) + P(10,0)*SH_MAG[1] - P(10,1)*SH_MAG[2] + P(10,3)*SH_MAG[0] + P(10,2)*SK_MZ[2] + P(10,18)*SK_MZ[1] + P(10,16)*SK_MZ[4] - P(10,17)*SK_MZ[3]); -Kfusion[11] = SK_MZ[0]*(P(11,21) + P(11,0)*SH_MAG[1] - P(11,1)*SH_MAG[2] + P(11,3)*SH_MAG[0] + P(11,2)*SK_MZ[2] + P(11,18)*SK_MZ[1] + P(11,16)*SK_MZ[4] - P(11,17)*SK_MZ[3]); -Kfusion[12] = SK_MZ[0]*(P(12,21) + P(12,0)*SH_MAG[1] - P(12,1)*SH_MAG[2] + P(12,3)*SH_MAG[0] + P(12,2)*SK_MZ[2] + P(12,18)*SK_MZ[1] + P(12,16)*SK_MZ[4] - P(12,17)*SK_MZ[3]); -Kfusion[13] = SK_MZ[0]*(P(13,21) + P(13,0)*SH_MAG[1] - P(13,1)*SH_MAG[2] + P(13,3)*SH_MAG[0] + P(13,2)*SK_MZ[2] + P(13,18)*SK_MZ[1] + P(13,16)*SK_MZ[4] - P(13,17)*SK_MZ[3]); -Kfusion[14] = SK_MZ[0]*(P(14,21) + P(14,0)*SH_MAG[1] - P(14,1)*SH_MAG[2] + P(14,3)*SH_MAG[0] + P(14,2)*SK_MZ[2] + P(14,18)*SK_MZ[1] + P(14,16)*SK_MZ[4] - P(14,17)*SK_MZ[3]); -Kfusion[15] = SK_MZ[0]*(P(15,21) + P(15,0)*SH_MAG[1] - P(15,1)*SH_MAG[2] + P(15,3)*SH_MAG[0] + P(15,2)*SK_MZ[2] + P(15,18)*SK_MZ[1] + P(15,16)*SK_MZ[4] - P(15,17)*SK_MZ[3]); -Kfusion[16] = SK_MZ[0]*(P(16,21) + P(16,0)*SH_MAG[1] - P(16,1)*SH_MAG[2] + P(16,3)*SH_MAG[0] + P(16,2)*SK_MZ[2] + P(16,18)*SK_MZ[1] + P(16,16)*SK_MZ[4] - P(16,17)*SK_MZ[3]); -Kfusion[17] = SK_MZ[0]*(P(17,21) + P(17,0)*SH_MAG[1] - P(17,1)*SH_MAG[2] + P(17,3)*SH_MAG[0] + P(17,2)*SK_MZ[2] + P(17,18)*SK_MZ[1] + P(17,16)*SK_MZ[4] - P(17,17)*SK_MZ[3]); -Kfusion[18] = SK_MZ[0]*(P(18,21) + P(18,0)*SH_MAG[1] - P(18,1)*SH_MAG[2] + P(18,3)*SH_MAG[0] + P(18,2)*SK_MZ[2] + P(18,18)*SK_MZ[1] + P(18,16)*SK_MZ[4] - P(18,17)*SK_MZ[3]); -Kfusion[19] = SK_MZ[0]*(P(19,21) + P(19,0)*SH_MAG[1] - P(19,1)*SH_MAG[2] + P(19,3)*SH_MAG[0] + P(19,2)*SK_MZ[2] + P(19,18)*SK_MZ[1] + P(19,16)*SK_MZ[4] - P(19,17)*SK_MZ[3]); -Kfusion[20] = SK_MZ[0]*(P(20,21) + P(20,0)*SH_MAG[1] - P(20,1)*SH_MAG[2] + P(20,3)*SH_MAG[0] + P(20,2)*SK_MZ[2] + P(20,18)*SK_MZ[1] + P(20,16)*SK_MZ[4] - P(20,17)*SK_MZ[3]); -Kfusion[21] = SK_MZ[0]*(P(21,21) + P(21,0)*SH_MAG[1] - P(21,1)*SH_MAG[2] + P(21,3)*SH_MAG[0] + P(21,2)*SK_MZ[2] + P(21,18)*SK_MZ[1] + P(21,16)*SK_MZ[4] - P(21,17)*SK_MZ[3]); -Kfusion[22] = SK_MZ[0]*(P(22,21) + P(22,0)*SH_MAG[1] - P(22,1)*SH_MAG[2] + P(22,3)*SH_MAG[0] + P(22,2)*SK_MZ[2] + P(22,18)*SK_MZ[1] + P(22,16)*SK_MZ[4] - P(22,17)*SK_MZ[3]); -Kfusion[23] = SK_MZ[0]*(P(23,21) + P(23,0)*SH_MAG[1] - P(23,1)*SH_MAG[2] + P(23,3)*SH_MAG[0] + P(23,2)*SK_MZ[2] + P(23,18)*SK_MZ[1] + P(23,16)*SK_MZ[4] - P(23,17)*SK_MZ[3]); -float SH_ACCX[4][1]; -SH_ACCX[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_ACCX[1] = vn - vwn; -SH_ACCX[2] = ve - vwe; -SH_ACCX[3] = 2*q0*q3 + 2*q1*q2; -float H_ACCX[1][24]; -H_ACCX[0][0] = -Kaccx*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd); -H_ACCX[0][1] = -Kaccx*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd); -H_ACCX[0][2] = Kaccx*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd); -H_ACCX[0][3] = -Kaccx*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd); -H_ACCX[0][4] = -Kaccx*SH_ACCX[0]; -H_ACCX[0][5] = -Kaccx*SH_ACCX[3]; -H_ACCX[0][6] = Kaccx*(2*q0*q2 - 2*q1*q3); -H_ACCX[0][22] = Kaccx*SH_ACCX[0]; -H_ACCX[0][23] = Kaccx*SH_ACCX[3]; -float SK_ACCX[7][1]; -SK_ACCX[0] = 1/(R_ACC + Kaccx*SH_ACCX[0]*(Kaccx*P(4,4)*SH_ACCX[0] + Kaccx*P(5,4)*SH_ACCX[3] - Kaccx*P(22,4)*SH_ACCX[0] - Kaccx*P(23,4)*SH_ACCX[3] - Kaccx*P(6,4)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,4)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,4)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,4)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,4)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*SH_ACCX[3]*(Kaccx*P(4,5)*SH_ACCX[0] + Kaccx*P(5,5)*SH_ACCX[3] - Kaccx*P(22,5)*SH_ACCX[0] - Kaccx*P(23,5)*SH_ACCX[3] - Kaccx*P(6,5)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,5)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,5)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,5)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,5)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*SH_ACCX[0]*(Kaccx*P(4,22)*SH_ACCX[0] + Kaccx*P(5,22)*SH_ACCX[3] - Kaccx*P(22,22)*SH_ACCX[0] - Kaccx*P(23,22)*SH_ACCX[3] - Kaccx*P(6,22)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,22)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,22)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,22)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,22)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*SH_ACCX[3]*(Kaccx*P(4,23)*SH_ACCX[0] + Kaccx*P(5,23)*SH_ACCX[3] - Kaccx*P(22,23)*SH_ACCX[0] - Kaccx*P(23,23)*SH_ACCX[3] - Kaccx*P(6,23)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,23)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,23)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,23)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,23)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*P(4,6)*SH_ACCX[0] + Kaccx*P(5,6)*SH_ACCX[3] - Kaccx*P(22,6)*SH_ACCX[0] - Kaccx*P(23,6)*SH_ACCX[3] - Kaccx*P(6,6)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,6)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,6)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,6)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,6)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd)*(Kaccx*P(4,0)*SH_ACCX[0] + Kaccx*P(5,0)*SH_ACCX[3] - Kaccx*P(22,0)*SH_ACCX[0] - Kaccx*P(23,0)*SH_ACCX[3] - Kaccx*P(6,0)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,0)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,0)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,0)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,0)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd)*(Kaccx*P(4,1)*SH_ACCX[0] + Kaccx*P(5,1)*SH_ACCX[3] - Kaccx*P(22,1)*SH_ACCX[0] - Kaccx*P(23,1)*SH_ACCX[3] - Kaccx*P(6,1)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,1)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,1)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,1)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,1)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd)*(Kaccx*P(4,2)*SH_ACCX[0] + Kaccx*P(5,2)*SH_ACCX[3] - Kaccx*P(22,2)*SH_ACCX[0] - Kaccx*P(23,2)*SH_ACCX[3] - Kaccx*P(6,2)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,2)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,2)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,2)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,2)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)*(Kaccx*P(4,3)*SH_ACCX[0] + Kaccx*P(5,3)*SH_ACCX[3] - Kaccx*P(22,3)*SH_ACCX[0] - Kaccx*P(23,3)*SH_ACCX[3] - Kaccx*P(6,3)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,3)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,3)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,3)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,3)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd))); -SK_ACCX[1] = 2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd; -SK_ACCX[2] = 2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd; -SK_ACCX[3] = 2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd; -SK_ACCX[4] = 2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd; -SK_ACCX[5] = 2*q0*q2 - 2*q1*q3; -SK_ACCX[6] = SH_ACCX[3]; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = -SK_ACCX[0]*(Kaccx*P(0,4)*SH_ACCX[0] - Kaccx*P(0,22)*SH_ACCX[0] + Kaccx*P(0,0)*SK_ACCX[3] - Kaccx*P(0,2)*SK_ACCX[2] + Kaccx*P(0,3)*SK_ACCX[1] + Kaccx*P(0,1)*SK_ACCX[4] + Kaccx*P(0,5)*SK_ACCX[6] - Kaccx*P(0,6)*SK_ACCX[5] - Kaccx*P(0,23)*SK_ACCX[6]); -Kfusion[1] = -SK_ACCX[0]*(Kaccx*P(1,4)*SH_ACCX[0] - Kaccx*P(1,22)*SH_ACCX[0] + Kaccx*P(1,0)*SK_ACCX[3] - Kaccx*P(1,2)*SK_ACCX[2] + Kaccx*P(1,3)*SK_ACCX[1] + Kaccx*P(1,1)*SK_ACCX[4] + Kaccx*P(1,5)*SK_ACCX[6] - Kaccx*P(1,6)*SK_ACCX[5] - Kaccx*P(1,23)*SK_ACCX[6]); -Kfusion[2] = -SK_ACCX[0]*(Kaccx*P(2,4)*SH_ACCX[0] - Kaccx*P(2,22)*SH_ACCX[0] + Kaccx*P(2,0)*SK_ACCX[3] - Kaccx*P(2,2)*SK_ACCX[2] + Kaccx*P(2,3)*SK_ACCX[1] + Kaccx*P(2,1)*SK_ACCX[4] + Kaccx*P(2,5)*SK_ACCX[6] - Kaccx*P(2,6)*SK_ACCX[5] - Kaccx*P(2,23)*SK_ACCX[6]); -Kfusion[3] = -SK_ACCX[0]*(Kaccx*P(3,4)*SH_ACCX[0] - Kaccx*P(3,22)*SH_ACCX[0] + Kaccx*P(3,0)*SK_ACCX[3] - Kaccx*P(3,2)*SK_ACCX[2] + Kaccx*P(3,3)*SK_ACCX[1] + Kaccx*P(3,1)*SK_ACCX[4] + Kaccx*P(3,5)*SK_ACCX[6] - Kaccx*P(3,6)*SK_ACCX[5] - Kaccx*P(3,23)*SK_ACCX[6]); -Kfusion[4] = -SK_ACCX[0]*(Kaccx*P(4,4)*SH_ACCX[0] - Kaccx*P(4,22)*SH_ACCX[0] + Kaccx*P(4,0)*SK_ACCX[3] - Kaccx*P(4,2)*SK_ACCX[2] + Kaccx*P(4,3)*SK_ACCX[1] + Kaccx*P(4,1)*SK_ACCX[4] + Kaccx*P(4,5)*SK_ACCX[6] - Kaccx*P(4,6)*SK_ACCX[5] - Kaccx*P(4,23)*SK_ACCX[6]); -Kfusion[5] = -SK_ACCX[0]*(Kaccx*P(5,4)*SH_ACCX[0] - Kaccx*P(5,22)*SH_ACCX[0] + Kaccx*P(5,0)*SK_ACCX[3] - Kaccx*P(5,2)*SK_ACCX[2] + Kaccx*P(5,3)*SK_ACCX[1] + Kaccx*P(5,1)*SK_ACCX[4] + Kaccx*P(5,5)*SK_ACCX[6] - Kaccx*P(5,6)*SK_ACCX[5] - Kaccx*P(5,23)*SK_ACCX[6]); -Kfusion[6] = -SK_ACCX[0]*(Kaccx*P(6,4)*SH_ACCX[0] - Kaccx*P(6,22)*SH_ACCX[0] + Kaccx*P(6,0)*SK_ACCX[3] - Kaccx*P(6,2)*SK_ACCX[2] + Kaccx*P(6,3)*SK_ACCX[1] + Kaccx*P(6,1)*SK_ACCX[4] + Kaccx*P(6,5)*SK_ACCX[6] - Kaccx*P(6,6)*SK_ACCX[5] - Kaccx*P(6,23)*SK_ACCX[6]); -Kfusion[7] = -SK_ACCX[0]*(Kaccx*P(7,4)*SH_ACCX[0] - Kaccx*P(7,22)*SH_ACCX[0] + Kaccx*P(7,0)*SK_ACCX[3] - Kaccx*P(7,2)*SK_ACCX[2] + Kaccx*P(7,3)*SK_ACCX[1] + Kaccx*P(7,1)*SK_ACCX[4] + Kaccx*P(7,5)*SK_ACCX[6] - Kaccx*P(7,6)*SK_ACCX[5] - Kaccx*P(7,23)*SK_ACCX[6]); -Kfusion[8] = -SK_ACCX[0]*(Kaccx*P(8,4)*SH_ACCX[0] - Kaccx*P(8,22)*SH_ACCX[0] + Kaccx*P(8,0)*SK_ACCX[3] - Kaccx*P(8,2)*SK_ACCX[2] + Kaccx*P(8,3)*SK_ACCX[1] + Kaccx*P(8,1)*SK_ACCX[4] + Kaccx*P(8,5)*SK_ACCX[6] - Kaccx*P(8,6)*SK_ACCX[5] - Kaccx*P(8,23)*SK_ACCX[6]); -Kfusion[9] = -SK_ACCX[0]*(Kaccx*P(9,4)*SH_ACCX[0] - Kaccx*P(9,22)*SH_ACCX[0] + Kaccx*P(9,0)*SK_ACCX[3] - Kaccx*P(9,2)*SK_ACCX[2] + Kaccx*P(9,3)*SK_ACCX[1] + Kaccx*P(9,1)*SK_ACCX[4] + Kaccx*P(9,5)*SK_ACCX[6] - Kaccx*P(9,6)*SK_ACCX[5] - Kaccx*P(9,23)*SK_ACCX[6]); -Kfusion[10] = -SK_ACCX[0]*(Kaccx*P(10,4)*SH_ACCX[0] - Kaccx*P(10,22)*SH_ACCX[0] + Kaccx*P(10,0)*SK_ACCX[3] - Kaccx*P(10,2)*SK_ACCX[2] + Kaccx*P(10,3)*SK_ACCX[1] + Kaccx*P(10,1)*SK_ACCX[4] + Kaccx*P(10,5)*SK_ACCX[6] - Kaccx*P(10,6)*SK_ACCX[5] - Kaccx*P(10,23)*SK_ACCX[6]); -Kfusion[11] = -SK_ACCX[0]*(Kaccx*P(11,4)*SH_ACCX[0] - Kaccx*P(11,22)*SH_ACCX[0] + Kaccx*P(11,0)*SK_ACCX[3] - Kaccx*P(11,2)*SK_ACCX[2] + Kaccx*P(11,3)*SK_ACCX[1] + Kaccx*P(11,1)*SK_ACCX[4] + Kaccx*P(11,5)*SK_ACCX[6] - Kaccx*P(11,6)*SK_ACCX[5] - Kaccx*P(11,23)*SK_ACCX[6]); -Kfusion[12] = -SK_ACCX[0]*(Kaccx*P(12,4)*SH_ACCX[0] - Kaccx*P(12,22)*SH_ACCX[0] + Kaccx*P(12,0)*SK_ACCX[3] - Kaccx*P(12,2)*SK_ACCX[2] + Kaccx*P(12,3)*SK_ACCX[1] + Kaccx*P(12,1)*SK_ACCX[4] + Kaccx*P(12,5)*SK_ACCX[6] - Kaccx*P(12,6)*SK_ACCX[5] - Kaccx*P(12,23)*SK_ACCX[6]); -Kfusion[13] = -SK_ACCX[0]*(Kaccx*P(13,4)*SH_ACCX[0] - Kaccx*P(13,22)*SH_ACCX[0] + Kaccx*P(13,0)*SK_ACCX[3] - Kaccx*P(13,2)*SK_ACCX[2] + Kaccx*P(13,3)*SK_ACCX[1] + Kaccx*P(13,1)*SK_ACCX[4] + Kaccx*P(13,5)*SK_ACCX[6] - Kaccx*P(13,6)*SK_ACCX[5] - Kaccx*P(13,23)*SK_ACCX[6]); -Kfusion[14] = -SK_ACCX[0]*(Kaccx*P(14,4)*SH_ACCX[0] - Kaccx*P(14,22)*SH_ACCX[0] + Kaccx*P(14,0)*SK_ACCX[3] - Kaccx*P(14,2)*SK_ACCX[2] + Kaccx*P(14,3)*SK_ACCX[1] + Kaccx*P(14,1)*SK_ACCX[4] + Kaccx*P(14,5)*SK_ACCX[6] - Kaccx*P(14,6)*SK_ACCX[5] - Kaccx*P(14,23)*SK_ACCX[6]); -Kfusion[15] = -SK_ACCX[0]*(Kaccx*P(15,4)*SH_ACCX[0] - Kaccx*P(15,22)*SH_ACCX[0] + Kaccx*P(15,0)*SK_ACCX[3] - Kaccx*P(15,2)*SK_ACCX[2] + Kaccx*P(15,3)*SK_ACCX[1] + Kaccx*P(15,1)*SK_ACCX[4] + Kaccx*P(15,5)*SK_ACCX[6] - Kaccx*P(15,6)*SK_ACCX[5] - Kaccx*P(15,23)*SK_ACCX[6]); -Kfusion[16] = -SK_ACCX[0]*(Kaccx*P(16,4)*SH_ACCX[0] - Kaccx*P(16,22)*SH_ACCX[0] + Kaccx*P(16,0)*SK_ACCX[3] - Kaccx*P(16,2)*SK_ACCX[2] + Kaccx*P(16,3)*SK_ACCX[1] + Kaccx*P(16,1)*SK_ACCX[4] + Kaccx*P(16,5)*SK_ACCX[6] - Kaccx*P(16,6)*SK_ACCX[5] - Kaccx*P(16,23)*SK_ACCX[6]); -Kfusion[17] = -SK_ACCX[0]*(Kaccx*P(17,4)*SH_ACCX[0] - Kaccx*P(17,22)*SH_ACCX[0] + Kaccx*P(17,0)*SK_ACCX[3] - Kaccx*P(17,2)*SK_ACCX[2] + Kaccx*P(17,3)*SK_ACCX[1] + Kaccx*P(17,1)*SK_ACCX[4] + Kaccx*P(17,5)*SK_ACCX[6] - Kaccx*P(17,6)*SK_ACCX[5] - Kaccx*P(17,23)*SK_ACCX[6]); -Kfusion[18] = -SK_ACCX[0]*(Kaccx*P(18,4)*SH_ACCX[0] - Kaccx*P(18,22)*SH_ACCX[0] + Kaccx*P(18,0)*SK_ACCX[3] - Kaccx*P(18,2)*SK_ACCX[2] + Kaccx*P(18,3)*SK_ACCX[1] + Kaccx*P(18,1)*SK_ACCX[4] + Kaccx*P(18,5)*SK_ACCX[6] - Kaccx*P(18,6)*SK_ACCX[5] - Kaccx*P(18,23)*SK_ACCX[6]); -Kfusion[19] = -SK_ACCX[0]*(Kaccx*P(19,4)*SH_ACCX[0] - Kaccx*P(19,22)*SH_ACCX[0] + Kaccx*P(19,0)*SK_ACCX[3] - Kaccx*P(19,2)*SK_ACCX[2] + Kaccx*P(19,3)*SK_ACCX[1] + Kaccx*P(19,1)*SK_ACCX[4] + Kaccx*P(19,5)*SK_ACCX[6] - Kaccx*P(19,6)*SK_ACCX[5] - Kaccx*P(19,23)*SK_ACCX[6]); -Kfusion[20] = -SK_ACCX[0]*(Kaccx*P(20,4)*SH_ACCX[0] - Kaccx*P(20,22)*SH_ACCX[0] + Kaccx*P(20,0)*SK_ACCX[3] - Kaccx*P(20,2)*SK_ACCX[2] + Kaccx*P(20,3)*SK_ACCX[1] + Kaccx*P(20,1)*SK_ACCX[4] + Kaccx*P(20,5)*SK_ACCX[6] - Kaccx*P(20,6)*SK_ACCX[5] - Kaccx*P(20,23)*SK_ACCX[6]); -Kfusion[21] = -SK_ACCX[0]*(Kaccx*P(21,4)*SH_ACCX[0] - Kaccx*P(21,22)*SH_ACCX[0] + Kaccx*P(21,0)*SK_ACCX[3] - Kaccx*P(21,2)*SK_ACCX[2] + Kaccx*P(21,3)*SK_ACCX[1] + Kaccx*P(21,1)*SK_ACCX[4] + Kaccx*P(21,5)*SK_ACCX[6] - Kaccx*P(21,6)*SK_ACCX[5] - Kaccx*P(21,23)*SK_ACCX[6]); -Kfusion[22] = -SK_ACCX[0]*(Kaccx*P(22,4)*SH_ACCX[0] - Kaccx*P(22,22)*SH_ACCX[0] + Kaccx*P(22,0)*SK_ACCX[3] - Kaccx*P(22,2)*SK_ACCX[2] + Kaccx*P(22,3)*SK_ACCX[1] + Kaccx*P(22,1)*SK_ACCX[4] + Kaccx*P(22,5)*SK_ACCX[6] - Kaccx*P(22,6)*SK_ACCX[5] - Kaccx*P(22,23)*SK_ACCX[6]); -Kfusion[23] = -SK_ACCX[0]*(Kaccx*P(23,4)*SH_ACCX[0] - Kaccx*P(23,22)*SH_ACCX[0] + Kaccx*P(23,0)*SK_ACCX[3] - Kaccx*P(23,2)*SK_ACCX[2] + Kaccx*P(23,3)*SK_ACCX[1] + Kaccx*P(23,1)*SK_ACCX[4] + Kaccx*P(23,5)*SK_ACCX[6] - Kaccx*P(23,6)*SK_ACCX[5] - Kaccx*P(23,23)*SK_ACCX[6]); -float SH_ACCY[3][1]; -SH_ACCY[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); -SH_ACCY[1] = vn - vwn; -SH_ACCY[2] = ve - vwe; -float H_ACCY[1][24]; -H_ACCY[0][0] = -Kaccy*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd); -H_ACCY[0][1] = -Kaccy*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd); -H_ACCY[0][2] = -Kaccy*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd); -H_ACCY[0][3] = Kaccy*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd); -H_ACCY[0][4] = Kaccy*(2*q0*q3 - 2*q1*q2); -H_ACCY[0][5] = -Kaccy*SH_ACCY[0]; -H_ACCY[0][6] = -Kaccy*(2*q0*q1 + 2*q2*q3); -H_ACCY[0][22] = -2*Kaccy*(q0*q3 - q1*q2); -H_ACCY[0][23] = Kaccy*SH_ACCY[0]; -float SK_ACCY[9][1]; -SK_ACCY[0] = 1/(R_ACC + Kaccy*SH_ACCY[0]*(Kaccy*P(5,5)*SH_ACCY[0] - Kaccy*P(23,5)*SH_ACCY[0] - Kaccy*P(4,5)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,5)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,5)*(q0*q3 - q1*q2) + Kaccy*P(0,5)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,5)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,5)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,5)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*SH_ACCY[0]*(Kaccy*P(5,23)*SH_ACCY[0] - Kaccy*P(23,23)*SH_ACCY[0] - Kaccy*P(4,23)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,23)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,23)*(q0*q3 - q1*q2) + Kaccy*P(0,23)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,23)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,23)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,23)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*P(5,4)*SH_ACCY[0] - Kaccy*P(23,4)*SH_ACCY[0] - Kaccy*P(4,4)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,4)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,4)*(q0*q3 - q1*q2) + Kaccy*P(0,4)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,4)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,4)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,4)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*P(5,6)*SH_ACCY[0] - Kaccy*P(23,6)*SH_ACCY[0] - Kaccy*P(4,6)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,6)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,6)*(q0*q3 - q1*q2) + Kaccy*P(0,6)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,6)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,6)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,6)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*P(5,22)*SH_ACCY[0] - Kaccy*P(23,22)*SH_ACCY[0] - Kaccy*P(4,22)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,22)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,22)*(q0*q3 - q1*q2) + Kaccy*P(0,22)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,22)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,22)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,22)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd)*(Kaccy*P(5,0)*SH_ACCY[0] - Kaccy*P(23,0)*SH_ACCY[0] - Kaccy*P(4,0)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,0)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,0)*(q0*q3 - q1*q2) + Kaccy*P(0,0)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,0)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,0)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,0)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd)*(Kaccy*P(5,1)*SH_ACCY[0] - Kaccy*P(23,1)*SH_ACCY[0] - Kaccy*P(4,1)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,1)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,1)*(q0*q3 - q1*q2) + Kaccy*P(0,1)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,1)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,1)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,1)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd)*(Kaccy*P(5,2)*SH_ACCY[0] - Kaccy*P(23,2)*SH_ACCY[0] - Kaccy*P(4,2)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,2)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,2)*(q0*q3 - q1*q2) + Kaccy*P(0,2)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,2)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,2)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,2)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)*(Kaccy*P(5,3)*SH_ACCY[0] - Kaccy*P(23,3)*SH_ACCY[0] - Kaccy*P(4,3)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,3)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,3)*(q0*q3 - q1*q2) + Kaccy*P(0,3)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,3)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,3)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,3)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd))); -SK_ACCY[1] = 2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd; -SK_ACCY[2] = 2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd; -SK_ACCY[3] = 2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd; -SK_ACCY[4] = 2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd; -SK_ACCY[5] = 2*q0*q3 - 2*q1*q2; -SK_ACCY[6] = q0*q3 - q1*q2; -SK_ACCY[7] = 2*q0*q1 + 2*q2*q3; -SK_ACCY[8] = SH_ACCY[0]; -float Kfusion[24][1]; -float Kfusion[1][1]; -Kfusion[0] = -SK_ACCY[0]*(Kaccy*P(0,0)*SK_ACCY[3] + Kaccy*P(0,1)*SK_ACCY[2] - Kaccy*P(0,3)*SK_ACCY[1] + Kaccy*P(0,2)*SK_ACCY[4] - Kaccy*P(0,4)*SK_ACCY[5] + Kaccy*P(0,5)*SK_ACCY[8] + Kaccy*P(0,6)*SK_ACCY[7] + 2*Kaccy*P(0,22)*SK_ACCY[6] - Kaccy*P(0,23)*SK_ACCY[8]); -Kfusion[1] = -SK_ACCY[0]*(Kaccy*P(1,0)*SK_ACCY[3] + Kaccy*P(1,1)*SK_ACCY[2] - Kaccy*P(1,3)*SK_ACCY[1] + Kaccy*P(1,2)*SK_ACCY[4] - Kaccy*P(1,4)*SK_ACCY[5] + Kaccy*P(1,5)*SK_ACCY[8] + Kaccy*P(1,6)*SK_ACCY[7] + 2*Kaccy*P(1,22)*SK_ACCY[6] - Kaccy*P(1,23)*SK_ACCY[8]); -Kfusion[2] = -SK_ACCY[0]*(Kaccy*P(2,0)*SK_ACCY[3] + Kaccy*P(2,1)*SK_ACCY[2] - Kaccy*P(2,3)*SK_ACCY[1] + Kaccy*P(2,2)*SK_ACCY[4] - Kaccy*P(2,4)*SK_ACCY[5] + Kaccy*P(2,5)*SK_ACCY[8] + Kaccy*P(2,6)*SK_ACCY[7] + 2*Kaccy*P(2,22)*SK_ACCY[6] - Kaccy*P(2,23)*SK_ACCY[8]); -Kfusion[3] = -SK_ACCY[0]*(Kaccy*P(3,0)*SK_ACCY[3] + Kaccy*P(3,1)*SK_ACCY[2] - Kaccy*P(3,3)*SK_ACCY[1] + Kaccy*P(3,2)*SK_ACCY[4] - Kaccy*P(3,4)*SK_ACCY[5] + Kaccy*P(3,5)*SK_ACCY[8] + Kaccy*P(3,6)*SK_ACCY[7] + 2*Kaccy*P(3,22)*SK_ACCY[6] - Kaccy*P(3,23)*SK_ACCY[8]); -Kfusion[4] = -SK_ACCY[0]*(Kaccy*P(4,0)*SK_ACCY[3] + Kaccy*P(4,1)*SK_ACCY[2] - Kaccy*P(4,3)*SK_ACCY[1] + Kaccy*P(4,2)*SK_ACCY[4] - Kaccy*P(4,4)*SK_ACCY[5] + Kaccy*P(4,5)*SK_ACCY[8] + Kaccy*P(4,6)*SK_ACCY[7] + 2*Kaccy*P(4,22)*SK_ACCY[6] - Kaccy*P(4,23)*SK_ACCY[8]); -Kfusion[5] = -SK_ACCY[0]*(Kaccy*P(5,0)*SK_ACCY[3] + Kaccy*P(5,1)*SK_ACCY[2] - Kaccy*P(5,3)*SK_ACCY[1] + Kaccy*P(5,2)*SK_ACCY[4] - Kaccy*P(5,4)*SK_ACCY[5] + Kaccy*P(5,5)*SK_ACCY[8] + Kaccy*P(5,6)*SK_ACCY[7] + 2*Kaccy*P(5,22)*SK_ACCY[6] - Kaccy*P(5,23)*SK_ACCY[8]); -Kfusion[6] = -SK_ACCY[0]*(Kaccy*P(6,0)*SK_ACCY[3] + Kaccy*P(6,1)*SK_ACCY[2] - Kaccy*P(6,3)*SK_ACCY[1] + Kaccy*P(6,2)*SK_ACCY[4] - Kaccy*P(6,4)*SK_ACCY[5] + Kaccy*P(6,5)*SK_ACCY[8] + Kaccy*P(6,6)*SK_ACCY[7] + 2*Kaccy*P(6,22)*SK_ACCY[6] - Kaccy*P(6,23)*SK_ACCY[8]); -Kfusion[7] = -SK_ACCY[0]*(Kaccy*P(7,0)*SK_ACCY[3] + Kaccy*P(7,1)*SK_ACCY[2] - Kaccy*P(7,3)*SK_ACCY[1] + Kaccy*P(7,2)*SK_ACCY[4] - Kaccy*P(7,4)*SK_ACCY[5] + Kaccy*P(7,5)*SK_ACCY[8] + Kaccy*P(7,6)*SK_ACCY[7] + 2*Kaccy*P(7,22)*SK_ACCY[6] - Kaccy*P(7,23)*SK_ACCY[8]); -Kfusion[8] = -SK_ACCY[0]*(Kaccy*P(8,0)*SK_ACCY[3] + Kaccy*P(8,1)*SK_ACCY[2] - Kaccy*P(8,3)*SK_ACCY[1] + Kaccy*P(8,2)*SK_ACCY[4] - Kaccy*P(8,4)*SK_ACCY[5] + Kaccy*P(8,5)*SK_ACCY[8] + Kaccy*P(8,6)*SK_ACCY[7] + 2*Kaccy*P(8,22)*SK_ACCY[6] - Kaccy*P(8,23)*SK_ACCY[8]); -Kfusion[9] = -SK_ACCY[0]*(Kaccy*P(9,0)*SK_ACCY[3] + Kaccy*P(9,1)*SK_ACCY[2] - Kaccy*P(9,3)*SK_ACCY[1] + Kaccy*P(9,2)*SK_ACCY[4] - Kaccy*P(9,4)*SK_ACCY[5] + Kaccy*P(9,5)*SK_ACCY[8] + Kaccy*P(9,6)*SK_ACCY[7] + 2*Kaccy*P(9,22)*SK_ACCY[6] - Kaccy*P(9,23)*SK_ACCY[8]); -Kfusion[10] = -SK_ACCY[0]*(Kaccy*P(10,0)*SK_ACCY[3] + Kaccy*P(10,1)*SK_ACCY[2] - Kaccy*P(10,3)*SK_ACCY[1] + Kaccy*P(10,2)*SK_ACCY[4] - Kaccy*P(10,4)*SK_ACCY[5] + Kaccy*P(10,5)*SK_ACCY[8] + Kaccy*P(10,6)*SK_ACCY[7] + 2*Kaccy*P(10,22)*SK_ACCY[6] - Kaccy*P(10,23)*SK_ACCY[8]); -Kfusion[11] = -SK_ACCY[0]*(Kaccy*P(11,0)*SK_ACCY[3] + Kaccy*P(11,1)*SK_ACCY[2] - Kaccy*P(11,3)*SK_ACCY[1] + Kaccy*P(11,2)*SK_ACCY[4] - Kaccy*P(11,4)*SK_ACCY[5] + Kaccy*P(11,5)*SK_ACCY[8] + Kaccy*P(11,6)*SK_ACCY[7] + 2*Kaccy*P(11,22)*SK_ACCY[6] - Kaccy*P(11,23)*SK_ACCY[8]); -Kfusion[12] = -SK_ACCY[0]*(Kaccy*P(12,0)*SK_ACCY[3] + Kaccy*P(12,1)*SK_ACCY[2] - Kaccy*P(12,3)*SK_ACCY[1] + Kaccy*P(12,2)*SK_ACCY[4] - Kaccy*P(12,4)*SK_ACCY[5] + Kaccy*P(12,5)*SK_ACCY[8] + Kaccy*P(12,6)*SK_ACCY[7] + 2*Kaccy*P(12,22)*SK_ACCY[6] - Kaccy*P(12,23)*SK_ACCY[8]); -Kfusion[13] = -SK_ACCY[0]*(Kaccy*P(13,0)*SK_ACCY[3] + Kaccy*P(13,1)*SK_ACCY[2] - Kaccy*P(13,3)*SK_ACCY[1] + Kaccy*P(13,2)*SK_ACCY[4] - Kaccy*P(13,4)*SK_ACCY[5] + Kaccy*P(13,5)*SK_ACCY[8] + Kaccy*P(13,6)*SK_ACCY[7] + 2*Kaccy*P(13,22)*SK_ACCY[6] - Kaccy*P(13,23)*SK_ACCY[8]); -Kfusion[14] = -SK_ACCY[0]*(Kaccy*P(14,0)*SK_ACCY[3] + Kaccy*P(14,1)*SK_ACCY[2] - Kaccy*P(14,3)*SK_ACCY[1] + Kaccy*P(14,2)*SK_ACCY[4] - Kaccy*P(14,4)*SK_ACCY[5] + Kaccy*P(14,5)*SK_ACCY[8] + Kaccy*P(14,6)*SK_ACCY[7] + 2*Kaccy*P(14,22)*SK_ACCY[6] - Kaccy*P(14,23)*SK_ACCY[8]); -Kfusion[15] = -SK_ACCY[0]*(Kaccy*P(15,0)*SK_ACCY[3] + Kaccy*P(15,1)*SK_ACCY[2] - Kaccy*P(15,3)*SK_ACCY[1] + Kaccy*P(15,2)*SK_ACCY[4] - Kaccy*P(15,4)*SK_ACCY[5] + Kaccy*P(15,5)*SK_ACCY[8] + Kaccy*P(15,6)*SK_ACCY[7] + 2*Kaccy*P(15,22)*SK_ACCY[6] - Kaccy*P(15,23)*SK_ACCY[8]); -Kfusion[16] = -SK_ACCY[0]*(Kaccy*P(16,0)*SK_ACCY[3] + Kaccy*P(16,1)*SK_ACCY[2] - Kaccy*P(16,3)*SK_ACCY[1] + Kaccy*P(16,2)*SK_ACCY[4] - Kaccy*P(16,4)*SK_ACCY[5] + Kaccy*P(16,5)*SK_ACCY[8] + Kaccy*P(16,6)*SK_ACCY[7] + 2*Kaccy*P(16,22)*SK_ACCY[6] - Kaccy*P(16,23)*SK_ACCY[8]); -Kfusion[17] = -SK_ACCY[0]*(Kaccy*P(17,0)*SK_ACCY[3] + Kaccy*P(17,1)*SK_ACCY[2] - Kaccy*P(17,3)*SK_ACCY[1] + Kaccy*P(17,2)*SK_ACCY[4] - Kaccy*P(17,4)*SK_ACCY[5] + Kaccy*P(17,5)*SK_ACCY[8] + Kaccy*P(17,6)*SK_ACCY[7] + 2*Kaccy*P(17,22)*SK_ACCY[6] - Kaccy*P(17,23)*SK_ACCY[8]); -Kfusion[18] = -SK_ACCY[0]*(Kaccy*P(18,0)*SK_ACCY[3] + Kaccy*P(18,1)*SK_ACCY[2] - Kaccy*P(18,3)*SK_ACCY[1] + Kaccy*P(18,2)*SK_ACCY[4] - Kaccy*P(18,4)*SK_ACCY[5] + Kaccy*P(18,5)*SK_ACCY[8] + Kaccy*P(18,6)*SK_ACCY[7] + 2*Kaccy*P(18,22)*SK_ACCY[6] - Kaccy*P(18,23)*SK_ACCY[8]); -Kfusion[19] = -SK_ACCY[0]*(Kaccy*P(19,0)*SK_ACCY[3] + Kaccy*P(19,1)*SK_ACCY[2] - Kaccy*P(19,3)*SK_ACCY[1] + Kaccy*P(19,2)*SK_ACCY[4] - Kaccy*P(19,4)*SK_ACCY[5] + Kaccy*P(19,5)*SK_ACCY[8] + Kaccy*P(19,6)*SK_ACCY[7] + 2*Kaccy*P(19,22)*SK_ACCY[6] - Kaccy*P(19,23)*SK_ACCY[8]); -Kfusion[20] = -SK_ACCY[0]*(Kaccy*P(20,0)*SK_ACCY[3] + Kaccy*P(20,1)*SK_ACCY[2] - Kaccy*P(20,3)*SK_ACCY[1] + Kaccy*P(20,2)*SK_ACCY[4] - Kaccy*P(20,4)*SK_ACCY[5] + Kaccy*P(20,5)*SK_ACCY[8] + Kaccy*P(20,6)*SK_ACCY[7] + 2*Kaccy*P(20,22)*SK_ACCY[6] - Kaccy*P(20,23)*SK_ACCY[8]); -Kfusion[21] = -SK_ACCY[0]*(Kaccy*P(21,0)*SK_ACCY[3] + Kaccy*P(21,1)*SK_ACCY[2] - Kaccy*P(21,3)*SK_ACCY[1] + Kaccy*P(21,2)*SK_ACCY[4] - Kaccy*P(21,4)*SK_ACCY[5] + Kaccy*P(21,5)*SK_ACCY[8] + Kaccy*P(21,6)*SK_ACCY[7] + 2*Kaccy*P(21,22)*SK_ACCY[6] - Kaccy*P(21,23)*SK_ACCY[8]); -Kfusion[22] = -SK_ACCY[0]*(Kaccy*P(22,0)*SK_ACCY[3] + Kaccy*P(22,1)*SK_ACCY[2] - Kaccy*P(22,3)*SK_ACCY[1] + Kaccy*P(22,2)*SK_ACCY[4] - Kaccy*P(22,4)*SK_ACCY[5] + Kaccy*P(22,5)*SK_ACCY[8] + Kaccy*P(22,6)*SK_ACCY[7] + 2*Kaccy*P(22,22)*SK_ACCY[6] - Kaccy*P(22,23)*SK_ACCY[8]); -Kfusion[23] = -SK_ACCY[0]*(Kaccy*P(23,0)*SK_ACCY[3] + Kaccy*P(23,1)*SK_ACCY[2] - Kaccy*P(23,3)*SK_ACCY[1] + Kaccy*P(23,2)*SK_ACCY[4] - Kaccy*P(23,4)*SK_ACCY[5] + Kaccy*P(23,5)*SK_ACCY[8] + Kaccy*P(23,6)*SK_ACCY[7] + 2*Kaccy*P(23,22)*SK_ACCY[6] - Kaccy*P(23,23)*SK_ACCY[8]); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/ConvertCToC.m b/EKF/matlab/scripts/Inertial Nav EKF/ConvertCToC.m deleted file mode 100644 index 0ef7f1996c..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/ConvertCToC.m +++ /dev/null @@ -1,58 +0,0 @@ -function ConvertCToC(nStates) - -% This function converts the nextP and P covaraince matrix expressions from -% P[row index][col index] to P(row index,col index) syntax to enable use -% of the matrix library type used by PX4 - -%% Define file to read in -fileName = strcat('C_code',int2str(nStates),'.txt'); -delimiter = ''; - -%% Format string for each line of text: -% column1: text (%s) -% For more information, see the TEXTSCAN documentation. -formatSpec = '%s%[^\n\r]'; - -%% Open the text file. -fileID = fopen(fileName,'r'); - -%% Read columns of data according to format string. -% This call is based on the structure of the file used to generate this -% code. If an error occurs for a different file, try regenerating the code -% from the Import Tool. -dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false); - -%% Close the text file. -fclose(fileID); - -%% Create output variable -SymbolicOutput = [dataArray{1:end-1}]; - -%% Clear temporary variables -clearvars filename delimiter formatSpec fileID dataArray ans; - -%% Replace [row][col] brackets for nextP and P arrays with (row,col) - -% replace 2-D left indexes -for rowIndex = 1:nStates - for colIndex = 1:nStates - strRowIndex = int2str(rowIndex-1); - strColIndex = int2str(colIndex-1); - strRep = sprintf('P(%d,%d)',(rowIndex-1),(colIndex-1)); - strPat = strcat('P\[',strRowIndex,'\]\[',strColIndex,'\]'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end - end -end - -%% Write to file -fileName = strcat('C_code_use_matrix_lib',int2str(nStates),'.txt'); -fid = fopen(fileName,'wt'); -for lineIndex = 1:length(SymbolicOutput) - fprintf(fid,char(SymbolicOutput(lineIndex))); - fprintf(fid,'\n'); -end -fclose(fid); -clear all; \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/ConvertToC.m b/EKF/matlab/scripts/Inertial Nav EKF/ConvertToC.m deleted file mode 100644 index 23176f36ca..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/ConvertToC.m +++ /dev/null @@ -1,213 +0,0 @@ -function ConvertToC(nStates) -%% Define file to read in -fileName = strcat('M_code',int2str(nStates),'.txt'); -delimiter = ''; - -%% Format string for each line of text: -% column1: text (%s) -% For more information, see the TEXTSCAN documentation. -formatSpec = '%s%[^\n\r]'; - -%% Open the text file. -fileID = fopen(fileName,'r'); - -%% Read columns of data according to format string. -% This call is based on the structure of the file used to generate this -% code. If an error occurs for a different file, try regenerating the code -% from the Import Tool. -dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false); - -%% Close the text file. -fclose(fileID); - -%% Create output variable -SymbolicOutput = [dataArray{1:end-1}]; - -%% Clear temporary variables -clearvars filename delimiter formatSpec fileID dataArray ans; - -%% Convert indexing and replace brackets - -% replace 1-D indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf('[%d]',(arrayIndex-1)); - strPat = strcat('\(',strIndex,'\)'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace 2-D left indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf('[%d,',(arrayIndex-1)); - strPat = strcat('\(',strIndex,'\,'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace 2-D right indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf(',%d]',(arrayIndex-1)); - strPat = strcat('\,',strIndex,'\)'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace commas -for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')}; -end - -%% Replace ^2 - -% replace where adjacent to ) parenthesis -for lineIndex = 1:length(SymbolicOutput) - idxsq = regexp(SymbolicOutput(lineIndex),'\)\^2'); - if ~isempty(idxsq{1}) - strIn = SymbolicOutput(lineIndex); - idxlp = regexp(strIn,'\('); - idxrp = regexp(strIn,'\)'); - for pwrIndex = 1:length(idxsq{1}) - counter = 1; - index = idxsq{1}(pwrIndex); - endIndex(pwrIndex) = index; - while (counter > 0 && index > 0) - index = index - 1; - counter = counter + ~isempty(find(idxrp{1} == index, 1)); - counter = counter - ~isempty(find(idxlp{1} == index, 1)); - end - startIndex(pwrIndex) = index; - % strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2'); - strRep = strcat('sq',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex))); - % cellStrPat(pwrIndex) = cellstr(strPat); - cellStrRep(pwrIndex) = cellstr(strRep); - end - for pwrIndex = 1:length(idxsq{1}) - strRep = char(cellStrRep(pwrIndex)); - strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+2) = strRep; - end - SymbolicOutput(lineIndex) = strIn; - end -end - -% replace where adjacent to ] parenthesis -for lineIndex = 1:length(SymbolicOutput) - strIn = char(SymbolicOutput(lineIndex)); - [match,idxsq1,idxsq2] = regexp(strIn,'\w*\[\w*\]\^2','match','start','end'); - [idxsq3] = regexp(strIn,'\[\w*\]\^2','start'); - if ~isempty(match) - for pwrIndex = 1:length(match) - strVar = strIn(idxsq1(pwrIndex):idxsq3(pwrIndex)-1); - strIndex = strIn(idxsq3(pwrIndex)+1:idxsq2(pwrIndex)-3); - strPat = strcat(strVar,'\[',strIndex,'\]\^2'); - strRep = strcat('sq(',strVar,'[',strIndex,']',')'); - strIn = regexprep(strIn,strPat,strRep); - end - SymbolicOutput(lineIndex) = cellstr(strIn); - end -end - -% replace where adjacent to alpanumeric characters -for lineIndex = 1:length(SymbolicOutput) - strIn = char(SymbolicOutput(lineIndex)); - [match,idxsq1,idxsq2] = regexp(strIn,'\w*\^2','match','start','end'); - [idxsq3] = regexp(strIn,'\^2','start'); - if ~isempty(match) - for pwrIndex = 1:length(match) - strVar = strIn(idxsq1(pwrIndex)+2*(pwrIndex-1):idxsq2(pwrIndex)-2+2*(pwrIndex-1)); - strPat = strcat(strVar,'\^2'); - strRep = strcat('sq(',strVar,')'); - strIn = regexprep(strIn,strPat,strRep); - end - SymbolicOutput(lineIndex) = cellstr(strIn); - end -end - -%% Replace ^(1/2) - -% replace where adjacent to ) parenthesis -for lineIndex = 1:length(SymbolicOutput) - idxsq = regexp(SymbolicOutput(lineIndex),'\)\^\(1\/2\)'); - if ~isempty(idxsq{1}) - strIn = SymbolicOutput(lineIndex); - idxlp = regexp(strIn,'\('); - idxrp = regexp(strIn,'\)'); - for pwrIndex = 1:length(idxsq{1}) - counter = 1; - index = idxsq{1}(pwrIndex); - endIndex(pwrIndex) = index; - while (counter > 0 && index > 0) - index = index - 1; - counter = counter + ~isempty(find(idxrp{1} == index, 1)); - counter = counter - ~isempty(find(idxlp{1} == index, 1)); - end - startIndex(pwrIndex) = index; - % strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2'); - strRep = strcat('(sqrt',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),')'); - % cellStrPat(pwrIndex) = cellstr(strPat); - cellStrRep(pwrIndex) = cellstr(strRep); - end - for pwrIndex = 1:length(idxsq{1}) - strRep = char(cellStrRep(pwrIndex)); - strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+6) = strRep; - end - SymbolicOutput(lineIndex) = strIn; - end -end - -%% Replace Divisions -% Compiler looks after this type of optimisation for us -% for lineIndex = 1:length(SymbolicOutput) -% strIn = char(SymbolicOutput(lineIndex)); -% strIn = regexprep(strIn,'\/2','\*0\.5'); -% strIn = regexprep(strIn,'\/4','\*0\.25'); -% SymbolicOutput(lineIndex) = cellstr(strIn); -% end - -%% Convert declarations -for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - if ~isempty(regexp(str,'zeros', 'once')) - index1 = regexp(str,' = zeros[','once')-1; - index2 = regexp(str,' = zeros[','end','once')+1; - index3 = regexp(str,'\]\[','once')-1; - index4 = index3 + 3; - index5 = max(regexp(str,'\]'))-1; - str1 = {'float '}; - str2 = str(1:index1); - str3 = '['; - str4 = str(index2:index3); - str4 = num2str(str2num(str4)+1); - str5 = ']['; - str6 = str(index4:index5); - str6 = num2str(str2num(str6)+1); - str7 = '];'; - SymbolicOutput(lineIndex) = strcat(str1,str2,str3,str4,str5,str6,str7); - end -end - -%% Change covariance matrix variable name to P -for lineIndex = 1:length(SymbolicOutput) - strIn = char(SymbolicOutput(lineIndex)); - strIn = regexprep(strIn,'OP\[','P['); - SymbolicOutput(lineIndex) = cellstr(strIn); -end - -%% Write to file -fileName = strcat('C_code',int2str(nStates),'.txt'); -fid = fopen(fileName,'wt'); -for lineIndex = 1:length(SymbolicOutput) - fprintf(fid,char(SymbolicOutput(lineIndex))); - fprintf(fid,'\n'); -end -fclose(fid); -clear all; \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/ConvertToM.m b/EKF/matlab/scripts/Inertial Nav EKF/ConvertToM.m deleted file mode 100644 index a9b0489de7..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/ConvertToM.m +++ /dev/null @@ -1,46 +0,0 @@ -function ConvertToM(nStates) -%% Initialize variables -fileName = strcat('SymbolicOutput',int2str(nStates),'.txt'); -delimiter = ''; - -%% Format string for each line of text: -% column1: text (%s) -% For more information, see the TEXTSCAN documentation. -formatSpec = '%s%[^\n\r]'; - -%% Open the text file. -fileID = fopen(fileName,'r'); - -%% Read columns of data according to format string. -% This call is based on the structure of the file used to generate this -% code. If an error occurs for a different file, try regenerating the code -% from the Import Tool. -dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false); - -%% Close the text file. -fclose(fileID); - -%% Create output variable -SymbolicOutput = [dataArray{1:end-1}]; - -%% Clear temporary variables -clearvars filename delimiter formatSpec fileID dataArray ans; - -%% replace brackets and commas -for lineIndex = 1:length(SymbolicOutput) - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '('); - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ','); - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')'); -end - -%% Write to file -fileName = strcat('M_code',int2str(nStates),'.txt'); -fid = fopen(fileName,'wt'); -for lineIndex = 1:length(SymbolicOutput) - fprintf(fid,char(SymbolicOutput(lineIndex))); - fprintf(fid,'\n'); -end -fclose(fid); -clear all; - -end \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/DeriveYawResetEquations.m b/EKF/matlab/scripts/Inertial Nav EKF/DeriveYawResetEquations.m deleted file mode 100644 index 3006c1ef1b..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/DeriveYawResetEquations.m +++ /dev/null @@ -1,106 +0,0 @@ -% IMPORTANT - This script requires the Matlab symbolic toolbox - -% Derivation of quaterion covariance prediction for a rotation about the -% earth frame Z axis and starting at an arbitary orientation. This 4x4 -% matrix can be used to add an additional - -% Author: Paul Riseborough - -%% define symbolic variables and constants -clear all; -reset(symengine); -syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED -syms daYaw real % earth frame yaw delta angle - rad -syms daYawVar real; % earth frame yaw delta angle variance - rad^2 - -%% define the state prediction equations - -% define the quaternion rotation vector for the state estimate -quat = [q0;q1;q2;q3]; - -% derive the truth body to nav direction cosine matrix -Tbn = Quat2Tbn(quat); - -% define the yaw rotation delta angle in body frame -dAngMeas = transpose(Tbn) * [0; 0; daYaw]; - -% define the attitude update equations -% use a first order expansion of rotation to calculate the quaternion increment -% acceptable for propagation of covariances -deltaQuat = [1; - 0.5*dAngMeas(1); - 0.5*dAngMeas(2); - 0.5*dAngMeas(3); - ]; -quatNew = QuatMult(quat,deltaQuat); - -% Define the state vector & number of states -stateVector = quat; -nStates=numel(stateVector); - -% Define vector of process equations -newStateVector = quatNew; - -% derive the state transition matrix -F = jacobian(newStateVector, stateVector); -% set the rotation error states to zero -[F,SF]=OptimiseAlgebra(F,'SF'); - -% define a symbolic covariance matrix using strings to represent -% '_l_' to represent '( ' -% '_c_' to represent , -% '_r_' to represent ')' -% these can be substituted later to create executable code -for rowIndex = 1:nStates - for colIndex = 1:nStates - eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']); - eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']); - end -end - -save 'StatePrediction.mat'; - -%% derive the covariance prediction equations -% This reduces the number of floating point operations by a factor of 6 or -% more compared to using the standard matrix operations in code - -% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and -% velocities, after bias effects have been removed. - -% derive the control(disturbance) influence matrix from IMU noise to state -% noise -G = jacobian(newStateVector, daYaw); -[G,SG]=OptimiseAlgebra(G,'SG'); - -% derive the state error matrix -distMatrix = diag(daYawVar); -Q = G*distMatrix*transpose(G); -[Q,SQ]=OptimiseAlgebra(Q,'SQ'); - -% set the yaw delta angle to zero - we only needed it to determine the error -% propagation -SF = subs(SF, daYaw, 0); -SG = subs(SG, daYaw, 0); -SQ = subs(SQ, daYaw, 0); - -% Derive the predicted covariance matrix using the standard equation -PP = F*P*transpose(F) + Q; -PP = subs(PP, daYaw, 0); - -% Collect common expressions to optimise processing -[PP,SPP]=OptimiseAlgebra(PP,'SPP'); - -save('StateAndCovariancePrediction.mat'); -clear all; -reset(symengine); - -%% Save output and convert to m and c code fragments - -% load equations for predictions and updates -load('StateAndCovariancePrediction.mat'); - -fileName = strcat('SymbolicOutput',int2str(nStates),'.mat'); -save(fileName); -SaveScriptCode(nStates); -ConvertToM(nStates); -ConvertToC(nStates); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Drag.mat b/EKF/matlab/scripts/Inertial Nav EKF/Drag.mat deleted file mode 100644 index bf456947ab10d41d468ceb4b1d8783c7d319390d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 18384 zcmb4~V{mTGzozrVww>(Q-mz`lc6Myrw(T9;wr$&XGVgoN$2l|epRVdvtNO!TS6$Vs zx_;fOWdxL^1q2CM=@^WBxGdaWMSZBWFTZ>U}PiwPa7o&(0@08gbczzZ<7CXx2yvreSLEv zLpH)=AVUFgNd2G)_dzaBE-}AX8rbgX@4hJ5N!X#}Hdrr04ifK9dG9KDUYtc((K7Zh zdVs;UiC{vY0TAUec*?(ta{pr@sG$oX{BxMAlS>THy#}UN_E#VlRvK0WwIkN6p98-5 zyIcOddcGH18CI0MJ)$0Pf9FIH34{>{kfb#D#lNxA|6}YwpdNd^7Sr7A%HNlyPa z>Hl5o|3>zL;y)|+>BfoWgW$b*M@0?GT@>a04!|fF~^NNdGO@@-@@* z`SeckJ^a+OPJc(>`+f6KW5Y6B>)SQs`!(hL{jl=9^Rx3llDpw^_j6$5wPw7H3Ma(& zbwsaM^V`?)OE32&@`s@NY$WyB_j~VUU_TL9V_j=16pHGwBf$z^!cl6JI z+DDg<@6XLD!p^JjOZ3md?{9+cyPwzSpO>AlA8Nj@l5aiV_lx$oJFCx@i!a~Lvzmi1 zIlV7W)<^rNnV&2*Jasy-b(R!ld=D(mG(TN_y17sy(s5fibQ&3uw`3ytH0IlE1+VaJ}X z+vA4xa17fMR4O62(?v5F`r*Fl=!zhJXzMR11AcL2W0of{i}23ZJ8pcpbsC;9e^Ti0 z={VDQ3$NEz{FO6XOGRp%sC-0zbSE*>kjEgER0bDHQLm#nCtcf9v%AOr&vs@V&tNx4 zm=IB4qi58Mcw~n>b&naF^cAP^9HM)&0tp#YD}322pwjtj0&*zKa)_)gTnmS2K$_lJ zY;a4hr8W zM!G2r^43SRf}};_rQbfRiGJaWXPmD7NZpBF-DW>L3bEbPB&F(Zj^^k(8CQL@7%4e_=hd%)?_DQVF5q#T;oH*WjUW$TZfBtxjzJjWnUBzMyp}hct}oN5kCxKe(E-n!{W`^ zbgrzc1V{nn1%V$5C#=~g^@z0}lO!_8EgWoEV+o#fw_)b3d5P#N-vv5*TyE8k%pUtp zcNdFM9w{IHK`Qh|(n~d;zrsMRtQ~WBOIyh6FTL{aP{lt?F=pEzJWp!M+&j37LjKe2 zZO3{H9S`SxC+?u2YSCtzq|5?DfbZ0(zw}HNrEZw=Ff|s0)@+J&W3(@$l zTLseFHFyF##=}3&WlHDBt6E8;M8Tzm0M@n6P1pxvLpdBZ6M(e6p@AD+Ha2KRehvv9 zm?8LIf=y<1ogR84&KjO70upm>!iZWulBqssD%DpsrwQBG^%T7Q#bMLlmI) znC}w(K;}|95+d?=!~+yOG|1#3eH3@vOt)2ts<_-TE3?smc~)OAu_;Dkr**~~4c^`E zLn|%fQq~_6b5ow_RxIJzt@0+UR<0?M13m9Y!6N_2Dw9W0(h7zV^ zApC2+L5S@1y!Dk3pB#GQAryuJSRP`Y7=w&Jgm`^opj0iFg+3edb9N+F3Oe7t`4`#j zlnm05E8Q%TPPUJH;D+S5oDl*Lu`&|9EDVz_dBa_m`s0Jh&`MWtmXCMA=q&THi;V#b zmZc^!uYn;rKdi%+&X@S}{G!5x3)uzvE##N%>jK6j;$FoM%f}XXB3w^9m?Q7ZgdME^MUE`7I zn90Nu3UtZb=){ylDUy7H`LX?weNOwmJZ^jd~em|DDe5 z+zI<538ba>;tZr)BaVrUx)Z7NDq^^gmD{ih@ojUQfS^{1_)kMu7=b}ijezUTZfgj^ z_MfM>;*QNOxy#mrB{EGQAoL}8DLr`hnO&k(KhZf2yHyTokKu11cGm2i8ScMu1up(J zc~ptu-h!~j+d2?WiHXnb2yziHw{q9|mbmf&4U>va;6oGZT3JafjAIZ#OSO6y7?so| z;l}9SxZu(JFR-rk`H@NPQvRihzA1tH;9fRW;_s;L^o$sX2ev-~-tzr8cli*UTW~t@ zfeYzr4^tJjt+Jo4rhi92M#}29U%=YQ{COqv+6G3$V*d{T?sE(aN{p= z!drP%?hVL1V;DW-_?y%8U@#b0JmKGwyYA^C?Zzy>(%OdTHJ45}GD(4hP4B6W#++}m zPd!X!fWZ6d*S$}12$OYDzs}B*GoO4k(@Zk;(nPn^NxF8Qm>S|irZH(|N4%waxK*KN zGzV*P8GB#KlxEmj0!t18FgJQS61_hsdOjC?-zw<82{vD1cmC|~ee=Gw33gcn9~ufY zEvG-}7xXvnz^yE_7u{#b-{y}9w$fH3zfjQ{K1KwSq&v#mv&hhDczJ0TdKuD-qDBe@ zD4#>==MFX#CwVY|KU4Ob1_~fqov}SGsGOX)5!ayf&N*5sg(X-^al-*fRdz}7rBao0 zs4|rT56R$1uZV-epJ3-89u&lB;$2ZU-5XQk!nVVNujbWZyTW|VjJZ$q)M&*dMUNj1 z3_Yf{!!*j1^i=BA+1&jr74;4BV|d;k!VPTgEcLnR-LoA_ga0N)O$Umd;ShHK=>ERp(yAGm7a}as z^MbS891KN1k+K>o=MX{b5Z+x+E*9+5{WsXQOc|;M&A9gMMPNxsUl!T;M5tZN!xMD% znToO#dh;1*+QU_`Ps?hbjK7Bd6pm$t#d!S6Yp}{P#0c~J?l7gQJ(K{7ug%M0HAv4f z3XW@N+c4l(KioIct3sGGH!5jf|4|9mqiQch?qo+|{q%%JM3c6+JQ|HMF9_a#&>16C zlsxBs_nVx`J$=KQ(+su*7dGF`VYcB5vu&Yq?MF;jPoF)We8hyFTzLm0wJ4ssp97hOz(|FojrKcLU^ONtFaXc8uUuB2!LsRF7bGim$ z^&$N{$`lp@_Zok-b#bPZuaNSS)W!~P4^qpxf^Niap}NG;L+?fLFt?-oZ|52b^g`1H z{dSx$pQ@({$y$bnM@gFc8rdZJr6Lc(OL2wWu0l++j!d59Cp8L88x}ki!(<1QwqHV7 zI(y6j+vF0`P9+?!{_^HVf(YdCi9cJNgQ7uY%;a^DbE-QxIYv#w>MBaj0H{5qO5p~3 zhUq!pOIk&}N-7glaS4+bU6{}=79pUeL{(7Re4wHA=R0UdK-SPP7+TX=DuN6%uKp4< zz4f9~@O&eJDI0HwR~03_c(TB0uqm9u>Caeq1?p`~UuDWKJh*}v?I;Lo+fh>6&% zk|?aH5f`m#svxx8M!0MzwnHgFunCuoyIou-F_UCw8!dI5bt|iU8l{L!kMV~Gd|Q_J@A9(i z%0=sW(m{{tHYA`*O?s|A@-Z*f>@1Y!etZ9YKal%LsqB93dEu#+LPW_2c2&EuaB#?D zQ5}Sj&%pB_sfXe=gy#2!k3r>l<$m~CiI<!^wO_Fc4sKF3>| z`ISF$EDQ@LD*oA%I2(E10cR!f4~+ZRAcNYr^jqr+1^bv4lG_;fxq2FwW(qmzr80azLhqAuTHw_*cg7u;#l95QVGgxJ@Tc*16PKw41Q(e9;R*kV8ny}6KbSDf z8E*#48m+FsRh-osGr#(p=629FzPhIt{uNRQxo2h;W4Vz5VBW4Yv|({<%6+fVT1R7{ z4~SB;Z=jByrL$C5l4nM2Fv*Laajd~@0!NbD%ALk_q9IfPD|Sf}KR#41$6hcA4k9bl z-l!MAWSW4W^TvIu%GAP|R6Z-w zJ!cOu_M@J~GZ;sw*e{Ie>c%_TAoV&kVHtTrmQKYq)){qznb@nl2yEr3Kt1Iogo_H& zlG&+?uN(}iRBOq$s?q82y9x73=&|^WTC21iRck_UwnD<>CS~-Jbspzy_ZA;nhAOi4 zk+5%%e(svp+yRI=>x?Z_^2(?{SQc7l%7lp%+CjEFqLAWe>s47nZMg@&>NBVDW#-P7 zae5^H)8g-Hv(C|@FCJim$cWd3AVrvA87>E7g3Mz5xW@e$ehX4y zl0Yk#2~ZF!TN2`&oSbnFoZ$9MzdRJ7E`9WByT+9-^(1xa?c9^Y*)}-e8EUKu?lJd< zcC8x+ z8RYHlV96cml|jr(Nw$u5)BdT15ukM_V2fzVPA535F(?_bKAj7Pz30?R`Z0%oUk3|c~^#7K$U_^q!+4)gAV)K_NB|fBc3G&TI^qv?74WLZZ5QZCcp|~_4yy^Q2ZALpgy{sdC32Bqh zZOv2m^-2q<{W0Z)n%>1^P%NlszW~Y^kr{5}i&T9UY+)!o{xz|42w*1cJG|l5DCLfZ ziA4p(mU&rdsJ9d5mWz2jX0D$#W?p4qPksnXnA>gElK&agZ4|;mwoc8uZzFRFO3VxL zy~cD`mpaj+dY^>$B>10P_o%V^={x_`FIC2Yf2L)?-v3GP(

z=x zjufPB#k0A#1;!#9g$FmOjhz6>i< zSi4F%9Wf)Q347yn@CAs>D_Jda`tmI&KV&4HugC%vOuYt~7pM73P|yrpnxKc;D^xR{ zhdYrP0RNzv!81E!hH|$+cE|uoJFKd19LeD*%a=s3Z9y_i4+Lr=$fC07ZMsv^9|xoA zdO7shd=xe$Jj;C~Wse>}+zm1Hw=`=vIqrQKHrOLlD5$($v7ATb6W*~x6fT#P_Rqgt z)IUFi>Lc)%-i?V3s|&}VZn-`+*ljy6 zQmM%WT}sFvRycGooSiLuo}HmVvk(~O`UsPWSV1zU!ikQ-ykJ(pf9xH<bFgl$J1#`$CFCby@p@40*a=!qh4~%>}N7W(#=h+(=ZTbxTg~&!B#o&j_Ngbdoy|L zdiNK=tc{gv5e*F#EX?o|eUH+61|wrk(!y$9jrcq-OEL90m|3^^tbdY!9=;}PN9i@m z@YcDX8|Zx_^pYeS&IC%iRB4f3xZfEs$vyW$-+>h=v2IPq`Pm^uAk*p&!K=Bf!YTs3P}~e*eq!9fL7yx8b=1?#NYB^ z3)tt#s7z+_+}rHM!7glc&94jNkiX zM*ix(9n9RtdVDG98QAjt#Q{4JU~t<)}q?@1<$KyF7} zaV7QgikG!84iNGSAc(pzNUU&z%5E+<)9m*ffM39rI`rM)`*oD+`+bzE_x;t<^LZZ| z`!i+a`!hjM#F^gTcj;eMLs{QZ1% zXUoLqO&qofTU}u(Q^TN5B|(V2^U^p&)xBBp=0$CoX2hxVYAm+<704cAXAsZD5U(GO zm~IP;z~pL+mCzi5IlwTztDbB{Y`IZ*hz4QdAt%0AL7py8vgS|ZLf(6a4jR4N|6L1% z=3RqJ6*!e`xMMm}RDx(xR3q2DF3ELgEtIsrmi58{M>+dh=E422($zdF=u%3K#zK6} zFy6qjbX;WT0kT-gP^2a9W@UYM^L&2E9B+su_~yr83$a`&&r5ZO$Zwy_F)ePI`}Y31)G zQl*}!A}cX$p%)f*o~1FE4(XHN0tGo)A}LuLlyc!n87|!W`U+M73BWpb--4Q$%R%Tt zVq!C3?7@M{U8Z)@%N(C(d_#uFFr;A~Sfrug;+z^-)pYr=KOAMmx44_y{@rLWimj$| z-k&>_k-s_>1iY*MP`=%t27nr6rnPi)^2q|WjHpQw05f%arl9GO?vz}uUb`=z1vh0o zB(8;9=;sF&T&pmBHOT0)(tW+6_Nmm5gSXZt3mL^M&VeE9u6+Xb`JmDP2YaG@0*TBq zgN=A=1*UtExl^Yj^A57sjL*4Gn0AaZ1A~ly@LJ&VBXmX7X^<{xN*hcuq9LggW5#Jn z2_b(tDN|4-Il4Lut3bIubsdXylThljll9qd9nMg9ow)trvS)_&4qXcB^eMJ=`J05Zr7o* zjbTv=Q4SRcL6ceMhk-@!L_YrxRY=MWc9otb^$WynUVql`U?;9dFf?u8d7xek#(b@o zn?(HgyL_10KZW5vm_3zq=o5a)ICh8ZDH`0T!{gyT@5ZJbFWM7%IVzh=c=OdFTTL{9`O33CmToeDnt)lC~0!=v*n!1BUFe|{2RrPc5GdEmYCTC2F5{uufWCkChYzg~rCsyF zOvb#WP6?fuMtYsT<7C7K{N2!DW8;W1y2+A<$B}@~RANV%8s!i$v>HVT*hem-K=Af; zo{yc8i?|n|*N?-O6_aVpTM*W0Rqwy|FWxJl+-NGn29Arc85=3Hz_taQR+XoH;?f}~Uqlbi^a9_pIO>^J(J>-~fEs3L z)LX?p721uY1FrD1v)*gkt*x|flCADSYLb$-3(n>MP1sip7draBPAGHd>7^NI@k)>` z@5fWR(*EDfHPFG`@tI5Th6GBuJKBYyHVs_2%-nsWBndX%AaNk)yST8kbrwA5Z&1U^ z8LO4n7|Bd+)XyA3zf%Euu8|CWA_ub7ewX z1!A{O5&o~Mcw<}hB(!H3lE4HHL!wC)@nbI?$+KVGjGA3Lew5wY#-w@|{qa0yWtEmk zZnDg8Ea}d(9uVfK!Ay$AZV<8x$ebRp*Ugw!`0EY$d8NG?Vz!rrgg#~S zQd%ZH9;gh%)@vU7M_+YNyGUr%U==%wP8`8B6ZE&%vTp&rfnY^Y(EDCSM|m^`yl59( z;&E)CVXWK?tv78V2&t&yQ$yv=(d>`T_f@Qc@1PXK@?$ba-fUQ~orNK^#=u)ddkiaW z6L1Nqpq`w&Y;KdlfGcB_1F2|23Y+X5X0Ei8OL2diID$kK;la&_M8$k3vTdf>{#i5l zy^fKv+U$;QLC3s`c!(+_alsOZ8<*UzuaIZHv)y+FF`(b7>wZ7oew*-H*$m@4kt;%8 zfu596h_2#JlXV@{Tpu9Lvpl-JI7`HhVYNVMzZfpvDuKY2Q*HzDZpEyiYNbsd%U**n zo)DzQg>2lR$l!1Uu>C7yo(_!@n;Or%O=I7SK`W1;0HS zaa{sMgO*6NQqdz$y4{at-4-b*6)YqVA~|ttaN8GpvgW}q!EjapFHv;=6w8dw?IG{gmz0oQoQCj|u0!S!PRyXP(6FT= z$AIsdtbBYQt`~&@4S2lJZzDT7(LU);daU;~YW!WKL!><-m~Ko-p?uO@r#x^XQ#xLl zrk!67S&D*PWW9N(_DH<)n>L98#+DH3)@L`tC58rno z8tR%MxTCz-7H4CacMEV6G-I^aeO=JJ0=}934D1H@9OArJJ;TU?gUSSBTjar%*`ptv zI~bkaa5=bw0f^)=!9K5>o<4f5UfIMNQN>IDKtF?0AuwLh;z#B5L`-T`Tv5+CT!UES zzWkxS5l>vx#!rvxI#srRHIZ)1z@GiKLAq1D5$TlhYZqNqv!wDNl<`>U67bUi5AilE zG~qkp)+D!`*qDBY*)x37;KH-wa|3J_wk{m2 zLO=1t(s6@nCV3ZGVf&jzT^jVso^=@Pv3L5 zWCqp%o;afh(P)fOk8b;!8)1sh&)LG2YfE9FSR^ySaH>brlcJAlrnp5)HhKx0P!fAy z(3jEBRg17wYF2reTKXWK=_i?V+P-au!`cb->U?HL9dn#+)W;AFBm!-IN*Zrf$Z@H= zKt^FOCDFPBVxD5R8_TdDZ_I+Tw`iig`IE}|$JyKjDfldiq zp^>3(T`dw@Q!9AWC|Mz65*l+$2jxR@DqH-w*WqG=h0x0VLv;DGB`{uu7ee=v#BC?+^d2*BuOQ+KeX4@{*mVx$ z#@n@G_wW~ic)%$ZB&J{8@WV1^1CGeZ9t$4PBIL^CYXMy`&~tNZuq7Q#g&kkEIdSQ| zPORCR*08jEaTu417yIAeUCPKy^JI@!=1~|Zyae`6nC%FcP;}{$?6GTeeE*a^4=j8V zd=iYG25)~bJfS;~zPDc?TeHLhLI>4IIrXEx9FUT`q^Ik0b;*-)eDFX z0?-XL)K4c_i?D=d91FxSq%f>ke_Kf{)%wkdfm7`53$87t8^mtE%7Zxna(B0vw^H9z z0c|V;={$NPo;Q;6UdL58q758dy#N7sYT~n)C(iz0iNvMJXS`n!_}x!DGQlz9Lffz> z`_R$`&4V{pqT>^a`vq6;@tiaZ$=i($l!0H?PmX)^XYW&_P};nR!a2wPHd|=`g4+8y zvFuRn3|XP7g_?*gGIj&(VQxKl3t#ra0^@X&H|6v$p_7;oqn(z#VI0T z>L*eJyHwLz(ESXwevPP^3kd z-^hW#{3MbB@h}x9FJ*CErO07wJ|ISpP;w3p4f%Qu{wy>QgZhMjhc$w2CiRfae z^{#V*f}Cfzfu5HktY{Pq-+;}!nBR{RmeiV9%^J%*b?VL(elKshFuc*4vISs(s}Ymz zxBl{xJJ=GaD5b&12g=rtKIph!mqG|5E-Vq%h3VLpff01kVp>6)=b9&JNq^o5s5Bn65%QQ!g18F-5?mL+N1F}1VB7XC z0?Au@Kzb-c#YvjS*iT;_IH=l8`E%Q(wHIDGWWWAL2aDueBL(;#`#0pKQq(Wxzrd*x z8P~iDvR%`HN{1lYpsikE-A1{f)gY>vF6DQ(9t$Y9nCJ;l2H&X~X~phP8u#5}#Q_c= zjlERkY(h_bLPXgq9KBY;rcEsEw^Wq^rn?$>(j-Fx!q)NEGoLpvKM+43GdrKGKOaax z)i-l|uRAdJ`DNSKy#}CZnRyB-G=fipDynWz*hP1imx~^1f^eRdnTJw_xaK#%7#54N zYz!ROneI1`g1NTfp=j15E1n( z(_*G<(q71*VtGUSB$xdGud~A&y2rJEd*&3R%drNhTDdC(x;xTXv_J9(+*?RdW0vru zbzYq7$|zb&P21d^%5}I(yk?2e2j08}R#g|o{DlI8AoQTe5&5Cn-~`(*PjP=pR)~Dv z^mb*ybNTh=&xUTfrl@FRp@G^0w|a`0Z6}>#c~i5fzd(kdDUB}%J{)|LfQ-EP<*>V{ z6FQp+I%fhk+=GQ2bbh(4m;m`7h)&U3n>#~%z_R}+7y(#%`nt_@L=ZS0h)Sh3FKBlg zsa#VIm%{3dv&^?9hBy;yI7bM$MG+>KF9|X7Lp3{Bco8u5FZUh}%BpEG`=chO*t`qq zPd{pcfA0Det$E(hwn&J&PG%`>i?9!M5|{vwuQ(hgU*iX{?DM)k{%CETVijha1DeVf ztUZCVglh7)sp7V6hrSTqIuO&iHqU7LN8ZCK(cUgsG9Xia^r6zMr!dB2tM+9w_4b6Z zBpgle=}8&KB5DEVnDFvndEQ2b7a6S%cNZN!EowLTPHPAZ2Y%+|QqEmdPk~i-9>Jan zw6ya%eecg==qtJkT*&(9+omNU{hR}9G+p>aqaY-?k%e=S8SJB<;Ea?zE>XK)7ebwbRI<0h_a{n@i8S-^0hmka)podaqj(;Z-=qz@bkKlyc6Ib&}ErR zu_TL?7rwfC+p}G3I#he6Mp2~l_94ztNM|^)WJJ74^b{zf2sGRB5`7%HOlx+gsfF?i zKnT!!l^!Ij1J_?O#1v=R$YpkZ!2fF-JbewmQq%Ed2BWyJJFI|P=x=)y9r^}MB1@*8!>l;^k6y)FLkP;RY7SfZIb;CL?=4{W%+Ha(-m%v@~v&&HI#Lg zUbb7TE4_5+@+^M|gT$2D7})KDby}L=_KTj3Y3)JJu?8Ycj2q8I{;%r?Edh zkTLqbQ-bhg_zrwx5Y}#!hj?Z;&YivaD?OJ0%lg#*q+I5xHn_- z4@W`cQAtyq>V!h)QV?Ce@hL;n__1l|=nabNwhi|d?|4x>o%v_y%~ahP!hsO2u2)=1 zz)tmoVj+n(Sl^>;rA07`ReDF8y#y*{6zUPAVimd3rcoTcksncYm>N%eC&x@R;{D!@ zyC(YZy6BGDyFq^+RRXH(A=LP5375P0j+SzHpDI-gc;Z_5p1ftl3s9(;fEX_#q}9y9 zA9{+S=>CzY>#V?rJM_u3^Z)_rOhj4H}{(<|9e`RTr1oB$D!z zIWJqQ_7t(+OCG@cy#nWcKczok8+Jdp8+JX6DHn+^W%Pk8a*1PAe`HR_Aj*kbXlen}BwbM^U@ z#35V1_Nz~=*86=7v%0f{S-hrTZi^mj?p0+=O#8Tkl@ z8T2k;vYJ5!q>58W{)*rEiDVF@#9^r?jP8Q$#NI1;{jF1ex>USiA%H|%TWB9l?X^BW{0 zr-y}KeH&~maHDb^wuYu*Q#!-u1$&<3yLAh(aA#oga5f~DHpey7z$`gq8TqxbcEpID z{1{-IeD0G`3I$^B70o^H zu!LyL)n!61z`ao!s4`Q^MiuXk=auJ$B}Q)+EMwE`_iLpKz^QZ#Md67Zz+|P7LS0!Y z8%}yj_E(^D8!BldqacN%H{$d(t#Z zYd!eMtN%2xk=De#2%-i=`-3?!3hi zWMTi|2%ouFv=q%({utnFZe^>fUfLyA9rOCSI8}ByRtmgxf5KwiHQ0=HfjJtaclW4k(Zvj97iLDsczqaGIw=304owAi4uAkmh8$V zc*eSunN9s`lXmVNhffyrtNlj8Q}cVVrIw%HpK17-n~0%--bay%qZY^{5#Yk0m>HV3 zOBAOFZB>ln3i)M`YR%F$5pao|QyfqT%p+UlS{axBQ?xI&xGxr)HmUOB_W)P?7=j@c zI+A6^nE^@XtVrs2mpVT1c@h*n&y!m~qe z2uWORS27I0OYx>)D`vc*1$o-WY^)ky$dvGNhAfE4=8*aDPhvnxssv0}Z`kVE6c|MNVt4HU&H;fj!HM*moY%bc!pZa+I>7 zm-6B*8!WsS^g6!6&ndI%xGVSDg*3#nVuA#~ccsPI<|-;kF}_I-yF~0NY97UT%EEH9 z;ee4Ow92_9n?FlAEY3B}G}$hhQteo_Rs{0x;=_a{zD!;Tct|%VP9VB5F<609piX#< z3DFTuQ!n|8j87}cRxhk|g%n{9mZoYVpKM2(JM}skg|ENDZ*hCwzy_Qkdu5-F6cT17 zQ4m6KU&lI)P!6i9qi2o}%~Zas1Ej*b@%;_Dm@Euy)Lqib+Zco0O+=%7UV6vB#S zg*v}Z=rj+B@2xOzYT$3`fM+a7zT!gjia?BqLwBo>aBea~Mbzv}>8E>%QBxz+Okl%J z2mOUYxWY~9C@N!hl^d2Jz64}i;tfmyFpq(xd z89I!ao;&=_99}Jss{d+3w$dVENiE0S(wQKO{civK>g20>ANoOxT_ttF;sxfbdL8i6uAm<;leR{Vd)*&d27;8bPD~Dug$eoHR zZ4LDC3GO)NHDQDcKxh8@kME^AN9r*@H=$4mE(bErH2fVfm1wRl$h43%*;OEL4a`}c z33fc+u>4KyA$OX!@-S|H{zIc~T)b01@zx8%ZBB2eF=yoZhkF!hj>Q#1rOzFok%gj0 z4u_BWUE~jzQAh3QwOoVmo$p{Zf%VTwsvFshvDPOa?LVwYN=iOQOs+>vzDM0cD-mR5lSo8Ud=p1+@qJ6p{d}$Q{n>dqcz^MoitV}i!T3CFg*6R- znK3h-t85G<@#x`s5L0l|6mVM|R=cs8D_X$FhVAH~=~i3cc6@g^n-{TVA7YR?njQLk zXb3|P3y;icyTK*Wy+&Tsb2I<73hr?`+$<7WQkr6Vc%=a|j}>iUDPxXZJd0?bqm;ta z3EE9$z$r)2(OYWZiDe(R9K~3}V_i7T@qR>CFeuF!S^5leC1xAr$H5iJSPCbC zWZ^&)E*y-`LreKJe!Bt^GgFi9#>7lVGIv(JozCTTSt$?h;wq8qsd36Qo?15MU2DK6 zNy$Zs56r96MM`LvE*in5Smu*!i<|HAX+=`F8GVHe zMK=zGpqv6eeXWu(?GXj1Ut|g%?qJ%DG+Kl}ZY?4b!-swY;cYY08PiC2QJ^X5W^_>X zglK)sq5_UJF}rPV3UH_YbY#(bSKfQSE>Uq-SjZ@e2puDcANm)mk`p?fr3loPFqlNqd?8P<;r7fk>X1sfzBzs^I=^9iCP!G{IXQ>T z(MS(98E>r9K7@Zq&pex5djnB0ucDE)bQB|eyzJtJv~_L#BTN4FiAUZ(t!uC=c@QPt zdXLL_KVcE;O4!2XZ|2DUN|LEZdZhGtfsf@UI)V=;Y1g;@Lx7^Z^k2pJ*RB4G?uF_i}j6%S}QCY25C!D;u|=MN{roq!kuK8&wGO}r-|On*Ubd+v2NNk?-R1eu5@==|wJmGt$H*h9m&I$k)&N?Cp0>GaeJLEl zt<=^Hkg zVK)(R-G*zHMXKUj_`^aF!)bvC2Y;~1i6E%m5VhK~OS@6C=eK8-*OFJCmQn?;mnVeo zsjDjdO-)8^21aJRX1bDq{O&>U^JE~h;vt}L*6`C=0YM>VIeei8_%vtie8xRLcwuD= z|2mE1vn}WzV_0_O4rX2`Ytv4*NM{xPxz&qN)y-MjQx?5XR>g_isDJT_`F{nS1!DR* zclIP|ru|F}!6IL(FHbX3b+?Y@7M)t;Ql&c|BG%V1BrtJQCWdvcB?Pyiwc+s?H|HFQ zG`P>$tX*)nx}}S>Z|(?H?>m%6 zv)(Ke6>!C&_m|_tc=+Bo7xIBZSq%|WGJ|nCi|JlI$4~OaBMFh&%$Rm6c28Jo6z#4| z6N*B~!){64bSN-OCRfBcVOq>j6(yc8jvXh$s`+&q?<;%j(GYXR!!B21o6H6zJZ-1?q63&g-!Y8BG(w)at67*5F^&(;LV-sqpxskF zjLO5X^oa5 z`hy>NWY!aF+AIC|h)89%LpLZuyWIm?D|exAUlgX_j}B3po$?rkd@^+ya3(X~(vIt{ zJC4n;&j}9XhHZN*wji|hXz>rl1K4a@l2=mn!e7k5SRc=?k~J>z_hB}h{?;?6TOSE= ziqr?+T|{@z^RYAPadF3HPz(>(D7By5wZuB4tW#8sA3l+%zQ-i4V9k|9(m+330UV6| zeh@edwKKzs2F>cVx3CYBZq#x=h<9*f`y%iuq!djDv;0Li%+dc|3mFTE)B1w*QitXR zbR3<|tL_aQJ|J#4n~^%W>$l0)Gl(~!Lzq3L5~eZ}D4 zB+b4o+Ma)l9X8=|cpw0sT|u*Tekk3~pe<3B5N2RGAQ z2YB%Tk~RsxUh}`?#0!r9_w!HIVP(ZAg7lWt%Z}k?Iv#M*(|Ph^+EYjlS1m9RF^zJR zaE+4-UsV$mDUDmXKFOl5f?_v9Ju!1f%UqqdTx4pL{1uh%pC{rD!_PTa)zbBR(qrma|T z*V2Wefa)s_eQ>8)x)CcZ1soH-)FAqIUit0cdF3y^@XFu(Irek>$Nyvh;rPG#Px$7y zzdFy~{y%;H-v9sr|NmoTfB;4)&C0+6X2bY=K->YuY(UHb#GG(-5I#s<1d@6tD4&7R j(-)@S14%y<7X8Tff$V36@)8>c(AXU^ERb;h=B+jGXYZQI5f+qP}n{Ljpu@7`o{vv)W9HEHv&Sv(1Y`Olm2}`oG(h>^)F*S5HH6gUO z<04dcu_Kf<^dKZ;B4lFaV&ULo{7uNr$izxcoUb4(taPkM8Yiqbfj<%-?gbwj1>WrCSkVfO zNctcHU6aA2kb^+*KoHXZXUX75aIc|Hu8yELTDYE>@BV1GDY)U3b~tZ=--x6>T?#(c z3Vb+AaAM`0;S9irJElU3K@Grk^$_|0k(B(Ol;!^)N)rRayV%zlp!xrw-gB42^MCX- z7{FKr-cgWFbjeA7n^D38UAaSG{6|pse+vGuZX>+>NAiDl+usQPfA9AwNC_}55`^>r zl@jc~q*VJ4H|NO^v5u9%#C7cXW~R3Kj1-ce>inN zm%l%Ce_y+)TCIU?R+A%)cL~g>Cd9&WoFVA(et#G}m`vN15!K+4UaY-G149 zmw${q{H%MMbZh)3`5v@2{C@nt{h9dY-<1C*_&(im(_cM9Tzr4@`F_WEedpI-^?Us} zCHOYKj(dA;g?7;fb7GDi2H5|+xcvB;^56cv|M(94Jjvqn*Zv&7_I_{ud~NI3{(M*6 ze%AFCY*yF3jBftq?S46b%^JTqW^bqU^7Z`u`e7J#^8@#LRR{RRo@)Q>`ZU{r@q7O( zdf%;fKm0zId!B#3)IV2#5cRKD558~g*Isw`dOsn4?%=fAG1?b1uK1NByXhCozcJ#XWGCPh6TRj~lcQ zC>WEFUQlT`*jT&7+pP<+3rvIU4H%rc{{(O#l#YYt96lNHGyTTJ(sTp+o@fyU(YGm) zctmhSy&X@|^00Ef@L3~N*TnJapt@z!0K&R{t!I#giBZw^P^qJas83$8k>~cGBo^HP zF7lw#&g}3%st8M;GmoB_@=~=w2o2(<50(qG3wi?Dvm=7H7q|l!n;RE%krtFqJTSM~ zbl-AN?z5y&HCZN6GIE0IBi*s7XCGGs>>li;WCW zQ1Ft!o(m~e6eiwvx>WID8I)(XJXVbn-yl9hKq#=zm(nMx@xAwGu9_v4Y4ofT2Byff zFLFCp`Lei%a)5bu;6%8PugAA|R)F#H>nDA$eX2SG)+nt8U$0fB$Ax$&l!LE4bW%}; zm>vukS;Iux;?0Ji`OS8J*rRPJ)O5E$!P^2x&=^oz_^!q3Y;mS0jBGH4Q=r7FM8Iv@R*+FGyTJVgm8wS{ z?@hs3)fUGw52AD9y%pm@!M4S${X*ce1NZ4SuXdmNL*k~5Q_!;pPZCR^9b)W_YkqJ8 z#&5cY8ynf4(8pYgerss?tt{0psOEbIhK=<4_vQuzL})KUTow3gbE%gio?f-mI)b4| z-q32H=51j`X^-~URvgmVk}VUIZPTnbMr)=ViMFD9dNBcZt%junp9k|Fi)a%XkOe;; zb=w);(ZW9hGp6)1XSN}UBc3FD+?mt#Wo)<#3bKs?a`L;iFqE-wTrP2C%R zv}}^(fZUP{HKCg~&WHoc<008#pcEFV1j=2hNUWyVhPvV^wb6;FS4La~dW?dlH21k8 zfL3QZ7Y(hWFzhZef{*YV%$%OI>q0&UNx-8A5(^hVz{y4`H2Qz7Q5X#7HQrZ<&44xO zoA1a}V5%ql3Ai=RIpaNxACIJk&re=aD?Un1p5nS>z3wnU#JcoU>%R7VaMe0CyFSsx zt)4|Ss4jKHuUE$nOjT;Ju|o^Hsx;5pw@`8(uUA9i#NT71he#-Yv*h?KLuUd)Z~_}b zkh)4_v5h)c-Q@)CNA1*5T_G6nG}KCCWEp`01;0f-8tLOY=~te2TEixq-T0PS#c!U;f0JfI{ zE9We!940g%zs7%Z4r4&-mll}cjjkkJI!UX>a4#c8+tMB=EkU)5P?Kz3gw9>hX&^17 zO>EnEhsJFbL|Ms=HX*JP-vOa89mw0%7=U`YKJAlYa0TlN+=wtKv8cSS^lrBaMIX#$x;pH;FMF3kZB&6 zKNcsTSl+^Id2TCfVlSyXaP_84L+Xu7KL+!^)7z+W zwZsLT(0N5#;OaDHy@LYze~XrP8-GCKqp^_8X;mS<>XK!gO?`s!*% z`u*{#8M@TG(wsp6^%U5r2--^&*QVpVY1{@6$V!s`P8aQ)P$O?gnU5syMEsKXy4Hpb z&u>4k*IP)56;eGE`ctGFtW%2w9t?nEHZ&Rse``83uL!cgPUH|N4fGX^ykDc6f`=FZ zsgcmNu5~ErG-?QE>W04qj-CQ#oQ=_P4|bQt%;PU%_8SmBdrN#?s+|QUvWNYf+&ZOz zBmx^nvJGb3sy4c(w&8Ra55ayd(u{Ail?-}zsQ+{P^F77=^X2B3|Fw3B_e zv7LlSRa4z6@V4EpT3XwGfmfHsCzt6Zzf~&S2sNgY3sA@RQj7gvQsxMoiVn9uFGhlZP`C*HKPP_aZL&BV*FJB*>7UTin)sx5qHs z6t-41c4i?Ou_z17k;^HD+G(+{h+l?dsn2U{0{FG5ZWh2bMq0v6Pt+dy;JVi9hxbNP zamck2yyjK_TovL0il@k=GzD^|?uyJS(?o74A))deOY+jm+eh5gN4>wQ>bToXrx8Cp zP?8WLUdw@zxYF-ITHB~LHcMv%QDt`}*rj)Z3AIW@=(SAw-$Z$xFJe128k5OOSVUPK zSX3Z!l_hO9G~HQV@KwDwc-0ytp8P4UDj}9TH3n^n_H?JVu0WNSPhzoukjysM$ct1R z!NiCiYYVTgiLq}rr7cnpvvZSjZ+F_kXPi~eF5RDy6oBXv#9@KQA*S_-Dcc|4h+rbD z2`AX`x-N7@BCuF$M9Ch_DpLu;y#?9kv9NC?loCWbuwIbt*@>zOs6Oed;Y*dwMn7G& z>-{|?B9K9bY13CQbpci(#V|v~iAc7nR&<#}=U}uJLwHpn$jVENaFUa7qZD)BO*Wp5 zBs)t!2owq;Ee9SDO$c_r7}2B&wR*LTBMK;Q;SFzu}QP2S>6E$CeRV{(51Ck zgJ{C=?=K=wr~U#dQpN;lYdA$VMRF@YonuSN?heN8g*J!Hv#9ki=D(sGF@1NQa4bBk z1pD~f%qJp&opVpFTX6{lG0~sd6f?5EpyQV=>REZn84mZFVFX=b;QEmGy}iy=AaxOX z&2DxzR)VefT^mpEZl;jK> zJsl0kEBD=qO<~dM#1rd!HJVva8cE$Z4Idh#lBQI-8l0JJj?!;j=?Qg(NM(pm!J$|M zVWE00FPEEdh~5#gUfDH~ zJ+f^^lWO>Lw#t$7`kJv zFr_s=yTBIrNX|V=@>tk9%OCy$EX8G%d=H7SCf zGrJ=NLb@3kU}-deRnji>f40skmB-Y6D!G}2B#~x`Mihj;Z^!~0j5Y=?e1-6IiI={- zAI0h4Ar5Mh2Q7N>pZWv~KBV@jjM`T{#W&)@$whSd_%m^tB+lwNG!zDA&hSj)odVF4 zCRi-8t(%`vY&CGW#sX#OhZRdP$`tooAR`peHlmwTV+Mrq&SgJ4)msMi@wY#KiKw70 zv(uevQ!HPNMupzPAVEiS3vg8lL2~vv4#S_3K#1?&u<@h1f^B^zBEZ2JLGW^>ZHZ8a zY=zHTOY=WoXcp-hZZ{>ahsBzWp}VhvStWzMFT@_P6=?HA(?Iv)I~l=zdU@)(x$S!I z3#ZOP&Pb`qHSrP%Y%t{s)2cXRhUG^OFn0BN%EIZQlL=k2$ZI(MO+UYY$$g^Nku%p$ zlq%UJhEbZGzh=N-1@CUD{0a@9Py!PNVb}~@v6;Rt|NIGzEDco<-r~v zg7gdh1;|0q41IXRcM|Dp4CopeI9cD=9`162Njl;tyT90WkJC@fGU46caJo3i00vXh ze^GmpS0IA}4x7-mS|TG%T*S~?nX46v-L>TV$-9am9e#y;wN3?>sqgE8FTV%*aS$ZZ zT+*dLME2=WkR@Il!RSI=927fVjh5Z9?Nw@4xY-mlSv zS9~XG+SE56s}F>VvviulW6clnZU0#R{^~bA=sVOqGU)kb*XMmifa`qi*t_OVdMB$A zNuW3KOVs+j`KsQ^V%2p%ZJw1&J5sNS7w63t**pZuM8ws#?r2N+8RD_OkJ8Tv=0fg3-(3}Ce3N9uY*Uerx96i$>nF8h(zLBJhqWSB)Szuc;U%9P;r z@N~}ozrv=g^cJw?F(*%+?s>X!$SJm?g~MFp{si@=iTOK@%pgie*VxaJnF1&$Ov_Rm zac9T%?7c`JDzk19ic-5FG*_>P>305D+7(86ee_&YHKZxp%}@*k`Uiw7O>wvmOcZ_Q^&T^=P zJDBd7mq|1RkarX3kQ5Ql@*70JC8m{-Y}lTL`3G8l4T=3df0)U-%N8f@fU%;HKEiJO z(ybMP7;AX#S~6tF3^i;Ar<-sGVkNq|>Qh??;m31YFb$`s2i61mwMmQOi=c}C844b zSggYgq2UJT2*|v#io(cm4$Nz18U0!&lu0Y}(ixg>De92R!3i$)X_}_dEWOXHPX`+f z>sr(34yHZNa8E$dO!6m|DW9Nw`&AxXsWnw#3wxi@Bi1;JyB0RV9pT6U%c0=c@=)gY z+oX{K!DOQ1$%uaHY1AL^>2*Y6< zmQ&6$S7Ef*bO|+oh;4mF-FSMtk-X2zm8$PTM5LY66A<1?-%!n%vU60HFpbd`XQ7qf z5#mbIQkMN^2vAQ8M(a(1+@=_6c>iW)(zH5Ffx&XKU}!n2CG6wDqjIUVhoSN z+rm<0lKgm+ynDT*HjHVEA?webFvHD&htu9w4Jx#b?XgiAiu|(`#LcX>8$yXI+vg#< z3;4cRgj%Rn21%rcod1iju`d4pi{v7AD)XpnFwFr$;Rcxv>UE2FIAmeX3z6u^!>F~gGfSg; zp^adcQI5`*TA^@qf10kS&K&f5B0JGYvH$OrM79Z<%nRn`Xj9-(4f`>diXHVOUQBw7 zj~>5|_T`TCSTn@LRSDOtaV&KmHnx6?jGJ1jqkyo@@7$P{U9W*2Y=F)SXr)>^aM|YfXE#~j^PFY9#y$RodgRnEN?hS?5EoAWf#f5GXWpJWe7gv+tiZ4Lt*2pCg$c(z7l&=*}`HYCvo zRPF`}bDQCw+cf1O>{Y8SFor?zcfIeaAK=Y;e^4T!WV`TFu`MSL`jl~!XJ+p9^|^^}jI$g8RZ|kel8$P$lq%K#V3>+XhCMT1IjsP2nW1 ztM>-v8$++bdi4d7k7b8GR9l_{G#?LLx9PPbwVh0uWhMEHe&&qeUMJ!ogqAYyocN|~ zASu|C#b$PemFtl(mg=L9t37U}*iPzvEKH?v!qyyoI^iEYY1m4UpL0fzjG0-UJq)t6- z!~PL+&oD4uDPm{&pedG1g_rGFh&b!-^LH2d{lCes$H`8y!bC}s_yhE^3stidU!)BX zzGw^N>t4WaO`Rr`=`-&js&33x!Xt0<-TX(hZ_Yw^w_Lw$tQtu_BM5iRHc|=gvJrdO zj6*0WY)tbQ6CYO>X|50FlVJr}kEO~#_N%6!WYpAh&DB)Z*`Yw`6F)fem?n4AC5g)8V&2>77GZbJF+Eb zhx4_8beZ5pi|zr`Asr;Sg@~JTlAe26s_($lY!txff14=kaEK#KJNv`^jLDgZ|u=>At$^uny&?Z0-YZ&x@!B1=LV?Y*-uB+f* zy6*m+lA6Wqf|PxLKFqra;7)F%v)4!ua6{rrpa^FdFhIzh_CH z`nk(h7rm+PsO(xJ41A=&r>>->{j%tVtjCIoV%=|){A#%ly?H=*G?zB zEq$bXb`xH|0cvXh1~WfPR^+mNLkam8j=Dsc!R?Mu%B?qu7=`UNs~=-nDF8-O9-zfW z>Qz(SZGXIqMe!yqRgaKuz~z|7yij%lqG}i%n^Hzh_aqM!PIDQWOZo%_*TA*kSDw2~OOTPjerQL;83*f9efQ{QO2% z<1_EwYNs>AB%4sNB2T(N%yZXik^OdbMr5j$-gouX%k9u9o zJWo~epx*U-%8R$90w)(^tb!8pOFZt4wDEcT{`myL$IL?opL3^pD0AO33Y>Z zD_jGu7`LKoHDWZWnNucFwFhHIw0{hvPI%l-!+t&p+B+jt#wtzXCIJ^+=;+=!zBbf_1~;h?sHt3zGPLxk@8>XYeIdpS^6(P!^rDh0kZN*udwnx8jmhJ- zRd3(;AqV2iwPtN-6@zVV&5n-i;R?%;JzC-%lD4QyTmdUEhckC2Y4#w^mM zRV;xsgyEvz=alP?vEw<96T#N4rDlou+c$~8kGfB1(T$yMHQROlaZm!=XX$s=T=Aht zIXB!K?fv`f<-c|IY<1mU3!hs8*_R1p&x?>rhx{waD1%oxkGL-{qJ-nLC3#X(^HBpr zn=0@8-=xmntXg%2b8E^sFGp`Z14h}ek#C1I!wFxHsDsYcvHG4_eI^n1w-V;FPBJxyYl$0my3MytHJ#8jw zvjM&4a(ZFicvmCb(+N)^}_6L4>tbF$M0Dbp22GEa5)d^wsjqluXL?n@QcQ>s2?-p zEm|ic+*)w0vk)V^2KvVo6)nJ3YEuQS?|26fftXFEYyvhB);P7D`4lsMT%Noso9cAw zz7Vpmuqa+&lpl(10mC}tzdRWgyPJHUbfXcFr;zF^ps1i))I&4#i6;H-9|0(0I_K#Zx2`-^Z~MIm!=P3D1v6=SuETnnrr`^(HP%vu z?NzuBO+nk>f;2>}7v02WY$i&9Hf-(yEHlngxeRH< zqKybE2cH}t-@D)UKee}IegJKsA7g;u!fQ?>_7IJO8$wS@ebzLMf{CH{VH)^M*=g?~ zT;*q>eX5z-so<)Xa#N`JIhDo+8+^626K5QWU_6545|6lOT?(5a+sVSy#H_e0udaVjcH#lck6ug!}}KD~hmB zXUDqjbEM0#HjR?cN<=#X@5q4w0T;o+?8c(>Nn^J&@w-;Oa0i0%i;I=<7x*t5b!!}R z6Z#At#BXFs%1>ln#Hv5^YinedHOUPUH`xZu0#cU^AM~$Q$Ui=)8>SEImFv2zMiX{M zLoJ(kgievj3}(eXf~7`2%HVB{nv<%|iEx}`^T(fwqS30Cc1O9H)C)4`kJr@>q=XiA1kap!D=Hm_r*6QN=X*Ga zQ#jiJ!7~B11(3wMubnIg(OZeH>$UIQrd+U@&k3FKTdZ5lJ$TAV6H%NNXV>p-WQbHN zQ>uxmI)ks|s0Jzqx(bsTx?nRhzXVl5)cN(!tWcI>2slcB{80TCP9eqtj2S(5Ex#r->khcbXh z`}bpOaC4f~Nrm%?{ue+9>q|SkrTIq?^qt>LKy`?t+a+b)%8wbwEY<#L@pC zH+u3-Qd#T82(zdz0L45aLwomO>;T`M9pSSSp;Oz4cQ|r`EESoz)n*q*0SPZ)ZLCA+ z#>@o8sw-k!7Xt5m{5LAM6B+z(C14M0LH>yWTQ(|gG8%NDR#ke_LEx{>$#>w9Xknta z5?*;YRg0~dT{G8&_b0QN0Lm=yz5KQ;_xhx=AU#+^mJZ+XvyA9u{)!7$ODtU>oLM|W zQ)Wy>Yh>jd-+dXAt;EgyKODe9}Z7wMx*z{MY?|)-EiQ;_D7zw3%gk6d`*As zJ~+>!;}bp87(8QuctSp`bQqBhg7F2qsWaRnkx7xaT=d~hRZNg_3j0(|8C_S`33ZG! zJcJ}3%&fV;ay3bGzg^M^k1G;8>J*!5xt+OcR{yxopQ;1l7&q$xZZ0t0!aV#7u42@& z&}C|nUh|*4@mqS;j$4Bl2Rn{KteT7=j86d8JISPZHpdTT*^#}Q%F=v$O+jj5?2{y^ zPEoRRwgX}4Vo)xQ7qVOfl z=Z8g#bUM`54O?jZ_S)Q4*WVQ)nO12!Zyq}KGMmR3)&47e4+4qo-GKl?y@Uld@EZ@NJ#~M9ktFwuT`?bWZyUXFZPMACIv$^L?(tvy3-J~ z@}A<64|Ocb{%kE6=QX{BH{( zg=K@ew^xt9JUPx-)_cW2o5neh`aAX&swDmjjt~rq@_Yy0Uc0UpQ*vL5LB{}(PbcYp zbRK$p<2}2`Rp2*R_e5DX5{%JB(E>MANoTz{CCDDu=^}i;!uB@UJKd`PZA0DViD=fhy^C=B4myXrwAYnc$-wAGFw%ja1R>8jBt|ZaJ zZSW(UGjom`ED^g+{he|m+Oc`NIxIO`U%rmDyUgz(Qm{tJ=8!57ThFj-8plpL6Q#_< z1J@v>curo7_8Sl5b(5KZFMA~kK3vW-U(NkbXfmzdcNPT+!!kDYyY=^W();G=4?j;v zVTDUw^UuXN;QjpvQ68f6@Z<{+M)7$1pH8e6OT}BeC%;#dZvfzj<6rOWnn%XyE^o#= zhOZ~fztNw>OV-o9p3J@P#GG!&PWS4!rlVF+$jSU(%(TG$c_M@GMDNGl`2^Vmi)YN= zN8={)T3f%qIEqfwUI)&9A4*K-@nC%{W1VmJtth;8QlS1B9$6q|(eF2=bkna`leuuf;{2FAr zx9?fWsw2qV(RX}I@tx1A+dqM26Fxn;bb5T^PRwEedAJV#IYmAe*Bv|j0=%fsVo&eIxw7x*i%Zh1T zh1TMEe|Oya!?#bN5+(2i-3V3dbUd8;Ui*FV1JeC|xcN_ZfBsn?bY8+oZ6PGK+GAGT z%JRnc%Lygg!tm<##%p0AJcRhJ;Z5dA#|VY9I#wQ9}X7@dHoV7&*DG$MaSpzh?Kj8FQaPCmSCw$g%iR=Lor)2ctlUUY zlf9!Wc_hvPz<+1bqx6C*1qN_j4rW^LiKoO0fDN6ADAdnASeg)WZ0*K^E^$Jjh(_F` z1}4|*TIy)6s^`^6G}TS{le8Q?RHWNdUGPHdsA!&N>ycS@T4#u2FfiV(_~CSQl1(jb ztkZ%m$(ZV$R;DS$ACND4c}HhweXcBJR3Z2Ehhz6u!6AOz02CohD`;!`2;asHjoxRr zl#GF8T;~?OHUcNH;2?{b4}tyT{ZB;xo8r=i*O$hehd~r>PYbP?mowo;E5d+EM7EZO zaK}PgJGO|?Cmo#TlLi)ZJ1!mM;xo^#-#W_+0!4vJu~;Df@%Sm@^B%fo@iI(ByITgk9VqZgIswUawz(%fki6*txmP94vDG%!|U}Och=be_wQNppsWdN zT8xtc%0m`@Yc|R_BhIn(NJj?O$0Q7f&1$fBp0Nq6nN(@&^8mNE;b~72a}k~CdBk~^ z!daYWkj{HDRZYGw6T43_&~zfC_yI=}ae3g1tj0C>Bkl>4QIF5|+wFJJuu;J6L<0IH zGgT)Q`N)BNVbISR5Cd&Xe1F*xMCOB5@<^-~<4yLg=IL5he;#EulIc)zrbADx{}!Iu zYrYT{+#}xGt>lz+L}q?5YQQ7<87;sjLR3ooY)(y_l&FAIp+SRHG(YeIpfYe6)uoRt=Q~Z&FeJ`3R~@<%UW+N|^G7r3=vd0v<4fhW><3`2K&5vqq!!TGc2fIG z6cmkih%}R5LL6?J#U+Zk!#m$wKxG3bIt}HIX<~S`@riT_i97aPao95ZNPLf0(6q?2 z2i&GA3K+o|!tl;fXSX4_eYuWaZ$5Zv%Z`3^EGg1zgJHWm&2u;bso!9>YU4Fr%7R}z zyw)rTAU)Btju*BFbL_e2Tr4mRXz7^vvrL~zUS0*}M^}lSV!WN4N=v)B2#MvrxDtgo zywT{F3dRi~sDW`$Um(5iypI-AV41_^y7_*ue~hib7-`R)qd#kR8XYvd|2ES3L?L}Zfp zsXqM-sRL^V(r5d1_Bf~SJWcUj?q=cPN3<&?8=;6n*Xw zxRB)$#Eg7N*s{z!!38eqfwkJvm6&0m{tj$&*9A85T!&Y&S$)Nw+2ZcEa_>!1ekXAJ9L0#7F-D|}^p z4>sNSt2%Dd*h*qZ&n$aXxaxkgc}X(%19#HKArqATC8zwEa5wW->F7K*40@1>T?p*R zdN+%eFeaL^G^w!5z^)fMGZt_`en;+p8~X3Hp243+I~ADU#K!g0L{mL~bqI;DeXR=x zBTG^{js@mi6%4)g;A1h$cu!KuvB9$t;M29-gC7@e7P28m04gq zN={pR!_7?V?RJ~zu+ZN2S)-IVRNr}A2w?B9n@x~6`X}@m@2x72vLh%JwF|@J%iuIX zd};@>Qq@$7=?*6Z%WRl+pe@^-IOwWl#Sm!>BaS%mT318-&d_hsN0`;`3}XYCPx#27 znLF)>S_i2p(~iLwN0$+3-;Sf-p^|={Bb?SJFHp7*Tl?u zaTD178uG@}h#$1wskGnL6j|!OKZpA)e5!4m&NM4TMo2ko*~jebt`?^Htck}_Zda2H zFN0Ih!_}{ZQY&VONpDrIAEd7M3YH-%7EsP&pJec*P*w~HV z@z18O{rFUahO_pAyOjHuv=&b8;UZOeixj-N%u`7PZ^Jo6&0W{X{T%0gkM4ed16Cn_ z9tnP4T7LjdKga!kfYrR8m%Q(q9|HWRpSQYe{aL@y*6%+q!b(4n{1}gOKCA5)em`G% ze~w=~@w`&s+#$oZK2J+yMbxmF6hgm_Ew7h>VbxXd!{}B^%!N0Uyq~?IP%@vbM@=cw zm8`K6@OP2bQyyqdK~rPa`uPf+)5u#gJ$7G(DnW7`>Z%}I!l~Pj7{b{ zB!A_FQ=)Snx;qo*EsPNR*gkbG?ZOs~PtUwI)OosHKRHxcxTen}j)C|Km(B@#I(5hQ zkw?BtQ}ax9K!=rFi7$(!diKMEVxWg{exz{0t>h;TOjk*A)pZo)4~hESc`|$W$c{^{ zZgydqNDhx>J}q>D?M2y@@c-q_u8~R#Nd`@ku!YeS zd!Q)bA`t}`F^$%oHD#^wrX~Z)_fC`xCyp_h42?nW+J&U*dvz8|YIJoOsncp0)=kl9 zX~tuX;y`gErG_95@W?SK`KpehskHzIaMi(JXzvM*aB`H@L>aXm8|dg3 zyS7>+SEI4AW&yzdCmKjP2<}5{vq^u}da`rA*lgCxIvWVv9j^6b4haMSv1sEM`i?fD z7%>h)XVP$2q!Xh>OcT-!n_=xrEs`d&^V=t_D+!T_ssu}8ePm(wdF~K=arra+322|*)kvZX@ z<7^rlhrq$*qqWvdu`#~4Fmp<#WmkL$&feO+P_f@$gk3qV1=)wWs3jGjE1KbA($o%l zC&*>tkZfa9F|SuY42v{jdd-Um=97V0l$V5w_kHa*KgqHY>%LO&UV2HPZ@a0k=v57S zb6~;Evl1hL-{J_Kp-wq$Mp6eW61~+$dMcWXfFIx$*yx`Jnkqfw++DHt-t<&q|Iz)(2|~kh4W;n1D##Odi4> zkUOH%#H>6!gJxXyUwX_!%IQI=9U8}$UY~y?=Ut1~`)sAYOP+|mj-fNI{S}PN88zp% zlpSb_zjzLVUBeHa-;Bc&@|T5GY1U9?G$EAMR zQ)Hpu;XIq<8nb75l!x%a96^lvw+IM5M$YYZk!7uzLMLNev=CuJ6SmI>JNn_h_M7IY zhUoi%F3SC>etN6XAh!I8KGf42dOo%-zw`98Ts)f65STXYC&j?iXlbAQ`1~MQr5Mk+ zE6=tM%>WkUCdD5C|!pfUdzBN)cS*^CNdL-_v%@r57Oj;jBRXk-r>(vODc79 z#c*-Z#14yWt_%OwmJtl@Ilfo(nn2SHzhDTeZfoC#`xQVVV|0$huy{eM^F0&9&Hj>P zBt>5i3SbQ2{Iatobt;3w_0ImVW>@*8TM#TG^VRJJ|57tllKxqk-j1SBOi#K4K`JH) zo&1qFDrdJ{JqV_0e7)aoH%75rS&Qa9Xm5UC&b_3|i%Vi;_EOl}`*O|$J~kWSE2#oX z9w#xvlMJfe(q=9do(XgB2vr1Ptcbhxh+Vpe&F0(`ENNRyIx|3iY4G6F`yY`Z=d(s|MXYz9uv(!Jh z>(m|hMYjh8oy@qoaY;_Vp?LpWLzJ9ImVmm2Tn)+gb70#|gr~@UNJXOA8*=maj|(3%xnnU7dINhW3`CUs9l;{+TXhvu87IE>nwT0p2f4-)9(zq8Mh zNdq=EjOO?1hG2Ph4zstENrg|qvDVQ0H8P}nog~vC_zE#85_=1ujzJO7Tu1dwNsPG# zGHK!jl#>N1xPTvS*OJ9Yp0hY{a40{Spl{Z(Piex8x?+Vucl%IcMFD73q?!)atAtPV z>bkV1lJWU~Ct)!h&GPhyR80-@Cv>m@AQN|V0iOnU1hlWCz!1{0D~Jh!p8<}KzWmsA zXI5YI*HT&Arbby?WH+XC-dg{$1*#y1kBJv(r!8qi#{#LlDj3d_5~@NXD3+Eac4@5l zE!Tk0^JZCa6{Ax+YQ3m@F+Y3}dJPXE0a@w?KZ0ng&FU2?N{=UUV$9x=$qjVam zmjxS@0s%4tYYB=TYbm_e31MvA>?#LTU~Zx+ok^l{Q(_6sDXQAO{*M3|3g-1i)!Z4# zD(!FhK|7bT(?*Xz@kM_R#4ZV5*q?|`k(#TR(w=ik7HioS?oU)`g)%~e9>NowE(|{T zpNKPoGN$-3k1xs?j0Jlp{ecZs0;)YZsy(rp!ysIr{QGx^S7DYC)c=AveQxWMgB#5) z*&s#KN4>EpCst;uvIaFC{IUfTF~?{BgqNIe>Km~syWmS4m4CI{3v+`P^;Uja%dc72 zM1%zkmf69No@ijSFvjl^1p5I?Od5V)|MGXQ_Lool-~aIc_i6v@AO7@dfA#-gKFwdy z9q=Ew$3DQxOp6!ZGqS4cXA*;8`E`i;@jO@iuB>1nWNeeFG+k?pu0xdN*h*m--nyp9 zlP{6w*~|pm?*$(^Xt?qsZ2HCWvaAhtB?fy|vef0(&{fa&;B`-d@7#kAu!WLP2#oD~3TL68eMg)Y9(R)^oYcgwD zwivzpl;F+iYtpfzee%da(qbmA$%}5$%V3EHZwSOtB#ML`J{AS%%kaXnBfe$FC{HjW zhphOLt7tNqa30p0e10L0&JVKYjG+$Yt0yH13f2wCv_Vj#QoCrN07)OEa;G5sYt=1e zQVV2^_bYPL*;#}q!Qt*?E*V!D5#>t(`qU7#f0;>w5(ExK$~)AnHkA4ZEbcOp$GHa$ z)YwSD*4oD$)E>e*oAFJKb;9));rXHPdE@%6^x}$rDor|s-Wo)TJoaJR#pqMe&_eh0 z+R8l>BvQO9iPRvQle^N}`_Y#`(w>C4FgG-BLJU+o`)T~qiy5lh_Z(53Zq*ZPm?;0n zczy3teaNzt5ZYk|s_DxX$zl4r`)~A7$=jgNQ_|2?x`_0_Y?9;=iO5^OK;J5kMzZhB z-<@{oBf?SYTdlPhJY}p=8NYh|o*1xeyQWrOmzf`bxr@{F$0J+TFl{IX)fWP~Yv$|} zBEL_CX4Kk-;9cpI{8vYmxYeS`N}{hTXv7f>imv>kd$}D_O@D8{B^BrTVY+Qkv^X&i zO2UbAlPSET9L#4gTDv*|Eb&^dhWP1Fw4mCj@5Vqt$6$OI{o$~mhXr@oQ;TplB3vv1 zDvAn0aiHGbNmIQgtXf_{qnNw1zI)0xaiZj3i=NO@_}z2@1yOpPfv4q@)wvMAAMTZf za&q%TXyBU)&tk}g^oqqosncvUt{83` z#W%d6(13g>zPMG|9%(z4lh{FcXzekG;JhVFPR!03E<7;xT>zr&fb|vegi#Q7xwg?0 zWrK#!W?{m$RwkB%=72MU2XMibldnxu0v?7 zG3NoRi1&!tnuUTOfuQu)<`H;U%`i5_U#2hzLHO&(rQj9+F>qIKR!|YE6jjRm;NAM8 z8ER_BfL|+OHYUnO>~$BPfn7+dJW2HnvJ?Ju8kC9-rVCz+izhTsL{3dOkUeuJgq$az z)V5)s%g9I1qx8hbr!OjBpWul>rFC zuJ4#;3ODrZA-?maVU_qPXt%9JELgYSnw9T5N9Iu40LI-MgXM%~URaXey&HRz!BmMJ z>v1r@g7XUG7Q&wUOdHS*FDXN?FyEn(DvG!Kn?j}C~?CAyRD9TF!-xl_>7yS>L5i+x0U#M@%ltuSf} z?Az?DeUG|EoOKl8eZVJE*1qtc>HdeJ{hIf2qTVy_Y`|w+p$i=f#_GFwM6??spw1N} zV-$Q4dd7*ANXhR+fg}(^tFgagzlt=0IB>=sW*t468C()rd|JY}Yd;9b)1`L?%bqCB zg=MCkb4hlP!f?)QuNSzJXo0vSNC5*qep&0!Wzkx=iIB-GmNS;<>(HK9;nTDaDVq4l z0`b^)F_Rh$!BwQRF|cVcBT4^%00030|E*8SuB%2AyzeP5N;dh3>>?J;)C|jOgTc%; z1~=@?XajEM!F)kJC7&7JmwT0~C{i{#QmIv?s#B*On~g6HvF9Qwrwmm*fNpH3#G(yJ z*-giUD3ASZwQx01*217|RCAC$E*r7yS#^6udY0}6&q(NGE(Lr}JUbctHhdUY7u=Z{Xlk8`-7tfy!wj_5sX_B>p0^~N84&I4_O>Uo3g-aBkoBi! zk+LoJU<1>>xvmvo?jX0hg~?JEglc;=SU3>-URVW&CIZ|a#i4>0Emw)FUW8Vd*bAw} z$v}zMvF?yg%p9|VeQ_e2IfQO7X8!FAz;6x~e(1ck=pD?@43?4+)&*+}(C6?H>^ z!n-q3>OQbV8{7t#cm=)rh-fmqYcLtllwWw9!=meYzmdM&1k~M=vxYwfjyf>HhL25U z+>|ik{KsS3Qi^V~-MDAWGc!fgydj2g?d1!Zms8m_4T1OT<68p z;L~!#1!f9$i5~={x*?WKb|jnQjclrfkd%4;ARC00;S&j`O&Kx+ zv7=Q>4@>aCWSHvam@^u=umm*cWZ4Oat~Baqng=?}ySu?|s;6i^#W8S>6(bOmk?No@ z`#5nMwy`ZxC%QUqKsdz_Mj3h?8z%Slkku;65iV{;s5iLwr`4co@nB$!Z5}>5Muo}7 z%wCWmEVi1}y+@9;5!a>A&Lu2lvu=^k2;smu3f&&VPA1Q)(z6IF%XKuP#cIkqQWQ4C zvQjq)>5r?}FDGG@GO52Nfls>yKlQd05Rm!Rv}{tR0VPaXn8l)*xN4 z208ny631Q9_w1A_sF|m`S%-MOdtSHZV+|sYD(1(T@RL;FF5QX3TMF_u&Ualw9W%|Q zAh!h4(hkm17HZzZ*DA~DtG$LC 100 - SymExpOut = SymExpIn; - else - SubExpArray(index,1) = SubExpOut; - SymExpIn = SymExpOut; - end -end \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Q_airdata.c b/EKF/matlab/scripts/Inertial Nav EKF/Q_airdata.c deleted file mode 100644 index 690324a82d..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/Q_airdata.c +++ /dev/null @@ -1,89 +0,0 @@ -float t3 = ve-vwe; -float t4 = q0*q0; -float t5 = q1*q1; -float t6 = q2*q2; -float t7 = q3*q3; -float t8 = vd-vwd; -float t9 = q0*q1*2.0f; -float t10 = q2*q3*2.0f; -float t11 = vn-vwn; -float t13 = q0*q2*2.0f; -float t14 = q1*q3*2.0f; -float t18 = t4-t5-t6+t7; -float t19 = t8*t18; -float t20 = t9-t10; -float t21 = t3*t20; -float t22 = t13+t14; -float t23 = t11*t22; -float t2 = t19-t21+t23; -float t15 = q0*q3*2.0f; -float t16 = q1*q2*2.0f; -float t24 = t9+t10; -float t25 = t4-t5+t6-t7; -float t26 = t3*t25; -float t27 = t8*t24; -float t28 = t15-t16; -float t29 = t11*t28; -float t12 = t26+t27-t29; -float t30 = t13-t14; -float t31 = t4+t5-t6-t7; -float t32 = t11*t31; -float t33 = t8*t30; -float t34 = t15+t16; -float t35 = t3*t34; -float t17 = t32-t33+t35; -float t44 = t2*t18*2.0f; -float t45 = t12*t24*2.0f; -float t46 = t17*t30*2.0f; -float t36 = t44+t45-t46; -float t37 = t2*t2; -float t38 = t12*t12; -float t39 = t17*t17; -float t40 = t37+t38+t39; -float t41 = 1.0f/t40; -float t48 = t12*t25*2.0f; -float t49 = t2*t20*2.0f; -float t50 = t17*t34*2.0f; -float t42 = t48-t49+t50; -float t52 = t17*t31*2.0f; -float t53 = t2*t22*2.0f; -float t54 = t12*t28*2.0f; -float t43 = t52+t53-t54; -float t47 = t36*t36; -float t51 = t42*t42; -float t55 = t43*t43; -float t57 = 1.0f/(t17*t17); -float t58 = 1.0f/t17; -float t63 = t18*t58; -float t64 = t2*t30*t57; -float t56 = t63+t64; -float t66 = t22*t58; -float t67 = t2*t31*t57; -float t59 = t66-t67; -float t60 = t37*t57; -float t61 = t60+1.0f; -float t62 = 1.0f/(t61*t61); -float t65 = t56*t56; -float t68 = t59*t59; -float t70 = t20*t58; -float t71 = t2*t34*t57; -float t69 = t70+t71; -float t72 = t69*t69; -float t78 = t25*t58; -float t79 = t12*t34*t57; -float t73 = t78-t79; -float t81 = t28*t58; -float t82 = t12*t31*t57; -float t74 = t81+t82; -float t75 = t38*t57; -float t76 = t75+1.0f; -float t77 = 1.0f/(t76*t76); -float t80 = t73*t73; -float t83 = t74*t74; -float t85 = t24*t58; -float t86 = t12*t30*t57; -float t84 = t85+t86; -float t87 = t84*t84; -float tas_var = t41*t47*vd_var*0.25f+t41*t51*ve_var*0.25f+t41*t55*vn_var*0.25f+t41*t47*vwd_var*0.25f+t41*t51*vwe_var*0.25f+t41*t55*vwn_var*0.25f; -float aoa_var = t62*t65*vd_var+t62*t72*ve_var+t62*t68*vn_var+t62*t65*vwd_var+t62*t72*vwe_var+t62*t68*vwn_var; -float aos_var = t77*t87*vd_var+t77*t80*ve_var+t77*t83*vn_var+t77*t87*vwd_var+t77*t80*vwe_var+t77*t83*vwn_var; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Quat2Tbn.m b/EKF/matlab/scripts/Inertial Nav EKF/Quat2Tbn.m deleted file mode 100644 index 663d19cde8..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/Quat2Tbn.m +++ /dev/null @@ -1,14 +0,0 @@ -function Tbn = Quat2Tbn(quat) - -% Convert from quaternions defining the flight vehicles rotation to -% the direction cosine matrix defining the rotation from body to navigation -% coordinates - -q0 = quat(1); -q1 = quat(2); -q2 = quat(3); -q3 = quat(4); - -Tbn = [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]; \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/QuatDivide.m b/EKF/matlab/scripts/Inertial Nav EKF/QuatDivide.m deleted file mode 100644 index 20e789dcfb..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/QuatDivide.m +++ /dev/null @@ -1,16 +0,0 @@ -function q_out = QuatDivide(qin1,qin2) - -q0 = qin1(1); -q1 = qin1(2); -q2 = qin1(3); -q3 = qin1(4); - -r0 = qin2(1); -r1 = qin2(2); -r2 = qin2(3); -r3 = qin2(4); - -q_out(1,1) = (qin2(1)*qin1(1) + qin2(2)*qin1(2) + qin2(3)*qin1(3) + qin2(4)*qin1(4)); -q_out(2,1) = (r0*q1 - r1*q0 - r2*q3 + r3*q2); -q_out(3,1) = (r0*q2 + r1*q3 - r2*q0 - r3*q1); -q_out(4,1) = (r0*q3 - r1*q2 + r2*q1 - r3*q0); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m b/EKF/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m deleted file mode 100644 index c603ae8817..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m +++ /dev/null @@ -1,62 +0,0 @@ -%% calculate the rotation vector variances from an equivalent quaternion -% inputs are the quaternion orientation and the 4x4 covariance matrix for the quaternions -% output is a vector of variances for the rotation vector that is equivalent to the quaternion -clear all; -reset(symengine); -syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED - -% define quaternion rotation -quat = [q0;q1;q2;q3]; - -% convert to a rotation vector -delta = 2*acos(q0); -rotVec = (delta/sin(delta/2))*[q1;q2;q3]; - -% calculate transfer matrix from quaternion to rotation vector -G = jacobian(rotVec, quat); - -% define a symbolic covariance matrix using strings to represent -% '_l_' to represent '( ' -% '_c_' to represent , -% '_r_' to represent ')' -% these can be substituted later to create executable code -for rowIndex = 1:4 - for colIndex = 1:4 - eval(['syms P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1), '_r_ real']); - eval(['quatCovMat(',num2str(rowIndex),',',num2str(colIndex), ') = P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1),'_r_;']); - end -end - -% rotate the covariance from quaternion to rotation vector -rotCovMat = G*quatCovMat*transpose(G); - -% take the variances -rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)]; - -% convert to c-code -ccode(rotVarVec,'file','rotVarVec.c'); - -%% calculate the quaternion variances from an equivalent rotation vector - -% define a rotation vector -syms rotX rotY rotZ real; -rotVec = [rotX;rotY;rotZ]; - -% convert to a quaternion -vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2); -quat = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)]; - -% calculate transfer matrix from rotation vector to quaternion -G = jacobian(quat, rotVec); - -% define the rotation vector variances -syms rotVarX rotVarY rotVarZ real; - -% define the rotation vector covariance matrix -rotCovMat = diag([rotVarX;rotVarY;rotVarZ]); - -% rotate the covariance matrix into quaternion coordinates -quatCovMat = G*rotCovMat*transpose(G); - -% convert to c-code -ccode(quatCovMat,'file','quatCovMat.c'); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/QuatMult.m b/EKF/matlab/scripts/Inertial Nav EKF/QuatMult.m deleted file mode 100644 index 357c545d22..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/QuatMult.m +++ /dev/null @@ -1,5 +0,0 @@ -function quatOut = QuatMult(quatA,quatB) -% Calculate the following quaternion product quatA * quatB using the -% standard identity - -quatOut = [quatA(1)*quatB(1)-quatA(2:4)'*quatB(2:4); quatA(1)*quatB(2:4) + quatB(1)*quatA(2:4) + cross(quatA(2:4),quatB(2:4))]; \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/QuatToEul.m b/EKF/matlab/scripts/Inertial Nav EKF/QuatToEul.m deleted file mode 100644 index 09b1505b6f..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/QuatToEul.m +++ /dev/null @@ -1,9 +0,0 @@ -% Convert from a quaternion to a 321 Euler rotation sequence in radians - -function Euler = QuatToEul(quat) - -Euler = zeros(3,1); - -Euler(1) = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)), quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4)); -Euler(2) = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3))); -Euler(3) = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)), quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4)); \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/RotToQuat.m b/EKF/matlab/scripts/Inertial Nav EKF/RotToQuat.m deleted file mode 100644 index 3c9777acd5..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/RotToQuat.m +++ /dev/null @@ -1,10 +0,0 @@ -% convert froma rotation vector in radians to a quaternion -function quaternion = RotToQuat(rotVec) - -vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2); - -if vecLength < 1e-6 - quaternion = [1;0;0;0]; -else - quaternion = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)]; -end \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m b/EKF/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m deleted file mode 100644 index b86f59eb04..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m +++ /dev/null @@ -1,666 +0,0 @@ -function SaveScriptCode(nStates) -%% Load Data -fileName = strcat('SymbolicOutput',int2str(nStates),'.mat'); -load(fileName); - -%% Open output file -fileName = strcat('SymbolicOutput',int2str(nStates),'.txt'); -fid = fopen(fileName,'wt'); - -%% Write equation for state transition matrix -if exist('SF','var') - - fprintf(fid,'SF = zeros(%d,1);\n',numel(SF)); - for rowIndex = 1:numel(SF) - string = char(SF(rowIndex,1)); - fprintf(fid,'SF(%d) = %s;\n',rowIndex,string); - end - - % fprintf(fid,'\n'); - % fprintf(fid,'F = zeros(%d,%d);\n',nStates,nStates); - % for rowIndex = 1:nStates - % for colIndex = 1:nStates - % string = char(F(rowIndex,colIndex)); - % % don't write out a zero-assignment - % if ~strcmpi(string,'0') - % fprintf(fid,'F(%d,%d) = %s;\n',rowIndex,colIndex,string); - % end - % end - % end - % fprintf(fid,'\n'); - -end -%% Write equations for control influence (disturbance) matrix -if exist('SG','var') - - fprintf(fid,'\n'); - fprintf(fid,'SG = zeros(%d,1);\n',numel(SG)); - for rowIndex = 1:numel(SG) - string = char(SG(rowIndex,1)); - fprintf(fid,'SG(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - % fprintf(fid,'\n'); - % fprintf(fid,'G = zeros(%d,%d);\n',nStates,numel([da;dv])); - % for rowIndex = 1:nStates - % for colIndex = 1:numel([da;dv]) - % string = char(G(rowIndex,colIndex)); - % % don't write out a zero-assignment - % if ~strcmpi(string,'0') - % fprintf(fid,'G(%d,%d) = %s;\n',rowIndex,colIndex,string); - % end - % end - % end - % fprintf(fid,'\n'); - -end -%% Write equations for state error matrix -if exist('SQ','var') - - fprintf(fid,'\n'); - fprintf(fid,'SQ = zeros(%d,1);\n',numel(SQ)); - for rowIndex = 1:numel(SQ) - string = char(SQ(rowIndex,1)); - fprintf(fid,'SQ(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - % fprintf(fid,'\n'); - % fprintf(fid,'Q = zeros(%d,%d);\n',nStates,nStates); - % for rowIndex = 1:nStates - % for colIndex = 1:nStates - % string = char(Q(rowIndex,colIndex)); - % % don't write out a zero-assignment - % if ~strcmpi(string,'0') - % fprintf(fid,'Q(%d,%d) = %s;\n',rowIndex,colIndex,string); - % end - % end - % end - % fprintf(fid,'\n'); - -end -%% Write equations for covariance prediction -% Only write out upper diagonal (matrix is symmetric) -if exist('SPP','var') - - fprintf(fid,'\n'); - fprintf(fid,'SPP = zeros(%d,1);\n',numel(SPP)); - for rowIndex = 1:numel(SPP) - string = char(SPP(rowIndex,1)); - fprintf(fid,'SPP(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - -end - -if exist('PP','var') - - fprintf(fid,'\n'); - fprintf(fid,'nextP = zeros(%d,%d);\n',nStates,nStates); - for colIndex = 1:nStates - for rowIndex = 1:colIndex - string = char(PP(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'nextP(%d,%d) = %s;\n',rowIndex,colIndex,string); - end - end - end - fprintf(fid,'\n'); - -end - -%% Write equations for velocity and position data fusion -if exist('H_VP','var') - - [nRow,nCol] = size(H_VP); - fprintf(fid,'\n'); - fprintf(fid,'H_VP = zeros(%d,%d);\n',nRow,nCol); - for rowIndex = 1:nRow - for colIndex = 1:nCol - string = char(H_VP(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_VP(%d,%d) = %s;\n',rowIndex,colIndex,string); - end - end - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(SK_VP); - fprintf(fid,'\n'); - fprintf(fid,'SK_VP = zeros(%d,%d);\n',nRow,nCol); - for rowIndex = 1:nRow - for colIndex = 1:nCol - string = char(SK_VP(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'SK_VP(%d,%d) = %s;\n',rowIndex,colIndex,string); - end - end - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_VP); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,%d);\n',nRow,nCol); - for rowIndex = 1:nRow - for colIndex = 1:nCol - string = char(K_VP(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d,%d) = %s;\n',rowIndex,colIndex,string); - end - end - end - fprintf(fid,'\n'); - -end -%% Write equations for true airspeed data fusion -if exist('SH_TAS','var') - - fprintf(fid,'\n'); - fprintf(fid,'SH_TAS = zeros(%d,1);\n',numel(SH_TAS)); - for rowIndex = 1:numel(SH_TAS) - string = char(SH_TAS(rowIndex,1)); - fprintf(fid,'SH_TAS(%d) = %s;\n',rowIndex,string); - end - - [nRow,nCol] = size(H_TAS); - fprintf(fid,'\n'); - fprintf(fid,'H_TAS = zeros(1,%d);\n',nCol); - for rowIndex = 1:nRow - for colIndex = 1:nCol - string = char(H_TAS(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_TAS(1,%d) = %s;\n',colIndex,string); - end - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_TAS = zeros(%d,1);\n',numel(SK_TAS)); - for rowIndex = 1:numel(SK_TAS) - string = char(SK_TAS(rowIndex,1)); - fprintf(fid,'SK_TAS(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_TAS); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_TAS(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - -end -%% Write equations for sideslip data fusion -if exist('SH_BETA','var') - - fprintf(fid,'\n'); - fprintf(fid,'SH_BETA = zeros(%d,1);\n',numel(SH_BETA)); - for rowIndex = 1:numel(SH_BETA) - string = char(SH_BETA(rowIndex,1)); - fprintf(fid,'SH_BETA(%d) = %s;\n',rowIndex,string); - end - - [nRow,nCol] = size(H_BETA); - fprintf(fid,'\n'); - fprintf(fid,'H_BETA = zeros(1,%d);\n',nCol); - for rowIndex = 1:nRow - for colIndex = 1:nCol - string = char(H_BETA(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_BETA(1,%d) = %s;\n',colIndex,string); - end - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_BETA = zeros(%d,1);\n',numel(SK_BETA)); - for rowIndex = 1:numel(SK_BETA) - string = char(SK_BETA(rowIndex,1)); - fprintf(fid,'SK_BETA(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_BETA); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_BETA(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - -end -%% Write equations for magnetometer data fusion -if exist('SH_MAG','var') - - fprintf(fid,'\n'); - fprintf(fid,'SH_MAG = zeros(%d,1);\n',numel(SH_MAG)); - for rowIndex = 1:numel(SH_MAG) - string = char(SH_MAG(rowIndex,1)); - fprintf(fid,'SH_MAG(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(H_MAG); - fprintf(fid,'\n'); - fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol); - for colIndex = 1:nCol - string = char(H_MAG(1,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string); - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_MX = zeros(%d,1);\n',numel(SK_MX)); - for rowIndex = 1:numel(SK_MX) - string = char(SK_MX(rowIndex,1)); - fprintf(fid,'SK_MX(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_MX); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_MX(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(H_MAG); - fprintf(fid,'\n'); - fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol); - for colIndex = 1:nCol - string = char(H_MAG(2,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string); - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_MY = zeros(%d,1);\n',numel(SK_MY)); - for rowIndex = 1:numel(SK_MY) - string = char(SK_MY(rowIndex,1)); - fprintf(fid,'SK_MY(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_MY); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_MY(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(H_MAG); - fprintf(fid,'\n'); - fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol); - for colIndex = 1:nCol - string = char(H_MAG(3,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string); - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_MZ = zeros(%d,1);\n',numel(SK_MZ)); - for rowIndex = 1:numel(SK_MZ) - string = char(SK_MZ(rowIndex,1)); - fprintf(fid,'SK_MZ(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_MZ); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_MZ(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - -end -%% Write equations for optical flow sensor angular LOS data fusion -if exist('SH_LOS','var') - - fprintf(fid,'\n'); - fprintf(fid,'SH_LOS = zeros(%d,1);\n',numel(SH_LOS)); - for rowIndex = 1:numel(SH_LOS) - string = char(SH_LOS(rowIndex,1)); - fprintf(fid,'SH_LOS(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - [nRow,nCol] = size(H_LOS); - fprintf(fid,'\n'); - fprintf(fid,'H_LOS = zeros(1,%d);\n',nCol); - for colIndex = 1:nCol - string = char(H_LOS(1,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_LOS(%d) = %s;\n',colIndex,string); - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - [nRow,nCol] = size(H_LOS); - fprintf(fid,'\n'); - fprintf(fid,'H_LOS = zeros(1,%d);\n',nCol); - for colIndex = 1:nCol - string = char(H_LOS(2,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_LOS(%d) = %s;\n',colIndex,string); - end - end - -% fprintf(fid,'\n'); -% fprintf(fid,'SKK_LOS = zeros(%d,1);\n',numel(SKK_LOS)); -% for rowIndex = 1:numel(SKK_LOS) -% string = char(SKK_LOS(rowIndex,1)); -% fprintf(fid,'SKK_LOS(%d) = %s;\n',rowIndex,string); -% end - - fprintf(fid,'\n'); - fprintf(fid,'SK_LOS = zeros(%d,1);\n',numel(SK_LOS)); - for rowIndex = 1:numel(SK_LOS) - string = char(SK_LOS(rowIndex,1)); - fprintf(fid,'SK_LOS(%d) = %s;\n',rowIndex,string); - end - - [nRow,nCol] = size(K_LOSX); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_LOSX(rowIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_LOSY); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_LOSY(rowIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - -end -%% Write observation fusion equations for optical flow sensor scale factor error estimation -if exist('SH_OPT','var') - - fprintf(fid,'\n'); - for rowIndex = 1:numel(SH_OPT) - string = char(SH_OPT(rowIndex,1)); - fprintf(fid,'SH_OPT(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - string = char(H_OPT(1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_OPT(1) = %s;\n',1,string); - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - string = char(H_OPT(2)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_OPT(2) = %s;\n',1,string); - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - for rowIndex = 1:numel(SK_OPT) - string = char(SK_OPT(rowIndex,1)); - fprintf(fid,'SK_OPT(%d) = %s;\n',rowIndex,string); - end - - fprintf(fid,'\n'); - string = char(K_OPT(1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'K_OPT(1) = %s;\n',1,string); - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - string = char(K_OPT(2)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'K_OPT(2) = %s;\n',1,string); - end - fprintf(fid,'\n'); - -end -%% Write equations for laser range finder data fusion -if exist('SH_RNG','var') - - fprintf(fid,'\n'); - fprintf(fid,'SH_RNG = zeros(%d,1);\n',numel(SH_RNG)); - for rowIndex = 1:numel(SH_RNG) - string = char(SH_RNG(rowIndex,1)); - fprintf(fid,'SH_RNG(%d) = %s;\n',rowIndex,string); - end - - [nRow,nCol] = size(H_RNG); - fprintf(fid,'\n'); - fprintf(fid,'H_RNG = zeros(1,%d);\n',nCol); - for rowIndex = 1:nRow - for colIndex = 1:nCol - string = char(H_RNG(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_RNG(1,%d) = %s;\n',colIndex,string); - end - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_RNG = zeros(%d,1);\n',numel(SK_RNG)); - for rowIndex = 1:numel(SK_RNG) - string = char(SK_RNG(rowIndex,1)); - fprintf(fid,'SK_RNG(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_RNG); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_RNG(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - -end - -%% Write equations for simple magnetomter data fusion -if exist('SH_MAGS','var') - - fprintf(fid,'\n'); - fprintf(fid,'SH_MAGS = zeros(%d,1);\n',numel(SH_MAGS)); - for rowIndex = 1:numel(SH_MAGS) - string = char(SH_MAGS(rowIndex,1)); - fprintf(fid,'SH_MAGS(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(H_MAGS); - fprintf(fid,'\n'); - fprintf(fid,'H_MAGS = zeros(1,%d);\n',nCol); - for colIndex = 1:nCol - string = char(H_MAGS(1,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_MAGS(%d) = %s;\n',colIndex,string); - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_MAGS = zeros(%d,1);\n',numel(SK_MAGS)); - for rowIndex = 1:numel(SK_MAGS) - string = char(SK_MAGS(rowIndex,1)); - fprintf(fid,'SK_MAGS(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_MAGS); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_MAGS(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); -end - -%% Write equations for X accel fusion -if exist('SH_ACCX','var') - - fprintf(fid,'\n'); - fprintf(fid,'SH_ACCX = zeros(%d,1);\n',numel(SH_ACCX)); - for rowIndex = 1:numel(SH_ACCX) - string = char(SH_ACCX(rowIndex,1)); - fprintf(fid,'SH_ACCX(%d) = %s;\n',rowIndex,string); - end - - [nRow,nCol] = size(H_ACCX); - fprintf(fid,'\n'); - fprintf(fid,'H_ACCX = zeros(1,%d);\n',nCol); - for rowIndex = 1:nRow - for colIndex = 1:nCol - string = char(H_ACCX(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_ACCX(1,%d) = %s;\n',colIndex,string); - end - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_ACCX = zeros(%d,1);\n',numel(SK_ACCX)); - for rowIndex = 1:numel(SK_ACCX) - string = char(SK_ACCX(rowIndex,1)); - fprintf(fid,'SK_ACCX(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_ACCX); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_ACCX(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - -end - -%% Write equations for Y accel fusion -if exist('SH_ACCY','var') - - fprintf(fid,'\n'); - fprintf(fid,'SH_ACCY = zeros(%d,1);\n',numel(SH_ACCY)); - for rowIndex = 1:numel(SH_ACCY) - string = char(SH_ACCY(rowIndex,1)); - fprintf(fid,'SH_ACCY(%d) = %s;\n',rowIndex,string); - end - - [nRow,nCol] = size(H_ACCY); - fprintf(fid,'\n'); - fprintf(fid,'H_ACCY = zeros(1,%d);\n',nCol); - for rowIndex = 1:nRow - for colIndex = 1:nCol - string = char(H_ACCY(rowIndex,colIndex)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'H_ACCY(1,%d) = %s;\n',colIndex,string); - end - end - end - fprintf(fid,'\n'); - - fprintf(fid,'\n'); - fprintf(fid,'SK_ACCY = zeros(%d,1);\n',numel(SK_ACCY)); - for rowIndex = 1:numel(SK_ACCY) - string = char(SK_ACCY(rowIndex,1)); - fprintf(fid,'SK_ACCY(%d) = %s;\n',rowIndex,string); - end - fprintf(fid,'\n'); - - [nRow,nCol] = size(K_ACCY); - fprintf(fid,'\n'); - fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); - for rowIndex = 1:nRow - string = char(K_ACCY(rowIndex,1)); - % don't write out a zero-assignment - if ~strcmpi(string,'0') - fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); - end - end - fprintf(fid,'\n'); - -end - -%% Close output file -fclose(fid); - -end diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Sideslip.mat b/EKF/matlab/scripts/Inertial Nav EKF/Sideslip.mat deleted file mode 100644 index f6d8de8d86e8169e8e561aa1aa6b91afeb9d7bbe..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 9809 zcmb8PV{9dW7Oq`mrnZe~yKCFFZQFKtZQFLIwrx&rciQf{Q}>>e@9#;zd*5UwEANjd zPu7nmFRUgfEJDK0z(gW1tVVBPZD$T3QL;03vv72==lM@3D$T(LAQ5#jH+C~OBXM-# zAyIXAAdxrrCLv)aVP@lD<=|mqCt+b?<{ZVH|0fa3#DfIsInvVw(HZJq8^{4uVhAzRGaX6+YYGV_EdX==U##5!#QsNg{C|oHaU=h4Q`7JzFj_*G z%>O^t`2RQ6afmW-86eE{f3fZq|GjB!<_6N0l!nzj4W3n^)gUoRe#brVtcsFo>B3u~ zV%g&j+B^D@Z{E;JFz)P~QqvDyK)<5$uZE_ml4fWs{!2r~*YEX&HOK3So41UH=sWrE9ZHVphY#YfnU8|w@vps)gWa_o)7b8( zj|=Yp&r#Q}i}#m?Z~xC(;<13`?*p{PO4h*GHv#%(<~;l;Q4`L#teRSifyP(45q+adKU<8B&X`-a zo$XoW5kMgB2~C(8hFfY2Y9c( zhIT7w(q&Qy{bF*53#gG?e<#~LS4=-J=67I6{W(zC)tYPy+V02QhaXFYpB6%k1U}}b zd^r=%GF$Kq*OKY6S}mC3z_k*pkq}wDq>VEsD`^p&n5^&bBR9bWnk9m6&8nj@yNy}@ zOpJAt5)wQ^xHifZg(tc5W=f4BMcw1c9uROvk%q>i=*~N;ifN7Y`hCkEF~c^_y-d4l z>yL>T;ox#KKvHY^&kY0$iyy?EX}KiKmAJs$v6;yO64xHLAZtn`o$VA9T0bxBC{>lx z-eyz`17px|!h}54tlFuMzHAvjIlBc3rGbd-Lh~4O74iW!c84b$Pt}XEXmGP-J9s+v_dCx*A?>!Ru}wEPh>=~2ACRzv za76=cm2#B+!P15rDlGu@x_kQa+c6M}nl2~=hU|3`TrnrvGHS~;W;~?n!W_~yQz>1e z-{|DPYDNf_6gVjr*E_zvg_3)O!=r)DM_Z~T7rznsgS)lwVhrq2i{C0#-q((6C%DK# zBDNJ@G<3uS6#1P*&@x6G+4NSvrkf4(poEjy+8Gzm$G2EkmjyYzGgMM(AH)J-+`Hxt zBtj3=a8Fdi@QPK5L~-Z9t47Tk>t%Ad(8C64?t5Lc;aN z+6R@S03<4^(3cMh+AeS9R4b1xF3pu8OBKpFe^_60X21><%kMRHV;YM``M;suRsi>x ze=70jN&33%)(-W>urezw?UNOE6d5E#N#Cp*@;o4S`0^+Gq; zH|ECxhva(C4a%oujf;(@&s7aT zqs?qgGk}CSxJ@9;xY4GFhuj`}4UHoucn2ehg=l~jJaD7TPXCK(aWLOCi(tV-yS9X( z@`2*YJXXl!kmr+s4kbJj=bugqnz2aCZ|Mi?N$=^I#pcR3Pt8Pk9 zzlueEa{!mm_*GuOI z##rb6l4xNURoL{~W$M{Tw4+5w2kD&ah}A;C4aoY-b?UPgm^!lB{tf!X(5uObcU z77r$E0s0mIP%=blni8x3kc}S8+n^BsUPO7a92WXHl`bJ&?&Z%)fe8HCMlei@V$4sW zm$;qF{M_u6U-9uc4X%sr6g9gWKhz^2;{*ibIot#5=9{Xp`?>8`#~dRtZ=I2RKLQB1 zT}sa75f|sa280lw)1>sa`f+a&H|+9B!FqR+6>Y(fII13Ca#%vQHGs&_o2CY=JYX-* zF7&y(d|a&t%mep1=j2gHYDMMOPi>KSqhxtmzsS4~8Dpf|&|LDtzpIoR-1o`F(ebE> z$}u_!6ujHek;Q2Blj!ZSxZ}?c!ePS7m^~xUf-nmQ99J_yaTb-r(lLz5}zq z=RQ?c3pxbLTF?x1m*9hr%%<8@dMCj%9F)X~;SjEr51Ai1dMX8J;$|L9TD-?Wr5X{ zm>IhYrjM4USC%s!oV3qMRhu_6Foz7Q?Pszp$AiuiDeCTaK!$lQ3*)nBmhLg7G2|72 zGg39@E^>U!-#xrQa**cgxdFAleUBTH(^y=S*%`rr?MXfea+C`QJLNU^$tMCQ?5my&n1BrbKho(}dkCB$KT> z&ho#<@=(HL&74s(P>Xo5VRiqzXH&jF-@l5H8< z3aKP6{G9%N&BFe^`5uUw+xuo_3|I~f!{)CN(bRK7am{_VaGTkB#W7M`bWJD~Pz=jt zSMN?9y3(7oa=~VdhU@&D+WGo`_rBoj_W^D2;TC=GVZh%|&~n_u<%x9bFx!lBlWe}PCYg$H5H4;a%Bp1OTK_P!d&(?=?DIUEv4h$QnGzqBmH z{gJ{$0BI3K18K!1Y@`KnJmM~?X8tFP&_Zb5+FLt?oz{s`OsL>5o;CEfZ3`6uD9@ze z8l`RmXwwaQyhiqP`*vi7)?^i#Qr|YuwDL;IoW?Ev(n@1^p^Wk{oL5GJy&(9|bqS;5 z+0HMlIk+bXGGd2L;|urPbu_gG{Gv;XKx$E~Qa^X<#!=iCQKny&7<247hX;2QCS%C= z^wCwHb~5fJpHO%N9q_~u!A&YwEZ&LH|LWT1)8V^Z<4T)D&5==r{zq5uM#;^eH5$9_ zR)5tkBxFgJx){%T-Z$nxDoNysK z6|Qs4qv@pZoXEGhG9u85Ag0Xf0W)d$hqPy6sozZu&y#3r^V0~%4#a(<>k1@EJqVEJBY#n`Z6zR1d4l^d+8Kv;>uX4wPLj3wJ6dNe^$ylMi<$>YX#!JWXm` zDpN6|H}qB$A>@GbBZqe=mmDETJCJcL(}dB8HMB~jHFF~s{&cvUcdbVeC1Q?th4vEc z@Fzbv26m`(2xk`?cs4h}VHr2bZM3Ns9HP!c<}9=*hC(*4A~v5R{79E?N3{cF^OCxf zL?3g`6Z1sD-sY(H%6U<8Qr_N>oPCNXc;>h>Z+P9jWi@hFr;aWu;w03VVAYjC=+1S6 zr3V(Ho+aZOpeTfDCuJH$#h}an=&AI8IG&(Tv!wqO4D($n9Dva@X{I|_mQmJsRqStu`6MRECopw5u(xb??SHr%172^^ zX^iVwqdkbcdIx>eF(MpM5Q>7{>>{vYC%(jD)&_uM{#^^4o#-ng10U!B`0Ozuhknfl%Y?lj~J3w1Hb zm}#=YD<6z|3*`M7KQPw+NmUqX1>IA<|3SJjjHjc{#Q!H z7dm4THe=hbFj`G#ZU8i}*2C#god6r*jcg|$00^if^hM&>2QtMz}LmvrQCzWJfKAnM$Cv42U_MT`DH zw7&Q8vKE82*?UcRJ7ynT+;2%cldjkM)sni!&ykKC{F@)uY3v3*kZZ&3ShIH^^228y zcEa$bR+EPGJ*WICy4BRhAm8@unY_?1G&d(f&H@Vj+;ipg5fB|K9SnrIx1E19vPMM57@dhqrqbb9`R;HxZc@tQM3PW z!DM2m7q%7Th>1*g&sejz&+qOqxljrd9Jq@zuEhg(9^m$X$OD6g4Fvp#&VYw}ypmK$ zbGMsAX6Rq9zXelNG3I3nS#u@3gjTrVq+b*p|K!us39=V5d^zPYWf!;!^0Bo7&&0iC z+;n$a}>;b~_%2%e>9>M8bBQ-gYJGjm}a9j)Ej9SSy9xKo!okfuF1 z5<$rMt)z8E5Cog;%Y~f$Aqqnf#7AL*3p*02kBKfsnr*9~O*7@-6NVw1p$2XEaTjgD zH)gnXoY6@zG+SbpCX)MkN zwUCfsVS^p5A0e|)kd7X&AR%^takS!B$v_$97Cj9MAHl!&l@5eqhIv?VUg;pLKICBt zkI+N{BK%maU@m6N+TcknhGFe_x1e%M?ZZt{Ef)kx;#SQ&-Rs*Qlx}exY32DEX zK8?=Kh7KE6v=KJBtbaSfTTL_tZ&A_vCw@*cH2Kggb^rF8+^DU8v>#_?1r+Mb)+(sd zEyRSK#q0c0kHP~YlEDC$gE)J#ZTzvuSWNBUbz!$;PK(A4U+R+_8Dk>14pTjfp733^ z!TUvmLlVuqn_b2A#e?|+UsCce;YP8#=Imx-4Y0+3@*25D9{YHtZ31D2LJrv(qo(m= z@r(+RHyys8Wr;qoZ@yR(zWM`SOo=|rzwr{?hbxEIn&+>*4G%>Ec0>5|C}9yd5K46Exb~CP$xq} z@GB<%#=V@Qx`#fBAU^e#a`9%P)u})eHWOVVI?4EC;IbKOy&oJlZ@`X+#O6a92e0RE zCnFIxqjP2&m2vy@q^+kE<_FtonHLprn4qT+{xwI-rSmMO2cPB*_yd?AI}7*W0La_X z<(~)(ngh&ufAkMoma|v|D%zSrqutA_r6W)_Wr=_`9Bfav1n7G=25Hwbjh|ts+iVLq zN7a8uOGMT}U6rP*D(ZLVz$t~=?Mjz8j#wNpg85zbN*Z5GEVL#S?+@jmWv?Y7CNXnE z?^xj(dB7JuyMY;~6XhesE$OHiE9y!bUuLeO!%SqVNb$?|F(f<~$THtp5Vdq^ZhBf) z5`j~*mx&pgzq$0bcX!9m`^rX_jcIkQ!j_d_U!}(njM=a)wtWd{O=MBtP90;Vieltc zKq-s4*xPA_ (B?-UeIbe<^D=M=nKrMXY%WchxGUhHI>p zn$%q?Ke9L5;2tbi_1hi1)Yadj_!uMa3A=4Id%|@hFsh_@hCiI?U?dH*n)YS@!YFW> z%p#pml9(ofqj~14(KY7=kt?d4ET^Aw2#G~?h{^AdMJ?Apl%Y&iA?5e{!(C#`s1^CO z+OuX*GU2&t6J>FDLUb5w%D7C6O)+!bR+Jm4OepBkA!q9i>Xk5yW`aWfjmHrEb5;jp z3!M*Len8F`iR6~%cQuxxy)W>J?+=3CP~hSNF89?9@gv&=19Q!hu(HJ`{#AOD@^ zD(R<-cn9S@s|gdw=wTrO>b@^<2+n%DgcsRJ!MF`!N|bg-wfbYSq%PS#KyextG-^_$ zceC}Kudci5!(_|jU>XKkbY2;ObT`-tN6Z$W2#yNDdp;Oc=y0cK{xEg8P69Da9C1zo zMiE7%XW+W4`%pBmkgV9Hr~KmNsiI~!l6{g2Ze=+)?pxf+37Y((p=^>vHg!8RC_i&zlZ+XbE-x{2nBrdS?ptN|OPlwB5g49ddV zBhV7mSU2@~woj;ckoSZ!YBD<>nzN4&wEf^oETAGuH(FHZ*8o!XUW2cDHj`7__Zs%P z1&51=p=|F43@+vym}ws2_In3AfU!a2n2p0^RzMT*hS-P}chvAupw?ujgyTVsU6qI= zkMu44&Y7|N?v!H{Z&7$A#0}jn{Hjl=jRqH`F2R7r{P0hLI~<9X>tei)3{|*bI%LUH zO6~3rAEz)A;aC1P>9>BkQ5gGu)@3~`40gOOWx|7;)>YLOk568KZt}t)7fw#P#0C;#ifRTy@j~(YmEUFn3)|80ZDU8ebwW--?NK3JrwU5MX&fS7 zgyP=MVjLwwn0g0m>X5qT@NUydCgpVnfe=PNalBboMcG8`QJ&vh zCKsEy_!B+BR}Ou&fkpQiskyeJfDN+z!vRaRxK^=KcuPhK$da{M#`W>1D@;^wLu_8! zMX;wioZyEt7`wy4y1EIcoSi@S-fcl?maU7+N@6&f&Hhg;C&qkks6zaiq%R^?2dpJo z!K0EEaV|%J1`KmkV{~^47=cFS$0kw>Thz=KzZ3?0Llf-G7u~!mu6V3$MJ1r&VPo*2 zeO-X$uZ`2kwDMAof0_)W7LlVTw~!g1a6lxuu&YV_F(i6>H z2kc80NLE^yubWA&O^Y7Ld|p?tJv7 zx+Ek9aK}IS?5)nyN0lH6={HM%qw(C{Yp`zTjb8TFjk$^L*J!YJ(GRMSEyE0~iILb| z!C5-X2Tu8v>DU}rm?^;rwOAMCyYF5JtZLu7hLQoAncQaY<+~0T8vmWOF$yed1V@*m z%NTw^k`i%ld;qhv_9srwOouagp(VAur*pXQ-f-~jC5(kM%(wfX%OmV3$f;rxbn~?N zU^FOk@V4gfkxb75B$) z7ZR+Wch0Iotupr`Qbh?bttNFOCa8$R8&ev0rDTf*D<`tQljWdwSd4EqUr=M2J+RO~ zpWcz^CWQ`e6w3E<&(Hio+Kx*NjPQSVZ1T|gj$~%QLcgQS<2_ugM{p#0)lB7!Yj=*# zPr0IOfpMgp6^XI@bzUu?vKUqSh96gxq>ex4Pb!j1xdvgZcx!?UuyD&RL#IYk!xO2p zN&)vGe#=oH=nhIz)u(CDmHev` zvpRfJG}~mDQew5n$U4VkWo8UKEdy=*BKvn$+2(aH(yaUREV11cA<9LDi+{H%Zwch;TQV@5 z<&ntf`VxRN;&BJ+1m1eV&V>L8g`#YK1DctGu^Y!|?b^FKZMp?c`Jt)~$|aXZhL~yw zL%bsP5Yj~bFS73BX1-a95IzBUcw5kE&-Ht5f&EB37MaBj60bMrFqADw(*T0~nI=xc z%%y#dMUvNGe?JWt*?aRKbKHU3QDp*r-P1jalk$Ln&P77u1mYwoz~E%_6wT%CC#rUf zU7qH(qCsoL%dEYt;@`5y8BXhRr9ZL_40#kWh8>1)F!=6?{;cx3e_b!eHM@^xh4XU7f*T(D_Ey9|a1*H0TPrIDeIY zT?(U%m_5$9OS?QQd&u}A>z~wrdm>(mev(&xplxIfr1^t6Z4=s`1`<~xlvGKhwtn}S zc$r_pt+WT^@5rX{F-*x?nP-qLQSL4Nl@_PefGF)KxcMh7qG(IZuB`Yym@2 zxV?rjw&RUP|JZ~kMEfCa2Yx*51$^9pJg9$NG71KK437r(OcnH<5C?u~e-Eoy_xn9u zzGf6WU08K>y!{J=HC^+1&}w__|6=dX3m7P*W8j|0a=m2*LzXl!TVM}X6|t*ho^93( zH?SdgxX6Ak>PEtUyTR>Ae~DyA;QAa*hU@EQ6&((jLlG%QG0psPHdHC4)$*4OrD0kTF9S$|}12)23nlwK~j#`x>1+F`9GlT3P19CA=N zbqkH0GMhWWSZh@))bpOdNh$h?sOp75c@fD-WO!F}h>~Z)RdmrwXrjJZ4_@wwn|qSd zRnm4wYhRU9``MB$DmGsrGIOZos^m}Ba(3()QEeA`-*g?&8{lgvsHBeXBmYJf35O6k z5P^?{X3-ounz~|Etui^zYj-FRL-`Sh(}MU>_UFI z8UqOzo?Q_sf*K`$Ut$!UEu$?loUoI?f*uZCX(=ZmBuYQy2t{g;O-K=5At76}3zxLQYYVOja{5U8Xo)Bv z?Qn9Fa}d;gHeK!o4uDfU_+i8($DZGnw9wHQq2el)wQ>KxNa=yDf-0vub;+fSzLgc7 z)GfACQK&j*;krEucL;^U%|cZ}>S(d{(>4FxlUrVL>wNeUG51j{FDhC96+N1b)b=XZ zi!t=B&)_yEZ}<9~cp2)<>R9j)I}l8O1f^`u5Td*t{RcP4a|}lAkR>{E%B2HUEn&)Z zxU4Lm=y{|c7s@Ry;A{s4aL_t`r^~B%;&r6grMLM+{pCC;G%;CUjSixhfQE7Vb84vC z9^$5rr+q_C5n4lP6w8W9C{_MTqr3DV@L|>hG4AOH4Q-3bP)Fpd0VaC1CuQh*BFlXU z#judk&Nfp7+mboK;0+qUQ)%q6aBL}O<0EOAQ5A@YZ$j&Ymnn&ZmjY(Bm)`;&;NxcY z;=R%$UF$z{WFp(uLJ@>Y>$iQs3m<|y6aRp`ENS~w`=-mRLdUe_@%3U0t|eWdn>1wE zzNr>c9a(bbr-y>O{PLN{HStKwyIvGKqYXw-aF4)wQd{4oY*}{7dku zkm#f&8i2_#u;rG$H7T~_`H5Z@{B@4x>89d4%HZI35K|m zGatjG>K5Y^U4HcfvMStEN8}wgX!|4{3t@prgh@W$MICJ{qx-G%UQif^WrU~%{)x6a zx5H&=y6g5m&VvQ9Ph5*n^_~nGylyvrM9?s5mm+}+@xqw$G5>90#Tr(im3bVUS8*xK zu2mzZH4ea%eUQiXF0M>!vEB`7@^grn`8{3Q;bVJo0|Z?<1A=|Uu>KafV(jWQCdUL3 zayak)P7F}WRwNBslBAu|?Qtc!^c`96+6iEr_8TX%6fb+Urgm3s#!Si!NM@1iW%8|C z)z)HljjX4Q=CcAHi*<6SBr9LVHxyj*Yv!XOYNh|Zvt(%o=dVb&UliBm=crQGO|pzE zk_#w^b=}(BS#u(!@V3)4qZM5CxQk!&2jvY+%q4Kr+512KaUToy^!9Aks#w!aVEX0J zWPt)PCqp3P^Sa=IaI8nvzJQFY4r$)+X%~`5J{o+78m9VtTK=|h9Xo|n9vrcmHI#{g z)hATm5~?7}J5F$dsGW@!WZ8-hthqAM&NUODe=pYZi3Bgo{r@&u+27cu;a_^%3fWFNqa_+IP);%4I+S`YN zXw0h8yMvo7HMn3w>qclPm!vMrFuNIxAI#j6BoX~)H~5sdmo%Rv%Gr6-#4A3KIh;4g zri^=2bjeR<#K&30@O8U5m72JgxgO&!>PQ}(LfTDjS{k5sO*5Pl)*`*!n5u{T@>Va_ zi(fzebE~1qd>};IafF_JVA{gO)~u#eD~|}}$GKr?OFsY4rN8_)_+hxs6{)&U#WaLj zj}YETc7Bq?53gc>ofJI7cjW6pl86K8jy``$OYK3P4^WIfx+WWR4-AI%g|Pu4JGkygo^yP^_91B^=7;1!ty43h&|0p|>U)4_tVg>eswlU6tEl@n9QX_7D^-((P{) zezF*l_&`&Hm!RR$!@=~n(&aBBqw=!8af|TqkC08Z z24%9IV#J}02K~y0URc@4_FYs>8X}vW2@;g)w1_z_U}(kfX~S0#@wM%q@>-uP0&`*;1LmY7 zTh)`^I1J>V)hwnP$L@J7Gr(D!6VXmu7*I$}>GllQE zA*c9r?P8GPNf31-(iG?2K-|%wu{2^NM`qCPpgSQ50q`}B$G*|`M1$$Z{?@F+s;qiq zN^)Z!)7CBjWGni+!y&~28&V`#S7Lp1A0L+G&^7I<8xdM{*@7-+fmIfK$pL+mDdQM% zwHJ$=Wxv*`JoTWG8+o>^aB0@sWf-7A@@O|{V znu8p|{?eydpXrH&Z>u3JWl|{ImE42JxfHt{Q==XUY#7(kZs!%_v^hVCRUqWx7?6 zG;jZ$HjN=XW&=) xr{HJa^MT{l_e=BF5pwpOt?$!!QM3yG{{tcP^JxG8 diff --git a/EKF/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat b/EKF/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat deleted file mode 100644 index c87c9e4772ce95a0912885a10aadd2f0888dfa6b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 135809 zcmb512{=_+givIT5X#&j8JbhaOp@Z1IkU`TQOT2%N}3f78Vr%B49%rZ zI)`Q=m7z}f_S*Y>`rf|pjkTWZ>Uti}x%%B}uf5iP-}^p$|2bH#T4rUfWT>O31_RPd&da}94&WC6`iB!&C!DJl=3EuMhJ z3rV|8<$5PUd z1pbg?abOJ>U)dx;_(SH#fz_oX(Bbz^+4Nskkk@OClB3<3G$)>KE;7*vM|aA~?w8Hb z43-^<_flB$Gos|@x{_$46S7&(+r->?>TldjQdJWu<+jqNz5=715SoPo<;9_gsjnx6 zW}rZYO1Z5?sO(9gvri)(K2IsPT{U%ZQYg~lW2M|nHL1gsLXi%iibD&jKLGTvb#lpU z_~o<>>>yWik30QkIfW@X>Mqr~r*)F|>;Jo;w*PHGahGzRQ04Cf<6QPXu}K-Fs%w&F zN+pc^kCGm9!#M_&^v6PrPRvWw<@W6Kwa&{=ddRhz!tXckGJ5Rz)Mq;Mm z%-8%YCS>kejdtt_54pDI@Nb-iiFWMCIP(|(rb(D+$G-C+SMX;3&4Bq=8>Ft;PJy#Q zjtaQV`07kGpM>Yc2g&0Gw&fkw2=M;4e>|HVmgRCi02Xp`pRo|%KYWqY;7NhJkieM$ z*hLMU6vzV!^u>WxdusS(z+nm`P!b1nH&M4t1`Owe1g^$`V{55ClLEORfrdD6bZio! zK!)fy*h3D*f&IUzRuhg82L{6)vM>(pJwP>?6bO6B4ghF9Iq(bYA#3Bn&&}k{`|D1@ z519db$XjvXC%;Ly9st4~@^k5R z+;HW#O$;x$J(<)6fcMu$w8YM#4fdR?;O88Zq~=aMlh>z{`|d31>q_}=4`WtN&6Jg0_y63U{mo7L372x09uW|lkXpYYI2w&gDdj#B zFF<526OHwtKGIn60(#`D0RLoTEkdK>74+nN0a-x(YavnBa?pY;BvE)Fk>R7(0^ncQ zy@1_aNTb>YjOD8Vsyg7U_fdlAq)}Xp zuL9v(>V$!mr}`IopfL^<45F5T17G#~!2etZfy6Qhzq?Fp$X+`&<9{!N!OH({AxPnq zy5J{m{v>RCA#6iti{R{q9<&MXt@rMI|D$x@PU!1uKJHM$s`!apqrbVHy?e`kqA*v+ zu{6wt#_l4Ewux$7q%EBtc7gnA;|4lHp6Jo<#k+66KlvBWeb#m+-BrF~R<(NWX zbDNQ{cnG+6h`)K#(dM=wVeuGnpEv*A$+35luy_!-?>>JE!2W%w>FC&L*$LY_=-kmw zgx_h?@!b&UK)V9i8ozh(d7_xdKUAv2-Wn)-hAFiP0=o7_51x!ATtB!Lgi?qwfA#Ob z@#nrbS8B7@7X7~i;z|Ew>nA>mQ2O{j`IBGr*N(#*=U6vdMbiYn=}gTl;@?B%>h{9k zg5mt{FTQxKjrCgVSv|dqEzNZ;&h)~2b^H?FELXG(u5QQPRwfKu>QVOmCr0~*$7reF z_?83O-!7$I0%7o(xh`JwPNec3pK!LsYSn9*2D&pZCDPQDx!+K@tg0xT6aNhh6N{$* z{u`D$lw06${OxSJDGiCHwWk|rIZ?ZgM4X=|xU;-6PEUnmU_*Wn*g~$%{k7fl2kPh(AmGFN@nq!=y zo5BB=3+w;521w&8)8jhr1S~xVi7txMJvC^=xpueLLU$jBY%#}1Gx(rVDUQN3QIrb}ANOWF6zeT$Y z=zp(#|L)8jNIM}JM za9$i9VoiGo4F?+)7Oo25=V>3H;b5P_!XU1G#qSGSomB3&!Y80!@(|vg$n`r zPg)-={36Oa*TUgpn`mF4;VA1|9>AZ{zQV%wQAR!=hX^4A=7D}q2VYy zR2jfCY5mY}lpQ(+z<Z9yXeH^vO0V*h@(E zMF&&?6&jIOLBUaqqXK~Mrg5N0j#3=W0eqY`3Jpgoj@+PrZKaJv!%>Rk3;=(LaYMsV z?29-K53|E4&~Oy{G6%q~V0_SU6#GKM;o&neB9@0)ib(clHh|}2G}w_}L|NyuI6Q(I z6NH5$*%uQW9`XX?g@&VyoDYYGF2RUrcwy#TA7zKm1n|q40QAUFcBmu{4^zc3XgJCa zH3aZHObEayyIka;+Lv=wyr6p>p*@GCgE@!JU(SIH-dmjZ0?;QjM>&Y*=+oi3T;B0C zChW+O?8`auB$anRjmX?!W*y1CoC9Bf`M8lc>M(R9`*IF^1?JOA-l@aTk?hMk@HLq4 zeDYQuf{tci&VjGOd{2}2>M(R9vwjYk^^McuZq~tGLb5NKpu{+0Q^10ptbP%2l;S7{ z;5RS{XgEr7Gy^3@1CxS=qZCIF$A|65WT4?F#Ze81hf^`)Jq@TiN3k!`03L!7FKs}< zQS6H`fOlaE(9?oqUqo?ugfpfD3rDgqx&U5-DMQ0i*0~}MkJQ3cVc|&j#R7+i@nO@U z;RqvV^5Wm?gau=Y&~Stu%CrS=HZ}tqj<7?SsyIB{2~&ZFBkWM74}e!=GXZ?Ef}W0Q zUji}wjC!|{!4V7{$-V??;B=q)aLExEJD7B6a}+oY&`;Bd*C-}4N9l;>=(vqPm+v$h z@iqm_ydc?^Kq@|R-*oau9fpo%UjkFM(1xeWM?Fs}4a&voC>0fc})c zSBIe^*_S{OobI;}?q(gVM-=L5W$7=|aO% ziX#tzH(+z1;V8w?8iz;B!3?0`2=;}k4B*9>Av7GpzA(LTc%(393=K!HFU%&LpfliZ*1>v2vM*~u z{W^x(!ju>Sj#34r81antWM!Q=p(yqR^lI2K4|9edIf{J&eH(Th$A~wYVMdN*UqBCsC<$y8 z?8s5RJYs@=4pE8N8dx}zeF424A~s-7&~TI;3i>=m+`)*)bYbQk$-Y?O@JM59CG^Nq zcIXTMFU3|v!%=pqCl22+6bC{CU4azGe?<-=E%|7pKFsNd8ZC@ zPLb?OA9$c~Q#E<34naq=FMZ&NMt?2xULA&xWMBHgBaQwC;BMBzdPK4>E}(w3W3Dh& zfPkYEM-xzD7GUnsa1{Fz3E-zOPiQzwapVK_OBVBnhNBcm(2Qb7D&`9fN3k!UF~yES z%nurlVqZX$iYRZ)9~zEgUqHi(s0Ua8EF8(cfaVp^78ubNY_jV#Sm#Lg1vIjVK8ppx zjvUFpfTk9aDcB}xILZzM4K5`>77V#j?f5E_oMLqQXa zD07Sf;FA^fTx9z~#OaP?a0Ej~voGLjM!#yfNRmasE}W?#VbjGGQ% z*06Lm`vM+l^vB3sb(nca%3u1x6OI0nev4mIez)vM->6$BqkF0yG?D`*lT=U}POaFiYDhr^@&v2*~Rq@Z6(L$)tOoE}LAM=*3W`vRV3-1Gq|IU>f6W?#VL zjQ(pe7ucCWvoGLzM*rK`23R_peE|T4R7%{F$SGMDku8V&rIV9`%7o8UqHPZq|wQh-6>fK^2&ZHgum_)M%y>hJ~AiUNRZQlP#on|2>Kh^erP&^ zf03UG=%FiU|3K3b42--1L9e3~LDLZ&jJzPAhw0D`!O~GIjJzg6FQOfWrXwtNc{xB2 z$7n}j=_n>f-VD%#yJ-iY=_q682lSBTw1d!egq$<8?LBJfXXk+66qKjWAN@IDJ^ zQIL4FM8EbOc*dBn0T;E@YoLnDvfgYl^f8dK2v`?ATGhSt=r| zaD*Z4IxHQ<))dVLV-FRXxaZV=R|0*My~;z-qsg9AF!PS`Y)p|dLH|Unf}R$E)vud<|p^q#_K^8E+@-4xKb>q4zal8#a%sRaEX)QTkOC^d2xpa%;=tw@rNQX{1ZdL;ac zBn%DgiZmkV?`bu#>z&AGVCf=&99)4#S2RRF`^ z*+%w5gA)Mz6<)v`2qc-2J<;FM3bYQOp!hpVAbm9hTf6!5!k2XP%nYe-4A9NJwGo7IK zOx!^24?2qTSpew4E@T6>NxV3I0mB_9iu0im^d{N^uyvv6D9(or>ihY0LL zj?$jM(vh4G8Q6={oCxGUB zi2Od~3?>Y}gwUK15#guGVxsT@XwF9z6ra=-OdMJO(fQ~S0^hKy@B(PgM-d35d0>*z z0*KDX0th5qk?+hxts*q%L+2s{&XKRp!U-VlWV#X%NRcDoorM!X+R1b=LLiNNeHKmt zX(!9yx6~8nN*SE^4cgz=_ka5VLpmS202qF-$;7?a{-7f`A3Blg2bWFUd+iT8g7cx1 znZD%2z1RMrBRC&Ak?Du*Bzv#@?Gp9}9l`m~iA+CafJ_IV=m^e-PGtI_?qoCoMMrQx zbRyFaZ6$L77&?mcp_7@u85s&d(Gi>voyhdVPC_OEzdHg$J0Cg`>nC3#o3Kq<4+sII zqE9E1{S;-g5!>J9C-z@!g?I2qvR0XXxV1{jB|EXf4Iiyu65)QzFR~jOoB&e2q!amm zsz2G04Nd^b`Ot}oKlLd#3+k#%oM9yALnl)HG#gAEUI5AY(21Zw?L4LdEr8~H=tS0^ zJ{{A77eLztMBJaAf$2aCpgA8pk@%+skgwZ9ts+F{Lxlb*&&fA#;RMi}50U$)E+k*N zg%d#A$wc&@T0y>f3nzfKlZo^{O>yFvZ+~Bb|7|4<>wL)c-+SV=ZGX^FoR1FhRUr=` z^C~cQ6z4-k|DhHWw{81l>?qEM$o)gll5N}mc7^$aj^ccX&_7IW;%MLxI*Ril692Gt zG8%vyJBsrm;{LEks!OLjfo{it{0Y{_sbTiNNn>!tl|1z?N8=L@I$s~gQ)T?A4x4)f$*nh1R-v1lPTAlX5tyS7AvXL9y@X_ie zk@BbIlg-@V1kjuhk@BZ=VHQwVUE&O*IUgeBPv44J!3&@{A0p^ae}UOR3m`flBJ0ms zj4gy0KyyAs+@Em~TMR9L=zNI8KXotprY_VfLUcYv=%23_Q2#P99?zD9z+j3`$EGX2kG=no^?zrhKhl}sY&PwOUIz`+S1l}sk@A^~BpbZ4>++~3Y{?7!9u?;eh1 ztqwE`lyIk4lYQXehL7ZYn5tm-85-C+sH-kfng4DEcFD}X?{0ogTf(t7Z_jWN7{J|S?J>>LLtfL1cM1Az=-vU?ny09wh!2!S1B z2RS$aw34}$5O_~^k^9>c`faW7-s4EtYC|J#tuj|(#Ji|)!$)&Io5AoiZ(_kvmvv$Z zp*f#vKp;y83xyUybUy0|fdd%vMk>@45wCK=Zvs;wkS%}_@1#NrAUdBNKp_1iwjFwk z5ST)k%?yh3 z5h3V>WHbOpM{z!mfWCvD%mtw6D9%TRpofy701O?;`2-U5?uiqD|30fo&PM^zqn1M^ z0z`viq|Aru1_Ux($%b>2)&oKS?RM9k5V%V=or4oVE19uCAk&y^JO?L$Rx-H>fn#Lz zIXD5dl4%75vc$;-bZ`P_vCj-bAQsC2m#x3;fZx^%?_7>#t;||*Yn9c5WkFpOi6w;Q ze8j==zO*Tfy}8`CcFUJ5zq*Mcq|)U z0BsxF5dxpdUSLq0Kp$-z&jtcn>&bp#Z~|!C*q;z+B71`UZI}JN68zgX8S*(FrYxXG z3QpXh?hiVO^Kl{Qk&t=SzipFY$8JK<-%s44?vJsfIG;#B-?5r(Q3o~eD9(qEpf`}w z02Cd?`78wVs5xXV07XY}J~Ih=F&PTL(2<;vFQ7*YlZgNn9mVNO67(oC4w!Vi{%)KM ze>b`s2xO*Wg-{jkck^U;0YgIIJ4Q53gF6DWk{JpFvOF=OZyKBcTFDe91llm7bsC%i zTFG<(0@-F5(LD`L0L}Sm69OkNqJbKm09x#`6$s==Vx?de{cRKcwpMuGb0lkZy#cpY zIf+;q^cqEUKCZ;@`!S+h71aD7W=e8J+BWtk1O~7(K;Uni;NR=}MZ|tS%&CCB!;p-u zpy(*hXC*;D3YS+AV+WHD>wF9VJxZMHL=QD~6z3C6(BsHf^iXsZ=OakaKa1~M0bqN6w;O@e-h3u5V;NOlDt)EYY{8ap9V1nm{1Ai`D{F z4hR9HiYYG#1X7%6ZSVp}6;s}f5V%3R4=;dJG3EJzKh?~&&y{|K)(Jg)G~*+00fwKlpKK}yRVUCQpCT?Gkje+wSB$s-5Urb`qv0YY zLSQ@XCHxd2ZQ~*_Lf|#+6}$k_HZIZw0%^-=uP0hU|6YLg5hI@>0YGOwgZgbI#snR~ z_!OxF`nH8MA~O8j^8aJ(2*#&KhM-r{h`bPrj$nL><^lTl88jj+grXxvK1I9)J(or# zg-~<^=ToFg&_`)dF(GkQ5u8tv1fT~8(9XfGNEGK&G?$=vz%>;IV@G++r$`9ULl%=w z#V4z1#LS>MpCT z@DJHS3~q|hN~SUpNaiQIh`|XUIiI2_gg^+{NDNK@$@vuN69R0qml&J?lJhCTfIx~P zT(fcFB%?W>A`L>|I$X1HQUGlqOalU`>Jv8=``23G0%+E1r5|psl4!K|(3=3!`B1^| zlfq~p;RVpP@hn21o7Mv_fVPdLfIzY{?K8Xp+BP;K1gdHMKw#2qB)Ex0xCbs00rc(M zP`~iRSRc?)oR1Dc526u~A=KDWoR0!Qe?=qmLMS?l^DzhXV0#)77DCZcoDVlaze*#L zLMS?l^O*tYA!;$BmVvhSwi3wT-SS20Ig)20Dv~TL zpq0#-Kpjd0L}Rr0)bRPvX>Z~0Gjg=CIlkLc4ANhh|Wiw z5O@pMY@9gBXy11gO$P#LE8v=q69Q_Ak9w^@c z3K3%FL*)J`!Wb{S0NOStqW_c~7!_UsZ5tEmf69AIV4@T7J2ydW;ao(d|G}DMFg)oM z7|>Ci4-x$b7m}$k6dlF+5V?N{KN$%_(NUZa5&DOOl6f!`9mV+&iGN7<#9{CsXBEZy z5OII#axxcy8as;fA+r9^S~3)Xp(8mTBIpm(hHEOWkFQ81=R>6YVFw`N0HXgcn)4x2 z{^T;auJ>U0sK2>O5ftPU3AnEJqyU=pA%p$|xUTo408+`M6G4AUA6(aaQUIxB(#fEI z1KCAv(#tw=4v?G=oecV$$wp#u0!Yq>P6qvkWG^u|0VL-`C$j#uBXFI@iF1JFeCR~n zpDs#v6oWbfH0MJn694pQxMt(T5uiCAIuZJ(xMH$US9Y+9&|)7tk^86I#pEWsvj4VL z$j#V&aBG!nj7^6hKH4@WqW{!mm?FFY+BPQA|1@!I#zgb;`x*&4h9YD>bTa*4O-5Fe zUc~?%!THdM=s%voyhvbK9ZpT3?0S!(21Zwd@Wp4aeaKvpg12ok@AN(LdF4L zMWTJ%MJH4K2XI~Q!SE5C51mZ;&EdMF#7NF*pG<=R;)u=`C=b#))%) z=zNH{Kf?sB(>N)B^lcZNNc=O7lTF2-rU=pb5TSo+Dy9#0We2MW(fJU$f9iK^F1!HR zP9~!NG*8TEqD$CsYlYg3UDFGNkwpg12Q^bfm(2|?3QoDY%shZ~d80Mxvr zI3FVJ4=*Ki0Vq0(^C7bSh^b^K07FM|K19$TkpS0JTpwRENX~~y`6K!v;{dQC5uFd2 z@^j$2-h<(zIv+CS-vrn7o)kc(LrCY;wy(b0GN+uEXrYmapD{xIv*nL&-8}t zG)@YjIUgeN&%6)UX`B#1bUsArpLPMWhq|(ZRfOn#h}=J20ds&CK-sp-Yh4+uA*7(U{SrKSH9 z=i}ie5XS#NOQ@!8>}Hlwi%{d^&J@%4fMuKG><@ZSUU(ou4}qop(8pf&sMnfO27TG#)=-8g^O_ixsNmDWGDI9JM) zbSc&FCPfy|?9S!VtSMXzrD=O6IQV)?_=5+}t>wh<5l>wI{oqA-p%0$6Ol%rgIhF4R z7~tQD0$k9AEzn)HKyyz! z^{L=ZA?N?hVIUFFv|gnWJGg&3H@<*Eo`hapy->z-fV1G*b^M=|5|Yc8Ul5lR=UaGQ zY*laI1@J%3E?nPWc_HS@XSMS&E$gEWn$;GX3^TvApLJ80FmhXS+_`3C{e`Nf$?|Xa zFW#n|`HZ6L<(4)ubxq4@=_`znr@k#LEz+G`-Fx?vM|;woscL(feyq*ICmY8``qziO z?UeksxiR~|oZuI4Oji1}A2Ra0E77LwUh(PIss4LEjvO&8_HG=@5X1sTUuC{8zgjDx z*%jO_*`Ga4L|0;wWBXv5i!UYS9i(UbSKeCqIU(gFi1utb;#4MxY}#FGi*#H9v;8V{u-LM91R%7@>}) zyqM_~xhkA%KJK}_-FIrtJB*b$DL%!PL)p#dyL)Gje;jgXu9@ATwmZh4BFJ$_yV<<3 zcSBx`W<`+0(AMS}?T#xdoFSiL>!In*=6ie1@?z8~`j-v)`@Bx>eRQWrv%_y!jB!Q3 z<4~bb@xq}6%{6Kre)%!F740)P^oo9mp_t|x-Hu@u&X`Z};vq`2d2z2`bF5N_s50lK zkNc7#o90;c4p9{j+s7iYcX8gRdWE9Hkau&eRtG-j0UwKmUbVbY)r#U}Lo1tOH9DMB zI3Io7ZHEk+V`p`=D|0%0EK+;l=Z(&pApXH63R=7J2$u-BSbOb4LT77Dhd#~h;8dbb@95QQ;RqNWS?x)tLNLvhVDQ@X|#tiB#;?)6o!EXZzg)6tLm(cCMkT)8%@ z#cj5J)Z6CX)%XwiC))Z^gU!8$%9YRXPc-$TJ~#J<;y>V@=;}v}HunlER~l!vbgJt| zbvO4q;6G%ybZY5GeQWO3R<68)f1;ru)zjR&75@SM1lQp-_fA)?497pgb>27ky5c|J zpWr&f&AkhhD~ItVy>>wVKw*K;6G%ym}}{CzBKo$ zC|Bm9vTb8*#Ls7CSSA^Nb+sEPWpO84l(d&gJcUPwi6_ zW?D#9#m#3}=rSzuzhlc9yHhusST|ZAQ$`uS$1~i{+gFVL z9qFn#bH=zeD>jl9ix1bA6}y@h8_J4xV8w1_jWyPd+9z=EVGbs6gcCT~37i7T@ByCT z1C-$nJi{9(!{xkv?!wGkTzLUw93Nq8-ROn{&ejCZ70PfwZ{KVDGhD%fQM-UqtHKz^ z=gf~a)=)RPIDzAuz{#Zy7xVU62s7iPs$Ng88lTS?U&tD}S2wygfm2Qyj^Y`Pq71+0 z?W+}L_6sumMVWEZRTg4Z7BW@i_@HfAV@-9VP6?c1%5W`jpD16SD1Tp^Fmqh0Y8)TU zV%AuD-Kcv4rtPnPZg9F~Ekm3J&{8^}UqrYu8{FyH*9cSM@trS(Gv2 z0@-6X>PLSha|E$rX{kP44W>K2s`eNo&YwM2Q9t@NnWKpfua)X6$3Jkbsy)t#+sdx- zX4kA_k6o)D9ZcrPVZ#RnhmEEBf;5=nvzg)A%-1*oA3*?n>~j6+=VXo{m#Y5NRsF72{VS{bH&pentrFE`E|%z1 z5gcBg%=u6svy5F6$BwmR$9k}1o!PMrw)tg-#W9A(MTP}FBWtU2b(v}seNzR8?UFff z>SLC&%`Y*E*Hk&_GW{g_3S3kO3|fCadnlhE_1C!-(u;$CW*c#sXk4?;cjerbuwolne(YW#(`aPnc=>+ zs@S=zcwJTTsw#Zui&s@aYpL8yZIy|KGx%Co%Ku}3_ z>Cl?o9lS(EY4Vm3!=JNzaK`uL<*hG`3NGy13jTvx;d*-C)-4As6+e%SbVeF`#B*`; zj6Kt}7*$NZ?a5dtFOu3M&ajoIExx^+u}WTeanl^eGI_zK+uIm)dC{gO8pFCsK<&02 zV?~jWTGMRC(ju(vwm)Nikw{sSEMrj-&F{7gV@;8;Uy}*Lu}JX!?MQ}Ok?8v-K86)t zK>GGV#&Wulbkj_R9UUvU?aOeXixf0TG8WQlYj3Y+tfmXEZ8Bsy&;_5}4rRE~MV~bZ zGi;aw#i4MFT5s%F(W|fV|#^$)Q3PvPjvtP5W zj7*QswP59rOdZRrWtES}jpf9(w2P!s*md&+L^5`=S@U>B(rwu8j7{>?)J8TVLY{l^ zt=$YCdA`Ms4;Ufxl%`u37+d7|n;Hig(M4Qpx6&D2MZ9W_cNp7?c*<^_WNa>?mNkB4 z>?q>)yH&{8Sj6Yo*ue-ZqP)L#l@VCP|Gx1DBbLr3eJh#aN#~VrY-DVs^Ay}FW%$#n z1&wbRk#z30xAro8>3nM&pD;q{lxMdtF}BkApEV9LVwhaUx3U@DOy2Q~16>=dOac=& zbgii}4BYv$D{RD=sW6^n#>%Oh+Lv{mb*@UTFDHt1s7ksoo5QN9QtZp+XC*|4DP*Ox zN+M(wau&08M@T4SH?b~6$SdTkvC<=^9Lg$Vos5`vD94Xg7$J2i`#tMwguTI&;EV2P33AvxiyL5sIC; zqO6@GV&+-7tfM0`<~dHR{1MS#YUasK-3B9C=Be%7f+NZ`NxI#dBN{a+<=t{4vucuq zy3Iy(YEsARhX*LX#KyA~*-ay~*opnzO%f+ z*n=i=({o*l%6Xo}>;nv;oCey6)R}s_d3>)^4Dp?n++Sx}l=7YxIgjGUzW!ywi1{zB zSOJT~V1|NDo#1(HY0s%TH>Kz)TdR1?7?S61W`3DMPc2|mnMoTMa&8G`4Cz=Qi-h9( zcGmkZf+I73VX?xs39sw>S;JrWM^t{%#)RS$YU^LK`oD;d__ZtVUZOfB-S+?f>*5RH z@$j*}*4v$1KDGY5$w^7zjPI|d`ql1EC@^!>b{C;$4*p~C^2CPK!H>7zT6@<;&1d70 zZ3fcDM>8|>(~lo?o_YM!iVXqwD=(Mcb8&e3c=NoAdgbeqO?)3Re$|{fB4?1X?@ILM zt9rZD=W5z@eA#h#^UOlQ=bM`zZQZnQuTjC$jI+`WpI1C7*6&^4()UlNv$FT6iai*= za%ueMesf%zv2o^r>6vm|KN}|H3sf-aeHxmC~sJqi9VjxUZMJRXyB z=<>>k)>n?N5DCkgdRW9`^OWuOk8X)6&2`n7V!lMw?EAOgs@%Csw1+!f!uRi)Z|~I1 zjNK7;XUL{7zhsRBroTO{L27!m@iDfKWqaA3XV!X`#O!-rF-RCIyLXnZtv&TT{l<2~i!Jqx z#w7OyREOFJ3>-($WW@$FEP>(>~*b9n?h|eJmG4;RDPaLd6@5>w6)hohJT%Ucj)NswX}+h z+4~F=S`LKi7;C$Jefw*4-Rpew%b(>gT*&02Qv15Si@kdnRDJ&v%b&r2r+(^pnvm1E z!&$tq;%Dp458yLXy+1E1smrRAzw*gI)c)yr7rDK0pmG~{M68e8ZEVslWHoU3;m&e7 zvH7(`(P8etD$@IWTS__i%l*yw4b&@c#&c#_a(6KQ;dH7R^qF-q3;4GkwoEv*sKnYL zhtsCqKzV+Y@lJtt_~sXz)uV$4&)I+R5INVPnavrEi~jL7$h{`|N6-7Qp7*iCSI2+7 zP5q{85j+0q{n-25TKD$;*k5kvVwe46@vCHiQ>V-O9qQww!&jq?mbdRKQ7^ba&j>0J zU0CaPz3knb)KI;U4N-KTZTpPoKi3lb^nIpMWUluS*4xV-GYyX$&OUK$Ls_s1X1Knr zY1L`h`_=11KNqZ=dC_fP-YIV5uSPzGl{z1HUaHc+w7P2p+q35Dd5y8D?XP1K-nUzH z|M*s(CK>tG{%*xu{g?%{pXDDr zG}&Z3iL6mR9q`(6d{l33`&KuRPn@gzE;~o5Rk3%6uZ(z$B%V=-#dQkjGbJiGhd0+g z4G7n)9NSp6Yn=VnrSiw3?e{Os^#+I>`Bn3)T>h8vv!1eQ!#su6#`jYdb%LiI zF%_0!Yd#yh)A03W*Qc8I73+@vGGmk&j?Q`am(Yqux zcy*RU`7yOi%d0PR#|&XZ%a%u`t#39da}K#*@=^9>uKmZFl`G z|EuoB=~f*jKelYZkTXuAxYrPZj_S&-RTOKY`r19=%Vh>cB4m|kyfF-Gy zugrbz_@QXg!*fm7EFW2A^-=PetLo15GCMiJ{Ux)PyNF9o3DLiD`^w?IQX>iMgpURn zWk&30Gx<@452pkiY<)LQi#~iUC?Ip6hx()YuL|>?j2VT5?h}*lD*T@FQ|Rcf7jI>+ zr)hi5RATL-Q0$W4Ewr1WC_d1AFr?@C;JLCB;+BQqc79WOr)s@^SH>2$+nfi-&yO9_ znYrC6{ZiennIQw$ZX`|XxQHpJ7Z#@j^1U$3n3l0+P+W@b$dpW|4lv9}yj77; znNKOUuUNSL!<())WB*wLbHpWR?94i7(?zTY~qSG!L2R^-MP ze3w<{hpzU`s!(y*n&@i!Yt^m?@_HTBy$?53I6kG6onq~cz4lKa<>jWw&Cd%ZvDKzK z<5xO8`Eo%eVU_GTTctHddGS#L%jPs}nzJKDXXmE}>u0dM2K5&E-1jiF?K>uyfG z%fXjQ`Zmwv1s6YAM18X&MErpL@hvO5%7qpLHJh>DN>rcT%(Zn$!(*^3@4%;DrM9b_ z(-JRby$heVVw)|WW#qJWSz~j9t!fLBKUGFpw5kNV{&>^+kXy?21UuSL&*jvWwZ1|8 z;~$S#-%v_@sLI2vw=(t8JIa}Iu)AjAtfg-+E2t@Z|8u~Ff2s6g<3~q6bBXOcBWk*p zKg@nj-?=+I*G1&a9ly6aRjjv`Wyxu^KA zRAb`GdUI8G-+bc5)%i-Tjf?Lk$}YL3KhKW);mrHl7wN_^#S7pVKlk(3(?cWPVg^OR1RlhM>#$tbSbhX(Ph8xtaM;n6bsw z;aKv^i;>-}+i%H!XrpL^P5snsboUO^lCS^cqx@#etv>NZ`H~QQs4|ebyW@zke**-eJyl(I^2|o(ik|@o`40%%5euNSnP}K1cIM zdHwxsN;|H5AV0XrffUUR!DE#jQh> zvJ)5h`G$cRD^@SU`)~s2!bxq92@Wk^c z2IDJlUA-~Af6rt5u55Zh+{+6`%r<*cRXzB>E-dX7SY0%yu}ph8*EHvf=5K|&3Iksq zefmw@ImFYu^U4C5TZz@@!ldgKT|F0gqRO%R#P?&7M+!|>+>Q22@OREMnX>En_RV{^ zc8A_KsuTNSeRo*_Z)w#m+hmWwx7E%2SK_l}<7v-iYG;)v zj4PJA#8$fnuXv#w>A*{;Nc6a7mh69fKiA=W?qS-IRVy+5x#?~y$pfD#HL7hq3iC#U zlGet2UgDziq_aOiLfmt=_@;YLbn=dV`H`^f#5;{jnY_UU_B#*lpVMRxdBr}iKKfBl z>%NhyF-363p6--8gm5%bchr+$d>{7H|ovU&;OQ`_yL@ zyAR7h)DV=}HdS~*#TxFndffwlbGl8L`>O`DLU$A>U%RFpy>42YxnE0kT5;%)wpRPl zq1bf6v;29I&I4S+!yE3rxGA>fP)G}v+9bcItgO0LiT3Jo#JanETHAMye<)Fv{y5eW z_BruezM$WW)agkJkB{A8`+HFGcAnoJG_XJVUQ}1g2WxZ9FY^txS<~y@oj;nr+TwWg z!+VlNyZ9b1J@Ld+W^iu1chIIABEkEPi6`k*u1-G~aooseTWYxAGIvFbTmD`Gi-Ps& z9_8Out}0khy*9fw<*0c2$eG!3blY{Na$}?0yi8YFc>TII^6ah*wJmd4xU|!-*5OlS zdNdjr%N42M1Q{P+L(nYZYDAZg}ukgHO~V%CvPKj4LaZ<1X#n z>N@dss5RT4XN{*_MK?1@PA<}I4qv? zz{GaNxs>HU=gjP1oVsGqfWD3tKevkLR~dhO-@RCC)El+))dzf>GWf4bFVIzRVZ{~2 zGwwSX=ru_Ny!lmsaal}jX5mh%#RHuk{m(W2S#emvSXjARU3qql+S0?!FcU4Fq8X@;}1-6HXe;N{m^4x3y}*KWUm_vPZ}j3(D~y>PCguhRn8sty!x2@dDh3ocLc z;onVbSxPy0f0vxmouNe4p@{uf>?U5&ewq zO42_zbDd>FIKO-&Z~cDBgP+yB4Z<9+mQa2b+*%Xl{5>*VCa-AUK;=oU@`##nMOxhn ze$^YdyT43*m{t_Gv|;Y`r^72UgE6*LxR~UV(-jqB+4pZ) zh%VglTE@2Y7WI(+eP#COv8o^WZr)CPm&>I0UP}D@_Gj|(U58XZS896x3cGp!)Pl8H zs+LFUy)GH09XubleMY>gTfMQ0M1)Al`jy2h$NIUJsiT8Z4u8-){3?`JP9~~ryWm6d zR)%1A$?fkasIwjw3sX;BUMJMN#m6<|+`*#LoQ8gnM-fXFsb@#nG@t*VmS!ckdS1RI zFQ2jR&ojDTIiFN*^Ak#z+XtO`D|EO7lPdnQzeX%B_hXEk9o<^gY31SCsMfgA#AmN=UR?~SyLYVhudTl9o@x! zf8>I-x8R;pv+py7jn1_XzMOToH{V+9&Q`I}?g-7A`O|LLY+N(5?*64rMXo)I;zH%` zOjGCceAv@UR-ERPxGr#Q`nwmJ*~j6SO3QpId1N)la)O;!&$Q&!FJE$6wTOSUibaW*OwofXT5SPI@O?bbm#6q>z7+DJ)gf{IAgA9YW%(CiywlON>&}f zZTIV6i+TE54c_t`ecLXF@jY(qljGKTnq|K%Mf<6x>ey796$`}lf-pH-sSgU<*k`Uk zPoih&Obuv#;<|J+yQE-x;R?4q_sh;LzQ3`5%F{rdm=m4unzoeWdCXH0g%xQ_y)<}F z-Pb)&-cI>e>2`BL&bVGi}>!-C!AhOwLY@dEAj^=I`ebN={1afFZNy5T6Nd+m3^ha!}T#2 zH41mz*342nX?Zy%rEoCWSM;c?vpZ{(MF;EhiM;GJykAxs_djY+zQ>z&%}Z{1ZA-QP zg{p9!ZGxtYE)UA?^$6Je;rYfZt|A(3t%`#2g2vx=E5v(OM$W3Oa@Zi+dCo6yXP^EN zFN>pXo=1!7vscY)EuXb}_lrAXxAUVMcIwKW66+RQ zeQmp;wa&Sq2M-f`M76l9b-7PIII@0ovb=lb<&5S>dlZhCnH}_2{}F6CtTBCub>O?hEIKVF!5dGlixnO`RM9_u4-o>95*-9O6aqRcP5Tifcu2@NKL*lXDFZU&&_8-&@txg3Y@& zU^^pz;AQ#9$+Q#Y_YOX!6=u$TEb~@B@00hj!2`4z#-~2W`ouBjE9_dBzv@oQ2lKYu zi>yuLyb9N7MDyn=RdQ}Gob_J#{KIPx+P=9=5j@}LKBLX{`W27GQ<5TAH7IB3WKQWU zIi|6XPpTl#OzF_Y=XwG!_G_>_6muBw_~M@kKYG!+E_J6;&Ev58$WP49gyGE_uVLp# zr^Q*EudtBwKoUzJ1LIHsB=zSI@F}#;MJIT z-HiAA>i+AB0q35XXk6cPXw&;cd$zxJ@tpQ}Y7XzQtN&~;w3VoQ{F2XHBko%!|*EN-MW?$>xvT=j76*ek`jU!Skn+*2Iywon)! zuo%BFeqeYkE^b({r}*6X==;$fqO})Co8q(zUbD8?8>|_BuZX=JXbd#7ZEiXe){hAQ~yAaP`!H*rhystCA0!EGvti7r&G^vv%O-$m_%)R&AYe zPn>(rRrS&G0S6(b${v}R)RO1VN-VB>6no0uE?fV84_RhxGi6Q*e0!hksFswU!MtnS4Lnk{W4xoG(|wk&a^h*}-{`uuc$Ii;^` z7xk7!^4Anr*y_p{1g%p^+;?jC8or*AcM4fkf>mehD!VRT-2C{Z*rM5qtPAsp2lGsh zZ_^EHNiu)9CCSpNuDBvcCrGnld8yY5yTAy$75mkXF1Nkcz2{t4Ynb#BbNB7twLHZ; zQvIHP-&zp;?uql7n6#I!v&)3f#@gDrnHzd@ZQZl@m21a?Fq7o@HLR;iTi4$n{I%Fw zwX-cP?eKhN zv#I|4vBr=PWy!ZGU*k>-*ss0kq$~Gm_Ylu^#K?O^^Wmq*Z;uVR4sQ)!vRYrG1QSr@PJjdTQ1P)yAYr`H4E|rt^lp%?j7<>DPVk#qK_F z%UN;Gr8hh6)7WyA=goZfr!zltnTG2;bl;e6-jx@x(za#kDCKPr&C+i( zIcUCM>GF*pomwJ18^xt)58r*7wfEZaY#oo{7vGoM6OG(M(LEITp>~Vqybqexcg(2= z={(EYFL}6}<(d1VFzmtmI->`9)~h2niTxP0RXtj~O880Bx!bGbgMC)6wWg}ixuIPB z#ap1(Gs?@~y1%k?O}%4>JELKGxXO`wZpPh!t2TRWON^Idd z2frkITokvw?)4kU!}g4_{%&A*yMC#wq$f$o;z}65-!QD{@`jNFNYq%OH70Qo--BEU zph?zfVuTVefL?E#>jxPJfD&nYMzWh`@^2xviZ`cuU&(HVtKCG9+X3Gxxlt~Lwp93n zLo}Z;KX$;b1}fSX1kGy_X3-@@M+r4EK$84oq*PdzfZA-5Q)*0t@142beULVD=F58OnO?`e3hxy8trXYA$Ss=qX zju~2;*6Es#pdM8oCJYXZ7qAnKqBlhYsR7_mE;nNL)mzpDZqQfmBp$*3bGx*Vj}KDZ z@#Bckp@%sl_w91^3{7#EnZE_OIWlm#Hwc+v13r`2ak(GN6EviNn;K>flo5D|u<_Bml03|unBBM471XC~hsd@{s>eQ6U9gK? z$RZk`$JTHfTW2C+gMHwXgKxD&1qQ!TB3{6G4fJw}gn41ZJwvMw37cNdkBf(}5RFhr zCkn-=+;biz=&zA_D@8muOU+B4b~@V9L6;EGwgLZ2SiP7ea~($Z;|0UW;dIQ&FIy0A z<55uF1Ax|6lm3L$Q`ECdDM~R118i%Vld5RaBScc7JTXw|G4hQ>bg=h;H$%(Q)iw?Pn1J{E_sC4CFo$3%%c=6UR zr3FP8brHQg(*#!gn;uqW#>mc~0KoBCQTlqy9@DY|Z?B;mX)#8iNDPswW!Ts#a8N0q zP!a&%w=z}W1u@K*;6pZZarg_Us%j<7SCxUKsFIrOW5<#l2jlwUF&5 zBn_~gl%UJ}4CT;EAf0y6!Cy5Xv60;QmO+Vv=fMyeu~FGnLOo-GNT=X}z%I`lP@fPd!+g)(WP$>*MnP()$%&xa zX*>1ko(DAQZY6SJKrz$+J{8PxLF9K4-;j)?WJm@pxHF;PrR!o#<%ZEa&4U>~=(Q6- zZb>3j?x~M-)l){yFo1p)uz#TRLKu`xvP=w{(5XQbhk3BzLh|S#I1W5$fWvdmpw@B~ z*w6D=dx`SdE}Rcl>)hHEp{5?w3DF3$CWRc`#iQ1Zw&rjL}^ zl!F6=bpjGeqb{H;Q;DfrmAFKPL1w@Z=)OWd%GVd zg?J1hunptQ&2oA-psh8fK2McWaUzSR9q^-PI9s5_ zplr41$hi`g8m9n%Ee6Hou-iT=27sK|wQjImCPhV;^B(vd&=|>s2$*sDldZc&d#$BPG*6F^b*)8(LC%cqK1NE#KU%`B?%Sm+h}bXm zW8na+&2LeihpouARsIEF6D#vEysD8JWLjI2-%pr)88E$i)&g3jvxE!|(HeYmzW~xz zRs`X=a-f&z1~@_kQ$+!-l^koMOwoVjvhtSlhGFQJbM0pf%~!xOnfIut3_G>I@-cjV z;uPFmn!etUqiFR&yNa^~XuN%qw>sON(&(w8UceDJiU|&>DeX}%40*&r$_6&ay6wPVhR#$~p{wnG64tu5h7W#f2=S zs>9A7M0Kd&n0B@zq%bQ_019)G!A7d*StE|8%cap+vA4jMp{#2hC((f|N=44jJZxR4^>iD1o-Cg)rg z3iEoDx`rhP3g5zECu!V-fi?MqnFvI`qSUtg(Lw_Cp+Oks(drBOOOXQLGs3psYvHsb z3p}pvGKcV1YKwT~p*GzT4tH>tQWJ}XlvsgmgafpwHJ6a`u31;DL6j|2z6UheUvHH(nP`raPRPDLLb#7e z1dCUbu4k;}2svwn<_fNYhIC6wK!Ji%1OJSpvvm68?X5s30Ts5LQJO`1+vGezhfi_* zCezeiy2m~NbW^_Y0*K21${SfB_k+T%4jj!5kB^l5qe_ckTK6KSh}Yc0@}w-sBxk`WP%F z6p$z_`NC9hmPcmZ0Nk85DnoD@zTBu7*pzK3YMXJY=WPeA08HE>aI$K}TPoERGEaed z5=eovd;N0{mOW@K19A)iEwe?obWOTe;X)hP>S2#QPpc6vBN3K1l)*Bba!mK5L+i5k zct;oH{vEH?0H2-4g)AOnLnAd-ap?R?J^u~>0RR8Q*~bp^+IAS=6CerfNsvbfJW9X; zdoz%M*n6i~X6zK3*xOU(3H(4F`4PhZl=%Gj!oEO&AOr|vOAsY+@4eQymWF%Bc9Z0x ziN9eK{$MIn(swF`{dK;!OdV$<^>NsNA$|MMtC7wsP6!Sz<%G`V9+!ZlTfPSJx|)V4 zbOxqDGYAXa%51SxI-L#n(@jqLinmObs1VYPwL=DpQ3$Pe(1Mz3@$%2@DL5>@Ro$=946 zMbXY98UdkWwgPWBTucrCp^8@N=Jq3-o$Jn--jg9{76s$YKRs9nnfuA@r5$SxPo zSBG{Q8uznhypwGMG=sKo=0v_4DN@D_kr}t?WSWU7tV^E~RC+4CGW#S#$I(twje z&sS|{_yTkFx`p`dA(e3Z$k&SY!$rsCq&|SE1B9UMg4)CNbsZlk1(bG3scYEnHGpGq zvR!Nls{()TuSEt|tUU-NHDai|hCDQBfN$O%B7C(^;5 zgrlzI7Yw{KsdAbepRAlYh{no~{d zprL}}W)9HkUPZGSFr(F{D(!3@`GIP4(~{!l3@{bz$2=pUom0Z|MQ8d-3>;yy&n`(dj8OJ|pe=Q%-}FRgenBa7d+sQso1Z9^ z!!gmqPhfR!VSA@d_cAmuhCoLvo>096TlIdOh;ND@myT~Nq-@XtWNg<%#sV13gn(3m zrSurHgVUagWXb0jHKV++xs4&L0H>uzZ;LDi!bR*ugM!a6TI1?}{=@(H<=5lCJmD?jrQYqGoFg=fx)VJMlaIOB-^^*Jt8;E#^hNEM6Z6X{|S$MPy#5!3L1q_Jx|t zmIvoW$+cD=*b)iA3(Ra&lh>5ur|hIQY>-g$4jL#?##lh5mA?12)YGxG{g2jSY>9+d zXL+d!5>?=W*}MS34ctaz2RY$}o7{hLN{zz00_$^k6DlXe3xRtCEzYIj-p}W99ShK{ z-k+^KFIt+`4whQiS}Mos&q!uLV5hSx)wJcSvmNKNq}r~+LYk41WDa1su`4+x=YC`q z(SHJ{G|JZ(Q;C*p#1DApgSqpt*>7x}(N`OGZ>J;xokJ!Nr9z1iytoXDs}VC!iQO5O zzL&O|*}E{uW#+GTIP8J0^9Ur5iljj{WQJ2;j8wBnZ(X5`Ja-&mlo>^Air0Z7Msh5> zYR~88a@yl{I#z59Q3N%d_azDPNN6lx)<)EQJjziq;D&}z1yL0ZVYDcPoX>_TIAm{w zR9!RoO6wH^=ddKk(94MAyUuGBVmwSeBD;!0j5q=*?e42hZ{7yZ($H(zP5FQzmaA6j z8t|8CAP9W$NxKE?8z`VR^QJKJ+Ypr#!wNIGclyrv=|iA;MpuH>V(vW6SB=5?o9U6u zed5fx(wHGq4x7<9>(Htqf3P(uG()uz4e+dTq|9i!IN8AWvp}i=OdE(ax-iD7tRp!y zY?G_4)(P+dgSb~A~^vCG}6iX7R;ohBFWgi-%oSg$Y1XJt77|=2bXm9uC{5swP(QIz*YCiz=u|e>!DotfE{N zR=$6+*92Q@gOt2{YRyI~NK4$ix~2>%v1T$qQBIJNPbmi1o--bPpehzcoS8Iky=-h- z1H+ZifkmyiocERqOXwGxK!X=O?83}Upi<9c)34GN>?WE|fHL8&)yKosu&c+Fx%N+s zAJ2GSDek(#xKaiOOSe=BrfJQE5?)>g|a`9byDg5{+yv&$UVmi7nnlwJrxW-X-j zffT&}-9R+C312~O8bLHMw*VBrzvC7y7M5uFq2Q@%X*N({XsffGC}*w3Dv(0q%rjrC z25Oq#{8>h)n$edx(L#o#9P9#&pCb!GEBpNNTu(@Att_U?c4k_&?yZb8l%;FaS}OmE z@DLeWeE}yB>+NQ=Vl`a`R;&-!<`U)_Au_lm{r83U3+IVRh`t%vL&DSqt~9VxZx!i- z>1*B)@ixX@A3AqAIXu^{$pNoi&FdoroJ_-W!@VVlHv0mr$ALDB(oO;`S8plPeX~3% z+7rnD0!DHzdtSi2A`7w@ZV<+#7$DODFj&us3b>NkHqzn=+C#aIux@UPCM=pY$6Po= zw||LSNGb$-!(5Ig^VPg6YjN*2PwMH0`_Aq5Bkr$7>`1pm7!45!6T)dgd=i(NiPV=k z>U9!lINzjtZjA~+6H?5D*rN-2Ax~Z}bbX~DRCanx**`=HDVOUJHGnMg@&?lmoO^hi zky`~#G0}24QH*Q}2DB+#)w&A$*`!$Jmgl{lA&y2S8CLT*e^+>e=Dm(aF$>pn3<^+K z7^!d=l)mbmh)q?hF2wpgGw^GXI}`LKkCe4GRZd-A7s|KI6Cpv4X&W!)q%bISzAn1n z&|*68OS0UUzE}cOq9(ixOKy8nDP@imA{}Pg4eU+VhjZ9LSQDu-l;+_hvXx3(s@uSg zYM_OUki36IoFid?a}v!eza@#v^b6kKI&{PYN(%u*ak&DGomDW|k8mE@F3E&cP6)dY z_$5_#_VIAq$|X=i-&k4Jioa|dw?kugh3|`8YaRh}#wsAIt?QM~a;n30fog)Cgt~#D z@YVP>VCaVLjVEU@WNT2zLkBX4cKRq`4>*9qM_XBeFnFVV!5Nzb6gv~H=CIFJRFxHr z5|@XVI5&=bIbeh{;6SoAX^@45I|R6_6x%o+4&J=LK~G;MhFnFBpnfS*V4svYVeM5m zvw?|Q!P)|HG?N7d=E{*oCgeEPtw#){Lc_}xM*~W$Fy8IiL!F7GM?zoEq}1*PmIP-H z4LILHn}$^12Y%S+|CM|A{|on+f5ScMzvrHBYwPVGA3nJ3@RjtU0(Sn=0(clv;6U{W zRO{r?p_yawGZh~LSAwswXnSl%c6?cLW6JZ47N;0Ei2Pjl*i8|r43!=o+~z>6It9GC z872A2b)+2m4kkswOFjC}la0bc*oWE7LfG zjZn@qXGmqW19bEw3dY&)6*&~S%B2SI$M1BQR@`KVCn7D-&pX;X8tLJ0AH+daas?g@T%5B0Nq#^xvYH0dAi zS)uVS+_Q#8|F{R9QFudtb59KWKioqauY}_RbhkB|R^L&Z_=mnNFNF1844y{=g5wse zN8Jf#ZJNAebPd)DZfZ%$>2s)B(y^L39<~=lVdT)zBVBmT+zoQ0f@bfkQRg&FoDGb= z1|ZE9Rh_}^dovIB@~AR~MDVZR5WZx{`Al zqLu+0bbnQrkwd;{D1QSy&=F_CrDRB1`~c7l-#x4cSGyrtG@Ds3D1 zThG}i@mXqY{mDJtAMTkS_gpAf{-t}Yzj9CWvwQNtau4vcd)^co{Z02wU+WL|3_rOi zxqo%fR7-}655K#oc|Go_e|8V_n|na3H~(;tHw9$Q{>eQA>i*%L_-FT=e{zrY7w-A| zt9$C--7^irU%6+Uy5HP0{_39gU)_V7(D=hW5|I2K_praar-OfYPyB~_SiOMs74*^Q zclTua`LlcIS;Ut)_`^M~$36bf?&1G%&znF~;CJ`DeszzGrXvftgA0<&NU4`rrBs~A zqH(_Y;jNtM5|E&6p(RVpnUaxg_K)MUN5tp0TzCiPbLjqz)or z#zA+sZWis|>IO&|gQMTrds9}Gftn=@BwICYwd@>paw?b~@nV~omGxOo&szYL(EVjf z$GA9jrql)s63_INaoc=GT}xj=wsoT zLI*fkR7pli)ctlunDd_w)%4pe+e zo@;{ExzyH8-5QJP*hF{5c-bzVdtSTVq5zmO0_5knqrGwa0tyahP;pab`N9K!$**$N z6xyyml%~neYNQv+nyNV7jLEP)?i`0~q8qQdWJIX6ad@lcz^m%vdG27?v_1v#EL=GH zRI5-x{o$Vf_V@q3d;a+^-1E=>Kl(i-_4+u#OAr$v3`z;5ZgXZHa<)rD-i1ZpVVNKa z>buQVM`a`)4)FFy0sHB9zO8^ngKY1zbiO zGS{#2lUV7(iBu5VlS=T`d+%ZRl>pF~_l5a3fE?wm7l`J-`lvYHPbu&X ze1$@&Ui($EK)v}-KD~%|$cV57Tn{MF_vt*zylRLlPasA3x?CErd*2d+Di#MjT)OdQ zRO(ykPn^1*ou>ds)E6{y7jji3R22~|552RX(fA&gst?fKD3JrV(Av1N8i0wWu9JjZ z1%M`RwVUj~^hb5M;P`k0qkz#!u@=0_TlWcTy*a~f;Jgt7&fyEcT(FP$KlOZIg8Wm@ z$B%)>J}yN6)bk0?KYBhEOAYtbeSE$uA?Kw#vrE5d2^odTIbF#iURtWC_o-?bfRu4& zH?Pf1aVhgwlj_OBlB+k|N$(MZC8)tpw3|6a#BQj%hP-wJ_7?4R5KcZX z^ob<{A}K>qsJbqR`$fqLftVeJ%BxDK=SW=Xh&+q9%e@bvkDHrdZ{DM2Vh^vHOpC80 z(&@stc!a1}x!ktfcS3IU^Png#^E z$BQd~5_I61qrtrHvBB5O!y#jv)7_nDC%DWR5lo1bRTaQthrRr;vI6NHdLMiL3EZS# z%TGorU^Sm0;en6dR|KO($%W{e&uWO3Q`$SEfl*Z;HPxj#!z7R#4Z_mBf z#E&%lHHTlMsU{&b0B&jdGOt}@D_DaJ0fw?Z)dkr%$R2#?&#pu8#|nC~krWsX{qNzE zm37fV1f@4_15T77WdJDUzvAKS=<3-YVh;BUZd2Qf>U@ zE1C{oa)|F;^9H@W4(`tj%p)9{b1Rfu_($8P83$M{c0287N24X%@{97I)=(gO4D3)B z)Mf1=9`0f{tu1W+Oa(sEz&ckBwj?D5kPGGAu$7rskH)%Jeo-kbRkTX+6fu@MnT>)e z?C_0F)c3}QNsdEtx8XN=f5tcRmC*=!*r%ufBkU|as2R9>d2eo|pdaK_j~|g@vOPt1 zG2d-OC6%&^eR|Vd^1R8%G(M2TwR3xQ;N%tw(Xz7gpeJT;wZv}#o=a4i^KKsm{k^q1 zbS;6GVyCtxk`fjhmFAFAVi>SJd6Z?An9_(Lj?C17 z9&%*`cF;NRBl0qHoTms%tdg=tDTp%l@P3KRGnDFGqu%tnn$V0wqwB7);xj z{DQa%Y`_ZeoqZ9Q3A){>cgKd=VXZL-cCXWR|$U`;ikB+23lK;=@+3>bitZ#Zh=AZ|9=C6ODv4=MI(LK(WMkh=4R`iI*yyqia1bK{ zk3^M)8^W3>5shd!X^ywP7^8hA@NHVbL*P}|v;X|R*#G()PP+g8XD9s_|JzQ2e>e&H z(H{cb7Z0;jJF1FJsD7x*3a4IQX}nq|^9@arrf<@t%aWQ0I=m4Wfil*yF*zJxvhE7} zoo$B%R!pr(^9~7b{_`cKU_F*Q&Ak)Q@9j>=)rtC{Elca%x0t?*yJuOksm&WW9zAU_ zf6`rsxrtIX4OBKb7)zYS1oX?CS@s|<9HOzS#8~)ikeA`%ankrPIr?)BsbmbxU_WfP zNxu6KM^G;#rUfJeY`6v2WOT5hocXlgiuF>r!xC6n0h=`I=8VI7w=B`eCY=g;>;DD- z0RR7l*vSrrY<3>tKT;CeSwy)8uM)9fo-BF648>T?uwyLddA`J4l2_*MZcBC+9VCRP z1ts8o=Y0-o!CZ9^nN~?vnEq=D=CKQfKyP|8hDQ^Ng1FtZA18}-#0&hEORqJwWv!sh zxomFm5Vk`^l~*BQ)6r=T25RZ#L-7uepFR#=u9S^+nB{~zRQpX9F&1J28N&mq z?kel^KADdt$8Jdy*aQ292G+$_AFk+FeehjbnaE<|&!|B*Vo&7F zS(u6&kP7+*kOMch7vS_-JW(ZmvJAz{Q}ANkK-%I`mb=`7q47$`YUG00g`QI6Chv&H zm_IR<->PV<&Y%KK5j|irfB7;c5Y(9sqPgtqM?rwxv6dL`=8Q0od6>ywiNrN-AKVSG zvQblE-bF{Do#KF~qYnZibB@pxQ4l4DbsR2h5E%^rkSBtbRf@-*O4kGYtwj0@*hJLF zI0854ayFTzXp|WMw3Bj(?=Od3?KZ8Op%CRZFEG0%hB1MbOx7+~;1(+?sKutP-@A24 z2)*kK8we%Avm6;4%Qv)mIR%zThPVZ$ERUOwE3~a5$xYpr?O9RE<n7OE|RLM5LDpJwDMc zydPL|8+6pbpPjs-yzoWN6akEBk0dmDGWU+3FXsz8SxQbZmDt&5i{uunH^-Kry|g5thIL=#Ps?-LIiM}_i1SSGTCAw-@4l`b8lKu z{usN?f)XtOXU}#9|I3EQwD^Q6FZveFj?HMTx zWs^(Yk+1df0S|l@`vrcb2CW^hDeb290bTmE8u8C{0vGyBV)-c#Ma7#_waNp|QU<8~rVL{T%hEqPVOy^#hC*Ak3vuizRrf?I${K-NzpYudJeqrbPq2 zssp%#c@V8&Rt0R=?n_Jiyki#d-tN=n)p*7^C708R3%k}`Q5NiPb1OGq8!Dlbv;6EZ zT?BY!!6AQclFgnz)|P+r05iT4w09a1*Zr?03MW&#a=|r>{&G0LQX(j$93|PV1iZC4 zd<Y zbZh-mX7xzeMR(xYn*FI!i@I+xl~yTC zrRcMocN+V0beuIwD^GNA{DlQZM{ICm;|`i>@5T6bucsyH;>}JX5_<7?5Yo*5~~p$@lXHfkH2jH z^WCBU@y8$k{XPD}yZ_g}{`|*(``h_{dG~+)$G;x`-2T_!&eyvi)8?Q5xm?EH*_L|v z{qKJ&K94W8+kpx;W-Qy(H#qlIVa~Z%weoS|=P1~wepDwOoi??<$|mw~96ZAYMTeCJ z^qj_y%5z*U@Juc2gH~NkiSY)`VA+n8Z`~GC_V9N|l26!3VGlHM?Jr;i566~M4=F9x zhq=Bz4&Eicz}bia_)0swbKGyuPG!FG)6n|#tqpIF3`QZ6hYILL`dThzKboUV)10ht z%n)X9g1pAG^0_ogZog&#hU78^WNR`dqosSB2%#Z7EkAxalC3t?C)%SfV1GX45ZT}i zTIH4GlcpdSL&cl4j>lRu5tjVc%xtUYsCN`VV;pWlA4^^9uud_<&Ll41_U0(!T)vag zrZsu27DO{?P@UXq<|1@+Hv|v%Q$5xdSlgyeOwAo(IUxx6b|f=aLX;j>_>JCEGtd5+ zX(aTiG20ppf_9!fg+JHK z$R_mr=pivz;b}Mz*z3fA)I?1KMNf{TT^Bmaq#%ZoCyk4-*$|L~N#CF=^8wt- zP*f`k8@(QN1hJj6u?=*zSEyFPzWlAZ?G(w*+gt6H{Hd32LE={M$OH3X5ARUP*eU7V za(`#^zH57a-G;qxQ$9WPYqj3b+7lFu&pPf~b<`j6L~BXoI~?Hn89p-*J!83f`t6ko z5R>In%say)O#q^FQ8V5e&q1HkSJ@XgvIEYc(PtWSlSqOa^2* ztNRH`J`&rB3jJucICQG@O-NPL!}bi6vR=1g17_X09O0h@_upiJDV^Ag%f%GT$)Tez z^+&kL6-}rkIlJ-1u@lri^@5>xB!c%{`N?KIymkmP4n>(15t8TuI#YeD7~V+_Y7X$UB~x zSWG|v!ENkOV&3aEp|;q<;{x>1`o~@g^isB+?om;sBR&q^F(*$SIILT-oDdMg>ED)V zfPfYBUSF3A$N0;~inr&R#eVJs!z0EWZmDWAZ5EE-2qI;6z;ANFP#ij-D=y1LRfR4XP~Qbh zkAFn-6g&VIaxYrcN`!dD6aq|h_!Sz!`9VjEuMU*B%#ZAyG59?5b6FOIpBS6E?Z*Hb zJ@@IcV2}QY5Ja5b5Ri2d&HGJTL56nJ;FCfoTtN5?+ml0{u?mO03Xa@1y4#;?kSI-u z0KoR)k3{M};_uTD1&yAmA^!F!CCwyxWViq_`r-6MHjEP*rs*?y&!4YHjYk=bipuN2 z!6JjMCbIs`X}gqDp&lIdj@PmgW-Q9Om>@qJG!rScH?lwIsFFzC zAm1i`$R>lUXAC3+)1}<0?wHi4R6u|>$)Z5e22N_$i}|yw<~+dyGzKiwyQp$wPOx0zB|7jf@5$<~ej^5L=ix3HKniQvh6@NhEQCUzl7wc{0&2 zG*kWM7JY-35hhGXuxm|IfEEyzQI5;+c_So2=OGa%@?x|lF~y^uoPsA$tqKl{zlD%8 za)-Y7LsLE=ytXG!_5=l40NF1~Pcm11lh_X0(;V$jd=>mu<(QMuOxjeIVdgo*fc;E7 zn@S=a@dW8d+Yb*|vgyYeyU){cyEwO@eG(7ZjrCoZZ}UAc?H8gEhE9(7#v2jo;HL|z zEw5SXj8-&B+~Av7%7f+_ZWiR5NI7iYn>S_ro_^;0H=Mj>@yYk5AflOZR7k55B~}zE zHj~fDi@F0tU{lZ>FI(@@0UdnR~)e0W*ab6IJ>=uh`#n%-7KKV4M^9A?~6@ zGu7()NGrI$oEAb1(+pDL)|U&w50A0$XmnBEMC^(=@1RvYf>&nAAr7~n6zJ4BwDrY8 zTB{#*1gnK{!P)0L_lQ@c|1?YTiO&XXb`IpI(Za%2IYzqS*lY$J92Mon9s_epkqttP z?IQ;|E#|_7P9Vv4Hvy=!JjV^wgGo zY!V%~-E8iJcK*{!yo8U*LbA5hd|Gbj&@qn(Q0z?b0YSBZv9Fe|f1J8k5(khA^D|w9 z39${_LN(Cf?%By)CcCvBY2({v2+Z-+8~+g(=8uzht0+u;t;4q(oINO4zOjuq7#*v; zQ+zNV=*xs+TfnEU;6rs*aF+~M-Ly&_Sl8H|tvzP#f0cj&pr8xEGJ5@B0r#;3ah#@f8GFF6-RB9w!}`CgCE;Z#i0ArC41gD_}b&LaWVzZ>_V zl$$m?aUV4o@mr=O6P^*^`_s%ys;A6)))kQju+7*#Zjh@er%ss-$;H-I+EskU0}<$y zmy9}xDY~K&Oq4}Lz0s|X+=|&2@LW?S-h4(j^V)7 zpxs)0a%s(eN@t;Z88WE8uF0NvMQ0ewVjN9$z(g}GxwH)Vdbg}ZvVQ0WqvabwK&-97 zgX-HoG^!(@>8r&1*vdNC_8D$~9nE*FI7?|owR+$%sS2I2>X~!hq5Rg8;ZnwtA3Ij0 ze|;3eR#gVmwrBQlkS5ASR0NfroV5CHa*r~c)7Mb9Bx?)@#K6O+ z{*v;YKc+=(*NZF5gfI9`V7b3}mWn84=L&5psC2cxJkE8R)!^k%FW!IkBK~^uwh3=b zV*I=2@ceo)o+T0%X&1;`>UdNxD_WY0vjemL-HYRSgH!o}&R@RstqULG1p39PfA^xGX1R!{vCEbO31?{Lg2I-sK$_%(x`!)_ zB@C@Us}mNx`34DV$;e5wO+VCHM(UV%Q#UaEN;5Es#r#mFJy8hkPp&?ir;TAL<-D8} zRzh*MTi^!)zg%u_!P~{jim3`%|BVzbbr)^y#-n9=agpdS`Z&_BHgTdwL|Z#ScoH|{ zEaFaD02Za%JephIJql}76mDN-h^b*&4sY~oZlQr!BH&jGou@J7u7L_Ocm;1bJ2*m4 ze-xHQUV9K)xF#Hq9OWFi+o1p%i#4b#mt6PT^-$l6=^sgcrj*u9C>@VL44HFPcQ{id28Jtb0v%=Mhn5@md^lXV{_hb_eJ!fbee+>+0Y1UtF(-o2E z8*V0U$N1m8sB3@m;{K8AKfOr))r<5==-YHflI-?-ML~yx-e?=AHS8ss3jB8R@Nv7t zt<1T1qp0=<9Xe;)&UnApV8U!4aI`=q&6jz=KU@m@* z9=C;U_z7KD|72|Gai1E~R8aoKp*&!P-X;nBf#IQHm9UE&2PtK208sK5_o=vDV4}w` zdXhg&0{Pw#c}%u$_lki)$1*{zu%bU>^jUeMey4x>if@^>^aTgm<68q~+JcRb8OE}1 zX9J`^hnR*!ofa?DQts6v);sR-IK|8TG(hc%)u?w#4sS@tUl&|rv%;3103hok_cy3X zjZtOEMyPjRO|Xvg>?6>w+5Vp>T*MlE=?FcR0nCJ#wB>JlF42_51V z!fcjpSboMmCxbclaXc0zf2{sOmiId{KWk;9^9Kg%DX$giW^~ z$H+PohY4?-t<4BI;STxMb0DMQz!R+w8ryhoexp>&`c2XAvbs|JP0;9@3_s~eh6|aD#uDJz;(@i(ex$_;x~=@_tgoohiqn zK@;X%9ShNz&|`2NlVRuM5{r*F+tiF(`#@rVbsCk+`^Mj!?T}KZjy1iPY9rRcN@=LH z9-nnw;^X7}gMLWnof-uAg$*_0=0Z;S6DEE6VGjk=5N24T}xAFp8KXgfDG5 zqw}-GA}dI&77SO9=InV%fWzo867pd1(_iH@5|jxy<$5mT{DB^&6io%;Td@iZh>Ql` zqFCRSIn~hQIYlJ9$(idwY!UIb;l@K#~0_R8q_PXKdzUZ7iy!dYb00960teD9P#N1Yee?dhQ#Ak4%WRSkQ zpirsweWg!k>HEIV)0^wP69+o-z~OM92C3}5*ZNj2QEY#}N4;UsK5~o`*Ez3|7#2X* z*-8Uw6j!ATR540QJjkTR+$eU>9u7_|g(}mL;V#Rf1pXG!#t`CSgXbpV+vGUtfT6IR zt_fyoP82pyxYedD$Z1|S*bkjPz1~V&EETZLnM3#xJ+8~(q+s-K#z2{Fn;XlNrq8m3 zt^gM`cfZg*vn8K&ce-mLFF(pX>}y28`Snm$6`nA?5lpcIpH9jeT^UI=qb*HH*sn_= znPkw#lPjZ+)gckvU_}KeR#Z3AE;TJ=FnY=bj^>*R&Vk6DI;_DbzcbH8@%hmG)js?O zUx#&u=;7Md;Q*aG6X@Ru{(VI_{t34%o2K=A;5iycI)^sItf?PIxgAp750wv&8I=$S zioAKiPIi3r;Sj#8If8+Utf4P@W~N?0{7`42ir^tm3C_fHwLeS(?in$C|FlQL5p|d? zePY0}t(WIF-Cj}M@j)N7jpG{kmsFw3o!q_4>JYQ2?_D>!wb6#&p7#^quQrgepGRow zArk4dAhc#k#6@=vd)T5Nse3!)N1R4JZH`OZfqWkj*zFvmx?=SvV=n17gZW)4A~mn& zH^2?9ZQ)XaL89`n%`s2a6A#iffibE(?Xha|p z9aD&m2|nxc6RyAs%&tE79P-ppXoFPljLqKOCqE!9It9U8)4AeW(NExgU@Gsr{R#!^ zf+>NP1!9ZyEIOZtKm7vQF8GeX3BbfU_?f$T2hG}3BEVM1IS)!$lmy_Xtcj9>z*+ct{Rl;E}PA?$a)9Z-=jrH_}|_{gZVI zeALOxas-eHM?{QGdTO%Z=mN0FSX_-oC4b3GzoJyO^D8zm-vUa++R!$!PR=bY#`o_pbXs^f# z<)gPrXNBL7gI1fj9Z>jy02U=62grd^RuAtU%;(mz3EF6*DBnSX`XbAi_$-LR5-cUR z*NOp8F7=?|aZGi#G51Chow)HgI z@7%skV6WrVlCA_s!gM3doNm{z-w3-Y{f~&^#TWUsuLi{VcB&>il``O;JotNe5VCBZ zA?H@Zk;8XcKrWyOEuH6m{^MW&{f|HX`Hw&TnEvUHKmPtb|KZ(#`^R6_-~ILeH}C$_ zKmFzW^ZJ*+-q*Vyljcuj%FN&8 zSHd<$R{G~xNqslfuElsUq8|YlkUWfLILfu|m*)scWH>oBM6vlCrs{?3iwV;9#^w?V*;pzlW z85uc`;Yhv8;~iX0>YI&v0{ATA#SP>UBhr)O!p?|@Q8Zc&i5`R8KyLYA&F}yXu>{5a zgsbSUk;Op)8!q_pnN4$~8vu_u>d%@2@G)#y3fO$H7|ihFC_(=eMItNeV;-(&8ux*@`(K+>Ex=`PXPh7VLV+)s> z(>^2Pd%TlcxB+)x3>Y;h+Q?LWWmFu^7Hx31;10pv1B1IWK!D)x?(QzZT?P-X!QDMD zXmHoy?)Lb;d+%HC{h8BM)jex^^_hM4K3!GEF(`kEL^}Xb>B8jJcu)b0;uroCrHCb9 zhl2p~uM|XHiE~=auQTz3V!(q}@Bu>1+*W-vjY$FOs&l=?uFYc%;|cpW-^=S|KpJ}v zV-~uFcnB^NJY=omv=FboF>pbJk2LdmOeL;%iPJ{Dj%$A3{AuE+3X2aOdbL}$3{4*+ zP%bX*ugnbU{jpGsl#IyixM@O#yS?L3XE?&7u#(9BiQjyGwx6_Qmrm4T%8c7*KwtFi zVx4yW`c=}W*>R!aE3ih%3FRXGfm@T&mw2UiHJR=lbE#EBX##lGR`TJM1`17Mi_mr# z3}AgET?CJ_yprVpw3=k{JZr?pc^c`y;>go0CZ2%yPe)X181Gf1I{rN;*(G>fPU1>V zPRXVpHdH{2ZX>9$YiMCA0-%E9K!)Oe0@2FF(#~^GTaV0qAdJ8TTXtF z=16F>li`luX)Wq1DkqX26SN5nT`nVfC6-PUD=-FML!>_?nr9qK6Q>}E>uA)qMa5@8 z1%5xI`bMaOxa34`(P4I>#9isHC9LE-ZpUk2|Ju}=JN8JfG~cm*pPw5FU2_-hX$bWj zYi2XF$v)yXbA^RA)(Cvwwn-~RA+faR_z0b~<;1^z`s;UDm$F0hw1I8;(sSx3A|cET z_RPQNu7O`$WB+y{gU223dJADJc2pgGtzR|;`2jtt6T~N*AG<5<95G_vdVCmL^8x%o-Xm> z(Q1i(AbQossS2z`UUejD1Eyn_e*q8Fi75IbjxxGf$*-csU%}dlE(KY^Muv8jBu^m- zDiUffhxcR~@!y-vT096M*|&KE5K%(wSZcMtK=&(7w+k3kE4OtzTZ%i#Bwk@n^FmPV zhG&Hnqt?Q{-So5bnX1AITV#q$|4_1se7K3f6NGFZEzOeD-=CCc3|e2H{Sm8wlh4#) z0KtPD9{~#+Jlv;0#S+SCgcN?FkM{tokivrWh>01^vzT++20`MF+4=6RQDx(O>R^3C z!I3fPhj+hH+-t?inA~6Bq;I)twv^h&wmg~Gn56B7oG&S68FBfYMx8>cA-)b38IBrmkxWO}zAsU1e5W1;G=e_haW z_e6cj^RP+RNSWzPT{vdd)aX74C2uLvE(-J>J;PsF;i=+{4FPU6KKKLVK*-O@E9g*L z2<{T2#7dwjjf_>xTrr^9luxY`S#xYtFBIN}EycCrsG{MB?u&1w_V_dK_)DBZqVFh% zgb_Z3kCM1Bpxy);e!aLE&Rmc9$FfQg)7f$6NdYatr?%5ue2QW+9*Ufq7OHT!{nFqW zZ}Dt(Q%-ssOpEH&2u_Pwfv^6@6zx9G7Y3Ih6vuJl9^)ElL8+wvWxLo*-=v}b5Mcyz;(Mf@A#cG`0WTSv-*k76)A@Ux zB?Wz)YgoUoRf*_Q=e=Y%NTVfSThQM%jGyO4`fNoHaU6~Ye_F-W`5U8}&!QPe>FF4Z zrI8d2Q>)RhdXB~!nYE9`?y=neb-15|(*);}{UHX#LMwl@8tV#K9doReK$oI(w4wZ| za)8}?Dnu?vhvs<6QJLLEjaNZFJPOURNwJibe+Le~pAZR>r42 zo~FSxWyc0{HhkZR%@|AL*S5WbzEX~HUmj~DTw<*)<&LG(Glg+SurX_? z2Te@_xw5Ta~IzE+{M~Qodt1jTjMlBWnHnm2#cKj`LmOL=3qY77y9_pbeY>` zf0M(UX1%`PB%;tn45GsK>|b5x;>4{cr&oed1(l};&E#G@E_K&8xEXyxfcUhZ0eVKn ztsCXE5V%&l3MJwZzO1XF9G3|+>z!Nml8oZsA*Bk=8w(%@WU376Jx`f}`@an+@Z&^& zC9v4&K}3-VxVp*jo{Sq_LGEBJJ$z6YhN`9;6UN)o5ApU1?;Mb{Z8tnhSNhOG1scB^0SDh1>UY90&oPzknG zwDV=P3``E#=$@rOVHoTc*ZVbQe}{QGR)V|HVxQwJcvGC*^7#4hEKH@Jmbh6{brEkA zci(5h`e1O-VPPs8k6g<2zw7n3hlKo>Mwq>6FPeU>*dT?jC_=KUe=f{3kLp471wx znk8H8HJFW1yJ?zU%5u2ntRW!xQ|DX*Hlkmndv9R*^FsoZN@?#oFJ;gTZ=5I?ZZ=}) zhbq{0PAQL32}2e;^A$|6Ys4-;j)6|MORIY$$m5%;xTm`}Wn{C!*Efsthmd zj7Y!~yP2_lsHrwk`U zA~g8x`0{&w&7C5F0w4Hm1Lv1==yhwbI8kc(&=0z4WP=}yFuv%PXScg@aqWLI4svE7 zYLJZK7Q-{LLC$vQ6!cZ+5tt~XiGC|TJO(7Np?~>DB4in>BgM;oE8f5|UU|K1CYgNv zbBpm!P0U8Uq6gQ$?G?tN+gJf@iB>p6)h%Uk8Aa5M_A!z;`7hz)2W!Kd8s#!<2kTAu zRgyCmVVCwbO5Q&AeX4%l>nRhf=5dbfHD725hbz^rq}RkSlYZ7BNI!mY<~*;xt#(1R8I> z@ciwC$h#!Vu?w;y5YTgxVi*yW|16B*i2UB-OX>Gl6d*)lCsAL_Q0&q0ArIt>vDue0 z)4HknSk*4~K)DBzSDEfAlG^uEHV+Z)#?pz5)5yCNTM0YYcY&=C&}}EWG1S{3HKGaK z=fKVz1HSk>YB`4i&FMp}@5%fqT|tum+h|C>wS@MMQES-rT1nSGvb`2;AdCvnpl1q( za2tl;Tz+W(siVqu=w~IZb;qH1qE`EcD=x{9X7E=;8zO0vY$y5&u~UAa=#%6Y*fW zHWUAdJ{p`&=s(HI>&%5BXqRYIC!2aJhE8~^O@jjkjVTFWR1i|t{esTJdLjIz6&Oyd z`-c_<_}Qy)Ta|+JG|UC`ofPiz3E+FmjzSbK;&qd$XMIgSF5LYkVlf6^T1_vZBb-(; zu!Kp0<+!U>$87aGj)iq}#d9E@rH$cs%-Ex85Jy^A9?A3^g{eu%QouQ&skZ+Fs+W^m zz~M0C7*3|ldhhc43U`23@G}AJSCyW?@A`7B&PXp<`X4&lh!n5OqT&Zk7Z#eV%g#go zTTxLV+)fONp(YZ!c!%WkFp&d&Vc6CX?R);i!!REaIqf;JZ;Vgx7warOD$k1KYCpMo#!W+xq*hfBn$t}u2aP%OimUtZ@DT62OAB~f%cU2BqZw5JIAjp@IS!7IU*14HpIyH+iL3_Q3CU+#8rHj5KUJi2@ z!7rD@E>7UMWUxcKs6bJoDx%nI4D^u|l|6(hE+A9%L13wrTRA#Yp)>Z<7}P<`D)Qm# z36F%rYTO3}$W#45Y$@7*^HQl74kOzvMNkh}L$$b!lNPe)4|}fA%G?%@@)er$A|yrj z9Os2Z=k!vFD+x`7O=uPqL?fTV! z$F92wE%~_>3g-Nk=qT~ob{VRkUY!^9Ug7e^M4_o@!499oR!4oHMI24kN~E3(P+7zC zUdC8x20SLL{S%taK1imKu@5`}Z7luK88UwGOP@+fj;m0fUY+5E5yTN#a7=_WB9K_1 z$y&~x=l%RQ*igiyY)XpE#r*Tb8tErCQ;x;Zp=6wIbv}W_t~j~UdCU%i5g#e=LF|C% z+iA4>;`8Pp-g$E~>fo$RY;1W(ICddUl9i0@k%q2@@RKZ}z2*UPSpNpww%#1*AE-^9 zu3bE0Y7!{JcHqjQ5Jh`g(q4M=N{(7!DF)dE^<9h#*1WU~Vjmys1rfdvM+1IYX>f_e zaW9HQq3>qLi}8@9Zw$#cPENmg%P04(PmQXKv+`^5>A}Ibkggcba+|~|2w%#8Y@Z^m zvMOqYB9i|g5_QrgXY#d@?wt#oPk((kutM}PkzshoxEZj7@`J@`2JeiRM}5rs!1b{% z3rtxbgs#+D%3S&Vb5)PbM8K7e?+I*XtejE3N8hDUc|yV;`yEvd`(SJ)uEqD)D|X>w z(;`A2jn4&7Ijq5X(*qT^qg&3A&+U+HyTV$l6IOR+8k^#e7kvBlMC~l9;E76&h_O+z z)!McU6{HshwFt#9rQugoq5Si7ZOB0_t#6k%mgoy+Cvupmg}4(43Ur(iHHRI20T%Ni z@=*$VC3s69Oy>|F=WD*%PQc*K}6! zOPa9MuLa7vP!J)z(s#eWmHBVp!V|W_5-D79d}6|5N)+W3>84T8oKN zxDx#pu@kZbeiG_4(j;&)Bd4>ke%6vNJLeS6Xj*X~QL6X8X3Jhl{g+|jSfcA*@>wL$ z0pAAtuPYn2Sj*JzoSMoJ125%!j+mK3)~3ekG4*EYQfWYH%=8IoT+SlytepODa6?h} z>rcfbs{yx^I~3L4QzBnkcF+)Dbc>F)JwNw)d11@I@VCI~6<}job-CUl12;ptBW8qz z=!=%kDZ$9yws#YB3XjN6@q6D32n3qk1&p{H<#E8(#)t>!4~8s#~a!MQk;3TCY5t27$rh z*tqGWm+e`;PTIK%q->|{Uo%%K9gM-FX#h#rlCpl$yH9`Ey)qk;Aoi}=c{L~J)gw(pI+DyaEf zEb_&TE@OSrOycE7k9%?f+xFQsBDN(6qE95DbgF+Gn54& zo1Ih%L}ne|Mp{-&vWO91`k94NBQ>jc(>3Z6A5cvb`SM*kr!M7GBCjQZ%GGB~$iKHVE*@hwriGSFd2%xxVohi1F0a5L8ATc5 zbD5)Z81N6Vd8>C@+s3&M8}q~k?cXaQ;1(e06EMY`qhDRXTFL4s6f*Ys{AJDHN3mQu z0mUHYPgSR2>ARFk_k4>;b{{Y*UuaStp!zLRAGavcF8LI#F8xVGZC~)h3ab$c)*wxvSLoJTX6MJBvV@&^Gt` zWmm)Z%dABUAr~8lZQ@sLNuf1hTQIR0zkKi~Bd^P4*?nG-AW6S}9gYAAjtrgbF-EXU|#<7L&Zt zsV?lFg?_Bd46fFI^U_*sG{(Fykz|j7;&M8rv-M3|7*f14KB=m;)LhO#iNkOu!kMr| zq!^9urYpC7(hmm|7-PO-F?s|Yzz$tH{n=>MF$UB?(V44li<2CAhOu9Ui6@t0SEx68 z5ASKxKtI<*!wn^tyqqYh}ye5aOU-U`8%^c*ZP%?i zm(ynT$vnMWgL3_rA2;Q^iRz*2bLd9*>sIsQ>ow^cUw6%U_xSmZH(Db$+Yfg$rtt{g zLMv|!Zv9=27&7QT;C`ZCZjaL+^G$?st#|AOf{(|ECRkJ1!LfghQ?gWLN{>zkzduhn zPXrq|N2Cm@{}Ogj+5db`(qR0GjlPXk|)<&Nec z-?hXxR;LQ*FE$2Vd!>@S6A2_F8XG!k_9-uNy->|*pr6zR6<5AdI9(itt8U>+IC5q| zTugFlWHPk!gEhao^*KkCR5v*J7aUJa=|x_tI@YXZ+TFgkA}k|w)o0IU0huq<5X&Ly zNP`q@*$Cmi$?3lUa>SxGq18;$BN%#-rCn#xBkHmR-xN@T&^OI2W44PrmL*zj>Nb0L z9>>9y`T21v-5;;2zcv@Bu9lWKCh7oZP#|IzzDXRS@t!~9|GJI9Im?TIs9NC|w~(}# zjApC61|&+UmnR#`!GUtr7dt>Rxa~P%pV*2W$-r!eeF|jxFmz|bh}f(>i0)GkSrA34 zapZ|L0&U&CkbMt+1CCa-LZukUWGUqLf;^rY4}1Aprd7O~nZD6Pp$&-mNt9uT*@)D8 z5??C66U31VR%1!Hv9|%}LiA6!kG=`%6`ys8(^E6TC-fNDZ=zHUHtz$LpP{}Y%&^d%f?`E+n zVHR1-7bG!{+3dqFKiLVa$Cz3AHyLI4U^Y?y{B9GzCK74eY@1m*aW|lo6db296QZ-Z z6;c`ISbMc9+Uw~-_M?@6&=sS-LO;zEM$Dpj#h;DliD^D^P-ZB`$U^8ptISJWVM(lBMzR@3_p#2R2~6*U1ut5kL!&>&ra{NQO&fQV z_NtIQ=_(GxtDNs=K6Xg?@|MnTjf(rZXiOE|h)S_BROAe+@ynLM&9K+QlwXTAn#W%Q zhf2N~j1DAShEHv)_#u*`J3n#cGOMtIfBS;V*g*T9)Q4Yc<5jDBxgw=t(Au5@;kH(- z-@<#t28|SX@V@PiB%kF$(rKYXLQrC51n}O3gp3ntyUyO5(=&Oyb%l<6qLMyd(wro6 z$;h;r;weB*AGMW>3y>B0)a=IN(lG)$R83?MDs_;>yV7O+`Xv0|oJ?wrLpv1vnQtC) zN6(A3D~Kr@^QCS=CZ!gG)J?Fbh)$_`g$+-Mot@t|bR3^*;K~^CL4ZYYW_GS@0_Zeh z7h(4Gm)@U@Sd9di4Wr${LUm>Fu@kXEmcMlXT^y`NlpDm*F)p$YM8{Ekfh$ainI_Z& zy1M|>lqQNGn2Y^IMHmrjf;mfT8GoD#k7k#xa^tQ1PTy+v-*9YBu0;~kn!C&7Pd5Bb z*JRWyv5k{Bgz&vY(D!hXJ?J369E7|(3KCevKckp&2$FLcRE!7@6YNhLj~sZ$}r9Q&iPck4G+GKCPmZ zWKOl^!(sdHk2VR414SmS&kQ3TXM>X&0&eq`8DsAM_)r}%r9HX$CO$^_ivMJq_Of|R z5bPxY$HB}VR2Z;@#^Ah7&!zR8W*4i>u?Hpec=>bB{h-?xz=? zUsG1iftz`nQ+hag9Sk&Og`H6APxfenrm2^vWKl-UA4&ckjr$sr5$r*8P#Pv4B@+?K zDS$F~?yNOW#H;A7%tAlSDT-vNh=L8Zx;#{Y8YpfbrAem=Lyx|q!`D9~Gh*B@6Ui-7 zCFlvLm6Q2%b>y)0Y7@Ks<%(QUPU!4=t3YjHeA_puYnXI#2(3ToQ#Ss+Fe6 ziAa84`)3t(R0wz5H;PE5G{3L3SSqFHk`tC5YMpKB({1AEwc4weB5Udam96A35TNAp zdUAzWEUf(;8+4+8!`|c~)(jn*g?+GOW!_COyv&F_QnFQoYVTu5r&lMrZ!tWeSw$snUrn%F%r-YFAmBlfWtp%B9 zPJ(2X?{sId0yzZ)m)_V4tBr5RS^>5{DT5LqD2w_yWQ(f&V`bi0(#M1Q`+FR|mKoj? z*3^L<^PTIy63dYH`0$uxc)xB&$!}!g7kuyPolo(LSzFNs^HgY^i@0Pf2=+%}%?8g) zX?E(OH{;USZO1Lk08LUq)BPPbTCE{Qo7}d^v4*tQ8eZ>aB3^_3aeCgX&Alji=^I4U z55FBhft;|on%|wf+}n1ONFchNFumR#1>*Hg=sEIT7!uT> zu0uji-K8S0+5tC+RKU1*w+_>AJ;<%yb>V4-yC2@>)pWggs+cATDUCVNv1rRoXwz_S z^mM0I=y-rhXmHos|1E$O7em(kMt}bK+%aK9xAn!PJ$+tn8rhDV{5JRS%gaexjYiy= zaHu+N|46{AoFh5Q-#Q#NqfgDq3e)@I!fiS{eTGK%DLxn|Gyg-^&7cUFxh{&R4v^ zk8RJc@f}er1n2#u{&6*%&o((j5DRChDOO2vvd^Lm=%EPJ_D~zQHOF;>m$6YM2b~hf zQT|TNynD~Y#=*+wD1-mYF?5;iaTe(!CRvH1k2-N6k1``8W4^r?C*XK*r!{f&`^T)~e zJ(5Gk_nm(}6wedt9Q~6MrM(${8^IHe{VJuh! zb*qZXf*kS+Nxgx_UnWvlMmA5hs*4#S9?|Y$20cCTn+=;pv_Eo6e-lxOH}MZJwv}TE z{KUv4&J9a=yU%UbsyeXpt)ACx6l}ViEa`s}YkVVGubE8i_iLBqaCi3Y1lP}bjZk3t zbY8?b)yoeD7UK7g!8hPQmu=b-b`F(=KJzRIP?NL=`M+=Z@Hz^1s+8a-O2{abf@eZK zguK7-4PSZ0>SlA8hur#e{p8qZ6g%$V+@q*4@JDCPO6ctqV^$@~2m-mF_Y=&V@op{BGXa%=B&!^!QH!`NiH3M?i<&RJE$e zDHjw2R{7K#3lAnZecJhz-^Ywz7nY^KW?&#wtG&aogz8&B@=Qw=SeLasWxOInp!slT z**X8xom-K>%~gZFiG{JtT@md!`NyduoECUK6TsEk} zoL+@}RqS1`sSfQkur|9+0c8&Np03L#R{W(7n_G)UV@ts?)cSk#Rjb0+`a4q>#E6Jz zL#L^!|95$Xj5z4L-oeB0r0@e<0ik10-N`I3y|4PY9abCqbJv}(9scW$=k3ONmCXkS zS#=}Xz8`n}YH6XW{m#OtyH5lWGsPPWUYZRkw#Y_xCJ;vA2iLiR&$87z0wYi8qrQMf z2j*F7c~8NBn}|~Zx3`Dj>$Aw~Gg8;gUv-s?m`Cq<0zcP-F_r)XYXC%?UlI-73)cIq z7;xvEH+&_fn8RYn$>|(az`%KgY?W^j!S{Y<8g_Ul5?WiOB#U8CF4r_$@8Oo$$)au)j+EeST2vR0oVr;oPyFa9{~~OK{Wug033N= zv`QvHR7&_2v-@}C_?J07w@saF+X8{dw%)uS=G+!H;vTxI*;V-zq5JxP(tC9U^@q7H z^%t6B+famGYonl+IHxz*V^LTllieSZ|W4@SJSr~8-8zXGe9i8oI;2tQ7z zI_^H<@c*xeOxP_8qxW3?>mmFf9^w@Mo79%zR0~xfXeQG+q>zUrWx{5`P%SJQOQGIrhY`r=A#cb<04nI%zD zaxIaXSs_9D;x4sb8v#8ttW5LOgB4sb<_}Csg@1k#Pc|ff8)O0GO*C@Ul|FJeN=B ze0|CbbeQI~J)|V_X#mRc(na7^&87e`#lz+;WlK2K58+r zMR1>EnRFLY@a0Y_bWED6^QwUalIGe@BV#Je&-fVDnkI(R*R$%&ooiO~3RYF53s=~a zy%8;4RuIMnLOI(`QT{cyAy$fI0EV?Y9Meh47(4H{I`c%y=GTb-n2-v6^d{!4H>}G|rU<~3BjY2)bs)ZRpYw?B1>dI&8tcWBT@tqAUQC07}XK-3tm!QqT z{G0h#lQ-LC)*$a~bTKtOP@EsQ2NmKzAFB!2A9-p{-tWJ;bXxn_yN7BNo10g}(5|j1 z^cCI@Z?(uC43b^NI5yx(4G|!2w9v!0N(g*grg}`IT6eHRvmIlBUEq+0grzaew}$n| zhq_~*wjglnf^~m1!OL+|$yCI7jXrBbYj#W2k>F-CSwAX>ZuP@A>)S?~cAEM%adfpl z^4x27uvkC#DET*DUa+mF{(1-P`M^(ma?(qWySngw^-!(^O()OQYX%>3qreq*CIfuwTg(IkOM zE(EpAJsjNN;e+N6rAr`;LgoQ!K*YYjBUSGe>jCee<(1+O$Y^ZVrC7(DTfZZ@%*nfJ z%2oOYKT4L)Y@2zWUTpU!<;HzRVMn&+97U0CqN3%tE4ABj@k{xuDY8t@rdaxM zCW7AI00BA9Hu$;YO+iAJzWVtfv*5CX83pV1rX&+A-x%`SeRb3E+Z9$O#;%xD+dpS& z%^O}BBF601Hi#w;PbA+?uXeKwI=*A=N%`u{AE^1}FgzbfT)$Nc#!Z}`dCqp!eD!40 z*3lVHlQ^qRLx@|?33;_*8(m4Tzbmv9Vm~LlpYZ>Sl!`r`Z|xBhUmr(BXqDC--42(* z!N-F##*dWXOwGsrOQ0Gw8RB;IZiCz*e0&p{1f`$v>ebf@tM}nPeXTI-qQ@?wDsa{L zuE0J7J^BH()R@?PgmVzZ zUOvt?Xj9trFOehAL7e0pLOJN7-QP{B{SBY=spO1fL26DYE<4ttDP23y^*h`_>x52p z`{?=-E*P88XjvgP+uaDRei8lr6$EcS7UpqY!h9!qHC+Ap$^s6j0f)cO(cM`#!r}Ke zDxP;2Hmq2U_q(u|*IipQ>*k-(#i$(75he>1Im!x1h%J41-kb_h`7U{g?lzgZJm+eF z&Q1H4U@vEhr)X_nvh^dfST>vD-Hqq|*FPryp&W1*|Ce$&^^~7}+|j2%^381%HV+hT zvBx??8RF(4zdWKRae+H!*>AhLfbwNyK86_dRX?qm^mpW%BsJS#zBrbjTdk_Q2{@?q z&Yj%XTe!;$D=_K8mRosz-LE|Knr{oM(ri1ejvs3#X;|9s&24>;bI=YH?){o8jx0R= za;lH5+B9()y}dIRC)g_+7+Ftk49)kMR09DO4Kq$Yu5wLB7HSV((k|Pza;LLIQR#ip zKM7FfFEDYXRgpoaj4?Ybd*X^uokc1@oUyT`+dIN1#tNS(&gytjJFmuoq zvs7>gOC*apRUDgO@>-IQFrNLn7DCH>{24QV&LUgo>;=9tYPj_bv>#kQs=|t;ulDB+ zE*N->R0&Muf~rb#GKX#qozHMEk9Eqm6kPfU9MAIC zqw8)^jd6eI&TJ>qClUJAzq9|x5K!b`4sjL+5wivORBIB5Tmm3l>FEy740K|~J? z!PCSW)!L^Uq}B{BT9SGG>mvKgQKngg$6|4|u$HK+50;t}>`rsyp6@4& zGnoo1D)xBg?9Ug-Q3SIeci1yaoLC8Cq1=g()#Jfov#LPVwM4|i_$NmTI%hF97vA`D zB6tBp=9h*A;-I}O1IE5;63>IodyWj+H&dFKq)3LLC*y;|b@+bEc+7~Zy*#o8_tW7F z+1#K|AG_$WV>p!EJ!wqa5>9(c-301B0*jZ|lk2l~A=TS&f7;8})o;ztJ=^{5yV?KY zbW_MxvnC(3kFWs@aJ~z~kpge{I|@3358g-VI6+axR!32?iQD$F+-n5-uK>j1gY+db zEup9*qHb66`-_*F4reXC=Xf}eY;Z3fGMUtzqDM&z5r|?0<2EVjS(DH8e+LR9NFBfu zx8->E3*M^3-qwJQ9}AWX;7?8%RUA!6Z%h6BGy^syPeGo*h+|J_CDusET> z{bhm!A#DXw`dZ!zv=YUhYi{IJxB8t3LZa0_aqVqs{Fy)ck<)U27RHgoI+qRDJ54;d=f8x&H2Rce(rMwJ?5ew$6Fol(7)LKvOb&pg!@!zka{) zymNgjrdRn@UH!Oj#C(O_a5}u+LCYTJku@`}WZH=!#-1!UZp|khALkE!$qe&s$Ptp1 z`$vji9eC8m!?8)PSF*qhNG8xSm)#mHgNnQ0?VPv#q4sL4}kLfu^_Y3 zxeZlw!J}T4bdv4dO})WseI6Y+G0j`%z;5sXI%FVigNVDRT@b4sF9;>GQg3CJJM-HjJoao&Ko)C2HH=>6L5xH@J5 zON~B0bo08-vc?b=TY8noQ*97V`${FK@c#ttJ?J+9Ohf5QUM}_ByamOd#ogL8OAf?3 z6vgd-C){O-AK9M&$6(WT=efFhX%kxJ%KZG$tqqfu-|UZ#1!-KoRz6(=@@H3uO2G(U z+~>d9YkxWAur6z^`na%hZY9U~gyX+4CH)^6izT5_R@PSjgrZq=^rfNE^m7>!yzkdw zhoB^g6Jn)kHdYe{BWXd}AAZVdugRMOhTEeBS8JVjq zlBvSs*ep})MHP$15}_&a^+ec++d^})zW%W)nUAecCg2!$uyHGYzafv|e;~kpT}QXz zhoF)7zGLUDE6DP(8Q4fJ%8rBLs<%&Rx29283t=HEyq@*yeKL3ao_WHIa7$r62l}Ye zAu?P>(DU5_AkZrl@W%pR-0O)dFY+6J0b>3LE#Qnz@knP=EqYXKXe0;U#xXrJ+pxBKOellk6CIIo9eMCaBSz0RDO;A+eI;v+ii?WG%e`q z%FGA+-vOA*Vc>b-oCmvSmmxIW>XqFs+q3=(Sj3U2fU=BXv6|B=aO7>CwmG-4xx@|gw zFPo%HY$m3-%@Dt8O8e+c=r~3Gw=cW!D&zA3l6{s~WS#%#(UmaiXT*dK+n|OT^wE`l z>E|)blD`Z(XAIIWoSugza0(8=f0kxc+-()xoP6>ttzA~ zD_GJOmnqV~Q^g*wFkcQWnV};={ED|>JrnAkP@*if&S5?Vk&$xdc zr~T#^WW2itR7-w?&IA1hj~lY*=_-V|I>7kOR>BGm;KeYZ$y3-UCcT4bfC{Z_2QVB! zhb7SQ0oQW0PafG{vlz^kMm#`r`{ULJt80pOSoLnQ!bbmO9!x(a=DSw6Toa;;J4-kK zTb&`Hr*$wsGN>+rWSI;>&O_PF!srv&y@$@`(9q{PA5;Mqzu<+AN8oH#B#hmEU*XNf zTR(`ih7*{hj`-8F$nmP9PApTM;3Mh_d^CN9_N1o6a85Co$(&<^@Q{1UYcMR@i3#HFQcyRrRwg-^QOsN}8G2&f&7x?~t$k zrVed|@{c&)J%yG?DD$5-k`939=j-3+KTsoQYGs|sDEATRU-|kkYM_UGpoUlZ6EdJ^ ztk-VG*5N0K!`B^y5S0_gp;a41JQl{lmfkz4y(yZpUvb|+Je3`SyE9@RRCLPexuHGN zUl0emCufMs`ixut-F&8`*V5UJ>NRqxO3k^mc4sm23f}=oFPTH68Y&0hh**IS<=M*9 z*FFeWFC+7u8E(V%$C2*@gt8yslgmYepz3XVxw~3t&C%)gd2s*fn}S+*)5gS8%i2cO zj18}l5ZVt$V&*6f@nIfu{h1Gtl4wC)LdodGj291 z^S~Q?7c`xvYv3ytIDZ!uWKpmOlKxmuhNA^e*u^5_YybuB>IfGX?h#3YxlGbNFlAJR zjN`%~=hVpznbS?>JIMS#p5lms{-AWZ-uzef~fqW&I`Z+|g8( zLM5^z2JiFwea^haQ--)22SHj8+B-cB7s_l4b{kNPH~k-Q0Z&&=I>LvB-gmJdgwBTc z4gCg`1AJ-mw1CXcQ|9B30l$fEu$=`r+2OcY51{l8$WA~1+q#|exzKkWYfZA@8Fo7B zfN%^?O_BkkeV{{BGfKjfe1XRb^uG>=iw84^UpCQ5A7IxV+j2#@=#jUtyX`!p-==ilKl0bS{NJ?K|EE$;NA0YBQ+`83 zXa7p(@T=43A&hOv=X806ljlDa^iDa-y6>~s-?DF9(iG6mxf+}t{!59Bo7}mRLZH{K z_3yX!O%gVEQ|D=ZOabd$L&2uckT}o{G`)DKR+7_ zpx$_N1ox5hg(g~4C&;xYv($%_%4s($cvV#jNQ>e!7Z~y2K}DJw)FcLGgb7f*q-tB; zl;vi&VoPUuZuqw}CJjX1YmbcVx2?`>agL=+l5Qu=)YF6?W<&H(48*5r$(bmZ*HKLb zT6<&zcWwf*CP6}P+dd~er0?n_Ru*ARg>{v7)Us1T1RpVdo8Nbs$sy>zqb;WX(^;Xb z8MM_wDcm!JUdi)rvYi3TmMd?J3LNu}t5=M3l&9c`^ei`S>3*=u_&&MN(oy7)Yu!{Q@?L-qC6hR`}>t`Sw zK&MB}w_*X2(qo1JUS}o~U0=$i?^BXgP8bMgeD`*-o})rt^Z|guE_SK5nz?s#z5}s2 z^|uEhLi5E54rPFZT{}!{OO1yo!#M@swWY=1d2aH^shO*2zqxW#0}h%FWIwa?U; zZAtn;@OQiR9)8jWCvDPyyB13Kp?aa!{Hs5J{0 zO4a3f($VMneI~vGHukM|N;1JScivpDOqrCZ$91%fRN&{bI=gMVpAFkPS)S+jUwg0S zD_;Neos2;LAAj^+$GQGk{WaJ@x4+k2GpTpFu8(!y@w_T{?skqAR|aCPt~Qe`4o;CB zh~yD-u@km1$%b_>4-lmeFfWSQV9`L_e&Fy8fUdI|0@(KUAZF!<&# z&r-gMSDt6{JBl+Vc&~h{qU9DX;ISVMVH&olNFe2$Sid< z^$DwXQJX6pq-M_ZC}s^z$oH9u%|}jabvP`lni5pP6-a|K>?1DbsF3>O;hF?d|Q@2mV-9X(pWcfD=>*s>!cp?7? zS))t0_e+w|^k3u_L1JO3UZzV39Iw_hi9b)BBA9RE4<@l^R!F>!g>R>p8% zPN(taB~;%wQ8#n#VYL^TqR5gLRU%T9R(xt*wPHnTxkV_q;9%=kkSnG1lf2`1dH*QZ ziw(qCdHQ13jX|nj8n~PH^{V#}qI=*N8O|KKWAGRm)*Ml%_c$7s212m+_$C<697gA( z7tS0i<}T~{V>1Ky#OpJER{{=aGd)%Xjyj;?Q5D9D_s^{UFP`Lt+(2Y}+92#5yn1N5-Ip1;ka0RmFh~wl)B0mC%3?KR`pmiMsC* z0FmADz@MAv& zrmuusl<%NeTMas0b4>ghgZP-Oit6)N;kz^Fn*4|%?rqX3`I`KU;Y+jahr0b3)doz8 z?_sr`YFh{vx_n+5lr#J#61 zr2gT7*J6`lT=xgfmhZIu<_zLNvme)|nn9GpER?G#BN&2783&?KqI<3-Kg-k>1jQBn@z`Q3!d>?ug_k{o7I9~u+_cx~6M0 zswk=_1x~&+qWc~B9z-$k2NB?GLGnvqBWu!fmt#@0wfwX6`^=>O` zNP8v|x@`w(&tyWkB}jWF6S}Q~v}ZD*+xn<_rr`Y2ZDUAKzIQUA+qy{KQ<%_ggP-V` z?*IS*|Nq?BK~tMR6bEoSQ#(C%`WZa6r^FbQW~K)ce8^@~Ppg2r8DbMR>Un0dp%?KjNw-ut`6*RLR+>UP|X43nz4m8-wPfNC>KvrN|O{+U3q zaG>SI$88;`Hv(%Ow9WOR(O=v|1I;^XuFYuRDxrDXyt}r7!1mJ>gml@!|lnL*mb>!i_}Gl#T|*XeNQ zA*2dkr_G%wkg0f`6Ygw4c81qU?X6QegUrC|bhwj1W}eTv1-?(C`s(H%Dy{U}ozN46fTBzoOId_uEIh_=v3)QUtvnsF< zt&<9;4lF_ITnMKL3{gLkoMYh}14GnLB&RE!Q(%bpPRZ#Arw0tt-YGdf;S7P{?JFbU zOd&zEcS_EgaORMp+&jtZ-dWH25cL(=>C|&RM14heiajMqnr9Lzw>?CfXA&v5Jwdgv z?3~Yf&WEV4?3~Z2mh&O*D?GQYaz2!KCXsU6A<{gPNV)ALWENUyEw|lZo)c=hZGm}C zsO7e;|8Y)u1^@v6|BTtm4#doQ7T_R3lmrQ}WSv!z7oMj`S=jB_?WsL`=XsuIUl13> ziZvI(8vn^bBnVEhRBBbrQme|Y`kuFXv+CL_=SpbZ;&WC5ZshGK4^K(5pe0?#p$JML zsZ<*XlxdwaY~o zXKBNS%>toLeF4dP91{gM{|Q!7V_K7fYPxqim!nXlb$U#-+6_J1|Iau%RaWI`U>L4o z*!cWojDL*pKmE<0AK!of>wh`EKl=ao@jaf{ZifhYn4};2c=LSBcZu8aBdyC{K@%$> z^1jey%ahC_d;`0ilOTxDh2%4n#1rrAXHIzzDtY7ksefyLarDVxZ3BbYXV~RVj;&K8 z52HBnKI@Y$i%k()8}i9@@+tBdu_4=9LwPT>2)HTlg9M28bmG_nKl2Bu1Kp8(m$4!(Q$0TGx?pu1khW1)rUsT{ zYi6V-MEoH?&$fSXIotN;$fsQb{LP|jsjadVNu5J*Cl_X!4R*+F3xB!l(Mz{iQ}k(m zMBbjA4~vlXD8N@(eH@)};=sHibRuuh>`Njf8IbCOU&0Cs_D^O2_prtq-eLe2G#j%C zKdhN5%22*$ItF-eCc>DAE?+{jL>F@A%9jA_i!OhTuoxtdBk1rOBu?ifU*F042sb{K*U`6`cu9O%C$b`|cT;>tWT(F{GGp38X ztWzd)0s3&X0Nrz%jw`zc7-(ew)F2y@yuaBy&}1S%9YyKSx{+7~3@1u8r{t%91P@(ERLLqic!C|{LLTlrN*sH|>Wq2HxNP*W+LrrmXt|0W;R z0Q%==T;nn&v_CX$@-v%n*6*JIJ0+2vgf@ZV3r|)^{kVe((>!$z)B}w?5YXE`xDhN( zPNiEg5LBOFMv#HO*a@{;{qU=71`M8R#x%e?{Z72k^uC0HhXE+6CZ4LI%g&O%PCI~Q zrw7SNTHbquu??*c^l*HH>@88VwxE?VOtA&T(JfGg1>OYFz~43S;mr4Ny|#`L=m)M= z^S0s7(Txof-gAajf^qBV-};*fgs4T|TJ{=zXYT_*Ub+K~KP!9byrH1%KmvvhWY13w1A}7Kl_(VVOd|=J3(_sU%M}b*c?g<~0E!gRhSG7E&> z*Gx|jX06zG?17Q0K>=SG4o<=9K-=A$!GJ#Bile2_7%8yA<72puivI2}75>l)#;*`t zh$Z65Q>vbTq+^L~kPreV9Eq@Dm~$!%^ck{x(wg%X@CpWbq&10b1aVmt{me7z!+u`o zoA70JGKrTsh2sgDBTU2NrJ4$e^-l$%wj9{B!N`i@i7K??0)8)9hc4nd>aQpy*B-#T z0C6cg-;3~k#Qot?SS6U>Lrz#ez`WKOouJ z9YQ}QM#vOIrCySOfQWg*5PJ5Vs{|>W6~zd09?zFJ>_A< zbg<|r*8xGB5$QCF?qME!zpAGcgPMqbO@Wpv!n{i{yF!bs7BK-R?r^z+|8}73Tys){ z4J%O;E1;PH1sO7HqR+eE0@vE17?>UYVDxcXX(@nD;cAo@TA-|}Rq2zQ&ol;}VAWH4O0iY*( zi0n1V?7$*hdtRrD;zN8a975WM)V3p-&Oq51jv?D34+y5p%yV#9NX1}kr0_S7>ks+c z@yIha(Pv_%wxOhnHZ|!%Cn~|q3LCJ_#FK3DfC8m}m8HxNSOL82c@XX|uqVx;CWBb; zZp#vdpC7E~Oqtn*f-fGkNMh2?k3F=r;zA6Yp!RQyS!yOve+WYZbl3RQTtw0J0>$>E-V>DAunS*ycJ_z%oZr&EfyKf2qIz z^FI8`|HFrW`Dc9y|KUR@Y>u|QSZk&~cHOE~2NUIoA-0*l)2X#u&YpjSD$ZLW8bcVs z%VLnpuE?XtD3GOnWSdBLiD$u5LtC!8Nta&@HDH ze8}WlY%Q%}GvEe2_Dmua?5NuTglu^F+>Rbwa1PneAZFqwKwvW(#Qco>IGb4jeT}33 zg?K$W|6DQ~U7n)R>qs_$;CnDR`uzc4#}sQM1K#&Gm3ZZ|e4wlvv$ouHCE&LqK6n>> zs4QsVlK1OMsU#WEj2-(+m@Ue!7=q12$piM9q-P zynQo}KX0}6QD2)4*95l^b7O{kX$6AU4-K<+`{F>QC8@!t@ zHH*2a(D>B}-FAW}Y!j&na{S5@ip~pC6llaYm69_|V_;hT1BaP!8q0?(JncGQ2q)5f zIH|9$$ZOgyU;Q=CRr0W?*}xRwyg4S9q6m)>i=V+T5YsxN&9v0C2Xr?#q;Kl`kOnG8 zwCvl;{D3+r%z>~H*w-f65q9uERKNq2813q#SXev)1LXUw%o`h=xhh?VNg+noeo0O< z8wX9^`1@I$O`Ke)#PFjoE`XR}YBl_Bkci|qhD!BMXohV7&&J|hwLT43bu@mN02+Jk z#4i$Q<1N;tUYC{f(8XnH5h&9!YjzYlv}j|rfQvbhUUZn%z*&+sYYQ#QR~(jW@853z zgN-kC_qeiDUS&ef{Ayl$^Jm2;u*uTo2be%vNdvO$MbkE(A$V@HaMUeWEkLCtZAMS+ zfoLt)wYUF;oBQ3j;AM4vx(Hx&k*ecIYevAk*^lFybn7s!9|5#`l%6A?AC z*@7U6H}niixhg&0+U;QH-C@6L6XsQvD84S&(?OYIAS~qbo*Y1f&UiP+r%-sksO` zg%kOB=VERiK8rc4e#^|7EEQ6xa%(YR#~IkXmC z?gS1m4Z9)n_1Ohd_wotrvDGWm%;vTIn0DDs{&tjU!KNW_axVk8F_FY2WA=`X!^nHi z6TY%7&!M&LE*9n0cn-&Wxs>h4JtFvZa64PjHq_+ zQoi7;VJ|lvr^QDkFzNvbud!t1=owK`I)|9&(E8ymkQ|2%1p<~AwTBFPat^tRR02C8 zqt$~^7}ogG@Q`SiU;@ZVOZo=6k&gNU`kpXkTJU=>tU`2zvEF7fsGX^igO-6h6SNlo z(sBX&yh^UFlBf!r#kw~ZvIOy+ICdT3h4Ln`9dvM?ErMl+i7d^Y@y#21&Wmn#p`YNT zA^Otr85e{nXlHx3Ur){aGqd{h=n^8T#l9u>_$$NCj7xwcUp`d<8r?KahKJD&Mk|Ep z`F))h$q4v$4rg>rj;(^`IsQ-_nDil0(;&A8AD4wJcAU0qL;)~l1jw%0A%-FSt-%AV zD?PL=>Y>TyXdzTObgtmJy{3-j?j|MKR=KIV){IB!1zxr4F^k4t#U;62f{=as<7B<^O-3$+ZW@u6si%?xu;c93c zl(VnopMKUg)aqUgXqj;yOAut}kj6ht=AE%jd8<3IG}k^LlkKu3wMYXn0F%jKo+Me2 zP*477z{ZcOb_I(6)?%IM?5WNw%?PI7~$y+!wMzx5d zI}46^CnG;QGI^QRF4?xzPqY%PXF!C&9%f|kLsSM>j|Le4BSo{^PT5S74CfK9(qlue zNXh%EwEU2oy7fIqfe3mO3DPM&dBX9{>-6|&O7zoc@WZj{%mI4JJthL6w9|efOu7y< z7E{kta3gf3U7!^#kSN883g3NLPUBjWbyJaw1k}`2gL4tDe(gk$16^fr!u~QO4t@ew z`nh|&5ySd35X_)yuPx*rUJyC1RR~{EgM#hWarv^CQ+%7x?VJJwS<=d;bV+jI@4KyT z`q;G&^wxfsL-j9|2wv&_p~jgP5v%7(T52{k#1E_zt9IC-+oZ8?=k$J$XRZZQWf zk^#edSQq{qspRzmQWd*Wa5O;pykZG5}>)YaxHL19T` za=wOtrn#WScP23MdlKxC=$W3u*~l1f4B`}gfAjik6?NWBn7Yt*ih>G3{vbyM# z)FLY2jmR}Wdzo`CTm5v16eQ22UaSc0NB@3VMMmc#<6%0-d zmu|w%w(51_9DNxThYDtyt>Y3HAC6kI!AJC4@@GCUUFJ44W7+0I95Rz}w_4p*n7&xh z1`z^yZY6&57U5F73gWA7E2n2>BQ=RpACRP6Y@ysRdMAY+Puqk z#TmX{Jr!@nPcDQ($#`AGpbn}El<yI1LNjZwkz8PLOQ|5y@X(^WfG^+ncoirJ|cN7c4Z6ByvJoezI*0)fBl z=w<=8dF6TV*zh)**Zfd|D((;d=rV2{N1*!Bzxx0_XA z$O{pQ^l(qe&s?G`b5PHSo{~(IJ)yq}l)wz54`;0JOd_`sMN8V(kA|Uv$=~r?pXLPF zFbmxsM63JiEz>+;=U&j@{Ju`AoaX{SWo06IX>fDi0=LI8-B41X2)Z^E^ik3 zL^*qJwG<)>a4N0lR_I=wz1O`D=q*K{FK@c*1#nyWlsb<-i9*&qVo`C^or*I{H^&h? z@|)ba^`QsBKu%Dt9%oO=%(EzAEK3qdm=%u9g9$jBwAPe6@^L1F^gwjl0d>1D(&Aj- z@Cz3*U36R8pit8|kV)7&nzt18{0{Nkw%|l^%ld#0ZJB+~6v3^z7(nq%uFk)|fmC%aoUQYJjuco=Lft4=f^X;oO3Q}2tm z9^x_1fmtVtI`lkE2=%ZXP5KE!m2S*%JJt984x6R9ozXuQH9Wr;(gQ){$&VecY-yulz=%)i@102}qLhwoY4{R^-#SJP{R zNtALzssD;hn7ymrY1S?J+>WAt0#(R_^OAr0g z|9^O>6QS)u`5&e)+tkl?%Ot_vlSC;C9wVKq;!ULEyuO z5Pq&s>#!M1F24$br=AG6ntGzyB@WR?q1gUNXmpd+*`hwljOr7&?t3pe-&4gtxsRS!u4kUv65 z_ip+@*TasThQ_nU;L;7p-};VkTOI%Kvr`CfuC3aECB5jI_W66 zS_{1Ob+`sSr7q#un|9v@5ZDBa>nf)SPmPMA#@7k6^UBl2MPj!1y~aQk>MO!DRA7WT zb_YtY3jbyvLSPK-|3D(3Udb8>p){<{v2Ih?Pld!`gQhLMQ^M{Snl@r zDA}?1XO!NycP?7ZLQ3DDWwMwO7z5L5DTT>S|ETnHhi`iM$yoXz%>CWhq^vbhzgg<` z%Uk<+%8qOw?A=l=CrnTn+gWQ4UxZRW%8xrAzl1*f=fzAtMHw}Bg z1wB(ZpTIfJMq8DA-xYFUmP}oWNxcal{PTTBq=&L^PS^uYB17BrhQ=(06+Q4=@xF;( z`)?v#;rf)=qU)zBSqyR5sRyGm?{o^gbNcn2q*-uEw8Mrc>Ad{|c1*h7)u3W5DSX6C zCfQZofhh90596NxbcaSZ<&%nd`yldS*xKaD7{2)hcAv?)Nt;alOgp983(>7#FO|LqXx2C?OsQr zqnabQp7RGSrq@nz=V(AtdH63{?^a+nmPrzAVG#wTtV&fc8)t@}ThxS{w2~7s8NwVl z+}N4fbz*vf>@uK*a4+Q`H)!tLboV$FX#RF_AA z+1P7m2#$G#Jt)ZNai&NW#Kcr4p{8ft#g17VZGD8@^xIvqdgxOqlFX&sJF;H*g1`G{ zkF(b%94=-)^h01BMOng-djLBgfhL6h}_3g4EK0Y>c2Z zC5IKxCH(Oj)>zR<`z><|w}&vjBCxq1w6#>QCS~H_$HCvFHOFT*T|M%}z2KI;IH|C= zrX`Ep{^W5&E@<+WURQD}lcpcAlw$EN@0>+LPvKfo`AWzyK(reS~k8}#pqIGF* z9SQZhHVw~kubbAu6l+_Bi0siNQXzI_@chUbv?w!E(bYbLFQE=jmz-g7!UL8{+X z%q?b|FVd{Mhv8fwY}6tm_bRJbBI}!5Ohz1ic}T%JG*0Z>f?CGHo7SC&KvWO7sO2q1 zedyPI_NjRvFp`sCq49>vV;I&K{KY1HGaa$9J13Gzdcp<=f6LfA*X`1thnd77aLKMq%t56G48KWB6GeU=n^qQB}1xi>uf{LHps%azvZ`V zXS}_eK%ckFsXEdCJ1^-Z3~bupp5U|HJo*WaQDr?C6Q$fdK0yC{jIVJF<2vMSj`}b3 zFqx|#B_<)hJvhxKm!d5@=-=saE0O)XGSQ>sMP5Y4xZ&XpCGd*;e2rlK^$4>@E-pN_ zS(%|qh_rL-TmJn7gh>a?!+`9dv$`t>fVW`_=d0k+FTVI8k#|J>^QH|T6mKu52PTz9 zfsMjD=RpuPbAz?ZO5Gd*@+~bIJw+U)W*Iq#|Dvr9$c8T>UM&_Rm6WdV z2?vcQHClTjB*|8kd$Tv4vo$Yph#mUT{lH@tILe(=h2o$8^^ZUP@sF$h!ykYA?e+S* z>-pM5v@N_((e8daH3sNo#c~uB=7K2rSOp+)ejE?mun=>(rsME2ld{zB4B z{Mmg&931v72=v`}>BmbaES)QOgYXKLKQiqTg}z0NbY$T@Hhq4;)oExAyvt_xa0ElG zOvjvjIA6N=S45Lk(<)b3`>;H!TjdBO+^6S@C@utf`u=_e-X+${Ak z2ixp9ow`>gl104*@Mu6b{ATwcDW=wvC!*3)l;Ej23{n}QyZ|-&j>et#@z?4la=|H> zSGToT%s_iiLzda$+^hwBjkN9g?*~?t<6R&xwY2VQiTuR0x@C%Xfm9`+?TVpXtc+g# zc(=@&=|FZPNh%v%$wap1b_XZ+1;9NjPD&`}0?gRS!HSq?6`?g6+#&U1P7 ztFhRAX!i}(p7=NtTxV-3&kxO#N$aKF`aUn!A8iEcPc|+lxT(yK=Eej&up1)~Tox&J z;7nLAWQK|F!_!^4JliI*dQhs_%j++e-@HvhQdH-4q`T(#LEo+{jRt)ULC164uISj3 zRJB!qw{8ALq;;|KLQ|aa{F9Y|MXShrj4P)xx?Bd~G-IjH)z>DR#JgS%k=d!59*A#( zk1XSxj`m>efbBl;r05U+dvS5w8t8{;``jMgtr3>`SxkDqEfUF2Fo>=K_9?Yl;^u=U##?g`EZ5+!27ed-6ZK+_=HX0M+@N-~`7a|G3Xn1t>7 z?b1KvV-TxhqWY;X*6p$P$3~tH>dn#}bLrh2?sV)B)P%3ApX$`_+c%#z9DaOyDw(ez zHNPUuJldtSNHRfGYaSKHcOL$>ktRqOJE~+VBwJ7E^Ksm_IU>4>9gc%S)zq}2afGs0 zO}KZG89L3)J{>Hup9-M{?v%u)ekv0yB7LL>2T0&s@q&Hx><8&d&}eW>7dbI;^T#yz zTv#4`k+O$6ie}GU^&xIO^;SJiGsmZaiCj0&jdg8me3gf^&^ss!<$&Z8YhyT#;DgkA z#m0rk3cpfq7;!{};Bn-bK!H0n`Z~p?px*P2Y1>n1g(5JSI4xWH2x%*T$Z zUZcS$ktV1DVWRYagCX>c^XWffMdY-8zV?O8tI*&1a#n6K zky#z9@6qLX^~zmp-49{9Ci;()(;&s}Y&)+Hf1U%^PGB$GLjD0aJ(_c0 zKKhu#kEEm>;dng7As#pf$g{sHoV8tW3SThVd}~hDMt|*8SQylkVL}YLp5N$q%oLX} zZ!YE}rgGzoF^H;|Ingu4@ioeFtxl^z@P3%N6PY?&$03x$l|P)ik$Iq(!w`yO3?n-; z#;+PZ1zWIg8q-?v#@(+fo-kZqzI`qnQLY{$4}(`0!RKu4>$tvYL4WbmG~{Yd=$&WA z*;3KyK6iR7F+-fI4_lz98y5J=vV+IK>f499l_9GJLN3xObrQ#b^-qV`&G@oL6oTrv z#BL91W{%X+>!!-tQ{0?KLfawuwB!i<6kY4UoI4&?d=N$pl_@t@hleRmu)kr#gDyw^ z#=m8{xlp}uxKDM5uBaIiie1c;uMLO=tvjB-ePi_=ukkrPXv(yx_%;7Dt2T%I``(Q8 zeS@eiV`|Nb2g;_njt_W%h$)?9%uW@c(7jwx=I^wYT^!9tzcd5)*8>t%12rQ&w(wF= zx|0FtV>d;=7Yj6e5)=9H%1rPIG+}#1JNKM!3&9lP8aYDhe)%07j?z<`Y2*^m*7m+p z?Dpa8@D552*h#~`8Y!Y94vUgd_@Nflm?A$0Y9cAhy37~L@d9zb9Q6G-U-n29#O9iu zK4bF(2y}UzvD9Gpn#237h&ZP+uO+QdkWlXemHdUqtK{wX&hiF}u$%m#UECr_c9U4! zn=^ZkrH{&nq4mJw>(noNR(I76OHCEB4j1Ws$Ki6Q{H4+bB!?N^;9Vf}H)x8Mn`VY} zHvfp7>SEPBub6Ju4NKt2LU@L)w6?mkmXb>FqXI3wDL6=c2T&DB-+nuBnz&mfiyG%G zUmQrkowdS&Z>|M~$bRD(E^e1e+Rn@1%*Tl}t#G%kZ_nS2B(_Z>!ko*A)Ba6Z4$5Vw zN)+svz5E%K#ZgFJK4vRM;ngF7sNqE!3YFxvP1{xw(35JM`a)?W)!!|oKeu=6kOL6= zC+H7EIXWHV18}<4jf&+H8AG!B4|IyO=eYK;ij#AjN_(gvg>?9~Er{ke7Hg3X9S3x* zVfHxOUuV1LLB3xBRQ}uVD>zRw_NCP2Na+>vqi=&7553D-{rmrL@F)ELIhYOqHwXXO z|Gzl6ch)48LxRJFSS-WRc6j%2s`^*IU$3v zH|W-Q#Sdq*_I_^ysSZiYFnm_y(@1@3!&h3cpX3J;O(@SBJ4$C!IlwL z-OSl{f1z?2(7>}IIPv>eB2;}Nxa8GZTh9U`&U11i8s<2#7^C=J7tub`^k=eLK#pZk zSR|SWE56h-;dpEW4F#@a&WIcKP^n@BHd>^GLIJ%z6s||l-VYcezTi>F^iIeZ-i{-7 z!vU6ka$zGUvG64<@8fFp6|jMZgMu(V$}f{*GENkLLF^jq?2*4j{uDR_zM#$J=$3Fc z54T=x^S8eF+r9fRy~>rt-r>e3AR;sYH%sODU=nPjOofU2}(STTRK>PLNx3c=-rJWAT{-SymvOY z0{@$K9viauS$6p1E(NO-$F2;U>YAZXHNbIo^4=wGGfXH?riWx+r&`u=gv zu|%7yu248PpB$gsQIwo^vXGmvh2Zp65uKGPKU`MY;3B&%Lzm$lqLJ`dhmt^UsdAfx2J#!VCwo!fMiKj?xEuM)*QeTUY;z zvuw{ARhV6_!tqYG?i!RY8) zI)~y5XaY&L)(_VnG|&^71#!{)>&ffkZ!{U4!McUy78Kp52F1=r19vfTEKJw|#COf_BDokxM@%pD`3V8Vk!_)#mnrQ|{j zfo-8HN#r>yI%&OL_Mf)w32l4l1pB6agCKC5+P-Xj-Uh%Zf&KzXMs9N@B4u7b%IE<_Nutyz13 zVFOxG$P0%*_fP0hRY`D4Z#Dp-Khtpg#`i^DyOQ)E$)5MSSr^J%i)%o!rlLp}doi5- zE%*@oFx|j&&?=p#Tm)u%T()5*1%JIK9Bj9EBE;`{QY>2isl=*t^+|5{{q~rs2Y;X- z<$lprNo7?olXOf#0*u9DQZ&Dn(0x$;bD zi(AvY1V0DFIalSD?^rOwUo}*l#giG8*9%SHQ-lc@sgd?-?iDt&-lorGR}wTIkPj-D z^38#L`HqQ1P*c^ErCg(E>k`EA4`<1*qfgs}VTU39icG_3H?w@d~8T zC-~~p5Wc03Du%+An-;>@STC}zoKwDMPi_#_^t@`5SC`m@CNZ8%rM1cO1L@1ue&b`Y z%IBV4G=(A5vWT!Vuh}>@PLloT%Jv@5!@$tw8d<01XH1?y56j=u=LugJ@Xrf1Bb~l# z1!hZJV+h8FsM@amUYART6`Uwu!U(44RL$n%*G7>E)u2eh@3KbspgIMi4X&9JYAt_G z3M{xJ>A;ZDuO#8I$$&5#fcpsWRES9?s4`jRY?|3FtN7beASb?;G2{zF8=sKf%5rhk zp{gO>_m-V}X+P}ar)<6J8>gLi8fM;Y^kce2xoOcVS6w(#0&*RM zXuxzmi<=Lob6)&)h3(l;X$>dG=UJ=l?qE5t^<8SGT1W!g5AmJU7!BQDozL@{Q#t1TGdq+AlqgxFgz-rglvGmnkE9in1oIq$+z zOO_3ETTfY^9D8j8r&lg`m(Cs7Im_3{F=-n+U7-Ww2gO%m#Y z*(W>!E}oRwhg*31%d?UFU>h#5qX@EaRe8HL8C?_Ec%L|s!9A;2m)E3lf~)c6@m%xp z1tgqe5In#sgUiN+W87>pyKhLl8=spGObr?Rj%PG&t3^gliF1D9uzJFR+;N)PlFwdq zMLGPM@|*765KA+^-;;yWm+wzw>o6oXdUZ31XZsW?X}qdM8aC6aCTVf8u;wd&-O@|g z65K%9=3dWarL#hFSDL?nzigE+r2z6fiEsLrnKjLya|-kp-WBOIvD^%5dLi z9hdoiN?vROKcx$&_PT9;FXk6p;Bp~BmOEebEem-jEt=@90Z+c2DcnYS5;bbvMz~2;S!L9vG%2nn-(#k%*pQM=45_E8frov!&(g%rs#*

asCi zX=|ozUjlgCo;E}6sbi^seg}Lb$=ReyO+JpWTXXFyo?YNMBrGE8$t;TIs=m~)>6~`s zJ=Gse%7ysId5){_zjtC^BFeSt8n~mQ3fF;lzJn`&woph6N6c7{S2OlOPvPE79~g%K zq_~lhQwb&ZteSM;rh3H(g8QD=FwL-iCyPwJO%yI)NdNM@$o)I1M2#r^a*#?_s`PAj z&XO3C(|)(fioGvqc?H|w_;vymU;>g4N6p`;ujgga< zpKk(9hq?sb7M8M>^UAK`Fw0O?p=~P>6*l4X-t$Sz4BH`s(~G5jEe|k~ zoNsXLsq=oq_P?ybv<{=Wzgy zndyhKiGFueSsbonsN~|qyA=s6#~qGA^aocfq|C}C_P5Iga2G5vsVkf$tGo0@dEdSTUAw~HuugqGTj(_a=ckaXeIGGTcl;>?Ii0(2MATr3*wme z?~~~EBK?c?&E9pp{2pKV9H^o91)lWe?Z-h%S(0-!x8QK=A79Dw-ksluY$<*1g$%po zqQ>?J!TPf`x1KlaJ73Er?$gS_J}njgcD?v{2FI}mJD%b|wrOzLOfEOBaV%|j-YBve z&A1V?i}Xy(>tCXS`_I$ky2DMg|L>cLa_j4zxfhWl!gIDTlQl|-U+&(Dv4_9&StlFe zj5iJ5k^<(qPyf~zGu!@16#JUnfqolG)Qtx!!je*2nYZ5HKKseu^RQZq6L2Tx&Wga& zuC>Z6IifO|1A&F8UVdBTN~w|l5_`lXmP9pmqM8b6&Vwwf2@|hw33I=mZrcs^te0Ui z?-$A=6!;x2XhI1^^OkS12y98e_r=FY z@2mpL>>|YGCvwR7_s&>vQHAlIAzc5G%*{5Y(iN&(TPT_y0BRj`(v^%RBPmy|CBbE9 z{DzU4GZ@ph{X{Oz#-khw6XR4?CM3*U$-uu?=)NCXFi?ojFqA!!Q=I2Z_##hGeCJnx z4P?wy>q|<(M(+G)IO$n2%|Rcbbh85PUXkT=f1FbDG>+PgAI9?Rn)fHz`K6Z5;kp;u zuDUy^0jV)jd>LhlERI-;mIl(C2Fbm(Xf36Iv4Xd|j||yRZiWev!IM8Hdwdps zpmt4Hw2})W(dRV7=_w^k=sx!@FICsRIQk6Y=FmdT^i;O?} z89RF4cE!Q#Ry=+@EY;YmM`X;agw>ICqb%>`ljVLlD(<^WPuxVS^U*j9@zUuIWqyr{ z=_02`N;_=UQRhnrHuVwsY;D=Yu`GFs|=zU-gX8}sMnZI3}53HVg(T@TD9=jP^>4G+| z8#mF14Z6^99WQ=>$lP=+200Wb-RYjD+ji{_x!`jbANkAcJw;z z{fwGNyGI8-i-|QDmX9`o-Lug%N*<;LG*6COG>`s~gv2fUTNC3u84YN8XEMDBi}y^|8+i&{RGzB-nZfH;-s)l00UDtG7Ps(6|0-XIgjbO$MLa=Vwv z9sQ*uC781*%LynAYA55I6r(3cP=~=MjuF9QGP#1Uj z-kXgiWbD1WIlPJzME36Z3kB)cV+k(eT-tDL=3Np(9*RAO`V#zk4SRA=c5bMg<;IE& zt5E+&4$DpdzROP0tXrYb{KB}_*L{cswxwVHb&K2y-ng^Q6mD*z4{#eB+SfVqBIaja z^1E38s-3U~wm30)vdYux^~+;%l@Bfd3FaYJDiGgG*s^s%;aS>~_IRMaPVkw&~P0d_a~r;YAN9 zAJY7TqkFtN6VtdQdw?~6Q^Bkz?-xq^eRvg=b$)pjE3C0N)DA)z#No-qR1A0U+twz^ zLS}voNUtTUIJqshvh2m(fDU&;#1v~vG05%bSWZ9TOYizhvA3B%RGL!r?EBj?5lg7i z0&G*%rMVgoOT2QOfN$1$#eVYY13Vu;FslR8DE)N?q;}%COzU!?CX{NR15AJx>uL z)VXgY6Q4u>UC-nMt@Z0LTRpa1mB?YRySS7VPFrHYMPTGVytFw8@w9cg#6Q$HR1sPt8fmCDszfF2IiG}8(uEeo&#ob z3swQN=3lw6p5Re+rnbFY0lqRp&BE+l?j9FS^@Dta8=I>EzgI^$zbUBk2K*`?n7(r$ zk+wQEVa9D`A@z}m$UfSX)~)gu0z8P8$~fzivU{!r%aY*W`9DER|Hp^9wL$am*T|M1k0rdZBh{wt7)Ymu^+)8p%MBCB_Zdu?|J`!7;j zfotsuW4~w~b3>}LfB%Q07{YwXr&}67rLr2Dtn#fDK_tS|-#<*f{b6c*}#X5YIkot;cWT6c zoJqVCjemK`@pwcnytTQ4u~FJIm%SURF~0eemcnY4$JEPCu729AEMBy^e5G*n@BFg? zA^kkm-%%Ftg1*sDHTxv8l56wEIS*#v(>y6W^XgB?36JCcOB~&3TqXV0w_t_0=)5k# z-MNv>@5G7?FW&6-%Gt{r~tP-lb zj1)|B7q-v%a)V7k6(8=zxjmY_$>QTN+<&XkWJTFeMw@O(neS#Xp`L3jI~QTMPZaTX z`c$!Ouk^bB;}8Gf9|sat_nF<^p^8n^bme^LYJPLtr81@c6O>N}N-p`l!8AfSPP~y# z!&!P1=Fn_c>qTP&o}46Y{jIaFuCDJPMg4KU`xR2JVp>0$)VOn}G68Qf%w&ekP&UA= zpK8F>k$nWNHaO)yZmBetk{gywN-vU6#Eaz0%?8=jBrOHP{af<;O^7KS@V1qp^Hnaug%p_7i+p6M|eC3@le<_j9Wf%dd)jz!o6-PTmvg?jU`o<&I$C zy0clETP_3Qe|mD8c2I=TJ>LB79+x~p{pUNIAbcgm*0d^fWrjypi-|jEzT6bQTKdn{ zytfB+!9kt(RpFTew52-Zh%-9)A1|aR1*unfNS9G2`^}6`DunTV9tgLt=vhZwdj79p z?=w_b`(;?+DswK2Z_hQtUlVlJ{UYl}pskd#n1&5vOBm&2@Z)Ddl*6W}y2sLwg~b^E}+k-2eV~j19^+ zqED2c_Fb0M?w5OX28|FNr&Asdi+x{`E}77#%PWk=s`~MOk~vG$5L`)(a%{GUq_RTVSu$ z_h517ouk5d!*k|D29E<;!2-7;WueamV&Hx znytOD*Zer1gH`cE=>1x9 zZQ=RgXfgDUf_@XnYUB-82d|CVPqxXwMH>qJV}Ae>caU~W9La3&VML|hRlHHnVj~wm z{K6*2v2Uo$@dRWtGWY_I9ERpYN9>KZr*4^>7wzx^{kkMFbgcq4o(_bPRZ8%bi8M0L zq8+s;O@Cqai`OvR^Cmx`Ib9lgx1^jO&I4%SvUt;n1O4;l$HXL??Rf^L0?JF9mEx#x z6VMcEv2F$3-bwD39(ba2+6!6(uPZ?8vZ$bN)O_Vts|}ZG^lq!JKru1Oo*yU8_t8e- z3UH9xGPzIc@8X4s+Q50*$sXd>*M4z`@ZDgxXkK4hi(CqS(=&gQb?hT}9U0i}so)WHUG|D&=|s^3HiZ z`S3tK`2T!BacL1icBt^_p0T|NI+hYPuoSu?8my6u{(@Bv#&ffD{$mxF7LFq&Ps>!vUYoJerG9CM!3_NMckZ|rnzW${ zp<^+4Nm{m^1mEJ7C)f9RiJfU}$YCx^-(A+ieS&Z6$}?6QjALE}+{W&SL?q>k45Y zT`Zk=%#hzrF(Kv+;C5g^Np)4&CaUf{iI=DG-^-sj`2mcLlC93#0$YGIL1#yB&+;4E z4vCwv+siqFImK9gy&o&z?)*1iSn2OT0#XHs?g#zvK$LBKBKXo7k@k-3uOs6oRPEGc zIzSuG$3^@?-a{S!{iWVlVHgfV>L#LihgtSd2!rfcnl9M~ZTpQ(lW4BS!F`U!0onC=axh`a4{mOW<`C^#WH_iv-@$XsW0eNf- zNz*D%Cy9=2$@Vi?C*&mvq8ChUI%@}*I(duUC5`3D1Gfkd7Q!QRHbz}0>P=Vqj%tUm zu`*m$_Bo_~x;AI)zir1So(VfQ@EbT6wPElx&wtMJ&Lbdurax>iJQqsK;;(v6m*E^5 zxtCv+5~ejzf?^5RAme$;;e)S`rDuuMvETK+ShLbpGG4w(H$kl}Y z97TA#FFKqEZB>%92)^yzRfCw%)stdl2pM*0VkckxTZWm7%<#`tZm-dV@&_U{lw{cb zE!vRI{|5j7|NkUd$Cj%~4E!J#q$Vv*#^h*VM-C>L3}644`xbMWV}x32sY_L(b@|(% znqoC82_*_O9bU|@XryHTKJ^_-jsJO)IER@+TwgN_*$>ds`h8Wv;*+drW%hzRl4!j$FqapnG(&v<0kWR5Vk7>$4;cjB>oI=3|F4hp% zr0-o448;~GUeW;k@)awWljHl>a3lWnr=Sde565gR_ZY&6LNzi9xd%h~+0t4jeZT1{ znVQ5h&BXX2L(wXwoiOidB!NfL-B_mrTq{3G| zXu&4sU_vRX6nalw6NodIqT*H}gV6aP>Nx}B>Z2`@k(UO4OoXOmMBO(tR9E<{jtK~5 zl~eHWfo6)Z2Bu5+%@QEr!#2=z%)BxrSq!p?n)lgWEv$vo%~{R|Q3!`p=N1bfLi-ic z(#r4Bt#BNm?Y60d0bpFC=GsAlM%CJn4q#o0B1)3QC~soe+l1JmR~fq{sVLBfB*|M@ z6JAKkLwtus3%ltl;a~V2lJL8c;G%?rI;Zg(veGcf#d$~kQX4QI=DXsHa|}N6`)==_ zl4X7ow=ZjYgK&lIjHDx^WtkgD<%>pvwe~NH++EP_d&SZa6|Nu5qWkM*&8_4Yjx)FB zIoKWE$2=c8`u*Nu4IM)6&~ZS<_uTANjAGhsF% zjy91G30w3ghYtf8=yT3<(3au*jG>6auB2ad5MIJU$M)WsXd&NINN7iyeA$pBEX`^! zYoUTquV?Dg#L01pAw*4TS-WEOgP`LE*?=}jnWL|=m7e&aH{I~baf;o}iY^~DXx+x` z5yrP|%9=$|mmF{+V=0V3yG9fr6maizr~`;!KmzU=VBs|Ll3-rf*T_7LG2%{j!~DYg)_GLGqcVQUk~8ldVXZpoB)y3*nqm%V)wPj!P)?Ja=%1{*kZqeaY71>7jKKy=nS)4%6D$)^l z$~u>XoASCBR+YsD*6VXT2ku2jy2|1W+&{~492qdsMXUgQ=^V?R3pk9{bJ)jL<}3H5 z%f%~x0h`~|P6vmz2O4{VP7!$Lv8}|`2-qWP3gI~)dX&u<6CR^n7*KSKVsBTZF15|e zWRQ;U_$~2Tdk+kisQ4WY6mV!8DZZ%VZaMKqEN=E(+ZpF8)s9R{7CY16%)h;B!eV` zXK`YVz3Pw`lTGX=KNAZ6?053tB7MB(n4=fJm1^~6cgHNZh%@pJuaY!|%xs>}>;v}N zRPgFXU23MIEsa-fOc1RPy8(d-9(Z8m#;t7--v{BL4)-JJ`T4dB^W{^zL5voG8SjIl ziKT8d%5lr;n=Yu~w6h|BysEMGHh8`j(NL_+w2vsp!Pt61_ zx35Y*`6ePf9N%UCZk=*4r*dI|y04p`zgwknsFvVlfx`J;YFnXm{N!~HICt2TWQWu5 z!S7rZFW<5$`;}=V)~8VX#i6&ol|H0QM>1}#PtUmtc|L#G%asiK+w^?vtN@es!eC8{ zsFsQICDCAXJn^W}K4a9aT*N^gGhu-yZZjC|eUtSldLaTUzw&0n31o<1ghc#o7>QS0 zdttw0Af79JN*gAZi%Qh8vOID%3s;W+y=rK=gwk|)f>bxffeD1~#tT=h7nE7oiHy^^ z_boE;uq7dd*j;>GEhdo-_xHmyfGn*eD!;#dzCHrUyfUtap~kToCeP`C?9~UI=@786 znllq#*B91SAkEf_G#fk$BVck=zq4?Ol}wV(w^iQTP-o4qyYU3$6X#_VFRb6+r-Yr6 z4zPSsL1=ihIJA`6$jlHkBuZFum~GRI4;UWhB%^H-ZKA2$wexPR!a)&Wq^$(84e%lI zU0Y$D8#7qrHrI#mou+|xznvSM{k@h?lmnD(sP{&yE&zB)i?A|LKbv+Q7MnIn@aV`amK<-W~FWWCk(w z;F-mKUj!vf`8xE`@0HUEdR6fsT~msY&;u~;;@Pbz^Iup}U=Pfz%HDgi_fvHH8wQ|T zbjTAv7jecZ8(0OA=dm@T)PTslXkZV_4a03UYoJr|7h+WHf$#Xhh)ivHw+y#rw2IV| z$01Zg(+W3AaY%-ggkmUKyMQm|k0F|XVQ18X6lU*scdZRDL9R6BL`n;W{Vd0pd(DJ* z!zErFQIR!{o;yNy{+lU?nS1u>kA%4(`h<3G(~i5;qz`japC;qd6!G@%bM%mk6_@Pc z8knKm{{n%W{B8n;>jHz!qm`ZEXh7AqoN{wkT-g$>;K#`;$wDImRRb#XxHWP6K+4}w zxWC@!r#w&(S07%qz|T$ISJ9L?c+JP{qf$Eu?;@LUHqFV9`2bZw*k7jpsHE#mslS}(EZq; zC{7pVj0iR=hi}{jNeqsXVyUIZw5rlv$YQgox=PbE?Q+Y(K22Jc z0-?)ZT_L(xf%5PfdwzzrE_iHvv#K{>av^M+8wu@dwZk(6vI3L%7ISS&wx-RFV92f| zV^!!pQEBOHh8OS1gu5Q6^X5hQ*(yZUDTVT6wi^i8j&Am=m$*du*-H+;W0>F6c zzUX7lpVW_#us5ph_EPFi4BC6&#~-|(Y@j^}WWu0DsytW#?vF)4)3^W7 zx8-9YRwBR31= z6+?sCHVwbE17=EIs^u9x$ieJ-U)5lQz93{w#^B5k_U|7W=VrSq)3It+^~f1C zdxddjR}f4=lgkgs!+O;aJh~@{A||M(1V%=MekSB-pP=-uLK~K;L`tPeIc+EGF!uC= z-k3&O4Qk+!ewH3RcsaE88rWUKz;EsR^vLezj7uAT9?6=Z;$VVCGC>)_M%n_t2@0Sp z2SHD9)*pI?0Z9-!oi4?NuE((B&T?a3DtRik6)@dh1D!KDu>yRT%dyePC%zeA$zLM4wo$y^+l zytD%|_*vQcj12>i*S@ctj7*V7Dsq(>B;Q*IRTY+ zZqqs((0((#o*UD-nI*emyS?NC)$3ug>YpnWHl(e0!3Xi=zycP28Kty>}rJ(p?(oR~U*cjMAKQT!#xCQi7fQOIL14i8H9L@r@=4Iol2^JLI9W zR+7{>ViFop-lf2tBQ9E57?u*gD>jo$dswEBL9Rgv z?-rsEB~mo^QLY!ZdnE#$G>aGom_;fHQ~x?Q3n#24t)4v|egKWX0!*dT;o8u=={wob z>28EQ$!bCY=~jsyp=2pk@v4m2@s?nc^-!NPrQfDSUd}70^N`-?RxBBc)^g7GX@dUH zeDbj%_l$kW*~JtS^`RB>$&&?TfpT80hBd zvW((-cO@dmDSppzAp)lM-aZysU+;Xp;i8hc311@GH5YZupOFniD&6J|T&L>aT>cQ5 z_f4+=Hp^;G9hp^$ciFXyw@w}_kiC_Cb^xQ{RhDLPLo(p#yqXz=vnW?7YpsS}yIlU9 zL;I8x)%5f$$iDDB^qkqjZcu3^GJSu=$MVA?Uq66pNq=-1{*Y}YClnLUyEOcFeNvmyoUbWK8<(_2pjW%WX=^v1p2cA~=W1 z;mm*LfaN_HZ<6VbBZJ2v&R?H*WVq1N2Ic=|MCmYY{DVd)K~+XWWbd=<4|#|U#-Y#o z)_o8A_nTL^%dd#Ydj(;$7hn3C6Rlc!dDZj$MtvNbX@k-LLq$49F?*JWaEh08`KHE| zq|)%-(ccd?;MgQ<_>Gn!7>(R?D70M|nsSFty&%kBdfa;U0k}m(IGhAl7G#4UmrUiB zOg9%lcV5(l>dM5Gp8h~B6}tbv&5P@)i;VkAUbrVlT+k1Mq-#p1Kt`iCL*9*8Gh*Mv zwhk`ryaG61_T?(WkMl+E*hepF1suMVN8KJM)l}qOd#G4HLTyz`1vXKX(r_P?+ZaL6 zBki#enseuonIn@gBHj@TP_biz+T*WTa=D%{(Lfme83@Pp_#>=;{>b8=KiLdW%Gmwo z!+0k}81i$+7zb)$*7V_PrBe|#9qav}W5|1CL?NVo4@q+9RnntCURu-(>w^t5MW(l< zAM!^pATP@dEYJ&E>p6Xiu)!*CXs)q75@)W1Uq%+dciXvGKsV=e$m3#o2LYNQeQ-~( z^tVv}R;9~}Rhj+psyL+;F3m++cACTVm{kY zc+Rd3u`Gh_na7MQK37ye4n~CRhD~5o+Wdj&c7amOV=}wbBfd^G|2?m_3bxYg9^{Di z=Sn4pQ#Z^P;)PLx%2wA&mG^XLCTri{aW=PBPbYd7HitmAo*-%i3i2 z`|^$=N+BlcWZ_d*{CQWVqMY~ZGxr>n{H_-k3ND&OHqh`HQ_HCw2i0IO9*VeVI3B8r z6vz7@4)GdJQr2mFJN6^N#{1If_I@M{t%pi>(KXNyo~CH_;~yoWZXy*|@I&Tb#0;+4 zbi3rF&_I-h8)b%N6OZ&rD^nijL%8PKg6!|8oTYUb-!a1=AM!uD^B$-V#&DAN-Po4o z=oV^uZ5!#9^S>F?RXrn#p7;z>!#-Q$E0%`OL~^A+x=Y2~4Iw2l1e6oC{s0@319lHI!qIy!n66EQtf z*5l;5KP^~~>MJFP4m)_=dlae8-p{+C>TR7gUi=q-ZC*yc#oJ+T#}#VEyKbvy=M+T! z*?YW%adzf03mCf8F3eE`bRVENAbuOu?-W>dw-+h*qq}am;3wz#Yf~&uNfx;iM4bZF z)bG=?4h@j0^@o#&e)9p(oHzxInSv1z+tRS#XEJ4Mg0P-um&dv|_qRH==lQ-D=YV#J z4xlGtw>ktOp#+;{clS7i;!`^L1wdhhAE-W0!SDWo#T+q=t$dKXwA{_}B1phHn-TAKSucUpMs$?j0%P3nm(SIX97x^)QfoRVVgq0*4+Lh^VP9GrDizNp_s^ zoPu(!iNz}c7HH%lg;Ombx6;DB|0x@@i1X!j3x{mAuk3k0f%lg;Tl+;AhxWzI#>vRj zI>oXCr!l?ZclRr6XU(P9jSk%O2KMP8pqT;=^T4xSd+{#m7OLI zep&7UeV>5GAH}{YZ3CZH1FR70SxAd>DHz$i~C3Mf_;I7H}8aOy2vRT((N`#7GDM=Rm}X z_`itb5%zRK4JC%_qu^G=^hB3bVEAH9mkX7C+IQd;28y1_QPRQqA!+@|B9T@`LGHT; zalWR*EgMWi_r_sC0f}37-k-=*3{@8G%$IyWXRZbUgsjUcW%kP2=nl4n9lUAH6jQB3 z7j?*&6?c3bw_6|Y?rNt(F!hR zF9qVw)@E=6onEpzdsH5xfM6r)mZ+zdCp%ZT8GkGoS|@g49f;=w8NPScC^g${;46C| z!Zr%rGJof32yD!%x{qHMHgSuv+QcOZj!W)w?c3I(b*if5Z9gxT^zjsBX7$BDvpo-U zupT^`n6a7{AOPG9yqqkh#F$b>m&9tlzD+1Oxf`Ic6jez5`HD(I%n>oCCG%Ihi`(f% zvNLM*?35++I&GaI*8XdYWW~{ETXcl`84LFt3(-9ZwA(&_;wd>63{+FpYD2>c7c1@d zZa!|i3)k7$%=Lokm$ysOt87x;n(2zb-r3;|2K5~5J9csu+}7=!ee!6o;XI#lc)}h%vp4!;S0x3cuoTZgJ>d;(AEqx%wR&0YhHtLF=0I z2Ia9EKuT+X9z=<+?cB0F2v#3c1FVFzn>~_q;Ko9`Rq=w1qIxDNPkDM(q=h*~^eEpb zeDdZ12wnhUi4u|63bKvnAvR4_aawYU306tj=(@ar`N4s|=2eiK63-RCW#X_W#ozwd zXcm;t&5nuysuw8id07c?zik}2`|G}7A2h;e=Zls=#>6dgQw2z5Y`ybnu5p}e5G3fb zVvegZtMa~?U{UtNxtK@ojxkoS%P%ge-Oer{>#0JX3!(cANs|5QAiaI!YV>e(?gm`c z+G9I@e%H*+qn_&tvvSn%(?_Ex5@5#Sjxn;|nCsN74}4_#{lW&ihi+esB+4oM5O&^b zne5ijh^a4GZ*84082*J8tDO;tb--nbIqF9(UDbF))C&vSzcjv%P{i1i0SsR(ZEJHB|sbjB@+{^5H?U>qw}T2Tl3KJ`>t9z~XTW#`ny_*ct)v zjK=6G#xulxVSM*luzg{BOc3=0RnJV6IDk6u1=g`XZsW~n{WX?@wdi`7=N?ZWphJ492RN41yG=%9mnG4QT3(7#PI!o_mtHUDjZ6fft~5>O~Ost zeu}%qy|Io~K87?vEk~uUacf3wvwVQSgZ2&3ChtCV{TzS75!dl4Vy2CK=bhLyg;RME;dpB4oj;(!>`Hr+?e@ zbFEH!dsv_KR7gx~zjL;uvFl-s8Z?%`gRel*SW8kXQwassw6D7JSP)npK}7|5KR9X;!l7LDXs2zEp5g-$I+t+_frW{PV#N}F4zdf`l^FTG0y62+}yr!*+W+@ z0mZ!dVaNHih~8HZ+;+N==DoGxc{>nj&!gB-=0=!gZ+jEoa3%-15|HRyCD6nEG-Xs z4*v5Oz@7uPegXEf%fsl;sjzPxofUyA=KO*Cg7K6f3WmMyerokWsWX)&zt#6V)4O+9NW=;P7N>z6?;GP1|rJ$yxC(pV{qv9$IS_CKm z9k(Ufw7k^BN5#h>j91>7OoN2wD<-X4dtkp<%EvELFQYtxHtvqcaJ3R04VJo zyEPH382~Em4l{4cm0rLjUqurJ3I}Ts^eke|=Tk6L-d4f`C01KdLdzH^yINdv!%G^3 z^re7fi;Vk2PiQ}gY1Im%u=-RL)fOQwMsUcTc>U&ZEOnN7p&o=y>$E_oX2jCzlQcR2 zD&Ewy!@b8-smVSp_(I|PB9f#)O70|6rzDG4~3T5-IZsC|x z<9g*IR-}EU^Ig(uE;9_y$u&)JFH7dp)=0`HUE1-9|q9i2W!E~tYsao!y zyo2o?vthn21C?Hj%V|Y5D^bNf@0OS}`l>kI8-WpvB`HW>DhrulcDmbzfq1EojLWd2 zO62Qfl23qv3njP&%-!Nk2UUpFkC0u`(ju|=#(K=_By!^v!+p)*PcV5hhf z_vw&7F1!vMbt35H`b4sSfCX4cZ3G$W$PtsfQ!sqcyd@ix{PBT?lXzmJcy89UP>>b8}migiz;+phS5EskTErj02R8@Yix@@bo zOy@UL_kkQSl#@cICjr8oSi@oq)=@!EUfL6DOUtLc~r>iVJyIXvWJL@Q53~82V zvNSICh?W<+1&%A6D$II~Z?r{eDmJRivZCW8@HXVH9EinN% z2ul~7r5NL+1B9iLNtGuA=98p&$kK9{WEz;r?8FwQ) z(QU<#)}R%^gFw_dxyag+I3LZ|y|g}ki`2!%4M+l7e276)DWem=Jj@q;w>kW|)K9IV z@Qfyddjp103oHjwH5;p#+h~q_sOl(qQkY4(z>^th-7!G1430j|SU`1gs=<3RfzAqX zKPxYKKsz}Qq?w7=X-P9Qn&%kt%J?oQPw*<696J(I4+0?ZE4D?4;K;4X#uXL7%5mZz zVu^^Qd#>R{fGTKcQCqy8jTgu~Pab0v%W7GyQAVR`Neb&HcQc8f0sJH$?nho$dZa6ZgG#(yv4CjWQlR5b=Y8^6m;5&3uPP{V9r_c!Op#kzR$}+i6l53#J zkYLwR>){x;E}C+!MO!F?7z3Zyp#MJa(v4I_P6#@&O7Eh4r8`Qke0Nn+SrKVmgF05s zjG%X9f9*0~>N1Y2X?jDfU3zQesGY@7c>7dK;iQDjzQd zXC6SXjxlzi!_0x7z=>vc{Q!`4ko`3EB;!=)y%wMw2W&oE%}L(H2;NbGMP<&^3W5|^ zQuE;)K8TN3GNRc6O)sp?6wsQ@17A;Fhs!6B zWTD)>X2#lCZ^cd&!TEZW$^o~PmI5L3T2@Kj#`K+?vF|@IqSmvI_2%W>>kZd>f|14( z+lGG`?jCdq6(D0%LZRQg5c*NDPdGb~xC5IMY}fq*%@w?}APe8o+&pAxk}woyXDweE zq{^fR>0nq>&o9y0l?<0h^ElsLZSD7l9TH;Q+R>^66D7t*)K%_*I81#O?6^0nYWVCn zmx4--)`*fqoyV&b6P}zEFEgi4E7fa#?@q1HbPFT|93umjglK0Uo2ni{4=5fl&1#3s zs(th2>`s{(Y<~AEEhfs|2@DkLvE{(%df$riqtvGNy~7bm*Ck%uGN{XAzxx=lJ388+ ze5-t!lPZAzgw+V|>+?FrCkXJgDr@-m?d`tDx4-+3AOGFo{o#N0cYplt+3(N44EAzv@82`uL?tlD2(SM&W{~!PV%m01<|2Mzgf3G@c Oy1#$td;Av=jeQ+Cy)YL5 diff --git a/EKF/matlab/scripts/Inertial Nav EKF/StatePrediction.mat b/EKF/matlab/scripts/Inertial Nav EKF/StatePrediction.mat deleted file mode 100644 index abf15241a0fa024db2085fd88adab239999c1aa3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 106832 zcmb511y~i`+Wu+SLyLka0uqvfbVvv$k}8PkCPZ2#q`O-{um#0JOhi!GinIzC*nw{P z#Y93RbQAwsGw1a?e*aHc>%Fe`I)}sAzkAJ^wVt`>-ZKxDCbkwPrjlAJ%OovLY?pd? z?{Zg`wAr;i$ir{XZhicJTWO}PsVup2kNfr@cQ;8tUwz37~+ruR#)g;w4^ffj0 zm+48WFH_T&g#E|tI}`a2vzaA7{@>Q%|ArOx>~L{8&*Z8t`GLvRgoDe)&BbG#*=45Z z$~@IAQ+1Lt0q!cS5;iTYW`?S^hDryXaJ=TOT>tE{+AW@gNU3dmhf~H|G$W zdHNJCK^`yPIX#h0l*LJ z0W$+1vO)uWabQC^d)drDb9QK;I1a2`!G3WTATu=32?y5n&IW9PKV%~uSQp5Cb>6 zfL~4lU-~ac5+Vi&s#azUEUWT(DjML)(#3p4F$!t!N z6^K1^RIM0Ont9ki60pRyZ4E zC63#S?=~Dpv+%6hA$iQewtV2w2E4!RpY2VS7CB7MfQ6jhXROBe4|_OA;H*G4NT4hL zc5?*J3S@xv0%WWTrkR*G%{^wsc1>T@O)JC2954s?hg{^&iKikQW=a2yBFzn5=$ zGiJDQ+aiFM+pn1%hJg3iMYPV$vIX{>OXKGpo5zte^GsIK^ciu`GpT`Jua(PwR z$1s3e;s25u)qmfeC4Ju{HvBw#lTfCp=$8L=d-gXs?a5ojTy&a?XGUuM#=s~vDr*t* ziWkt+-?=zv8*2d?6|bNdo^pu->R$_qyt1V-Y$1un3(0(Tjv4^` z>$>MMdjM%vTY#}_8{&`$yp6w4{HH%|%dI4>0v?(3D=02)j>{}xHm=X$)KFihG1vZs z`XKW?gMB}SO|)A z1F7xugYZBd9LVj*Q3MX$_V!_G%Z(jQ7?D4}SbvbSM$}y1E~CC?PaX;@0SIuIJ#v;$JAt zO>y}Br6MO0>;8RVwOCq=Jw#bp#rVR?6XUD{Q#&Iz^f3|^kASidbKaUA+l++8L!fMZ z2y9Lh5*7~u4~}v+%{tngM@U#a20XNb^TF)c2S`{v2t4$Z^AW)QeW$6Ym?@j_np>(g zt0utjv_<%C@U^5=0Jh@qU0g~O^SI}dwOFr))bfyqYv5x5{lVLC`5$4uKlVCr$j zdcko1?=QaCOm&t$uJ8QXC-JDM_K^+s=)+phxx*_qKH;`|g7ucikE~e6df-1X+AtoY zr4F-i0JOhdN?o}^;4^boyyl%xWj{CLYzfrJ*U&XoWp5-<`q{$oAG~12l(eo_@9wx z>tv{AaQ@}O`hQ#lMDUgAQb{=vOV384^W*gGiWK5<2s8F{B)SBkAEgjR28N!6LRVqJ z>8@Oq64;r^M52QmS=UfXDJ(q)iOvP+A1K5ZZ4fh4k%mN{kJES9QLaOdy<#sCoej|M zQf>nJ-z(q0J2P8H6AT`*@=@jS%C~2n(g_U*V~2&$$KinyVc`3D*f zb}B5K4TlGrQa(V#!A6CJ%K&&O{D3yTpS)OPWb{22ip`Dt_k3KDSgmzuuEa# zJOKWS(hm#2j2MTeY1v7Fa`=Y`NE~9po25?4U=t%ZO1>6+4-lg1wrK8ywR&W{B zpftkL(d>&5pr58R!_v|03pY-86QmGdML^6uqJ3Ei=y83aA3o6e6#Jf}<2iaRASyFrY_{QXGu{e3~)|4M!=C%%Fblrc6V_QHtYY0Dq4$ zL&H(*iy#gUF~eA);VAY+4Ztffc4#2Fya|rm^oKR*`cxkeiP$@9y!Vmorl9iWH1aG zjk~2{7v(3*c_n!CpeLFG`@qSYdO(f}E{>5pa~^ zxDdeaU~{42D8+F(C^3qdFf<&cID$An0H3X( zr=!{zUkpE^J06q45eyy4zW6HQbTZlyPq z`vMB_HU-SQAlVmR4t(V9>Ew+%3?0e7_$~qTaq>`)&7|AZNVkwCa zhK^=mmg01GPq^d=j2+FsECTe`7$5A+fLVt(M}|1vV>KoK=(CxlEJSmpy~3Nx<2rex z4m0a$_C*>Wxu-OFrw&6$voAt`o=4uQL(mcJiv~{jVkYm^Vd!Y~g&WZQ;cnK!dPK4> zn?e0Li>-nwF$5f?IBJ6`Fb7)$4M(vrp!Y&p9JUS`j#3;!ABM1gY&|p_r8t6~4B=GF z0ve8DUqF9`@CO+2jP-0~oj9Q=_6786h|tAsphu2kUqIi6h;tb6Ml;OFk?afT;Sf0& zvxOZw%9lrU(9a<<0ox1JW4^`_d1dX!KSl@6}=GNcN>4Jksc$4|lT;)+3UAaRBw}3FZh> z1qe7wanu7P#t?IchNIY*Z~(uAZHI=V6i0SYzeKSe&~TLE2%1quq+;&Sa1{Fj8dF4! zV4l!$6#D|2R7CE;yrJPJ_60PohCQ3Ab`KdBB0?Y z#gQA-uT@wSG#sTknge(l76T1Ou`fzEJW3LagNCEnmz@BfjU_#(qQ38_62nCh`5HuL&H%<&WXb#C9p(jILZ!P3*cE;GBg}z zhspss151U5qwG*m93JJ3r33gZ1-&8-*}f2QdN>&z!O+p{3wWB*>l0LRM2sEHzJSLW zz3njv*qK4IFW`Ab@B7#`SUQ@00S`3#Xpy(-F!PRPU%(TMK4-~$br?EY{sJCp+$luf ztj}VO@Ux267$63J=4fXEd9MyJaEen6drPaQNa_CMKPPzwQx2IW*HgG9ueZkkf)t9L1yw`ahH-&~ya< zA|?dr!5b;Zpy>z(MofdC*HQ|g=?D%+j2qBHR46B4=_nRPOo^ZuP)72 zD5qiRC?-a1IiLsjQ1YSaC}ZaY^q>ut{LNh{3Ys36e75T!6R9j?85->x_}Y@J9;E9lbu80 z(zV6Zd(yIB3HJRL{DX{?lH^cPr9l_QV@Bn(K1KB4IX1$}>ngV5l-blFxJ9d`wEvt{l8#a%6#+e9)68Aa{-C4G z`vQVq2el%}v7^kpKA`VWg<6p$9i>Kc5cK0vE0Uz6)W{`(9>@)~B1t+*jT9#6;qWVx zFf_0$Qk$TEq*TMMcOs*KrSk!LkR7FVCVI?&x=!M|i9>Mp(24+I4rkA4mo*$7DcujK z97ZP>@XJizv6&?PoQ?8-{W+T#?nzIX61S7R&>a493-Vud+%AD4`Tun>{cGD$aWMSE z7P222oB-Ib@B&6aAZa<-6AexPY*}~#W-60NB3p0u&&SLZysD3lJ^`UO<5mXr}yv7eLzwqCg;3 zn=%0{KzJSaBhUi^Nh0KnuTUqMusiSq?1VrP`SvTE0NPHLB?N}Z7hvH8(01}XAdu`t zz5@#0euhu%nj83prbgSr35{C<_2ni&{3SvB7**P z<_2ni&{3R^A)p63kPXyk@#6Re40oI;&WDwtH&UK~tqVm*aXw^VuSIzQO-FG)L|`9u zhVlxQj^upEz+RB@2AYm?heZVT!LgK9K%aHH{*H3sD;km6CuNeI)c$s2WB#>Pco2nT ztpa!8)+%Y7Y^4S_e6)H=#P-SVWIHuD0knEaB=^bB$(Cwx0%-M;2=7yj$+l{60%*>M z$nR4wW4!Q72+jEr5q_#D#t$!m=6v`;@kvd=1fc~GosTLZFpLSo3!ph42_TT>g3W^# zKy*IFKp@$Kd}kJF6`?sFDia}4LcTT&CxEn*sggh-Wg+?QESvz+PNoVF0%_#yvv2}P zJ6Y`Miecl~o4NPeA9MugLnSi(z~Y&Eul+$sa6VKr z)1NnU@3lYZ2+oH}WcopgWbd`VUBdpLBRC%_k?98wk?8;w9l`lfiA+D(nT!UY=m^e- zN@V)MkI7sBhK}NVsAQ(UoD2n^=m^e-N@V&W7a$XX-yMOWoe!0W^^+*+mNke%4zhL2V+iEuwP>cJgA+hGa3Z6ziTFM#BHs6^18R*ES?3!ph4Dv|Z4 zFT#}J1<*DD5%;HOU@Fi8XwHX9B>pKo$=7Y6RuQ7}AwvI@x8xhQZ~|z~hsga?SCcQ@ z!U>@5WFq=cEhFE&g%d#A$wc~}CNcBNx4*Bz|F#l_bv|VJzhmaMZGX^FoR13dRYA`n z^C~cQ6z4-k|G~yHw{81l>?qEM$o+$_kZs%kc7^$aj^ccX&_871%+bIfbQI@9B>o}k zWHbOZb`zXjRD4Nd^9WD-Gt>MgR5+uu$=%)izO@BfWttuDFX)+%iY*~krU_-OT# zNcq$Dlg-@V1kjuhk@BZAVa8BbUE&O*IUgeBPv4E1zzd)`A0p^ae}}Dv7C>}9MAo0N z7F!K3faZLNxIg1Mwia3d(fJUGf9gT zkFldTA0qA#TTJExFk?q@K19|ZmP3XDP;`{_P6Yj7laPr3(JC1Hi9C2?LInM36teZ( zto48pKr8xW&>up!e}fZ1E15*lpVmXRfP)i2Dw%Y)HC((*={95=xWAp@n18Jm-aQ=2 zTIDx!onlU}BKyF>4Ijz*&}G2zGZe8cP*+{z3?n%ox*H*I7~2XjfaHAWbAdo68%Bi| zKyyBHTS6cJbAlH@+5~h>LZBUUffhh>KJ*|UkXA#!kdUna6WV@pyPjpO%*IqC^~}kp&JnN{WCX<`(tJhoDV$;(D6URrU5;61m{C%A?SW& zGypX-2+oIYO3>TLTmXuW;C$#(fF5o>b13lN^N!+t=&l6)78we_j2-0(XZjpKkC1~* z1b(+LhIT&mjX)q>gX|hNYd!eg#u#2eoe(%lc8-G+Kr5MhfItQ>**y+U0Ig(Vgg^w@ zK@Ls;tz@nz1U`~opVep@TN_c)TZ+IA1OR++XK@h&Rd@X?%)4;X%C9To_6Stphd zn)6uz1oo<6!O#MT&SxtjkdF~>q(V&*@hTVmCeQ~0SzH+LPAZfDqVtIW0_mTzJWL^cvj^cc@33}Dcedhj{85HLe0_fpNWS=>xnL%+r zd<6X{84WG;wdO!%E-R>F@0uRWhb8rG^B{K#HWa^NO=img;N+vTQaF%R7 z2Pc45GEIQMUO}<}9h?AK?6a5x2MCTI#1TuxNOn3pbBcKohaab0- z0NOS-BLu#Yy}+P0fjZhYRsaHfx03z9-~`aNu{R;mNcIH#+b;WkCHS{(GURhUbWuPL z=bpJi-5+!m=i@-o!y)skf7>R*j$MzSf1J5R-5+B|aX#UI9$`nesDqk!6z9WE&>P5T z0E&*{d{zT`q#BtEK+#d0k1RniBtrohI+F8o2lOajG7*5Hqd5I}1U-_B17_W>zZ)mR z-;LSmyK#LIgfe}s9;En*TWCjC)z1uOOZyKBcTFK-k1X?hnbsC%i zTFJBo0$Iy3qI(*g0GjhzN(h|Chz4qK0%)<%ZXl374=Vzz=x>|gx3$9io+DYS$_Ct8 zWhY?8&}$UY`8X28AHaxiRZ#PTnE4n2fgCDEG^~OWKy*IQKwz&Sb{2Yy5HlZELf|5H z4qgCl8?OWcSt8hZcmcF+EKLX`VHe>A(6;doLSP8H3QuoDVla|3*dwP;?aM zV-Dz1+sIr1ijLxZln8n=84AGAk(|#?K#$fT69FhXit|}W(2L;W0AfXIqGdkYfWY2s zSQ$(`AO+BF7MBBoEOG2QyZ~Csj3fjyv77J$XeE=A5E#dZ?rO7Lw#X?$E17G7K(;$Z zG+2WZKyyBFguru*=&=STfaZKWfk2KiMl`j86F`f7LYFrfdfLGW+KiPq1jOiT!W=`C0ss)s%Phw|`=K`=gI(gb}kR1bTS zj$nMmgb4aCs2=tN9mV*FX#jeN7gP^>l8#_}#JCCiYqE#^tXC8;?`XzHObG}icT*lg zl>3rFoDjG}c?vIpR58Ukfk3JPd^F=DW(a$ftmfpyyDC zq!5aZ;Cu>X2>K)iDkdb(DuVMVm<#BEJ1HfwD-y-|6lfCkR=B3(VC*Q5`4sQ~deB<3 zsrYOajhGoU=To2z1QIuq6(3Fj$@vrzo;k4=uIQ6TfaH7%48Rd2Ev0nAR5Vfmtz@zg z0>{V}VsKN0Rx+i4Kr$!UMGQ^=$@vt_Aq0ZRMq+RRNY1A~oe=0Edx^mbAUU4`3<#uH z!8IEvPBNPFDNrN?D&d-qlLBb_U;z+Fm7lq(*uT~a7eKRCn>=xAl|-R@gx&;*&W8gG zKPiOr8D0Qw8!sUQdMIDv1<YF`VIdS9#rZH3^jj1n zDTJbEZ*;Swb1d^iRy5190kNEd1L>>ttlF^*cJVM|E*+>jd0L}Sm0fAI*vX>Z~0GjjRB?Q9Ac4ANhh|Xsz zA)8{wLb69QPtrco&73hLntK^dy3zTmF zg$Ob8A#(o|UW^T10Bsu+(SJ$=#sM#YwvCDOKjkCFHPZ?Botq%Ga4sOy|3D=&7@qYC z4CpA%hlu_IkCLe{6dlF+5V?O4Cm9Jt(NUZa5&8!OlX)-{9mV+&iGNVf%wg~!XBEZy z5OII-1~M0b8as;fA+rA98Zs1sp(8mTBIplU3fELz9bb`1&WA|(L-HZx0HXgcn)4x2 z{^Vk~uJ>U0sDHUj0uiC4&Bxez>mpqySRMq>@4Z zHnNM@te17-93VL#DjD=Qk&VRQ1dyB$l??i|$X;S_0!Yq>N@V?Mr{Ow{6XyWU`A~_t zKb@cKCw49aLvYvBS3RLR3h|Gal}NSuIykHp~XH_BKJ>ufGwQq%KqD0 zAva@p!>v`S4z>t>_-Nahi2hU0ViNEIXxo@b|I-Aq#WT&%?`tIJ7>bbjP|5V)j*P5k zy@~-kg7cvg(SJ|_WL^d62+oH}J}aX}8G@b}1m{B~LjT}G$}lt?!TC^$#6N_Wj0T|Q z9l`lfiMT%`lFS95=m^e-N@V>ZpUF@FhK}NVs6@~oY7f^`TpeFCD9(pUr2L`xAmaeA zBGJC>qLL~9Gq|qzVEBm6hf1dWMsQv4NdYA1L)9nl)-SCR*?F*pG<=R;)u>5t$#jT7ep z(fJT@e}*1hr*TpM>Dw+Uk@#nvBb$mrO%bB=AwvJuR7@S}$_`c$qVpkg|I{CtCcFUJ zP9~!NwC$MoOqa0V)(W*5yOJwztzC6V%{y@Km{PYR$pA68(k(oNyI-jf1oC6fsH)31_U z#AdxH66XNT`4B;Wh8WpM3{C*e`4B;WMmpI`3{C*e`4Cxu#wc8;apD{xIv*nL&)fmm zX`B>5b3R1kpZOH7(>Ni3=zNIKKkXW34s~S*s|eBg5V?Q4IA#eifVPu~=s!IZvxXNy z+sQ=wpFWOlnCTk%+ghPEWB=-bTdNFrY!jeQmoPDf6$HHsK451zf5p%kZyWeWAtU4b z`M0c!@>qsTeA||{ZEYD@ZEaa?j&B!fL`AaQ414|VD}A`~BR#Z(@$r@NTXolO>Rih7*U&T(BZAk93Yj(=&d)4fe*r(O(l-YNq*(tRzxY;SEZ=|_Bu8+65 zJ*_XRxjm`RvblY4-{a=?gubQC?HPSnn%h(Qb~m?Y_kC+_kMCR5+@9WdytzHO&#}2Z ztFNuOJ+aTQxjnP5s<}P2FS@xsr*E>^D6WsS*(j|qvDql8Z)LO5-oA!rql7-`W}}S0 zGtEXReLI?svio|PtA$(T_C@b5ySj2T-Oc%M-`Cve=rU_528Uar#c0`d5Mzy7$C18G zP1TfE9a)BlTjBcA$IaF3traqi&2AmX`rhx0-c}~EKYBrH@&4%LWq#J9i<*oQ`!t%X zrCUR#7|LY>7Nd?$M)`e#P1Q=Rq0$U$*?`q(xLcw1s9{sJT!?jGp zdQ_+>MyeGbbF-Uqa$j-Yq-I&6)u={O%#zk8QjAtNJqlm1~-k zX)Tmuyl{(6>3fnlsaIxfJu1>PCDrOI#dzozo7{IbFIu$KS(@?HEjF>wFK=>bS&ijr zXw#HRYlk$0;Wi!DCy_TPT^4IGYTY!Y&}uBr_~uaO8b;fMyj0xzhHuUchqz&-U zRVj2;UV12debhjb|)m9m8e(_~;#TXf;~?vD-SQMr$V*#WR+(j^WG!{1^Bqbo^tTJEx{_)Ih!(5mKfSdx#zE&V zzYbe}2mb#SqIJxt#p?J^J9Wlv>Wm5OoSLkik!*u*5Y-q(}!(;W)E?q|F z{UxZh0XrJM0zFl@KT@Q>0UPT|W~9|e7uH9+bs1gsmvE#FY@-d>({dE(jl%u>@^pzU zG~@IB9X0;3{#{f1>L*VnGnkVZ_yYvjeQj9SCywb;}|;{J~)X@v>YWm&KAPP(vumuR6sIgD4B7# zeln>(dQX>8k$>Su*v9aW2 zhC(s}AJpmk=zy;3v;NLoX{t)}!y^4k*x12j#>M*R;I8U({?6NISC!~*MEVzEWBB-A zaF5||4eqh5WQIdBqcxeKm&~}8%m`0roU4xx?5ZyE@7PK!bf!7m(D0%ADbZCG>8dL9 z3X%Th*x2!8hHNsUD49`GA00YW^HBWGm3OQ+IKBB_x0^?%joN&xx{>P5|3>^&RnF+T zZyVm7xiR*t-8ypb=v%|~F=nE!H12pgpN(n7v*_~O?|p9Wet#*&<30W#Hyt&O;Xh1W zVf0i5`mQ*+BL4K`wp#1ejcWcDV%&}Q1N^CC{Edwif71dkx%+1R8w+^k8WsH47huKr zz5TZq@D(?T`mdSbZb{PUQPx#zNh$9U)0JyU4(u_|Rc%Qf?qQ)xsw8puNYUg~QZjqy z&}3AS&3n{o$||W3doY?*Ns>~JB2BR*GUTux?^a@ z;&GhaCSl9O6H>dkhv|yPukGFtrYWA-*c}jNa3W5w+bm4|L_%@5cbNW(c+YN!FzpkG zAG^cDjGW>`x>tv(IVBwKb`R5Yins5!3)6B+eA68qX4oF5(`^~1(VkGzy*q4qdwgiO zW0+2R;#hZdn6XhDfA`AqWkv}(-8;s0jpC>8#&+!=pKFx$q_ccn%qT~-Gjn{-)I#U` z_Jej&yv~g}gO*X;9rr^A9i#X=8u8o8d=2&b+6J@vr5?C?WU_|6~C$5R#(d^@kMu#u3<}@Mz^#7 zk^)iBI)?!5f<+%|^#W817JjS?4=^f_5UFJkSWKNSQnxxljVgAyRyII^y5Mk~dw?EQ z+`e{RfGkzizRoT{i@NAdtyaKN>cTg5!2yO;37uNr0BQRC>5SNhnw}*z1>cnX9%FxQ z`kadTD*+4X!W9kQ0yyb{q4mcDMCkKE8`=W+=|W@mRRI!ok+Fu!AtnO>{`$lr0fV{x z4GlwF26J-i&kW5s5YB1n8DcXKw65PjG}mCBb;FAx9s{8#_2oli1|m-yhKE@02&mR) z4$ZkUSGD2c5O!zI)%r_A3+@PCZTLFGaYxXvK7UB~&OE<{_d|Slga+y>hs5uQ3^Xu? zn4<(F>QjdVqvlF9G!9WF8@$Bkm*B)Zp*tHmS~c6*NN#l$;z+mCC-BzAO%jvG$J8FyQcYfL57bo-4j zpNeN?pQ>T?cM)UbyjSP%FUFF3_msbv7)R>8F8?qw=CyZo{oTaa*G@K*j-c;t>z?N> zVktu}H#AB&3XLtQczy96i}2Hn@iJUoM_f;KVwy=z@rPIxXaf&r!|bYL;#bLx({5ZPiocrwp+8%8 zJ$AW%fmotMZGbMbTvw_;X4N1%E_k_Nr@wFv&q9B`mWHqX%qsQPoyCJ1<9ZV;F_god z>eP507on1RVQS)T8lO(*lD^udG+w{@ZT_6?DV6^G{S7L2Sou2_Phc^;HSrzw1D#`o zoa53HlqsIr_?r5T&VfPx@sCYXx$9)+q_6s)|M*d}XyR+zqPD4Li{`hre;$k8lrz0{ z>R5FTpJkPy|GVA@UFx^%$9GlisJI-SbM);6k>hE0X|k6$)@SVYv08j6Q0j5mgME%| z2UqgGx5>DDx_Ileru(-&?7zP&`^+|>_ITMF&8l6DO(PFBmObBjZr+QJSBj5)-dLKu zAS~6f?PlQX&6YdozdFBdC%d}GnMHm)OPysLN_kedtV;AzrG0TYa>{Yk?ak?Thdp=N z@4jQ6`>I-hQ|0$xZf`Qad?~-7w7hlRM#r8)#~Pwq)89Vv&0i+QIcV|j;_-8aE3Mxv zc%QP{c7b#GeTo3*GA)H$MGDs_0*90j3W=XhUG&Rfd4Dd)&&2Caua6&k``o%o$LLpd z^3cH9XI@dtU!!+a8ug71Jot4ja`V?=-|6V%z5Flo^P8ofO!P{f7*Z|Hy>X0Bqp2bL z-df>-s0N3(6!jy=l9)f6`5(W_!)y3YuERp3_MR|<+K(GTPK7SZlphs$J#Nux898xa zbi>&j=VZR+l*}pfooB9^&1xi~yM6!7x#iq)>-T6V&^W#^yL0KD_ih>-{qc3w(D>lU zrOW%z1i5<|s7vl+^YXMe;`X!Klb-dJ{==c(+spku!)Vcs#;?A6QZIay&t2}Z zYfrIOpmP$|+tOgyzvPY5`>>sbPEO%Yd`&N8Tjb63j+-50Vv-TIGGF}0f8dMhM~lVw zrJ+ab_3ZE6dtTboRdD-|X^DPn=E0px|6B~Do%OZO3RJ4N`SJDJoX4>YXXDNoyRj~F z?b4J68MAn{WcGnL7Cq&0!|M$)T#wz4nkFfv{JNrEayg`JM_5s|(iO)X8t;Js-nZ?q zESE`rp>f>UnVnmlt#saTj`XOeNEfVJU9GBOYg^nnG2t9>NXNJ zeN(k#X9ds6QWptvhS-jQ_=BmV?=OZisT!Wnc{wFIN(ax~emoYuA4NdgxDM z19Hjr9l6EV*A$#`mHGXVrl%V6Wp#0kB`cU)>BkuDG8+BMTj_^6H=L}EKe6VNsc|-= zMXG`I?U~xT@ptTV(=g+gdec8M#6=tmig@|8xs&vxDeKyMj>w1ITDrw7%i9uD{QUYe zHQO}n^=GwLJ|aO^H#;(VZ7Emm4YGY|v+nRZJicuH%SE}w!eQR@x0sdiqkca0 zobK_A9czu9?oA!xH$E}lUp_t3IPhj9U*gHbS2nE_jyK*7M%6IR-166sbFctvagPxKj<{L$7$4c zb_BX#UK7nq4HYeYt@1T+@~Q3z7wrMfB)_eC4?j8fSSK&> zW}Az1pg`|fuW@hZbT{Ldw{O-zIa4zB??!oFcBMWnNI7rCx@P>rjo9y{6E|tM2W~09 zK0ozfq)pX7dQ78aU`navy~z7uk;M|+`BxtA{vc={(-%Ck?(37Jt1WK>w>{!z6o=_* zKR;$vkm9yk_A0Mr#qOkI&m;J>R~jna&@Yvp%fw-^bY*l2<(|@Kv)#6SePM-F35J_& z#ZL?GX;GNFD99_~{%Fs0w*9uB-w0GOSbo^G_C=jt7O=Jdji;+kh?i0RRui=c29m=9 zPt81nZ`TR2FI~sIQf9}Yyx_0(t5)C9Gj(NtYQMmJD2{hjR8OtZzLLqSw)l{&(Fi?XIETPqXpir{qH*|uBY#BsT}yeevd=@kJ2aJd5;P$PHeJRcf;U= z$kjFXB5bAfw@PFPAAV%CLoL_zzEtGx3KoxziFLl)SPxeyhxy2{^_)9&Im5ATn`im( zZpP`BYc{TXE&Q|Fb78@krmf#jm{_%Zm8jnm%rQkFaFL<0iEiihai4=B~KNV=pD3w9u@! zD(u{m?bqj5867qm@wy`5e=XvOw7OU60;!=5JnP)o)?RMeEpYVI&f4TNKGLjv6mF+I zmffVSFl3){Lg%03gL^XC?X=>wBj=Sy1TS^lwVc1bcecZdS z@WjpiCrn2i?i7B=y;TzR-QzP;rpinI>IFxXU%kInR*^Vu`@+d--%9T}%)VWci`0%h zi%xF|=#UaB+OsW3C|B+Adev`FQRw?w&KO>H*yb^N1Jc`>d z_zO6vNj)0=xS3M^@Yeo^=Q!nqCoE_u4Rqu%q2K~L0*$f-_-@@b$Gw{ z^t?KgHJ;LCzkH*(!!Z$+B%9}*;;xP23Xjd+7arVO{!(9Ob%_n%*Ao}b_I>C+<6g%j z<0@Kkmg)1HPix-wo@?X3pIH`H6@Iqr{mDK0M;6y^`gXl)t%!>2vR%W$D<-9e+sm7y zf6dW3745RHa0A!PjcJFj=)CVeIk9DOF(=JM%l9*fuJ-XI&+B);yEEjW*Uc03k$GMv zB{*bZubc15gs~qN%-l|DtJf-NAB>_cS;!h8QKskPQI zyTcYyEH-jmU5MNM*6!JjBNlExB_)UMC~_@Pwz=h@pXGMl`-OBw<;Th~Sw}%BvD4U? z1K+;PojY|aBwLw>GuuDBk^8o~$Y$6x$m6q=RH$QE+J{|1DX0GV6xZ*d8Gl@&t{~46` zjuysxy2W-a9AEXa^PjD0BFBRiDaV7^bNiI%>~?(;zsZ(8xl(O@c+-YdQQ6XphImt^ z9-Twu^690QGP0hh$%$M%Z)A{Zb8$5OwaSt>dg)oYeT%xh-Y(w1?^rl z&QR@jY^lWowh_+_o>?qXaIY?})jWSsjIwT%p4R%fCVj7kE?i-Y-OrgPY@`aEHeELG z;_KMqkUB^rf%oCntMCGrN;k+QfCe z*GHsz!}!FCCANVF9eA35b@>OD3Pe=Gu6R-d=N z)u|iUT}HL)RJ@(acVyrCxl81o*PIf}Sy?RdRl_Z{og?jXjVD$vm|F6S?b)9DO+K6X zPu;p)wEDjO`Ra(OS~0T+-)&wzWmdnwByK~7WSH{9s4Fg`W2Ls-4b zxwt~O?xc;NLZgU5BgJBFm*nYtG3z|1j(%1VFjn5vyENYVPTPy!zsk6cMik|jO5E*L zIq-tbL+2#(w1I6E=k5b*+br9kdKe`s>vvs<+x;tjXX`o}KJSCLCVqc>lr^v%y)D*q`;+X0V;XKAaVULP=4 zky1X+J3XbVw&0?6IVg6^`|8zAgKj$?khFiE*m5xa`n^Y8D+_j*+q$)Ce#=j0a5a7slTxs6ke@t~H+E*F!h%WT>0Pv*l2u2RGg*c6Rw(6-Tim==$#mqFy=hru6Prg-yp>lIZ@yck$(%@oM<)k& zB&b;KEc|ujac|u5X6E>WEkZ2L`va0o4>~pa2(Wcpy-c;}kT^BDE_&0hgX_)bSW0ad z7M}cAq$O3Fv~o>Bu4tu#N!861TiU6Y`O0Y{Yv&7?iawvCe6p*OHQSYC`=gAO(f!v% zmlxBYGEbSiUeb~Ob!QcinbpJbR@*pxgO17^HkX&IbDVkt#wp&bT8!-*G%H_^ZOjbB zx`abrw|kbToocE+Ol8A9iHlaXUr@SKRwj`3^o}wA>TMnKR~6mmIHCSjs%!Er?dN`{ z9oGFfi$xCJNch(KEBV~M6Efe*m9|fW)RkT|wBIYU;&i?14ehk!r6GG3$LTxO>qyTH z;|tolsZjdt0MmMz$AU&}GSS%@7Y^MSI&m|3mky22!*)w-@P(|%gVj%uaS6XoeIYos zl6%(!bCdjnli_yt7Pn7+T6VH6m~G+w$nHJd&jla*bN8IO|KmKzk{5-%92akH;c43C z<``6RyxTsU~7S|B#(bF`Bg)s)|Q)5)5M$2F4)Zzeu_It>?3Pdg9C_QyW1VFlrA(or40 zyYC{-US-OXludL2hYQ>D-v5wa1SWmIH@0a!<}vC@A6?_#|r`zbZ~QT3_<1 z?udQ#J-#`oAG+)~=BE{6l0KPm!F+yqbfm(_j&l8piBk5$%`Xlp-xl(Eqr6c5<_&Ax zHJn2)*|fE`ilu&jV`V*vMXNlv_bz&oU%BM@`u%}RHYb)1U9A?3N>#vaOZK=Q?yq%l zet+EJgrvup!=IjPYV$rNUn$7TZkd<-XfFG`Rkn>gVnnYm&^|fm*^B#ILTGNhdYS2S zzbgf{M%yksyp<=Ul7m(id{1M!?0g-D;<2}fC&qmEtG}{*`E}4IAy=X0+Va)qFA7+= z=x5%3f0lMTi|1XF!~)mHEvJSK#+-GH(O+Fq!CG8Ws^_zGMduIhld|pGnGW&mpVT^K zWPbQ7^~vF@yP20PZYeJ_x-M&?t(m2E>BRLPq9LEt`c>D2*8Uj${{BL~2x%==1H)N~>6tb?Fzwj+UjZcU5G&`0#)TohPK$iWAck9?HF<5$f36edPi3 z=9ONZt{Q5qeMV2-9NQoJD#%vnADi2&;vzNId$N4^VROn{J*dgTn{L#(YFCSS<@^=B zEI)jsy_d5kX-U*O8OQZpI&a+}{P@MHrG;nj`+Bw)?GbR!z|H3yC+^ILx5^)S$jOgP zn9@#`T^HfxKD<`z%=@+?U_Nu3yu?jdH7of>vP5~jl&qGC1sOk(R{DBBnQQ$v`pUk$ z(nYf0QZ8-wKlJX1Ev7 zdGmZ;)@HUrTb+RyPm&+9?Y-@~a6`?bD(`EwZ*O{pgZA$Fs-al-TJhPn$9V0tEyy<( zW)kuJAZ^Ih=rL>&6OeyX{Ep)j8O}EMRrg}n9B){om%Q+T|1oDvDd`*g7dw?7{ilJq z_1M!O#n;wvQW~T>;}0GSRg~Bht1>+JNr<)S&h4m^kJleQCNw%fhwC=GTk0}q&*9Q7 z$6ot9Nc?so>EOgk%@HFDMIFCln>w+=aj(P``A*g>9pi9Tw_SXH@x`ECpCl@^<8%>gv-lR42xpd!XI9^MiRo2; z-!(6{Six{rXo!BJ&G(OS{dW34;+9B>pGZA6=IpZn*E5-KKVIxG-}UPJ+GXQ`hib2X zPjFc4`uW9ZikI5>{Z%M;1Z&k4lym}NgkIK-Rig9s|)vjk*SIa#2(i%%HJ15FL&qW(p^lh%C zDHpr>Mimws4XW9UZrEvL0P~+FstAVy5&kUZm7%KEum)C2am&2`R zpjL%Xn!Lw_3hpS|sSn2U*xWxE<$vZZ_O=6a zh{d<^jn7}H{_dr5fVy)e{Yoy=GM7;;t#EUNm=+bEVvgb3R0FOY>Z`D{t<6OzZGtpU zI#~5R*^RxvH14vu_!L{T<+TV2g{1|XQ!K44A7m@v+HE`c%w?f6E9T?#UZ31%VD~I< z!ZpoJW#jE{(Fc6eD(YX`y53lTzLu3wv|*vNRL9*C0m6-qO1c!^sf(=_w?y1>i5@nU>RO9zH zJCRr5=b-Xj4{1I}eudK_dync|7Bn$`weC{Fh)p2>nsmLq10uO4XQbV}?B+T%*JN^i?u?ShQX9T^3S1JC47_Xe!DAM&W=mMK)d zHRK#`UDcqm>(@$R8m zL`+G`r%r5^-AlWWeImR#Vf}$&8dZhHTp{fH;Gl`bMvFJ0dvkU^Y<-j9ATgBn_(G}g zW4#nDg%Rd;Ix8AhYU+z?4XoKz8vgv+*7+0jmgzqn6%H+gybB~x7p%hQCxUOtU!Rn{Dfy>JcqaV zSCxKUths&5b60)++>AXdf7aL~6TTt#*d4Kq62?uv zvx{PG%jiI#ra_(*qgPoO9I*e9jmZugUUd3{I&;80nBV!`F zt_=p*x}MKDvh{nN?A^gEhqsH=AFb*SVmiKDeCnH54jsF8{E}hBO+jS zOMi{%#{#QD@gjMRWq!h(%;j8}Y~F#v``5QI=8BYB?~XXzn`-bNUgU|v4`*#ju^;nT z1N+mhRG80w+**2hGxM0IzDKcEuk3=-V%7um zopPPV)si|&4X!*^L}i zW7W6S{vC%RuRP0JwSV1#wY`JdB@!&=x^7GKj+R$0(B9^`zG$IY;=P2o{%aO5wiR4j z$FjiE(EpGI*N%?yFHSj+TV;Fhix0AJ$C{UR&-b6*5g@y_!*BlUpzwxuEHXaI z&lc?0O&c&NVS6gGQ&Y_}U1Y`8ipL{~ds$bcUpJI)wbb}ov2W|W{1Yp1@|9}( z7B_H!E-$ijn_^Vm;$u&aNGx};XPvj3JK=*CrTkOg=`;TiLqNR0kao8p*ut2MD;@Y! z2g9w5hnn-J9+}*bTMX1}u^f@b%=`G-dZo~gkKe>`r$31z@ncbCPCEELIN=q zK9uFNzQ`Y&lM(TlLfnVG=WHOVA>YC$lISoE_uyXAkfyVv)Iqz!ZjkwFzWKlZ4d49R zf8v{e`!D$h{*!N@Klui57#{x2(4;C~LiIvbRyg(kNz>igS+uE&H2;zTp&}%&kK$OD zZN>PeKEH>>k-y@7Xj|On8p}YaA(j$sd|oaEV`r;P4YLNy2)snt_~>0p9%fF= z?%V1L>eIGEWLqWGW1p%n*u^en5e?8|YdDRqGm)^tK5)vxw_2hCgI_5TFW|fedbvcx zys+V(p;d>3O)uxi#Y0$#MyR6`g<@3hIS&%_*GRpUA|9Ki=A}d zUQCj?4kP>Vf??!vI_Bh;Er_@AC@Aj%Kx?Z>f5Pc0>e;0frI>>OwzbSjRW#`lA}LXx z7^w6Z`9`8TX6zo8`31ihv5EK7IqZ62P4HPk&8Rn%1~UG~5l99K)7I@uUDc&wK1(;q zd*A2NM>sg(27alwG@G#jK0TLrKpd*oe#)-!IMO+_T>R!L*;D-3<1V=~@!4>2W*@5d z#$qPDvUXE0Hc`try-yYlhaQ-dq6-o}-Gy@4KmLeUkQ$}mJ1A0wB9EFvv;~ahL|LCj zhg`&g>p>$_y7I72b%-gvc|a~nJVyt80JgxA)C24`~_51wG!s5%D_@oNlkV$ zAa__5#j*hNTZ;AKUM|vF$aWKw2G~wY&}Dvxa_A+HPP^#fuO;I1MM1b@!ImkPtqYXH zS>&8Wy-tR*a>-e;J@8RI+50%MPoKotNbY>gphUs*V2F&^sB9{so-skBQ*c3Gm*)+r zPl%IYzUOW-L4jDKAhpxvL{ROtoqBZ70~&R=5;-xT7-|5Y3TC(<^1Fy{NJdgJB!d;) znNaZ3b+M&#!|0vn!HggD+6f@HB#|li)JMANDI;bWK)(vuKTvuh3`!?oxBmeYal!~de`NCwhn~ytl%R#6vEX5a!#R=7jeD~$`wUnz@U)X zi^gZ0VVS0@HHTuK3(B}Njz>t!!2!ZL0g0qh7tocd#MHJdWieCV`k_^e7>!kU>hc^( zVi02 z8mMubv6Tv?+{!flY>qj=Y@GJ=7}G3FdFn$WV8jrcGsoc^f5mewg`BX40`ThMqv#J) zQ7LZ=*j6aDdh3x2Vp;R%F{M{{paymH8N6)kqC8tu4v#CrrK! zm|i_=0WH#5LI#Iu4L-SF0O=|#f^b|p(93fJ9HD`!qJY*)j=mnfTc zH*l*ES^peT5*NbZ+wnH~XmOfCLcw{YkE4;EHP@HOyR-L^1iPaS=lvbD$N;=zPZeHb&($(! zXZ8s{Yl^E@_D%W`)CI%T+{KzUH^gVHqSKwx@C)`Wlo8wr5%EzsYzR*V^Qg#Gov4xd zoTV;Gi*^5a2+@M1-L)m7nnVANmip~Vw{iI6@t50m!Luvv0vWmyht7rSpaC>V%yF_K z4FGV*z^V9-3n}uQ2-f^)a?VwuFt10cYgmGy@GTs6lEzIKSd%}Pi9qx#N^QFzEhJDM z8iY|Et-heY6e$2cBW&xv7EU{|z~kC3a|myxwuo0AYSS&@a0h28HL+Moi5193zFY1@ zNQ_go{Mx#!Kg8)$a?(P_0imcBf1mfGK?+>iOeQ`&k&(R_vjML)Pe!j$LSikaT$&;f zHkzkA-ctGa9j(PhyvlV!BXgykgOMXxEOUH(wlCz8)9SIIP|HsE+zKS8{3*qnXbu>_ zfJCX|rIMHDSi!g?R{yyO`e}1QtO^>4D9Yo3g}Q8MoRk+#I6#Y9a|tQ$nswD0MA<^+ zdq9Kz^;SufiRL)zgzWnxg!_m@uy{4;dd6Cgkh4Z;uHY(YNVk*(6euV)@Xt6pOQ%oX z-U@UQP+{8{rCFr6P0j;!_!P%)GELp3d+ZZHH{}a2fVd2xypa`hKPcSlz|q|B_(-`w zs;(VPn#(_ zP;Ndd=GVCJMQ^;v2`1NgV5Oq@J@8U_^Y|2h^S(u2&d zGu#F6QMSe5@BgW87#vo$84)m?RI) z`~#zKgQ-aQ2AmiU_r>g)IxR-(;Vr!6Vc7| zKC%S1^d`zN=J2Z=Q^hwmvLBvWcC|}})fPruf3Fvie?t?Em~=17Sa3HIRljADZ~12u z-&z&X2>4or6}aQ+U~&KmRkTSDchupZAN6%25mj;h5R&9q>Kk5Gj7+(G!s);fAb$y2I2n!00960#M#FVwAz{; z;BCpax+j(|!RnDM2ISO|2ZSQ$3^OuCDvAtOQP^U+2MEZ{~UE6$&zIxgjSU* z!Q%a%ZB_OD!0Vj)|Ok?Nx0`twfMfmL@wQ&2$$BOsEMW^MY-hiqDgrMz$ zI>60!9d9QCly1zZYuxQMgcI;d!_)sHyc_KwUOX$v%hw>2D=otN_h9RU@n4~o^g{vjhRtY*th6G{ff?%=w8!L z#udS9EzA`2Q9w}=0nzb9I^5G}(zW7(0Y;N*r!DZWRj>!qTE&rmZZww>2um|{hw*sI zLl5m6OV+8&mB1hHwI$RTjN1vJ?szv5zeZ zKErrT>i_u<|Kpe6w*T_%|MGXg{PLfl<8%1sKmPvkFaPO}@qc*sfBL)MkH2pJ^^fs# z4WF$2$A7Hm&)}}}d#wNaKa|QHh~2u2P=Lkl)|C#!rA?6f{p&*;a`tM@)b~2fiw9Y} z-VV}P0i4HV(PZHUklXfw+S-;!hoR&~YYuFQ1%Lr_+tlPWqh8b%l0HWsQSn*0om3ar!foTM*dmoJzG_#q_r0yq8Sd zRanS!Qj*LOj5cm1zv;OjIYkWb5UPxd`Cuy1QLXp}&tf$97iy#YmS2tMzvSw>C+qiEu_;0k)Nq1J66A@{IJ|0%xPN<;qhi1f zjqe(wY8t|5QHpqhgRV0GS;ycu=rRr^RMy7&|+FDuSYLlID2MAU9GiN&$2-Kxt1*{7W* z7w?2o|6Dk5C?OH7$wMwaaE0SDlyH4(T+m%DGd?c;JcsMHTN6DY20|8fH02%++o!6g zR>~ZrOwVNJ@Uh=868?7K6aqsGyv82SA>HMUeAS3Te z46nUlJ^VmbERK0IZQW+sxUK=ltC&NJT5q`+EEAT{5A+3%m*`;^X66Ewc^;R2l&<79 z(R>2Rgttzg4pYOf0axb6KdskvCi_|&A?&_a0HZ5oc;yfoFf)bu#J6WbJ! zvhmF3Fd#888N$vQkV*h|xV==sgXhH$fgQdp;r3^Lg~!&t6UA#G`;z~Oin$cjQ6F59Ladt1z5j87KB#znekjt$QrFG zznAUIv})a36>BI*H>S1J{*&+!nOH*!rx54uX1rnzU4>SnkJjcA<{Be1xTM2}!H0$S z#56(!7WR-RGl44$t;}0R`e29VWr%o}V9c8?TwadOwQmc+t5)-R%K$Ia@Z9o&1kvVD zV$C$tW?9*3s1@oh<+^WHCq;WA8A8BBuGR1oFt5zRJb_z;H7ORzbpQ<4Gp0hmBDRgR zUIgu-{99Nzw?k77&D&!xow46P#4V&Xf(0;Npy_Xhy%uTnz2TvE`~66UYY}_W z?GaW(1j2-H8W5kt<>n&wA&zGKiZfhnQZsi(4WKD07DD3DCB2a6OD}bOr65#x220uB zLk$orEbr_SngKmy`2$`#wHon z^QQ=EyhV#aN8^Np8#w_5C@PIiI1EZ(bza22b*3)F`kXWHW05->4(1D~8tq#(TG#~1`$xoi5(aoD)vW4Mk*G{F@E+*Ukq{^?gb>9S z3N-ar$>zVtd2G8R8&L%z>_X^QRMp!DjKWqSfeIR6Rn;i|vTghhP5BiC7r8bs1T0vq zgsiS<)>mFoJ*Ep(8}203k1U0+#-{}%Hwv~FIg1h3fI1#IkU4hWw-OD20~oJ(t7;Gi zZ*(YmW0Qb#XQR~|_t}c;s^(DQ@(>f}r|DIV7~zaKkgjbO=27X6AucQBHcf|vw+uMy z8DnC|RW=CfS0V)tX@wKkUgt9xnz$9N9U#XuSyEu39BFJqj#J-y%u*^eGNw2gP*z9D zZqFX-d^vg|^z}?D?QUU7aQ4W6iygFSNcDZ-SNr_Ga?k7k!ae5SaF6=$x#!c_dN<04 z4=y`=rGuz|y}xt-9>o+mQbP(gI(hVH?ijC`N{*4QKqe}?0h^H>U)I9-7DY~r-vl^_ z;#~K{%@C-HlmQ*x_CTyA1H7i4B>Aazq#XMWCdB}w9>e>oMo}qjMmSwoSZL&)v)S## zZFSHXY`m6FS%NHk_OL>8hCsf_Dk)7O(epsW@Lu24??AzeF0D`1z)B0ckPBE=mWQy? zyKZkPZ)Srg+{4u2TpZP@8Z}20dIaN*+a*eSX7cO0ogH=?9f^{A7r_q0wxvp})8=n} zoY3}81>LrLdN#i3ZIg9hWl0Vjk(}qwn8|7n=;&J%jI-Noax4p#&kkOCprGf4aj=E9 z755A6W;uZ^N*8##wPGmI42M!bqsyjhd+o@0U>!vnM|3X0soBx%t2FBy%oKip5Aq84 z1K@WyQvMa9yRF%@ z8pK`dANsa12mIevBhHDMCjU*KGIo2KNSnZsQ8^cmqHs!{` zjz-ydA)45Bm60agO2B}3=538Be+6m`#1M|8_7`d@elX3%;TQsXZJwAy9cxe^M`x9Z%7v0 zpWH*B?hp4QKfCArlY6YcaL@Z+-P8Qxp6?j`m3!82|GRsp-`vyvt9x)0ntpYU1f>7R zJ=`De>ES=zll*WGr+ag+K+}AGUIgi$`ZlZJ{s)Z4liXf}P86@u_F{L!|2m3jufH&5Gi1 zztJGL5iV}WNJul)^?QTM`!F?em8KxMYI}Sp{7f)C);!&vA~QrWhn`!WI$C zIU^*hnd&yud3BLoxDg6^TX?R}Ao+S!dU``IQAsvU+r(*No08>VQ;{5cq zH*Ft4!ND9VZ>p*oJbW$1Rjryr+l`0PG`U%=#Gst1iqp-SEH~iJamXgRy)>7M36(Yu zZ?znFRXw~Y91Q!ePeVKl7mhyNDil&b-1Fc5{@-`cKmUb${`vpMdQU|$ZwFumF$JQq zl2Gb4XZ9gyx3uJ4TI3y7DUzZdQ%|o*I~Z`=d$k;p0EC&Y2a+nGj1eAks$MeqwJ%>w z&Z^P2JQir@@&g|@V)f&^t{ctA6|od6lqRjdC%hpAO@xIIO0jncLRosB?W}Vjs5^x4vH$17sE2;IQXHm4#Y z-h=_?_<B8pzU;>j3VhV8UPtYOxdT=TqA`7DE$Z zxzh%*TdHXwubY6qLkAs%(>H@YSu!G$GKQt9>yo%%l&lbl*<+}@s)Tw@#Fb6RGl~1c zdjtBkxhWRBJX$99=xWHU{5T?=9aZ_+zt#2N2OyKCg~qj=`=<;{eKaF-C75CqXqc3I z)6ojAQYa_@#SX_%VxU%(?b-cPHKnK6fSr6Jljm z1#sM9uQ;r{LJ+ z$8DEZ#^$6?_2HLSDT?%~JNHHt-_qz5%?UGjO=G#xT>gztSDfZpDO_nQHW7>DM`@ogW$hv!?qWBsEnM+j3Vf!4b*>$3Nh=B<7s|U~D>tnH zO?0pNpi)$+Xr191Vk}KM8zo!XQGkA_&y9^=If=yGh6D0`PXY3g(HMEyr>Foc>>NF+ zIky6RgfR>JMG=%(r&Whi5M}Do&4}zXl$u?n zeT3oV_BO}Zm+9kf0is|uLfLH-uWG1V?}*?rk(NH{N=`(Rl{ChDk+#WjwbOC%(0b>& zUG&#xJD^mq$tkP?B~V{6{B9re1L7vI0W0w8?Tf%n(Cts9h_nsCyrMucn6v>`6d~tC zuc$tv%Sgf94RhffAjGW*!XwsWsEsLCAO7?OFGExxI4K!k75r(1o8mtjXl)6mUxZH4 zC1<*Y1%~BQhZ(`3zvCx39{$J=f1uX7!GJW8B_4o&>n~d+fMH}MQRjjDTo|&ACqkMF zhstiCT#`FNKpM0~%#(3S?MtA&owmT*ceMg1O%Az`2vs+eSwLQzUWzYqBAb=t^%P{% zy?y1jO-|mnRaiA{bk&78h>@X3qRPULQA1RSMs%ArC)-d?@jesy_Fch4c&V^w|M`Eh z|MfSVbpQR&PWpBHZ#xP8)k)B=>qCIcykPFzO{!uOsvoJc!mHOuny${teM3{E?cqp? zRh4z_TSDLEJ#d`ZHs%c+kDj*Vb<%x~xv5gMEmXBQm`J?FhBRZ&9Cr{G4$;_EVI2H1 z$jk8XByIhKocy_fR5F2Oa2U7SB!eNs5!B0x?*ft`Hr|43vO3sO&U{*dVlnD=SONzt zV4LOrob%Y=Ru%f#q*p<2%}VBchp}l@RDey@- zSteM7jn9u5Np(}%fDg#xbD{d+tK9Yu(-jQE4;vv{`4xhXYI+kyy{A0MR?(33#|q~> z!6;HOs1@@?zpddV8GLWI2lATp9NIgXq|f$=i{egt>a@a?N!9+?&G!}OE#Mx}gC$5i z2xCefELqId2G=M==DF{3^1QER5%Wc)T9EP6`m}!qisJz#M{WD9t5m?f&s$0&-FkP& zbVtQOn=ik--vai(0RRC1|Ag4d4uslv9^exxiR=ubJcCDx7|?f?JfLq2D0-OD7Jc78 zVjjsObMC#C>C8;30Qmm-`d|yY`p4(ZYRZ4mJK?1+bg>o-D(&eezGskkH6$I zu@B~3pg|L>lO>)6)KOD56AWRrUZ7Pe*X$jxAEK~Hjdi21tpdKV?p{f-ed6W&U`BBcr(UhK4u(W5jtE_gV0OOn7I*f%t= zF243~MaO;y-<6e#EGGVp8e}8(MBbc*ski~DpkDwva8r8$POrriRg#)zC}y657vl!f z7MHTzp(mmsN(}2bT-G2m z7_O2hf|XT@$DKyk1N^N-`U}`Z)WyQw7*BdqvN`hxOGB%cP=<#w2ERhUx3rtxaHyc;z zTSbzawkzASqLj;{4b&X#%??g!2g6OvKLifqYvEVHyjjYNpxu_u-xQbpV?)*&;j!xi zD%B1A1+t%WjBS^2Xt{|iGeuIqo2rsu(xrT_DbcM_gve8o!Xf@W_yA5J` z{T?9#IL`YtG=7-3(b~aa+wqr90_$x zI$$s~q<5T;f|(U)M1?niR)qG9l!da%<;Uali=tf(Ep9b!dDu`A$~nky0XJ=9tV&RT}O2Q=H z?UxFc#{0`DJCJ7K?JG1_WV$a_7ub%xXy!nNwvD;t%pOQJ{vs3)*UcZq{YnR#r!;Cx zzL*mO!akcC4(b{u%abGzpeAt!oSXph?7u;8$7@QvX?;MKKCMRlbDhA&XC|@yl!v0? z&8b@D0cR-#)PGZku>)rIW)waoXVeVG(06>x-1~5_r;@)l@Jm~34>3_(R+{<&#tIPT zQfS2zJk|CSowx2|6~Zg4sG@1nL9gln?qD87FPK#U+qL`B(mwB)1-!TWG?tpj?rHZ2Uto3Wt5{N+m(Q~76;XkJ5L{+=PZigQ<_Y)m6 zwZQmu1d#<70`ydeDB{qrA4t37LO{3HFJ)GblrO_Hx%C_*Us11_qSE4Hszo>1)`YEa zML-%+f-AZM&(`cujat-wgQ>JiVJb!HYTjw=%h7QjnPCVaCZAvM)_H`Blu|0MPsM`B zS%0u=y@u8*D~3P8`@=l&7Br1ci1gX$1DqBrh%QoL8`#zh1d2(3Yt7y=fL;ef({S@B z?(0TA+{CW=;3=^>!4d!TfByK(@ju@a`X7J%@!#Lie|XRT`q!`j_-}u^{x9$OumAYh z&p(g<^|$Nw?#HzG=YKAjv3Itm9)AD(p9=NyrFJ_|!N!bboB9Ulz8cIq_o`JsPW&7N z+cZ^k;?ZeS`>Sjs568hXY*2JqX+Y0u>}Wj4b zha~xgjTH7k6W9I%M(}WKIrWgzQmM@K?Q!rf@deIC48T{~*`4ElYj!I0m7j*zr*D0D zdt@*QkvvpDFVfd?A^XuBWt!$>ePf0&gA?R6rj^g7Npkx&1280)F(6x$DH$!@+e8Qr z;c5A?#Z1|1Q+=X8+5-0HQx1_0&Y)FZNj~WcaxqlAN$Ys5B@xLijiMNtl!|M-_Ne*hwJJDJ zKGgYU)z5o7YGo}kO$F^dc?y57nUPKC_xXgxT!p9MJYfGJ1{}Z43!o-y8Yp^lB>lS3 zQ6>d3j67*vjLn9CBux4ST^SW{D?`z&By2wQ_(2fcDI42BM|*{8CG5-Jn%hp1?7Y3z zZpojY(k)2b3Lbf2RQB)=jf|a=-YxfcM(?|}=hto6>o!I0peyN16ac>7r)5HJ*b$rLVFtZe$0XL#NX` zTxz^HYNv>SP$<(e5)BcSem(z7V^pH&La5)r;uI+ixKxxj>Yib1a}i5WF1;z}3u^He zU6DjZRIyBt`9nob98*zBz^z+yEQY4WDyB}V*z5_l%AJv6R*}td`*8Fj!*FMvpvYM% z$4<=5L@KOl49%NyKFK&?2AB-Ua@O_}lzb$%6Ak*&T5)`+);A$d(GJ@)P|AAUh7Fi? z<8p+n3+}(k0#hGiD=rsPFeitOy8JxCO(rTKQkM5B5@_IMbppHyJ8;^*U<4qtUTfx9 z3EyLo=8Fu!Fx@Ux@VstQT8v!As%teYI>>~oKDij`#|WWm;@v$R!mBVTdGC@lKy(tK zUgCjbH}z$WBMZphDt+%O0aw?U6>=|TN0?$tu5;td@t895Rjn5;q&D|Vw)Ujo;#&@# zE<*$2qH-m1)APN*4RO;}Jt8L}m{?3d|G{nSQDWZfHleoI!s7z;(EG<;3G`C7o$gUl zq$55KB0kU42M+62EGGnnaQe4pIv`*Lz1P=elBr5}WU;uW2ICai$N(PEKZ*hrqk|XY zXBF-jUd^aEuW9ONn8_i$L!M*p;RZod`xt-uSn>9Jv)IplV0gs1!!0#krp?0f8$qPZ z4){$j7>Yv&bj4-4sHV^b1AcbF`ouq?c?uqY3%M69Y9&IvVhRDKIs6I@;8fAk;;REC zE>o4gGX|e$elE*`@DpQGw^a?G`Q$!57VOa<5rT-*8v^o2MDu>rSCF9{El^X)gbN6t zVS94OGuGgc*T9k6MtA#j4HBj45CGUd{EDQPCjBf|xd(GT}Y zWWzY2VVcyzd;WYqT0F{NR8(FE4i*`7HIemiPTQrN3iaSC zlW-4GKLx=3Fo`5i@C%b`Cr>8&g=YGBxkcZgWrPV666{*j6rcx$Wt8Lcd)^31(0NG2 ziM$wnNlfu*C#T@aQ?G)<;%_0OjNG9w{?HW_gxB`O$)2Df3n2St=}G3QZxY)%ZmjRR ze4Foq>Aw(-Fm!UnH{OWo1FBs}ZF$X7XSAY8;s)QuQXVwdakC)bM9N|F-n=Q}_f(zl z-*EDl#V6mJf{13uQ6a5Llvq)u*hosAN9}@D03iyT1q_^9-%LYo&nAAr7~n6zJ4BwDrY8TCX2%1gnK{!P)0L_lQ@cSDPhK)w6Jhh zj*)ISHk&~QM@2cY$G}`tWP?y+Q{~`8i@9*26G*b%O#qrK`Nsg9^Y>uH%nS~e2N+5K zvG7-dx~rCyzp;a|0z=xmI+++M?fKM%p#>Q14!=*o`Df&Has@Ay5Zoa>x(G1r;jv&# zAy9~}`K0Cwl*gE2r1a=BjWD)afMOZ}@1a(ZPlr7?q7eSESM6IGpiiVw%xt=NlQd7j zDV0F~@*my!$3OmkH~!y$_|Is^Z%*t33Z=?AMdAS- zVX;mMm&Ol?#)rco*+^BsKF1Ga3ld=s^Dd~tRDCC)GEl(qpr_f}-bTDxD9Y`|ZV-=M zxlp6a7Fmb81q6xa+o?r6J+&ntn?wh0H=8@5o&WR_FX3adkgP3rpO)J>bj;%c6gv}q zK+r5;?5pMLALqv_i37-m`I#=lgxCgdp&Dp#_v~aYlim6g>EqjF2+Z-c8~+g(=8uzp zYbZ>8t;4q(oINO4zOjuqm=9KYr}$t#(3c6twt!Dxfl75&aF+~M-Lys>Sl8H|tv_b% zf0cj&pr8xEGJ5@B0r#;3ah#@f8GFF6-RB9w!}`CgCE z;Z#i0ArC41gD_}b&LaWVzZ>_Vl$$m?aUU%g@mr=O6P^*^`_s)zs;A6)))kQju+7*# zZjh@er%ss-$;H-I+BJN}0}<$ymy9h-{3 zQWZL3)idY1L;0;I!=;QPKX$B0|N1C`t*Q*BZO`oAAWf8uXb37fIcfFZr>~)I zN!A(`J3JVc!qO@GB$t^q5d#lu{Uzl)e@u(ot`}F9319G?z;b`{EEQ48&K3GnQ0Z!W zd7OV}R)?2Ay?FoCi}>rs+a|m%iSh56!}IIKc$P?5q+K9$spHYOtY~Q}&JN7}cQ20T z4Nm0?l1I2mgg=+_q)kPq&0wdsRD%Np{Bxi0qE|B%oWJbztqULG1p39PfA^xGWx0rG zvCEbO31?{Lg2I-sK$_%(wudW>B@F#}RwpcW^9>T#l97{Un|`SEjQn8UP2IrsE6uJ{BpUy1#cH8E2b*o&u^q~sk>-nHy%Cv z6c>pOqmLu~Y7-|~M6|UNgeP%B&LZxl2VhZZ&7-^Z-J`HhMd9{UhL{?bt)a>;eST@UrG znEsKZI;FI3Lg~N=3G;>0+iqVkqHxAkWq)=tlIF`iU?4xV zN8fk_Z@e$?AJI6->yV7!jU=R5iZM#>pO(DtPxhfF3PPE>KVCe|nWff_OR@m6Vxt3M zKD`9=zT~l24)Ukl+mC?kf$oymQj79b7JKIAEjQ3LS{igF@oR_QgzQIVM58gUB9gsD zoK!w}CpTt2N}$x&Y!mrxhYaT8*XVIu*oL3bh4oLymL8|pn682HFAn7aD?V+Kz$y$6 z4XcD*+&D-nV*`Mazqn7u?E;fe45KIcvm}u3{gB6G>vpdg2tHUQh!s}!XN*28Z`AMf zPhasZ^VVmCYjiq0pwq3-y$HwTShOJ3LPDaz71Fdt!C! zU6R8alJVCCm)NYZr6&N$y2$+vYLeq@yR5@j8@jVF#E#3<1B(HivmKI3)gzF?og zPwG>cw!90l-LGfx^}+ZF5Z>x=PxOL4#pcp)FZhNXb;X-$l(|$4CPC~5xpi0teQ5-w+VUmu_ zTM=-%yb~L?Fa6t-kM>QPi?h#v`A-hI{yz>9|DXLc@$c@R!GAgkdL4uT&bw%ZF8!N> zbnzoqVjS=-?kJ`peCvptmnJRN)I8C_M_>rcIQxq!;lwB(M--lHKV|U8R;s)lQ0GW; zPr76A044EP139_zSjQ0#kJV>rbMcxIy3m`$2hL^v?SqrnPC!wr)F*`cjAs6vnTB7@$i!GmmKalaYoeaM~i;b2RDVM4|mic{Vg0N2&LGQE1y7oy{Q?rKGfmY z*kMvs08TqNzQ^Pt>Bf(Q2cLuvaSQU33!@pDVR7HEtc-%GPxI5m0kB6)u+qB~o=dRL zUkc9AGWotdU)4#E2kr7U(1y2MP0LvH{9WcAg~QB;bcRX(%{Cj!zZQ&aG#&2nMbHI+ zNGeg)dD!`){cOO+h!_hYvZ4r^Zb6QbKS&%Vym7WRBjkiTPfJDkKaUM9751V^d<&nb; z{$=)r`N1rHjmY5J31Q0nNey?V9E%Q3m~V9~L}Nma!F5cAosUZ_KHh9oGj8nzi2>GW zR4(rue{Z%!$`5s{>Ah4Nu?|*BL#6fjtm6`@kM|F%lFU0b2=K{;tYR@a&iv}DH+aGs z!qQfh*FhuuaZ5BTHsr!6VvZ2LwB?M>&k~EQAhB97Ts@kz=OqCSqr*tZgF&^w%4sAh z6K=}&T*g_29;Fmb1>sw<3Ji#h2H&Ds-c(0hYaUdo_7Zm!H1Z|^A?e5y1 zuq4Tu>p*N4u9#fOz4$6v7sn15{VoDL`acs))=ZfT1OANQ8A5bBDqy(m;7|R~4GnSt z+t5bM72C8GTC2Y!*BBS;OxjehUUET~wMVGCr_^se5W#|5~YM4!5X56=y5}R z8w5swXAD&7wz;ufYu?#Ar7OTgtvx6W-)hOX3%T7jkyj8GKE|0baB@DX>(UctH-c$S z7|`jjMpstRt$52)5)SI0lujz>;pvsX)#s9kWAd^Flq+hOS(jP%o3lOTy^}weN$!ED zZ`Q0KQ{1`lq4<0l!Rj2rgRgJ5g6QGe*5Lr%JD2D$1AjRgPQKEus@A^&00960teD3N z#oSsze?dhQ#Ak4%WRTu2D4d-1e$ta!dhb0?Z?5-F9O%dmo6Uv{LcX?EOzZi;b2N^0 z4sD28Q$LP!JEXcFDjys(Dj^UQdGmms?D*!xA$(bL1OpdYLtpgFOuc^iq0U4V!9$!9 zoQdgbf0zW^Gh%v8q({RMb(k%EV!*Pkm*+R#UQylgK_9e@;~MvuRH4e9+`Y@{5VNT7 zT{pS4(T3ih_Y>c*HjuHOM`-FH66v%cv}Q=eMRyH**rFh*dpqMtoJKxvj!WBtd>;_l z?Hr=IV)Z6tF6lOd`CTa@HLvA2zzwc#;ZlM`2Q2aX`aL3prUdka`_u{@X?nvM_!xE> zL0&WZRB{z{EQEnY(!h&Dv8U zz*fgO4@y{+1mLHviIRfAe8OBG(S`xi&jptNVWxh<(oHW?6!S<`;_9`Ds+7`rNDg4& zk+G2O(=Kgqhp&z|(p>2MlXVMx)XB(y08FC44ES&gk%nF z!LDOeR{3}XK;)~6SQ3*Uc@Ja{i)3Nmv7Q!fT~JzHei4j|MT2pkQ4~3xN}J0l#C|cd zcDlD#P9FePyN>hv#>hozugD4Iqc=%sh2M{ZR+~2+Q22lV79}7D$bnK;5APn#=hm?a z+GwLFUEqNFBFmWgEQrDqEG4(siUCh9^`PQ$Om(&~_gXy?L=yo022#kWDEj>zPO<`X zssP`2tKs*U@6U2Kr2tj7^)%b>+`dgGT@&)_I%C+v7=Lkt; zI5{;$vH2XQnAFUL>r-)hx~nutk?iWFKW0ISOE#+lL=EHv@Z@v&5mJx$^+}VxxJI6?2899&PNWIGA9b8T7n~iz`_$=bZ4df9c(v#!D&WMOn zG+GUb9)sLKZuw!&@Bj_51jYS?tLU$h#X$iZF8J`7O>?9h0FOB8&zb`8F>F{0*nF`V z%<&yZxZ_BYk_>K?#P$cV2RjgL;%v9c=Q)vvsBR&JEwc?#g8nIrL{`+tJY3H-?gMl8 zzc^JETiwibp}rTNxNZ-|7A`fXeMZLjcq_GV1Ma>UFltV;k>dnIp*^+$b6DwG>}t34)Z!y3t805a{) zXB5%=@^IO7y>(EW&C)(h@ZcKUU4q-7YPzv0{nQM zb57M;U)A@|&h*S~)x9-WwOn&|w~>foREx1FOX68`T}MsTgZ{c~9vZ&b^yY zHclcb*AjVE9+MXmBFGt*w&%r zf5Lf(SxTRz`;?`Fn#)Zuw4m+(%6pHS7f8&*t?u&|+;i}84zIAz{9RG22FWbp z`kx8eoMFL+>GHW?DJq6p7P|2Kh(fRqmvoCI`Ax2}S{*HUD40hYtW8Vj52w@j0Wzo7 zAVWP9F2)8ul81PcI391HFz?HXMp3G<(!1X0*bE#67>|t)vuSSKYAKiq6(zwcmks}+;Rm{Y=5Px%53$ST5QTA3aL&sPzkb0UBGAVR9BB;? z#OIIsoDxm4{w)uCgBS36EmxeEXq^A&B7yNDS&wW`+mb$`FC=Mk)M=h=yEdF({G_2_! zb6G)pgX@XI&(sj}rdwLW`ELQb%?=!LZ@IrD7u$Q+f!9D^h9XQYKI6CkUWm3(0YMl<9)yco1qkjhO0>GU%K!FKUXj{!t*=iOZ22@al2Qxp)VgNMA>P8`9#prJ-jGn{hZBn_I9JWC!!vy_YcNJViJj{rgu~($ z?b6S(5XvyX37z2XtE2_T(GW6V{V+psB*!?9caDYzCt{MjXKj*#JY1M96+gE8yqGt} zliC?wOS}%N@-7%OSLc{>gnIHBXK`I(&vtFT9qeT8Ee@1!=_SEBdv3sQ!3FQ>#b4F5levR{yU`?V(zHEHp$*?H|w5` zK~GV%=Nphb?T0}aFT(k?BFdYN7X1>GfWEcP6s_Q9rIhxO-jC_?tb{RKW~9*PYXNgg zJ-)&}@}5`ZQ1@_no>ZzwUl*z5bl1t@7Wef(FfXz#Ji{pLmOKWbaMXWc<}eUu+BfliHVLpP-6Kb>9=Q|^V=`$OO%2*|XL~BB--J)q z@T0$Ze-rEgAMQuh1JOEO zG9;aiUVb-#+L~z8J#+na1pi_iABAyKZqK?a5xtxXP<*M^X{As-J(Nse+wp;_c7~1d zGt0|)$eNy(%>oW zOAq0n5|05Qk1%`*fg*Ube_E(!%KYcm7%+_gYy*q0dNQ8h(;~7v@;CMYWKmD+GF9Tm z?MPcO-`YfSz;fTFBL1!@cXwlNHMRiDizCd_J=_Un6PV%p5~Di}Y|jWYk(V%V%b*y6 zs$a#96(9co-qO$%U{45!g1VLm`-C8`7!x+-c+fwJqY>x1ul&F|ED~+hx3m?a8p`?; zmnf!W(i=}aS#DVCbHo8LVA!Q4jgllnZL-r338fVEc$(h2AKtTp*TtWA{!42DlK;ZT zx-(9;PL>#@p3RxQJ8Khg-gDLU>FV=$zAV$->&zID<@$S9nDe|=hjBAzfcIi7p>1*5 zOCIHsRw`gm8?lslI%(+ZLg*Bcjw+97ht9<5LkM*|_oHz@qADpcv)My$C5WhYtd5Ws zpFY?T3BAl7N)=5HW%C0_Tt@;7)bA;=`T$J59 z@01Xk=Vuz*ZrCU150$x}5;Cf-%42t9>Vo6$s=vB%P+J<^|Il+LY>2N1Gao)Toh`vi zS4-HX4;*)!iVuC^*Lo4!=E?m!DeuXghUJ4PO!*tD$@vR!a1?G96IT!HB%Nq@*Z$wn zu&kSo-QRz9ZaP7qDa{EyYqVpNNKr>4ArMu}t$zzwB7P=8MMM_c^Ke>sa;+|$tZJZ8 zw}Z!-DIfjj^MGLKknY5DYhsSdOoO?>1gUu$6y7!?dgeG@ zQt;IR0o~3%-cQJ4?Rl-B1Q{%KKg!}WcSm(W$9Bi`=O^lP$y-scx#eT&p0vys@xIzS z7Ct2xG7(38B<=^DFv)mbi1NszKB0qm!dP1@AyP6Nqj*mhA`9790Wnix`HD!hfdM1x0RL|Z*8uo z(hwN7?>(*P!T!*zt@|w&o&8?nGp^BGc42ANZmP-B@+TgQQz23`1;I+kU&PNrdKsqy zS93NQCACX+lX40OKNl6Y3j7Q*LpiS%o=f>$g*Gkk&R&wF=VBXCKgWbK2HaWv*0eU|%`IvWT*|q1Tdf3U=#V7dt0>;_`1^$3-vh(9wa5&6djX# z*PwWkFTREdG*8mLCeWDh&W;MpHH~UbY*w;gE#NBbtCe56+5D~i0QM*GOki!p%N6dk zoe!3uPGyBJFFvCMkJqEan*LaFH{sA>5aKqU)b=qbfo>SC9>Zq-ZqYv^;H>y1F>}Ma zdZr-W3$KYDj%XxU9Wla#UPDoo!>@KI9j9eyXYp&{8+wOP}{A(LHGMh$^){h2ks=5pP?taa&| znf%M^E{yC=Zwg##Ori`14*->=NXUZ41PDTWP+X*ga|%idg~L(j=5Q-va?tdYu{&g~ zaf9eO^boVbYV$?sA)L5W6YRjzCUIJ=>#J5{gc8PX(Dk*TvhKC*nE5GbCMubO8;V>a zO=f^MwV^(q&cpGB?bvqd%M9|9L2{B0d7HCy(vZSfCb?!e+w;G!=lfg|Pz zSSUi~O!`*@#vou`+ zdGs=rp)6q6w<}W;eTDkc*YhC{*FAhzHD-33I$sU9lewpHV%r~O zws7f2Gpls~(C-YYJPy8WrR@H)f2-2)}bJ*4L;v8Ka1m|K6fJt^+j<+%-We(b_CE^nZZx?F+Y0Y zSN0&N3>y5XG>!%U9rRav+^A$A$}Hi)%bar%yx=}DZ5UDs$DVz0&;@{c&L2uTdJ7K| z;kSq1xUsVVLW5Ba^#{yIA%N^*T{AX{-ga>?sdZY^7H&XKRKACQbEnpKQahUdC)6~+ z27CL^&h#!I_I%yr<;SKe&-sC2FbwC-4{f63)NY+kqb8^Zlhc!58eh1I^EkBGW^lLA z=n?3LkMO)_Sx;;*g%>GSa}}(CkseFGGf~_)Tu5;e7Ujmht~OZy;^Q$V?^lKOna&2 zGyhL-;WyVO?CW|9vp96$em@5+u080F7%gJ@=goL!{xprCw(tE;9yraP>t(R6awvfP zMCc^qv32X`crWt0wf>U(_8`*sJazvT)A5ojtaJ`R!|A+tD*JlgYJomcp?%D^zO?%| znrZ#Dc7sED^=D1ELj-B?e4we7qq?5MtBQv7BxsRWzYAi7Cn6V z!j{j(K&Dn$&{ph%SD21w)U$6Vd7WPOZgRh)IeGEDMI z+PfGPoJCnVq<#UkOJV|VlGW(%^L=v#x7(owGs5<|uk3qG-O}g|$WsQC+WsB({+f)b z7A3Z{6McQ20qx;yG?%gEunFHw@u4J}CsmipB*!e_Vs(*=4}s@0j~!DQO_L(&nb2e! zvtz!nLJq6`xdB7f*L+tQl%jXza9iq@`o(VSA1Z&e9Nuv-wBjLZDC$hab~mr6e=o6W zi)8)8)PWFJm@@EfsIB3K%2vo@EUwy-;~DomZ5*5AY|J^@9SZqL1zjgoV1 zK&0N{czUVSYqVO){-WuqJ}1+s0>>*1-YVO}d1yZs+}$LKc?Z80#Up{kRMGm>)bH;F zT?U&8o@avxj-;H#Rgh43t@vx%$TPVqk_=TI8Kik2gTxeLQ*`|qH(ZgL@lI|UR+x#S zpDHjVH1OAOE-z7%Bo4lJeXN3W*-!h5Qn2Jp?~)&od&M;{`y!27f6rZdxzj?aj>xQ-UD6rG z(-r9WtD7gn2w&*$XSF>%JY+**0`npxC>dM?%@qEY z$i)Kke&h0XgZ*td#(n~G5Na5V&3?S$;@@Fg$-1};OI$_DVqF;`Y9ASJSp%|B!Tlwp z)_NYnbJO#0tKW6?(VD6+u3f3t<5ZO=c$UUlV$-}d3;whL)E3x@w#J@`u!-Hzd*a(!Z*;`Ji_oGp^_BTL!_y`qTJF!MFI^}2L=#uHVD`z z5$ssF$e|s6si9D%&(u=O{eO*DaKmgJrk5JN|Dhc;qE+>Io0lNJdYS2bO%kjOR4;&zDbB!vc* zLXVhQp-U*Be}iC9;7)d+)K$NWCz<@H8TW*XLD(Xm$y&x#w)PCgpUnMjS6rg*``~y~0p{%VX;2C|)0c8b z3YSlYAyzS6vEyeZ+w9Kjo)n~bH$QDtmDQ$FTc9GPomp=`B6U6dQ*~edfkERZLiG(&uToIA zP$&|jty8o@b=3#@!MR{ozEzX0_RN#|8paP&hS(xlysLO=vbJ37D6Lq{rZv)Ir_jv_Y?HoM@UYZ%nd zGE7Mmmme#EMG@_a`i169YezgTl!-f#!b^5DuzY{Pi=%O+3y!@C^#ax~3xKfKEp9JV zp&7Pp-e2x28Zo@Oz*_A3@+?Knt3p3`w0P6HGd}i{_jP7M%dF82m0HYWosvPsdAbZ7 zrPvio#Nkh+UosZ`FtJ>_2N^1xSSb^vy!bgLs62SEBz4i<@JWH!z6DrIUuzb+^?RuV zRoP-8QBS~Ptgjxp;S7}+2##v818L#B&SDYYcr;C#30V{f`_^&*%j~IwMP^Zs(dlVu zl%WB7V_Y0UN#rT+DHun=N2}M~z1_;}<42(mx#!+Rv{ui(j*RcEcWn^4yF!L@* zo>sE1B8nB!?f7Lsihj{JP)PG*beZgHg+>m3n(PJ~%107SyeRrT4fOav-Av^Q`#eou zzYJf$lwQ9)E9&L=Jv_a*LH;Rl?km{yevNiMDNytJFzoHld&ecXwyXCV&!C0Nqm3#1 z1(P9UK0<-K4OLCA8KFayX}mKBouUc$tCa`K?U31Sg!v-IPioC$Ed#^GLb+RXrM+BPY?6-8EZ} zFV7c#SzA3XudI(g-fu3xVQa!lD&5pOznC*fm1LBvSa(l!`R=4K0u4)iBk{AGYtQQ@ z$QtV%)po($o{_ZpUoB)Ln)3T&BI$8Vt_-8jp6qY34OPNZSZb|9nL`dKhp$MuXsMvb zye(zV2Ab^5zp%hI>49k{2I8mNb+Nl=t~CST@ZWRcFQs!mta{3&i*+Ls@d;q;Bv_l0as> zDMJG7`D|wi+o*zI!vJM(BVv6jJWZv>%!YE2l+XA}o*DC}1BjQA&+mvUE}y}I8ZC>d zi&oZR4Lg$JRG{RH7KpJ;?GUls)|8$hQk$qh!1pL0OJ9(`DZ8asO;u@?Iu4wk<&y&= z96vw}OV{Vk3uL#P1*cnzB7Z8A5Uo6f<>akifRR2;J;opD8pR#s86t^|l#gTXRy?YBtwT}~LQC|X*zHWQcJY3);(bNY!@=y{z@!hFr zHF4)6P2Op<@xm~?f$l`TNG%6y%-0ua#efcKh>>7Y{s9@N?bWfB{>K2A+i`T_qs4x^suRknb{q%Ki+Wuk0vNwt_)t z)VJ-O_095f*dkMw{~Col^$WSYPyC;}de`;q(!0Yul*LJSuU=g|DlbZ2frE&?XbeR1 zd{Kn?8fogHwB>@68Uba8&4#%J8Gd^4ldz1Y2_U3C)-%N?0g98~<&`{78>6@R+!c4~ zz0uJAQ5E_a8C9saS!2W|<|}^DbG&;)Oa5T!f@tFvLNu#>Oevy>jgu0fDx@RB)xHKf zd5;SPen0pIU5!>n`mWxY?1OEw2CJ}3ismGJR|c-K0{g*a~GHq^*1=bu{6SA z9)B-}MH0fMD&)j55-nG{=s-9PwPABEP7}5c+(4oLbDbQHKsv%NCMH zqm!t^4w9TDF2;zAW)Sn5xsP9oaC1h9R2)nX$Cq80ReBKq788_EBxAMnHiCx^!z1RNQ5X4q@<7|Lk7PmBilhbJ1va4oYs?+_#m>2QZ>>T=a9Gxa^`Q*mp;fwkWIIA zfJRD`%%7K9Zix9nFY+69^joYLE+O+G9D+0x9n>xN9zK?QGewad29=7=Z}N`=e*4X_ zNiGto{!#E;g&0YOPkkd-=h>X|*?;ckZPgAwquP%&c=(j0d6UpmEj?O`!VES(@b9(3p}Og`MIL0GPv1uk zxPB;Y$f4Cnx^v7y_*pk?FRUtFJBb?paon=jHr;e-$YeiIwQ;3%i2pZ!-Tf2E3C?IQ z2NzU0RlPVL`ps6ILeQpXm=8RpwesV7BfgsOL4#paQIo`O%M^1AX>1hMr50xNI>DO@ zygm~a8u`i&cOBwPYU@cd%sZ7)(}9K38wWAAU&3+Djmp^jR*}Ii&wi%_M5{J_}dSz z946S~J54HCDakhZpj#XT{ftFC&R}{{Lap5VHprOo+}r#)H@eK(XGpn2JpWnpXi_6>i==sm-BT1~5oUnLY87bAXnlm#dzi;C}hT$F6kdI@s#qKM= zt{+Ca);Qwp08Hmt>XHilf#Yi2$c+kK_slXAi}U_oFtTp?b;K42iXMAt@pN;$gYmet0RYr-VeVrZi7#ytiqlsm07@~GMFuk z(lna0!TF*!naO_2Q9JAK7M4t+{h!mE( zjfW)i*D9HwRAg-t7rq_$AezRf;Oj+v+6p_TOJs1GuWGKnDeT9#S}8L#eg$ILT!p_g zagxOocD)F=)KW4Wto~4}DB#rFagy2LYlo>TJ-ayCJ@H%uy6o zW;-IEr_KoNddRZxF#ZqJP@ftuOc4W{G0JZ-2L} z%&8ER09Jd4Th$rMTp8H#<1qgIP1*U2SAtz%U(Ws;IeM~0PT8>V7dcCha=^_G)gZ2h z%dfnt0~QE_Ih zQ_rTpJl8E5Zj+s_FSjdM1J9MFIcT|4Q;$17dS-2{9qp^1JWRK6>k&n?VNIL0%;u&S zChd(vpHPi!x+hA?4FLOO0Fq;7(Vyno-OfMAy_Q@Uk$$+rcna~%)ouI7_C~*H>o0mwz zjts)i$yRL&nW75ADDW--RV0akL|%8j1s#N6OMy(3m64j$pkjoNKJ1hN!zb>sFhQJd zfw%XTU(EV7Vi!G}oEd{TajTs^XA8HYHi7eJGC$Zh@$}M+JUH|1Q($&%! ztLZiNc}xh7zsn}VQ855$Cm`BZDq7k~lE1F9-PC$EsdUCb2V7aZLrM0bb@p6?8xzLn z@2U=JmiSaOHIU~TOQe_$P}FUo{M}d?@YGqHYS2x+t*C-5oHpX! zzG`S}okCtbRW96yxwPz9S$REDHJ=3y`_`QFSX*o>V8N2;KRnsm0_HQcOTQElw0N_j zU$MnnFL0~Q=~EqCV^PM5Zz)O{K({FNYRd%H-?o!=)sD*FB6L+JO#Jv}P3+a`#Ti)> z#7Z%EXX=cY@qkde#3#U5hhIIn3nWnw9nM$+A9pn(^0K8`YosFJVbQGO{-Cn}`GjFX z<{$T1!5uQ-j<

;Hw5URQfkDukdUHss^X|EGOdhWJL3xC8G0nW#o%4XQI+P<#Dl7 zoHovqAEApMV^osML+*WP*N*8YpI4Ff(KJg3vdR2C%(K0L$Mp1&a2J>n&mbjDoGW3Lp>D1M7np0d0@N@+3&h z1$X;H*z+Uuf50;St~GGrw>2#*YYGGf1I=HeOvh#Ghd$GSg{?v9~@u)L7Y$>dI?Ys9_*JfvSee0KK@To_Gh#L$*F zWo`w=e6$K*1bhQ)tAni|wSIiLZlEc<`B8C>&kFDhBH+qYvKNxgO$+fsjyO@*8P0zO?lGYd9?v!W zg`9@Hs@oP``e*soBO5a1yNsrpHPV9LoEC+xFR=|%MIBu>YyRH;3mO+T)vfW^um7RH zmnZ+R9tfA;Eh};=ge8FGo^?t7d(vYDFYNwIAZS`E?{^q@E!8LaUyA~6B0MJwljr9< zXu@(B->rxE_*v;f1LvFXg(*_K;ttn{suT=wU7zM)WX3~v?W+$DDXiJq;KZ+MTT9!u zIJY0fdSR`YMduBAlJZ?h-)!rK?PshsJUwpnOmzIZn!7A4{HPB7qeZ-W9}Z?pgY>4t zDsn14)fS#BHqV90h!;2N3~=|q5_SY-aM#H%zR3X9_+a#j-p`p&MITPHlnT_^yXp=)UH!-k z=Fse~JH-+Q3ui@AuEkJI;ak+Wngz!bRAf5Dsi`JdkZ83wfn2hIs}-S)W0( zl&kdNA63CH$-q7aD0cnggd_tfod;R~+hn*)xj;@k)-Wp5{N?tp{w_28L0yef{@CxP zj*oj=q6wT~pV^CA1=KnTv&Fdo=xCcoevZAr?i%t_G2UqnVbCWKw3d^1g6%t%>&e-= zu|2_gD{Wgz$agK@m9SR902WlS+ixMcVw{rP$7!<}E`5JOCtA>0r8c&daP?b)Vy7E<9>vFD-be zWT$o1#_5*d<1IYa2$SWyo%l459yt3tuxpL2>#?d}_K3u`A9#a!ca=Mc6x$u1KV2z% z#q)A3AomW!YuFUl5I3-|bg<3EE=2*J={_PxECObM9}Ghl3Ctmck^zeZmXJ@9sMT{zK!o6!MZgRYS(2nW=_31+Kec&w zf~zn5iukGV>ZoYDdA89Rao|lIe?~$W^}FJT7Ico|qhq5&U%-ICn(pUw4gvy>1&uMk z19r;&THxbeI9Ki!%?4jE3tl~rj8`@Gx+PDU@4=j7KG7Yc;FSe1?uF5+0$O&f7o=_c z+5G~dE`Zk=ckj#Zc|qm(JM$k*x*vK!C}!)5htk~UJze}3Z8^;u@s%-ZZT9d#)&(j+ z`fNMIS5%Vcju?oNg`@y6IfjM58OAJblCl3I&WbOn zss9xvlD$eC2|H|%5?>py@&3Q+ij|nxxwkd+FAE1I-&2OHRf?Hky-5jg$C#3rNr?W; zh7Y4F{oM1&-)}!2(P~JM(Ksn1RgM^~Nx49lg+bCAFyzAz!qyc1>#Sb`ZGiQ6q||cx zqI$x+J(t0$U^Dke8rI)RX5QO0E?792$(zA6fineYEKkj|fAul6 z&%4g>ll^RzvFUs{CIMmP^K8BnJ3=z~-Jh}1zzM+Y!q>mI=#cA#>qp6KQ%Y7#Lvcoy3~=>CLV8n7SW zc!YhsEW^OX6SWkk>uHW)VZ5`ZIBseBO8st2qxH} z#!_W_j>wD8mErwsRn0}m)}cqE*Vd-lBV+eZ^{xGtg)@c0{?^jIrpUAmLY7~1(^;W= zsK!x$1XWZvsJ~dGjIl+8w={8$8&ln`hK#sv8^xnO8Ik*qS~x)fb)PjMi2`B0V=$07 z5k4|F$X?HWGCy!~8r5e2M+&(mC5`+27>SDqoF+)(N0%CehZTM3y#==_k3g+p3LFaE z(hj#TRPUC*rq;y%njQ#%E3-_B~9d-a1!r_P>@`M;x9eSLoI>ZUUW7fr^CB+*AlvorcUr@+$dLHYI>_$nNvcsyEGZ-P~hK+Mlm65oJE zb~jmq&sJv~LB!*mNBkrybf_Fi6z0+r2BK6AYHHZNO9}~nbQ30f=@RCFbOSE3uJZwQ zWMpJrYg;`Q;ER`w^QWesp6<_M2T#APRa1tXRf@TU&n2Aq6%=d{1rTA{r&>&DhUswrOYbt5C6-e1=RQ@f7hY~4AUR}phWRrqvkoQ2rA7_;&etl6If*;kh5;5rAWWgby{(83ES19QI;NC6VK0$CbNh+pcli#ox(Ox@f^w z*qLV|Y%EHzaz@PCl*)S1CbZl5KML&Rt%audV57C$1fB?p{m#}tX~CV%87;OQPq>q3 zaaqGI5=#2Iaff3uwLjHSUC|>w<^5kg^Z`LiIPm&e^CgufQ`=)D$QA0ZMLK@Ha+~aw=7c4wn=hg`$A?Se~F(m7zJ+Nnk&Xn(qX$WYHMF0KJke)uu zYjS{-q%@vwb#T}}$-#Q?b>w)rOrv4PAyMs9haA;jFG=gL8c$PHef6W5fY1zyO0GGq z`nA!;9QfrnU%;>9x<=rl6-{g`lp=*v*mPAniyHq>$Mhc>=wFlIaAD52K`D)q> zd^LXIO7PTzv&fYz+H`#oxy^9Wss1|}`I{gEc@wob zVh1%%upi#fYKk)$ml-<8<3hy1``rbC;cTb$wt@H!J47e&y~bAn`vv@ct1C{ML0ib+ z3P??hqa7tto9ce&SiOPuE=>UDW-^2VqRS|d%cNQl+=6;}*g3lGi#wI_>0W?n_~a}> zES|6X4DSCH3CC@8wduM}@w#brM^1rxxTZ{7xhQuk3q`kveRqgZi#_`)?6TeZ23N2DG5&9ufLwYT9)^%iCLE{_VAOm zI*lh=4{d>q$4(G5$K=OG^J}`b=Wj(G#V%K!-zX=Us*@loB(t6G zPJk`}`6ScE{pi)0>;#kE+paeT2>q>6g_}!)DZHR8ph@RP4CaQH^PKKfn0rp$X3O!T z(p^T#^qQvZ^Mv@3jhU!T6Ij>oEBggP*Jew9C)thXn$g6wSZ%9MSjE)evu9(kcvu{h z^Bcc28xsIXq7-y#Giiiav+`!$zO&>N^9f&)BT2sG3Vr`W+yO-NK&W z$+m*Ro+^i`!PXsMMbvo5?`n=BsD$QGK>SS4jdUA&5U*0?8vkR2S2$zg?={)HWpmtt zhu(W#t!3dJrm4LrL+D(4^7#>u7f3RnNLtQ`XD;Ab;-=CD+Go35HHL+IcWHwiPt^nCO-qpu0v8-8i z(-id=yR3iI#q1hO!d-^sv5n$?2CcFu`5^|cwND{92d{F;o2ld*YT>hMq*NPfBer&x z%0S-)HAxFb8uDX(_y?qF|MmOuJX_fL}J#kUC zi}dvH^tr3>^i`g3J<-*@sDP|JM}JPLtmHOZx#=|`>=Hr0HEI}19w9nOc;f|CL8 z88&)#`%j%MOVPA{ojO}V@E=fh>B^rEQGSLFw^d~;@eAGaJyK>BJHc_uoKp8^K%Eu1 z!1uSoM3&VyfmC)n|1;^f_k|7pXXU+;Hy4*>gipg>8smkc}U?-Z@iZcmF#t+_vf>qa1X?wln-|@97a6vvf$1 zwC$3g?wzp!BkkigfR%STVT%+N2`nILvSC$?wb?$hX;n^D*}_FK!=q@~vq3u> z`)hCcEF9u(-&|vnAW)G?HObin{l^lRV<`=DtRM=Ud~HB`?s%6IniRcS?77*Fd_LmA z*vtzv2r1IO<{^YL4C#o!YiINE1EZk*EB}r+BOv!xt#qMY;4Yg@!D=x}(Nc@5c`MQ=iUzX_<BaBw>Y18q=TUT_vSg zl#&t$18B4@?ywS2$WEt;PdL!Xp5Bv7PJ!==Qk|V+U&y5g+H4UmUhR*^F93F89b#1M>Rsx?zyyQHwJ!roP2yh zfOKtlzNuxej^GoheOGQOQx0;p8oVRs#M8|w{Kzcv+-(YkfS}$Zn=Y`&2kV$6xpt4; z!d3K7*_a%Q;Z6!&u+EtSLd~-&+%}*OvO;Z%}0N4kBfC-e6+c;Maz_u5h{_h5@;N3A{pY3dLy6WVR z$W(I$=BNq2yDeS0FFEKl+8!<<&;OW-s}tYDt{n6~+B;n~Ax%^A!J93gp5?y-H`jav zPY^WCq@R5BLhEyZPcKdLuqYZL*@#`$aULQ^w=HSu_d&XLSp}=;?&lz+uCph*yNs#- zniw&~LQ$Mv2Zece4qtdh+q>zCpAEELc|&Rp`8t}V)0_0)@|@xG8C68$5kwMXL?XX4 zHD;6EHH{u7$P;=#7w2F6p?exxw29&-YGoPhhAI6Zq^2nvNV>eIUSHxnc|&GOBE(mz zSgkRvf^DVj*WSA4j$w*PZ9L0N>AH!UIIpj`^HaI8*5ke&wQIDw94qbK*DAHfh2>Lk zzn;uii*QZQRcDG>t>E*)%Hv;8E4TNOY&mHQkSl)ejVU}X5=B;4I*%{U_Z}1ddxd$E zrBthkVb!ij%>#=h(*{Q^&7qrpQO4kE98a<$JIHLJApb=cLI?%=&);Sp%m0V)=jDhYZnNk)2NE zbRk1DS7hg)a{7=Vnk%x?t(*~Lh~~=1^Evx+Qpga^m5t}Ka^{dBnkyU6XXPv)Lp)d9 z%6S60hSy03U(O$pU*UB+m9qx9f!3LbeQ1bzPe{Z*bSv)(uYYkqOU~#2)v23?Y1}bQ z(*q^)=1U{G-%;#A4D)`Fh&^bIYCjO~oqXvWivD$qv}bau+p0)=CSUKiY7=SC4{6WjQnw9J^-Rh6N4KSrqI~bNYZks?52H>P>tDdSpLr(3|%m} zCE%oxJWv$niF)j#6l%PSHiurTK^h_U_6fnv{J}u$*jMHFCxLQ(HB{0WDD=9})zKh_ zto!Zg%fa94V^zPadSBIts^0F758gG;hgH2a_xk$OJZ~S8bh$Y{U*~Buy~{%ud_M9? zxmhGyFgHlxk-)fxWNtW#7Mm7T3%gLiMj_P~wr*SOLi;g;aOFbJPfq&=P=5mUGVEKM zRcE-mk4L(H(%o3`$WtQgq5F7ehoKW>HxSYFMRCJU$9|NW>@kp?G~PQ4$ohDlE_WV7 zrr>ot+<6AMiq|>g&JN_4c%9T~oXRESCSIq@odj|Vtuvy$uY{0o@j6}ZWRMkjoepO*sA-ZE9yeP!a^*G6>jQOrR-RP(`-J4x-F&&tVn)vEuqDzFHx zlM1H+EJf>F38w`NQ9qHKQ{kKfL)1?srzf0qV2Jik$>|EG4-C=XDLH-NjDX?oD-+?& zAVsuyO3sCF7LcOcJIQYE$ZfRFR&Kk+JSVhr+YxRyD8K=hM$E-=XAt)12+k{{R30|NrA-U|_HSVm2V= z0AeNv2w;Ws8KHbexHt=#4de4sL4OC5eoid|No8WINKb`l%YgL6d8($2Fci{LB8%%sY8z8zt`T+?f>-rF4nrwJ+CMCmG@`uwb%OY&;Ho^%i46Um8qGMuI5}NYtyxJ z+_&xAs-a}J(jp&u;4H?NKG&J^xt@@xHchBOsP;gqP{iT|qAI`c*!{X>XDqit zi%)0q?@3~xwy3Hs2%0I(b^bK}jtPK#&_EF!ctMPR*91TwXrK)MhVy$*0Hixs;y*h95dM(4abVeL5@`Krr+EHX736bWuViC!Dan?nZ=-a?k>NcuGKXYR z)cs`!cDpL97z{Z+xZ!w&-Z_~xJ6|y;p6WXf<5biHPIH@TQ{I75jtfmgf%4(dW0aN& zp(!X(q0`)EB9yiXptCL_9X`)#Zi_O?uL+?@hmW1+Ua3y$pAd?4_!JzPOZg3;W7f$P zv*4GLFW5m_$USa5#&QZ$ve8RNQ$Vh5Cz%^ofSOIE%TMKADo!+p#6aXRCp-$ zH20(sN-Thmbv5L#H7rdqh=slyK1+PcrMZssaKeH1iozeg2iIwC`3;mK06KR0o@v1i zS8f}{@N)Ytm9hx%##}@zEUX)0&-rxxoMTdy%<*UPHe(@YH}p*E;MZ%7_Y==#W`Gd< zDfhrF<k)0FvEl{v#18@LL}SfIqv93x{9^$bKpnG?%+#>ffGs59cp;JIr&IvonCo7^ z;sK;lZ3M>hT`y%O;I001;y?d&TW+Id zje#FCNH2w21*!qr_Rnnm_cq{j4N}s?p}m%KsL*$nop@2rZ>NrX_vI@fD(&?|!OOiG9?W<`SgTC|4CLNpM{g_VHTGuf=nD_x~qG>%(KT#6JF2 zfHu~p)Kwq|J~P+CYu>p;{xjpwmRQBi3Z{i+vX z7Hjg}1>Z5&*>+T7izYQC>!;aL+E0XBF%aBSbS--BbY7h${|rDq_~MRB^zY#MPngc*A>5?v9{b7_Q;fuX0P&^5Vmx~l-K z5O!u#k?7z?)-{-R1(u$PL>B<`Pc-5OZ4fh4l7vK;#_4YBXxAafUXq4H=L7V+w3~oF zcIEqLXJ+Wn2ZM*Kd=4}5%I7ygYlVh`vBScpak#%btsNQ;wkj;#0KlKqK0w34PKAZ@ z;qU-6+9zl@*r>2@6#&0N`wR^S`xF)~iNgaGXkVe>V4K3i=L2{etqU3sb}1}e2*3wv zELiw;ly$Cw!-G6&J`-kS9_T?MLZ*o>7b&(G`e+3mJSpT7Oe2z!iL6!% zqSacBTP>GQG~zxBX5>irMN=4DM%U45z!`<1BiR>Ca8uxVmsSf)N3$=y;4-R1tAnMZ z*%t{wKS8?>OGmRWf;gQnP9uI40Wt50_C*fRcheq0jvdjyPyxN0)&%GiZB($AknD>l zr~=bzL|z32M=6d90G>tTK#v@yI2r@^C~X)Tj#3=CLH*iA8-<3W6h~zMe~WQL!%^&u zI1Uf8z<8nIDE4I@fR|wW&~Oy{Lc`&~GcY2Shgph9_GK1;XJa(jkzYqy=Q21vgc}ot zg(KOQ1vot54aNrzM;SRk4i8*`5zp|#%(*to4xItuH!%U|k)!NTDI6Z8f??2blpU%I z;QKHk0H5e`k%4Mo3MqI&cRfLS4NC`e4xPUgf(+hGoc0FLCo)GFi00_)vH4tXyJ<|= zkt5lcLhvM&TQ`l!++b!M$-WeVAAivu$s2VTI+A@U1U~|!KO*ncVdzNqr4alWY^xD@ zs}4a&voD3>EZogH*h@(EMIDqF8*CC-kQ3D}0*+D~v2u`f&qP+~$cb(ktZz)^~$5rB7MbD-fU#W4bun01&IG#sTk z@&I@ZHV+z(QXI{2c*s0V2O5rGUzpPXya3aMh9lS)rYjB)6~^?T;RyDHISIhSu?4Vj z6#K$l4d7oeLufe4I@iYGVfGmD1ouRi5o{t9`@;0Y;lXn;;%yu#ILgQ|06&Tuz)lNF z`odg^!$X9yh0t)69XcDp!!Y7OKd5Oz*`eD3{4-_@Mm|wNPe-;d!gxWa50JqT3?0qB z%)#kfJ>ZffFm^QiG8NEYVj{3J17;oG94*4>?#nPSK%dAQr6Zany~W$O+^>^2>M*m8 zW?!b`Blnn2-l@aT(d>%^pzkAZ)gkDJ_C*J$dvcTa>M(RP`yvSFK5#edU_Bz)m-V22 zox+yFlo$ezQXKU_6_|uAhlZor7tnhlWH+_~8jeyNK_7+?7Pb-^j#3;!Pliwj%nBNg zVqZXihR_EX@r?CEWt}*oDE0;PY6vsH?4U=EVqZYthOjdj@kTSu$dT*|=;079iLHel zIm!=@n4q6Scr3Oa7LH_JKyQbT&6q7T9A$@sJ`W-HG2$^@m^nwXFQzy=R3BRdJ#v&C zstn+#v31aJlpX4V!^0#ndjOxPpr@kR7nT5C(A}q#!4V7{$-c0_(~R!>;F2RSb|m}4 z0*^C#aARt)GlOJbSm1d^4n zl;XGml$b@B6EqyfzJvn!Ma%^nj#3=?LH&}!+@Rqo#St{42usAaLc>w)3usIc_6zfX zhNIXQ(4->V4ci6{N3k!UVMX{8YzHhH$-aQ*6%i&F(HCr@>oi#BNcIIZvWU2hdBctz z$-aQ57NH54Co~*ohk^zdp+B+h&~TI;3YuMnxnet^;V3&4G`BfyQkZd8-aH?@0Lz3p~-dEtI@hpU50#BAO!w(`{VaK9jfV5Fx8H-iSqJM8$-b-s^=l9df~f)o9HltwfD+@0g+RkmilaY(zr@0z;V8vX z5Y(@wSOhd2r8rswcrg|Q4M(vr>Nq??3EK?~N3kzE06YVWg@&Wp7dZeQ!s1}zNcLqj z4v*Z9B|yVb*0~{ozs8ba;Yjucbnpnfip4;~QASS1;o*wd9%wkq4mAhxbSxekjvZs0R*@*oGwo_yh&LBnjER5OI1a863gT(d-L&n$hz!RB}X&9nHRg#~HWT zV;f;-2F<>J=NY%%!#2ax(d-L&pmDn{d8-aH?`ZY~JkhxQ6nU=>Lr2SBz$1-2B*>ff z3Ct0GR?!**MB&dI?T97s)geZX_T~`_Jkq$M7wTr6SdU2d#R*h_8CWJn6(Hd##nA-7 zv#~5_I7)Gh1|^0Y%Z7%-7sq1x>2dh$9znE&B)D3$ptfF2}Dy9!N5ur>LT z1U-^=4VsQ%Yx3t4^l!8q&~yY_lP?75!5hgwaWLy0#n$9&5cE1)DeTx$ezKHLSm6*| z+HF`mimk~v0%H#p8NcV$e^&x+l)cJB&?Cs6Q!w+6@@!20G=lz>#(_;ofK|Mlx^o^6eL%(cgb?1i>*j9ZZZqT_Z66v_Y7#dOTJp$cI5dm71p zXmA2xzrqU`1A#b0vL_mx0NAqd0^EeaIkGProB-Ih@B+#}AYO{>jRq$GHZHt?I3W;A z`w5m1oB-In@B;G)fo>WbUI1)gcmWy^NN}JHKnoBq2VP(nA#k5I2rq!P4`hHqq8@Ds zT7d96@JFx!2*gbxUwnl+$%Ng37vLuZ;>fpO;RMij@(e_JwYX za0wHJUqWckhlubKWiU~A0W{|$3W`r+0wxYEfarX*2!TFK0$u>k`6vQ`Bxg(tS^&}c zm;ixzQ}Ufzs8xjKd>pt4fkN`NSvUcto$R0l1QO)PcW2=Qkan_z7$J~EzCH^lfV7k4 zA1|IO%$4wK{5NR-T;Koc2Mpn{OKcJUP2cYN(&c}hs^aGv9XaI_i;Cvj2 zOh51unG3+sQJjwhnduvnp#T&e!TC54nSRiD$VA{zM__2@<3Pmv@i)jOY!lW4LIA1g zI}pi!!ZflG+gS4x^^di}J9r~mt5gr%S|wzXo!H=pk5(^Z(heVI=3{K&1RhOR$;n0!Yrsfe8AOu3)pF1<;(21CjM7 zPsKFg1<*DD5%(vjV4BbZXwJuhNcwL)c-);Q1ZGX{GoR22(RRK>R z^C~cQ6z4-k|A8jsw{81t>?qEM$o&H^lWp6^y2AWLM{zzx=pQ6Eel+kG9mV+&iGNTs z84W;<9mV+&aeq)hnG3+sk(>{a^#^YyLjfo{it{0Y{@`bjiNK#`!tl#OIkW(x^C1%d!~^7;x=^bK(fJUee-aP*vM!te+BPO~|0HkneO)*K zH0MJ^|4B^pm0dUiw4F?(|H&5PzqkA68VUL`qFf2c^gn(4j&Fa_QJfDE{ReU&^C~cQ z6z4-ct{=2*{ElybjUC1L5V?QQE3)I;SQmi5=qS#I2>pYXkL^YArk?jRWSG!dGN-B2>O#~Wb3yH z>j5EvR`kiBKZtDq1}A`4GKru+se^0*2Pc43GMRkK1%$bh?Z`H8W1Zosf2vFVreRb3RNVAdsv>4*=J^0hc7+ye|5I9D5j)N0GE17;kAVrw$9tS6YRx&X{AdKuF z2Pc45GFK7;on#lev6j#uYlZh7N3vF%YjJCpx)vkeMTHwan)BHXhM!u6`9odSi6w;Q zd}M(@nkE(qEr95JHW31O81Y6b)D#h~a=~wcg+L%(03+T>g%Ut?K4Cy0`3vR;Jw=Gl zhXMpr)?)$i0%+TKIU!I(HuQp zP>huMFdczF$`-QW+=TUj5J0=#H6{cekWJ^{1kg%m6c9+&CmYYf380lsZbIM`*?bO8 z0Ig)20)aGfvH=~O09x#$ObA3_Dd4g-)(-e%t?9Z->@{Oiz2av(43Dr z7=HQ&ECX5q(fO<*1ZuIp@B(O?U>*?2(8l&b3m`flUm%bwfu+I=pdA5?5ZH~S!waBo zV+%syJJ|~iY7=OqZR1%$AZ-)b4-8HKZ5wYR1nS71U}NpFKUac(+9pFj=fji%^iaX^ z8`S+pM{zzI33@1GUiDAgWZ1DUAn2Xrx2XGT>?qDB6wt%gkuB<=<{icP@DubJG8%xQ zqd1>sfF3@N%mtw6D9&dFK`$Uf0T?=x^VtgM5yE6507XY}`cecvoQwk|+^&BbC&S;3 zt^)$8iC8XFMf=k{8D2n_5cr7^P1E3x0Ig&O0)aFajOd#NCxBKmg$aR1jA)$(CxBKm zt${$gAx3mhgA+h=K640xa~RP;4Nd?p_SpplGNiE6U=@wE3I13syze=ZwYpt{TdRy% z>@4&eMRYz}h~al*M7Jua`9aKl76E}w2aIS~1toy!d?JBB+9K=}^b{dxKD>m$1?&vG z0NOTQ0tC{hVCUck(6;e(LLd&i055>Hjok==UhEPO7;6*!bA7*#*w2S40q9}6WMlMC5yowk*n0#30qXX#S;$$a!sIj9sAAf=#O}3(kqN6w;L4y9Bj0T|SD9*o7y4^t7)!=1>;3W|>6e4GgSBe=ZkIv6{O^DzPR2t(`u z^vs|*pJ;-99?Jpru^I&bahz!Ve2V2I@R!~K6`*?9r$P0wC+P^r zM_z)U4?^{@C+H}~M_vcegFK;n*pqYw<0CIf&|i{0>?gdUfO$tVKJw~7AikZ}096hM z0i=p4F9!q?Y-x?~0!S58-jEQuLwgJ_fK)N%sX!oc7VRm#08+)2S0e-t(w@T$AQ>O| zDL^2Jm-Z4~0Ll2s>k|S#wAb(gNXAEAln{7Fdjl$>t!XoVLz zG-uV}7W2eO^KRHWPhkf`A880N6AKFZ~|z~M;8br3X;9V-~`Z|k1!z+Lbelw5A~_!- z!gaj|!$Hj)1vYPNJ z2IvUR$AO6c18N}iDnLhYJ`UuwGV^G?&@+SJd>n|-Kd^w-2TezCJ`P0UA0$jh15oph z;Cvj2xIZYI%mtw62+qfW$ohl6kf8t!9mV-L5J7*iJzP_9ZG6q3I3EWhlfj=-jf1oC6fsHlceFg-jf1o zC6fsHlj6xPViR65`e~N~k@%;aA)AUpO%bB=AwvJeL`)m%$_`c$qVpkg|HPlze0Tx0 zolHdkNiLY)c$ct0)(W*5ySgiGt&$os1Nh;iZDS(+Pd3CBjyL>2&Ifc1MR7i4`X5b3 zRuf*u03F5o5Yd0&cgVa7&{3QZk^2X2#xUraL2*7r=pS?+6N09rI3FVM57sB60jPOL zaXv)cAAFk31)%6C&WFhQLnO#h0EUj_e2AbwBnGajxHi6Kkem;Z@`rRo#sOeOB03*3 z<>$b4y$8cbbv|Uu?+MrSo)kcr!l}uWYb$$bDEzmF!!*x=C#+}Bgb(`QX;xygTtyWA z-(Y}a69u@SiOGO?Ihvmhh!%gQ^S{qG-Pv70M1bold4`mte79=yD%v&jS}f99yGVV1 z6Xm5~m5|+k=Fp3XXdabPh#hS62k80N`B|^TP7?JMqV#Q)jyN*BM@Ht5Op3a{%)ny5 z6@wwi2R9s#&^srSX6GyB#8Z9eVVsJ7E;utX(v%-ynAZPFjZ5JVznf@4*)#AjODXb> zT_jja&xMwsGf4W+;>@e&JI=k}29@}2JhuM|<#WY{n(=Q#-R-I{##j%4vC2AA`o|w@ z9oks$;bUDmNVNh`tA8D9TuSI&!_owU*#DgXr3EVge*BF%b51r@9WYn@E0fPPNJ)g9 z6znJL%lG4dPS$R}v1VWom;vh{>hAGoAhhmZ2Mad-zmN7lK3dJU)VULlHUn|AsbI9W zrqp-<9&g3=2L@1~S8N`jiod5253P=~VmS+MA*|RMd=9iTs77F@W9@&Jo{aum z4NjA?D^+5`f$~5OG#(7}hXJJ&5GNQY5Bxxrh=IPOP6pU<3cx}`^WCroU~>b$oV56; zM9eXM0SE}n;DRmyC2_=-+D^>`gB@$};|1W~20LZ}*!%z=?OZFW34o3_+Kgbd(WZgX z+7(fW%l`ON3td|`gW9Ty-{3~`2~q)uA@)6aZtdEt&Fwnq!+jS*34mnf5et8Tf26yxRez4 zvMY11UcG&L-W9{Ew*_zCuJkJFxvYLAZrEr?V^?r@EZ7aCteh|{T|`(6WlTUKD)E1 zyh1>|Ju-8j-kJN;S30_X2>Wtq^C3g;^)8Q2dd?~Tnxxy{ChRz9#MY*3sfKSX&Y2z_ zT5Lb_(g-IfBZXeHl>LBSWX8TgFIvw2N-r{HAEp;AW51ynEn(lH7n!pM>E4Uk33TtJ z?0a-?Gxiy}_j2|py0_U3?ayEe>N&W;oumP0 zP;r3;`&xaJUUQQ&r<87z+0~gn%v0>Nf}L6)HLqDqnRA+6k=B)yJ&YAQS+FnGm+$Se z&mNX7cCuuDt1nOLdX+s)DQ;Q8&a02oY4%p;T&7o~ca>xhixjt5uy5B#={I*PbIRxy znO*(a!`#JMs+>La=%g;u>|ya@la=hc`VoWX0wqp1Jvyb!HhWm6*u;|ESwCXb?4-m= zrAH@sX=M*f6<4fezp5Wu*qpO>Sh%>tf?ZNSqSxG_%(+dE&g|;W9_B5MUct_+ADP!| zqRcr~Y_sl}r!*ah_?I?d6_oFe*Y zdRImEuxRn91-qhtM8A1dnS&*bhV$J|opvoJxxrCBw`?b!?U&X3(A>R9EVs;+&R&_- ztd4&%DYtApoxMG)`6B*>cy5_HoxLfmc?bT5L~fZEogJ3d{LS3m8~+{`I(vCmvmE{f z{yp31Z1=3@Jp2p%d$!Wq>$93S<6q$4vy09S%4&XV?%s`mj~ktBlhtg9e}R9`4m#T_ ztNAwm1^zuAbhcwwb2$D5{ypAwc2rg~$J|{}EH~1b&Nj_zrs7{r%8m4-vt6>96Y(#^ zb0g_=_NuIAbNma5jps)NxwuA09UiRvDb2?n-#c1%%=u(eObSMsIeGPD*DG;V_dai! zWVxY|B6#qM^Znh*(n`B5U%N`o#%9mndQ3Xb;L1UD9SQK1(}Id7Kh5B^0B7#^z-ue@8Rjsis78A9B!?Q3}}s7(HiC18nvM{Dx@`P zX=@a{HEL~Z6#n0>TBCNh8msy!$}utpnRQf_1W$i*497Qy(;LH~#c=RHpQ?=XYc-zX zQy|N*6=a^JvefulYE+gEPk(L<=Tc>4P;1o6)~M~R#%ewVa*Tq>i~f<72tncj^QN1 zz$KlgFq{+_(V9M^5v?QfmBXnqoE)BhEh@{2#;lNKw1_dH=lG0ix*q_RwC%x(c@ zv@By3A4+Izlt*ineQQ*3Ym{wkly~b$YUOZF45x^v--*iV=Vw*Wn9*{K7DdJ=KBmal zk;Ka3%oq-D3W`>>tMZP2)MO)sZXOjIaABu4jm9G5fWcqBEK1k}T?E7FCKhU9f*u zJf|w2^SOF>Z}srw>ftNZ!#&l*N!7#k)x+nihw=Y@sCxKyb);3B@l78^dq$=fQ%#a3 zA=ux6^>+yNOJM!@pDp4!AF3l)wi(~>DOk_2)neMtX4+~p@j25G?B5vA>8c*yUpPi8@hW+9bJxxr_@8&ZW$g^#GNSRN(fVj!ez2*SYC~*Eyl~&> zX6ZOTCrSJxkAEiNA6fh}8UHBYA94I6gMXy(&s6*)fq&%i&lLQl80Xg_7B|>kU7mfD zE3u}{mrq{YygJKQQeMiu=83P6yhL5~RbP4eDRniyzC8J2YSqcUlkz3iYVP}D`IF98 zpZAr`pM19Fi!UW#+@m_zcXGazM@_S@NWMg8b*ZmH{*=y|-@e=qVpFQ)eZ?Ilr_|K; zhBNpylP%kxGK4f!AGZBq@DwJgw>@BBg((-?zA`9<$vfKKFhrshqj!I64`Ud6?Urj_ z&Y0&Fo7e8nSl|`2xqUrD*K5z)_8`V0)^5Xg8-@-m_IA4$!;lpd-tNfIXYJv%M=?wk zc2nC;L*^>PCbqkT7%0S;x33ylG9<2$Ue|gxL|!3Ntu;Ah($TcDt>;5zk7jtZ=7vl@ zn%>!38lrGCb4qJ`h`3`~PV4Cq8OIF!)&n6@j_I#jZ-h*B%+zm950QAER?>PoMDBe? zaO;tfDeu$!TgyTe-)D-p?imm>PRndPIUsGEVcVKLAZeW5)Ou||-Z)dMHFaQ8d0J8H z#R1v!4DZ&wfyw3R-L1C=6v{IdTN4MwqtXgm&ko2$WjM9w3`j+#x3p&Vstv3ek{OYY zj;Xlo)K=81Kd^ad>WISV?&!NFZ3Vqr1NK95BU49XM(;+qRrFSLZ%M4>7D=q>5HOgQ z5jVYKrorrtgp(c82C5nHZXE^&ni+{59ef7LHgOUiDh9J`60$oa4QANHuj!a?FvlkG zS%;9p^u{=y4h;kK#)NAf@&;;+@%|k~23n1YeH}auCCxa&j%kdUnhB{L{XR5@NjcT0 zePtXb=hS@k<#!ObuRh=_YJd4eGP(5cru(=t`Sfca z`S>$=O733v*}uiR*@KPvXFIBiYOcRkpo4L-G}sTD|H+vK6765pJ-lyXxX?q`0IL+WhTy z*0L&Dth-+PauU3TS2yGTyJ}e2U{=dwGV`0JGKRa^yIQiXR3_aw3Ew}&iSC{k6wI+`roO7w^_}A&_o^z;caejlexEJft{l6#2(}<`e-^PO=+tQp*g1Hw0#WWcTBc#dl4es zSTpu1>g-q)qV+zpzk_!`$v955WBR~M*Jyrb$X~7?%vKV$W_2E6#sw+pV=A%EM#y)x1k36C~@XB_7}2v4#Sts+Byuc=7(Umcxsh#YR;-fLIyIg@9KG2N4z>)bdzVo89XWBP4ZYfx|t;TOb>BYj8A`ff*>@5x`h}l~$Um`Vq+ljBs9<@30?t9p@ zMe0!-ZRV7>w-_~2c7-oq&XBj~y!$mQT3E;Xs$*X1>xP$n;;$N7bH9Axd`RfKWQ)Db zUmWxNZ2g%efnx2n$O4-;ZY6!pRqh*<{OVp91;1F`KA>=n**yQ@yHn-b&pX*)zEt)F zEIoR2n{dsFE4g%KvlXjbX7l<9vkUcZYI2n_%PjjYW0F%c?@UYdfBAyDG(}!>{TAN6 z7wg;h1s1t5Hz${ul`OY8HvDdMP`qC~Tr|3UQRG~K8Uc&zPkM!U_SU3-cB>KGV!Qdw zES3GMF3qoGv{%14%b?UrNfjEtS?cVts66<_l9g5lg^q29*SOaBmd0DIt}gh#@I6Pp zAYA00m2&;+R~IyRbM?PF*z7C4+cq@3wk2DE734i|E06N{l`pkmP~NS(DmO9ku$|G0 zQwkMJoI~{1sU)}jc=WZRXNV8f0A|kp#&7kZ3AVe19QjM*DlhUm_()QDH~J{t4OI=6 z?A+LH<+x(9<3QM*pMf7{pQzx-zxH~4^;w2^ztdvVbLq#Zzg}_{rG%eJTYB1R|LO=V zQe|+_v-^k5yI= zEL^0#`FmVp?S&l`5$nvpNp(32>reNdesBJk?!_{x7eh90jvncDDj(|0(Q=9!em^?! z-Xy%cV)V!GU}8~cMR@n<(BSW@BOkwo8gqJ|47E?GozHI6ezD}Yw@K51lgXosA)iGR zmT!(4G&;O7r*&Dk!^U-oe=G<&_w7hY@fF>{Ydwz@jnc3v5e1*o`pu5pn4PW1*_n4v z-%N>~d=&rxv@p)SzC4qvzKe=ILtnk)Jobm%JiWfuxpLE~%!6mXe7q3Q_smD8HSSqS zH$!{*bQA7Z>k`s#%o<#KP1d~o=hvwTJv2u7JNxP1TRMC0eI6-z`0Jsf&+rfLgo|Y> z_q)jN_~5zcOwlu$zn-y7-vt;wALud4dUC^dZ$)>@fjc&RHc|bBR`;BZxnGq; zMW;B0i_w36mhjo~aZliydREPaVMfuT%MF9ie3UCb{*bx$TV;{wTVeK-Ju@pFRlM3` z(#9FoD_P!t(PY*O@6o!^>!l5^#9nP%A*kr^_O|H5rrH;gbzA2x|CRpDDVlBKKm5vM z*!Wf5Z&Cj(<*%#r1b&Ov3uO5HFls12*16@zt)OQ4WSQ?q8iF_d><+%~Ub+68VALgn z5}9Xuy>4e*%TJ$s7e$?SwDJ?5>VX$;I=^K}^*-uqY1tCKrq4jnzsiEAA&kM^XfA$v zfbS&rNod)*m@A7)O7$N(&ux2kc*9Q7H-`g?Y~5;G6k-%zisZpyfpFBU3x~jEszVIkMZml(Wuwev>{a)pIxfdB4<3W!+h7yQv5BmYN4{ z_!N|%Fhi4e+sZpBMt4lh>zI{lzk~a15ja@2pA2$sYmv*-wFOVr9Nqf9<~Gd=-hAkT zP2Jr^>lur5`0kblPXDxI%ZIx>t3+((?bzs6;l6>VJD@7zTlU(_c)q3YRn{b{oa%hG z+2Pm6RfhvgZy(i6jDFqrb&;ULF8wy{^x9^0_mQ1vkIo)#3j3 zBgFrTY`~^K!_Xmz85|Y+M>(C>`(i`f&Sv{Q3vP7W@?_KE8>ZXj48kY9usOQQaf|C( z$1rcrsZXu$TW#K+#-&;D_(Q_eeOn4E-P}Zf>bA(=E@%(AeYiaX6Fg4M>?!7w{I0C2 z-(z}iXWZr{{`D=^N{goZB5UGFfN`uX zqYc*AV~x*8J-4`>_il-9^kJ>Sxk3$YRToz9`Aol(y#0p!JWaJO$GDEcGK^WG+wx*q z?^<5m>^SD+%i`Q!{4qJF-R{opp1ST*=Q6t*oyGH~{J3wv#D(>qC)0PDopSvHN^@=T z0=|#hVNrX}3h1lkd{}9+VDGHVn`c+%9>03vXAo76o9mXIQ(XLupJqZClP+Y0yqta8 z?dI;-*elmA3l|$YDr?VJc64w~eNawEl5vR{rK3#PwpZdu?lUFx=(aVVk63-r(%t2y zA)+AqEAmOgTz7K6R$ayt0-d4R>z;8jAJvUu4OnQ2M(r%{J;vJRCc8F{U zTCnlxYCs(Dxd1gW_YA@3~SBCfCz* zJSrAia`kg-SnX$h*)TAhK1U;F$yR|6AFXRt$`^iF^=*p$VUcrJEWfL&Eo^;UD_mK5 z^(`my0eb~CZ<>Jns+JFLmzc!3oxE-><(RouW+*D^^0~H2b?;ZmspVNlJa`bW{B;<6 zYrbUnQ=Vmgldh?GWq+Ky#drSOl*Iwv^nh3+t}L4`=HJ#QU68-`vu5G)XJ2>Us+v`O z-Rg^UPs18T&F0i=)|aLDE{5CPests8P=ss!OCOz%<=kt&E*nf5Vr-zjQ4!ZDfA`*2 zTMGY4Zqxd@y)%XHX3XdKyzCZRw!7(!+=EW>+gzgNk&Er#z2Gu*e{V2Hx#x_+VEcx& zxw{XyS;X{Jmo73t8UNZoGNv z)h?!A&u+^#(~{Sn2QI%BxUxy}%DH)O!~8M5*=G;T(%q3HIm3EZZ+ZOv=~<@O*Vg?T zKMPbZ+S{10NHQ8Httz=E{^y>X6>6~YO;CDbu-mNU2 zg+Kcf&Ur~FNkpuY)>oOaTdnk?;HEf1hTv@H%G&P1uTCuzl7i1ZQk|wIciub{mwvQ7 z=gn{IgsO&n;Q8bowLLXr@=F9Zu-9MVuav!bS4%1H`r-4_mn41AykXo`dix<)k7uDP zuYgM9iI+dNm((tipxHE3%sF7ud{uibcJ-Z)bNOIUxBIzO?5v&3m9n{~@_$~&oOE;P ziv78djb8jGxSa7!LuC{KB)n4we;fKt-mCGmxa%1kK6}%#cSCAFk^S?VDRCv8sMXm9NB+)s8FL ze|$;!+N*od&QmFZG3{3T@g>a>7jqn_=m$X|}H3mJ1k3X7l zQah`#`9kS5Mp^h5_Zi|mhZK+64ShViJGzf=#L!kc>XpRWUzuS%zoR>7)dg*Mb1Wm} zQQxDFhh1zJk4&?zXPLZ|aNONj(HqxM7gGJpp3OIN=-JW5L%y~H-o%Z}on~{<>t6$@ zJ#w>Zo0t9;$g)q~@OZO|mh{3Ui82o|76v4+?Toh@FFm+1@s-WhqKK(#scCUyIf3zT%q~)Ndqru`8V4JmYk7X51FL!l4sAWS zyqm%FY;>PHP1WK=f=+T&@7z@nbGhLS*@gpoCEGzRvsw_xGf0 z%Uz()^2n^IsLT-VlP{f8Q)#*?GV*(NY;*92;dk7p53IHkONliQ=FIQtj~ChMd(>uy z$$InsZFScV@2dH|R=mum;GM4`N7M8eRhh0c`{R+>8!9~lU&pj^<=Uyd|J;0C{qb5O z^?mOzw6C)%w*8v5%I0)Q$_;_+onaK##`f#+&t@$-88?HnUOCE9L-hiW#k0@4Vv4nb z+EZ)8&Ru<2a=4>^*LTGrYKQCQ#q08J?7git(@kgpTSu$KpM|by+`3#mpGU*R*KoG;#|b^pj0r@WL`LJ?2> z5=7?C&Tv~-?Wv7hi4P%Ba4*_7wm6p}~Tv!S9{j ztnZ3Zznv3TT^Tgui?*)eTYJZnpw!dI+_nP2aLHtL5NWYslv_Ilwn^B3$jdrVFC-2Lg zuCJN@Tz)q5>`TF4O@bqb29BT5Y4cbo)o;MR<<-_Bl&KW!At8r@RRYyumSmy79{mM@xy@!%H<>AZTExr1%b*^H~ z9UhS!S(Pf42IZRGh>~^fwW2>HCf7dwl%6`Lj7QySqy9tvXGOn^)13Xp4_W@+-v8D5 z+mzb^x4Vyh3}fWTvAeYzj_;zq=v>?QTPWwj4)=3Esu{ya%)k9|UVhr+{FIB^obOfF zST1rXK3Q+xlQ=x@TFI=l`#KIE+oDuzEqpM`-S;Y{<=0)2CUNya_V%ZG9{W^{ZA-%~ zjtE(HpYim+v}K=&OS+%2!D>a^VSmpQ>k3@7LRLOf=Kb;wfnT;a>B_1u>NI~bPosZV z_9|+Nb$#&^!)1Hz99Nn>yU`wjX>hBhS}$jnmK~MkDtf4mwmDOGe_hoJ zCBd-IA+c4*vkc!h{k*)`CPjQw?UXbT!}x7yUGHmT9@`@!x}*CcmUDsrSkvgR8FPSH z<0`j4G%hQ__Q}oKLTQ%+Y^(QG)U8tn`%Mp^s>D2 zPVBnGt0$q|r}XX(p4>4gs#AUJlmFsxYfLtLJyvU~Ia7yI-|%v#@i|4Ov{NRz`_BZa zhB`YYSkyIno=X0;&VY9Av8@u*w@3Jq^P_cw`cGZn6c_fKTFdNdy!pht=+Gr)^;-2z z`Ia`T`f%B0YV)X}!P^(cw(PdPnf6wfRPkT)jm1{(h!whY=FE+yta2gQ zCtU(^^Ry!lWakMUw(-4b;p%2Ad!%>UXo%nEMAuy3FF)8<1%6Q5BEN2H8mKldUaeVi z?ph&xf$F(f@s%SynM<$j%v-|z_D-?kP)XPWC(fQz-}X~&gX$H0GW&wIr790|)1*^g z^-8tmWvjM{)Ykqw94WuJOiFS{DNwJi;@eFA*IO2?yHT@Wy3E%hYp39+Jw_`QU6l6u zwA4ynCobP&Qd{2K$ytW)s%2Cy^uLN+^ptvI+PL~wP(Q!S$2ZpC1oV8xxm)YR(7`F!78Jm;-Sah6XTto!D*YDLNmjl=_qn!1;`%Iem4 zxyVUcL=KAOcPLqR&dFilx|MOz`{r;f_EvNEx|a@xwT6W%+b&i8v_B-E+%`8o*Cp1o zXU#KRj?|`^b{i}k<}fZU7&wt=tz$-e{p9I&2A@RmhS^Jhy~dV)D8W`TwXlAZJI-gG z4a(=s%)tWB6w+k^o7bJ0CEM>i`^OUZYVO(o6;95cOBEwzZuI0*?3Wg2&0N(jaMju> zN42WFa^W45H6fM!_w-amcq8`QR9rtW>Kpq>?9Abm7g=|P4%#L4Ze=NUBln<;CjWQ<8pDd|lM9(m7jCulHBP z`syl2-pkQC2NKUI+sAw`my^F4lCbWa<&PDsX05t2HLmQ%=|eoao(XL>anpVuqi(qT z;+gNFmnI?C9v!ckIv8?BprIkvx=r91-FCVDnUgPqwT^B0@*}LKN$O;kW3=ak*zC!ev1r1*?YUo$e!RQ*Vv`7VWnt1BC%)rb0(}?s z9&m8@@!Gxmrnkq;YVDSrI^7p{&s$OOJ#WUmp7k@ntXAS-U!EU)@+;eN&nw$z^Gm70bPQacWF%Q`sB$9ev@SjM9aiq);sc38jem&w1Ie zq5gL5?nvRomo?~xdwCktzkEqy9~=yAIn*R5C7d~!aLO}t){H3rn&e#5_+*O(M2lwpDPD#am;J#IS?f_Da`qHtrv#vu)#ynoSbYLX#;H_P2bhvMcoLDBPV3eA^&Qf)5kd&tI)R=qP zOP7=!);$~gJpR=ox6@&wj@eu$kHR(F)Nk)?eR44Im)jI>lkkBHPaFqXfeqYteqTA< zEW7Kco=ZPqXDuH3zNYCat?X4lYo=bxj160bujnm{sOf9IdS}zag1q0WM@~u1-#OG_ zvtr~e?O9X) z_EF35_^gVG&wNtHT397E0;Bdeb4P4#juo1;@%?NYen|`dFxo(S6K&|?IC@UvXhk<) zjp^6UvJC5(mFaVjJkD6^>{J}{>7|16b+v7|1E(9SqbALLASYq1rz`zn<&{r2R(E}K zbzRB2^#0v3j!M1k%x3ngtC=&je?M8?GwHj|q06yT-!$zOxPLA;UPkr0rXSDR<=zG! z-REUFQKxpZ&FYu0O*tFjEcED>6K@;W?BgPLa*wzuHeNlZ+Rn43mnW}l^GesswXEFb za}H|c=~Z#!T(&RiH5|!k-WhsQEJt5m&Ne2_Vy$OVQ)^lAfn{Nr>W|%5*3a19aO=6t z!`02FgKZ1;7a1|zzNvHASy7eu#eROWNQ`zpZQp5jB111wu|-mkDhvw z@FRt?dSTi5s>v;$cPLutOQ|&_y(=Bh?Gm>>?sTO*qUczhljD|{T6GKm@+Ace*G3ln zKLA8PyT1-3NtWbyPMH6|35*zQ@cN%l_{aaj3IFypC;Z!==LGL(PS7PouB12+7)nyJ z$mZO%=dkdeW5y>-LFeY7`&PY;Zd15>{4BTn0m8j!dA>0L$_@jRdTbJVi)lY0C~R*0 zKxq(Xs``5KLvu6$dVA4b1n(zw!=OJ{27E1Y6`u1U?M$ty6Wlm(89|S*7<&;|c7;W( zFbiRxQA=K6lvx=dPs|}-!nnsPL(2VWaAT5C4hJAdaznRDOa=joFkq6XJ#VU9-m z4R|4D4h`g9W&zLoywZH|!UlfaOyFTYTa4@mEK;y9`}ug3L~zjEtm%_{v=C+kQ+U87 zU@m!Lmbh=^YtnMc%EvWs9#pI&0MAnQHiu7IUck_0Pg-!MH(R_Q#Vkr!{ z&y0Te);xr8q#qjav)f&!By>5AJ;q0f%=UcTCDE`V6uZIpU9-MwT1|iB1Cw%b#U;re zPzkFDpInrK1nMF+Ksu+0BzzbQWi;|R|1t*$Q*FwJ`~uHr#afVJGR6Z+!+Au=x1Ml} z9SY%zUXXl=Wo%Bf$j5xee=fY)? zH1-pDgq`h7>L!>g(GCP!=NR~`MAcnLsM}x7kQC{AxEhE@8GIo&^C>G@2%nfvAGuZE z)(B9Ji!Iba%fvp!riDu}(aQ);r=+rVpkWo;C8*U`S{HG)8lV{;S@U|#Q2I#ugekOp767#yJ~>mH*K*&cT7odg)%U7>8e-bFm!IBrXPUUVipRT`K zH$&9CLxB7h)9uLHZGL!DqQcYsEMiPoo=lc0h@i-C%J2DHu;P+x7}w2*iI?S%Pk96^ zMDhad+N|iGgrQ`1MKpNoPkWV+3WzWob2}P&@L7E4;Oj%uB$+sY=EB;GE<`n{ZoP<2 zl#mM(O$#2cgK9D~)u8wia+d){T5dV~iRhaR>B*tE*YBl)3Bu^+p&;GyCDaGA%J7lr z2vc8`&C0#+v+|(SY4=FjlD$2=cJo@tZ!}0?C4E?Lw)JM4uwTvzY1Ke|k!$(}*r=qC zbfJKE?p3qZAU->LN3Z$A4EGfLlqdBu^6%1BMBr}}k(Y)CU4Y!DCz`}-J}1L+wir|r z$A0nPdKsIR7*G9~k(=B%B<2g)9UvgJviRw-`^JIM3mNAE z(n$P6{qg1j)KqtwdummixeS`(Kog#Dw5d|{V@Jbk4J z%)<{LLe!5@nlInt%sgP%kZJi7hw>M-Wq-Akg(Bd1?88@k-Er%73wWm_IC35$V9By> zCR8;%mlbNhFaivuwl8esbl1Kcy8mX=MSH28-~8r3z3}&c`=`C|FTeUjFZ?+EWiPBh zygcla#TB+Uyr(Whq;r;gggZdOzexa5B@&ZOz89(LE%#u8agEP@D zw#r{#BF|NLtMM?$w~JuW*FZz${Dw4fT$=PMZJh=^#IOswG?naVTlgAa10IaZu0mKe z>X0HCyKCXcWvNaWz)uoDDV=9Zh2!w&i-Kc2SrjU$!B2)TOjbvu^{)wnX6plY!|9Q^ zYd(u>d?`D5Z`ANS7x?ppD>%qI zyTeXwL;E*dmsby!E#M_$r(OcaZUMzrm~`pU*LpJhu-V_!srE$;zrsGLlklAp&E6Je zjRWg-zT|#2WNXw6?e@b}JwP=o_OY;rPh*j>+t~XqVmj|M@mt=*J3%{{M^ti%spgp8 z6#49RhA5?D;XlK)Pst0+uq1w2|9H;uDvxm8 z3W4p79KKg7wZHN^IA<^H>)$R~@n#^hR+J zzII#p^XZvfMRoKSNh@Lt5&CY7TG)^+yEwLYh+9EtF3A~G?hIy3`}B-F)+%v*d|^buR#;VjSVS7E{6aDJ z5^b`7z-Sg`RV!O0LB+$+WkMM)4DR_?!?y7Bd>W|Z@AL;6H*?32Z@Qexc@jip9b1~K z`7LCawXEJeU}*Aabdqe?N2R3W!gJjp6D`kUR6XP}$JWk%jW%w zd*!4;>P%oTAd4Y6v)1~*9}Cp{rAID3i{r?VD>%AFEP?t)Aa4G9O*H8=R4gV9Nlc{B zoEGr-ftUIf?`|bK!zoP=!iLCuJ^x@o9@L~VTN?6ji?IRy831_bjO$<$V$&2atTU(s#ZIiBx0i91r@z8Z+B!Bp^8K zLwitlcHp6r^$&m^dk=e zjY=qWntSvFijHO;AW;L0pU*f)8(05f1j~O}tkYJV)96l8avFc&IUOb7^mk4vh;hq? zg)^|MoXH&wqn-3yUQVM#o&jcL`02+d z9O~>k9X|T$T3g*)PZkMw+bvGZ6r0PkoJmz6+%$yWz+n7Eq8U;Axc^ePSMsYdueXdSGL6U`4aAv~(;WQUZv{ zrFDWzvoI^zwB=qWAlUXbLx_1Gc12)81=43!$^64V{M~>5<1g2LeaHX&n?L^e&+q(B zfBeV4KK|oB{mcD-c*pvc%gv=RVS^5G&3COFx0JY<1i@d*fMIthfHYIu{Jm_0$ecQ#fn%;wOsVL2_&O<@=0nr&Aay%`tD#_+3xX@?z*?^_T@ zpy6TN9ftss{=lh*s16n`)ToBU4LVzeLMjVts(2KFXXJ1Q-LlM4;#6_nb8XDkQ7zp%zR|(K*)e+C}tr^ zqrS9AY&h6$mmh=J3dYwjkYmL>tN$LOOjq=B?`)zY$+bt<~~QB31#6u+LCT%bhZh)iNYX z0qRn}DTE#!${uJRG|sL^Y(=X3dc~R{?do?BZ5_+aT-qTJIqrK+)(L~jxIi7?treiH z%4y;k(JRZ`rNKK-`>`NeeM$j#N1z2#qh&p z908nU4$i!8Dx-$Ab_ly`FeUIngF*RSZ}I@%@x?j_zth6Y>a1(BP}br+Y_BcXSL{q5 z*7D%QcIJm|W!LI^K`Am|fevFsyx}!4f?qnt9G0LJ{1ZSwex?aN6fl>9jE3Es`V48k zS@@hpoh<(e9LP#vx7(2Ji9589fIA$m^f_HRE*%bxo<(!M;)lFTbK)=_995zP4 z+O|^bGAVS^)kL3SU}O<>-58-jSz%FoG8VS*(nm%(rF?*fh_OQ%iSHDtWD^QERC1FsD^YrqeYr6d^m)${uZ>0>P{8M{Rmi6g1a{ z#Q1n&z|YU&o$`?M4O-Cv7e@`$VS>i=vVNDTlE|xGd2*03rd;g%-z(PzOUEpNtj7C3_H$Jqb{`Ls}@jgZ) z_Ofnm;(wF@L>c|nVXM+@?1+4})C+xxx~!EJkhA7G@vJU+6uYj7Ldoy|d24qh{ z)GoRT7gKs7(Q$7@po|wTNurUCdKtCOlp%=yNRxdD0zTn%@u2BSW_;G9 zJIq+flqyA#4+5QX5)9yr$kV=o|LVW-t z!8FWZYOIhrdQ^O{2VvxgD^h}S<*-4Znt#th$n#<*BZQdR1PcgRue-U1mhkfo6Sc^- zqEPbkb8-a-O~ArQb{B=c(!lR98B`Z7Ib}f2#$z#AtY5%R1Ngv%9*7XOi8S=*t_dcq zvP(boZM7jKqr7pTT0EYCBd<7{ziFtY?uO8@qlA03pIMTF6dyaSG@k!oIAi)hoFR@k z@kJHBhQ>iX+q-g6j`^rcelHr_WQar)z`d6jv{yg9+ht|~nPRWmBRET%0ol6MU+sGa zBA);l9x2bgt;4|3!=Ujc(p*WX%aSg>MXi#nkB?o;?|-W3hdUKSZX#R(xMCieyS)uZra}MJ3uw@nQ|Yr;#di zfVLT5&bpJtD2&eSL<-e5vtk;8Cm1z(G|maIIU>SsnD`Bz&ksIZ*h4;?{YkX*RQm=Lbqe1Id)n!wwXDvcCI*u>;#F~jbgW7GP>wZpxe!b{F5_Eh(=`qIn&?I zT!dzovnKh%-yFAkmSWdCzs9yOC#X*|F!EF|;X(g&Kx-Ar@Pwj9yEhBAwwhXC{1OrW zi>%wX{-O*5r~_Y2;387tv#-IZKz>12AqE~1+LbqDWt$v&@p|nJOFxK{3)@p+IIcy) z1C?5Ey8A%H>>D|XkL^na2Sssist-4Xn$&5r^ZBQhU}e!Cy1eY19?Fh_m|Fxq)c0#lwb4#m%crLfV5*ZfFE4lx5`~TeE+A!1gmk=7ykK>8W4x= z%z%WBv~o#B=6C2Ye*?}XB2wH;@bny_NzufyriYIYJpLdc0Mtw~zt#V`E*# z7z^ktO#J~7`q4P^niRhxeX;$?7rfgwmPSdXj5k@!vP;XOrCqx1m0l;Ny@=06YPfmv zvBIvUNJ)-{$(+TY2$SI6^NzGdlIhTnG$9<2%q*7$Ak1PLzD|MVkFlwf=}FW0=#?Tu z=`j-HOz(Gkwvov94vPx0U;V{r$(M6c5IZDAGwq~iqbIrCK=H=ZAtfK>7(HHf$X)(^ zx#)JKJeD+Prxf2Zv|hv0(7r&_7Qwn9VA41>=^F00+9u8&n}?J2HL7SIA1a!I$=OJB z_98~gOFDP{s3{x)RRWBBzQB7snn>&h2+bcwu+gosF7>9NfWA=>V#~9vpiWoA{16&8 z?(V&2ryh|#lk|WFDaxq}D6Zq2`#C9^hdL>#D6#%wkU~Nm0&3Vjus=yDbNPOrFCu=! zjzovUuZS`s*?9aAIqk(HFUUL+movJ9<8LETo7ZAXE}XL(?1IhN8K=#Uk)9&`SYMO+ z*(4CS5&r}~iN+SrMDOqQA)V}=#1bM$fJ9)2D^L;ui6nbH5FmWb{8P2tEi>Nr;t%~$ zj9h7^gppq44&W}BOJmp)Y>FD#xKcf4vSPK10jCXUwvi6vCIddtu)q9{TUt~8l00Gk$eaur&v^=h(rug7CQLKgO(ELyW7~WAh znM-NR(EZ56HsmBV+@lMmXyRic^b?^VlO+jz3;z#i{NumF8Op!n4CUYC4ERsZfIwDs zm0J*CZiXgRv%8l+T$vr6{8uWl)b4^R=1fgZ(FGVV6})_} z9E!1bb@+Vz2nhlt_wbjE*RE@m+_>liJu*@!*RB#7XLJkemI3@HmSsWz7S-y1w`Fy- zGU&Dw#A7eE4lXCu@7-;y{2{Ya8Bas48+l}7PVz_8Yyev8N=ItZiyAYoBZS4IPy;TB zTL_{`_Z}8N&F?(INvi7OE~jSEXwm1%`P+^rZKa_uh$hQcaWZLS<2hzU z*vlcC?Sg_MbL;`Y^?JSrMukS^-dCjF=YVv~qc$sql0|XA zyA)B8ybF#>4y~$Q6CVLl1C6`}afKR(L=*UhDZUZN&@hvMcns2y`3E+?c zz=x2u=Gkm(K1ItHtTT@ZV}idyN}mlN35M~CB?Bo2UhslxHs%dx?XxHhNI6qLX;6U-0 z(TA^4kVWJ4s$0^}>#xruqM<}vKe@N3v)~qLSmTJd=g8>f1Nc$JU4y?;E}n+ZTS;!x zDF02wNc-A-1cMMnkr&^vt2mT|RyLl%8odTrV-iTG^^Lp`wrb}wo)c#HuQ5F4Kh zj=H{0&BdfGCJ%hW8_Fy&_Q49P5KVwn z(+%ZxDPI18KfOfLowi{QX5pe<<77h(Lvb1A&qw(I2SAlVY`gA7Mb-F85*^D8v_<;0|EnL&A;Btg*EPHex_Sf%p-kM)DO=LcO6W-GD* zV-74P@S)ZU^-MrDS8M#)tNa_(a)WE3UQFiX*ESa*;D#q3+$6K$^^ZW0Q3I#QsF1tB z!!ayH(us`$ALZ<1t;n4Huf3h^`d_`BN0I9}LKV49ShV0#tHrGp2dwmInhg&DYn8wX4CGX>sbI z)RZm!|C>Yp_P_kw9P&^9ibMYC-{BBca3H!i+a9y+uX@<+IC157J-&=9|2jJ{DnRWd z$YvE=({!B3`r|~ZBk@VC`Zm?CEcf6Xz_MAYOM6A!8eL)Th&eb)(UruW{GcyPgbzK8 ziQ?`C+*1I$JO_;=E7z*yn_5{8xS(HmMw@^H>v({=dw60wfA(17q#$hreF~zPSMw2p zrinmAHc$(GbxmG)-qFc2^RJaHUYh5vR|FPlNVM&Xx*)WCy|bPIjL2PDh>HP=8~Y9t za%Bk>w6|SDR2F(v3!|Oi;c@)6473z(eJWZOsA6%+<;=2D_O;F6M)Q}&?kN#huWg~*;~`wr6ftmTH) zDxY|=sD9OFo5kg)#lIhr)@!7G7Y{YLFS6}n~&C@qs4)pwCx21zi!8h%Lav_j6 zP`H~P7ZEIZAx6yH-H0Q;zsHvsNCxglEt0CCj|TbzqJ@+G>h3+CVtK&#+ZxanyDoiU zyG0;Ma>^+X$cr{}xPGPAm`7`yh5PqF```+okkFGt&`M12ELb{8oFMBXJX#2e}wDQE0>}=_$on|7XJx8}p zV!P-w8c-VJ-mru_)6b(frNY=~COujJ1LVygH`k{TXFKW**$-xODZV%3X51AK+YX-y zrv4q3G^Ar`=a0yku{B7vGO>5!U!zB?saDb2mS_i_itNviQIVZ zdly=i)J?dfP9Q!APn42diN0rvg#xx_vAZzimhHUqtPz2J<+9fAQeBV>Fll7XGYdn3 z0Qvo#lg@veDJ@g7vQ62n!96a@5-2$}cMqZyI9^K&oo2`1zK#uME#K=FW@w{5$L zQQXS-;o^JgmRChY9^3*wNQQYn^bIj@Nx99KLPGv2iHACNtWwWXliC1_6zB)Gwg&AnIQaw-s!NSyUhU6Y z8k#s5QsOf}-ar12zyA7%zyA8`fB*eofBo0@{CDs1pa1&T@n8Nn|4;7`|NXz7|BV0k zxA}SxQ`-L1Kb6ZgxZBc<)qnrTLVbLx+YSlxF;J*I^%ew}`$4m&elSzfMMJ8^IN&Lm z>t;dte3g$7?`FQA2eK>{>FwsJ#pmygd>!-X)1hxn95Du4qcrG9X8BYzY1}(T_s21h zSFu6U>Unp0*jRX2bUa3StumD21W#;P`N(k4-`mulOk~>AEk)BkB-@0%6FM_N)~gi7Ni#hV!PXSwAK}I zBEOG6B?*}Arh*ej*iZwASdsp0b&|Y$c2O@+Z3xE&O1s7K^L_uufo=Zu&mnuS1N*D` zF+D0L+V9Nh*n#7alY8-d zvmy1y4F$CA$^9Pn3TH}r#J($0=8XXO`eI}Hojw&9w_^Edg0+jbf29F{ges`gP&pQw zrR+B`_(5OwDLI-V-ye|uiena?L5d0x64C>*FRKc^=oJibG%qG0OQ}6VMr_hl$uyN( z*n@{U46R2$oZK#XV8&-<@l8+4{CB03+ou7O|M+??(X$Z7Cl{sRU)d;fDIw8j>w_Ga zW9D*k#@=c+2N(a)@u57xgP$g0=1atjaJokahb2I-&FhO8Qe!>R2yu2Okznq?e`BLY zxQ+@)zuX>=q}o(;YqL5TXG%!Ct#RN>S00*_;8y;Eo+qJGF1J8nA%EDYK8|%oKhGf3 z71(IHfk#cf$aJHSeB%X3)WKbUL9%rn3tf?fI(xKbL1_b0Fbt}{Z+Vupp8RH;gM|Tw zlD&2XA*SFT`px4wx%h3^hwK7FM0ehnKIEaXx8fWoFaWqCH1{SVh4bML?)S7Fbughg zt_*EyXtW@RDMQc#iZJ4x`uIudVgGf6zZ%L<5Oo+u7>ZAg&=Zp$_J1@Vv4;zCRCR;n zOJ|e0BP$QtKR8*=fNTvqe5wV*0!&A;xdmVY!O(g=NrHhdCgRO<uXU_4!O_F3Z3hQD5~8*CCJ>H*xq&x!~Ok3yd@!yB-+UKhQsOilu$YwY*^eCZ`T zA2kMU`~j6oqF-LH?5Gfc7?i(woPKQkx#$@%yU5@RYAWPe&R~po+K6H3anFtl3;%pk zljzSHL>fZ_0Xa{W@XclAF%zl@Z7EWs4aQAoBo4*O$T`K`D{hcomJ_Q*pg4vY#}Dbt zl-Q_#Uot`$q@ft1babhuaaap~h6jxJp>Dd0XY1l^FhXivs&sj#W;HD-g*<|X&&d~K z344?;u!UP0Fnx&KfVo(pUMJ&}#F*jCku}8K3Df|*u0MH-c4c`?&hwE>57_PufasQ` z&M}oin_3+rVI3Uk2+5XVc}=rEmH-i$Z_kC}v>na1Qz^G)1+ioa_*-HW2)iYYnJ>!B zSqNMq=In;-8MOeqWFjLA1i3(r@FuS~K;Fh%VMG@9!1oaXhh*=T zVp090LbU7=ERC{e?jDU?B&R%MSFNXn?WgJCi1fm>zX)9M>J2_0V5ywT1F5?PhTw?_ zAJrTNl!1??ec`0!C1IqkUEW>^{gH;(>WLO#pIOA?vqZCW=P!tgLhw=(xA{o7j z2>6)LX_xoNnqtEJneEQMys9@Rz{wI)G;|HkrTs!@g#&VVrtgObl=Drvu$}M>2+|Rc ze3Ovebi%y~8i0&eK0&~O%oK3rfppAWC+k2)vI5$UY^f}2z?@5cF`BG^mGq&8^^wfe zMs&&5QgRN`l5DG%KgT$K4&MuBTYY({@6)jdDVfF}Wo7>PpZ_q~pYeyw{@36A`&{-v z|L)&%*`N7;$Yo43cNfDYwV!-@Valw|+O$7)`{daddo_c!7gEn2lp>?B%{5&?DD=4j7`=}nhcqQ|jN7tCQl}Fw zL#_jyOyQt@q}~6h^%**LJ}{4i&W)dopF$caVQwfNy;&DT>J}Zok<|*iJx-DsA>CL!)J3q{xXt{DX2>iCc1>_K{%rN;}J1|9=;8kU2&jm`Y z<@Hu*@a#{Y^D*zIU%~*SDu^(~8sg|2^Lx^CVE@xRng#4ElnK9#3mL+l*kx3%c$W$H@6vU2%mPBs_LNK=n0PgltAiG0hUXD&he@S`Lu~PM)n56T97}y~JJbVrUo`N?>ez6@ zpj7H}O_#_1RYe;o%R`YLHG4tUd0cRz*-xtBBlR6&*l|#{DLX8}gyj|}eD&E5xwQvN zoWl!GWObN~78!>YRAJ5o-WCVxsE2pI(Zq3{eqyODZp$u4x~v!>T3;7Of%SF8i+=cc zW-W`m>HsPWXkd*yxh_pjES_i|@cf38d^^Es0F0P%T`jV;=K{oJx>!@e@JQ=};p^co z5g3>5!N7Su_RByK0mi`Kffx?S2%=E>T?T@@%JdOo`Uz^Fz++-|7v7)%c{;7w0z{7i zrY#ztHEgXun4Cy{nKy1zr6=QP^EAua%&nI!T}9J5re!8T0!{cnQlb{s=!hUBK0bhj zMZkM=igbBz{vMH3Xh~g@n0IXj zGl81zhFEef>K#Zc5RMW8ZL$)ik}lPgohu6GObc5BPM;B1!4ZBYLC|me{*14IFX&`e zUj00um%|84XGci0R+Bt(@#QdAr-WD)=mUE>jD0x_n_mulY*9aPS`BeKS0f#a`0|_F zoRw-IF{nFC@N$6X>j?Q-T&~%hg`aVJIc!$Kn5c$&8ea|r`N0dUxP#aJG;HhIQ@SG& z&mWPSbPnYp(9-!q7+8Vcfck^Mhsp9>KSD&6E4`dF4`9gubp_`VJ}J-|HMcMk7sV!& z#7m$Ka8O}Zknf!j-Zl@#3G>$psV9PpTR%=OG|L)_L8*>0fX3HtsV9Zm$V zoKQI|mE|%sxG>-$FwKQFKNg^`{K>=E7Ks%~isttCOXx03W9qPHYv9CfjhEi&NI)cC z0RwLwiX8~{%9)m{8`dmR4veD9^I9EQtd^H-#<0UJN3Oe#%D0hGNL3{LTDL*yTyJsk zE(In=48V3{65ie7jKAmbP8PsHY72eqN)D-0&;HK11iqZNL(a}i6~ytkXp`iAIiAtl z5Ht_i*niEpb|dbGp-RxCG5VlP2IFg~{c1HhSBlAsY)SmQ+;!HbD0PJz$}cmK^l1{W zq_2D2t@p+-k41;}eIP(zwZccVpl zO^<~f2Nz2dvx6|HPJejISuy+b{wERo^lzLYHaq`m)`j(cs***s_F)9|G5#ND ztYMI&WR}+7w)U%P56=;-+NHkq@*HNKfASu&W12Jo&N78=ZAo-}C?KItfL`J@mpP+z zsl8GG3LK5mUKiB$Rh@c;fr#wIxMrhHVFk!ETTAR8D+3L9iAR{8CH~q9L83W*{~Vg+ zBS5O?O^e1_hZ(#Di~GgKkBJrDW=R1iC3bdRPA)Z4zFsZPgBkg@dq-)CI%pufC9r^! z(EG`yPpFBk=P<}ql4J0O8QorLgKXHF&r2m*;w4m+=;B;z!-=}&AccPr1~tRwll#Bw zrUOXFEo9WZhZnM9#X+|1!=XqZ->< znn{3=D=1pTL?>YMn;^8=bs`$!{_MJcG>_+C!JM1H*>> zg{lhEi_%GSEsQ4XQbGl=@||k>92thAWcejFCkB)ZMr@gU@P^>c53;wB2paUZ73IOy zNE?Fk0JtF%7`^V9J8%pZ?F6$=!udSzV1edsQipivHCRJ)V2ZeECdVNz0BmTC73SF; zLX3tF8u3tW7m#8CFbo>_=WQopyO&MdTSL88+35S{OKq^{4pGUB?g*<*zllL{k*e#!PIv!gR%*@%ErbBLx+UW( zKL8s$VvK1-a_ma4PQ(~);}Kg_D{31Bh-Y!9;RUK*n92OgkSV4f15@Xg_H0Q9znqF* zR*ol1h0EDjqm&5oY12sa87gczBUAfLud&?~pL9(hp1R6hmLzFVB2vh2nI&TE8V6NQ zV>*?dH9(Kp1irCH+rXJ#$3=ctl@#72`1^s1aP(={`jm-Ab9^xc^v#P0=JeXi4f?XZ zLyPD_0qmf$`;2+M9a$i~oxt+KZ~^o{>ds%FCn1A1T<;Do%JZ5;lc9V$ zYlgLMn#Eq&9tX_BqoadK5ww@?&ykUBf{+Bd{fLbeBKieHaZJX}Z z0@@<3pCsF*czMy@Hp@yx(4s_`A16r7j+XOR?z&LIsR*u=l*6pO#jh>S(HE#BuuaE7 zw}p_Lj$f4nZRFoNbMJP07si+*_zc6K#~Mc;w{DT~T5Rp)V<@Mh(*PSSH)QZL|>9q>^A%*A=!m^jM^jx{EkxXW}&I{yFbP({PgEs{0 z#2!5doJl>1rm>bw#boMg%+>b57K)0i;GT=8`ZMo6qRJ$fvXyh53hl|CIfuN9{-Ngq z$*Y>Nn1)8~gT?eV6?4EA#K4lsaEdkz1ff+ZX@#FVl)+!V8)cZbX`nh*>VjljH(10| zm#;ZfqRVHbOu3LTM1-!e&liNx9suz6yc`R9@8o1g1U&K$GARs@rl_yS{N*qI z{6GKrD-+fonZ|M=%zeST649}@I~WPXDoJcMlFh@o-fO4|^{#O8i6GJ4b9*3%c_80FKY z?0QHWdS*DBZP_k!gy`KqmU08d0)!&b1=j%F!<1OM3&J#+Hx*?0qa$>mE$|MDRh(xu zb$n<%cy!Y7G)3a%AQ@e7^3}?+?!@%Ib?}H3dgWk+SPCnn)X;<2vBBra56*#8Krw^e zvS1*B^l)D8J#IS)n#5FW-qo2k!ojNavQ&nST%g%wmK3$PxKIJs3~RymR;h2tUdll* zyVhdtbzz=$NmC|JpHb=O2ZK>}-v&GQTO|kU`Ri%p9B7}7CXq#Cuca{#A9g(burP)N z?V|rM4Xqqi0X^-hLtJ!uQzIvp9Q$;S2@kA=zuM(oyphjg;{c9nNHo&}Ty7@sF6w<1 zut0SW0hGv}RizJFmr?yOvP~U`@%LG46kb?(r)~Do6{3ftQjs-?i0m^EebTL*M(K$7 zYU6_##?P>d`@BW*h18r@f^rcC*e6g3jpn5t5UtQFT(hDtR7)%<)lS*q@s#@(Sjgjq z5qEL=`*MvF0!qa#v!(bVUQG54T{!8yPL9A5Ku zq-3?dGLU4}e3UO*9L4f=YgtlZdf*4?Dg{UqzDOlgwFGTgUDh=6k@Vg~h{8F&1GVFn z0MwQ>Wi5?;NC){HLgSd`hWpG~w8$QBRiQIl{eFS*!l3VZN4!mg-P{jg0LvMfh}`aC z77@KRV!sQed`?QUFfD@6MVO#D$cr;*!43dO;ZqF00;Ec;)zHSTqYEUC20))X?L=i*n?-SS&hPd`%tS^Xj{t^ecy%4vLS-BSRF)XDZK_T|z}Zwv9IR4S8wlG~Q{<+&{ON>%(gI z8TrLG`-AuS>Hc`K&zWE*F#C9{?M?1rL{bl;&JgYBpPu9ulv_@cB&Hx1d{%@N8oLiq zT}33zlDFEYuh653-(h$ky(1@4nsD^$lUr_pZIsa_aVFORL~&3f9}wy085*Sk-%I|1 zVzcT6!2#kfSxgVxFa&x~!O+1~YM}3Uk}mC%(%j&&f`F+kMf5r#hUA$GGQBE6B)|An zk24{~e21q)YQCWl-W09^vh)@WW1mOzr!(pd87eN{ABqnQzcHe&b4J?rc&jEE>ZAzS zXA=miVg&EyU)%)cn^uAK1;?5cIcaemTs%B@a@ig-CC8U~!T)L@hB#T&?iE5UJJ z$G>-`@#^9O`nc5FDejr{lp{HC*5l+hWt@UrM0W@GuxCat9MjktEBhLL3)0k^H--0C zy1vM%r{MNZ^ zl0l)b+h8O$=vebb;uQsMMb#M|0$L; z;skehDAoeO9g16VD-MA|(cl)`UBBFW-~Vsso5{?6_Bkgr$viu2t+Vr-y-&oPI@A@^ z#Cg3{>gBd2kW>jVxT+8X>yus+dMK?4-H>*x>DPE_*~lOFPbhvkM@NVZJRy$HcOdJb z?G;NiA-2i@ywS)_@yGz2Nt-$avr44&T1vRdaqJ}EeCk!-sVXEOKENYzN2nG_2Df7= zTB@F+R{V^yIhHNtM}k%t+HQ@a0xo^PhHTQpUP&iZHnq-o=_p_8rG?WGeG@ya6-v~u zFa&SxUYyTI1t)!QX>hWdxL2-~U%yisZsYSN^FXU<+5Wsl==M1 z_B&xyK!T6Jr~BUEKxB}lYX*BBo`H)EqIfat4HHJKMD!_(;NtR@nOc{YM1kt)u^|7a zLYmQ!!r=?-@~8^6uQwmA=9Q6qM(7OTwN#H0(6)o-g@`np5xsImgRKm$A6hycD+kY$ zaR;^vp7_-&=kq?BRgiNPjKy2uf7Avxt~<8c@jaRJ;lwe}&x#{9%|_HGLcNA_!^Rq^ z+xVvTM-AAEX@YZNO4*de{yuo6h#>t^$m8H>yoEO?QKWP-#}6tCNiNlpp`*Lj< zaE_%$YyDMma4(i^&6j}qh|scdt&3VMpIQRwn%*wK_=>dT+1&VHb^dzdntHf7WJg(o zF`$iOQ4EI$mABxOmj`9v_^!!P>clD_`vcpxizM-Rh+WU>#gR2$sP#&$8t;ti<=^Q6 z#TDp1vnR`|BC6z+Osso>f!A{(oH&6pbbm!F&kY4AJIOGW5+Al=v76 zS02$#)aIzCV&iPbU8&~y5V7jmS;8qK@ZRiQ`_lwe_!l5_HA~j>()FNx&VD9iexJ>~ zQ|&@lW1-5~(~ex@z&#$ zvj$ip{f7gWH7?J=#ouIIYS0UZfDIF09fia%;NNT~+3OitW^9+P%!0{oMdPBd%Uok8 z$+e;U4@;WJQT(Qft?z2nx{KH!4uKPGXTwEkr&PGh&`;yQuq79^b_3X;oGvcjS1y+3}XgVX?i?oY=G{j57IYR`M5`Q(!g81mV}v zl2Jnqf49>FVgNc(E%r`3t04j#g53fN!)p3?!OA|eWcvy412@c}3L+S1>bhl4mr9|Y_c$=EnlPID_^2n zvLnda2LUZR(Bp8Gw&*v9@~ReH34w}5Ml!GN|W?UceUKI5;|;+{m;0ETJPHt?giTPD?78zgpWze5Wiu13lQcW zx`VOeE3;VensxyS-A7m_{$uT^O&KY9nggbo!Fu;i$jo@JIoTLu{$~cUkz8!sJ650@ za-yMl@amH2uW*uH)!T1pYD7BKt>|{jSGeE!jXa->g|*W3h~J0Rl>z&Vl1KAOhRQ@#zGQos zR*#{eXp{X^$0L17%3%MQ@|BR&rizWBZzdiYha;1?TOO3Oj`+v6z z_AHj}R<`4cnhhhd`VFMZe(~-AP(@Ud+|T;L50!5fXBqH4Fj*nHRKarEk_@|Grf)`U zVZ;qcAnbH0mT&u{cj2sbtF|dEJ87;T1=bc#O&r*gDHz4nYW9L;U$ACq3U&9po4!z+ z-_f8lrMkvPv()?`q-Wcowu$Zk=8-kWclSvYm=ZO%QxI)+g+PB-o;#zBx*%~5sflQ6 zzTZX}`P&B8VkoMii>Vv1Kia?H4mqwzXkT2f_LR&S9ptU~N=;OxFrO;pUnS3VaD=W< zX>J$ypiufHF3un?bT^IELueS?g(KXjQZbd!y)a+S{Vl@a_XVa-3pFCs-gK=|_L8%r zUBAscVx6z3OBy7)Cx%SBwiCX8kvBKZt0s@a=MMBJ@#=A~EE**pT+iR(G4q4LE;f^ zex)Q>CnDO}D>rGt{Pi8b)W^+6)e8d`Hr17nY0#0aH_V@48dNE+yw+j)91KH~2aR&~ zX`Q!H^3wkCZa=Vc$6bYkHI&T7tlvw2!>|#|HDUoGX`*eeyldp0x>Z!N&$)gIx}W1> zCS3{PO<>lm8*qAUJp3v%Bx@&XkV%l+$KsXDcln37SKe<0LPY7=c(Z(QtP&UFG6~#m zxL#g~+wa3}+ia2phJSb&K<(jjJIQgIuQSU9^4TaU z>rC+^VS-*|iPcysXcE)(tca=l2z#8FM8r01ef4JuzDQ+^fkH9$4Nu>@HlZ&UxGw>n zPsT5&H@*+B+ttm_iElNph(ZM>I*piO%NC-qAW%?8O?QHwhh*S@>fO=XP)NBc_T!=9 zxQ3U?72BwQ7D~<6wEd}QcyALQ{TiIbX_$aJ~yBpf!_qkn|MXMPF-j zSOe^6a~x5?t3xETy+4~UuX9US3mU=bc;seVBir~Cf3@Bq))V4|<&-^))L#?6=)t5o zt`OBh^23Ha=}iqkU4MzsA4*EMdqydP0MRbLqPmSzK1{MnDlvSsWD29o;KHH_HbU93 zQBj?Y_mHrLix-GvM11_l&w~NI?m_Vh$JY-~d|KtZFKF_(*Cb~QdESmhed7#nmWUyO zVeeKYQVIgjY@P6VO~*n_WB(eJ>V#|6h}h*5d#$r2+fYbZPD=D9au!8$Y_ntPEPZBF z6h@PbKr0%J!vA>}+#GpusZBvGDHl{aXP6=LdX-&P&GX&$#r&XTB~s{Wc<7j2`OX8z zXw2-c0fEUfi{a;*6+@bJo18OAQBdjR$A*gA?UswYMoSh3X8DTUXz_3{*n*g$R@isW zKWm38ToE^*>freriv_(?F$;F36wal-uV#F>QwJXsM=jh24o*wWhpzjS9Pa$YaHkDw zD0=%Wul!AEmSxr|u12d0Bqj_<9P}4bYFzOk_h+0YLo2y5=wTgtqM-xvp-aJ1PUr7a z6h}`r`1VFwA%0^QRKP@i(p6t`m3|258w$c-&`=Yb4eq5dObh*;@& zSzF&Tr^W;?G)SG0yKF6{f1CqnRnY>L?ei3pE=t8GV5|8zyXKsYQ|JunK5jel+#{BaG)~9cGtiR5R34~W( z5_3cki2$a1#cJYWg>K-tvf{YExAJ%LBppiLra_o$M(SIqZA~$EaOjgPa_nF#QVcxD3JndK&Zq7}N5y@Pj`7AEXJE5AUF!bXa zkA^cWndCy!L$bQQ*?u#4PA>Q`gmsH@W~sPEe4B}`=g#T#C)R~;TctPD(m{MaL~LQM zUc&!Zc$JF)9HuTY^omtMtbblxZWSpUfubIFnKn*l$D= zGy+xS^29`ABRTt6@9chOjwuBZ>ZeYll7pq;PYP1UNwvgvUA;;`2PJw$vk`Z91uWey ztPKvob`|WJLpxq*&Ll^OliZY&Te@}oST`=8LI`4LPa3GWi*cQKoiq{iy-D4sYvO}P zw$G~{Du-hR`=Dn@bxAHU)|95HRfyx91|tG`@~ct4+9r(9_L60URSGp*9ZOG^)sahT z$w(ZBYlfThkBKPUMlWd-4@)#gkevt1=d>u{Iq9bgVqzB`X4xRFT)Dvp+v)01Umi-+))iMnnr_#rU zh$l5DZ^D( zfi>^O7_6^%+#}bO`OPVmkmyZFYQG|LaBWHZ;ldlR8hdDYar^SikK=6*s{JaOT=<@! z`^f4uqInXJ)zS^YxJ`W(=;iUaW0UiwnqLE?@=3f*mW-qF)s*>i1FlAs;ZWBc^&iQ}%T!csx?H8W+NznVZ|9s&) z0qed$#}w@ucpo9!8r+X}vNe8@Du@FT-OuG9cTu(;2_HPPcapy;x&YoSo}Ba@42dKe z>Y5ESND#{GE0_0nu)nr=lb+T(Al&XW$o_ia7ni1GvPQ|mXY1d8Et8`kL~wn7(B)7F ztsg4{+)q08_zw!U#qhZIpTgT@cu#<;*~c6eouaF{`dEf83%G2&u0mx zK7UI(+dlLMJkrjUoNG8n%g@Ao;{2;qMn6+6FtZ}0wD~1v?a4ij6ZOvI15|~GwV6-t zNBhnvZ2(MeJ8|h#g|Y%YCF^Ghg0_AtWsHYiaoV#}+g<-f`O<{7un{?A_$0xvo^Ux_ z#5y@dTehJ}Ly+>lY+YW}(#`7K^W(Jmjg%j#?Hg8<3g~eM8IJo2Y}MYIe=U5>6C&`fr*jl10qN2 zD?0Pv4mr35V!-+E9cIMyG4h_tyK9T;sBYznxGoND z^M=Q_>(5ti=Zs4x92<%6jO&jgW@6k_aJ|jz88{sFUdzVG54lAZv!KU?&=gapdd$;g zzL|ZKXYmT}bl^Qcb0jjE$orrBtQn%Sz8cds0Or3mhNu;0OrXdQjfja=pCi3vgMah} z650~+8p;kX^HLFiS4|s49%{FF;}~U3k-l^v>D7&ao;T|>!3S>k{ggy&>nuaO{(eb` zYPV&$?v=I(k@vTDJ`AOKfjEJ>I_LDIw^ZjT4r$Ci{DZF|M$o{am}yz^+1WUw6rmZ2 zlrMo}W?LdXy~uVgjCww2Y>a4uzznV^{;xl}!3Zv)%F2b)Mfg54<)bpvGu#Kn_Kja# zMJ`w3C6fnZz3G1z{_U133Rd7Jui|;?oN|2=s%Tr)tB+!nv(61!E-Fh;OXvxw@OWP@_ZkE|!E(Z)Qtu%tBmhV!vE+jM@dSe_O8gtHOJwy|mMn)Fy{>>&h}? zMA_IlOGV~AOuwhnh4ip^jG9OHQMKqg)JYHIc(NZOZeS^Z%(KZj%I0hd3N&hY!{vVd zRS~p2ey2N(O8Dr7?h}kQ#Jk^}X`89}%Kw$o%)u7eKXz)9jFqd7QZcDfcR{156lH~zS)LMgq zBna(+iX)EEfChx;N zV$S*s!(oj^E0thZ;aQJCYH03S5W zmb*gp8nq7OpgiN=87oG0Ax|_K5>f<73ey;EgE!4>#7R2Oa@D-KS@W~#6h&Jm=9gfdl_0$c{ zuNfoLFZKqTcvVuq5%Ua(-Q#Z#WGT^RxyTt!;0XR*kL7L%k=i*ok@R-Q;y#IqTSM2Tgos&;Q>!_QVoxXX3#J$$9g4k@zDs?brgt!l0K&%rFWxT|U1XnH-Cj+OuXeD?x)jjpVLHZv0 z8PLbsN@uQi&hp1uC@3hFYkV$v6DDGu^PiJ_u&Wd%4E-+f?m+0>rRasu6VTnKR zoq{owrhUJvbG>n*WyM}LoykYD|3Igr=kS{yIVJ54*`ClnN3(kA!>Ls0Ho*p|vHaIGbou zrkKTXiF}$9n=m|Ou(i`-rIj~T)ihzIACBerio=?7GejmN7H9aTb&}`PjKq$7#4|mo*kHTYnF$LjL&! zDd&)@JxR}S=e6#=MXh>t`@uWmcN}(W^{m^oJBdrY#YyGCEJQiX6?Mb5VHwBEsa#Hr`h#k#(-t36>gDOzi| z*4Iu&(TAiTKicCe#(6YvTuK4R*Av+gJd& zmj}~_8)~WE%N{DpiuYYK33SeAInRM_Tnl%@-c_IPt$Ge_^ztz~%G_iP8(D+3SX- zWkr?6kvGVL)A}#Hhr9~>F9=lv7?TNMU zzrB2Y+tw)Tg?;%U+qt-f~r@!aN`*ls$h4E6SL3q^%FiXCXnzRdY@?+6l|X( zZu~I1@>1vF_c%3i|5@GG?x?t4PQXw#dSR$nP(dDJdCL4R?#Ze#{3x|=z}@J}?eqEf zmy%WEDF-1s3#M#F1;hqIih0gh$2rtZ@Csmh?bk~U_+FTaoT31Cbip5H4(T~N`vTxR z0h>8J{(9c(bVXzn^fL%xJpm`IT{=JM`N7ORdiLn=c<+25ciw0v1#EhC=F)|H5xAqI z9j-cxuFo}Km3188`u}*&I`+@!modh7>Pu#a*8T z{yhr3w9pfup+<;e^3Eo+D#I&WuhE%Xhr36?Aor4nZ`T0L$eD={;oE4e(CeKb%Rfx} zbqN}l;oc{^`JY{tAyV$f90Gusi6RDtuKfjVz2j;}2OX%0>oqh~Hm6#u-hi|2%hsM{ z10Vep4v%V%MPJ;P7F$V-#+-LmO*CY+^VYX!I0_PZ1eT(Dr~(Z^w^3g!Lk>=Q`YiLq zmai8VmD3b$wT}ix6_WJasHOL-KFW@HPjii9Uz6PR+3|3ch=bIE?cYXROm%9rt7J7O zo3!=J5btf@lx2%w`If@WL{E&Ul}<8Wh~p0Oz5Km)fSJOML2s?IFpt3qc4nZ(t+hj| zZi5D3g0M%oNCWV@Fq;X2zEyV?PYqh-^g4v7hA_buD!9I&H^6pV{Hwg&z3YwQRu4J6 zDUx7LJXqCgzrhq_^agXk{*soF^P@TJ6BVS^ObtqI;%BUKtX!5=09v608>TMu=);1A zk@iWfvqXg{`UBY3oPlWp-w+t%38g}1Z-1T0ByihKo+%~lmV8l6r2(8 zlwcS*KX;5JE*X|Gi>3_(xNx*gze+0rX&i&|4|&%Mn?dT==}uEnj{nTZZu6hO+GckA z6LeEYsBAk^WQRTVSvJjO6Fc_5W?~fy{|No%9CrJwS-Ui@R=^9i*nV;W^j0B!y1Z2p z?}+#-T;YGu`Cn+vOC}^s!2272P~RNBCA}`(%L8GKIMhYt9l@VD5|UnES<){|LF`l1 zRk}es>QUYLlR(Gz)ye?ZNaTmyxzyS*hZq&}#zT=2fC4S0}Pb;&-#Y*%b zoDL{?27~Sy47+f}yJi*5kEke&-C=X;aLW0^gAfY+`-)+*kKuA?dp9gri`(0KnaRd; zqu&01FWR$-uSD#PazyNMa{gXacB~p#FT$2-Idz~hF(AFIW0myC+bbKsvL;D~fzA(6 z-;?KC!(Gwh?NBKz$_=?le@7(-Yr*Um}`*`E_rL|yxa!X@Z}>r8g=FLcgPKmR0Hs2 z_wWaeX#+$}Xplyu0jMifLW9u&DRP5I9!?6ixS`NoTW5W9tCGB&6iRoqq~U9TNC+w6 zq3AGaUeh9-Wp{6h{UxPYA^1^;yAXMUTzn|a``ZT*K8HJ#eVQmPx8V1@L~#Mr>meOj zDRkyp2#FKyo6qEfC<<+gdyk(4IIoDhtUQ$ft)z+Gf^wfd+-dEfS%x|b4(&fsTjeYW zrmi$a)K7c`*EP?uK*Z{uSv!t!baU?7G_RXZzKRXHiRqSW9@z^c!y_r~d&kZ-1a95Idbd*_bG zV!{)irNVVX>&~$zqDJJE&+)*_h<&AemY?}PKg2E6-K%6}1aKzn0be|O4RkUZiN z!{xitmOlR!rA7whUG@N^n-{ehlF&HH@>?t9;^6&*<4bG#IjH(Ci*&3F;$RA;Q^;J{!OTOREe~ko}<|iB$CGb#AwQS3?%f+ODmEM7T{2==68sq`~ znWV)5y)~5hGx2t%z~AAhBYCPRX4FL-ML*+kJBrP0LdTAn>eX-lS)1ed(*C zo6d_kAH6w$Tq61*?8$ejKwXnt8R~h?KSV7-=J!9P7&o7U<9T6;J(QL2uHEy0nwHP2 zox9x;9ebdB$tRum3dm=4n_jA_WNyVimX=>y01>%d4XB0=H}}*n*Tg>V;O}HKk6n&7g+R_637)QVCD89%dInmT%&VY?NFGoql%N{qf?B6Y5sdRd-cu}rR@PbZ zZep(-^FhGEdUONH1M{f`#Hu{dJ*rhmAdPialiNN`t96#yULlS0PheY0#4#QW8E#B} z19~{N^ImU&O}?K`(*I`w<>1qsXFm5ED`ub0x30=EH&!z?qtBzi7p%zSIw~{P4te7W zR+?mO{oc`)9NK@TB41mcsC8p)G$N1{x7WJgbCoAFcxrH7f?EE%A1jAr2bl#&aR)dp zD|Llm#;b&+aV4#wT{BkSvF+o(p8JFN5=+|3fTU+@6ToQuGJMY%dGZQimJ+E+``~+5$88SdbOeHw@p;glG+y6 z*;b&(4=?0C6tdmVUpaOl2m>}M5gEqe>>oIW^hVFB?5oe$(qTMF*!x!nVe*U&53{<%C-4`YuazjYFqIZuB^i%&hI`cZDzQ>vJ=Y3ke52G9WT@NeAQ*ZSmtguILHT#nrKZ(UhKm5v!lssN>9o$-uVJ&6Pdb#>dnp=-hE;YMHon=9w z759@@$_h0_a&|!)CTb0w1;|)YyT$y}%y&gzq&tZH13)-R1hwvJ<-XRliiBWW=af&g zZ{=b30($yIv>fmu=nk~F3!$#TpNFJ_oq$f5Uh+v}==^+x%}qg1!>ze!*=(px*Gv}@ z_#(PEz(ZA{V2mN5TJZ%EEt%P<3yjPV#UUYQqc3b~ymdgZ?k60@WY9)?Z+Y&ld6r;x z&A?vAPDN*qZ{tP2(X&n37ixu7@4?Z~YX2UsnPPPZFHXUe7wr<;-SE!5 zx}P6GZ?r1$mrsUH2(kCFlF<qb`98p)0eJppr;n_38!fB9rQKj%+ndx?R=Nk(~aB zmmsg1J@Gm{Y7)vbKVqHL-fZt9CgV}EdwDNPy#k&*uxB3w_Ve}txu(85q>aKm5q=0 zB`(>*s8?fXc)w1;=7bee!M*(MNpg_%tU?%bYG!E4RbzL{)r@S~|JBbsr~BirIj`l} zgeIe=tMW=%mv~P5e3!W00ra^=*zx+L`NraH6(@%dNE^Zr`k35Yau!-myiJpi%QWng zgyonE#_e!Xi?ui91Db(hq};^ud-Inrr;LH`&)5A7u3M#PYB7O7Ku4gzwfWWke`6;a z8V!}ddpoS!LC15zc}}guFp;(6#xEyemPvp<(>EXnpVUEYp(_%fY z2{UcEP(^c1prf|fN817ueVBAzpky}L!>LPY^y9oONziQC{|wx_b9Si6l)q5+2SfJ> z3ccRJm)iYnKI3XqkW`>&CI!XLE; zSwm_EHoLAM=$;8ZLCPe`)QYYQvX6$4A%_!~Ie$p}IEuP%cX0~pi9^I=3;P`4ZJC@0C47EDUWe4|j^=BOs@uMz2qsT3~hNM=~QVQHqM8|RHm zfo9W;^A3M;s33#KiOUaY`MVT;eJjppL$hpL?-u)bFR|V2rtQ@s-hlt{hotMjX$_%* zc&5RL$>eN#d(#?TD*3abz(PjnphI>A^q?@tNs=kwT&hPIswd_*D(=%)YvX^Kc^yf9 zJy}M0ojk=YR>a-Br+s&&JhA<~1B#cV_sX%Lefk(FDtt{PxhWIEXLZd;{^O4qyfRad zjMcH9UE6um?}X9UTs&PriQ8Q%YkN1Kzo66Y72{Wn2tNMXyYG>+e5+K|lCmrI7EW zf3uTP_3&3$sPEFvg^^{;?_2l*8cML0+P)`Pa~*#8aMOvmoqH)u6A3aY6y>!4s$JqA z%wSscu+F02qd=_wBg8aCis+aJLYeZyq)<0j2Y+)b?moLt&2!_( z1E=tSch_-t+u=ckrrQY<2%4UySqM;t{qgHwb;Oy1Hw9 zvagzhK0sB{ICY4Lzoc7DxA9p1$xJqBFltFB8%He|B9{#zAKD4gD=uzTNF6CpF?ypp z-pDs<{(avH)pRxdM)G4TQf?3OPuZmpy8eTwU4>@sg2_6TLf2crm*jChbSNhBP0`V@ zIV%|HyIdD5HG`#k55SE;L|NkMX`8GPwIRgGd(Z%v5{$A3%APph5ddli5mdi! zFz%b|5TPFzl2bc0EeZb)#{Kp^H5MkWOy+~M)Mxu@j172U9}-n2yOdyJ{aIlV^gG51 zqRn6nb{)xL;gWHcb@E=)LzL#zMJ^f7vKzu5f7?Uq1S8$@gpGs>`y?J%XE_`k+9Vb% ziNnRho5BYSd~c{G+$@~fTVU^XN?rRvi(&A$<8)5A8l83woBoE*_A4d*Z-J7Ol8(`l-CWGvF$YLTwD80R72~H?UO}HB(6rW75V@Ro{v5Z|featXZ`USty z38OJtMkWW)YE~<3zbCoX=0sUoX-O>4g=~fYp>n=D^I6sQphtzisZ><=%|V{?JV2cf zz1dEFXus-H{Va_VXO%}F6x?;R76y_ zCeAFFZ(c*GxX?NMM3`8B-QNwET` zH|}VhD*)6hcezQ>c z6vj7EsNwKgF2ibSbg2mm&MK_iO{@Q)$B2CdC+)-|=D+%4;r}q>fBM2ZGE*o_W*MB6csn9ByVw?3<LU>QZ&s^E)&sftL`aI2#Bjc@)uH*1$q~z&y(N+J z3Ee(p9Ksk^xt#EdKdOC=SSNg;(Z1r3Zl|6dj{A<9j+x$be-BFLP#yUvsx$68TB@sY zGhqj9bs3x!5Tbn6K><3ey}>^78hxM=Z`^%|8^8R6uG+_^Ts}-F}USLLg4&K4UArWa8>qSEBgYJiHmL#*LKgL__=$lUDwQ*YE zz)?CPiNE730e61?>j3_BOiRyu+>a>&Tiido9RH;=i9DAaRY>7Q2xq2l0N1)`m~g|> zr~p6>VWn#^_ybq;EI=J%X7X0wDjzIVV+RjYw|!BQ5BepG(0%?I9IBG011%q@V@?qL zKAU|BSeFk|Fu!xkeZ)(){{Mz>;X`#EV$B!P{3!9I)rVQCdszQW`oJ`W%+?|wqL-ky zq^BH4C2-3)@{)b-t5{-v@`3xXm!R-jodBO7a@40|^_oeN9NnGgDl*#*ll72{8h5ALw1L`lZvtA%!pHzM@+e`gFMvhC)?%>N*F{_eM#c=g6-4N{j~ z9|=6%bpT9mR-gCk;Ycxh%%1mEW?F0(D$+7tT)ZnP=@aBsxlg=oB)F-gT=D2s`aH~( zn^^Ht7u655{87&85Jw310}!E>>X{|xY9!g%d%!SiW%WusWDsBmX>djHKX}s0f}|s0 zPYR)Xq)E^>Kq7@t0^h+^$Z<*#9h?Gqw;+xZ{~$i#3sClTv~?94;2+H5v4*I6|d&n)xD!>_qIXF^P4cclB64G{3*jbDUM@3is1Zn8?bWgv*NS^nc8^f6euOp1;(hm3Kr( zrAtu5#3g*k`w{L+IUYR6h+W|RKg9v;mB0)9qVm_1M=XR%G$>j_MTBtQg!VNi2m6vD zyFE)=W98xu>iHUh*QNUVXhTC>qv6D_@-2R}B#*@_wY{d7hARI}HM z{s+Fsicj6P#6GC-KjE@A+KH_y1fl&4xA$K5u6ON5;JNLajrulUh|XUC5s>&t@Sxty zQ7k(1);mt{6#@gkQHi#Yy}W5V(C7DHYk5YyvuN+~pD1UZ^;Pc=)ca z99UW4aVcC}o}r5g{`y7cMm!o2373v^+^80%J4m%2=V7i|$5~hOCSv+b_`ugB`5l@r zt@-qPLA0%^3@`~nkV)h&yRqGN?wOXtL5Wziex@QyFxp^g31i1eO4asQ+g!o9-yxV87`t ze96mk)5tdK_1OOlg_ih(p|>b&q}^1WNkKVZ^0^6}h=&mNQ(v~c3rEhNbUhkb zO5kl30or)SXCg0U$&(}}P4)Q{r6?(13@Kz^O1{=)I7uH7<&77O1`m|xiauY#x_oa= zU?t!0VMFOPTkk9QZXnK2laUC(5V+FwK$8)5_Q09qwu zoVT}hWoO!PMP!7;dj_X1E^fch_*@P}K{784+3`i)t0IXg9Nm4vbc}0{31AOYU-u~% z>((owWdSSn4CfBWMP@w`_I>IF^-KR96cy(9QVn#jK1^-h667`?vwN{MSoU!{=RCnj z#jekOVe@q@uZF}3q@#HFidq5GR&)>CvVrI+Gqzbh(_YBi(f7lsC0B0xxd!UucWdop zS;T7j6`)pCD>OxyM#$-2dx%SiU%#&qxtenY=w21xM{U^~d^rc%2b2)9Woe25a$FVL zrU2W?7l4vjYFLUkphJKOCFAW?s^c{b+FoZk(|$hw?CVOGYp8B~XBpt{%p*Km^{d+R zt_`fJS3T`D%6pZ%lfECZG>dX%(+Kfsv2Wh->1|tb*S`>5H*|=pzRPwBSc%w8xCu?& zn%-~o723ExxMj?Fh9M>FtW0 zh|$BZm0lyXAprH+XOHR|Qk&#oO&^kID-v%-ur~H|v@Hx`>fa4#v(bG)bIZfAh$TY> zx2H=DT~d75CqJPZ8%X7>o6UM^YPs6O1E$8x&utGV-fZbKE`|z>>tuPQ|1iO(bv7{v zL5cjTuci2N-{%}s#}zZ&973qxd!v#6>>(KD3JRXMQU$j zF49CtIk29ya>3ijkdp=xMgzmX@gMfp2x2l}OD_M4^-YWSb+cwx?_?@o>`I zGx-!$&*HJk;pA!q z5iNj`8P+4&u@znPl_8vhZ8b4uXjMQ8z~TGW_P&uTzE4WRdWUPX6hm6M+!kXOlxiB% zc~kmT-sI5q!f8cydD2wN6TML(qOCmHk=r0YMIkKMijaO;`KqB~!FCQLFH}gLP~0}% zfX+du<6@pE?ZKBarIel~|3=~$&RzFzEqaKYCMvpxZ1$Mej|2L6*;0HI?p+i*m$k`b z?jNB@7kYi7pA0SK2a&A$y-JDczPKFb)me+Lj|w;cB_Uqh?qiM&e>c_Mfg|0@pwi&< z@l{!(RIpNZu509JYH)^S;GCe4Qrz>d3|s^}U4(&$PrheN`px&3 zgF1vxFbn!EiF{ik8ehx`OE3J_pV%mU(9kja0lrbZzt%G;u=%`nSL- zFW9;7^iAsB5v#1Tmy@B`dQi&{R=qZlS;(0dy+rA5Qg3Mi7SMG!=s|3BY} zEa;2oVnPPvRh@@*@iU}nYlI|wh)p9q6PoTVHMDWL{c;d=>0?yjqQHRWeHNyrn!QYpT-V0|;O``j}&nWe*?#NuWEcp@!ki zclsQqX^!ql6U#D;q7G9_cZ+M9c|fJmu27Whu;Wqf%unQ_RhEY#=&Wyd@EZHN44Cxj zak;5gOIwFj1b}fK4YCBq2SL$fUbSGL246*}QDj7aGsvdYq78mrWJ- zp@pU7RKj|!WVk2ayi0)Wvq{wn%8-!Xkw7A0g>)NFF(GB<#(Rr9M@d;DZ?klU!1?GX zucG8e$iQeUn!|+%Xacs58=Ck{&3aJM)Wd~VuVuD$TZRG3LY@>mlM40N#i_IFSB5j$ zysPPrMk>EwKput1@=68qH6Ww_r^9KPvNBngi|1+^+fVv81Bl|Ph{(TNE6>q^AjBHC zI#MciNRIlWx%50cKRCu6$6jB2gg1pUVXjJC$vlq99H&qmYLtl*Ey$I`q+xdaHic_A`yP7vJ|{cL+?lLwHk69HGKo*_*!9WUfUmSE zrP9o|wx~H~?Apf4iA5>HiL>P>5fPJtHe<-u!k~?AbtUSvsv&$5oZ~vguQ5}f$~t&U z{#5|?oFH=>{XhzT{mcLSEZl)jZh3V>*V_F~SF8dtHPIS2?%H1xL zvwUyhE1npi^zZpnxU%=N~`I<=^X>^bqK60ZHMVjG#1i@^In~8izBd4q)QnZE43~05L z4KJu?3oJ?6V<3pRkq?o{KV_O*f15X|&KgubFDP3I7`=!oY44|`CU#egpA5s*$1_7K zv0sBkY~P!kA$)4ADUFeVk#Dy)&xutYpq^HFqYr_N9!aJoH&XqHLj2SnmZh=ueKm(SZjUC=j;I4 z3z^YIG?z%tQM|^7y4TkmIC0M zzd%r+29!)X<0FqPQGrienB3|42M59=>>{`f>Wn+@2(-`U;tQk@f<`P-&B@7Zfc#G4 zS5HZi@Q`i$Hpg3ITn^Me(@#>Ofz+kY^YfPV0~!uBp9irN_Y=yILL26!Y`K^Dd!scP zWbriux{(}o@q~^b=9ek*s+6d6I@px$5|3k)3WsF#esrZ1X#|BcJrMe#ks<6|<*a>6 z@dvWScSpmA{PKC8910}_Fu^;YjCi0g`&iWUog92yfg~D8UD--KhF)6`i+F5I<>zxa z3qZo920L;Ol;E!$J(g>2@39*Un*KBXKfbd=#_56U8 zVXHtxDqT1NrB=nPpF!R#J|1tVs0ZQ0yY(_~asb#`>+wuZt9vZ3hv}J0$dKC)r9iv9 zj*UbZwe$G(_WcTAY98mJi}T7OHG0$>koJF_oGJ|qsmzaKJ6!`b2WyNu{SZYo(snrV z)B2qU7}=vS?bU+<*|r`a|NUf~mQa$WZTBVxD+aeRJW}kL7N1n*==U(k9D$K(IYVU= z!6H_fWDn%Gw?Fn#?N&>OU_*nZYurp?2Dw8YuyUA&k|Xn;$OKf{@MaV6X%+5jYSj$ z5B)_4l7Vz`2Ar(8=y`s@)UV+^f~oQ(C8wkJ*Ss$T()}1uZ=fD7#+9vX&gyHX(|eWC zQ_D?#QBOxeE`Fk=frQp8%6;#3T^^g4P{X$U!S#<%E zdi9|O#;QI4fI*SXOMvjshb;{6cpNs?M!^xvN9VhLsCDx|Yg2C=**5i+#RI{uvM6td zWK{B<>Z z1Y`8_DZhMy{U`=&b{(E)0YN5|a>^yoHPC>c?~?Mu`+#c6x{f3TDm1LD8(^UJ1$r!h zt|7IMJq0ft6x_o7zg!aj6E6A7KXA!k{(rbcl{&vultuhb-Y58#Vp((+?=;)W`iuJG z5~KR+-Dd+O3w6FEO!qna?ish_T4sqJz<`SHfvGC z*8Qkzd0G^lyiN|zOhhjQTa37ViSj4X$C-x<96??b{LBLWj%ho5HfYt-r^F(QMuqYI z$wozOtEo+6twy9oIP(ONeK4@TAxTK8RGFEFG4TkFXMIP@@12yv#Jh81{ebAd?@#VV`zY~M@^(Y{WVkqNA`(b zj~ODY0(Rnf-SYNGtFNk#WAUqY02#N$Ks$vXLq73ngTK!qyfAE|pDVnLVi^5F*n9q# zfTfUEu~oyq&u^v&d?2G+yGQP0M-j&oZJaDm1@+bJQ+)W>I~U-o5%HC=+18S4Jbv5I z@MYE&EK(aDWDR5Om;5h7CrE)joMgwD0X)^?6uP%Ga^W*}*{T3OPSG91W3}GCYmuI> zazs9kI9Mn3?Zs&<-Rc>!W?c*C?1D;7%Y~v$TmQ4s4-P}{WAXg=VSNF;P$r{tnye7^ zB+XPWcbQT0sYhR&`ULrR)EWU!SD3m{!orsEfPteVM*yObDZO{~j-2DpH$5Mduy^LJ z5KzkB78i7v*zCm<6bqUBmdnH2c;B3f&Ibd8Y9G*}1N}KmOF3}X*Dg3vU78@B{z2Ct zlAw2se3`Q;cMgQa{R%PWwK{MvDU)u(szNNaY!{qSa4Ok_G=@~sa# z)ZfKH02nRq!EddQZ+I=;l!KCmVHyTDMXLP9 zs|9)A3dOA4O4jAjUFUHjDkWJaA3dpp8q!ZPj{tsYBnbLjIBQ;2eyLM@lPY|Tw;Bi= zHmGRleQYcR9POD69P`^;9vywHzfHc>ByM6S=a;%eD-A2q~w?Kh(;lK z1=+GUUKM@Zj=YT!;zJ^SJfcCaq_+)7E(lAa4a!RbEy7O;jh_Q%>9P-}0J*gHWAX{8 zAUEim_fJBgiU~E4hbOWN{gnDEq38g;RO!8~qYFKm51%Ca(&6NqDQiXu&{j9FYx+%- zE-8#dL7G`Lkh31Zslxj+jE0$#=5{6~CLAOo`&*O)Jj(WB?W)9ySUsee8^rS67Y+ey zgHEqTr=%d358RWfBo^PZ#d{QD=SGHD{;XQguiHFJO0xmplVo6h>5!C^rhI!gvN);x z)+GjCBuUIUMTC(I&3%Vj%z@+z6UAH0faU^|6Le1xbW1=_qw*(ez(VsfOd>(WnXQdcGXjk`WG1bj|02Bp4o}gZ)rWn9;ijg%(^KO`iw1x8`P&1; zCV;O#jQ!Z?^c9-^iw^c&+ofliJW(2Qy1+dSfxc*-qT6_9Hfy+LlCKiVb$x+YVnhIS z?8-nCLygH+6#(w!EjuY_Qdiy5F&Ac7D6N5! zz);B!xL1Ir>w(;J$1+Io3(AC)u~CQ`ho021eW_)=&O_#=@W7Q+jxBtPP$KRYQEpFr zb%O%dr#PdQB!EiW*nuaM@4$fhZ4)U%MyW3neGUg6|DSoX@j$VmweK0rkuL! zD+7w~IXUAj<#zxjD5-tNs{LN2`QBE zy#)#Nmj_Pi%N+;blKj5wyDR5P={>JU0FDu4C7jAQS`;!-E7<8;2rMVttG!-%r;ALd z5BcNBXIPaHU%r$W$-D5l;;9-vY@@D!Vvr0TW_|#?=PaE&T1dANIKjv=EY5l%nwzZ? z>98E}!n6Peu$z0h`T2j~vE2TQ&ou9gfr@}c7Z`IXyavyFjATHwCA4P_WiKE*v< z!?(rj;DJQ4Df#VN4%ds*gK?TJkpwjh-%kPYSf_ZWf@ydl5~8RR65vt}5V~qYeiAvt zz#*pqHDUDoh9;R#HgV157&XoILnI?p-!95>(uB2u#xeI=VT+}i6ge1a5_OrYSQ3CbRcfaZYkW)Y7OaaT7YDh|H{EDN`Z?QF$+0GT;ep`tKx9)% zAN!qbC^p4y?IIqYbfa(Ej7Lj22UN;`JzRe}V6bs_Ii8{#j|W^r{Kzkx+IZ+fXeoD( zP(kC|s$2CuIEweU7lGb_P~_PV^nDPd!r?9X6Te9T0r4@1Xa2LsJQrpC1OA``3kmQk zKwszQjxKb^ot6qW)AJC-Po7Hq5C#S04r@^rTzQ)r>0MPJfQ(LKhfjPHRB`IE+iM1@ zpZN33gwKU81##-lbc7glc`EsfAnB*0S`+bAo|K9#eC{YbVJpXfLYv3zBOre^l01Gj zXvO8a8-O9czhC>kaNVTFpOBn03q`4?oNKN?&|>d6g4zI()7b33V}ystCeaPL7Czdy z)CxC5Sjs<6(vxNOIT%|YC^_nmz(VRfpkFhNlUgJPYAEi~LvwbamD9F_bHMBUOYKHd z+9|EvfNq(Bp{&4dFhFj4p~vMZD^CmctzE$M%8MpvOn501cmvlr&w#*tjc3I35r-1pWoBWWCF5bY3kzkz_945cZUdCzo48=_NBFXeA2c*OY$Xn~&Yq^&L z!PtzRbM^Z{iE>6!S$<1>xSA$3pa&#a38 zS_g$6nIH1QtN}TsgScVaFIqRjg`(cAF>0Lvlw$O`&X>utABZ1ez%k$9G{J7-7PwUg5)oJ8T#O|VcD8=?}7yG(R} z&L@lBNW)2#bw-;4{dDkhZdmt+02cWQhllL#hlHr(+}x?9nM*Vk=ccW&Fi=b0qYvPR z*k~oC_We1_$e~&chAE#3D4bpPH-O>Kk;s`;&l1(m8pQxm=RDWm0F7)Nh?-b)6;R=E z_ha}4!N!H@Q}go3rqStp`9L=VN78TvMmAZkXVfrFfsR0>?g1ZTZ{9B&@Yjf>CO=|k zzi)XHyoYku#jqkW9PTa1Qw^XOA7Co4^=bh(V)SdLYQPXB!k5^V2wm=%Gts;6Xc>}< zoYp6PQ$Q@NQgG`^yxSmpDSZ}O&fu5Zc0uo#P|HV&{;7mJf0JYvSzYKYHB|P zu4@%_^_vY7eW|AYfpQoM>K5}VG@XT>-xe>!qqmmcs5x8S*ek$LfDem>ICy~#$!N5# zyPM|(_t-FnYpS*NeLn^@-Qxy~xO89%Cex50gV?JkG?mJSRLvp|s83oSWzE}|P8GMk z6&ZUtUPw+=dYuFp4Z@W zSDw%3DcjCrKbW`^w~3Rg>{Msh;jBG?3Nw{t(=i5zOL={jhbIl~^R=2b5muWm;#}I| zt9jv81U3r`J+(b2J(Sv~GENef9k@is{Y0UqKf-&z9DvSuWZtw-qa;ueCXjAf&Q^$l z)o~;p9Q%e@5yzo|8QFDihIh>hOkP$mNmFFLV^C`eZlzbO}V=ai|)S2v33=M zknH99p?Dlo&3bN`UU;)`$G5-9&TKE8W5q8O!}&gIM=iTmkK)I%?;o(^Utwf1K6Bmm z{gKAkK^JYD{Z1}Zg0lpQ`pA{%OuNR*G>al%Fvk4kpTiX#CJi8EzIc2%s}F^4;FuqX z_Gmj)I-fI-jvjp=9eQ>(-M#fT)uxR{I~e_o2J%EqY;MZm-y&^MAJ>Pg;8fldo|~%VV*vGCEo^Hf^9tqY`55}gr_b^-S0qky?BxD zdvliA-ljA$-V9@J^x#G}Pi_vuoMoD8_2p0HxveJnXG&2XwCWAnj~h?C@^P59penaV zHJ7A}`>zFs&wK<~R1rFI*6(e40tU2&b9QTb>2!Kj`BzmPy#%jN__zT&C58F$1MBTr zP7-=ETe-LH#sh0UZi|5YrG3MO=zBd47xMv3M9n z6A%^d8VY=IWUWhAWbD{xZ{d*_u5h2WZTfxpVJu?b@j{tFSy!gl<7`4I8;?^{3C(bl zFXqV<xA-H91*Hp7cELi6}4bS8S5v{#}^0nBT&M zxqF$=7Wk&2S$sZ zdfw|L(`G*M4DM8}%IX?iqF~0q3C+xLNv%h*t|DJ?Z`!jjNiy0Pn6rHpytBBt6kn($m5}f&&VwYRu|6f% zaP7I#J!HhW+L_j8TDgSNkhnq{0Fw^Q;o;6mvUAO5`)d^Y7bn>0XwCNUi{>M`WkYPurpx&F9J3p4lzS9eoh+u;}H#K8A4sA?Yvm11$j zDago0cH|SUeS$NP`$SPw8L!g?S7|h|pae&%j;5tHFfN|Dod-f1eh+qd;#|y{XKxI% zCgI`zSBIgxwdl~BvBTyutxrdqqZcj8JqR<1d_GQn8&w$o3azmNZ33#IFieqP?BnQQ z-L~c$9E!7R7g)p6DtQlhv)Q+8S#t69V(Bca=>;F3C;&ghOgw zL$w}4+0LzPQ6`;tux5R|p`bYXq*O6jeGqlfSY3KQrHIu0m&vFke@184J^E|2*InbA zmwVV=ufT?@rRtN(swm)@Qs2me!=rMGx@{$g?5i2hUR0HA9BA?0V>;2?+ zR`%c69$vAGI)JH1f)VVHby)>c6qe{>+)xAltOLR;m z{&jLFwz^+hyNiC8NZQH$fDJI>5A-jf9LG7RcHsbHx`I zTDh88LFT$G+V4tQOTiw~XBBcEnwXfPx0s|jvqikTn=s%T@c5YP>OD04aMxUftZCE^ zNknHETlffDgv&a7@K8o~^nHMa%VZSn_}BxXFGkE~ zd>O9;mn+$639kHTYQ>3rLC@sA%xNGcV0ZgxcYN}*a%xZ~DJT%QWFmb=VLKj$wXDxK zvhf|4HC~pfPy=!P$bO_F3b(ihG zxAt*7Hy<`1#^JyV@N&ngm7ns7sLAUEo9UA3sy^4wL#P>~Ov%lwpvOqUHs;jVsV%*> zWeC3yfxsigmte8}@4x-WUw{3dzyA8`U!Q;d^}qJ#|K9Ka`PVZsM8EEcCe?mDS>M|;+OPiI?=GBSx> zSBMExjO%53ye@~AveeI}KG&}}jYXrmNBQGWzM_vYM4z&LS(!g({5uMl!!z?0Zz4u3 zcjnoy`_fz)>*AAIM71=GNJ2d!1>87v=F_H*3c&|BN0h<_(jo)AAmFI``W@@{jjW|E z4B03R6iK4<-VPcUvOx;TRLO`dNasC7f?_f?bdyHb(|~k=ebel?_MhKT12S383dl)u zQ4Gy^Ou4EaZxfp4o`nw(G^T$h)td4hXtf;>w?a_HST(Haf^a z|6SLySs`(0ft##{Ps>P;QY(AAosl(8{6yN4<L6nK-83QkZ zwQgLuHn?K<@~^fJz0?3b7ATNki(pZmHPsfe8^5mUgQv3Aqb(Z7B_JO|C4R1`vLxjX zP4d4m)YtXPnfaEAt--A^dIW?bCzUP6Tpn8v)X(Gyp~ zT;>)jhjI?qw0xQZQ02GfT8hAGiT90}#g*a;D$|4LS+b!NN#dLHHbKg{NsOVx_!wR& zT-Y_%xt?RPPOoFRKC)?Bm~qC51#OOKNi(+@cZbTRkzQw9V5x(y+(VK{f_Rmu>4;^B z*c!W%^CPPe+E?eOh}C_Yx0BnL5kw^9rpEQxmUo$2BDWO$jC*(ux?A(z6Zo=_kJON7 z%1lpgFT@j9md>iMO7URRY=z9`xXEGOnpETRCHx}Xe~b?=mqk&d?Xw2`S&j9^?J^bQ zU3?KUY0D z8`0^wW&`n>k;l*bJ>3$gAt3YR^u>qd_u$gK{5sU9vD1u?^;u3PZN`o1KTAhk#|D}? zVaKH*vj}jk+Pp7Pp2RmsoJ*^(RwU$gNS3KF+;D30t*S&Er{hx@K*7)h$O96=2P|G1 zBTQ(=@co#Z5)$n#0wS_|v23rvim)YN!2g&tBaFCsL90KwCF)ZyqFirmi)7U;%q_`o zTD@?d{Mg3NrfBgZFQOQ1?X-%Co}@dzFm-)pTEHQVE=7Kk3`qiX3kB&6de?*}OW4~> zD%+kLSmFjGidt9LKznpLGY33v?NUgE=JqH?#U| z6IkTwq!+IFajyXQyx1b?{5->_scKag_9iTRiBwF3kW}A$YYvHj~|U29*@y;Bt`CAmb1)m77Od* zm$Jbv9QM$@p9DqVueeU|ihgXB4r{$fXv8mAc{jqP?i`2@MBTwF`yyd~O3hl~O zRJgp|E!yVAl@n`U*WA}#mOnPht2_MNCTdlZ{>SDN% ziHk?0iOASmsj`cGLhQGjdctH#ySVIPJ8FRYEQ8B2_1MasibuERn_Wb^XxE?p2Q?Gj z>+T-REcp41V88HTqkbtW?9mqPUeo0BHk546RT#h7HNU0B^|xT7k%sMEOE!(~GOo+^-xG`l`f4=d#?{C7~Ofc3USN!hzugX{9R5N4Y0Hr z0;!j)2NI@Vv8)&imChRM##teeoCwSYD{)Si?^Ai&heN?jeMpxupb@(vj*@a3v!!q7 zSRg$Ych!_6cya``a>fF%LWZ~goF{TVI(+_AP6ae=S{6p%p_AQTO_1J0UOb5eA}}en z^#e2$&STz?M0cU?q6+nNcT5#HJF`b6*^&bwe81vCm-Y1Zq4v%pQ8}+tAuYK*ZGZlI zB>)~zIU$Vv6zZZ{FF8OUDT)6C-Wj;x4xS{Wu%B}kUjvEC=lkmC8>s8b(WE^5mQz5~ zqm{dU^zx_6En$iTC|@V2`uUbl+pWp~$bWyD>~G$(E64A6w%8xA2^&p1QNo>f0DJSz zsfnjs0c$)&SfUel3E-d7&$7{JoXiEGa-3X&6_#S|k$TLgtN`WP-b*!3Rhljn58UF- z2f_wx4`aJB9DW=XlI#uH#7mllxIiR&N|Wc9bYfa$j}340n~F>--fH2CbDQLQMN~Fa z();ZMcXJtfp_$1k`P`=}e4~a}uyE3u?1qK&XUQ^*@)l9dx(hl@>xC`(Cw>>ViGPkY ze1-L&Jr;j_QdB=Lc^EFb_YSC`z3g6<1H+a9i)9P4@mNDJV>0J%d@xcAUlQO^YCNZVwGO;`Z7B_->{yOP`U$88?5G}NTNNzr0zSv(6zq!DQo&8x++t@+ainF zKK?w?6~9Is9jeDiu~(kU4mNtVC4YOOXrI?Duip+8p7Sgc34R|@4m$9jN4ATcN%c;Z z)QlIX%#zc;OTa4faf$6^;W8+}gD*b`f2K>e(v~W6XkA@rPXlM_$jJentc}7QOGzdn zeCgC7$4qjx@qKZ*{Osc)BV* z?fJXVW@4Y69cA~cUp!FubA-{k<8V^d{0Y=E#&VFpUgF*cXR!VLTiv8%|5NvS*L_l2 zIzA*zTqDJn-}t@jmLFTjD@(ey*)#nr`ZKMLSju;w9)*qz1PL+!MfYooi^F**``UTm zqZSB*+E;HT{A|R#Qf2!rB3II{koI(Gt1+jcADnQlpgy8f3NMubX!39 zG=gbWuoLn-BmD+Smw-KQ-ZMQHpTJ%>lg;Ragc825_Z5Q$&TaQ-grbB;DmWn*SuXTb z!N(>IQ#8L>o$XgrNmeK>tbS zygSmm5d-HL#(ETMfSGVAWRB^KdxlQyI}^U20H1ZeY+pB>z09$+r*a?h(n`m#binAM z>;dyN_k9%mKH~W%Qg+P15ou|0$MILIGWpBgm>ji1& zaT%@p#4(W)7q=jKXTJ|V9t~pzH`T_WVckO}D(bgh*KX&%3Q;{Gez1Wlsjm}eueS-X zbHxMtcz-G@xi9hqA4qqTf3mZQZrKU4p^HQt?-=ISh^L&)#M3L>+mJ4wMv(PpmyoRf^Zkg>Jm;6MOfQhBa4eZ?!|l{mdF=kKxoh zh2X2+DQ%i?4H)={jsu1Zub+JDD9iPtJ3B6bCr#g`~tqGE@|K$M>U_b>^S}Bay$HIE(jMHXwggMPu<5JRcGJh zJSHa1-MWs}_xM_2dehfRsy;Q(k?nibV09wIe&#&(d`0beQl~?;jy-!O886zS&GQ)} zb&7A3r{G)rd%Y$kwk0GE~H66(*rV0@1QT)F*iYL^EeY`ud@2> z(W}^-HhT*TgvsW96T1rZv$ZC6RwifdeEsNIZ1&VL*%g*fDofx!5r$lQxo&C-tyBel z+|o22^y4dju2!+tc9>aZGZsd(-n*WEsz z+vBhoCjCFf{$Ugbt$I~FUd+{vZ-oP|jF}>?qF=HlQ zB7w%v+po3e-yBc(bJ&k8rcu-CHFVn56PviMp4zxO?t8-xukTrksXp&y^N=sCb-YZO z)q8F1Lu-~9V=>S4;x=Ad=ksE*)WoU0Vwzn@X2xo)4&2Y6*lXD83H`aB@s{zdoB<(e zd7DB7a34!YT+{X%wH$qNc>JDj%(f;lX>%9?(h^pDzJMDWO)xq#u6C^YlHY32Q3%6U|X?1ah| zJSokT8Hl*i=l;+Dp9l%bcZYmgZdnQXMr>#Jb<5f@wxhIBFxAA?rO0V=5vkw)qUg0t zWo$MtP}b7oJeiKF)a}U)5Gj6!ug06d1+x4y_Y_L^qGI#D7m*(zxOA?|%#pA852s`{dSqbj-AW#Wd=16SRgF$B@ z_jf#V@{NtkX$YqV9+{AT$zVs1;^I)?$lF`6id}g#m4o&TF5!BDTEyFE3%aFFtRy9( zomvt5P<~D+rg^0(+BOG68D9$G*Y85i7b4+iw&JByEH0vqO5sFN*kf)G;m7G zcuJt0r~8-*ARza;`$CSMwx`qOx=)B7;%l9$xB)5R@6Qn&BTW; zI&x#GQ24l^Nmu8mCtO$~5mWE>8FBNqTCyPbtSgy-JPfTquGAsCy(aCt@kW`&cH?@# zhCR(A*rnRz;Qb$PGb-0|x`HkdUaAY1qxV!KVjwoN;pKk#p)K~0h#jSnuNR%j<{JM&%Dj{ji|JsDFu}#)nqa8mX%7oO)TVY zEv#p&mmgcuUumXEY=%Vvf=hQ>njyyKSd^^Kj>thIW15ls?{);(celuHzo z%!5dO-2ca7F}$aw!@*zD-fkuN;b!`NhlD;WsRyMd?)f7mySNs%6;t@gAFw)>0*g|}C?2in)Nz!Slbc86*Jo3%<2=~xM?t5_cQUtbO-8n}M$nkA zi7{sdgPC_pP@f>FfM^Q!!srdJTq^&G)f##3+Cvsh8jJb%6XEI}m;_bHVkjKErbakn zJnN7Oivj3cGZ>=|9(0iZK0idoezSr4M9BC_N~lK;RE1Mk?9`Osj1uh~AfeA$??MGF z6m@k!q}dPEEk~}j=}kc0xsx$2*%2;)+b^Qn8470JR?UG_q%yV7os@ocgd3l&kO{j! zxe5L>6{D|=eSwD*5l-hO2*QyaC*h$SWe1tN(gaiCQ(&^!p0aD*e7W+W(VG8rJfGVdtR1sjo(heEwqdypv<90M_5Md83Opv&t^mstn`$G}OyqsI(j zbqY|e+HL?$o&_Z3m7(vG{}}|u_dZf$+iT^4*D3AG<#LQVO}09}X$|a{(&J1+=mkpfTSIosbGa{{wb(I#n>1Oyy7u9uscd2f{m!T*Yg?_+<&|%9ukIl? zWLt|VtW!>PdLIj>NC2CbDrvn><3*t#2z?8OhvTGnedFJY8aJvR z1nY1r&r}4aVm)(c;fFJk@$4_wi$^wBFjbm~o2NLm+0_H=?6-fOshEl=f2l-(e`#>s z&o0SsVKefI!!aB6J z0!LL3&iuUN=^ZvzP0j@Iv0xk)-+Wj*7I6@HycdUdYcP1IY=XpWaC4l3t48AgFs_{%eBg28p(Hl#l!naJMf_f@s9mz8mkKbV>9p89q1fp27LUz zFTCEB_4yNFc=Pn)5p9y)Je!Zvj%;onv^>P`Sm4=(=3jwV)dcR3Wp8!_o@k9%A)E_q zo6H>|f$SGWnLp+;r|aIoj~m%%8XfV4+2D{phm$^9w%qh0r4fm(f@Kqp0cBv1$s|P{ z&YXF%dbL}|U&_)2C=TNMy{_&G?$l^Ur(Kbs#@2gtT}5o7_~{VgQUY$`s$Ckg zi1Qsz%399PzL~dUbYj|1vUVduImR84fL~%bd)siC!1-`syZSSNY>nM13)+{#lphp{ zezj_rDT&Fz(Cu9(rO`!jE|umEp9D&F#-pZ8b3qm7Y7F*~oh;$lf-qBhg_g zy=*x|RxfXM0g@Dn-px?l8bqUW=$+1RAn@h3x(^0RDw0CS*~in?2WD>;WW};+d+%0P zi+DZBHtHidFsfV+JAaHXnzh3=G4}uaFUK)WeOmg|FDGfta@%rBKl&}rg;6PT-!Dk= zBW>|9POts=qIG-k2+Bu#X|#fQw^BdF7*!`ydZGHReCf%=51D&A{gt1&Pwk^Bm5emO z@ruib=8>;LQS@@aiSQVJ>pZ2EemGCGBkPP5v9M5*qa(Df%D}aIagS&@@K+HzVfc}^ zyL(DEV{=b(YF=hqvCh1-QI-crD{uCon|2t$StpY$ws~W2j??B9$P1smc-w*+9Up?- z*0gy!Dtn>_Cy1}0Xj3aEg8XoFmrNysYx zkRtRI%iCRwDB!zK|LVZA%9cxFE5mv>kZt9HQKqUkQkLhDa?S;E0jQ zebH1A^a5D^Y)v9Y7)uI)=#-i2BPS;z-i+ZXEXJZWFbboCI0!`^^%#9+Bg&Ho~Uys zZ;XC|+}4N;Ik*kFCHfbrM)XK43{v<+?9Ye%(1RJ#R70QT-KqoHK1#wx7@qThsux6a zUyQizF1&7#aUaMTA=J$p3^hhto^fvoBDm2<d0>$pVn-{b z_U_g-5QqxBCT$}sve82a;36V_2J*wePxQb$A<%{Eg@ z(z^RT_g2_cp8aVN`Tqe9<{_MIE zi|v6k0B5Zwjf8NkVy)7Qv0~-6IJPo*RYN9(q-UeOT zEdGypuE&lS{Z)=P<;EI~JK3|%DXWGuo?_Mt=5pH+s=j~Q*nD?^GQQ3pkEo(=j4Rx`2N54x4mKnXI_r(j)&-YteO;YU(B2LmTW6Fz$khB$^fG+?`&`>6Gl*8O>Yuqx@k8tl3ePbV)f*;QwY4CV?U)NY zee~}c9q(t8mUrn}29Fe=h+DiFw-cfyP^z z$rK>Y3YpZq@MfY=X(fW?Tet-gEi$YlOoRO9&K7Q(jP^=bHGf~7vgyb?C|jOOSAn5wXqdbw-4pZCY|oNFSy1OvPa_A1|QhXo*D8U z+=+?y~aPO&yeE<-)G%$hePr-wdBR0qD|WzH1vJ=qlh5Z_sM7KM1j2lzhZ zv~C;SUd;%75p-#bxXpo!RxP;Eemd|-jt+$VXrXi$_$q27OcvZIzEb1UWCN-9Fi_Smx!oA+vdkR&H?om6r#BnXvdZ5!#Tg0js*w- zPA7OJ6OlVlF!b+Vy_Na9BeAo98|iKM4^7F}1LRSbqType9wbF{35I;`?yqMDZ;x3yq0~)?IQ_&& z1SV{-!7d9nqjQ#3u#j|;jBTtDhkOtTMxZAy*dQtxVyhw8=JFZRtD6P1l44u+rUwrr zj_3OemD?6Rp4-JprC#t5sQ~HCwxLB^VFDc8N{eAWlgrHCnPT49FIc(_|KLpMWhYEW zy8MZm%1AL$sP~HODTZVnqbsR+p2OqI-2EedBb@g{aYc)Y?Cq>v;%D~3Y^y3-U6S3tJD;-ObP*~595>(+0_PE>NW@Ki;Hk) z{sP6=zCeJl-3Gz2?cWI3s+C#qmZh6O$yv;z90f!r-c;!vQ<5n}mlG`T@>?~yqmles+=@p;uyV52*p zZQnBcF4TmuEoT`zJmZ-xPTo+dD(5-~m|Qi8xwVo#B>j zKM&Z}zLwu*@`&yo!#8=PwPkp@H*RKk*)<|w$>&q$qWfOE_ve5@^@6K>@1MaUyoHD6nAiAQ z{k|y22C-Ot&z4d~(RfOw2*&fmrS@h#+}bjwgAwMNBXeknKq?ONHdZyHY!a?leF&?q z8JH;0zNyQLTZr3rS$%CxnR~YMk>)AUAh^JOku}PnK;LGuApGO=Osi2XuXFiltDY^9 zJ;6$t8B@h-$($+AJl>~y?$JNnm@l(~I8?^#;Qj>*=a0aV(+LR5<`>WQr6XHSRSY#t zWG1K`-YNYm;E*|{kMb4PU%ggN@zT z-lgo4++2>^mIdr8vDj=WHpj&Ks1#!Hu3blwg@(16M<)zI@xa+?MJ=7IgbfK)VmDSw zV&Q(Z{C&BG%T&2-LiozRdo)GJh=H#}+T`#ByYouG)7K{>47B8{!uIHf%Un~jl}zWD zpRp6lA>Y1`2K(po1wa`%OC^Eb|LFI(Z)$~9Brp5G9P zp>QXR;H8!O67jKiV9&NuUY%>Wp9I|!KQ1VXhKj1S^;z`VvF${vOQ^7CKqLY=tLRa~ zVQ_{{RVPSvugngZ!2PY8>=^0~m{AagQ2+44>XEtPW^F5Cc2Ta{9w_)ydp+Y;%7BOY9aE`c8>7ytRHEL#GNa5`oALDcm{)V%6gNos{ z^I0oLWl)9_Kn7W=cZD-skE@U~`U_WOD$l}XuXBuU0_B)D4(eI-wy0oJkL458i;0cb zU!mQ6j~85s`U{CQ7CXE2KJ}L&2f}(maWH-rDrZz(k!V2p-uJPfd^mK*Ql9!y@>28i zk(W0Q!Y|ts8&7oCM+UfgJ9(!oMv@T4W9clPUKDN^kxZ5FO&caxib^ud4I>eB+wE=@ z9`#LJZW<=Mv+1=)*{k|Q$cNdDzdNV5=?g-eWc`9DSRb4=$~Gv0%Fjj?7djv~M9;w&uj>6~k>xziI@*bkD&3cJDxwtR{FniQU*j)=%>Fs< z+BrD>assK0o_kk_Pz5g|w^E2GrMy6R*_fkBprii-00960tarz@gi3Ja2eE+0v&4Jb z_+8_D(|Gv$sr&TiXhu5PX+=S~Br_u-le#a;drQ^qScMoHioMa{8zPcrkfYcf^lsGm zz)dMH{d9H_eA0Yu&L`B-sCc@3hV|#ZhTeiDhICZQ;vaqORz!sa`j?77^V!%P=CKiO zCf5F}6J^D2`I;C=AUwRFe}(pjhQ6lX`CJWiatHf;R%!bq66E&1lHAR;DXJMB`sb*o zBpmWzr3$-ex4zSDFJ0^Isv5!*@7D8^?~>chjy}D^ctnkCX3n|5C7Wm==_^%)`+I!Q;ZDAp|43yvN z1oZOwz1n%Vr0x_yp}pFu^C=9pWUl3Dx?VVjh#roi;e9G^Q&Dra+t}YCF5PK9&EL|0 zf2F6$G=5?3Pw>0wegHN+gxiH!)hI6P+Rl7g$>(F1gDwbRhr3bMbGEP$gh#B0$;BK3 z32h!Jov^2fFW*q^`O5{%;nch8`J zGfaatQO8x^UvCm$VJ%WdPDDua_=>*&!`a%iUXKn*B0{Z)qAT|9Fd`CuGf)XXqyfWi|;~Cx{e!_k0j#v5I*f?wc=DtWRt7<#c-*e zbEmEmwWG4gv@Q)jdbXK|Ier!m*?=*b6=%IDFTTx_2!)$V5oP~i3vrTLk+wDbMDs+y znX{f`NLukTZV0e%{Xn9aKy>JLT;=kb-I?H=PC5V1DM{vU3`;$JGPv;Pk1~2XE>dnu zyz^CPD`0oSO(!ae*J@2djj8W4X-}bIa#^nVt)sm#A3d`cKh!2mXDX6#eGd)Ih1JL! zLcOm+P2x|s(cl5M7egL!cHO&Kfz-Iu@=T)(Vt@O?O7-9P1;)WCGu6b zCo(DeCOG?%8itFikq2i#LbHLfOb$msV3+@z{82_av3=K>8aZDn0PY*$hEq&A5;_i* zci{i0*f5@7E8uUsTRiPn*G9bOfdW1aWWVE;S`Bkf{r(D7f?0m<=3m!|-%tx#{_naAA^Qp~vl#=| zHhYDMEUeX z2nyc76Del=npBWa}KVG(5)}5*qo!j_qC+I z1jQrqVN?k!U42fSgE{!#bZNCj zji7E9`z6dD)*B>e>W`cb_Bd&TKQHfaDMYpN_Qme2%%v?Y4BZR`~vrmDz=PI$W4fY|22x1Z2@-OkWQTp zR`r1<4!178DY`j+o|^d;zU;1;%RZpiA7^SrEQqwD;Ca{l-tjS;za}~CUf~x)c(+RS z2r6Ez?|YXhK{4=ds)@mVb0snj(fK7pMU`wt``%n!R~#)x=Qh~CK6loG{V+R;&$HuEEUz|W&0B4toD5)0(Mbzv6tydMxwVvaNwoO@*; zI1?9hW8g=#FcMXd5PLop5Zfuq(Lju@_Pg+#6407VPdy6Lk;PW)p~uZ zZ`nc<^^PVoTPviBKc8cCqOJOkmVzmZe7rI+*9}X|{NmY0=VB-IKEue$smR%lF^oar zEpkxuaO|i;VSL?+%2+OJp_%@I>SxdV19s`}@l3zNKV$~Q4=CPX&W^HtiVT=59->tI zFGF6hk_NyghO0ye`bL&)VysX(tYb$s0-ppQ;fgaWR@oum(gxj0{&{5VG0LgSXoroq zBOx>P^&+@vsZMOjNYJ?-%FO`=ydG_m+_Bz@fuK0=jJ%rCXNZEtMtchxC%G4 zT{$wtRQ#x;I5qH4tgHvF!_8^?X9x@5a^fc1ZnGS#hf0Nf5pO!o#0=_lr3+#L zfvi^MGJ0*Oir2qKp0JVX@^DK@)6IT5Kq$++vhVDnq$a1q=*D3yE6ns zL`cA1crQp1qUO#^a3toV7Nx^tTH2~9KL0Sp7CzOyUzNw?+7B8ZiNPMm!fq3Mg0k6$ zS6>2=hNKzVDUmUd-7nzIj1k!Cu_^)Ur$4-4sfi(sjbN3zuOCY1UsF376+!-sg@?1S zm$INg@_nqqbA5OSBItP;VBI7?&HE+pG)=G0%gVFo@?&eku4cLsb?>lc>na;Y`vt~g zlFyIZ@lxDDi7cKgGEE?rlubGY8mAY!_rVkl8YAm2lg)-e5{4!X3WN_e_?(Oqzkm#` zher$3&DY#gKUIb(!S^_+;YHdeCZ$4l`8~eTqGB+WTHDYL`UNO$59mcpYoqH>mK!B^ z7z%78`7c%DcAMJWztkC-g;kC#yW4crgsZ(Rz*DLA4m=&FdN(jbew+?2BguU2(qnlMEm6_#XE_8`3bT zfOR4j2Bk)5-rDJBhLG%JZz6G>3bTLc5q3y#v|uR8c0G#pgmn(=b2uq3?!ICPO|ErD zdsS4dl>`T_&AmhCA3a45HgaZP6X`8I=XxoOtBZ60S;f-p`MJ089`nD=-F1_Eeka-~ zE-E}O%(Bk;soW^EII9G9g!dEc3Z zn#;*7p-Q`Md`BYIHYeUeP}3Rn?pP1-x6eVn!(Pytx zs<>22i`s~aK4J+)rp8}>l)^0-FX3=_VbB+QHa*3^29;oFmNWOfbS}L7AnFbBv+LoS zqBh>oHVGtn=r?|BD%6_=IBnNtp)3*XwI(&dawmJA(qOREWXL-s$UemjRjpdEMv%(u zR&$S90=8s9K=L3?t8|3+wb7ROoR?9cn^H27KRpN@56{AL-n+BZ6kF0p_(}x0p{U*?@jG` zdDzMmzUxd@jppd%*91c*ZZ%}O!zhqd-~Q^~;lfuob%xKkQ>{F(cStk;xLp5`tU*45 zuPD;t`Ob;mtdKsLrLYlF)z{rI9Q4V*1ccJgk0?@`|C~($@FiC{{tBMCR9BGKq$32m zo~l@jr!Rj*jqMbp)J%3fe<5OExu>g4B>Aus% zkM9jRWUFc&p|tY!>Rt^k$4U82%}~~p$$-w@{^SxQtWPpyj|OpQZpM-mc{%}bH@4@E zHNK@Z5D)m!*CyuR8o)!5Ii2F&mu)At7w$)H8y}P}h_D0$hiuuG&7Xp`)3z4+DNifs zWIur%LZXf2@@d&z^=@IH>f%|$EQa-L^>qc5-{SM*{B|&>j8RT9)d_b|zA`?<@3Dh@ zOM>N~ieD&&ebo;~3sjwy_2bkGDbYeo3Z~3IW0`nN0&)7FDOkb^D4Iylmwon-R$gT5 z>pKh8?ee;WOnzJGMZHQ^38>@cte_Z?cFD{ub4}J)w)2h7oAE>Qn(kO@!8yA95=N*< zjljTV!Y>u1zVR8}3%k62it@6REL4kq*wAjc>CQkIRC@7H zrvyN7*CvK3u1aeIaiG}6%O$y(vUxGy6qil1qU(7>Lsc9440MS?kz&of*JLU&*g$+{ zzo}`IdMVRkl}x9&Z>;>}9}3G_#kXk^R|%+O{Mf0}dv1-gwgyT+76I{z{#|#bKFT40xk^g%p7+sBxuws zp^t~^exO^vRew#3qG?bdkn(=La|xrG_jI|{^x*R$Fi4V*F@COXcZ?*>%5Ei$r}Tg(w&CFRrWNQT z;CUm!2M;L%1UtNgxEUQN^#N!@WZev39ZnghKCUDVH*mef-OH#PPQsO~Z_z5dCUDCmpWt6@%QHnMMqM z_;v~#ihW1;d}l2!%4h3$IRAkC?ozc`QHz)|#u=~KN}XlAYE$=f8(SkP$|;R_D=f4) z_UjAO-z0zRacz{{c!`~7y|UJkpuao&r;;0c+gtYaHuKhn_R0dG;{*>7D$y(mR>i$30lz791v-@iIBkNcZXj?MtxMp3Z zq^@d@$HM4lys@z8*-m5eVRIT`2|T5J`bn>UVy2aHLgU(a)pGSWFFuO-BpPVvDkrD3 zX^OO6T_{S?78P29mw0SBZ_ToZLp`7v)0V%CGsB9Mr&;2u3%LQ`@Y`m5I1^xrssG~S zGqNX2W{n2E?Y&R~$i;&$23YUodm7;z65VZihI#(PRh)1aVoZETsoK1%bXFyvSu58& z`de4=AfKWbmxn+6fEiS7PuTAGWsW+iGZdc>!F1nsYB zLVPM#^fLyS$ROv0YtvsGWOgQI?m2x>_FJ_w?A&@iD_WK+inQ3V48Z@<#Fa0M&)(+# znS6gNyGf*}k8nZC`y(LP3oDlxv>juxU8Oy5YF-@|xW8o@x8Qx5>_HCY)ftUv*IEeh zm$yp{QrKeNkNJ036N=i?^NUN}98dSd+E`Y$84UcQLb{N6_zzAzf!I%JzuH(RrSvaW z*HZP3_kUq`|V zZ4Q5=LuN<3pr{J?>O8&y+OG;K_@DRb1dIboAy0Dt zy410J9Y8wf3l9pHlC?!N%0%4&)|D#ZtPA2nP`HQYQgF^H?|8{?FFT_72Yf~$!O#6> z3gVJWceN&&KVCbn;_t5McvGiT!lv~vw!u}JBFk?*`Wm6=hA3$H8@Hhplm$J*?Y5UZGq*?P+9*n&c0?k-Q zC@7-)qc0uMm#l4^k$KWAgBbC2{5Em4SGv5M5V+gh_YH9Bzp_l$B;yk!eELQWiqi&A z2mI7W z{&A(S14Y`m)n-TEI8O0-Z#Uod0|j@gMhL`f!bjlm^o~WrY`8f}-m#-F!R-YRJEFl` z9dmH!GWbB#e82~pwYLAC*6T6=O47UPpKU)pe*;ZLTs~|6Yi(u?qGK`$JL9QA|TS`PVbVa29hK)bl~jMdpF-M%bbKTY|PVTk65q z0e)VLJQ{$HxWD=>uj(3*M-H<<+wGqqD769nF=2I-wW^K%Q`(OHdF%fG00960gxJ>##N2uw;2lW9HXA~&!AA)?P;Ul9 zMrG8yM(s?!_pYn(Rrs2GIni$$6C2_!7zSxZ!$?}|`<{2Lx2CVNvi`iMh{`)uTv_q$ z?<$qJP_`(K{bt+sEsREwAX>x~|7rzDxgbX*UKK%q|F{49Pyf9AkN5R|{^n1A`tR@i zpWoMi{g=ak`fvZb|6l(2&fomY`ybo?{MY^U?#HD0yT2=zv3Itm9?Jjqx4HayLc1LT z4 z08B#glzpgAT_{c~Glcyqd8@6e2u(7uc=VVq7JEdxw#btU%wzKLAOB`~{L|y9b71Nf z-8}UzaNCj#EGTmaC%8Sfv>wjUN24BOd2h`&7BYq`G<{_oosq+j01ID;i_M1~HA@X* z$_>)mYkBi<7?FSqH1^RHls;`D{DJHl3)ZkX0Us7^5dI=nk~Q+QziTWbDo z*Sw0flJh$kaptI0=5`VHhZ57zP$o}NE0E({ReD3MtVaK1WpqdfG{8KDp?!%V4lZR2R1A z0?h22H+U2AVv|Wi4&pfM*Wa; zWat!pXvzE_q}y1~^w^LgLlU+rtGp4~X9A32ZvoC%4P9mr(Uizl)-bh#(n72?O5|k% zII(s_qly7*>=?g5#KGBF5B1%B8_9j}#v*c)2Z*mFnwPI8FSh=u<|jl*oSHm=D8YtFgJm z)BO)CGG=EfW>o~JjwMu!O<$qU0tO&4_+G#5WF~P^{mxjzg=!ecg+)>TQhz_G;P8YL ze^7A}(;wSQChghY<6k2^G+hl)PaQ9uDYh$m7Z-=Kz-w9o^z zm#{7X@~lztV#JMJkOb~Sgxnl%$1ph@wZhYFKKU8)5*@z94o{#^30Mx?`6W>txWW*G z>Uy^e?g_{Hj&UC!Xi(;-5~Ms28P>A~pxR<9EPZ}G293IzkTbBqAijc9(J<#fyg23f zJnXwS!QZnCsf$-|#U-L{@d!Yfw;|H_bp!F29*TntkKZ(S$mZ1d3?&%Y8=vuZH46pDE0R6=4>|48#YUEAtopbatu>vw&f zfa@_gJECVAOn`VZC1QVf&4esln5D%?*{V86&~irIdPG^Y_ce2*K{<$a4&5n;{;(+d z>1IQN(_4uQ<>lbwU?EZ_yvj88ebbwtCyd$){q4 zM#%=oY!f+16C{vshpGT$?NmY8CZd*nCn8nJe-s;>n$M; z%2H$MYYE`t+?SF-$*hBplRtov@*rACreF|p&n!2MOfdJsaUWIGhs26#`u62U=}Z%EG7|I5R(rkd>sxM~9Po z5uj}WPyMV>c>cpue>mzN{`{XiD*E%kchn#If89~lE{@K}f7}1;l)v3{1V+ky??W0T zR9z)jPf~520(z|KR+G<;5trdP#@AX94d*1NQ_#aM&?2F)Qa`^4sRJ$XsQbx@H6ayz#)&}JHC#BrBC9=cA>WE%0TUA z;9JYl1Adl_+_}Yx;LoykzZ*#N$A9^x?6N>p9;sntvw=0D(^N0$5yKP1tnNY1?g z10kN=iW+nqK&sY5rIMsE9yw2!#Laj!O{s;R`}-&ryVxXxWvLVREG2zX=Q~B9POO)m zCqJ-;qJ@jj0itfdU&+P(1ZAUu8vT!@_MUNiiYyZ0n2fytdZl8H5mwJLO$@Ob8!^9$ z&dOj0K_K0G1(7?Q89m>6yW5J6$Fgs6s!64ILvLmXC8}&+H;UhSI=@|}M^6mb=({re zkk*xok5Y##s{#TUIB@ykCRVJP=?Xlwc_8rc@bKZCKm$eZ=r?ey{bP0#z9cQmNcxt; zfa2ci@}Rd&Sk<&jK_^;0V~K8Z$>zR_Ply1}{SgLJ6HHmwzT$VfZRU_i;jmLWbtxlU zPY&x(bP}^k7UdVGBtTn+gH=L+XuGTKcU%@SQ{#5f#qEX710fFgzkev%$}I|95qx?$PrTL=ykKjUxJBLf7nSmbmW&#h%cTM7ABz;zIIY$@Zh~qMs}Og zms@Q2OWQc3-jeoT!TFsFLPR+-?3D42kb<2j5)f z9zmW;`6Hs`Vi6DL_>zliq$K*9s+Xci6M=c$@g&sJ1p=a>_0lL<5IdP<{G#Rp>J<&P z+!M2ab2rbMnUeL6p_?QcquP^9%4i~ol&XD^(BpRQ)~at)*fNa$p;X2jW( zdc`{tt-ZBZS~x7)CKPi)*NbTjp=i$uK}8(W<3>&CSrHq(rNtuoG0(5Rs72$uM7tgU zBkM%kI^Z$b0-9wFI^J}feYq^fr!Q`C@YysWC^RxcY<`v%(CMgzA41Dv-o4xG)Z-+n znICr}csX^Y1%{GQ_d@{gq0m&w`*BJsZD6MH%^6QHQ6E8)pYDYd8(e>!L*oy=rK12y zeb4BoFl}_jG4^GaaDo<)g3#f}c)Q7=d?rV{dIzO{9Io~WsMmH6%Dw)OL9h5)Yjkiq z6ilLv$stvo2JTaqlTcuWJBq9)8yTnFj)tetYN9BJia~NGho9u6yVU2+3%UULH{uiq z;SxLRf7BoTD+oiP99x!+q@L~#1NVpH2HdnUIArJVUd_KkDG&hH0TXTrp8|ws!+*L=Qs@-2lGY)<^&-N@Q2(w zbYLRi-hpZ7PfJ7f==irD;CPcYzzV|=$^=jYGC@t-T(LglbtWDt6^+Y@N0T!~o+by~ z>=Masym2CQ0Bwp8Qm+^b>n4j2wvWsLLcxiijXL}V@DF2^ZN)XGgmdg;oP+@${S9pJNV(2!Zm&}a ziUhMhDpzTAFP6K2*Mm}}LjiD4nFX-Bg@dN8T|MV4F8-mxyGV|0HAwy~W)Kh>`wkwH zvw(@{pU`TXp=xg=mQ^a1HT=QK8%@JT3)(LnMp?@J1q!siUrp1aMJYAgOg}lA$tE9V zi7q^o?*U}v49`q-QMb@}%Xj6T=eiAPWQUU(4@iv&#MDkcdX`1+!*yJgg>o@OpO5Qk zMxYJuqdAKa8p?@#I?_oNoX+zuXQy_-SbiW`lOb~Hz0u7^7rU5#Ep3GoI|$gYCX|c} z#sm9874g*-wzvCYh?T98>Q6yZ1GH`4kbi!(PN1Lm_*{XKjwkG+K2$#P>~!G+h42?P zt;cx!XUPe8&E$3Oh%dCq<*%f!XERv|Y;0)jhlfvt74Z38z!Bf&u!8a}>=AMOg+NpP zx|*wQdI8k27ObQ5eoqzS+{=@DP~iB0uT}z$mnSVG2QIf$rd&Na^rqMnf#1s*O}TEk zooR`pGDbF#IcLUZR@SNS0LCm14{3a>T)zl>W;wC5t9Ds)MRUq|6A|+ergwyaVxz zY+GSj(Qbnq{_4Sm&~8KB$dxuw*flN9zyD9FbaAAgBF zxa$_)K#cp1(zFKMjfbeIwy8%9H@b}+RFkbR&=T;3w?Zy@r0}Y;Zo}dCNdw(nV(pz7 zhn5sxp0+9TK=B6T{6fk>0mFXG_C_~B5m}FDyLF>IE zvp`=oC+enS>dsj(&pnR&Ghb7DYzhUyOQRDB-N>I{){!mou?>c7B6OYshFZ70-oSh) zyFBCRPPaU7ZUj#bLsCb<%tTS^hr*u@bxs;Z!D*y*jtxN^5{nvy1@a# zA?Mjh$rMS+6EP!YaSZB-LNK3boJ~csx$u;V15AH^D$+=!f;-JrClMDt;z(1;YhUHA zsv3AsP^&mGyIF`GLo-o3gr1B1gCkjiKW-gWDN#R~ln-#?HSy3>k;Q?MVN_w0i#WEh zuTPZ!$0tzHQ&p$+x^QjAjV5a-e`=e)*!Iv)uB1SEO=Uo0&kkwg#f}^j+<9YZzLo$R zUlnXvJ+toIA!MOg029M3=)Df=0}cU~-qymITr@l@f`jLUoZ`dM6_BDx9RSORV18&5x#=e5E^pt@6TcMWFELMrW|Y+L<{oRgen^h;OOY` zH_6Z)aKLzi_DLi0(0rWFML!d5Vr}t;`+x}f zmmjDFW1^%IG9a9Bu#e0D<+#{YapG`tfI}E4#SuGx5xfXPGahpd=>eQ8Hdu1%plP0F zDG2l!>7Fo6fD4JPcy$xs!ynL0re(!*30?hkS3RJ`UXZC5vL`oi0ENRH=)bMI{N=B# zyZq%pth@Z>|L>cgf?uXuzf(rnxA76Y_4%T^7-7poVRnr8a z2=*sfk?u%*Qtf_C1v53CvIoUwZ{&k1srZOisCG};AZ+ie3aUW+y)m>?2Zg{>yFV3q zUJR3&57FJvi5_n=%>ZIi2xM=W)d0K_1_QMM!T~dtyLnQRd&gE!((A^+uz_-v{2nOM zAO-#WJ#Zj;wg32RGmt}R_TCq>OVsgPzitFWPvB(;?Xeba9T)I>D9*&Ge!NFD0XJrK zVUfFh)FOTu`9VMi;V(Rlax=`^M%bR~x?Vr~_Job%%@O%0%wu%Ro^#akmpfbOd2kdJ zq8f|9fKLcRuI2^i0gQ$Eq>nr>Lh(~rS@Ikbb|Kp;FGh$GzZ9(SSg!!r>B$=m5Bsd{ z>!jitb1O~L5fs9Y0i#R<7q!0BpQ+5sF{>&oxtDBT)9l#CAf8g62+MS^LYgs$&G$n$ zh2063x+TIl1i-{L>n{ne(42@8-&I#op!?#T25wxGc3M;qW!TtW8@Yl75jfU#U?>@% zHiz|yp<~A1C-&yEgRLw#*TN>AyHWf80GaA(G8^m~e|T|#A^`3JhuD+(s_nlTEa_Dg zeIg)6y9Wn>LBCvJq`S zh264Re_y%$!6Lr-UpwFSMjMkpqpBUD@o%~_=?R*gny#`1#hO0Q6csdPB5ZI@?buqF z`?dHBA>y$sUjrqPS5{LS_x?Z32nL^fHak%7gd22F2&P3GbBdY{xA;(c_}+T_;PBr- z>{8PYY4wgRl;H3-4J(Wl z0$10B7y|A1sH6DxPY;obwMZ>YA`j(3TS3xidzY(L6e0liH-TOQdjs?%FePlSm_+hT z%-P@p`q*-J^cxgE(2haz582l|j1jiDaC=46AS!MYF*iJTg7WDVJ3bA=qj7UdyVWr9 zMuHw{J}SdpM^riy6K)VwaXdCWgSkIH#{mUOptb1wBGCl z3L6qRtiA+x%FTB&Wym<(VyusxzPS1#zj`|t18x49nU;KF&)0J?n%ri zQterhH4PH7M$J9e5t;|CcGg;MhnpPr(P+%Qu4s<*X-Ya4^xOYF{l!wg_?#0lw5Gz~ z6#1>so*bZO>`@8+8Kl(1L^L54LEHf-pkl<6Ph(M7;YgyX6@zZZ$Rr}5Of5yTHN)4$ zLi~5k%V0`WDL1L?-Dm~68K~w(wk!=aP)hE*`%(2j=wt3HjW_Kf6tI}Y z$e5%iN=>bp4cE6hhnDS|(?uKWJWC}X?jS7jA0^Q6aO6dvgawP2*^i%r+Pi@>89=s zqb0BKY<&7^jZ%MG@ZgACzJs6!zh9Sb4!2udEbFYaM+!g)*$+Ot8%laq-axGkVc@_j zh6y-2>0V8u4zLkNA+k+3w`|hgd`)1iTvD1XFFZkznylz3wC9+&%Y={2@4fZSbSoq5ru?)``>EgYud|p7rQ^zygN=BgX!;o`H8;igX$hEhW75na zf~0bY63W&c@5YC=0imf!XOGKys=2whjCmaKS&aD-n_8}(RU@usd}ixj{;KaiM+b<@ z<-P3sYVTL8iaB56O3Ssc)~S?N+rC=oPwgIkQWtXs)_+E>8CZFYFps&-iXWdkW$&Wn zN3JedPIcVVaVV_&LvK4eHhk6)w-wedSk)SMimwktu_PJboyJfwRmT~F!KJhIl*?ok z?WsNHGAn5%(6cU-4CM4)nJ#_65qA9IxY6ku<89dO(fDPsT&u;=^>tdw3ERb$22?mR z|F$nr2)|aBUX1VZZ+*E=Pmy%X^DgEJ6-ltX6y_B6lXbU!|5dwQOz>*)KegJcZ7=42 zwaTAb_04HaiDIcKBGjn|8q^=O5)C{^wu*JpS$-|NC$M@|XYi&j0-# z|Mjmw|K)G~<@~?Abgu*T{+7*m#m2nl_JUXhUWVX2M-o2F2yf3l<4=_YD&lV>K5FuDg6KpCf!D+GC! z9cUbs&MRLd8e*-f*iy8}=-tpP0tyL5(7+@;av(4eDA>?%zk7VPA;_U|N0(H`ue+I! z&-N8d78E1r)!_t@79OoyxMnI8M;dU~O+R$lIsOI(ZJ?`HLO+w@eosv|=ihxBg;@e_ zF50JLVF+vZP!$=-MG9|^C5R^xx;H@<%%+I%TQi172K6wNw(Hx+F2)bRS4_f~gGicm zKJGQV!7rg06<_q{?zP8U-cmK)=QmQ?a@sxOv*E{CIQu+MW*0amH`fC~a_3Zyo{tL$ zISyz-b}WE$WccVGZ&7vBg(UtaxyAYNL#O(axwr8$=T@I<;%dJ5F`08(3-b+m9f!Wn z9rq%SKojXUmW5L;^cer(zC{HRm{uM5Jos2B#`=ZVTn7K$c z7iIc(Y|)e@12ygF70OHj8S_QQDEC7dEXTXHC4@X{(6Y~$&xw8#sB8tPXHb8^kSE}f zWs%}s{YqaW29hdcwbMcMxOjBEaD75K4v%`;=P}3LLOars2O8QW8wOIF#Pjm$Fd-(4 zAImFw`1V(6(+FW#AU#IUHn_q18Z(q3UQP-j;}IhxrvcqdHW4w}(<9?sLTQ29bbKC5khBb6Qpz-XX#5uzc2v;<~u?b9M z#C`gcag9+RtMKG+^0+`kjtNU?2edWu+^Z{K(cANOC1XQDq^9&>g+fFdrXnD52mZN$ z`3bh~Y$t8fAqG;I>W}TH^{|q$aSc&Hm5(@pCE~pmm#%>~WR5$72Pt1Kw}&_aZUm>e zuk=?xv^|10{KF=x`K-0r-fs6@0KN%ra_v2m8yC4xH{v1~P@!-*vvf*@*S8?k<`k!? z<)FZN@+_#k>Hs;WCz3}o!EiWeboQZP*k>?k0+;c6 z*jtYD!?*MRKS030uCeK3D-9df^06!o#j*KjJexHyIqH0Ibc`M!hlZS%2!wV@k!7re109Sq-J;Jbs zR=o~C^5+vY%dTR=deW!Qg2wGlVXBnPQA74sRj^t3gO&JuZKhc?(?Pj`;zSLd5uSG- z3%CJI1fixGS){-0XW{l3TYY#`jCwZ5E{3#qoY{eh_OF|z`Sk3iUn&55aqR3WNuA@P z3U)j92l#UCLfu9H!mBU2JRFmTtU(4F8HMU{Q;?EB)cMpU@PoHQ1>_;I5u=K%GkUjZ-kqkC;a0-)C57tBBItHvc6f5SAKHic}u;Y zpoJ$R^r8wvN=b)I){ks{^LIbU1gl`Zh+|?Jy5N2GhmhgUxMC6}>>SHYYXj}~9d@p& zyVQUm+U6>QyC?1jBc%EvE&0Z(m;Zm(S!_FXh*|l`@OVTDk7y^5+_@Wb+pcpB2gJ^1 zXpNzrYNns*gpPmn@A$am8?8T*aJrv;Cs10v4u^^*flXEu8oIR@EO_7vJDsq@(yk0Q z*dq>Z3W&0_yWi5&bLq(8V8xd_ibxVgcSrb(h7nL^&Bw7m<1kdcOTrf@Vkz3f%_=~W z0A+dxaEt1kr~o&Xj=1ku%BT9*B(~x$(e5PCEQ_hIC*p@3*XqdGMKLWdzV*#!w{JnkLz@)KuWj!Fi(D3AN7L09KNc+GCB0@Drp zNLZ46)av@=u(Z|QQy_uRse!T9X5XFq;?x>n;|i7et4jc%!UB;6yTM=2S$J*QpY$id z8qJ;9@n+ir38$q|O*2T=4=*FGYJmFDfH?eE1oDmMlCrrnj*`m-J~>nC!8c|9xKA3^ zwZvE0L{-}85;r4fm zyj$ba#>Gs@A1w9mBjp@hy7XCVJaC`$mOWoaUh#H03VX=c-ti6k-APu=`CrI^Yje6aSGr}C}B!JyBYa5>VZmSVw))asCHzLg*g zF%-q+2_-$GDtFIKJ5w^{d)(&=^}YI>CNR?c5S@~65wl=#4@et3gKPu=&ht?2EXL=B z&D{}7dnFYD-Iyeu3cD#@azaS=fYKjonzbX-tR4b-|hJY zA24Z=nw&-DP3Jy*v&w<<-Ps7ZFQ+ktQDQ~=5{7G<+53x%5j|^2omjt*7eLZ+9l=lg z75Q|?2g_2gV+mxyHZxg9z}Vh;?^Bd0-$EUj>^hMEZC1qlF$48+&>a1%v@Kdp8Lj|F zdKipCt;(V&9PM-#bu>R>2M9aI^uenT9=lxgm&;V(B$o!;dk63(b{6a*L-G zsOiSWDaM~=UnK((fwClP`s)I-FM9FDMbfRWqfYEw^lZXC zvZ)gTH>L|}rI?SX)`V8O;k|*tE-&Qpz9Ix=fugVX{cvl~dd%I@yBUnA1kvH9V4T}I z1c=(#1Q)OmeD47>aFA*lqy~qA-pLW z-VZB7BNsF)YBI3o{|!=xdOn`smBvSJ10p0Y+*guX%kW z=~PYhF?1lTxdcVC#keoaYXrK)=$j;ShJ1P{R+3of2~i(OHftw}YHr{QvPv$S*YH_D zf~7>0;lh43oXg4$nWpD43b~1U@zIEl9U8KQUv;;S;O(2P8kT^Kz93;?F zNPgp?6v$;!_L2;sT$~s%3ZMs$Z71{8u-(>~bWkytH&*17&nCMhGF=f$uz3jxf?@yb zU%Bl6{wG}a&;O3g{`vp&eN1e(Lxg-x;x-t9(s362(eZ-nRJ~A@6;37Q_8Gsj^V>#C z;9H&9NYWvHJ4Noso4ixV&J2_8RRMmaP-=psWG36;<<6Q4jFlL^s-hus8xq9nX@|de zuS`*h_Be{8GZ3MFe1DhgD?kgZxWz&QB9JGtAFUq`0GO83RIfv%^1Bet#f^CzV}Rr4GpaYum@ifDF) z+Ffn!B$r4HSKv%cs3%n#S$?rM6%Cbq)4~XtXZY5se1I%R#rn%-1nIHX=in&PnyT8w zH6bUOH@!G_54_o^&N)?HE-O<)1lR$6xy%Mx`Iy?xpTF)L7lNA}qkA%iVayec=MM6% z_LtbbP4BB_dAW?43+3ciu3=?b$+J(}U-4u4brvV9AI%Jo@QehVz0+Y>M6FOs83Pt> z)nou^>g85a7fnV_-#y%x7}%{60(A%e_O8Vx6OV|zb8Qka^J(%ZUjfS%NgWIWA+0lb z3$LKNpeTo^B&Bf(>sRw`=r4Zcqq9JNNhM-Qw3>?6;Be_KT+H9J5c|Xe*;2pw5=MLY zOv;FrM@?cqfVTTs;(RU)lt#raTDP>)bw?vaUve|lq`XgKg^?RuIj%@Nz##)wJt9=z zN%b{iGv&bKB@U=}XfD!%p*XJ-;Kbf?;c0;HBDQ=?mrK@&M)0_yTm2HD9)z_jwm6e) zz93)i?dv+qmm0{;XA9AsBZ;$vY+Pqj7GM7q?z zGCCMC#~ubr+AS_ck%9?4xP%XE$~Luj&WGc?QRo%kE}*PU%fbD9mRjM4)Q2WjEL)$ z4yJyl=)5_w%44SFkegr;26GGFQ|pe@0P=N{m|Lx`Lm~QoUpD!XRlxPF z|<98>~oR~IaGv0ZmA7tQU`n8li3ETm8TEOvHq|Cu~tNdw0rX}v1Rj);9

  • ph0Hbd=Gb{MU(WHH`L~y zl+aO4Zh@pwmo&YPd6UQnfl@oM3_pH8P486HenMxu4VPw`by8krkVAXTQAwWw#O~Bj z4eGcvXT|WEGFAxCnwdM|{vH*A{67Ey0RR7ddCAVK%$D7?WLqwk%kneS$QA>Vp14vC zNJzS*OOk#@-}gNa;%D;6&t$%H{@-=4r7EP*B6h|Oh+ypC)L0gx$e`04}uFB^WlK&uhvO&cFMA-9LH7;ej`&a^{& zu_$CoH;CwY#lR?~ld1rnQZ{B_=FxuWc8CUg3Rie{gAcV#z!EH_H;QfvZoWgzjJV^2-(FRUZhYkgO zg(NdTn5|hq2s9~ds&8k+~!a2b``-pp>0i=ZAb0|>s*`Stj%|gj~qG<+I%>`+c7|l z{1epT^$u(g8Xr$rI@?2bD^NXltfSub{XH&Xr2=8aBe?81g1}DTM}oU+;8QGCS|9h0 z(jpaP!CXObdl28?n8G$zR9LrWn0}(z%pjkC`ImqGpMUvv{kOXf|MZu?{O240%iaFt zZ}VUN(;xl+aJRqy({KE_{`(*Oo0EN1{q?We)VJSdD!crD{BtT_PN#wmQ+?+++ePGC z6sLy8F4G|=0}ZX`RYxu;>?I*Ctw0qf{u8Q#+4JZM$fgkxY_()eqrC4I3|4MLH%@S$ z#pQyW7R-o%Nk5-niqm+`3uI^3;88TiWpH?)P@=8H`I8k1WyfeJ0N8wfE|dtG$ywbL zf;#w0!Z(_HVPZB=z1fi9gliM+s-ub$zDg@)vBHe&@glhv1g`JYQo~}6wE!g`<2Xa) zDX6GW(CTgt00bL9PYW&>2^m&QBm*-&$w$L218iJNdg|Q;`pQ4u#4rOkh$FyP$@Olj zsK^zMG#;)FYxpX1NFJ5g7cC6k(9JsfY;D}@Y6P@`il3)grmyzlBgI!)#>fMhqp)0&)afGK9i7qQk zXs4pLpG=_Qeg}((VA3^rH43w(t&nXfXr{i}tL>4COd^+mf5$O)eq=psmT?=!9Mv zhE=$bV&1gLRFP?=Gruz^qp1WfQ|CEz2n;pzn2DpcAX;c zJb}bs+#eEUEa@J*!J(O5_YRh|SvBsk7qeq(MD-B2mDiqOjvA=8;Y|`<_4L5KLji;% zo{1utuvf{cof1V=4caC#E1HIDZGQB4{95foX+MAebT4e3r z?=bU7YG64VG$<=u+<{)&)JnOOw4A}IJU4P}sU|zY6@AHfb%#{Ky|VJmuCF3syo*s9DbmR4 zAX6d3XttJcX_7jAbXd~Xi-c{CeysYkvIF7=&0C30FD^SXls40Z_XJmKkU){laj@e{tb=KZOEMTsk5Q zH;=PK2E}fSW(+UXN;yV(azni1JnBOTdW< zjALC7?Z5@Euv-xmgNyrQc0mrUZa+XB_~JcbLI$zubS8>1+}6Xkb#WR!C~_I}1<(x^ z<J(K>d` zxW3SC1Eo`-V6KH_q`|2rq_Vg&aq)@j_Tw&B1DC#_O`#F*+{R+F9&tlGX4O0%yGfuz zEPOdVQl6sL3cs<*t9IW_Dky#-7H~Q%=T%<%68iR~U1>{+A*7boOF{Vc)1zQ@wEd&? zJ`winzqVp)M+R$0?V(n8ikr>-NNrYcxsww)k@!fYZ?I3%dXgXMj11|O4Un$l-_=I) zx5`}niy_K<{{xx$_%iyZSO36pA6^DA=VE+Ig5P+rC%e~S-0S&;wT8zzciW$OCaJ$s zVMOs>XLWJEa5{xp|FXJwk5_KLUwGS_d;R2{-hL1zI92{%ia+P!;--`IKgY8Iebmz5jW?@5=Q)S_nWO@VX>cO- z19YPbOm85U#z=yvwY5alywh;m8#&wFspH|_e49w!c5t`axFdUq^xMQ5qH2`%UF?gl zdzO$6m#h|6+(D?$OpM4lH+K~7H%j+@34TvAzma4L(YW`q`{CQY&p&wG`+2}>?LxmX zp+8l1+mD}prhnsp+do-*a>j2IX30E%{Ic6`{f&2Fd@{~OJg48tg)eFUu^YFa_!}Ai zB5p3n`SBZziM2nO`m4qFH~oo`|JSgaE8fi;`-gMd%{3uLe{t@{|38Lq&iN&yfA8Sv zi{e-8{24cQeEh`Dmv!~@+08IG=&hTv@!P=}L1R$^!H@@M;q*oSR(Jj7zyABW>;L@Y z-|McQ{=cQWT>92sua6Dd(2|F>?!%|shqhCE-kw*Nrxlv6^XjhtVZPh3*T&bIJ(32o zXMNqGTdPJv%#O)B*7+~=gm4KZVirrn;`wBGTV}tnlEE4IOG_u9(@SlSE>e*-Y{Cmi zEhF2+?%K92MPHPuN;|Yu1<>Ft^3Gi?M9@}8XHG|%ia2HLS0Jz^0-;v|Zq<9zy1PP; z?1M*fM9n_Ml8De#NIVl4gH4}BK?h0^x(EsH6rN;BTB0;u%e3K`HE2xQ>no}@5JGRI zd4q5PG3-(=qWHPU75XZmCt!~-57W)p5JEzNK_y|S;>fqxGQz6L347gD-(%9`Yf`k@ zz4zyU9w%GS38c+Dhr_7|&`f*{`D$_B?yl*CiO_BeGx!JzNI#2oE0VSGoZcqh5qC`= zzmYe|z2yq)TsBr}lj@8E*hUe5v+o|}GScpnXxjHPpIc-b)3QYDs_rC0U(8dFOd*W3 zeVqIj5A*xmDOhwl(^RG(A)B?snO4+u=Pzm;aQvnnp^kWh;jxVCXx2E|o9W5fTKI#< zt$o2U=3xNiNtIgEQ2iY^Qjim-!Ri6>Y76DA9$^M#J(MNSPR6m8M#z?Evr>sS#+%I8 zPaoVx#d4(zIHqc^%MC>v+SltTi#B1=9nY8u9ycng;o+Vkau8aJ`>D9P6#VpHP1fV; zuC(riV2Y=XjI|X%LKzC|GP4$_p1%#2VA!2{QDu3dDe(>kIFuz-E~nfeUOd)&0+Jd2}wGzn&Rw>`&d4qK*7b?j9jO@ zWg$t=9yLl7tet+|?Fp@9(Po^|`(eB-nuiiiAKsbNloMT~Q5+tmLfi=+sCl+TvO0g;d2SoGTJ~z9~`G86ZI@f_{mWp zvW$GYxD}ug(K!-`=^0!@$R{Z}rrB0*dkAAwBb=_mWNLS$TmUc;l^UM*pu08{`Syd# z0xD2lT2brLiiJ$Qcs!}s$cEygTlpm)ijHGx)MEoE0FwfRq}Vlr`a+R@`zg|g=ye~9 z7-~(``W)98hb_w*ppSSJhuJ7E)!5V8x_1eOmhov2a=bi2ADtl7iP8rEMjugCK@Nf` z<0CqZ2eN1Z^bAku(Z3R|e!P4-P^4s0oYe$|j4>BGoy&<70t2GaUn>qh22VS|3|Q$VL=Zx>1Pb#*NR;Y8z98Iv9r&@t4~mV>J>4txe0v1(6Oh%T>#{piCn_ueO=n-j3@oCKyus*! z9MxHCXAr{-<8>*L?qYJ57Jxd97D1$FWka74GPc9eXX$Okv+#gOaEOuirW6VN1R1^& zvWGYa^9mE9ahJ#M;bEbRmkO;|5o&SM+kk5TBJ#BDa;Rr_6VtLI{rj@i3E=}HL8o8iDM9{Y_IJ2`DA@~)JfIXRzj^`^+JE%T40?n{ z+Vwun5~M!&E}ml}PzK_1&eRtX2V65*o)(0R?y;i5h2>(6wcY{Y7AM@qBP@ut7i z!-$D&fIV(2V84*lC5ur0|S+5{YTo}Dbk#wQ=(?f>xO}rNt`w9kr z?jOWA_A}%@PELTEx?9B~eA!O38TpTIBi14w(uYt_j~-&SmrWOfG!U%u;Z6NO8b^Z7eC48{PbZuNl=<8Ne(o=X>yo5-i#Xk; zG+KS52c2(@gFl77yYMv6`a5}6!b#JWz|wGqGIVC0EFBqpDc@qUAT$sRBBsY7Vue56T@Z6XYFmPra>|%Pk6YpL}q`P;!f4quJ zRi|R$qud&SMyy8IdbG=GC=r^5T4k8?qF2QH{txCy2mJ4U`1f_d|N4i&*8xBMe@h1l zHFE0!^8rdt)AG9L`C#~-{rmYP!RPsk-R9Lq94R%z!{ol@_>|U*1dlLTQH}ayL0q=+ zdlqgH@Pgx47O&4}EqSRh%6^PL_VZOpeX2$&WOQ1p25Cep^LI3m7N;=^UV#uT46EMt z-KL`ep=Ux3=?!Duf4WC3VJE3)g~W@{p@}V^~Tqi04_F zISSFXToxW&612V-9=+Vo-C(lKu~VyY``SBZb9cizG)|7Kc{yzldM>3qS!wIby122z z!?_|?6YZeFWSj@aj_f@DqMucWqhi=}F}bX`d{wf?$>{kl{FH@-V>4-kPDD@w3g7Ef zlKZWeC?8)CCQcG}E&DfSHUKg6=UoQDiO#J{c2hrKH5#W=V>(4% zi17heXO-@mW=ws^+@l&L#-Y{Tt`Yg&RomlSkNQqB=WmR1IAl=K_ zx1M{g{}%uN|NnfKSq{WZca}dSKoJ4N8gd2lh36?!Bs|Ykd-k2j_S~MQg|QG;!7}il z?~hP|5=*D6%aUEE%2n@qUVff141L19T$d;_B~jiit;*_z(-fDqw30WJikq-$T;>bE zrc(sfi7XBk5p!fi=~&Uv+!=L}l?913RG6wn+Bt5e+ztW4LLXORuFQ)dZ@+bfII}YW z*4LHWcAr{L2I=8OlkmK(nJploQl|kYG}kz@tOw!@i?DtsBCRixsNTpznI#O=8#iPR z$0YY_h}cJ<1sV4ffgS&&o2 z(&_0QD0|++A5b^pxq$ucjOY{$gHZYUnBnMV@pq z%5uY?zFcl$;{a_FeQjPvK~omcl$RGC5VK@c>8M3M!fGKNf|;V!X!-zK_u#-Ec-4@^ zOVE!r_c%ofV3X3QSqqc`FcDejBSe|IbFqMfD7+h)o!5x6&X+%uGIs68%WrEcqj|gd z=H9mf#)dEN34T?X*yo^t!~JNi1C4(kMZgh}?v1RRI?JAf!gv~DFUJ}Qvq{Hms;;nI zGxT3ydn^d#UUOd)w^Ai?r`Td$%~He#C>#plqJ2v(@IXYB-B!4s&eOxe&$Hz=sKe+6oLI23a9(#VZRC1&bsIz9@=kZ!jdk*iEdtI2|ul%_^ zLfSIS&mUm~?ES7-iexK;?tL#qroC{#F6CDNVS+NbTW{W8#P zL%MrV!bArn5)Mq`UUh7EstC2QKyMrv`Xl24`@l7m@uJspRI%+KRuuF}p&*H%3ojsx zr!9LT7JY=GCmNL~9xFRsQ4@T}K?xV`~(t;@$-y zm3Vig)G$)@L4S8_R**Y#7orS5kpG>e%e!>VY@iAnXDAL=m`Q0|IN*ZeJ^9MBtXq({ zE&33?+_xvLHe{dEIie4%f=C2^9#1<6@_nxh_3it6b!3A4tld6SwTCZ*2%hI2ynd&i zi#wNmJP+wpQdeH#5f0PL)ts^yZdywJcSRqHkregm^{>rk_g|i&aUS3D zp9Gx4z^S%BZFDFAS-v)dwqKM3WQ?)%FjaSx z{dM5>$Z#&0Y&Sj1K@~Mg%cJmE+^8X~MWF`Zb>bycup`T3dY;cd0rk+nbL@&rn~u$r?t(5;?lC2?>eyd3hd6e3Y% z@fnu>2B7*}brj8gC>}k!1$l4Fh>S_}N8V+-PTn&_mx7VBN{^L zu?Al%bvH9992+?@oUSRdh6753!Wt-yA~T4uLY#6%2$)5ETHD^y&xbfk`EZ+&^jw_q%pc1t2k#idbEBY2y9q))u z*kfrvub>J`zbPK&z71QDAAN7fa@j0yRc1^1oz#!*E0MR2y~1Igd_~%`_&erBI`{q3 z)1+t-zYS+ln~D>!hJeni0NkUh&b)v4hrjzDfBf-Z|M=sN|MNG0{P7>(_HF+7_ka9( z{fEEy|J@t^>u>&S|8o7ezxLOA_+{fl3y0tjTqT)hPW z=4NQpR0lH^T{NUxOhTR_L?w@v?~nPaj}e#qc_7PTk=btET5`^39O#%wpC9_h#1Uf@ zmgQWIzZ9J39&>~3Y8#3*TZ+^cptU9O-8KOfKa! zSf@=T1R5^$==f`+256j=NvbM$Y(E0n4KCr^8Zwoz^Ei9_g|aUG$BWZ*ssz}}jPH9V zZ!RrYkRd;=WfH5;7=>BhCn}zFAA4&_SC?{PM2KB%l5x@g zd4T@bHX|+7uGFaYjFZETl|-7k=(<^-%IW2Yu+?K{WYLn2QvBd|3vJztO|8;EbhZT4 zgS!3w?&yfH$nD4w{5a*>^_p`bMdaMbO)WpXr%K4XPLV?Mm972Helo3zsT>y?c;Wud&8UrXfQhg$kAoZt(%x2pdaB?nLCI!%PaBDNm zn4S<_SP~ji4K03wSG|}tv*l;N;QiCv`XI}On#NA6VT!pLJP*E#ctsq8zLUc z`U^p-^OvRu%>wj5?xlzUE}d`Ji%m?;t7gXGg8M7Oe3FzkAasZ7(vtBTd zS<`ZK8~92u)uGVxIABcxu|Gg*VitOOtaCj8;MF!7H$J{$-)Y|H`bz9+*y2uWBU7Fn zo|Pw>OP@P@R7gK2eL0G2@Z>548DXnqcfF@56XH;6vM6eFz|jyFRD3h*UX0g-^j~_j zi{)OFl!KpJ1qnvi0iF3<3%DtWxZDF{;WCMzB8Swzih?_nIgkhT8T> z+{-5`WPdTvEwx@0O~QnSB$Mw~C8zzgkuB~YSg!4{x@>DQf6|Ckph@@Cl@UJAs6|SyR8pP25CrWV1(~9CimRDy?GAeL_-r;w^tCFQRvDbLhjAdr)s-gY@?>QU!1DptJeA=O( zE7UTV#U)*pO;NX%V(#g^>Ihv-4@AG|hx5T>d}bTuo(mwNw@y`Cae0*%b4QV)h_t`@ zc`)sUzEEBe%Fz#2D!I@qq!0tuvZj+4gCX8rQ-AyP-iAb1#PZ`bE-P z9K(bFy%r!O2>9_R712YW%gS--T#v-LjSQVKLS)U-I#yiZ1UCl?3@*Qi44zHLHFlJk zYp)`l8L6|TeUHCl6b|YKsM!ugCa?%lxUCBTuZI9iI{sSj|HnIj^I!gL-ud7E`JeO7 zU;6(V?~LybL3!tONS3`0SLTjG=+tswmi=o(hea{D5X}<55H~@Do!UuSD^W{!$WNrh zggs(@qpH^@Q(-ds`(^>a*cd0V0n>FVgXI=y1&Gl@ww|;Pc7m^+`22p52aq-mR0Um) z&qwc@&G-69h^@pwL-8i9{}AI;6{uMBX$r%KnMVkZQ#$cM5*dHek58D z!A9p2T9Vct#Qp@JW-)nP`Jr2@l%T zhcedzaaoE;AnAT@Q3S=p0qERnP1Dd!M&~fVs`OjIAC4H9FbJo{j6g=;O|WsfhA0(w zWZrDvbw9uaB%EWzBoH3~g`2GMsI`rzZ`D;+aN56=f@|0KT=7b-W$>d7c?Cb8d7(hx zqQXIcwLIKmk*JbfvheoGY#yni3+=K+O}a-S<>0D&ZlB};irQin8C=#|RGK^97hYlV z*Jf}<(ueh~DGryYF0dnG_x`*!^%kHaV1JMamFb0)Z#kZ0bSGEJhCL)j zy>m>j>Q{s<*bxK#6cKL}pB00s`O}r>yKYYAwfq!g3eq1v8x#~a0LZv_7Qai~M6~yJ zRw}Q_nL`dTY=B3Xzbdp;Z^_>CFPwq>p~Jk_B>+)oCXWDUse=NspZhcbLgTs7#Yulnl`5>(Vw)nD-+s#>)8GvKo;fv`;gv2^ ze~y3;ku`!TQ zhb5>m5ac>D$#ypin!T9hk4gz*X=~7Z=XM_sLL@h#Ey}ZYh=W}HD8dCX+CZ5$U_{;# z)>?|v$0htu-9`ibM^)caLaHVp&5NAuDQR_5FnfJS09EA)qZLX({$LA)r~aLF`K4*} z8ZK*SZXG}p(e*Qt2n?p16ttE8jDVMU!YxRX^P`f#z{|_gbbS*`rXeeV@-CX1V1}ldoxf@+vh8 z(UgQDYd;Pj1~9Ob2EPA{I4nngH#6sbz1``zJN-CkS`wp>+1&dPPi%C>%PxWe>OzN@sb$q=s`9hHv7&G}L)LonVJO}Gm3v1hS zR$8*Dg;YT6n%rOoy6hnXNkQiWGR&L)k}HuPXgz-s>L93ND(CES{M_FTZA~n!$q~by zVxz;iS-xrLa(@I3AQy-FWX1zMQSjzuG66kil~hr*ley&6CbUj8nxSSetD9}ME(_gt zJ!phdVI4#?vN>FW66q(Jl=@(D{eY2Hk$YWs8k|}&LKPsX_AOI~wUzwcPAvyaqVM50 z)}51}HuZgaZ3qb7FTg{YKw;)`w<)b;O_hCRN#i|h$dkgJ%q!YHqh8GAEN)-oRN=MR zdb3A|%7vwJ4lb|gl9-yIStAgOde0^F6_c{Q9}IO!>Q~YsF#+LjSi|v&;5f@l3CE_K zH$km>Nyfqhy!P?4vgmV}tZ{s>C?m7BSbop-d9IQ3F`lqHNFG2YW7w<#Kj}d4;U1Cp zfE9-@xKKNMKn!aa!Zg%77jiOPQG#Bcm*G1QM|u#kvS}`^lbCTJ#jDHD1$Fz;G++xO zMzg$VS44h!C^bsO)}KXv6R0&uNU+>@^n%Y`CPgCK9Bio88Fn^ZTJ_DS>C_qTP$~xH@!q;$%b1?vCHR{Nf-F&zk`op6Z+6iE zAi@17TKNi%5v3L-Z{`OMj&b!Rb=^7o49%s>>qaofDFv;rDPuh3txLQGr-R~#iFAP* z^SP9CdPVWRO!9!7r3RWGeL*Pv1!Jp0m-!bh3$;KGVZKXcOI9`d<17yr>>91rUmsZM z!@_>#2_-5=&kcd9yCIOHiBQwc!kaJI$_3dRKzev6u76`RquWouaTSz67V;& z+|!q1ks+Im1vHc21|#+Rzb(F`23yFr75EQKa98>qJUwDWVZHEO;0+ZIMC+NI6am5B z9&RpD2bok!@*;b(#cHyh_miF6pHcS%@(l(=qnsBsorK9OgV!zNEB4aKo$Yy_?fcB9 z4w^R~HD{0%Po}n)T3$CpE=;i`S-#Q}y|R77-Lr|UbyL>mSx%Cuqr$#OeluogCB@k8 zL2lQNzGt_#ve$2Y(QUoX_|+!nuW>%I!aTC-C`m%U`O@#$uTOZ$H`jlRx*y+s$2VX1 zo~Yqwsrydwy668I&%^8Tn_Kh?;-U?b;wL7>7kNlV`!m>SK%95NDdgPva(Si0tiYSY z*0Fz$>(Led(N#}LVi-d)#j*&QH!50?`J6vx8BF+kXDO~d_L^HGx|6bhaeu8b{aYFJ zgE|y@*|7WG2i?~G7xzymry)!Fr!lnJLmhnj|L%V+6S)6r?1Bg7BhOmLFX(B@P4Jtc z&nDsV(X6CF=`*$Wud|QuN5;L40}AW3In3tDz$g+O4%@q&3DbYzef75xtMIjCK$V=uB&CdZJfkKto|AA9(=$3 zNs6I{FKd6xN!#y(6cX?FFeWBTy$!oV8uHlkfC|uzK6&#;NV$>@v+ZVY7GBwTliMmcf@+9?QpRuYt zyWL#O%-qeVr5_f&uuFzq$8aOjId^WPe|_*fb`nENI!0KRoAf2=*#iXQNsmTr%vD1P zcFBT%@JwP2oF@_!P;ftbZyIlf`5b!YV0dqU{OWm=3?;RQA7}Ix#ReC|p;arz$X@cF zFDFllClxUfp)8@{rT2kLeckaI+n&~~pG|Mcf#CxH zCwA*kwmUZJoCtiyD3_=B+(G&E`)&x&mBl$slC7YIfBABNv`t0FcIs^VB>jFDeIF7Q zx!k1%d^0xQ{{;X5|NnetTb9PQX6@yyY_TAxk!I1|~? zp4MM^Rhz};s1_wxaUuZQR;7ZSs81POCS{+=9tfHbOo^Vd231*(Zjx1ZM)MN43(V|p z_|mujw%c~W8hbT9u1y!2FLRfAssMGY>k_F2N&}K2se2U>1u?|=s#4sdJj%@&%)QfAf0zl%fQ{=gp%_S{W{`qEL zFVhmJ5?|6te`)Hc!LEEXv0@X@^Vyg6m0DDc7lW{fuzc=3WI-nwgnk7Ajasyk!RW;q zUpM=Rtd~Xi7g<=Tx;nOx&Mo-e4XNuePQIZ4UqGP05R6UL*uETYZjMMfJ%tFMEcILs zw#j|brO~E{ww&pARw4r2-Iiyy&(wLwT#nYLb*);62d&5Vq zS~FE-%SS3u_7gX(L)2vEUk+{!eJmeQu*Lp|Ho{K#-)s78=JJCCNj?==xRD{u_OobF z)WyYhuiD?Q1auJ&nwsu+xcv4nS}4fjAh{*_5%z|kQ6FaopX5#Yix+3+hzIkYlEshT z*We@vK)XnO12*b#@BXZ+qhfMGb7&OOKJmD+9JAUeo~FE>hIc_#q0>YOO*rB{K_S40Bl?i?hwm`mX(tMaU~m({o-NhaI&W|w$P2DT&(n3e=> zfH!rpydc>9j{Mo)(FF$6-wJkt5u)L-vFW=F+hwu3vj4Jd$4zJunvjfZMIO>B84&I? z_jm{wW>ABEWv4bVJ5{4RQ-ITPCte>ni^%S0w)Yv}tC>UUR28F0|N8!G4DL$jk>Ti^ zVSe!*mZ6IDF1-qAGqrGm_yHw<7dG4a@8a+%^sYI;T^w7*SCecxd0RP=fl3mZoy5PT z_8lS)&xy1iqJ!R`dc_DL%LKKpU#rM8hel{0jK$64y zhDrR^oh-M7-CSeR!J(MB>TC0Ec>0^l9uOX>sd(f&BI}P*)F8;kt;{#SQ6V z2uElprg+O%o#cQ3)o1EPM-eJ{h3%xewq%2TH0>v0``do@Bje7b`oR-s!N^R$>ju8k z3SPNzs4)?uefcD^+bVL^<1V1CtVFXFy@Eo3KMo8>#i}tl>SnWY?&9EMeughP4Lv2! z)s6Qh(PmCi^ar>2r*X6C+$g8faNnedl7oHerdcUTdVCz&Ze#aT&rWWtV0IgvE}cC~ zbXaGOWH?xZS7EZ>4y7+WP=R0T>r2OAkW&L8Fc;d|{Tfrl_HiM$cZ#RDLgP$qNZ9I) z=BJn|LJ}Lkx+{^I->78E3l)x9#=^g4o_g zHCRP$t_+I%qk}saO-Ys+(?nN;{Y=+EA^j0nk-NLaG*H`g4M7zL6P}0HSwq~Ep5T3v zjlO`|jnitNN}@?hWp}Zj%G~&pfOrr+E>m;_Wmwsy)v23RHp=ifRm{^z%j@b#Kwkt` zaAplSCw+s`3Bi%6bw){<7OTwgEnXpxM2lGBg#{#g>0q9XY)Gf*A%`>)Iw_+gVK5dY z1)3Z>o@Ed{Af#~)1_-X$c5~z#SMS|bOq(=G?d+rv1FN%17~Pd)BEOGt83%mw^cd|* z8b}xCLDK7{(>~opydl$U69h=YNR7Tl#0>~D`3fY2YsuDKkzpeEI|@QzP(I#0 zpJ8r8DzW=S#{|b##FvXSY;R<_u$lpuZgtKLd*nT>Jj9ngk-{GN#G@h;|IHZHd z)S#(kA5e6C9{-p9M?WZO>}gkJOB=pq*W5y2q8owhoNTx|=c%+g%@!Y_fb`M$Lk zzD{%QC8izX6&VrF^D<*`j}JO+VbkW}g*T&_2jF5RXb-!R?`owRMWQs8;oU@d92UW- z@`af#D$=2b*R`R&@4`sX$hBc6MGZSNHEeejS41=%Z6GO08WhMv+m&J5d-m0a+iV8I zK!v^cC>+{Qj0F$z6}r|I-V<{d#_}vcJ4s3peso-Xn9HM*C!Qz6910qo$EQutjP~h* zlP?5AJ8-aJXN~Wc1;0PvTCs8c%~RM@T{};oUq?vVFyHmeHYc`$Vaw ziyKks>i+N{+w|Ffaqfl166pFFnDz^wbm8H!o#li{@fz)J69gT?&q3`z;8TLBqR3ujAFm zn<2;NK7Jz}3z?1?ad&nXp3BC*0;fydh`2NZOHIk&Ltm1QZo$dS78OV~_J#x;i!KUD zr$^@%^k0=85C`d5HGVr$qQVlo?kT>_)c0mvCRO=_4QJHHdL6UC&%X=^GA%(4aI!Q7 zUPo#vXb~!Pc$1M@^Mel)22esW5!7er9tpL5*llwlvhTJLJ9z1$YV4zhdg;2U?Oo%}ga6`l65J68?jiJ@nzmq-Q*4O4@~ z5xYwb!`*X`ZiSu;`EJv%v=)-QF`5%O^H*#7;6Q6kXJdWv>m=M(6Isd=X5raF+J%K- zXZa#nw0ux6Sgi5l>DxJ=oVwSTG*DYoU(iI8teV!;g7i#>9jLj++a5PDd@Kw0I3Lub z`^tMS{bU|ck$Cc@7_WE`4E@Q2E}dt91%qeEd+5;7f;v_u?F{C=Iy9nt#(vr<`6c81 zx}%uotasiKZI2ZMvtGeeY;^12+KPJ1rfmj6;66hI55tsp9`qr*Sit9~=%Gq$sYT59 zQL z=X+$0LeF9D#k^j_9g-KsIRz^+9pjM_?LEF69}5 zUbCmz0s9qnkfeg<$>Ckmda?{;OsG`Ic1hNZ8($3d?1o?p=w=h}@3_b2ES93NACUgPoSoqU-(x5&vd(cSs z1FpVVtq)Z_UDp&X_S)jTcWBL`4wsJ2h0eWC){9M-#32o{)Pa%bJxA$*!f=r^=y_iS zrjgd}`4g<-`^q%~sY7Q1**mZ%?8mN(PYL=_+lU~V^knDPVbC`n-xa@JE#$i6qvrY$tJb}MWau>Kb9;zMl%FJ) zo8ctMMLh#E=wKgs-huLhiLBaIDfysq?;{)uS_jJR52NFcV0F}jp!HRxmG$+U{|5O_ zK$-H=d^>unx>gEpe|!!nQbI*rSHAZ>TV}SUsxR%#`bLi8qiFjp-&< z6$?*XB<(7VYcN*syuZ%-iQ5u|{P0JSf9`<+LU`{%a7s#fN%HId1K&SE`N_-<$>|Dd z+Mlrg4I9+hCtcMieSH3o>rc=VbrlzNeJ$o6kpBd9t*;(u zsRkZ?{dLaY)H=l0K^vk{*;$DiZLe~y<{Tm$d=NrWbZV8DQJk!5R+na!G^HRk(} z)ivz`>2@R+_l)n=d1quY8TE(BwYf|>mH$p!=vGOkK2&Ccf-B7i!+E#H@7@0a_fLpx zE6Si>WI^Gd5dDFK6}txI9+ZDV`v(>z_JdIOgZTX?Sbs;fM0$HZct_jc(ESPh)aJhU z)Pf!=Ve-@^UJBhVt7iHWW3o)oYkVG&xQ3tjpFhF=1GF_o${4OZ_VGUsr@ob{E7#-t z`Eeva_1=B%JIWfE30e#&Qm2}XIl5b*qBq3nqhA`#zw<@rKm7lF(f{~|fA5R_dj0?M zMgEEcod3Bj_L#IJEyibi#@O|$-!}->x<|f0&vRwSo~_lT-M8mfixV{#G9{iyo;_xu&QCQz1zW)9K+mMOQdT0~quk&LA4o*Y5Uo zkrk1i&N-O$uGn!19pCs54tIokmf4#|ka!uHSDX_;=VQ!cW#QBuQ!U999KFwi!gI3R z{UHY1BO|$D%`a!~&noz(nK!YTwk(Vl{20D>5Nn9?TFWhB7?1FRR>B;7wvh>8B0!6M zO#z_zIKOu&#?V>}y1Sc)M|0HLXLIu@kBFCrsr`)y+}jy2e)sDrp6`UP2}mRZ0zVP+ z@(A-Wj<3r(cxw?CF^0I2WKrTZon-pOfYv+~eFLdLrzW2dOh-@ddsOice2T%$D{8g>6nKr_2d(G7!0Inf>MPn6e!@S^@Sfj zgy&B8^TvDyh<$zw-Q_9kd3{DempcRdIh!cd08!&|fObjA5ZNY>!zP>pmb%cB=*Va2 z;dhbhg|Mqho_$m2W1k8r1iKwnEs(~ndU@`sw4IwB20tkK0#QvqtW6gh-SF&K-WG)< z!TNnczoN)~g(q!hPbDZ}+GvyG-jg?KiP!8&N}>6(wxd0|dux`Uh~Y-2@Ie#F@Z z__dB^e3C}M#2}-{9~}rK$SuZH5QsjH8ao-qP+pithHXAOsI@)PCrds4f)w*tnF-e2 zRCBixUMI2{-e(Q_k{v-2yQq0csY54dyo8R$83?!<@ePyVy@H;>3twqNKw5@?WeKiM zt8;kGILuVtQt_)PR_QQ{0)F_`^f@}>loNo-T&rD&k6X&Ds(}&K@|m+^mT)v!u^t~% zwj`ACsc!Cshpc*oJRKAMSUSHTK5Ot0yQCT>#o+Ef7#O9VH8muuY7iok$_S;4j^cUP zn7DC7TQsi$9!xv0AXb2yz;7@tNtm}-?YRNHMMq_*BV07li%>PaK+Sx#@g}7*y&heD z6V@tq_7+tu+<5s+o*m13R}B^N3j2L&=%n5U#Y^mEUB3eI00v!vTI<;qJi^`=&v2hG zpE1hioU0y!&#G=@ft*Ewh?dfKqcan<=4x^--;OWb=ICQwu2-(+$lA>KxlZbdE7wpT zXUwbh7h;sODvTzkC-$-X?GMtQ+~Pc(LpC)ei?RODS=&&oe8nejwF`7t+Qnc}m1ysx z?+t(?{u|g3z~0ot7U@WOxeSVqc2nbArX4JxHax~lMC{YX5OpujeV^^vP9Waiy?wF| zqhj8uvNHZE@{v_YoQaef!p5R70|d0byGPLZhmb+mZLjHs-QZ0yt1j*wbv9KA15z080 zj+jKn&ffgSNg>PNhcT0dHO!&HFVL@F2oGo}=jeBEu0X>`bL^jn2S{Ie-}I=3`j5uC z_b1u*M;<0;`;goW`-P}K`)*PV_F|Vl<|KDR5k#q5dgmbqmW6ycK(SrU20sTKzAzdBtWU20zwJe)lq~Bs*@9(k1;3Td}ZaEnUmJ}lor+W z-IaYfh&Y~~@0O+6tr8w~4VORI&Tfaw@r5o?b!AEhLy@+F(lS!o+s#>UMB1J(WMfA7 zG<%Gq%c1Pl1l~4acVV*{aNp)|tt%?uE01L33bK0RT@6okgXQHd_lO4#{Tb~J)|@*Q zl}oD=b$5u2;t(Lw^g&CG`gy?lfnH1Y&Jihfs(p~KqSRJCv$V*`B+90KF&nToeC9#L zm}lAmVGh(|4P^S9hKlHuN`njnnPaHl`MVsTq?++hbSQ8|_(he@cj2NZ9fgA<21vF@ z{Fjy7A58HN4;O5s(lRP9Q6Y&}C_p?@JU7OAAjap-p-Yt|>J7@KCgDuL1gZufwbA-v zX@qP4KL7v#|Nne>M-IeFmRt*v1Px{&2s%QqByd6R2GF8d^uFlH9liIS2h^Fg6n}Rw z^aacm5FnFPnas@h@tp#RbL3;ga{28x^f(w?!<^3`LGL1z;kAj_ulBn&gA?e*@* zr1{r>Etavjx1}D+|MAyce!M|LOqBh(r>YfEa?oS@tyIgh6p@{x2tH$E?9mjS(d-QmaR@qy4-!9a9VI<5&{VjX{(q zwA(N)8Z4Tb1T)fu!59sGq>mSg5;NZ7R2~)ofK({GxK688GK0G{Jv@ zbz|@|;~d18PzX2L(N;dBpO-w$=QgAfi9p=ZEq0VL2B)8~&Te8?7f!0asKFI zdMMd~GEc8mcsnL7uqFJ0f5>3^2WRn=XPEZ4(DQT%engmdpfgUlcFZwJa7!Z6!xcm{ zH#IS_KTqMA(UO_pZQ+3mj!nRslYoa)?Sh4;ai~qONLGk4W-~OPb;>660hjmCr%p&a z-zdx&W4oDA1sE&Zkk2obc}JbZnPlQ%h7D2(;3w-Hl)oOXZde03(K}?)*GvV73}+?i z=#+t6L(f>e&iqS8De8H;?Y(P54%wU4ilRY4ueq8rJQ%Ko>i-shb@2%)K0U(k^ zRMnje#l|*V6j5}!A{;Q)O=nb~U47HJWB@9@TKp%c+~lI%C)C#*XrRMZ@}y6;&gkq5 z#rYy8B)X@YNZ;K&FkA)1f?AFJv!(M{VGh~QG#54DG>sI$p2Kd6%z?v)>62=Y>J zOzn}#>fK<22oPc~t)S}%XX;j%{pKAaj9&HVyQ&{EA2U?0$E?;S7MxHN*%ugE(<$!B z11v#2KpeG_!4p&&2QpU@FXL}oR~8-}nb-tDmZb$@BctXLK583*$45fe_V*XnC~RMj z6a!&+R_Wrnidw_!+N($}=uSP}tp0HNakp4eVH)*s^T)^N6+Io_H@hM8C& zPB%I7&RpEMAXmI!A%ZyMF&#ovG+5>Z2{@x|XMEfMs6}oKAnJnY!0eXR%DtDJ9V={} zy~_avt%^Ac{yxwTc#gH9NVb#xhp-NFbHgT|!QY9h2)jQWKonw}B{#U-Ku6q1U5@9J zIZB zyOiG%1?7>~#y2EEVr%&s-d^TZ4%P{`+zEJ&iJU(l6(*0x{qB zp5IF3mb9rp$iDFoxo0L@<)7C#4*pAGf6#_WnsdL+(glgY@5(+ zyg}aU%L^RpCNf%qHTNtdyzY6{%Uq;Ud*HjTiIBP=u$@0LkzoLFmcc@OKMXSiq?}Lz z(H#d=#Ggha-Z;db3OsX`WJQ`}qz;Ph^?>i;Ad_2gnzEtJ9on#vhwq^xlY8)GA^bDE zv`FQsRp@#Lb^}hgu%5MW`9eXGfD!(Tg%LD>V(Z)H`1HWz>$$ zO!gV1yQfz%^dV|{Wj5s>#Dl)kkWc1*_wgpz9sUy|Hd`DtSh=JBkc^C%#?=F()#$h; zUI^ANcd`Q0vH~l+0@Yg)PUFvbDJ^gTC-?zj5Yu(myi@;wu|V;fZ}YmZ-xb*WN5RxT z15YG0%$y+1!ndx}TL-`E_!HvQ-lOCEAt}2e?;PUyy???Vb4U?PC>3KS1O-Dz$xsNP zH1zq))_XOwO*~gZ>iDhTW$Y<(uqW~<`x8ajV$R?LF&FLrQvTFo`~7|6G;vvC%NWkh zyjGZ6#-EW`r2PAjcX08Q`M;<;^-o%OuI63(@0WUz`AHp@6ZyZtgH}Nic4K0q^tlCA zp#XJ);z(Rd7H56d{ka?kLU=S#v903)0S$P(>zG^!l6UV7i9DQDJ0z-2&YNV_yO0HH z@QVOOoPc?AW5KIT{Qq!3$(q0M%HMe9e~Xv!XVAVBc)1f;c^D*FoxNN9Cv1O<*Z9Wk z^Tx}4W5oU$xFey_<^<6eipf@Q{oCo!h$(xY(tc;sd-oiHGIzN(iP=5@~VpbocO(0 zYOaty)%FDV!&}n&ifr^f7O+mn=o3XM=KW-~I~DNMQJ6V`bAo(E8Gop_^kw_=-oko- z?DIr@Ok;4O+bv0v@=!eLgXS0A?8qm)7N$bg(E)49uiE5WkfzueRq)j)^8&I;Aassh zz@;bgL%%M<2m^?_VFaj#|w&wOLF99o%(-hxf%A6Le@J?wl zhS#c8`v+WAe0=Sfk|ZmWsyNe}K4Rth`Lr9Ar}G_`{+oJEuUz$c;t$0%U#V#xrZKtP zxMBo}FkeY=sC!ve)%~cj4|YVp)Q}4f6%G}c)i-1Ng0271L=VQSS)k(dT79`(ja&=3 z1Qo_*Uy?Z1XXmpLZ)BwzhT!YU4@X}Cw(sJ(>xllz8+j3i>^wZMC%I##h|>XSd@Bp;>DBMkXODy?sI|K{_YS&RC{cKg&hR|?ILeqL)#PsA zoK76RckWwx)RAzazQD5^PJEilyJKnycQOP6MFWOHLgSeJ6V(rOkP&Z_!%4R&F-&YJaSK?Tjx$7lZf8bl9LMriPG( z->fYN$%snlg;($L&<=$U(r{Q?F5Yt|a;8%Z^^JD;&E7EzhezbxKFBIs{s8=nhck*r z4+!lLD+#bz<8eN&9v71yT&(xY$Z0c9g1;O+r%zC)e(tLugoF zURV6pDtU9`H*jefGK~=q^gT#YA5aV2=)PoD(2&}U*|{JPSpqowmF`Q3dRuSME{u@L zAt#LkcrdEYP;wnO7{?94E_fjQEKKi0?Zh@ybvtx{ziO^-8r|f5;oBB5Ws5EpGR?+s zZIuwP5QPk7%q@1qDA4(9t4>}f3ouANcoYgWd#m2@;2+AT+1!7)YE#() zr<#-*I=|7Qj)H?*|L}A$I-QW!#kD9ULWXCLW!_Btr2`l+F?2?NF{;V~`P9&`UTkEM zhuJ1Y;Y3Jv09K5G)p;5ou~j@to@zC&+KNY|SFrgteNlA7a}W!AI*Kj6H11E0D|qqs zaheRc1b%&+jUAkhZA<*Z1)2Ll(>6!bCl>~eJvcFfCSWa+9Z zqp%Eyxiu>9z(+R07r9^7K(iMSjxqbbww|*T|S;CJIcwMiT=$ zR7L8)jw?wQ;QX>J^QK`dbS!P5T9H!^zT~3-{QUjP1mle77i?`<9P&>DoN$B^nj%=c z3Od9WIrlI9C?^TF3|t13m#-N$ecX$1d3(fFWHKJK?NtuSl?f zZP~~qsNXCF;7Q#mq>hu)htN-uo`4TS|Ayp1I=o;J*TM;4!i9l^y48nX140Kw$Rjl} zNG>XvgNw+`^=a~E__*Yr6;2*$F`#1oEfNwcU0rX0+%cJ(@Fx3nTq|T+83bE991nGH zgFgw>^)vY6jtI0}Y^Ue>Xnx2EjA<`?l+$?7e2y--Ug?fqOKHkSW1eD+8n<91iCTru zQ=yp*{D--auo13b(}D|ygd7KR zj+A18-QskBez|n3_QgK}elY%qS@0)d|uf4ht}eAktGh-|hqDJ#QDW zhHq6U$cDVG|^9D2`Pa)v((`1h{6l@uja$ zEPq8;5udFGQ+JSCjKe5e_|gQL=`>iOA>A}NMn7x4OJ?M?U`W2T-uJKJ#~#h6Il^B= zf0k?FGQ8OH>T&z}c)dBk4qDWyhm&u2_Dnsg@PvVC!MP$I^|L8aRg#i8@S6UtuD34O6DVTn<9g7muK-~w-+8&cYMmt|1v?YZV3kCwHc69;t ziF+18!1@6b#`%2=R`fdA)N|&8dZb7FOEPN3)2k3~3?LO1Gyc)$A>gyuQ(9ywPsZ7g zQoOCjO~nr?%Kq?Ea;$%Nlv|I$;>z^ zP}3YyF@HErA^zpC-jU%@5;;U^DwWeRG9Rkx5yA{;cnPh`V49#yd&~50{!*dG`{yn*ZD8!xrsBw|TF;D?%# z;UThyZjlQ+lK7#ESB%l6`S|O%rE=EkU?w>T}C)hY>%HXI-i6HYmRh77u zVD09{jjXsijmN5r?Z&i)dpm@gM#5DvS&SD?&)H9@;2^`+%9p`e{k7yF+M^ggxoJqy z;nZm!FB8T)K1UK;k45;fmM&js>$wz-3?_ z^raeAx3c~&`st_&Cr$^#Y_j#k&i1zID=)zDPodUp>oa7}oGw3896q14O%G?{1s|Kc zm{s(UJ5*ciNo2@A*e#ujM;b>-xOM3 zk0H}l3KkX;dtKE@ivDFNuNDI%?cznD>$+=LYR}$9Lx?fKzJoI+pQMLd+EH0Ni0HBzS$3A_e)KJTHuxdnZsq!^|A2!ewYGTM6a6&6%h3%I46aDo% z+hO!aMrVvk>(sC@26QiR1KkWhUTK`NK;x4`KgW6vMWeqj2+}t^HD|2eI~f# zGZ+%r5dvwyYx1sZq-3Hg9jsT?*KMQoVx&0PtU~HizGOCDyxtJ%lE_)Qq$@zo4-REmFFm!f1-?5 zh|>fSl>D^7;uy;nwMFNe=m0`#c&P-;>XdeU_|DG*FFC0>sL?K0c3sFK%zpb`dsLt? zHYSq#O2L*RVv)699~lIE;;S%}1hl&EYQc(h1LbAHlfem$2x;Ald016{xU#DDc5$J? zTMs(sI256}MAXQZ8p07o&5H3TlQV96I{@E0!0Ty|YaRj;%@&J2Fg$;dSy*Jnc{?xT zrO?tb2;yuz^<4M0q$o7(H;p&1;yzCNm4g8LP*)lkO(-lM^oWOsa76>m%ZvJGay0bb z=l=o#0RR8Qct;MzYN&>FY2%|wA8}bAvZ^cVsbzSkx#})5;Ahc$#(!gu3J>EWC5}7ek}}51(t$V6 zxFKYoELLPWsWQ7n>V&X8WDn>f$V97pr&Dtv7GeA#FuP%+Cz)+CoZ(z(BmXS3TV(Z< zo#0goV}3nt<&(u;;`D%O6wRBOe$+=A$|F+p@?#?#65#C0l{>Zl;7CO;VaY-}2|YUd zA|WmmXd8ihk9^4t1w$BtCoOp9}Sa4tNDG}I{t>CEAO|jcI+S7o-Uh0f%W{fN7J$p@ zkE|BLx{(U7o4>NHb|OCi;UE6)zy0a|UjO-x|M@q6`qO`W@r6JA`+xlUPygYc_`iGO zzyHlY;-9bo@=tudhhN(M{oj|%Z*aDy8LR*LcZK};Lbn|PYS!gjIJJW^-_wgME*=aMjY4=?4AR}@TZzy4jC>Vy>C>ifj32SjHxqIbyP1 z_;K?`>LBOxp~JT!4gr`r?2wgK>=pwAd(8dZ&kz z#y{m0VO?6{~i?hFEh2OsUt8 z+Ih)bSDt#rt-iPHJlD{f#*tc*K_jIBi_$_-^dk^b zZ@}V=bB}RehP{xZ&uL^%6MlLu$dp#8h$Q~%lwP$daT;iColtzj&{u{fY*QF1C$tX? z$Rp4JZJ-(=qnC0aqcM}qA^RhMMh_dJXrMLWRnKPoPacXIBUkPZI zdg_dAB|sN;faODb3?}`7-g7oEY&JT8Kcn)+X^m_;e_(gu*rztti}oVgF^#ciiDim) z*fIt=b5MyNRz2Runo(<1#0YOqqQ`vB_D_uD4V07DK`lx-p$H4;;JxaAf{j3c%yV$k z*}Un(^0U{vDO+pFLs(V=sibg-eMH)U?$$?^g4vWZA=u&fgaAH4g6JE0l0^8S{vOud z4nNgK@QY{A{)FQyn0Olk%kmqsqP)rqA(C{)5-jw7a=09mdg?2luc@`qWcW)X6*{^m zi0KyQw1bIkP^G8KThdD1K+c6rgE{It#dfsUO?Wa4o-h%!=<>*7t&a{kl2jl(bP-6x z(_=~5YcP38N(xgP_vJ>IsK{sqq<;W-`FzE7ZyQpcg&v{ch^8k{i=nX=@6QsDbb@Yu zluGIas)0M<(Z)2VUltHx?Y9I0Y*hdX_!Z#?Nh@Wz$j6-ZnaYrHhA?=2Gk@{DN#@a5 z2Uix0LO)dziJ=+ej{!`eCaRw7)lY6MgIG%Z@N^`R50L(XvZDggK$Jc$=mma7z~%j_ z2@LnnWfS1WZy?TCxq;4s&M9gZ!xl5-{m@=I>N-vxp?oaI$_>Hjj zbE55f6jTgB$0E=lH~lCrJf~<;R_7!Bhz&moJ=%rf6RRajG6uKa>K#UTS%{ssUp+Mz zLwfn`h{brP>FG-EedeLN&?;i09lbUQqs^cIwb|^97oqvi*k%_k{K2I*bwsJW_HT`Y z`2A_IYGlGBp9>Z?{;aFW{e2s~dFPY(<@DYob_Z1ucgIbcYGv}x*Ft1jekVNOo=8lQ zdR7va}$;t#w5NYevpNo$;uloIFYJx_abwt_mo8n|P*PVHV1+JV!+2PT|dUIxym4@v@33>{+iwIMYy}a)`hUHTDnB0asog9J`v5^}#)&P66LO9@npzvEtCcT-0J{*0Ff!zd*&sC?i)R@?a zW$lSG`O=>t%o7(^xI_V$WOfc6VJ_$W)$C($C2cOsKm}5TFW&Aw+^l42b26!YpQPL6FF}Him7sw~^h+3XD+c0-1J$J>Kw8Su zYJJnsNAX@cbrO49iK&m6q#b_o1t138)F;3o5wLt^)YLF8J@m9LxFL|Hrn0;1wB<>C zI4*IB{X1CjEFr)WDEG~>bA2D}F^XklyF9WraoRd2$`^*}5xrI)SlX!gIT9O8!e>+< z|7@hDQ7Y+40e=_Dk!c*P=+KrBd%J|I?j_240~94=JeIU1kt;|*v1}Vupl(|O40Ece5C}8T z1f2l9`AE>+wK;;)fT=!*uLjSG=IFGMDSUyX{cg7>8q;Q^#?nAp>KH7aE05!%dl$@~ z(tWp-WE{i~uux-FNSdmS#Ssj0N2ls=sYRTknf9~y+~*wm0^JOhF?487wgD}+mcw8o z?jomno*!sHR_J9+tGN7b)=6=sqm!t^n*u(h;}|bq;VCjMaZZP-bb*RilPM7pN|SlD z^;RCZX4d+?*m||oT4-8eSrbn11(h^;6c+EmN&Us|YE9OQ$}cYUjkcZZGy6hhaN<2` zg4fa$IGkF?K>Ze)D&BH9a++=!5VUrUZ79M{E+63yRA|AHPMdWKN15zcg;d>n%=;i) z1B4k{LvHBJRYL5Y0);wVJZ+InYf#!e086|oT5B8dmX{g8oJBzGio_Ca(jaUx z)u2DZAlkTzdJt7fL1Sj^8Dz8AGr$Gm-!Z)i2S_I30*BjN#=POqbn8;tvnQG@iWo2I zg^o;^(NOhK-1O}>*{y~To6s8D{$%#n_UO=CU$7oUwxAyv6i8jD`a>3+n5x@qlHmlIz|Z7GIM=Zm;8%_~{Jjg0uR1$fF0H#WJR3 zfp>zX6NhTQQIF2wTL(h^6@p&w;SV??POtKv~QB_DL45iIYk1e5Z7USOmwcxr>(O*zP878U@l`5+I1;Y-z|72xC}Q)rwU z5Ca(RnT&^iZNKsgeH(Z?TD%GYST_pnhIwk<+YR2%0(je z=feZf*I*BDMd9>QsZJ!Zun>yNTwM16$@`;6*hjw_eOvjsTXnQ-!<6O z7fMViUldnH=Ql+!lwu-Ztu-XG>gs|e;AT9C^=cn_2;z0S>{fXt=^hgP;y~yg?v2@R zO)J(KXg{y>#6S=*@TYY`ysclm4gUt0B(2+2=R_kw>RX|hKDk16)kWe#0afQ8`kP(2 zGmr|L5I?iIgtREZU$N}(8)31U!SsE^U`KaYNzuXfi^qbt&=7!ga*PilngDUj$RBQl zhD`Ey*)X;7&%?S}IwKBgw};^L1QMh$f`T((}{~y)l5$u#_IdH7=yo-G2>ok?RW-8p5F(B#Nnt+t+a|5 zII)wy;EHUI4=NVcUQkel9=8U!`w(6n+4p3npAQhV*$PIVJp+X=hLds)Sj8QcU3q9> zg}$G1K)W~oh}9qXKuPrV=ikEae}U&;IE=HqSbX=eu;2J2e*c+g>AAcXRYl(TBSL@R zsLL7n78!(1oq)`~6R&-P{X?|bMO&Z>Ga>_sooK+f`2oToXZV2mG0KH2zLaSx*}E?- z3iPG_|M!N{oV}*`?M$E%1NOo56b-Y7DtMPJSd1hikx*CnLiYxgwyC`Qtf$upP7og} z$=`UnN=g|t6yi%S17bm_REzW=@82reJX%GR>W5V_) z=MP+uY?8<8H~N2p^Zz51t~iVE!Qsbu6$2YB3w9t$#e zoKgi}Uk_iqzkp{(!C1roKY#YW|I@$i&;Hkc`fGpoXa2AKuN*-9fpV6YFH#E4_NFMc zp9nqw%b1?W<&`5k-_N8$v3c1ey|3*xr=cFj5{MzXwT0fVd+lreE`BEPQ|>cc?6nPi zf{uZdT4qGHg}l$e`RvuE4@6Q(JRC6|>&_Na(KVvu@!%s`4p&DSnReC{+5#(jrVobd zUlkZ!y0KL{D@5u%DlioU^G9tvK$IKuWTGEtl=;iHL#OYpz4Em6VsZ*3yz&uPb?CXc z?Fpg1qa{f76{rrpPzj6`!XpC`iIt5x*x>ksqrRun&%0r30_QhOTm`mp(8v4*t;Rky z`PB~@!snJ0&nhmGA7$-5#`mylK&Z*o=xD4vQb+Ops6cdU1`$F_CzU8pnImO0%HZ5l z0+HYREQ5|*kk6L`SE$g+y8Y(5eCWlr#^c$b%c&z!da*#2m+55MXJ%(7TD>;wTs#+}h-3N5n0$E_wybB=u&OXdb_(DbnQqA#-*q;Vy) zSUsWL6h{EM0-k8fGRkx}$~cJY8ln@aM%dG<(Nr)}3|U@tm~ARpOG@Uq9C+mQ*|FkuEg&J}1kr$rvlwo?@vf33+M>^BLPk6%a+zxUs&q)YGNHB=66B zDya57gAYY|HqwlD{7pe?5W_a$ZD7X0SBBmDl0d_KnZThn=Y+{<=3!gE>OM8asf&wG zF+t!cfEv?dM;A^PSfq*4BPKib0h@$085tZmgSQXO;^Nm8l}&bM`7=2{cF zNb;uz5|9nwRp%9!6sg2u;Y+b1rATuZj`b4)ZGvOMJ!7gM0+v;;M3JJ$=Zi(5sNu=y z1izH6AN4C{kRL~R{SApoeW>`46yz2~P}e>N6I7Qy@FkEu;sEwtNr1J`segnxNrFv< z3<~9O0yk=@mJ)@7wx+Ir9`2{lzC!++JOp(06nck9+2_aRp-|*E5w!&n5 z*wSj&sqPZu%AA8Y!h3vGV#gD{*`fRT{b+DZRtfcj%EmR|FWU!0V0 z-4RO`f8IqG;GU((;?iawn{s8zRK15ja^hi+N*E-TzeD`2sKg3g;R)98$^loupKOp* z9`?E4(kwQB0Y=MnCyV5QNEM8OlVcwQ{k8CX?;xM^K3++veY zNU=Btp_HsHjW)*ojlIRy0$~KFydB4BIMELXdo|OS*oq;F1!F&<5+W_S)i5eaKRtw9 z?TzBrmkj7v`oe6#pocd^K1hYVjfBA-@VLeF_td(%lg-7CB-l^{(NoT}>B){->qkNz zO{nEuWTUOV|NnfL$quu?nx0RjBs#t5DA&-dL`W&-p`~7c z0b|T$GqVa)nR%YCB+FcC_)m8hXVHyR;wzxaxQ6$8UO*W43@)qBOqJmQ1cvuj?ow-l zF}Uir@i84S?n<&AB+X}SeW%Vb<)|SLDUpUhnt%`CoePsgIoUYd8uqh5?8IJ%|lB)bD?{nF}J5ty1ix(Qpm2I z&f;F(91tfVUTAlIO#KnffSQ^)p9?wY=&TP}=*|0rTRb6ZQ;MtUrUlL0dg@D?>r`KS zR{?*!!3zL%D0Pmxj*Qha_$_cwh|Mnu4p5cTxEp2;9#BBI*CF^EAI`@(ayd=ohe&nHP+Z2F#Mis zA|ykdwdoN-bTh;Xy8__nvEFqY4XpIKUP})wo?+h+W5DTyxHV(WAS;pzeU0ke(<`j_ z<=6}MS*gA4f z)=6dGVo>q@ziq zbjglUU-y;Tnp5TB1dyCPg7gW~0tY(m1or%iFqgKUYIW$8s(?;NFj#6%I?yvx=ry|o z`1WsY9`KBq>h$s#f_m^rcdM&^$!87OK`D!#`pKWI?(;W)hQcXu&#vYX zLby#7$G;Ej;EmzhjTeDz)z9t>ix)MI{fX{wwY>L$RzdS1&Mo0j`|ig#BVr1;NFjHO z4X{yE5iJ}hb@1(J*#~^(bSIDpmUC0{O9=Z|5l&Rwoj^m z{%1J#t-VZT_xm6Jl&e?vtg-EhdMFTNTRkNSdklu{`o(^$$or*=@3U7Z(tNFwg!VG8 zUV4@Px2-$NqDR(3vps!Ek>J9uEHm){V#34u<5l3F2)3InT z1WBsRfm@D;#CNY$snfb{%i)E)?WTd!%r%fM%thy7)S~)(5u5Bw=xL8XPiC&jXEvtE zyv0aRQYb!P7{1c#F z8}>L6#f^jvbw%dniO*{i9~D#SA=Qc%0UnB2OU^08;vwMdfZ$xn?OxZ8qvA}FR-&hb ze77}{1f=Nze3pz}w7s{|WWuMxt7THGwLNJ|vJ>OgsgkexKmXQ*kU~N5`w%V;ox;ZF18#F^a z<&2!JKC55zFlt*iecBrFN?i?A|Hr77q%$Ie!{f!Yoiz?_>E6jL1Z#f3--05H1 z4~Bx7>rod-Xxh+2KbIbyX9;&Bxxl2CosEqQksl{QSrik_kF68aA5@FWA1*^IlNfJM zx08@VAcRNIl7%VAe8?TC_rMI6>XanM@#;B!CHeB}r&5#kPn*$!oY5a^b-y+r2bnP1 z-6G%C#B%1|Rp_b|6rH{lD&H@al5$+fNjQ?LLJ3yua9XOVVbj|$rEhN zr1L`+g@|!W-!39azCBa%T#tb-HIC0DnERI_uWLY6TalE$!LHsD+qOyOV2BupnMl7` z$$kj|OSTkSfmN+CKL!HH9Ce;iP;N0xD9rphkdPFyj%PF~#CPk}1tmKXMIjTuODSHY!zY&l@h54H2z$S?HjmBa~gP{_2D@&zm0$Y7WlkWrsw^x zOtN|ZT{};qbg?F1yt@L-VF(*reCsQmm_?O}DI*)}*v}}0x>8;FLsr~QowD>*j^Y$z zGc8>~sbk5NkSb8HtBN1d9xw%T%1D{)L z&KT|ddncjakGm{WBf?qh$j+Q+s;PWp2&)HHRbNEAUWes51D0$Ox8c+jsyDiO0p}8+Jmacq>Yp z3FdD{cxp7zjm(Ho#|pQhLw;yp2d(y1s5MMHs5#GoL6C0>%d4`T#~aKJZ*`ixpuf)B zzvFugLKj}{#9BF>3px;Q7OLG#!HPAqeD zZq9pJV%ccC8+8Dh>c=}hL@y=2AR%7lEd<4leX4Ij8y?RrVhZ;6##YWZuGn3=hE31& zrd(0(gH@U)bU9^qHI|ja6V<9Jnqn;eQ-v5H-q~6sl<4<_myG*_w`K4puz*joW8NNl zarl?MRAWm%Pc4G;@|(a=<8h4W+gj42zs4|0W*=+BD|fy>Ch`|02-tglAXIC-5h@DQ zv&*9zOTATNotv}s5YgC)eMKI({aBy}kB-KXc{iAIAd!YYf}$3mq_DTstlxB0KZTs5 zE^J8qC$jtFBZ6VSWHl$4to4y04r>$7YDp;Zs$fo&oKJz&b}I;q{vE$h~5KC z)uwh=LO|;D(hu<_HcRKRZQu43K^j!cLXgQ^n<)*lL6X6+_0<)EY^AF=h10`F1$M8B z;QU??#d+#{J^(82AOB4SarBD}3f|A~`@|NA$s!RCiZ8DJ*vmN)H!rr+CP8r)lcKOz zNkt)8)E7R_-DRIgH;H$u4{QHA2=T5%Rek-yEMY%HIIT0rJY@?VDrJjF58(@HM+j?l>2Wb6DatisQX74AwXB_<6C3Y)u{aMJ7FS6L9 zzB%jTzP{3xhD=sKe`T4~>^p4;9-| z&#nb|aBhf4UhCmYuFw`L3to`%?6y<^=g&Q8f)|Sf_3f+#Bby$fU&?)EVZM^JfFBkz z5j*Bab$zX{qL^{6e0LR|R}{=YU<__U16YE5z?d z3k^u_aN*#w4GA4eu31@)?4B{!t&3Ckk*i}pk?*==Y_mYp7gT+RJGpgwXuDox$qv$T zUQm}g78UI3;NW^*zlmr@S9EcNuluTQ6G$Y_>cOHFlX&e%Nc*ez9&)P1xo9Xx88p|# zCSoHTW8P*s${Gxx4)xkl@55bQ<8P+4#WG(&{Nl^AI;9kB#r|3zIx?yE>ZcN&jVii@ zfqcA1Px_Jh4X`%Nr@O~_Znkrogrw`ox$+qFJ=lZUL^@1~&AvW$i!4zl ztDK(t*`9V+DzALA^SKu&A64Dz8JFv6$62G&KAjkR>frM@-md&%&PdSc+%gRAIs2*U z(uhA_+bJ>js(ew-KpRz08`n>_2lF~^YAXZz1#YJo!JSjrq4Ex?K#0~C#f1bygx{Md zvswLnP>g#}3y`t~>l&a4_~mik+LRS->xbrG2)lLpAqHm?UtHed>M>i`(vt5I`nq1N zcNv$FXW9@D_!@mAa=5X{}%7$o9C$R!HQa7renSyBS)q9 zAf}ati?YDrxg8)E?QVNcpL>A(o;SQ!HPu(h*soL&>Un~lJM^QY$&c$aRKwbCe<8VE z#PIDcu3UMq%GUACI}y*#u3P3!j$F;4j;G)y^W8LyCgAr(Nl?OpiN?y5r&|+xEPG_7 z^kuP;@UlpbRjv4~w_t$pqTe@LqSEXRU&71HCh$TPocQWl97pmdj#uTdn-CaXUoffU zUjZ>cEo!=vZyUSXPF2ujC`xEMhEDx2UIHs!c4MwH*82rn)T7)iu^Rb0D~rvGMC)4# zeu%{!n8!TMhpTw}k;eqvYPL z6U3Nc+*F$Y@HC&Huo)kG9}ev`uh&S8xz*h07gtBi>C{mV8unm(ZcF`z_K~kwnJ(9L zLW#!K!xJx`Dv{vHOS!nK7TF_9?&yk+ODvT+>#7(7y$D8_xT3Gm6T);7JK|cU@8}Y> zreJ{wKht=RW2}Ujd1|~iDIg+g53)AGRnaxHlIt=n)TlAH^h`d+VO2( z(>tH@z@B$;A{5y&!6VMcrBJMkWA8!f%=SAYH_D=@DCX;r$9LV3nMx&*d*Pdwopa1J z6fiZs;wE<~U#^_gSW}WRCD;t&J59&%E7gRA;TM%xCqIEOF(~_!k$fv6irN%c06*OB zb0zGFMW(+fQnr>ti>o)+QpZ_D{uxhaC2j$GF_oFgdnFu)vMWUR-E+7yn~Lxv+H{Ys z>a#(-&CJbK1*yc{<7TTvaa^;jjp1=+feL(^@8Bu&kU9?HQNK%~nXlnYY`{i;V=F;N zj$Teh3X{5y%3l%-FyvUotVl+O=_pzAn#D?6y|A2c|H{EHUpxh|EUrB_>oE4-n@Vt~ zba2ykj&0TQVt!|nTq2&jVVf)1M)DfeuSd@PKnTJ-R4|(c#a8c;L6Qb!dCv5%31tbp z<)@9wJc9+L$nlNUCHbDKi$?D3dybIZ>tQ|b)&PtQz3in2D&qr;goLitOhNW3!3&v0RyCqoj>!|J(3-@Q zx^=)fs@!N^NVsp^>%?7y{NP=wD+^o|GD5%hj2`}g+Y=&TZj}h}@E%A&L*E(=QJ1bOLbu&3Q{Nm}xC~es8O#k85 zK%Ug=hRG}_oVRy;Xn78Vd7{)yN)FoU*{9GqNkmSuo1dlqS_Au3jWZoA%FIwgqN}Rw zR!A)a>~n;zOYL4?x28oOlG6Ga&1En>;X^eew+m#b7wbi0s=THs`+b3_?(FvX@mz^c zw~Brf8OPI&ewsSNW`v4780QDFDeoA3=MnpD8&?7elHE+sT@sVY2&5%uaHAp65QC1Y?y4;)i><&dt_fI7pdQgA(2D`Rc%9Si0E3ZZaxW%4bUiffCvi zSJ8m_L)gNy6{h}ccSuGo`@Xn0hL8adF#lX$1UI30%3U0y3~H#b&4QV}atW+We9kJ; zQR>07WsmQf!qX3)0z=zu*VJa9$~EjN@B&K8H(+S)HOvSjGEzezB^%23<}NX*THo1f zDG2@~n(t>@9qd(SxZimO^PZW|s1>VC9hrz~cf(+OqW)EDbhYXq?gids$<&G(L@YPh zhEA^2=@d$$4N>)DJq`7aV}X(`hB+OCq=)D87+Eys93LjUg|((?7rB`D+Y|DJDEh^w z!zP0^@w{XY{ADeKV8^5Ms~x!SFH^kE-&A#?ZuU_E40mSpa)ciH3nZesLNfC2SW~5D`YJX{4g83=?QPRsp<}xavmDtYM z;GKERcB5wdHrQay!)U8$ErQCokJ*^T9gi-MeF0Ao+e~|N?qS|gxu)}~+yYU}(~SBK zWp#KO)z5u5CxeO5C534K0lj9^vekgjlOqpSnYJlxdKyD;-P`wr_zlby1#jy(AnrJmH5&ta*@l2TYeZpS8v&Ik@IWt8cFq-3yFt<*XT)%w`0xhch$^7g_)yXSj z3)jK%D4O=;-P)Nan@R7K{Y=HGa?eMuKoD7m!~YoDdy)X-dZL0Vjf))Ce9aTS^Y1Y{ zQ5VlW>4=bx20<*FS^Ygb?palfcOtL*Z74Qr7$zWVs6Ub?8Z5~BjVSa`b{3894uOo% zY+jD6c+-Tr2B>I^iD ziU@_t@!hj9{Q&j%D*JGcv|#NvG4Ytzjq-RVzQx?Wr_E0t4Yia4Hpn>Cn!9{H{DsWw zoJ)xSX)@zqGiU3<9C5J_oAEj;Mw)6Nsk%XJb27koY@zdWh;wDWr}Xp z8{39aHaD)u#f^`6Rw3L1yiiarCQZPOB)!p4-%1S}G#JWyA4ove%4sLy1~RH)r1g|~ z#n}T50|qLjmJ+T?prFvhJBaN;;xPl#=gf6hwuLYk0-8|IoCNH@ubU zWwRQq`!I>i)rzG$7==Sbd9ow27-gPogDSN(n(u4L+lF3^4KMX(;S>vU=@ZGAjqSE3 zE;qo%BPcmsw@AU*54IJ_3w?2rTRPcLwH{Y%SMdIQ>v4ymSf-3t{fXcUf7kP(Ci^?y z5A0v~Q;VsRa%uD(_80!tPrJa4Li`E+3;)nbX%Z75kl#Um;h#D}Ux<<^CbnA~610-( zAr@!CK}sFQvbA_XQ;q?5H6DocdH?^nHIsl$7Z*Mbpdq8&P;8(;k3#x~19LbtP#U`h zrDq1guqwqo!X*|Qi?o5XXsFEQIG{al!fHkb(u;Dq@pmM!5i_MEmUOpc9tu3uo9`&U z@TZnXjTM;vJ6>P-Q=c|*R{T%6U-*Y^!p9n|#_=8d7yi^oDy8cj{>0-8|IpN!<4VfU zDI>%zdnN}$KNK{m;W%HdBneIGomU$!Gw*fB`**y)@TWdG;w-o#&R=pm8&~#y9#T~? zemPU*<&u(Q?PHW>AfQBFH-IBIicV*I$v~5)rk(xuuYdaUx9{zr-}u*m`u6Sb-u(A( z{Ncy&w?F>u|Mtdz{^Jk+X#euFKkjuK6@U2)9NPLe4X-Bqw?Czq!}A$zkz9JnSWji%mssTa&g1dMS*0tCxg%^tyK1BOKN+#_0tnbwZk(E-&`j(@?Ys z^I<>b>_Cp3!q;kTOc8aP)yap7Kd{Gu*#jBlO3;%8Ao+N@MP2~e7F6<-bg?$oX225d z2_p!$;h3qmQb{c5ne=Y4g;$i)XS;c9l(J^^XqjiEQq$x<3>h*%m~5l5=S8UvA1Je? z2;&*wr{%t3=WAhQfJ^0V6SAC;uN_lw@2dxW<+oul!l|u0Y(9>w%k&80hYBfyv#~?m zNe?RJFA?946_R7#@|g20tHlCV#4bB4<+2EAE*3fpu1mbzm)MDCC*D@XuK~|qqaLx2 ze9px#yu|gBzFibNBVtca@lo-j$P*A;>;s`8`0;t29&1rlH+Jza>ek*UZymfEU+$$j znZ%w@~|xkxy9cEWVhA5EDfj%6rPchN{i7Q>26} zkyz(4aWjz;G_BA$q>s$&>Vrgot*7QGR3%+SDiQ1gsRit-Ia~opQS$`{>`IX7R?tpq zuqRx)k(8$t2c10NnH1nK%03qrb_vL(G@EAyhdaL{cjqey*qhvt5Tg6Awh6 z<~}p+1`Cm@9Gl@GTE36CssRraZ3^8pyh?{Pg8WTr`U(g82IXn_m_&22C*&~Uhf`NuQ$5#ijw={@%VUt zux33s5xS_B6QD~>W2?Z5v=mg&bk`Cq&GwBI=bLQ4PEJO&fGgOor|}XY2EAhTbQ2t$ z?x)@@uMhkdsn__C6BAZ33w*)Ho+NQ`?1fyrl0DE`JX6g|&{8su1UjtI5NNRKiiru8 z%psjBDo3kLTQ?#?f_%hvd=-dD975dd(PR^zD184q$P5mg1fE{Cg3#0x-(LZDdS&FU znfc8HDq`Js%t*R4KQB3`Q*yak(L-a+F*1C3VZ{_DZz=*3nKE$Rnx6NN3)ni%a}?LH z(w(_bxui4J#iC;)q#d!ry#L|SvBr$PnAJ3fFZx0XlE8h zL>bszil3;~Q)Z}~Jz-+Oq}10|@mpUjqV{&$Eq%<;)I0X~7GCt+q5YJYOYs#oXEO0A z1KVI7R}ZtyS9BOfjUx4@#t)g%Asn$*2mQT%K7lmD^X4TaF-%C%KrihG^`y+z6`v9SoEuG- zaXC!!#MK(hRz4bq=~iYZER6OhN zs=e0pECR8Ff!KF9bais#j8gOz7pN;+M;@&dHu_WoCIc$zG-V|OWw8h9lJY7n8%DM|9jYRdan2#eQcQKqmt)A>To zH|D;&9FCOOjN<&{J&qBKi0wwKvOcx`^|4c$Ig)Eq6$-3%SQKpQrX zgn8%YbEZ9pv-S-!GJtbYk4eFet5I`)cF$~2SnQemlAH1XTp7O7$2$$UM^nnFm}1Z( zZwpy2c@_s1F{ekCL(sFlc?V{ER|)H-Q`O~`MiPsn7g{Ncl1(WfKV_!MY4ErQ$V@VV za(Wj!t$PC6=?tiw4G#8+$mtna;3ie9YcGU#XC8-zb8y=N9)`TZ-AI~sEVd!9A$vmk zk1RbM*J+aChOQ0rie5Rugn9E^z?4s9e+{h^2}LM#f5P(G-)3W`M*9p%Xd& zPB7buQ0pZ|IF5497VyyK`hj}aAXx2y^_V$~rwx-3?-uc7SE+>rPIH$V=og|19K@j+ z+yiUlvH|j1z#V?90mSfQ4TQcg@2qI3f}M*2&;_sp50X1rmfm7W=)L!Wc;HwrE~(o! z5;%DvCcraX4|E==yTwukI7}&>&=}HjTe9z%d`!RQe2$K+{+kh4dlh_b+ z%ke1=$`R6)0^VOMHfk9JH$U8@l%yDoJ#x^%h0SrC2M?#ixdm$Ez*|0LnaNQw)j-86 zppJLx6bB1RICL|t05=tuov)BgfXOJV9veuZyzfB;EV<$|v*;s|fR88E_4kij;iFFY zk8l57C;axMPWb)j*ys3%J@bqG?|-AN`1U{M`NRLY|G)p@{||q|KlsJ|_n-Z5zp2+x ef8~FP|7*OD_<#J_|KO`&-ahB~X#WKhIiZoYMG%?* diff --git a/EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt b/EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt deleted file mode 100644 index 5e225c8f2d..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt +++ /dev/null @@ -1,742 +0,0 @@ -SF = zeros(21,1); -SF(1) = dvz - dvz_b; -SF(2) = dvy - dvy_b; -SF(3) = dvx - dvx_b; -SF(4) = 2*q1*SF(3) + 2*q2*SF(2) + 2*q3*SF(1); -SF(5) = 2*q0*SF(2) - 2*q1*SF(1) + 2*q3*SF(3); -SF(6) = 2*q0*SF(3) + 2*q2*SF(1) - 2*q3*SF(2); -SF(7) = day/2 - day_b/2; -SF(8) = daz/2 - daz_b/2; -SF(9) = dax/2 - dax_b/2; -SF(10) = dax_b/2 - dax/2; -SF(11) = daz_b/2 - daz/2; -SF(12) = day_b/2 - day/2; -SF(13) = 2*q1*SF(2); -SF(14) = 2*q0*SF(1); -SF(15) = q1/2; -SF(16) = q2/2; -SF(17) = q3/2; -SF(18) = q3^2; -SF(19) = q2^2; -SF(20) = q1^2; -SF(21) = q0^2; - -SG = zeros(8,1); -SG(1) = q0/2; -SG(2) = q3^2; -SG(3) = q2^2; -SG(4) = q1^2; -SG(5) = q0^2; -SG(6) = 2*q2*q3; -SG(7) = 2*q1*q3; -SG(8) = 2*q1*q2; - - -SQ = zeros(11,1); -SQ(1) = dvzVar*(SG(6) - 2*q0*q1)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyVar*(SG(6) + 2*q0*q1)*(SG(2) - SG(3) + SG(4) - SG(5)) + dvxVar*(SG(7) - 2*q0*q2)*(SG(8) + 2*q0*q3); -SQ(2) = dvzVar*(SG(7) + 2*q0*q2)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxVar*(SG(7) - 2*q0*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvyVar*(SG(6) + 2*q0*q1)*(SG(8) - 2*q0*q3); -SQ(3) = dvzVar*(SG(6) - 2*q0*q1)*(SG(7) + 2*q0*q2) - dvyVar*(SG(8) - 2*q0*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxVar*(SG(8) + 2*q0*q3)*(SG(2) + SG(3) - SG(4) - SG(5)); -SQ(4) = (dayVar*q1*SG(1))/2 - (dazVar*q1*SG(1))/2 - (daxVar*q2*q3)/4; -SQ(5) = (dazVar*q2*SG(1))/2 - (daxVar*q2*SG(1))/2 - (dayVar*q1*q3)/4; -SQ(6) = (daxVar*q3*SG(1))/2 - (dayVar*q3*SG(1))/2 - (dazVar*q1*q2)/4; -SQ(7) = (daxVar*q1*q2)/4 - (dazVar*q3*SG(1))/2 - (dayVar*q1*q2)/4; -SQ(8) = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG(1))/2; -SQ(9) = (dayVar*q2*q3)/4 - (daxVar*q1*SG(1))/2 - (dazVar*q2*q3)/4; -SQ(10) = SG(1)^2; -SQ(11) = q1^2; - - -SPP = zeros(11,1); -SPP(1) = SF(13) + SF(14) - 2*q2*SF(3); -SPP(2) = SF(18) - SF(19) - SF(20) + SF(21); -SPP(3) = SF(18) - SF(19) + SF(20) - SF(21); -SPP(4) = SF(18) + SF(19) - SF(20) - SF(21); -SPP(5) = 2*q0*q2 - 2*q1*q3; -SPP(6) = 2*q0*q1 - 2*q2*q3; -SPP(7) = 2*q0*q3 - 2*q1*q2; -SPP(8) = 2*q0*q1 + 2*q2*q3; -SPP(9) = 2*q0*q3 + 2*q1*q2; -SPP(10) = 2*q0*q2 + 2*q1*q3; -SPP(11) = SF(17); - - -nextP = zeros(24,24); -nextP(1,1) = OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11) + (daxVar*SQ(11))/4 + SF(10)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(12)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(11)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SF(15)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) + SF(16)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) + SPP(11)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) + (dayVar*q2^2)/4 + (dazVar*q3^2)/4; -nextP(1,2) = OP_l_1_c_2_r_ + SQ(9) + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11) + SF(9)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(8)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(12)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) - SF(16)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) + SPP(11)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) - (q0*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)))/2; -nextP(2,2) = OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) + daxVar*SQ(10) - (OP_l_11_c_2_r_*q0)/2 + SF(9)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(8)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(12)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) - SF(16)*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2) + SPP(11)*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2) + (dayVar*q3^2)/4 + (dazVar*q2^2)/4 - (q0*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2))/2; -nextP(1,3) = OP_l_1_c_3_r_ + SQ(8) + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11) + SF(7)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(11)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(9)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SF(15)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) - SPP(11)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) - (q0*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)))/2; -nextP(2,3) = OP_l_2_c_3_r_ + SQ(6) + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2 + SF(7)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(11)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) + SF(9)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SF(15)*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2) - SPP(11)*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2) - (q0*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2))/2; -nextP(3,3) = OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) + dayVar*SQ(10) + (dazVar*SQ(11))/4 - (OP_l_12_c_3_r_*q0)/2 + SF(7)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(11)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) + SF(9)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SF(15)*(OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2) - SPP(11)*(OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2) + (daxVar*q3^2)/4 - (q0*(OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2))/2; -nextP(1,4) = OP_l_1_c_4_r_ + SQ(7) + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11) + SF(8)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(7)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(10)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(16)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) - SF(15)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) - (q0*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)))/2; -nextP(2,4) = OP_l_2_c_4_r_ + SQ(5) + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2 + SF(8)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(7)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) + SF(10)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(16)*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2) - SF(15)*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2) - (q0*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2))/2; -nextP(3,4) = OP_l_3_c_4_r_ + SQ(4) + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2 + SF(8)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(7)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) + SF(10)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(16)*(OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2) - SF(15)*(OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2) - (q0*(OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2))/2; -nextP(4,4) = OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) + (dayVar*SQ(11))/4 + dazVar*SQ(10) - (OP_l_13_c_4_r_*q0)/2 + SF(8)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(7)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) + SF(10)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(16)*(OP_l_4_c_11_r_ + OP_l_1_c_11_r_*SF(8) + OP_l_2_c_11_r_*SF(7) + OP_l_3_c_11_r_*SF(10) + OP_l_11_c_11_r_*SF(16) - OP_l_12_c_11_r_*SF(15) - (OP_l_13_c_11_r_*q0)/2) - SF(15)*(OP_l_4_c_12_r_ + OP_l_1_c_12_r_*SF(8) + OP_l_2_c_12_r_*SF(7) + OP_l_3_c_12_r_*SF(10) + OP_l_11_c_12_r_*SF(16) - OP_l_12_c_12_r_*SF(15) - (OP_l_13_c_12_r_*q0)/2) + (daxVar*q2^2)/4 - (q0*(OP_l_4_c_13_r_ + OP_l_1_c_13_r_*SF(8) + OP_l_2_c_13_r_*SF(7) + OP_l_3_c_13_r_*SF(10) + OP_l_11_c_13_r_*SF(16) - OP_l_12_c_13_r_*SF(15) - (OP_l_13_c_13_r_*q0)/2))/2; -nextP(1,5) = OP_l_1_c_5_r_ + OP_l_2_c_5_r_*SF(10) + OP_l_3_c_5_r_*SF(12) + OP_l_4_c_5_r_*SF(11) + OP_l_11_c_5_r_*SF(15) + OP_l_12_c_5_r_*SF(16) + OP_l_13_c_5_r_*SPP(11) + SF(6)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(4)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SF(5)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SPP(1)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SPP(4)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) + SPP(7)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) - SPP(10)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11)); -nextP(2,5) = OP_l_2_c_5_r_ + OP_l_1_c_5_r_*SF(9) + OP_l_3_c_5_r_*SF(8) + OP_l_4_c_5_r_*SF(12) - OP_l_13_c_5_r_*SF(16) + OP_l_12_c_5_r_*SPP(11) - (OP_l_11_c_5_r_*q0)/2 + SF(6)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(4)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SF(5)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SPP(1)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SPP(4)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) + SPP(7)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) - SPP(10)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2); -nextP(3,5) = OP_l_3_c_5_r_ + OP_l_1_c_5_r_*SF(7) + OP_l_2_c_5_r_*SF(11) + OP_l_4_c_5_r_*SF(9) + OP_l_13_c_5_r_*SF(15) - OP_l_11_c_5_r_*SPP(11) - (OP_l_12_c_5_r_*q0)/2 + SF(6)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(4)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SF(5)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SPP(1)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SPP(4)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) + SPP(7)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) - SPP(10)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2); -nextP(4,5) = OP_l_4_c_5_r_ + OP_l_1_c_5_r_*SF(8) + OP_l_2_c_5_r_*SF(7) + OP_l_3_c_5_r_*SF(10) + OP_l_11_c_5_r_*SF(16) - OP_l_12_c_5_r_*SF(15) - (OP_l_13_c_5_r_*q0)/2 + SF(6)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(4)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SF(5)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) + SPP(1)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SPP(4)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) + SPP(7)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) - SPP(10)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2); -nextP(5,5) = OP_l_5_c_5_r_ + OP_l_1_c_5_r_*SF(6) + OP_l_2_c_5_r_*SF(4) - OP_l_4_c_5_r_*SF(5) + OP_l_3_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(4) + OP_l_15_c_5_r_*SPP(7) - OP_l_16_c_5_r_*SPP(10) + dvyVar*(SG(8) - 2*q0*q3)^2 + dvzVar*(SG(7) + 2*q0*q2)^2 + SF(6)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SF(4)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SF(5)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) + SPP(1)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SPP(4)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) + SPP(7)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) - SPP(10)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10)) + dvxVar*(SG(2) + SG(3) - SG(4) - SG(5))^2; -nextP(1,6) = OP_l_1_c_6_r_ + OP_l_2_c_6_r_*SF(10) + OP_l_3_c_6_r_*SF(12) + OP_l_4_c_6_r_*SF(11) + OP_l_11_c_6_r_*SF(15) + OP_l_12_c_6_r_*SF(16) + OP_l_13_c_6_r_*SPP(11) + SF(5)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(4)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(6)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) - SPP(1)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SPP(9)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) + SPP(3)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) + SPP(6)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11)); -nextP(2,6) = OP_l_2_c_6_r_ + OP_l_1_c_6_r_*SF(9) + OP_l_3_c_6_r_*SF(8) + OP_l_4_c_6_r_*SF(12) - OP_l_13_c_6_r_*SF(16) + OP_l_12_c_6_r_*SPP(11) - (OP_l_11_c_6_r_*q0)/2 + SF(5)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(4)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(6)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) - SPP(1)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SPP(9)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) + SPP(3)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) + SPP(6)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2); -nextP(3,6) = OP_l_3_c_6_r_ + OP_l_1_c_6_r_*SF(7) + OP_l_2_c_6_r_*SF(11) + OP_l_4_c_6_r_*SF(9) + OP_l_13_c_6_r_*SF(15) - OP_l_11_c_6_r_*SPP(11) - (OP_l_12_c_6_r_*q0)/2 + SF(5)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(4)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(6)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) - SPP(1)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SPP(9)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) + SPP(3)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) + SPP(6)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2); -nextP(4,6) = OP_l_4_c_6_r_ + OP_l_1_c_6_r_*SF(8) + OP_l_2_c_6_r_*SF(7) + OP_l_3_c_6_r_*SF(10) + OP_l_11_c_6_r_*SF(16) - OP_l_12_c_6_r_*SF(15) - (OP_l_13_c_6_r_*q0)/2 + SF(5)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(4)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(6)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) - SPP(1)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SPP(9)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) + SPP(3)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) + SPP(6)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2); -nextP(5,6) = OP_l_5_c_6_r_ + SQ(3) + OP_l_1_c_6_r_*SF(6) + OP_l_2_c_6_r_*SF(4) - OP_l_4_c_6_r_*SF(5) + OP_l_3_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(4) + OP_l_15_c_6_r_*SPP(7) - OP_l_16_c_6_r_*SPP(10) + SF(5)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SF(4)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SF(6)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) - SPP(1)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SPP(9)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) + SPP(3)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) + SPP(6)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10)); -nextP(6,6) = OP_l_6_c_6_r_ + OP_l_1_c_6_r_*SF(5) + OP_l_3_c_6_r_*SF(4) + OP_l_4_c_6_r_*SF(6) - OP_l_2_c_6_r_*SPP(1) - OP_l_14_c_6_r_*SPP(9) + OP_l_15_c_6_r_*SPP(3) + OP_l_16_c_6_r_*SPP(6) + dvxVar*(SG(8) + 2*q0*q3)^2 + dvzVar*(SG(6) - 2*q0*q1)^2 + SF(5)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SF(5) + OP_l_3_c_1_r_*SF(4) + OP_l_4_c_1_r_*SF(6) - OP_l_2_c_1_r_*SPP(1) - OP_l_14_c_1_r_*SPP(9) + OP_l_15_c_1_r_*SPP(3) + OP_l_16_c_1_r_*SPP(6)) + SF(4)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SF(5) + OP_l_3_c_3_r_*SF(4) + OP_l_4_c_3_r_*SF(6) - OP_l_2_c_3_r_*SPP(1) - OP_l_14_c_3_r_*SPP(9) + OP_l_15_c_3_r_*SPP(3) + OP_l_16_c_3_r_*SPP(6)) + SF(6)*(OP_l_6_c_4_r_ + OP_l_1_c_4_r_*SF(5) + OP_l_3_c_4_r_*SF(4) + OP_l_4_c_4_r_*SF(6) - OP_l_2_c_4_r_*SPP(1) - OP_l_14_c_4_r_*SPP(9) + OP_l_15_c_4_r_*SPP(3) + OP_l_16_c_4_r_*SPP(6)) - SPP(1)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SF(5) + OP_l_3_c_2_r_*SF(4) + OP_l_4_c_2_r_*SF(6) - OP_l_2_c_2_r_*SPP(1) - OP_l_14_c_2_r_*SPP(9) + OP_l_15_c_2_r_*SPP(3) + OP_l_16_c_2_r_*SPP(6)) - SPP(9)*(OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6)) + SPP(3)*(OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6)) + SPP(6)*(OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6)) + dvyVar*(SG(2) - SG(3) + SG(4) - SG(5))^2; -nextP(1,7) = OP_l_1_c_7_r_ + OP_l_2_c_7_r_*SF(10) + OP_l_3_c_7_r_*SF(12) + OP_l_4_c_7_r_*SF(11) + OP_l_11_c_7_r_*SF(15) + OP_l_12_c_7_r_*SF(16) + OP_l_13_c_7_r_*SPP(11) + SF(5)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SF(6)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(4)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SPP(1)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SPP(5)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) - SPP(8)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) - SPP(2)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11)); -nextP(2,7) = OP_l_2_c_7_r_ + OP_l_1_c_7_r_*SF(9) + OP_l_3_c_7_r_*SF(8) + OP_l_4_c_7_r_*SF(12) - OP_l_13_c_7_r_*SF(16) + OP_l_12_c_7_r_*SPP(11) - (OP_l_11_c_7_r_*q0)/2 + SF(5)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SF(6)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(4)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SPP(1)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SPP(5)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) - SPP(8)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) - SPP(2)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2); -nextP(3,7) = OP_l_3_c_7_r_ + OP_l_1_c_7_r_*SF(7) + OP_l_2_c_7_r_*SF(11) + OP_l_4_c_7_r_*SF(9) + OP_l_13_c_7_r_*SF(15) - OP_l_11_c_7_r_*SPP(11) - (OP_l_12_c_7_r_*q0)/2 + SF(5)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SF(6)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(4)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SPP(1)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SPP(5)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) - SPP(8)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) - SPP(2)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2); -nextP(4,7) = OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SF(8) + OP_l_2_c_7_r_*SF(7) + OP_l_3_c_7_r_*SF(10) + OP_l_11_c_7_r_*SF(16) - OP_l_12_c_7_r_*SF(15) - (OP_l_13_c_7_r_*q0)/2 + SF(5)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SF(6)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(4)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) + SPP(1)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SPP(5)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) - SPP(8)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) - SPP(2)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2); -nextP(5,7) = OP_l_5_c_7_r_ + SQ(2) + OP_l_1_c_7_r_*SF(6) + OP_l_2_c_7_r_*SF(4) - OP_l_4_c_7_r_*SF(5) + OP_l_3_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(4) + OP_l_15_c_7_r_*SPP(7) - OP_l_16_c_7_r_*SPP(10) + SF(5)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SF(6)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SF(4)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) + SPP(1)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SPP(5)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) - SPP(8)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) - SPP(2)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10)); -nextP(6,7) = OP_l_6_c_7_r_ + SQ(1) + OP_l_1_c_7_r_*SF(5) + OP_l_3_c_7_r_*SF(4) + OP_l_4_c_7_r_*SF(6) - OP_l_2_c_7_r_*SPP(1) - OP_l_14_c_7_r_*SPP(9) + OP_l_15_c_7_r_*SPP(3) + OP_l_16_c_7_r_*SPP(6) + SF(5)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SF(5) + OP_l_3_c_2_r_*SF(4) + OP_l_4_c_2_r_*SF(6) - OP_l_2_c_2_r_*SPP(1) - OP_l_14_c_2_r_*SPP(9) + OP_l_15_c_2_r_*SPP(3) + OP_l_16_c_2_r_*SPP(6)) - SF(6)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SF(5) + OP_l_3_c_3_r_*SF(4) + OP_l_4_c_3_r_*SF(6) - OP_l_2_c_3_r_*SPP(1) - OP_l_14_c_3_r_*SPP(9) + OP_l_15_c_3_r_*SPP(3) + OP_l_16_c_3_r_*SPP(6)) + SF(4)*(OP_l_6_c_4_r_ + OP_l_1_c_4_r_*SF(5) + OP_l_3_c_4_r_*SF(4) + OP_l_4_c_4_r_*SF(6) - OP_l_2_c_4_r_*SPP(1) - OP_l_14_c_4_r_*SPP(9) + OP_l_15_c_4_r_*SPP(3) + OP_l_16_c_4_r_*SPP(6)) + SPP(1)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SF(5) + OP_l_3_c_1_r_*SF(4) + OP_l_4_c_1_r_*SF(6) - OP_l_2_c_1_r_*SPP(1) - OP_l_14_c_1_r_*SPP(9) + OP_l_15_c_1_r_*SPP(3) + OP_l_16_c_1_r_*SPP(6)) + SPP(5)*(OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6)) - SPP(8)*(OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6)) - SPP(2)*(OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6)); -nextP(7,7) = OP_l_7_c_7_r_ + OP_l_2_c_7_r_*SF(5) - OP_l_3_c_7_r_*SF(6) + OP_l_4_c_7_r_*SF(4) + OP_l_1_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(5) - OP_l_15_c_7_r_*SPP(8) - OP_l_16_c_7_r_*SPP(2) + dvxVar*(SG(7) - 2*q0*q2)^2 + dvyVar*(SG(6) + 2*q0*q1)^2 + SF(5)*(OP_l_7_c_2_r_ + OP_l_2_c_2_r_*SF(5) - OP_l_3_c_2_r_*SF(6) + OP_l_4_c_2_r_*SF(4) + OP_l_1_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(5) - OP_l_15_c_2_r_*SPP(8) - OP_l_16_c_2_r_*SPP(2)) - SF(6)*(OP_l_7_c_3_r_ + OP_l_2_c_3_r_*SF(5) - OP_l_3_c_3_r_*SF(6) + OP_l_4_c_3_r_*SF(4) + OP_l_1_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(5) - OP_l_15_c_3_r_*SPP(8) - OP_l_16_c_3_r_*SPP(2)) + SF(4)*(OP_l_7_c_4_r_ + OP_l_2_c_4_r_*SF(5) - OP_l_3_c_4_r_*SF(6) + OP_l_4_c_4_r_*SF(4) + OP_l_1_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(5) - OP_l_15_c_4_r_*SPP(8) - OP_l_16_c_4_r_*SPP(2)) + SPP(1)*(OP_l_7_c_1_r_ + OP_l_2_c_1_r_*SF(5) - OP_l_3_c_1_r_*SF(6) + OP_l_4_c_1_r_*SF(4) + OP_l_1_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(5) - OP_l_15_c_1_r_*SPP(8) - OP_l_16_c_1_r_*SPP(2)) + SPP(5)*(OP_l_7_c_14_r_ + OP_l_2_c_14_r_*SF(5) - OP_l_3_c_14_r_*SF(6) + OP_l_4_c_14_r_*SF(4) + OP_l_1_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(5) - OP_l_15_c_14_r_*SPP(8) - OP_l_16_c_14_r_*SPP(2)) - SPP(8)*(OP_l_7_c_15_r_ + OP_l_2_c_15_r_*SF(5) - OP_l_3_c_15_r_*SF(6) + OP_l_4_c_15_r_*SF(4) + OP_l_1_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(5) - OP_l_15_c_15_r_*SPP(8) - OP_l_16_c_15_r_*SPP(2)) - SPP(2)*(OP_l_7_c_16_r_ + OP_l_2_c_16_r_*SF(5) - OP_l_3_c_16_r_*SF(6) + OP_l_4_c_16_r_*SF(4) + OP_l_1_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(5) - OP_l_15_c_16_r_*SPP(8) - OP_l_16_c_16_r_*SPP(2)) + dvzVar*(SG(2) - SG(3) - SG(4) + SG(5))^2; -nextP(1,8) = OP_l_1_c_8_r_ + OP_l_2_c_8_r_*SF(10) + OP_l_3_c_8_r_*SF(12) + OP_l_4_c_8_r_*SF(11) + OP_l_11_c_8_r_*SF(15) + OP_l_12_c_8_r_*SF(16) + OP_l_13_c_8_r_*SPP(11) + dt*(OP_l_1_c_5_r_ + OP_l_2_c_5_r_*SF(10) + OP_l_3_c_5_r_*SF(12) + OP_l_4_c_5_r_*SF(11) + OP_l_11_c_5_r_*SF(15) + OP_l_12_c_5_r_*SF(16) + OP_l_13_c_5_r_*SPP(11)); -nextP(2,8) = OP_l_2_c_8_r_ + OP_l_1_c_8_r_*SF(9) + OP_l_3_c_8_r_*SF(8) + OP_l_4_c_8_r_*SF(12) - OP_l_13_c_8_r_*SF(16) + OP_l_12_c_8_r_*SPP(11) - (OP_l_11_c_8_r_*q0)/2 + dt*(OP_l_2_c_5_r_ + OP_l_1_c_5_r_*SF(9) + OP_l_3_c_5_r_*SF(8) + OP_l_4_c_5_r_*SF(12) - OP_l_13_c_5_r_*SF(16) + OP_l_12_c_5_r_*SPP(11) - (OP_l_11_c_5_r_*q0)/2); -nextP(3,8) = OP_l_3_c_8_r_ + OP_l_1_c_8_r_*SF(7) + OP_l_2_c_8_r_*SF(11) + OP_l_4_c_8_r_*SF(9) + OP_l_13_c_8_r_*SF(15) - OP_l_11_c_8_r_*SPP(11) - (OP_l_12_c_8_r_*q0)/2 + dt*(OP_l_3_c_5_r_ + OP_l_1_c_5_r_*SF(7) + OP_l_2_c_5_r_*SF(11) + OP_l_4_c_5_r_*SF(9) + OP_l_13_c_5_r_*SF(15) - OP_l_11_c_5_r_*SPP(11) - (OP_l_12_c_5_r_*q0)/2); -nextP(4,8) = OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SF(8) + OP_l_2_c_8_r_*SF(7) + OP_l_3_c_8_r_*SF(10) + OP_l_11_c_8_r_*SF(16) - OP_l_12_c_8_r_*SF(15) - (OP_l_13_c_8_r_*q0)/2 + dt*(OP_l_4_c_5_r_ + OP_l_1_c_5_r_*SF(8) + OP_l_2_c_5_r_*SF(7) + OP_l_3_c_5_r_*SF(10) + OP_l_11_c_5_r_*SF(16) - OP_l_12_c_5_r_*SF(15) - (OP_l_13_c_5_r_*q0)/2); -nextP(5,8) = OP_l_5_c_8_r_ + OP_l_1_c_8_r_*SF(6) + OP_l_2_c_8_r_*SF(4) - OP_l_4_c_8_r_*SF(5) + OP_l_3_c_8_r_*SPP(1) + OP_l_14_c_8_r_*SPP(4) + OP_l_15_c_8_r_*SPP(7) - OP_l_16_c_8_r_*SPP(10) + dt*(OP_l_5_c_5_r_ + OP_l_1_c_5_r_*SF(6) + OP_l_2_c_5_r_*SF(4) - OP_l_4_c_5_r_*SF(5) + OP_l_3_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(4) + OP_l_15_c_5_r_*SPP(7) - OP_l_16_c_5_r_*SPP(10)); -nextP(6,8) = OP_l_6_c_8_r_ + OP_l_1_c_8_r_*SF(5) + OP_l_3_c_8_r_*SF(4) + OP_l_4_c_8_r_*SF(6) - OP_l_2_c_8_r_*SPP(1) - OP_l_14_c_8_r_*SPP(9) + OP_l_15_c_8_r_*SPP(3) + OP_l_16_c_8_r_*SPP(6) + dt*(OP_l_6_c_5_r_ + OP_l_1_c_5_r_*SF(5) + OP_l_3_c_5_r_*SF(4) + OP_l_4_c_5_r_*SF(6) - OP_l_2_c_5_r_*SPP(1) - OP_l_14_c_5_r_*SPP(9) + OP_l_15_c_5_r_*SPP(3) + OP_l_16_c_5_r_*SPP(6)); -nextP(7,8) = OP_l_7_c_8_r_ + OP_l_2_c_8_r_*SF(5) - OP_l_3_c_8_r_*SF(6) + OP_l_4_c_8_r_*SF(4) + OP_l_1_c_8_r_*SPP(1) + OP_l_14_c_8_r_*SPP(5) - OP_l_15_c_8_r_*SPP(8) - OP_l_16_c_8_r_*SPP(2) + dt*(OP_l_7_c_5_r_ + OP_l_2_c_5_r_*SF(5) - OP_l_3_c_5_r_*SF(6) + OP_l_4_c_5_r_*SF(4) + OP_l_1_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(5) - OP_l_15_c_5_r_*SPP(8) - OP_l_16_c_5_r_*SPP(2)); -nextP(8,8) = OP_l_8_c_8_r_ + OP_l_5_c_8_r_*dt + dt*(OP_l_8_c_5_r_ + OP_l_5_c_5_r_*dt); -nextP(1,9) = OP_l_1_c_9_r_ + OP_l_2_c_9_r_*SF(10) + OP_l_3_c_9_r_*SF(12) + OP_l_4_c_9_r_*SF(11) + OP_l_11_c_9_r_*SF(15) + OP_l_12_c_9_r_*SF(16) + OP_l_13_c_9_r_*SPP(11) + dt*(OP_l_1_c_6_r_ + OP_l_2_c_6_r_*SF(10) + OP_l_3_c_6_r_*SF(12) + OP_l_4_c_6_r_*SF(11) + OP_l_11_c_6_r_*SF(15) + OP_l_12_c_6_r_*SF(16) + OP_l_13_c_6_r_*SPP(11)); -nextP(2,9) = OP_l_2_c_9_r_ + OP_l_1_c_9_r_*SF(9) + OP_l_3_c_9_r_*SF(8) + OP_l_4_c_9_r_*SF(12) - OP_l_13_c_9_r_*SF(16) + OP_l_12_c_9_r_*SPP(11) - (OP_l_11_c_9_r_*q0)/2 + dt*(OP_l_2_c_6_r_ + OP_l_1_c_6_r_*SF(9) + OP_l_3_c_6_r_*SF(8) + OP_l_4_c_6_r_*SF(12) - OP_l_13_c_6_r_*SF(16) + OP_l_12_c_6_r_*SPP(11) - (OP_l_11_c_6_r_*q0)/2); -nextP(3,9) = OP_l_3_c_9_r_ + OP_l_1_c_9_r_*SF(7) + OP_l_2_c_9_r_*SF(11) + OP_l_4_c_9_r_*SF(9) + OP_l_13_c_9_r_*SF(15) - OP_l_11_c_9_r_*SPP(11) - (OP_l_12_c_9_r_*q0)/2 + dt*(OP_l_3_c_6_r_ + OP_l_1_c_6_r_*SF(7) + OP_l_2_c_6_r_*SF(11) + OP_l_4_c_6_r_*SF(9) + OP_l_13_c_6_r_*SF(15) - OP_l_11_c_6_r_*SPP(11) - (OP_l_12_c_6_r_*q0)/2); -nextP(4,9) = OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SF(8) + OP_l_2_c_9_r_*SF(7) + OP_l_3_c_9_r_*SF(10) + OP_l_11_c_9_r_*SF(16) - OP_l_12_c_9_r_*SF(15) - (OP_l_13_c_9_r_*q0)/2 + dt*(OP_l_4_c_6_r_ + OP_l_1_c_6_r_*SF(8) + OP_l_2_c_6_r_*SF(7) + OP_l_3_c_6_r_*SF(10) + OP_l_11_c_6_r_*SF(16) - OP_l_12_c_6_r_*SF(15) - (OP_l_13_c_6_r_*q0)/2); -nextP(5,9) = OP_l_5_c_9_r_ + OP_l_1_c_9_r_*SF(6) + OP_l_2_c_9_r_*SF(4) - OP_l_4_c_9_r_*SF(5) + OP_l_3_c_9_r_*SPP(1) + OP_l_14_c_9_r_*SPP(4) + OP_l_15_c_9_r_*SPP(7) - OP_l_16_c_9_r_*SPP(10) + dt*(OP_l_5_c_6_r_ + OP_l_1_c_6_r_*SF(6) + OP_l_2_c_6_r_*SF(4) - OP_l_4_c_6_r_*SF(5) + OP_l_3_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(4) + OP_l_15_c_6_r_*SPP(7) - OP_l_16_c_6_r_*SPP(10)); -nextP(6,9) = OP_l_6_c_9_r_ + OP_l_1_c_9_r_*SF(5) + OP_l_3_c_9_r_*SF(4) + OP_l_4_c_9_r_*SF(6) - OP_l_2_c_9_r_*SPP(1) - OP_l_14_c_9_r_*SPP(9) + OP_l_15_c_9_r_*SPP(3) + OP_l_16_c_9_r_*SPP(6) + dt*(OP_l_6_c_6_r_ + OP_l_1_c_6_r_*SF(5) + OP_l_3_c_6_r_*SF(4) + OP_l_4_c_6_r_*SF(6) - OP_l_2_c_6_r_*SPP(1) - OP_l_14_c_6_r_*SPP(9) + OP_l_15_c_6_r_*SPP(3) + OP_l_16_c_6_r_*SPP(6)); -nextP(7,9) = OP_l_7_c_9_r_ + OP_l_2_c_9_r_*SF(5) - OP_l_3_c_9_r_*SF(6) + OP_l_4_c_9_r_*SF(4) + OP_l_1_c_9_r_*SPP(1) + OP_l_14_c_9_r_*SPP(5) - OP_l_15_c_9_r_*SPP(8) - OP_l_16_c_9_r_*SPP(2) + dt*(OP_l_7_c_6_r_ + OP_l_2_c_6_r_*SF(5) - OP_l_3_c_6_r_*SF(6) + OP_l_4_c_6_r_*SF(4) + OP_l_1_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(5) - OP_l_15_c_6_r_*SPP(8) - OP_l_16_c_6_r_*SPP(2)); -nextP(8,9) = OP_l_8_c_9_r_ + OP_l_5_c_9_r_*dt + dt*(OP_l_8_c_6_r_ + OP_l_5_c_6_r_*dt); -nextP(9,9) = OP_l_9_c_9_r_ + OP_l_6_c_9_r_*dt + dt*(OP_l_9_c_6_r_ + OP_l_6_c_6_r_*dt); -nextP(1,10) = OP_l_1_c_10_r_ + OP_l_2_c_10_r_*SF(10) + OP_l_3_c_10_r_*SF(12) + OP_l_4_c_10_r_*SF(11) + OP_l_11_c_10_r_*SF(15) + OP_l_12_c_10_r_*SF(16) + OP_l_13_c_10_r_*SPP(11) + dt*(OP_l_1_c_7_r_ + OP_l_2_c_7_r_*SF(10) + OP_l_3_c_7_r_*SF(12) + OP_l_4_c_7_r_*SF(11) + OP_l_11_c_7_r_*SF(15) + OP_l_12_c_7_r_*SF(16) + OP_l_13_c_7_r_*SPP(11)); -nextP(2,10) = OP_l_2_c_10_r_ + OP_l_1_c_10_r_*SF(9) + OP_l_3_c_10_r_*SF(8) + OP_l_4_c_10_r_*SF(12) - OP_l_13_c_10_r_*SF(16) + OP_l_12_c_10_r_*SPP(11) - (OP_l_11_c_10_r_*q0)/2 + dt*(OP_l_2_c_7_r_ + OP_l_1_c_7_r_*SF(9) + OP_l_3_c_7_r_*SF(8) + OP_l_4_c_7_r_*SF(12) - OP_l_13_c_7_r_*SF(16) + OP_l_12_c_7_r_*SPP(11) - (OP_l_11_c_7_r_*q0)/2); -nextP(3,10) = OP_l_3_c_10_r_ + OP_l_1_c_10_r_*SF(7) + OP_l_2_c_10_r_*SF(11) + OP_l_4_c_10_r_*SF(9) + OP_l_13_c_10_r_*SF(15) - OP_l_11_c_10_r_*SPP(11) - (OP_l_12_c_10_r_*q0)/2 + dt*(OP_l_3_c_7_r_ + OP_l_1_c_7_r_*SF(7) + OP_l_2_c_7_r_*SF(11) + OP_l_4_c_7_r_*SF(9) + OP_l_13_c_7_r_*SF(15) - OP_l_11_c_7_r_*SPP(11) - (OP_l_12_c_7_r_*q0)/2); -nextP(4,10) = OP_l_4_c_10_r_ + OP_l_1_c_10_r_*SF(8) + OP_l_2_c_10_r_*SF(7) + OP_l_3_c_10_r_*SF(10) + OP_l_11_c_10_r_*SF(16) - OP_l_12_c_10_r_*SF(15) - (OP_l_13_c_10_r_*q0)/2 + dt*(OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SF(8) + OP_l_2_c_7_r_*SF(7) + OP_l_3_c_7_r_*SF(10) + OP_l_11_c_7_r_*SF(16) - OP_l_12_c_7_r_*SF(15) - (OP_l_13_c_7_r_*q0)/2); -nextP(5,10) = OP_l_5_c_10_r_ + OP_l_1_c_10_r_*SF(6) + OP_l_2_c_10_r_*SF(4) - OP_l_4_c_10_r_*SF(5) + OP_l_3_c_10_r_*SPP(1) + OP_l_14_c_10_r_*SPP(4) + OP_l_15_c_10_r_*SPP(7) - OP_l_16_c_10_r_*SPP(10) + dt*(OP_l_5_c_7_r_ + OP_l_1_c_7_r_*SF(6) + OP_l_2_c_7_r_*SF(4) - OP_l_4_c_7_r_*SF(5) + OP_l_3_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(4) + OP_l_15_c_7_r_*SPP(7) - OP_l_16_c_7_r_*SPP(10)); -nextP(6,10) = OP_l_6_c_10_r_ + OP_l_1_c_10_r_*SF(5) + OP_l_3_c_10_r_*SF(4) + OP_l_4_c_10_r_*SF(6) - OP_l_2_c_10_r_*SPP(1) - OP_l_14_c_10_r_*SPP(9) + OP_l_15_c_10_r_*SPP(3) + OP_l_16_c_10_r_*SPP(6) + dt*(OP_l_6_c_7_r_ + OP_l_1_c_7_r_*SF(5) + OP_l_3_c_7_r_*SF(4) + OP_l_4_c_7_r_*SF(6) - OP_l_2_c_7_r_*SPP(1) - OP_l_14_c_7_r_*SPP(9) + OP_l_15_c_7_r_*SPP(3) + OP_l_16_c_7_r_*SPP(6)); -nextP(7,10) = OP_l_7_c_10_r_ + OP_l_2_c_10_r_*SF(5) - OP_l_3_c_10_r_*SF(6) + OP_l_4_c_10_r_*SF(4) + OP_l_1_c_10_r_*SPP(1) + OP_l_14_c_10_r_*SPP(5) - OP_l_15_c_10_r_*SPP(8) - OP_l_16_c_10_r_*SPP(2) + dt*(OP_l_7_c_7_r_ + OP_l_2_c_7_r_*SF(5) - OP_l_3_c_7_r_*SF(6) + OP_l_4_c_7_r_*SF(4) + OP_l_1_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(5) - OP_l_15_c_7_r_*SPP(8) - OP_l_16_c_7_r_*SPP(2)); -nextP(8,10) = OP_l_8_c_10_r_ + OP_l_5_c_10_r_*dt + dt*(OP_l_8_c_7_r_ + OP_l_5_c_7_r_*dt); -nextP(9,10) = OP_l_9_c_10_r_ + OP_l_6_c_10_r_*dt + dt*(OP_l_9_c_7_r_ + OP_l_6_c_7_r_*dt); -nextP(10,10) = OP_l_10_c_10_r_ + OP_l_7_c_10_r_*dt + dt*(OP_l_10_c_7_r_ + OP_l_7_c_7_r_*dt); -nextP(1,11) = OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11); -nextP(2,11) = OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2; -nextP(3,11) = OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2; -nextP(4,11) = OP_l_4_c_11_r_ + OP_l_1_c_11_r_*SF(8) + OP_l_2_c_11_r_*SF(7) + OP_l_3_c_11_r_*SF(10) + OP_l_11_c_11_r_*SF(16) - OP_l_12_c_11_r_*SF(15) - (OP_l_13_c_11_r_*q0)/2; -nextP(5,11) = OP_l_5_c_11_r_ + OP_l_1_c_11_r_*SF(6) + OP_l_2_c_11_r_*SF(4) - OP_l_4_c_11_r_*SF(5) + OP_l_3_c_11_r_*SPP(1) + OP_l_14_c_11_r_*SPP(4) + OP_l_15_c_11_r_*SPP(7) - OP_l_16_c_11_r_*SPP(10); -nextP(6,11) = OP_l_6_c_11_r_ + OP_l_1_c_11_r_*SF(5) + OP_l_3_c_11_r_*SF(4) + OP_l_4_c_11_r_*SF(6) - OP_l_2_c_11_r_*SPP(1) - OP_l_14_c_11_r_*SPP(9) + OP_l_15_c_11_r_*SPP(3) + OP_l_16_c_11_r_*SPP(6); -nextP(7,11) = OP_l_7_c_11_r_ + OP_l_2_c_11_r_*SF(5) - OP_l_3_c_11_r_*SF(6) + OP_l_4_c_11_r_*SF(4) + OP_l_1_c_11_r_*SPP(1) + OP_l_14_c_11_r_*SPP(5) - OP_l_15_c_11_r_*SPP(8) - OP_l_16_c_11_r_*SPP(2); -nextP(8,11) = OP_l_8_c_11_r_ + OP_l_5_c_11_r_*dt; -nextP(9,11) = OP_l_9_c_11_r_ + OP_l_6_c_11_r_*dt; -nextP(10,11) = OP_l_10_c_11_r_ + OP_l_7_c_11_r_*dt; -nextP(11,11) = OP_l_11_c_11_r_; -nextP(1,12) = OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11); -nextP(2,12) = OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2; -nextP(3,12) = OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2; -nextP(4,12) = OP_l_4_c_12_r_ + OP_l_1_c_12_r_*SF(8) + OP_l_2_c_12_r_*SF(7) + OP_l_3_c_12_r_*SF(10) + OP_l_11_c_12_r_*SF(16) - OP_l_12_c_12_r_*SF(15) - (OP_l_13_c_12_r_*q0)/2; -nextP(5,12) = OP_l_5_c_12_r_ + OP_l_1_c_12_r_*SF(6) + OP_l_2_c_12_r_*SF(4) - OP_l_4_c_12_r_*SF(5) + OP_l_3_c_12_r_*SPP(1) + OP_l_14_c_12_r_*SPP(4) + OP_l_15_c_12_r_*SPP(7) - OP_l_16_c_12_r_*SPP(10); -nextP(6,12) = OP_l_6_c_12_r_ + OP_l_1_c_12_r_*SF(5) + OP_l_3_c_12_r_*SF(4) + OP_l_4_c_12_r_*SF(6) - OP_l_2_c_12_r_*SPP(1) - OP_l_14_c_12_r_*SPP(9) + OP_l_15_c_12_r_*SPP(3) + OP_l_16_c_12_r_*SPP(6); -nextP(7,12) = OP_l_7_c_12_r_ + OP_l_2_c_12_r_*SF(5) - OP_l_3_c_12_r_*SF(6) + OP_l_4_c_12_r_*SF(4) + OP_l_1_c_12_r_*SPP(1) + OP_l_14_c_12_r_*SPP(5) - OP_l_15_c_12_r_*SPP(8) - OP_l_16_c_12_r_*SPP(2); -nextP(8,12) = OP_l_8_c_12_r_ + OP_l_5_c_12_r_*dt; -nextP(9,12) = OP_l_9_c_12_r_ + OP_l_6_c_12_r_*dt; -nextP(10,12) = OP_l_10_c_12_r_ + OP_l_7_c_12_r_*dt; -nextP(11,12) = OP_l_11_c_12_r_; -nextP(12,12) = OP_l_12_c_12_r_; -nextP(1,13) = OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11); -nextP(2,13) = OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2; -nextP(3,13) = OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2; -nextP(4,13) = OP_l_4_c_13_r_ + OP_l_1_c_13_r_*SF(8) + OP_l_2_c_13_r_*SF(7) + OP_l_3_c_13_r_*SF(10) + OP_l_11_c_13_r_*SF(16) - OP_l_12_c_13_r_*SF(15) - (OP_l_13_c_13_r_*q0)/2; -nextP(5,13) = OP_l_5_c_13_r_ + OP_l_1_c_13_r_*SF(6) + OP_l_2_c_13_r_*SF(4) - OP_l_4_c_13_r_*SF(5) + OP_l_3_c_13_r_*SPP(1) + OP_l_14_c_13_r_*SPP(4) + OP_l_15_c_13_r_*SPP(7) - OP_l_16_c_13_r_*SPP(10); -nextP(6,13) = OP_l_6_c_13_r_ + OP_l_1_c_13_r_*SF(5) + OP_l_3_c_13_r_*SF(4) + OP_l_4_c_13_r_*SF(6) - OP_l_2_c_13_r_*SPP(1) - OP_l_14_c_13_r_*SPP(9) + OP_l_15_c_13_r_*SPP(3) + OP_l_16_c_13_r_*SPP(6); -nextP(7,13) = OP_l_7_c_13_r_ + OP_l_2_c_13_r_*SF(5) - OP_l_3_c_13_r_*SF(6) + OP_l_4_c_13_r_*SF(4) + OP_l_1_c_13_r_*SPP(1) + OP_l_14_c_13_r_*SPP(5) - OP_l_15_c_13_r_*SPP(8) - OP_l_16_c_13_r_*SPP(2); -nextP(8,13) = OP_l_8_c_13_r_ + OP_l_5_c_13_r_*dt; -nextP(9,13) = OP_l_9_c_13_r_ + OP_l_6_c_13_r_*dt; -nextP(10,13) = OP_l_10_c_13_r_ + OP_l_7_c_13_r_*dt; -nextP(11,13) = OP_l_11_c_13_r_; -nextP(12,13) = OP_l_12_c_13_r_; -nextP(13,13) = OP_l_13_c_13_r_; -nextP(1,14) = OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11); -nextP(2,14) = OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2; -nextP(3,14) = OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2; -nextP(4,14) = OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2; -nextP(5,14) = OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10); -nextP(6,14) = OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6); -nextP(7,14) = OP_l_7_c_14_r_ + OP_l_2_c_14_r_*SF(5) - OP_l_3_c_14_r_*SF(6) + OP_l_4_c_14_r_*SF(4) + OP_l_1_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(5) - OP_l_15_c_14_r_*SPP(8) - OP_l_16_c_14_r_*SPP(2); -nextP(8,14) = OP_l_8_c_14_r_ + OP_l_5_c_14_r_*dt; -nextP(9,14) = OP_l_9_c_14_r_ + OP_l_6_c_14_r_*dt; -nextP(10,14) = OP_l_10_c_14_r_ + OP_l_7_c_14_r_*dt; -nextP(11,14) = OP_l_11_c_14_r_; -nextP(12,14) = OP_l_12_c_14_r_; -nextP(13,14) = OP_l_13_c_14_r_; -nextP(14,14) = OP_l_14_c_14_r_; -nextP(1,15) = OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11); -nextP(2,15) = OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2; -nextP(3,15) = OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2; -nextP(4,15) = OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2; -nextP(5,15) = OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10); -nextP(6,15) = OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6); -nextP(7,15) = OP_l_7_c_15_r_ + OP_l_2_c_15_r_*SF(5) - OP_l_3_c_15_r_*SF(6) + OP_l_4_c_15_r_*SF(4) + OP_l_1_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(5) - OP_l_15_c_15_r_*SPP(8) - OP_l_16_c_15_r_*SPP(2); -nextP(8,15) = OP_l_8_c_15_r_ + OP_l_5_c_15_r_*dt; -nextP(9,15) = OP_l_9_c_15_r_ + OP_l_6_c_15_r_*dt; -nextP(10,15) = OP_l_10_c_15_r_ + OP_l_7_c_15_r_*dt; -nextP(11,15) = OP_l_11_c_15_r_; -nextP(12,15) = OP_l_12_c_15_r_; -nextP(13,15) = OP_l_13_c_15_r_; -nextP(14,15) = OP_l_14_c_15_r_; -nextP(15,15) = OP_l_15_c_15_r_; -nextP(1,16) = OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11); -nextP(2,16) = OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2; -nextP(3,16) = OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2; -nextP(4,16) = OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2; -nextP(5,16) = OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10); -nextP(6,16) = OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6); -nextP(7,16) = OP_l_7_c_16_r_ + OP_l_2_c_16_r_*SF(5) - OP_l_3_c_16_r_*SF(6) + OP_l_4_c_16_r_*SF(4) + OP_l_1_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(5) - OP_l_15_c_16_r_*SPP(8) - OP_l_16_c_16_r_*SPP(2); -nextP(8,16) = OP_l_8_c_16_r_ + OP_l_5_c_16_r_*dt; -nextP(9,16) = OP_l_9_c_16_r_ + OP_l_6_c_16_r_*dt; -nextP(10,16) = OP_l_10_c_16_r_ + OP_l_7_c_16_r_*dt; -nextP(11,16) = OP_l_11_c_16_r_; -nextP(12,16) = OP_l_12_c_16_r_; -nextP(13,16) = OP_l_13_c_16_r_; -nextP(14,16) = OP_l_14_c_16_r_; -nextP(15,16) = OP_l_15_c_16_r_; -nextP(16,16) = OP_l_16_c_16_r_; -nextP(1,17) = OP_l_1_c_17_r_ + OP_l_2_c_17_r_*SF(10) + OP_l_3_c_17_r_*SF(12) + OP_l_4_c_17_r_*SF(11) + OP_l_11_c_17_r_*SF(15) + OP_l_12_c_17_r_*SF(16) + OP_l_13_c_17_r_*SPP(11); -nextP(2,17) = OP_l_2_c_17_r_ + OP_l_1_c_17_r_*SF(9) + OP_l_3_c_17_r_*SF(8) + OP_l_4_c_17_r_*SF(12) - OP_l_13_c_17_r_*SF(16) + OP_l_12_c_17_r_*SPP(11) - (OP_l_11_c_17_r_*q0)/2; -nextP(3,17) = OP_l_3_c_17_r_ + OP_l_1_c_17_r_*SF(7) + OP_l_2_c_17_r_*SF(11) + OP_l_4_c_17_r_*SF(9) + OP_l_13_c_17_r_*SF(15) - OP_l_11_c_17_r_*SPP(11) - (OP_l_12_c_17_r_*q0)/2; -nextP(4,17) = OP_l_4_c_17_r_ + OP_l_1_c_17_r_*SF(8) + OP_l_2_c_17_r_*SF(7) + OP_l_3_c_17_r_*SF(10) + OP_l_11_c_17_r_*SF(16) - OP_l_12_c_17_r_*SF(15) - (OP_l_13_c_17_r_*q0)/2; -nextP(5,17) = OP_l_5_c_17_r_ + OP_l_1_c_17_r_*SF(6) + OP_l_2_c_17_r_*SF(4) - OP_l_4_c_17_r_*SF(5) + OP_l_3_c_17_r_*SPP(1) + OP_l_14_c_17_r_*SPP(4) + OP_l_15_c_17_r_*SPP(7) - OP_l_16_c_17_r_*SPP(10); -nextP(6,17) = OP_l_6_c_17_r_ + OP_l_1_c_17_r_*SF(5) + OP_l_3_c_17_r_*SF(4) + OP_l_4_c_17_r_*SF(6) - OP_l_2_c_17_r_*SPP(1) - OP_l_14_c_17_r_*SPP(9) + OP_l_15_c_17_r_*SPP(3) + OP_l_16_c_17_r_*SPP(6); -nextP(7,17) = OP_l_7_c_17_r_ + OP_l_2_c_17_r_*SF(5) - OP_l_3_c_17_r_*SF(6) + OP_l_4_c_17_r_*SF(4) + OP_l_1_c_17_r_*SPP(1) + OP_l_14_c_17_r_*SPP(5) - OP_l_15_c_17_r_*SPP(8) - OP_l_16_c_17_r_*SPP(2); -nextP(8,17) = OP_l_8_c_17_r_ + OP_l_5_c_17_r_*dt; -nextP(9,17) = OP_l_9_c_17_r_ + OP_l_6_c_17_r_*dt; -nextP(10,17) = OP_l_10_c_17_r_ + OP_l_7_c_17_r_*dt; -nextP(11,17) = OP_l_11_c_17_r_; -nextP(12,17) = OP_l_12_c_17_r_; -nextP(13,17) = OP_l_13_c_17_r_; -nextP(14,17) = OP_l_14_c_17_r_; -nextP(15,17) = OP_l_15_c_17_r_; -nextP(16,17) = OP_l_16_c_17_r_; -nextP(17,17) = OP_l_17_c_17_r_; -nextP(1,18) = OP_l_1_c_18_r_ + OP_l_2_c_18_r_*SF(10) + OP_l_3_c_18_r_*SF(12) + OP_l_4_c_18_r_*SF(11) + OP_l_11_c_18_r_*SF(15) + OP_l_12_c_18_r_*SF(16) + OP_l_13_c_18_r_*SPP(11); -nextP(2,18) = OP_l_2_c_18_r_ + OP_l_1_c_18_r_*SF(9) + OP_l_3_c_18_r_*SF(8) + OP_l_4_c_18_r_*SF(12) - OP_l_13_c_18_r_*SF(16) + OP_l_12_c_18_r_*SPP(11) - (OP_l_11_c_18_r_*q0)/2; -nextP(3,18) = OP_l_3_c_18_r_ + OP_l_1_c_18_r_*SF(7) + OP_l_2_c_18_r_*SF(11) + OP_l_4_c_18_r_*SF(9) + OP_l_13_c_18_r_*SF(15) - OP_l_11_c_18_r_*SPP(11) - (OP_l_12_c_18_r_*q0)/2; -nextP(4,18) = OP_l_4_c_18_r_ + OP_l_1_c_18_r_*SF(8) + OP_l_2_c_18_r_*SF(7) + OP_l_3_c_18_r_*SF(10) + OP_l_11_c_18_r_*SF(16) - OP_l_12_c_18_r_*SF(15) - (OP_l_13_c_18_r_*q0)/2; -nextP(5,18) = OP_l_5_c_18_r_ + OP_l_1_c_18_r_*SF(6) + OP_l_2_c_18_r_*SF(4) - OP_l_4_c_18_r_*SF(5) + OP_l_3_c_18_r_*SPP(1) + OP_l_14_c_18_r_*SPP(4) + OP_l_15_c_18_r_*SPP(7) - OP_l_16_c_18_r_*SPP(10); -nextP(6,18) = OP_l_6_c_18_r_ + OP_l_1_c_18_r_*SF(5) + OP_l_3_c_18_r_*SF(4) + OP_l_4_c_18_r_*SF(6) - OP_l_2_c_18_r_*SPP(1) - OP_l_14_c_18_r_*SPP(9) + OP_l_15_c_18_r_*SPP(3) + OP_l_16_c_18_r_*SPP(6); -nextP(7,18) = OP_l_7_c_18_r_ + OP_l_2_c_18_r_*SF(5) - OP_l_3_c_18_r_*SF(6) + OP_l_4_c_18_r_*SF(4) + OP_l_1_c_18_r_*SPP(1) + OP_l_14_c_18_r_*SPP(5) - OP_l_15_c_18_r_*SPP(8) - OP_l_16_c_18_r_*SPP(2); -nextP(8,18) = OP_l_8_c_18_r_ + OP_l_5_c_18_r_*dt; -nextP(9,18) = OP_l_9_c_18_r_ + OP_l_6_c_18_r_*dt; -nextP(10,18) = OP_l_10_c_18_r_ + OP_l_7_c_18_r_*dt; -nextP(11,18) = OP_l_11_c_18_r_; -nextP(12,18) = OP_l_12_c_18_r_; -nextP(13,18) = OP_l_13_c_18_r_; -nextP(14,18) = OP_l_14_c_18_r_; -nextP(15,18) = OP_l_15_c_18_r_; -nextP(16,18) = OP_l_16_c_18_r_; -nextP(17,18) = OP_l_17_c_18_r_; -nextP(18,18) = OP_l_18_c_18_r_; -nextP(1,19) = OP_l_1_c_19_r_ + OP_l_2_c_19_r_*SF(10) + OP_l_3_c_19_r_*SF(12) + OP_l_4_c_19_r_*SF(11) + OP_l_11_c_19_r_*SF(15) + OP_l_12_c_19_r_*SF(16) + OP_l_13_c_19_r_*SPP(11); -nextP(2,19) = OP_l_2_c_19_r_ + OP_l_1_c_19_r_*SF(9) + OP_l_3_c_19_r_*SF(8) + OP_l_4_c_19_r_*SF(12) - OP_l_13_c_19_r_*SF(16) + OP_l_12_c_19_r_*SPP(11) - (OP_l_11_c_19_r_*q0)/2; -nextP(3,19) = OP_l_3_c_19_r_ + OP_l_1_c_19_r_*SF(7) + OP_l_2_c_19_r_*SF(11) + OP_l_4_c_19_r_*SF(9) + OP_l_13_c_19_r_*SF(15) - OP_l_11_c_19_r_*SPP(11) - (OP_l_12_c_19_r_*q0)/2; -nextP(4,19) = OP_l_4_c_19_r_ + OP_l_1_c_19_r_*SF(8) + OP_l_2_c_19_r_*SF(7) + OP_l_3_c_19_r_*SF(10) + OP_l_11_c_19_r_*SF(16) - OP_l_12_c_19_r_*SF(15) - (OP_l_13_c_19_r_*q0)/2; -nextP(5,19) = OP_l_5_c_19_r_ + OP_l_1_c_19_r_*SF(6) + OP_l_2_c_19_r_*SF(4) - OP_l_4_c_19_r_*SF(5) + OP_l_3_c_19_r_*SPP(1) + OP_l_14_c_19_r_*SPP(4) + OP_l_15_c_19_r_*SPP(7) - OP_l_16_c_19_r_*SPP(10); -nextP(6,19) = OP_l_6_c_19_r_ + OP_l_1_c_19_r_*SF(5) + OP_l_3_c_19_r_*SF(4) + OP_l_4_c_19_r_*SF(6) - OP_l_2_c_19_r_*SPP(1) - OP_l_14_c_19_r_*SPP(9) + OP_l_15_c_19_r_*SPP(3) + OP_l_16_c_19_r_*SPP(6); -nextP(7,19) = OP_l_7_c_19_r_ + OP_l_2_c_19_r_*SF(5) - OP_l_3_c_19_r_*SF(6) + OP_l_4_c_19_r_*SF(4) + OP_l_1_c_19_r_*SPP(1) + OP_l_14_c_19_r_*SPP(5) - OP_l_15_c_19_r_*SPP(8) - OP_l_16_c_19_r_*SPP(2); -nextP(8,19) = OP_l_8_c_19_r_ + OP_l_5_c_19_r_*dt; -nextP(9,19) = OP_l_9_c_19_r_ + OP_l_6_c_19_r_*dt; -nextP(10,19) = OP_l_10_c_19_r_ + OP_l_7_c_19_r_*dt; -nextP(11,19) = OP_l_11_c_19_r_; -nextP(12,19) = OP_l_12_c_19_r_; -nextP(13,19) = OP_l_13_c_19_r_; -nextP(14,19) = OP_l_14_c_19_r_; -nextP(15,19) = OP_l_15_c_19_r_; -nextP(16,19) = OP_l_16_c_19_r_; -nextP(17,19) = OP_l_17_c_19_r_; -nextP(18,19) = OP_l_18_c_19_r_; -nextP(19,19) = OP_l_19_c_19_r_; -nextP(1,20) = OP_l_1_c_20_r_ + OP_l_2_c_20_r_*SF(10) + OP_l_3_c_20_r_*SF(12) + OP_l_4_c_20_r_*SF(11) + OP_l_11_c_20_r_*SF(15) + OP_l_12_c_20_r_*SF(16) + OP_l_13_c_20_r_*SPP(11); -nextP(2,20) = OP_l_2_c_20_r_ + OP_l_1_c_20_r_*SF(9) + OP_l_3_c_20_r_*SF(8) + OP_l_4_c_20_r_*SF(12) - OP_l_13_c_20_r_*SF(16) + OP_l_12_c_20_r_*SPP(11) - (OP_l_11_c_20_r_*q0)/2; -nextP(3,20) = OP_l_3_c_20_r_ + OP_l_1_c_20_r_*SF(7) + OP_l_2_c_20_r_*SF(11) + OP_l_4_c_20_r_*SF(9) + OP_l_13_c_20_r_*SF(15) - OP_l_11_c_20_r_*SPP(11) - (OP_l_12_c_20_r_*q0)/2; -nextP(4,20) = OP_l_4_c_20_r_ + OP_l_1_c_20_r_*SF(8) + OP_l_2_c_20_r_*SF(7) + OP_l_3_c_20_r_*SF(10) + OP_l_11_c_20_r_*SF(16) - OP_l_12_c_20_r_*SF(15) - (OP_l_13_c_20_r_*q0)/2; -nextP(5,20) = OP_l_5_c_20_r_ + OP_l_1_c_20_r_*SF(6) + OP_l_2_c_20_r_*SF(4) - OP_l_4_c_20_r_*SF(5) + OP_l_3_c_20_r_*SPP(1) + OP_l_14_c_20_r_*SPP(4) + OP_l_15_c_20_r_*SPP(7) - OP_l_16_c_20_r_*SPP(10); -nextP(6,20) = OP_l_6_c_20_r_ + OP_l_1_c_20_r_*SF(5) + OP_l_3_c_20_r_*SF(4) + OP_l_4_c_20_r_*SF(6) - OP_l_2_c_20_r_*SPP(1) - OP_l_14_c_20_r_*SPP(9) + OP_l_15_c_20_r_*SPP(3) + OP_l_16_c_20_r_*SPP(6); -nextP(7,20) = OP_l_7_c_20_r_ + OP_l_2_c_20_r_*SF(5) - OP_l_3_c_20_r_*SF(6) + OP_l_4_c_20_r_*SF(4) + OP_l_1_c_20_r_*SPP(1) + OP_l_14_c_20_r_*SPP(5) - OP_l_15_c_20_r_*SPP(8) - OP_l_16_c_20_r_*SPP(2); -nextP(8,20) = OP_l_8_c_20_r_ + OP_l_5_c_20_r_*dt; -nextP(9,20) = OP_l_9_c_20_r_ + OP_l_6_c_20_r_*dt; -nextP(10,20) = OP_l_10_c_20_r_ + OP_l_7_c_20_r_*dt; -nextP(11,20) = OP_l_11_c_20_r_; -nextP(12,20) = OP_l_12_c_20_r_; -nextP(13,20) = OP_l_13_c_20_r_; -nextP(14,20) = OP_l_14_c_20_r_; -nextP(15,20) = OP_l_15_c_20_r_; -nextP(16,20) = OP_l_16_c_20_r_; -nextP(17,20) = OP_l_17_c_20_r_; -nextP(18,20) = OP_l_18_c_20_r_; -nextP(19,20) = OP_l_19_c_20_r_; -nextP(20,20) = OP_l_20_c_20_r_; -nextP(1,21) = OP_l_1_c_21_r_ + OP_l_2_c_21_r_*SF(10) + OP_l_3_c_21_r_*SF(12) + OP_l_4_c_21_r_*SF(11) + OP_l_11_c_21_r_*SF(15) + OP_l_12_c_21_r_*SF(16) + OP_l_13_c_21_r_*SPP(11); -nextP(2,21) = OP_l_2_c_21_r_ + OP_l_1_c_21_r_*SF(9) + OP_l_3_c_21_r_*SF(8) + OP_l_4_c_21_r_*SF(12) - OP_l_13_c_21_r_*SF(16) + OP_l_12_c_21_r_*SPP(11) - (OP_l_11_c_21_r_*q0)/2; -nextP(3,21) = OP_l_3_c_21_r_ + OP_l_1_c_21_r_*SF(7) + OP_l_2_c_21_r_*SF(11) + OP_l_4_c_21_r_*SF(9) + OP_l_13_c_21_r_*SF(15) - OP_l_11_c_21_r_*SPP(11) - (OP_l_12_c_21_r_*q0)/2; -nextP(4,21) = OP_l_4_c_21_r_ + OP_l_1_c_21_r_*SF(8) + OP_l_2_c_21_r_*SF(7) + OP_l_3_c_21_r_*SF(10) + OP_l_11_c_21_r_*SF(16) - OP_l_12_c_21_r_*SF(15) - (OP_l_13_c_21_r_*q0)/2; -nextP(5,21) = OP_l_5_c_21_r_ + OP_l_1_c_21_r_*SF(6) + OP_l_2_c_21_r_*SF(4) - OP_l_4_c_21_r_*SF(5) + OP_l_3_c_21_r_*SPP(1) + OP_l_14_c_21_r_*SPP(4) + OP_l_15_c_21_r_*SPP(7) - OP_l_16_c_21_r_*SPP(10); -nextP(6,21) = OP_l_6_c_21_r_ + OP_l_1_c_21_r_*SF(5) + OP_l_3_c_21_r_*SF(4) + OP_l_4_c_21_r_*SF(6) - OP_l_2_c_21_r_*SPP(1) - OP_l_14_c_21_r_*SPP(9) + OP_l_15_c_21_r_*SPP(3) + OP_l_16_c_21_r_*SPP(6); -nextP(7,21) = OP_l_7_c_21_r_ + OP_l_2_c_21_r_*SF(5) - OP_l_3_c_21_r_*SF(6) + OP_l_4_c_21_r_*SF(4) + OP_l_1_c_21_r_*SPP(1) + OP_l_14_c_21_r_*SPP(5) - OP_l_15_c_21_r_*SPP(8) - OP_l_16_c_21_r_*SPP(2); -nextP(8,21) = OP_l_8_c_21_r_ + OP_l_5_c_21_r_*dt; -nextP(9,21) = OP_l_9_c_21_r_ + OP_l_6_c_21_r_*dt; -nextP(10,21) = OP_l_10_c_21_r_ + OP_l_7_c_21_r_*dt; -nextP(11,21) = OP_l_11_c_21_r_; -nextP(12,21) = OP_l_12_c_21_r_; -nextP(13,21) = OP_l_13_c_21_r_; -nextP(14,21) = OP_l_14_c_21_r_; -nextP(15,21) = OP_l_15_c_21_r_; -nextP(16,21) = OP_l_16_c_21_r_; -nextP(17,21) = OP_l_17_c_21_r_; -nextP(18,21) = OP_l_18_c_21_r_; -nextP(19,21) = OP_l_19_c_21_r_; -nextP(20,21) = OP_l_20_c_21_r_; -nextP(21,21) = OP_l_21_c_21_r_; -nextP(1,22) = OP_l_1_c_22_r_ + OP_l_2_c_22_r_*SF(10) + OP_l_3_c_22_r_*SF(12) + OP_l_4_c_22_r_*SF(11) + OP_l_11_c_22_r_*SF(15) + OP_l_12_c_22_r_*SF(16) + OP_l_13_c_22_r_*SPP(11); -nextP(2,22) = OP_l_2_c_22_r_ + OP_l_1_c_22_r_*SF(9) + OP_l_3_c_22_r_*SF(8) + OP_l_4_c_22_r_*SF(12) - OP_l_13_c_22_r_*SF(16) + OP_l_12_c_22_r_*SPP(11) - (OP_l_11_c_22_r_*q0)/2; -nextP(3,22) = OP_l_3_c_22_r_ + OP_l_1_c_22_r_*SF(7) + OP_l_2_c_22_r_*SF(11) + OP_l_4_c_22_r_*SF(9) + OP_l_13_c_22_r_*SF(15) - OP_l_11_c_22_r_*SPP(11) - (OP_l_12_c_22_r_*q0)/2; -nextP(4,22) = OP_l_4_c_22_r_ + OP_l_1_c_22_r_*SF(8) + OP_l_2_c_22_r_*SF(7) + OP_l_3_c_22_r_*SF(10) + OP_l_11_c_22_r_*SF(16) - OP_l_12_c_22_r_*SF(15) - (OP_l_13_c_22_r_*q0)/2; -nextP(5,22) = OP_l_5_c_22_r_ + OP_l_1_c_22_r_*SF(6) + OP_l_2_c_22_r_*SF(4) - OP_l_4_c_22_r_*SF(5) + OP_l_3_c_22_r_*SPP(1) + OP_l_14_c_22_r_*SPP(4) + OP_l_15_c_22_r_*SPP(7) - OP_l_16_c_22_r_*SPP(10); -nextP(6,22) = OP_l_6_c_22_r_ + OP_l_1_c_22_r_*SF(5) + OP_l_3_c_22_r_*SF(4) + OP_l_4_c_22_r_*SF(6) - OP_l_2_c_22_r_*SPP(1) - OP_l_14_c_22_r_*SPP(9) + OP_l_15_c_22_r_*SPP(3) + OP_l_16_c_22_r_*SPP(6); -nextP(7,22) = OP_l_7_c_22_r_ + OP_l_2_c_22_r_*SF(5) - OP_l_3_c_22_r_*SF(6) + OP_l_4_c_22_r_*SF(4) + OP_l_1_c_22_r_*SPP(1) + OP_l_14_c_22_r_*SPP(5) - OP_l_15_c_22_r_*SPP(8) - OP_l_16_c_22_r_*SPP(2); -nextP(8,22) = OP_l_8_c_22_r_ + OP_l_5_c_22_r_*dt; -nextP(9,22) = OP_l_9_c_22_r_ + OP_l_6_c_22_r_*dt; -nextP(10,22) = OP_l_10_c_22_r_ + OP_l_7_c_22_r_*dt; -nextP(11,22) = OP_l_11_c_22_r_; -nextP(12,22) = OP_l_12_c_22_r_; -nextP(13,22) = OP_l_13_c_22_r_; -nextP(14,22) = OP_l_14_c_22_r_; -nextP(15,22) = OP_l_15_c_22_r_; -nextP(16,22) = OP_l_16_c_22_r_; -nextP(17,22) = OP_l_17_c_22_r_; -nextP(18,22) = OP_l_18_c_22_r_; -nextP(19,22) = OP_l_19_c_22_r_; -nextP(20,22) = OP_l_20_c_22_r_; -nextP(21,22) = OP_l_21_c_22_r_; -nextP(22,22) = OP_l_22_c_22_r_; -nextP(1,23) = OP_l_1_c_23_r_ + OP_l_2_c_23_r_*SF(10) + OP_l_3_c_23_r_*SF(12) + OP_l_4_c_23_r_*SF(11) + OP_l_11_c_23_r_*SF(15) + OP_l_12_c_23_r_*SF(16) + OP_l_13_c_23_r_*SPP(11); -nextP(2,23) = OP_l_2_c_23_r_ + OP_l_1_c_23_r_*SF(9) + OP_l_3_c_23_r_*SF(8) + OP_l_4_c_23_r_*SF(12) - OP_l_13_c_23_r_*SF(16) + OP_l_12_c_23_r_*SPP(11) - (OP_l_11_c_23_r_*q0)/2; -nextP(3,23) = OP_l_3_c_23_r_ + OP_l_1_c_23_r_*SF(7) + OP_l_2_c_23_r_*SF(11) + OP_l_4_c_23_r_*SF(9) + OP_l_13_c_23_r_*SF(15) - OP_l_11_c_23_r_*SPP(11) - (OP_l_12_c_23_r_*q0)/2; -nextP(4,23) = OP_l_4_c_23_r_ + OP_l_1_c_23_r_*SF(8) + OP_l_2_c_23_r_*SF(7) + OP_l_3_c_23_r_*SF(10) + OP_l_11_c_23_r_*SF(16) - OP_l_12_c_23_r_*SF(15) - (OP_l_13_c_23_r_*q0)/2; -nextP(5,23) = OP_l_5_c_23_r_ + OP_l_1_c_23_r_*SF(6) + OP_l_2_c_23_r_*SF(4) - OP_l_4_c_23_r_*SF(5) + OP_l_3_c_23_r_*SPP(1) + OP_l_14_c_23_r_*SPP(4) + OP_l_15_c_23_r_*SPP(7) - OP_l_16_c_23_r_*SPP(10); -nextP(6,23) = OP_l_6_c_23_r_ + OP_l_1_c_23_r_*SF(5) + OP_l_3_c_23_r_*SF(4) + OP_l_4_c_23_r_*SF(6) - OP_l_2_c_23_r_*SPP(1) - OP_l_14_c_23_r_*SPP(9) + OP_l_15_c_23_r_*SPP(3) + OP_l_16_c_23_r_*SPP(6); -nextP(7,23) = OP_l_7_c_23_r_ + OP_l_2_c_23_r_*SF(5) - OP_l_3_c_23_r_*SF(6) + OP_l_4_c_23_r_*SF(4) + OP_l_1_c_23_r_*SPP(1) + OP_l_14_c_23_r_*SPP(5) - OP_l_15_c_23_r_*SPP(8) - OP_l_16_c_23_r_*SPP(2); -nextP(8,23) = OP_l_8_c_23_r_ + OP_l_5_c_23_r_*dt; -nextP(9,23) = OP_l_9_c_23_r_ + OP_l_6_c_23_r_*dt; -nextP(10,23) = OP_l_10_c_23_r_ + OP_l_7_c_23_r_*dt; -nextP(11,23) = OP_l_11_c_23_r_; -nextP(12,23) = OP_l_12_c_23_r_; -nextP(13,23) = OP_l_13_c_23_r_; -nextP(14,23) = OP_l_14_c_23_r_; -nextP(15,23) = OP_l_15_c_23_r_; -nextP(16,23) = OP_l_16_c_23_r_; -nextP(17,23) = OP_l_17_c_23_r_; -nextP(18,23) = OP_l_18_c_23_r_; -nextP(19,23) = OP_l_19_c_23_r_; -nextP(20,23) = OP_l_20_c_23_r_; -nextP(21,23) = OP_l_21_c_23_r_; -nextP(22,23) = OP_l_22_c_23_r_; -nextP(23,23) = OP_l_23_c_23_r_; -nextP(1,24) = OP_l_1_c_24_r_ + OP_l_2_c_24_r_*SF(10) + OP_l_3_c_24_r_*SF(12) + OP_l_4_c_24_r_*SF(11) + OP_l_11_c_24_r_*SF(15) + OP_l_12_c_24_r_*SF(16) + OP_l_13_c_24_r_*SPP(11); -nextP(2,24) = OP_l_2_c_24_r_ + OP_l_1_c_24_r_*SF(9) + OP_l_3_c_24_r_*SF(8) + OP_l_4_c_24_r_*SF(12) - OP_l_13_c_24_r_*SF(16) + OP_l_12_c_24_r_*SPP(11) - (OP_l_11_c_24_r_*q0)/2; -nextP(3,24) = OP_l_3_c_24_r_ + OP_l_1_c_24_r_*SF(7) + OP_l_2_c_24_r_*SF(11) + OP_l_4_c_24_r_*SF(9) + OP_l_13_c_24_r_*SF(15) - OP_l_11_c_24_r_*SPP(11) - (OP_l_12_c_24_r_*q0)/2; -nextP(4,24) = OP_l_4_c_24_r_ + OP_l_1_c_24_r_*SF(8) + OP_l_2_c_24_r_*SF(7) + OP_l_3_c_24_r_*SF(10) + OP_l_11_c_24_r_*SF(16) - OP_l_12_c_24_r_*SF(15) - (OP_l_13_c_24_r_*q0)/2; -nextP(5,24) = OP_l_5_c_24_r_ + OP_l_1_c_24_r_*SF(6) + OP_l_2_c_24_r_*SF(4) - OP_l_4_c_24_r_*SF(5) + OP_l_3_c_24_r_*SPP(1) + OP_l_14_c_24_r_*SPP(4) + OP_l_15_c_24_r_*SPP(7) - OP_l_16_c_24_r_*SPP(10); -nextP(6,24) = OP_l_6_c_24_r_ + OP_l_1_c_24_r_*SF(5) + OP_l_3_c_24_r_*SF(4) + OP_l_4_c_24_r_*SF(6) - OP_l_2_c_24_r_*SPP(1) - OP_l_14_c_24_r_*SPP(9) + OP_l_15_c_24_r_*SPP(3) + OP_l_16_c_24_r_*SPP(6); -nextP(7,24) = OP_l_7_c_24_r_ + OP_l_2_c_24_r_*SF(5) - OP_l_3_c_24_r_*SF(6) + OP_l_4_c_24_r_*SF(4) + OP_l_1_c_24_r_*SPP(1) + OP_l_14_c_24_r_*SPP(5) - OP_l_15_c_24_r_*SPP(8) - OP_l_16_c_24_r_*SPP(2); -nextP(8,24) = OP_l_8_c_24_r_ + OP_l_5_c_24_r_*dt; -nextP(9,24) = OP_l_9_c_24_r_ + OP_l_6_c_24_r_*dt; -nextP(10,24) = OP_l_10_c_24_r_ + OP_l_7_c_24_r_*dt; -nextP(11,24) = OP_l_11_c_24_r_; -nextP(12,24) = OP_l_12_c_24_r_; -nextP(13,24) = OP_l_13_c_24_r_; -nextP(14,24) = OP_l_14_c_24_r_; -nextP(15,24) = OP_l_15_c_24_r_; -nextP(16,24) = OP_l_16_c_24_r_; -nextP(17,24) = OP_l_17_c_24_r_; -nextP(18,24) = OP_l_18_c_24_r_; -nextP(19,24) = OP_l_19_c_24_r_; -nextP(20,24) = OP_l_20_c_24_r_; -nextP(21,24) = OP_l_21_c_24_r_; -nextP(22,24) = OP_l_22_c_24_r_; -nextP(23,24) = OP_l_23_c_24_r_; -nextP(24,24) = OP_l_24_c_24_r_; - - -SH_TAS = zeros(3,1); -SH_TAS(1) = 1/((ve - vwe)^2 + (vn - vwn)^2 + vd^2)^(1/2); -SH_TAS(2) = (SH_TAS(1)*(2*ve - 2*vwe))/2; -SH_TAS(3) = (SH_TAS(1)*(2*vn - 2*vwn))/2; - -H_TAS = zeros(1,24); -H_TAS(1,5) = SH_TAS(3); -H_TAS(1,6) = SH_TAS(2); -H_TAS(1,7) = vd*SH_TAS(1); -H_TAS(1,23) = -SH_TAS(3); -H_TAS(1,24) = -SH_TAS(2); - - -SK_TAS = zeros(2,1); -SK_TAS(1) = 1/(R_TAS + SH_TAS(3)*(OP_l_5_c_5_r_*SH_TAS(3) + OP_l_6_c_5_r_*SH_TAS(2) - OP_l_23_c_5_r_*SH_TAS(3) - OP_l_24_c_5_r_*SH_TAS(2) + OP_l_7_c_5_r_*vd*SH_TAS(1)) + SH_TAS(2)*(OP_l_5_c_6_r_*SH_TAS(3) + OP_l_6_c_6_r_*SH_TAS(2) - OP_l_23_c_6_r_*SH_TAS(3) - OP_l_24_c_6_r_*SH_TAS(2) + OP_l_7_c_6_r_*vd*SH_TAS(1)) - SH_TAS(3)*(OP_l_5_c_23_r_*SH_TAS(3) + OP_l_6_c_23_r_*SH_TAS(2) - OP_l_23_c_23_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(2) + OP_l_7_c_23_r_*vd*SH_TAS(1)) - SH_TAS(2)*(OP_l_5_c_24_r_*SH_TAS(3) + OP_l_6_c_24_r_*SH_TAS(2) - OP_l_23_c_24_r_*SH_TAS(3) - OP_l_24_c_24_r_*SH_TAS(2) + OP_l_7_c_24_r_*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP_l_5_c_7_r_*SH_TAS(3) + OP_l_6_c_7_r_*SH_TAS(2) - OP_l_23_c_7_r_*SH_TAS(3) - OP_l_24_c_7_r_*SH_TAS(2) + OP_l_7_c_7_r_*vd*SH_TAS(1))); -SK_TAS(2) = SH_TAS(2); - - -Kfusion = zeros(24,1); -Kfusion = zeros(1,1); -Kfusion(1) = SK_TAS(1)*(OP_l_1_c_5_r_*SH_TAS(3) - OP_l_1_c_23_r_*SH_TAS(3) + OP_l_1_c_6_r_*SK_TAS(2) - OP_l_1_c_24_r_*SK_TAS(2) + OP_l_1_c_7_r_*vd*SH_TAS(1)); -Kfusion(2) = SK_TAS(1)*(OP_l_2_c_5_r_*SH_TAS(3) - OP_l_2_c_23_r_*SH_TAS(3) + OP_l_2_c_6_r_*SK_TAS(2) - OP_l_2_c_24_r_*SK_TAS(2) + OP_l_2_c_7_r_*vd*SH_TAS(1)); -Kfusion(3) = SK_TAS(1)*(OP_l_3_c_5_r_*SH_TAS(3) - OP_l_3_c_23_r_*SH_TAS(3) + OP_l_3_c_6_r_*SK_TAS(2) - OP_l_3_c_24_r_*SK_TAS(2) + OP_l_3_c_7_r_*vd*SH_TAS(1)); -Kfusion(4) = SK_TAS(1)*(OP_l_4_c_5_r_*SH_TAS(3) - OP_l_4_c_23_r_*SH_TAS(3) + OP_l_4_c_6_r_*SK_TAS(2) - OP_l_4_c_24_r_*SK_TAS(2) + OP_l_4_c_7_r_*vd*SH_TAS(1)); -Kfusion(5) = SK_TAS(1)*(OP_l_5_c_5_r_*SH_TAS(3) - OP_l_5_c_23_r_*SH_TAS(3) + OP_l_5_c_6_r_*SK_TAS(2) - OP_l_5_c_24_r_*SK_TAS(2) + OP_l_5_c_7_r_*vd*SH_TAS(1)); -Kfusion(6) = SK_TAS(1)*(OP_l_6_c_5_r_*SH_TAS(3) - OP_l_6_c_23_r_*SH_TAS(3) + OP_l_6_c_6_r_*SK_TAS(2) - OP_l_6_c_24_r_*SK_TAS(2) + OP_l_6_c_7_r_*vd*SH_TAS(1)); -Kfusion(7) = SK_TAS(1)*(OP_l_7_c_5_r_*SH_TAS(3) - OP_l_7_c_23_r_*SH_TAS(3) + OP_l_7_c_6_r_*SK_TAS(2) - OP_l_7_c_24_r_*SK_TAS(2) + OP_l_7_c_7_r_*vd*SH_TAS(1)); -Kfusion(8) = SK_TAS(1)*(OP_l_8_c_5_r_*SH_TAS(3) - OP_l_8_c_23_r_*SH_TAS(3) + OP_l_8_c_6_r_*SK_TAS(2) - OP_l_8_c_24_r_*SK_TAS(2) + OP_l_8_c_7_r_*vd*SH_TAS(1)); -Kfusion(9) = SK_TAS(1)*(OP_l_9_c_5_r_*SH_TAS(3) - OP_l_9_c_23_r_*SH_TAS(3) + OP_l_9_c_6_r_*SK_TAS(2) - OP_l_9_c_24_r_*SK_TAS(2) + OP_l_9_c_7_r_*vd*SH_TAS(1)); -Kfusion(10) = SK_TAS(1)*(OP_l_10_c_5_r_*SH_TAS(3) - OP_l_10_c_23_r_*SH_TAS(3) + OP_l_10_c_6_r_*SK_TAS(2) - OP_l_10_c_24_r_*SK_TAS(2) + OP_l_10_c_7_r_*vd*SH_TAS(1)); -Kfusion(11) = SK_TAS(1)*(OP_l_11_c_5_r_*SH_TAS(3) - OP_l_11_c_23_r_*SH_TAS(3) + OP_l_11_c_6_r_*SK_TAS(2) - OP_l_11_c_24_r_*SK_TAS(2) + OP_l_11_c_7_r_*vd*SH_TAS(1)); -Kfusion(12) = SK_TAS(1)*(OP_l_12_c_5_r_*SH_TAS(3) - OP_l_12_c_23_r_*SH_TAS(3) + OP_l_12_c_6_r_*SK_TAS(2) - OP_l_12_c_24_r_*SK_TAS(2) + OP_l_12_c_7_r_*vd*SH_TAS(1)); -Kfusion(13) = SK_TAS(1)*(OP_l_13_c_5_r_*SH_TAS(3) - OP_l_13_c_23_r_*SH_TAS(3) + OP_l_13_c_6_r_*SK_TAS(2) - OP_l_13_c_24_r_*SK_TAS(2) + OP_l_13_c_7_r_*vd*SH_TAS(1)); -Kfusion(14) = SK_TAS(1)*(OP_l_14_c_5_r_*SH_TAS(3) - OP_l_14_c_23_r_*SH_TAS(3) + OP_l_14_c_6_r_*SK_TAS(2) - OP_l_14_c_24_r_*SK_TAS(2) + OP_l_14_c_7_r_*vd*SH_TAS(1)); -Kfusion(15) = SK_TAS(1)*(OP_l_15_c_5_r_*SH_TAS(3) - OP_l_15_c_23_r_*SH_TAS(3) + OP_l_15_c_6_r_*SK_TAS(2) - OP_l_15_c_24_r_*SK_TAS(2) + OP_l_15_c_7_r_*vd*SH_TAS(1)); -Kfusion(16) = SK_TAS(1)*(OP_l_16_c_5_r_*SH_TAS(3) - OP_l_16_c_23_r_*SH_TAS(3) + OP_l_16_c_6_r_*SK_TAS(2) - OP_l_16_c_24_r_*SK_TAS(2) + OP_l_16_c_7_r_*vd*SH_TAS(1)); -Kfusion(17) = SK_TAS(1)*(OP_l_17_c_5_r_*SH_TAS(3) - OP_l_17_c_23_r_*SH_TAS(3) + OP_l_17_c_6_r_*SK_TAS(2) - OP_l_17_c_24_r_*SK_TAS(2) + OP_l_17_c_7_r_*vd*SH_TAS(1)); -Kfusion(18) = SK_TAS(1)*(OP_l_18_c_5_r_*SH_TAS(3) - OP_l_18_c_23_r_*SH_TAS(3) + OP_l_18_c_6_r_*SK_TAS(2) - OP_l_18_c_24_r_*SK_TAS(2) + OP_l_18_c_7_r_*vd*SH_TAS(1)); -Kfusion(19) = SK_TAS(1)*(OP_l_19_c_5_r_*SH_TAS(3) - OP_l_19_c_23_r_*SH_TAS(3) + OP_l_19_c_6_r_*SK_TAS(2) - OP_l_19_c_24_r_*SK_TAS(2) + OP_l_19_c_7_r_*vd*SH_TAS(1)); -Kfusion(20) = SK_TAS(1)*(OP_l_20_c_5_r_*SH_TAS(3) - OP_l_20_c_23_r_*SH_TAS(3) + OP_l_20_c_6_r_*SK_TAS(2) - OP_l_20_c_24_r_*SK_TAS(2) + OP_l_20_c_7_r_*vd*SH_TAS(1)); -Kfusion(21) = SK_TAS(1)*(OP_l_21_c_5_r_*SH_TAS(3) - OP_l_21_c_23_r_*SH_TAS(3) + OP_l_21_c_6_r_*SK_TAS(2) - OP_l_21_c_24_r_*SK_TAS(2) + OP_l_21_c_7_r_*vd*SH_TAS(1)); -Kfusion(22) = SK_TAS(1)*(OP_l_22_c_5_r_*SH_TAS(3) - OP_l_22_c_23_r_*SH_TAS(3) + OP_l_22_c_6_r_*SK_TAS(2) - OP_l_22_c_24_r_*SK_TAS(2) + OP_l_22_c_7_r_*vd*SH_TAS(1)); -Kfusion(23) = SK_TAS(1)*(OP_l_23_c_5_r_*SH_TAS(3) - OP_l_23_c_23_r_*SH_TAS(3) + OP_l_23_c_6_r_*SK_TAS(2) - OP_l_23_c_24_r_*SK_TAS(2) + OP_l_23_c_7_r_*vd*SH_TAS(1)); -Kfusion(24) = SK_TAS(1)*(OP_l_24_c_5_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(3) + OP_l_24_c_6_r_*SK_TAS(2) - OP_l_24_c_24_r_*SK_TAS(2) + OP_l_24_c_7_r_*vd*SH_TAS(1)); - - -SH_BETA = zeros(13,1); -SH_BETA(1) = (vn - vwn)*(q0^2 + q1^2 - q2^2 - q3^2) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); -SH_BETA(2) = (ve - vwe)*(q0^2 - q1^2 + q2^2 - q3^2) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2); -SH_BETA(3) = vn - vwn; -SH_BETA(4) = ve - vwe; -SH_BETA(5) = 1/SH_BETA(1)^2; -SH_BETA(6) = 1/SH_BETA(1); -SH_BETA(7) = SH_BETA(6)*(q0^2 - q1^2 + q2^2 - q3^2); -SH_BETA(8) = q0^2 + q1^2 - q2^2 - q3^2; -SH_BETA(9) = 2*q0*SH_BETA(4) - 2*q3*SH_BETA(3) + 2*q1*vd; -SH_BETA(10) = 2*q0*SH_BETA(3) + 2*q3*SH_BETA(4) - 2*q2*vd; -SH_BETA(11) = 2*q2*SH_BETA(3) - 2*q1*SH_BETA(4) + 2*q0*vd; -SH_BETA(12) = 2*q1*SH_BETA(3) + 2*q2*SH_BETA(4) + 2*q3*vd; -SH_BETA(13) = 2*q0*q3; - -H_BETA = zeros(1,24); -H_BETA(1,1) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10); -H_BETA(1,2) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12); -H_BETA(1,3) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11); -H_BETA(1,4) = - SH_BETA(6)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -H_BETA(1,5) = - SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) - SH_BETA(2)*SH_BETA(5)*SH_BETA(8); -H_BETA(1,6) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2); -H_BETA(1,7) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3); -H_BETA(1,23) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8); -H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2) - SH_BETA(7); - - -SK_BETA = zeros(8,1); -SK_BETA(1) = 1/(R_BETA - (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP_l_23_c_5_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_5_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_5_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_5_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_5_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_5_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_5_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_5_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_5_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP_l_23_c_23_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_23_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_23_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_23_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_23_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_23_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_23_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_23_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_23_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP_l_23_c_6_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_6_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_6_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_6_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_6_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_6_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_6_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_6_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_6_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP_l_23_c_24_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_24_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_24_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_24_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_24_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_24_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_24_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_24_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_24_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10))*(OP_l_23_c_1_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_1_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_1_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_1_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_1_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_1_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_1_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_1_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_1_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12))*(OP_l_23_c_2_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_2_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_2_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_2_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_2_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_2_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_2_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_2_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_2_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11))*(OP_l_23_c_3_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_3_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_3_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_3_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_3_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_3_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_3_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_3_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_3_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP_l_23_c_4_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_4_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_4_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_4_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_4_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_4_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_4_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_4_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_4_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))*(OP_l_23_c_7_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_7_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_7_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_7_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_7_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_7_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_7_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_7_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_7_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3)))); -SK_BETA(2) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8); -SK_BETA(3) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2); -SK_BETA(4) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3); -SK_BETA(5) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12); -SK_BETA(6) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10); -SK_BETA(7) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11); -SK_BETA(8) = SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9); - - -Kfusion = zeros(24,1); -Kfusion = zeros(1,1); -Kfusion(1) = SK_BETA(1)*(OP_l_1_c_1_r_*SK_BETA(6) + OP_l_1_c_2_r_*SK_BETA(5) - OP_l_1_c_5_r_*SK_BETA(2) + OP_l_1_c_6_r_*SK_BETA(3) + OP_l_1_c_3_r_*SK_BETA(7) + OP_l_1_c_7_r_*SK_BETA(4) - OP_l_1_c_4_r_*SK_BETA(8) + OP_l_1_c_23_r_*SK_BETA(2) - OP_l_1_c_24_r_*SK_BETA(3)); -Kfusion(2) = SK_BETA(1)*(OP_l_2_c_1_r_*SK_BETA(6) + OP_l_2_c_2_r_*SK_BETA(5) - OP_l_2_c_5_r_*SK_BETA(2) + OP_l_2_c_6_r_*SK_BETA(3) + OP_l_2_c_3_r_*SK_BETA(7) + OP_l_2_c_7_r_*SK_BETA(4) - OP_l_2_c_4_r_*SK_BETA(8) + OP_l_2_c_23_r_*SK_BETA(2) - OP_l_2_c_24_r_*SK_BETA(3)); -Kfusion(3) = SK_BETA(1)*(OP_l_3_c_1_r_*SK_BETA(6) + OP_l_3_c_2_r_*SK_BETA(5) - OP_l_3_c_5_r_*SK_BETA(2) + OP_l_3_c_6_r_*SK_BETA(3) + OP_l_3_c_3_r_*SK_BETA(7) + OP_l_3_c_7_r_*SK_BETA(4) - OP_l_3_c_4_r_*SK_BETA(8) + OP_l_3_c_23_r_*SK_BETA(2) - OP_l_3_c_24_r_*SK_BETA(3)); -Kfusion(4) = SK_BETA(1)*(OP_l_4_c_1_r_*SK_BETA(6) + OP_l_4_c_2_r_*SK_BETA(5) - OP_l_4_c_5_r_*SK_BETA(2) + OP_l_4_c_6_r_*SK_BETA(3) + OP_l_4_c_3_r_*SK_BETA(7) + OP_l_4_c_7_r_*SK_BETA(4) - OP_l_4_c_4_r_*SK_BETA(8) + OP_l_4_c_23_r_*SK_BETA(2) - OP_l_4_c_24_r_*SK_BETA(3)); -Kfusion(5) = SK_BETA(1)*(OP_l_5_c_1_r_*SK_BETA(6) + OP_l_5_c_2_r_*SK_BETA(5) - OP_l_5_c_5_r_*SK_BETA(2) + OP_l_5_c_6_r_*SK_BETA(3) + OP_l_5_c_3_r_*SK_BETA(7) + OP_l_5_c_7_r_*SK_BETA(4) - OP_l_5_c_4_r_*SK_BETA(8) + OP_l_5_c_23_r_*SK_BETA(2) - OP_l_5_c_24_r_*SK_BETA(3)); -Kfusion(6) = SK_BETA(1)*(OP_l_6_c_1_r_*SK_BETA(6) + OP_l_6_c_2_r_*SK_BETA(5) - OP_l_6_c_5_r_*SK_BETA(2) + OP_l_6_c_6_r_*SK_BETA(3) + OP_l_6_c_3_r_*SK_BETA(7) + OP_l_6_c_7_r_*SK_BETA(4) - OP_l_6_c_4_r_*SK_BETA(8) + OP_l_6_c_23_r_*SK_BETA(2) - OP_l_6_c_24_r_*SK_BETA(3)); -Kfusion(7) = SK_BETA(1)*(OP_l_7_c_1_r_*SK_BETA(6) + OP_l_7_c_2_r_*SK_BETA(5) - OP_l_7_c_5_r_*SK_BETA(2) + OP_l_7_c_6_r_*SK_BETA(3) + OP_l_7_c_3_r_*SK_BETA(7) + OP_l_7_c_7_r_*SK_BETA(4) - OP_l_7_c_4_r_*SK_BETA(8) + OP_l_7_c_23_r_*SK_BETA(2) - OP_l_7_c_24_r_*SK_BETA(3)); -Kfusion(8) = SK_BETA(1)*(OP_l_8_c_1_r_*SK_BETA(6) + OP_l_8_c_2_r_*SK_BETA(5) - OP_l_8_c_5_r_*SK_BETA(2) + OP_l_8_c_6_r_*SK_BETA(3) + OP_l_8_c_3_r_*SK_BETA(7) + OP_l_8_c_7_r_*SK_BETA(4) - OP_l_8_c_4_r_*SK_BETA(8) + OP_l_8_c_23_r_*SK_BETA(2) - OP_l_8_c_24_r_*SK_BETA(3)); -Kfusion(9) = SK_BETA(1)*(OP_l_9_c_1_r_*SK_BETA(6) + OP_l_9_c_2_r_*SK_BETA(5) - OP_l_9_c_5_r_*SK_BETA(2) + OP_l_9_c_6_r_*SK_BETA(3) + OP_l_9_c_3_r_*SK_BETA(7) + OP_l_9_c_7_r_*SK_BETA(4) - OP_l_9_c_4_r_*SK_BETA(8) + OP_l_9_c_23_r_*SK_BETA(2) - OP_l_9_c_24_r_*SK_BETA(3)); -Kfusion(10) = SK_BETA(1)*(OP_l_10_c_1_r_*SK_BETA(6) + OP_l_10_c_2_r_*SK_BETA(5) - OP_l_10_c_5_r_*SK_BETA(2) + OP_l_10_c_6_r_*SK_BETA(3) + OP_l_10_c_3_r_*SK_BETA(7) + OP_l_10_c_7_r_*SK_BETA(4) - OP_l_10_c_4_r_*SK_BETA(8) + OP_l_10_c_23_r_*SK_BETA(2) - OP_l_10_c_24_r_*SK_BETA(3)); -Kfusion(11) = SK_BETA(1)*(OP_l_11_c_1_r_*SK_BETA(6) + OP_l_11_c_2_r_*SK_BETA(5) - OP_l_11_c_5_r_*SK_BETA(2) + OP_l_11_c_6_r_*SK_BETA(3) + OP_l_11_c_3_r_*SK_BETA(7) + OP_l_11_c_7_r_*SK_BETA(4) - OP_l_11_c_4_r_*SK_BETA(8) + OP_l_11_c_23_r_*SK_BETA(2) - OP_l_11_c_24_r_*SK_BETA(3)); -Kfusion(12) = SK_BETA(1)*(OP_l_12_c_1_r_*SK_BETA(6) + OP_l_12_c_2_r_*SK_BETA(5) - OP_l_12_c_5_r_*SK_BETA(2) + OP_l_12_c_6_r_*SK_BETA(3) + OP_l_12_c_3_r_*SK_BETA(7) + OP_l_12_c_7_r_*SK_BETA(4) - OP_l_12_c_4_r_*SK_BETA(8) + OP_l_12_c_23_r_*SK_BETA(2) - OP_l_12_c_24_r_*SK_BETA(3)); -Kfusion(13) = SK_BETA(1)*(OP_l_13_c_1_r_*SK_BETA(6) + OP_l_13_c_2_r_*SK_BETA(5) - OP_l_13_c_5_r_*SK_BETA(2) + OP_l_13_c_6_r_*SK_BETA(3) + OP_l_13_c_3_r_*SK_BETA(7) + OP_l_13_c_7_r_*SK_BETA(4) - OP_l_13_c_4_r_*SK_BETA(8) + OP_l_13_c_23_r_*SK_BETA(2) - OP_l_13_c_24_r_*SK_BETA(3)); -Kfusion(14) = SK_BETA(1)*(OP_l_14_c_1_r_*SK_BETA(6) + OP_l_14_c_2_r_*SK_BETA(5) - OP_l_14_c_5_r_*SK_BETA(2) + OP_l_14_c_6_r_*SK_BETA(3) + OP_l_14_c_3_r_*SK_BETA(7) + OP_l_14_c_7_r_*SK_BETA(4) - OP_l_14_c_4_r_*SK_BETA(8) + OP_l_14_c_23_r_*SK_BETA(2) - OP_l_14_c_24_r_*SK_BETA(3)); -Kfusion(15) = SK_BETA(1)*(OP_l_15_c_1_r_*SK_BETA(6) + OP_l_15_c_2_r_*SK_BETA(5) - OP_l_15_c_5_r_*SK_BETA(2) + OP_l_15_c_6_r_*SK_BETA(3) + OP_l_15_c_3_r_*SK_BETA(7) + OP_l_15_c_7_r_*SK_BETA(4) - OP_l_15_c_4_r_*SK_BETA(8) + OP_l_15_c_23_r_*SK_BETA(2) - OP_l_15_c_24_r_*SK_BETA(3)); -Kfusion(16) = SK_BETA(1)*(OP_l_16_c_1_r_*SK_BETA(6) + OP_l_16_c_2_r_*SK_BETA(5) - OP_l_16_c_5_r_*SK_BETA(2) + OP_l_16_c_6_r_*SK_BETA(3) + OP_l_16_c_3_r_*SK_BETA(7) + OP_l_16_c_7_r_*SK_BETA(4) - OP_l_16_c_4_r_*SK_BETA(8) + OP_l_16_c_23_r_*SK_BETA(2) - OP_l_16_c_24_r_*SK_BETA(3)); -Kfusion(17) = SK_BETA(1)*(OP_l_17_c_1_r_*SK_BETA(6) + OP_l_17_c_2_r_*SK_BETA(5) - OP_l_17_c_5_r_*SK_BETA(2) + OP_l_17_c_6_r_*SK_BETA(3) + OP_l_17_c_3_r_*SK_BETA(7) + OP_l_17_c_7_r_*SK_BETA(4) - OP_l_17_c_4_r_*SK_BETA(8) + OP_l_17_c_23_r_*SK_BETA(2) - OP_l_17_c_24_r_*SK_BETA(3)); -Kfusion(18) = SK_BETA(1)*(OP_l_18_c_1_r_*SK_BETA(6) + OP_l_18_c_2_r_*SK_BETA(5) - OP_l_18_c_5_r_*SK_BETA(2) + OP_l_18_c_6_r_*SK_BETA(3) + OP_l_18_c_3_r_*SK_BETA(7) + OP_l_18_c_7_r_*SK_BETA(4) - OP_l_18_c_4_r_*SK_BETA(8) + OP_l_18_c_23_r_*SK_BETA(2) - OP_l_18_c_24_r_*SK_BETA(3)); -Kfusion(19) = SK_BETA(1)*(OP_l_19_c_1_r_*SK_BETA(6) + OP_l_19_c_2_r_*SK_BETA(5) - OP_l_19_c_5_r_*SK_BETA(2) + OP_l_19_c_6_r_*SK_BETA(3) + OP_l_19_c_3_r_*SK_BETA(7) + OP_l_19_c_7_r_*SK_BETA(4) - OP_l_19_c_4_r_*SK_BETA(8) + OP_l_19_c_23_r_*SK_BETA(2) - OP_l_19_c_24_r_*SK_BETA(3)); -Kfusion(20) = SK_BETA(1)*(OP_l_20_c_1_r_*SK_BETA(6) + OP_l_20_c_2_r_*SK_BETA(5) - OP_l_20_c_5_r_*SK_BETA(2) + OP_l_20_c_6_r_*SK_BETA(3) + OP_l_20_c_3_r_*SK_BETA(7) + OP_l_20_c_7_r_*SK_BETA(4) - OP_l_20_c_4_r_*SK_BETA(8) + OP_l_20_c_23_r_*SK_BETA(2) - OP_l_20_c_24_r_*SK_BETA(3)); -Kfusion(21) = SK_BETA(1)*(OP_l_21_c_1_r_*SK_BETA(6) + OP_l_21_c_2_r_*SK_BETA(5) - OP_l_21_c_5_r_*SK_BETA(2) + OP_l_21_c_6_r_*SK_BETA(3) + OP_l_21_c_3_r_*SK_BETA(7) + OP_l_21_c_7_r_*SK_BETA(4) - OP_l_21_c_4_r_*SK_BETA(8) + OP_l_21_c_23_r_*SK_BETA(2) - OP_l_21_c_24_r_*SK_BETA(3)); -Kfusion(22) = SK_BETA(1)*(OP_l_22_c_1_r_*SK_BETA(6) + OP_l_22_c_2_r_*SK_BETA(5) - OP_l_22_c_5_r_*SK_BETA(2) + OP_l_22_c_6_r_*SK_BETA(3) + OP_l_22_c_3_r_*SK_BETA(7) + OP_l_22_c_7_r_*SK_BETA(4) - OP_l_22_c_4_r_*SK_BETA(8) + OP_l_22_c_23_r_*SK_BETA(2) - OP_l_22_c_24_r_*SK_BETA(3)); -Kfusion(23) = SK_BETA(1)*(OP_l_23_c_1_r_*SK_BETA(6) + OP_l_23_c_2_r_*SK_BETA(5) - OP_l_23_c_5_r_*SK_BETA(2) + OP_l_23_c_6_r_*SK_BETA(3) + OP_l_23_c_3_r_*SK_BETA(7) + OP_l_23_c_7_r_*SK_BETA(4) - OP_l_23_c_4_r_*SK_BETA(8) + OP_l_23_c_23_r_*SK_BETA(2) - OP_l_23_c_24_r_*SK_BETA(3)); -Kfusion(24) = SK_BETA(1)*(OP_l_24_c_1_r_*SK_BETA(6) + OP_l_24_c_2_r_*SK_BETA(5) - OP_l_24_c_5_r_*SK_BETA(2) + OP_l_24_c_6_r_*SK_BETA(3) + OP_l_24_c_3_r_*SK_BETA(7) + OP_l_24_c_7_r_*SK_BETA(4) - OP_l_24_c_4_r_*SK_BETA(8) + OP_l_24_c_23_r_*SK_BETA(2) - OP_l_24_c_24_r_*SK_BETA(3)); - - -SH_MAG = zeros(9,1); -SH_MAG(1) = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; -SH_MAG(2) = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; -SH_MAG(3) = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; -SH_MAG(4) = q3^2; -SH_MAG(5) = q2^2; -SH_MAG(6) = q1^2; -SH_MAG(7) = q0^2; -SH_MAG(8) = 2*magN*q0; -SH_MAG(9) = 2*magE*q3; - - -H_MAG = zeros(1,24); -H_MAG(1) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; -H_MAG(2) = SH_MAG(1); -H_MAG(3) = -SH_MAG(2); -H_MAG(4) = SH_MAG(3); -H_MAG(17) = SH_MAG(6) - SH_MAG(5) - SH_MAG(4) + SH_MAG(7); -H_MAG(18) = 2*q0*q3 + 2*q1*q2; -H_MAG(19) = 2*q1*q3 - 2*q0*q2; -H_MAG(20) = 1; - - -SK_MX = zeros(5,1); -SK_MX(1) = 1/(OP_l_20_c_20_r_ + R_MAG + OP_l_2_c_20_r_*SH_MAG(1) - OP_l_3_c_20_r_*SH_MAG(2) + OP_l_4_c_20_r_*SH_MAG(3) - OP_l_17_c_20_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + (2*q0*q3 + 2*q1*q2)*(OP_l_20_c_18_r_ + OP_l_2_c_18_r_*SH_MAG(1) - OP_l_3_c_18_r_*SH_MAG(2) + OP_l_4_c_18_r_*SH_MAG(3) - OP_l_17_c_18_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_18_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_18_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(OP_l_20_c_19_r_ + OP_l_2_c_19_r_*SH_MAG(1) - OP_l_3_c_19_r_*SH_MAG(2) + OP_l_4_c_19_r_*SH_MAG(3) - OP_l_17_c_19_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_19_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_19_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_20_c_1_r_ + OP_l_2_c_1_r_*SH_MAG(1) - OP_l_3_c_1_r_*SH_MAG(2) + OP_l_4_c_1_r_*SH_MAG(3) - OP_l_17_c_1_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_1_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_1_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_18_c_20_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_20_r_*(2*q0*q2 - 2*q1*q3) + SH_MAG(1)*(OP_l_20_c_2_r_ + OP_l_2_c_2_r_*SH_MAG(1) - OP_l_3_c_2_r_*SH_MAG(2) + OP_l_4_c_2_r_*SH_MAG(3) - OP_l_17_c_2_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_2_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_2_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(2)*(OP_l_20_c_3_r_ + OP_l_2_c_3_r_*SH_MAG(1) - OP_l_3_c_3_r_*SH_MAG(2) + OP_l_4_c_3_r_*SH_MAG(3) - OP_l_17_c_3_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_3_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_3_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(3)*(OP_l_20_c_4_r_ + OP_l_2_c_4_r_*SH_MAG(1) - OP_l_3_c_4_r_*SH_MAG(2) + OP_l_4_c_4_r_*SH_MAG(3) - OP_l_17_c_4_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_4_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_4_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7))*(OP_l_20_c_17_r_ + OP_l_2_c_17_r_*SH_MAG(1) - OP_l_3_c_17_r_*SH_MAG(2) + OP_l_4_c_17_r_*SH_MAG(3) - OP_l_17_c_17_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_17_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_17_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_1_c_20_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); -SK_MX(2) = SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7); -SK_MX(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; -SK_MX(4) = 2*q0*q2 - 2*q1*q3; -SK_MX(5) = 2*q0*q3 + 2*q1*q2; - - -Kfusion = zeros(24,1); -Kfusion = zeros(1,1); -Kfusion(1) = SK_MX(1)*(OP_l_1_c_20_r_ + OP_l_1_c_2_r_*SH_MAG(1) - OP_l_1_c_3_r_*SH_MAG(2) + OP_l_1_c_4_r_*SH_MAG(3) + OP_l_1_c_1_r_*SK_MX(3) - OP_l_1_c_17_r_*SK_MX(2) + OP_l_1_c_18_r_*SK_MX(5) - OP_l_1_c_19_r_*SK_MX(4)); -Kfusion(2) = SK_MX(1)*(OP_l_2_c_20_r_ + OP_l_2_c_2_r_*SH_MAG(1) - OP_l_2_c_3_r_*SH_MAG(2) + OP_l_2_c_4_r_*SH_MAG(3) + OP_l_2_c_1_r_*SK_MX(3) - OP_l_2_c_17_r_*SK_MX(2) + OP_l_2_c_18_r_*SK_MX(5) - OP_l_2_c_19_r_*SK_MX(4)); -Kfusion(3) = SK_MX(1)*(OP_l_3_c_20_r_ + OP_l_3_c_2_r_*SH_MAG(1) - OP_l_3_c_3_r_*SH_MAG(2) + OP_l_3_c_4_r_*SH_MAG(3) + OP_l_3_c_1_r_*SK_MX(3) - OP_l_3_c_17_r_*SK_MX(2) + OP_l_3_c_18_r_*SK_MX(5) - OP_l_3_c_19_r_*SK_MX(4)); -Kfusion(4) = SK_MX(1)*(OP_l_4_c_20_r_ + OP_l_4_c_2_r_*SH_MAG(1) - OP_l_4_c_3_r_*SH_MAG(2) + OP_l_4_c_4_r_*SH_MAG(3) + OP_l_4_c_1_r_*SK_MX(3) - OP_l_4_c_17_r_*SK_MX(2) + OP_l_4_c_18_r_*SK_MX(5) - OP_l_4_c_19_r_*SK_MX(4)); -Kfusion(5) = SK_MX(1)*(OP_l_5_c_20_r_ + OP_l_5_c_2_r_*SH_MAG(1) - OP_l_5_c_3_r_*SH_MAG(2) + OP_l_5_c_4_r_*SH_MAG(3) + OP_l_5_c_1_r_*SK_MX(3) - OP_l_5_c_17_r_*SK_MX(2) + OP_l_5_c_18_r_*SK_MX(5) - OP_l_5_c_19_r_*SK_MX(4)); -Kfusion(6) = SK_MX(1)*(OP_l_6_c_20_r_ + OP_l_6_c_2_r_*SH_MAG(1) - OP_l_6_c_3_r_*SH_MAG(2) + OP_l_6_c_4_r_*SH_MAG(3) + OP_l_6_c_1_r_*SK_MX(3) - OP_l_6_c_17_r_*SK_MX(2) + OP_l_6_c_18_r_*SK_MX(5) - OP_l_6_c_19_r_*SK_MX(4)); -Kfusion(7) = SK_MX(1)*(OP_l_7_c_20_r_ + OP_l_7_c_2_r_*SH_MAG(1) - OP_l_7_c_3_r_*SH_MAG(2) + OP_l_7_c_4_r_*SH_MAG(3) + OP_l_7_c_1_r_*SK_MX(3) - OP_l_7_c_17_r_*SK_MX(2) + OP_l_7_c_18_r_*SK_MX(5) - OP_l_7_c_19_r_*SK_MX(4)); -Kfusion(8) = SK_MX(1)*(OP_l_8_c_20_r_ + OP_l_8_c_2_r_*SH_MAG(1) - OP_l_8_c_3_r_*SH_MAG(2) + OP_l_8_c_4_r_*SH_MAG(3) + OP_l_8_c_1_r_*SK_MX(3) - OP_l_8_c_17_r_*SK_MX(2) + OP_l_8_c_18_r_*SK_MX(5) - OP_l_8_c_19_r_*SK_MX(4)); -Kfusion(9) = SK_MX(1)*(OP_l_9_c_20_r_ + OP_l_9_c_2_r_*SH_MAG(1) - OP_l_9_c_3_r_*SH_MAG(2) + OP_l_9_c_4_r_*SH_MAG(3) + OP_l_9_c_1_r_*SK_MX(3) - OP_l_9_c_17_r_*SK_MX(2) + OP_l_9_c_18_r_*SK_MX(5) - OP_l_9_c_19_r_*SK_MX(4)); -Kfusion(10) = SK_MX(1)*(OP_l_10_c_20_r_ + OP_l_10_c_2_r_*SH_MAG(1) - OP_l_10_c_3_r_*SH_MAG(2) + OP_l_10_c_4_r_*SH_MAG(3) + OP_l_10_c_1_r_*SK_MX(3) - OP_l_10_c_17_r_*SK_MX(2) + OP_l_10_c_18_r_*SK_MX(5) - OP_l_10_c_19_r_*SK_MX(4)); -Kfusion(11) = SK_MX(1)*(OP_l_11_c_20_r_ + OP_l_11_c_2_r_*SH_MAG(1) - OP_l_11_c_3_r_*SH_MAG(2) + OP_l_11_c_4_r_*SH_MAG(3) + OP_l_11_c_1_r_*SK_MX(3) - OP_l_11_c_17_r_*SK_MX(2) + OP_l_11_c_18_r_*SK_MX(5) - OP_l_11_c_19_r_*SK_MX(4)); -Kfusion(12) = SK_MX(1)*(OP_l_12_c_20_r_ + OP_l_12_c_2_r_*SH_MAG(1) - OP_l_12_c_3_r_*SH_MAG(2) + OP_l_12_c_4_r_*SH_MAG(3) + OP_l_12_c_1_r_*SK_MX(3) - OP_l_12_c_17_r_*SK_MX(2) + OP_l_12_c_18_r_*SK_MX(5) - OP_l_12_c_19_r_*SK_MX(4)); -Kfusion(13) = SK_MX(1)*(OP_l_13_c_20_r_ + OP_l_13_c_2_r_*SH_MAG(1) - OP_l_13_c_3_r_*SH_MAG(2) + OP_l_13_c_4_r_*SH_MAG(3) + OP_l_13_c_1_r_*SK_MX(3) - OP_l_13_c_17_r_*SK_MX(2) + OP_l_13_c_18_r_*SK_MX(5) - OP_l_13_c_19_r_*SK_MX(4)); -Kfusion(14) = SK_MX(1)*(OP_l_14_c_20_r_ + OP_l_14_c_2_r_*SH_MAG(1) - OP_l_14_c_3_r_*SH_MAG(2) + OP_l_14_c_4_r_*SH_MAG(3) + OP_l_14_c_1_r_*SK_MX(3) - OP_l_14_c_17_r_*SK_MX(2) + OP_l_14_c_18_r_*SK_MX(5) - OP_l_14_c_19_r_*SK_MX(4)); -Kfusion(15) = SK_MX(1)*(OP_l_15_c_20_r_ + OP_l_15_c_2_r_*SH_MAG(1) - OP_l_15_c_3_r_*SH_MAG(2) + OP_l_15_c_4_r_*SH_MAG(3) + OP_l_15_c_1_r_*SK_MX(3) - OP_l_15_c_17_r_*SK_MX(2) + OP_l_15_c_18_r_*SK_MX(5) - OP_l_15_c_19_r_*SK_MX(4)); -Kfusion(16) = SK_MX(1)*(OP_l_16_c_20_r_ + OP_l_16_c_2_r_*SH_MAG(1) - OP_l_16_c_3_r_*SH_MAG(2) + OP_l_16_c_4_r_*SH_MAG(3) + OP_l_16_c_1_r_*SK_MX(3) - OP_l_16_c_17_r_*SK_MX(2) + OP_l_16_c_18_r_*SK_MX(5) - OP_l_16_c_19_r_*SK_MX(4)); -Kfusion(17) = SK_MX(1)*(OP_l_17_c_20_r_ + OP_l_17_c_2_r_*SH_MAG(1) - OP_l_17_c_3_r_*SH_MAG(2) + OP_l_17_c_4_r_*SH_MAG(3) + OP_l_17_c_1_r_*SK_MX(3) - OP_l_17_c_17_r_*SK_MX(2) + OP_l_17_c_18_r_*SK_MX(5) - OP_l_17_c_19_r_*SK_MX(4)); -Kfusion(18) = SK_MX(1)*(OP_l_18_c_20_r_ + OP_l_18_c_2_r_*SH_MAG(1) - OP_l_18_c_3_r_*SH_MAG(2) + OP_l_18_c_4_r_*SH_MAG(3) + OP_l_18_c_1_r_*SK_MX(3) - OP_l_18_c_17_r_*SK_MX(2) + OP_l_18_c_18_r_*SK_MX(5) - OP_l_18_c_19_r_*SK_MX(4)); -Kfusion(19) = SK_MX(1)*(OP_l_19_c_20_r_ + OP_l_19_c_2_r_*SH_MAG(1) - OP_l_19_c_3_r_*SH_MAG(2) + OP_l_19_c_4_r_*SH_MAG(3) + OP_l_19_c_1_r_*SK_MX(3) - OP_l_19_c_17_r_*SK_MX(2) + OP_l_19_c_18_r_*SK_MX(5) - OP_l_19_c_19_r_*SK_MX(4)); -Kfusion(20) = SK_MX(1)*(OP_l_20_c_20_r_ + OP_l_20_c_2_r_*SH_MAG(1) - OP_l_20_c_3_r_*SH_MAG(2) + OP_l_20_c_4_r_*SH_MAG(3) + OP_l_20_c_1_r_*SK_MX(3) - OP_l_20_c_17_r_*SK_MX(2) + OP_l_20_c_18_r_*SK_MX(5) - OP_l_20_c_19_r_*SK_MX(4)); -Kfusion(21) = SK_MX(1)*(OP_l_21_c_20_r_ + OP_l_21_c_2_r_*SH_MAG(1) - OP_l_21_c_3_r_*SH_MAG(2) + OP_l_21_c_4_r_*SH_MAG(3) + OP_l_21_c_1_r_*SK_MX(3) - OP_l_21_c_17_r_*SK_MX(2) + OP_l_21_c_18_r_*SK_MX(5) - OP_l_21_c_19_r_*SK_MX(4)); -Kfusion(22) = SK_MX(1)*(OP_l_22_c_20_r_ + OP_l_22_c_2_r_*SH_MAG(1) - OP_l_22_c_3_r_*SH_MAG(2) + OP_l_22_c_4_r_*SH_MAG(3) + OP_l_22_c_1_r_*SK_MX(3) - OP_l_22_c_17_r_*SK_MX(2) + OP_l_22_c_18_r_*SK_MX(5) - OP_l_22_c_19_r_*SK_MX(4)); -Kfusion(23) = SK_MX(1)*(OP_l_23_c_20_r_ + OP_l_23_c_2_r_*SH_MAG(1) - OP_l_23_c_3_r_*SH_MAG(2) + OP_l_23_c_4_r_*SH_MAG(3) + OP_l_23_c_1_r_*SK_MX(3) - OP_l_23_c_17_r_*SK_MX(2) + OP_l_23_c_18_r_*SK_MX(5) - OP_l_23_c_19_r_*SK_MX(4)); -Kfusion(24) = SK_MX(1)*(OP_l_24_c_20_r_ + OP_l_24_c_2_r_*SH_MAG(1) - OP_l_24_c_3_r_*SH_MAG(2) + OP_l_24_c_4_r_*SH_MAG(3) + OP_l_24_c_1_r_*SK_MX(3) - OP_l_24_c_17_r_*SK_MX(2) + OP_l_24_c_18_r_*SK_MX(5) - OP_l_24_c_19_r_*SK_MX(4)); - - -H_MAG = zeros(1,24); -H_MAG(1) = SH_MAG(3); -H_MAG(2) = SH_MAG(2); -H_MAG(3) = SH_MAG(1); -H_MAG(4) = 2*magD*q2 - SH_MAG(9) - SH_MAG(8); -H_MAG(17) = 2*q1*q2 - 2*q0*q3; -H_MAG(18) = SH_MAG(5) - SH_MAG(4) - SH_MAG(6) + SH_MAG(7); -H_MAG(19) = 2*q0*q1 + 2*q2*q3; -H_MAG(21) = 1; - - -SK_MY = zeros(5,1); -SK_MY(1) = 1/(OP_l_21_c_21_r_ + R_MAG + OP_l_1_c_21_r_*SH_MAG(3) + OP_l_2_c_21_r_*SH_MAG(2) + OP_l_3_c_21_r_*SH_MAG(1) - OP_l_18_c_21_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - (2*q0*q3 - 2*q1*q2)*(OP_l_21_c_17_r_ + OP_l_1_c_17_r_*SH_MAG(3) + OP_l_2_c_17_r_*SH_MAG(2) + OP_l_3_c_17_r_*SH_MAG(1) - OP_l_18_c_17_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_17_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_17_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(OP_l_21_c_19_r_ + OP_l_1_c_19_r_*SH_MAG(3) + OP_l_2_c_19_r_*SH_MAG(2) + OP_l_3_c_19_r_*SH_MAG(1) - OP_l_18_c_19_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_19_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_19_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_21_c_4_r_ + OP_l_1_c_4_r_*SH_MAG(3) + OP_l_2_c_4_r_*SH_MAG(2) + OP_l_3_c_4_r_*SH_MAG(1) - OP_l_18_c_4_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_4_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_4_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP_l_17_c_21_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_21_r_*(2*q0*q1 + 2*q2*q3) + SH_MAG(3)*(OP_l_21_c_1_r_ + OP_l_1_c_1_r_*SH_MAG(3) + OP_l_2_c_1_r_*SH_MAG(2) + OP_l_3_c_1_r_*SH_MAG(1) - OP_l_18_c_1_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_1_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_1_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(2)*(OP_l_21_c_2_r_ + OP_l_1_c_2_r_*SH_MAG(3) + OP_l_2_c_2_r_*SH_MAG(2) + OP_l_3_c_2_r_*SH_MAG(1) - OP_l_18_c_2_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_2_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_2_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP_l_21_c_3_r_ + OP_l_1_c_3_r_*SH_MAG(3) + OP_l_2_c_3_r_*SH_MAG(2) + OP_l_3_c_3_r_*SH_MAG(1) - OP_l_18_c_3_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_3_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_3_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7))*(OP_l_21_c_18_r_ + OP_l_1_c_18_r_*SH_MAG(3) + OP_l_2_c_18_r_*SH_MAG(2) + OP_l_3_c_18_r_*SH_MAG(1) - OP_l_18_c_18_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_18_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_18_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP_l_4_c_21_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); -SK_MY(2) = SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7); -SK_MY(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; -SK_MY(4) = 2*q0*q3 - 2*q1*q2; -SK_MY(5) = 2*q0*q1 + 2*q2*q3; - - -Kfusion = zeros(24,1); -Kfusion = zeros(1,1); -Kfusion(1) = SK_MY(1)*(OP_l_1_c_21_r_ + OP_l_1_c_1_r_*SH_MAG(3) + OP_l_1_c_2_r_*SH_MAG(2) + OP_l_1_c_3_r_*SH_MAG(1) - OP_l_1_c_4_r_*SK_MY(3) - OP_l_1_c_18_r_*SK_MY(2) - OP_l_1_c_17_r_*SK_MY(4) + OP_l_1_c_19_r_*SK_MY(5)); -Kfusion(2) = SK_MY(1)*(OP_l_2_c_21_r_ + OP_l_2_c_1_r_*SH_MAG(3) + OP_l_2_c_2_r_*SH_MAG(2) + OP_l_2_c_3_r_*SH_MAG(1) - OP_l_2_c_4_r_*SK_MY(3) - OP_l_2_c_18_r_*SK_MY(2) - OP_l_2_c_17_r_*SK_MY(4) + OP_l_2_c_19_r_*SK_MY(5)); -Kfusion(3) = SK_MY(1)*(OP_l_3_c_21_r_ + OP_l_3_c_1_r_*SH_MAG(3) + OP_l_3_c_2_r_*SH_MAG(2) + OP_l_3_c_3_r_*SH_MAG(1) - OP_l_3_c_4_r_*SK_MY(3) - OP_l_3_c_18_r_*SK_MY(2) - OP_l_3_c_17_r_*SK_MY(4) + OP_l_3_c_19_r_*SK_MY(5)); -Kfusion(4) = SK_MY(1)*(OP_l_4_c_21_r_ + OP_l_4_c_1_r_*SH_MAG(3) + OP_l_4_c_2_r_*SH_MAG(2) + OP_l_4_c_3_r_*SH_MAG(1) - OP_l_4_c_4_r_*SK_MY(3) - OP_l_4_c_18_r_*SK_MY(2) - OP_l_4_c_17_r_*SK_MY(4) + OP_l_4_c_19_r_*SK_MY(5)); -Kfusion(5) = SK_MY(1)*(OP_l_5_c_21_r_ + OP_l_5_c_1_r_*SH_MAG(3) + OP_l_5_c_2_r_*SH_MAG(2) + OP_l_5_c_3_r_*SH_MAG(1) - OP_l_5_c_4_r_*SK_MY(3) - OP_l_5_c_18_r_*SK_MY(2) - OP_l_5_c_17_r_*SK_MY(4) + OP_l_5_c_19_r_*SK_MY(5)); -Kfusion(6) = SK_MY(1)*(OP_l_6_c_21_r_ + OP_l_6_c_1_r_*SH_MAG(3) + OP_l_6_c_2_r_*SH_MAG(2) + OP_l_6_c_3_r_*SH_MAG(1) - OP_l_6_c_4_r_*SK_MY(3) - OP_l_6_c_18_r_*SK_MY(2) - OP_l_6_c_17_r_*SK_MY(4) + OP_l_6_c_19_r_*SK_MY(5)); -Kfusion(7) = SK_MY(1)*(OP_l_7_c_21_r_ + OP_l_7_c_1_r_*SH_MAG(3) + OP_l_7_c_2_r_*SH_MAG(2) + OP_l_7_c_3_r_*SH_MAG(1) - OP_l_7_c_4_r_*SK_MY(3) - OP_l_7_c_18_r_*SK_MY(2) - OP_l_7_c_17_r_*SK_MY(4) + OP_l_7_c_19_r_*SK_MY(5)); -Kfusion(8) = SK_MY(1)*(OP_l_8_c_21_r_ + OP_l_8_c_1_r_*SH_MAG(3) + OP_l_8_c_2_r_*SH_MAG(2) + OP_l_8_c_3_r_*SH_MAG(1) - OP_l_8_c_4_r_*SK_MY(3) - OP_l_8_c_18_r_*SK_MY(2) - OP_l_8_c_17_r_*SK_MY(4) + OP_l_8_c_19_r_*SK_MY(5)); -Kfusion(9) = SK_MY(1)*(OP_l_9_c_21_r_ + OP_l_9_c_1_r_*SH_MAG(3) + OP_l_9_c_2_r_*SH_MAG(2) + OP_l_9_c_3_r_*SH_MAG(1) - OP_l_9_c_4_r_*SK_MY(3) - OP_l_9_c_18_r_*SK_MY(2) - OP_l_9_c_17_r_*SK_MY(4) + OP_l_9_c_19_r_*SK_MY(5)); -Kfusion(10) = SK_MY(1)*(OP_l_10_c_21_r_ + OP_l_10_c_1_r_*SH_MAG(3) + OP_l_10_c_2_r_*SH_MAG(2) + OP_l_10_c_3_r_*SH_MAG(1) - OP_l_10_c_4_r_*SK_MY(3) - OP_l_10_c_18_r_*SK_MY(2) - OP_l_10_c_17_r_*SK_MY(4) + OP_l_10_c_19_r_*SK_MY(5)); -Kfusion(11) = SK_MY(1)*(OP_l_11_c_21_r_ + OP_l_11_c_1_r_*SH_MAG(3) + OP_l_11_c_2_r_*SH_MAG(2) + OP_l_11_c_3_r_*SH_MAG(1) - OP_l_11_c_4_r_*SK_MY(3) - OP_l_11_c_18_r_*SK_MY(2) - OP_l_11_c_17_r_*SK_MY(4) + OP_l_11_c_19_r_*SK_MY(5)); -Kfusion(12) = SK_MY(1)*(OP_l_12_c_21_r_ + OP_l_12_c_1_r_*SH_MAG(3) + OP_l_12_c_2_r_*SH_MAG(2) + OP_l_12_c_3_r_*SH_MAG(1) - OP_l_12_c_4_r_*SK_MY(3) - OP_l_12_c_18_r_*SK_MY(2) - OP_l_12_c_17_r_*SK_MY(4) + OP_l_12_c_19_r_*SK_MY(5)); -Kfusion(13) = SK_MY(1)*(OP_l_13_c_21_r_ + OP_l_13_c_1_r_*SH_MAG(3) + OP_l_13_c_2_r_*SH_MAG(2) + OP_l_13_c_3_r_*SH_MAG(1) - OP_l_13_c_4_r_*SK_MY(3) - OP_l_13_c_18_r_*SK_MY(2) - OP_l_13_c_17_r_*SK_MY(4) + OP_l_13_c_19_r_*SK_MY(5)); -Kfusion(14) = SK_MY(1)*(OP_l_14_c_21_r_ + OP_l_14_c_1_r_*SH_MAG(3) + OP_l_14_c_2_r_*SH_MAG(2) + OP_l_14_c_3_r_*SH_MAG(1) - OP_l_14_c_4_r_*SK_MY(3) - OP_l_14_c_18_r_*SK_MY(2) - OP_l_14_c_17_r_*SK_MY(4) + OP_l_14_c_19_r_*SK_MY(5)); -Kfusion(15) = SK_MY(1)*(OP_l_15_c_21_r_ + OP_l_15_c_1_r_*SH_MAG(3) + OP_l_15_c_2_r_*SH_MAG(2) + OP_l_15_c_3_r_*SH_MAG(1) - OP_l_15_c_4_r_*SK_MY(3) - OP_l_15_c_18_r_*SK_MY(2) - OP_l_15_c_17_r_*SK_MY(4) + OP_l_15_c_19_r_*SK_MY(5)); -Kfusion(16) = SK_MY(1)*(OP_l_16_c_21_r_ + OP_l_16_c_1_r_*SH_MAG(3) + OP_l_16_c_2_r_*SH_MAG(2) + OP_l_16_c_3_r_*SH_MAG(1) - OP_l_16_c_4_r_*SK_MY(3) - OP_l_16_c_18_r_*SK_MY(2) - OP_l_16_c_17_r_*SK_MY(4) + OP_l_16_c_19_r_*SK_MY(5)); -Kfusion(17) = SK_MY(1)*(OP_l_17_c_21_r_ + OP_l_17_c_1_r_*SH_MAG(3) + OP_l_17_c_2_r_*SH_MAG(2) + OP_l_17_c_3_r_*SH_MAG(1) - OP_l_17_c_4_r_*SK_MY(3) - OP_l_17_c_18_r_*SK_MY(2) - OP_l_17_c_17_r_*SK_MY(4) + OP_l_17_c_19_r_*SK_MY(5)); -Kfusion(18) = SK_MY(1)*(OP_l_18_c_21_r_ + OP_l_18_c_1_r_*SH_MAG(3) + OP_l_18_c_2_r_*SH_MAG(2) + OP_l_18_c_3_r_*SH_MAG(1) - OP_l_18_c_4_r_*SK_MY(3) - OP_l_18_c_18_r_*SK_MY(2) - OP_l_18_c_17_r_*SK_MY(4) + OP_l_18_c_19_r_*SK_MY(5)); -Kfusion(19) = SK_MY(1)*(OP_l_19_c_21_r_ + OP_l_19_c_1_r_*SH_MAG(3) + OP_l_19_c_2_r_*SH_MAG(2) + OP_l_19_c_3_r_*SH_MAG(1) - OP_l_19_c_4_r_*SK_MY(3) - OP_l_19_c_18_r_*SK_MY(2) - OP_l_19_c_17_r_*SK_MY(4) + OP_l_19_c_19_r_*SK_MY(5)); -Kfusion(20) = SK_MY(1)*(OP_l_20_c_21_r_ + OP_l_20_c_1_r_*SH_MAG(3) + OP_l_20_c_2_r_*SH_MAG(2) + OP_l_20_c_3_r_*SH_MAG(1) - OP_l_20_c_4_r_*SK_MY(3) - OP_l_20_c_18_r_*SK_MY(2) - OP_l_20_c_17_r_*SK_MY(4) + OP_l_20_c_19_r_*SK_MY(5)); -Kfusion(21) = SK_MY(1)*(OP_l_21_c_21_r_ + OP_l_21_c_1_r_*SH_MAG(3) + OP_l_21_c_2_r_*SH_MAG(2) + OP_l_21_c_3_r_*SH_MAG(1) - OP_l_21_c_4_r_*SK_MY(3) - OP_l_21_c_18_r_*SK_MY(2) - OP_l_21_c_17_r_*SK_MY(4) + OP_l_21_c_19_r_*SK_MY(5)); -Kfusion(22) = SK_MY(1)*(OP_l_22_c_21_r_ + OP_l_22_c_1_r_*SH_MAG(3) + OP_l_22_c_2_r_*SH_MAG(2) + OP_l_22_c_3_r_*SH_MAG(1) - OP_l_22_c_4_r_*SK_MY(3) - OP_l_22_c_18_r_*SK_MY(2) - OP_l_22_c_17_r_*SK_MY(4) + OP_l_22_c_19_r_*SK_MY(5)); -Kfusion(23) = SK_MY(1)*(OP_l_23_c_21_r_ + OP_l_23_c_1_r_*SH_MAG(3) + OP_l_23_c_2_r_*SH_MAG(2) + OP_l_23_c_3_r_*SH_MAG(1) - OP_l_23_c_4_r_*SK_MY(3) - OP_l_23_c_18_r_*SK_MY(2) - OP_l_23_c_17_r_*SK_MY(4) + OP_l_23_c_19_r_*SK_MY(5)); -Kfusion(24) = SK_MY(1)*(OP_l_24_c_21_r_ + OP_l_24_c_1_r_*SH_MAG(3) + OP_l_24_c_2_r_*SH_MAG(2) + OP_l_24_c_3_r_*SH_MAG(1) - OP_l_24_c_4_r_*SK_MY(3) - OP_l_24_c_18_r_*SK_MY(2) - OP_l_24_c_17_r_*SK_MY(4) + OP_l_24_c_19_r_*SK_MY(5)); - - -H_MAG = zeros(1,24); -H_MAG(1) = SH_MAG(2); -H_MAG(2) = -SH_MAG(3); -H_MAG(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; -H_MAG(4) = SH_MAG(1); -H_MAG(17) = 2*q0*q2 + 2*q1*q3; -H_MAG(18) = 2*q2*q3 - 2*q0*q1; -H_MAG(19) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7); -H_MAG(22) = 1; - - -SK_MZ = zeros(5,1); -SK_MZ(1) = 1/(OP_l_22_c_22_r_ + R_MAG + OP_l_1_c_22_r_*SH_MAG(2) - OP_l_2_c_22_r_*SH_MAG(3) + OP_l_4_c_22_r_*SH_MAG(1) + OP_l_19_c_22_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + (2*q0*q2 + 2*q1*q3)*(OP_l_22_c_17_r_ + OP_l_1_c_17_r_*SH_MAG(2) - OP_l_2_c_17_r_*SH_MAG(3) + OP_l_4_c_17_r_*SH_MAG(1) + OP_l_19_c_17_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_17_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_17_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(OP_l_22_c_18_r_ + OP_l_1_c_18_r_*SH_MAG(2) - OP_l_2_c_18_r_*SH_MAG(3) + OP_l_4_c_18_r_*SH_MAG(1) + OP_l_19_c_18_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_18_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_18_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_22_c_3_r_ + OP_l_1_c_3_r_*SH_MAG(2) - OP_l_2_c_3_r_*SH_MAG(3) + OP_l_4_c_3_r_*SH_MAG(1) + OP_l_19_c_3_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_3_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_3_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_17_c_22_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_22_r_*(2*q0*q1 - 2*q2*q3) + SH_MAG(2)*(OP_l_22_c_1_r_ + OP_l_1_c_1_r_*SH_MAG(2) - OP_l_2_c_1_r_*SH_MAG(3) + OP_l_4_c_1_r_*SH_MAG(1) + OP_l_19_c_1_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_1_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_1_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(3)*(OP_l_22_c_2_r_ + OP_l_1_c_2_r_*SH_MAG(2) - OP_l_2_c_2_r_*SH_MAG(3) + OP_l_4_c_2_r_*SH_MAG(1) + OP_l_19_c_2_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_2_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_2_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP_l_22_c_4_r_ + OP_l_1_c_4_r_*SH_MAG(2) - OP_l_2_c_4_r_*SH_MAG(3) + OP_l_4_c_4_r_*SH_MAG(1) + OP_l_19_c_4_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_4_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_4_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7))*(OP_l_22_c_19_r_ + OP_l_1_c_19_r_*SH_MAG(2) - OP_l_2_c_19_r_*SH_MAG(3) + OP_l_4_c_19_r_*SH_MAG(1) + OP_l_19_c_19_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_19_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_19_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_3_c_22_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); -SK_MZ(2) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7); -SK_MZ(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; -SK_MZ(4) = 2*q0*q1 - 2*q2*q3; -SK_MZ(5) = 2*q0*q2 + 2*q1*q3; - - -Kfusion = zeros(24,1); -Kfusion = zeros(1,1); -Kfusion(1) = SK_MZ(1)*(OP_l_1_c_22_r_ + OP_l_1_c_1_r_*SH_MAG(2) - OP_l_1_c_2_r_*SH_MAG(3) + OP_l_1_c_4_r_*SH_MAG(1) + OP_l_1_c_3_r_*SK_MZ(3) + OP_l_1_c_19_r_*SK_MZ(2) + OP_l_1_c_17_r_*SK_MZ(5) - OP_l_1_c_18_r_*SK_MZ(4)); -Kfusion(2) = SK_MZ(1)*(OP_l_2_c_22_r_ + OP_l_2_c_1_r_*SH_MAG(2) - OP_l_2_c_2_r_*SH_MAG(3) + OP_l_2_c_4_r_*SH_MAG(1) + OP_l_2_c_3_r_*SK_MZ(3) + OP_l_2_c_19_r_*SK_MZ(2) + OP_l_2_c_17_r_*SK_MZ(5) - OP_l_2_c_18_r_*SK_MZ(4)); -Kfusion(3) = SK_MZ(1)*(OP_l_3_c_22_r_ + OP_l_3_c_1_r_*SH_MAG(2) - OP_l_3_c_2_r_*SH_MAG(3) + OP_l_3_c_4_r_*SH_MAG(1) + OP_l_3_c_3_r_*SK_MZ(3) + OP_l_3_c_19_r_*SK_MZ(2) + OP_l_3_c_17_r_*SK_MZ(5) - OP_l_3_c_18_r_*SK_MZ(4)); -Kfusion(4) = SK_MZ(1)*(OP_l_4_c_22_r_ + OP_l_4_c_1_r_*SH_MAG(2) - OP_l_4_c_2_r_*SH_MAG(3) + OP_l_4_c_4_r_*SH_MAG(1) + OP_l_4_c_3_r_*SK_MZ(3) + OP_l_4_c_19_r_*SK_MZ(2) + OP_l_4_c_17_r_*SK_MZ(5) - OP_l_4_c_18_r_*SK_MZ(4)); -Kfusion(5) = SK_MZ(1)*(OP_l_5_c_22_r_ + OP_l_5_c_1_r_*SH_MAG(2) - OP_l_5_c_2_r_*SH_MAG(3) + OP_l_5_c_4_r_*SH_MAG(1) + OP_l_5_c_3_r_*SK_MZ(3) + OP_l_5_c_19_r_*SK_MZ(2) + OP_l_5_c_17_r_*SK_MZ(5) - OP_l_5_c_18_r_*SK_MZ(4)); -Kfusion(6) = SK_MZ(1)*(OP_l_6_c_22_r_ + OP_l_6_c_1_r_*SH_MAG(2) - OP_l_6_c_2_r_*SH_MAG(3) + OP_l_6_c_4_r_*SH_MAG(1) + OP_l_6_c_3_r_*SK_MZ(3) + OP_l_6_c_19_r_*SK_MZ(2) + OP_l_6_c_17_r_*SK_MZ(5) - OP_l_6_c_18_r_*SK_MZ(4)); -Kfusion(7) = SK_MZ(1)*(OP_l_7_c_22_r_ + OP_l_7_c_1_r_*SH_MAG(2) - OP_l_7_c_2_r_*SH_MAG(3) + OP_l_7_c_4_r_*SH_MAG(1) + OP_l_7_c_3_r_*SK_MZ(3) + OP_l_7_c_19_r_*SK_MZ(2) + OP_l_7_c_17_r_*SK_MZ(5) - OP_l_7_c_18_r_*SK_MZ(4)); -Kfusion(8) = SK_MZ(1)*(OP_l_8_c_22_r_ + OP_l_8_c_1_r_*SH_MAG(2) - OP_l_8_c_2_r_*SH_MAG(3) + OP_l_8_c_4_r_*SH_MAG(1) + OP_l_8_c_3_r_*SK_MZ(3) + OP_l_8_c_19_r_*SK_MZ(2) + OP_l_8_c_17_r_*SK_MZ(5) - OP_l_8_c_18_r_*SK_MZ(4)); -Kfusion(9) = SK_MZ(1)*(OP_l_9_c_22_r_ + OP_l_9_c_1_r_*SH_MAG(2) - OP_l_9_c_2_r_*SH_MAG(3) + OP_l_9_c_4_r_*SH_MAG(1) + OP_l_9_c_3_r_*SK_MZ(3) + OP_l_9_c_19_r_*SK_MZ(2) + OP_l_9_c_17_r_*SK_MZ(5) - OP_l_9_c_18_r_*SK_MZ(4)); -Kfusion(10) = SK_MZ(1)*(OP_l_10_c_22_r_ + OP_l_10_c_1_r_*SH_MAG(2) - OP_l_10_c_2_r_*SH_MAG(3) + OP_l_10_c_4_r_*SH_MAG(1) + OP_l_10_c_3_r_*SK_MZ(3) + OP_l_10_c_19_r_*SK_MZ(2) + OP_l_10_c_17_r_*SK_MZ(5) - OP_l_10_c_18_r_*SK_MZ(4)); -Kfusion(11) = SK_MZ(1)*(OP_l_11_c_22_r_ + OP_l_11_c_1_r_*SH_MAG(2) - OP_l_11_c_2_r_*SH_MAG(3) + OP_l_11_c_4_r_*SH_MAG(1) + OP_l_11_c_3_r_*SK_MZ(3) + OP_l_11_c_19_r_*SK_MZ(2) + OP_l_11_c_17_r_*SK_MZ(5) - OP_l_11_c_18_r_*SK_MZ(4)); -Kfusion(12) = SK_MZ(1)*(OP_l_12_c_22_r_ + OP_l_12_c_1_r_*SH_MAG(2) - OP_l_12_c_2_r_*SH_MAG(3) + OP_l_12_c_4_r_*SH_MAG(1) + OP_l_12_c_3_r_*SK_MZ(3) + OP_l_12_c_19_r_*SK_MZ(2) + OP_l_12_c_17_r_*SK_MZ(5) - OP_l_12_c_18_r_*SK_MZ(4)); -Kfusion(13) = SK_MZ(1)*(OP_l_13_c_22_r_ + OP_l_13_c_1_r_*SH_MAG(2) - OP_l_13_c_2_r_*SH_MAG(3) + OP_l_13_c_4_r_*SH_MAG(1) + OP_l_13_c_3_r_*SK_MZ(3) + OP_l_13_c_19_r_*SK_MZ(2) + OP_l_13_c_17_r_*SK_MZ(5) - OP_l_13_c_18_r_*SK_MZ(4)); -Kfusion(14) = SK_MZ(1)*(OP_l_14_c_22_r_ + OP_l_14_c_1_r_*SH_MAG(2) - OP_l_14_c_2_r_*SH_MAG(3) + OP_l_14_c_4_r_*SH_MAG(1) + OP_l_14_c_3_r_*SK_MZ(3) + OP_l_14_c_19_r_*SK_MZ(2) + OP_l_14_c_17_r_*SK_MZ(5) - OP_l_14_c_18_r_*SK_MZ(4)); -Kfusion(15) = SK_MZ(1)*(OP_l_15_c_22_r_ + OP_l_15_c_1_r_*SH_MAG(2) - OP_l_15_c_2_r_*SH_MAG(3) + OP_l_15_c_4_r_*SH_MAG(1) + OP_l_15_c_3_r_*SK_MZ(3) + OP_l_15_c_19_r_*SK_MZ(2) + OP_l_15_c_17_r_*SK_MZ(5) - OP_l_15_c_18_r_*SK_MZ(4)); -Kfusion(16) = SK_MZ(1)*(OP_l_16_c_22_r_ + OP_l_16_c_1_r_*SH_MAG(2) - OP_l_16_c_2_r_*SH_MAG(3) + OP_l_16_c_4_r_*SH_MAG(1) + OP_l_16_c_3_r_*SK_MZ(3) + OP_l_16_c_19_r_*SK_MZ(2) + OP_l_16_c_17_r_*SK_MZ(5) - OP_l_16_c_18_r_*SK_MZ(4)); -Kfusion(17) = SK_MZ(1)*(OP_l_17_c_22_r_ + OP_l_17_c_1_r_*SH_MAG(2) - OP_l_17_c_2_r_*SH_MAG(3) + OP_l_17_c_4_r_*SH_MAG(1) + OP_l_17_c_3_r_*SK_MZ(3) + OP_l_17_c_19_r_*SK_MZ(2) + OP_l_17_c_17_r_*SK_MZ(5) - OP_l_17_c_18_r_*SK_MZ(4)); -Kfusion(18) = SK_MZ(1)*(OP_l_18_c_22_r_ + OP_l_18_c_1_r_*SH_MAG(2) - OP_l_18_c_2_r_*SH_MAG(3) + OP_l_18_c_4_r_*SH_MAG(1) + OP_l_18_c_3_r_*SK_MZ(3) + OP_l_18_c_19_r_*SK_MZ(2) + OP_l_18_c_17_r_*SK_MZ(5) - OP_l_18_c_18_r_*SK_MZ(4)); -Kfusion(19) = SK_MZ(1)*(OP_l_19_c_22_r_ + OP_l_19_c_1_r_*SH_MAG(2) - OP_l_19_c_2_r_*SH_MAG(3) + OP_l_19_c_4_r_*SH_MAG(1) + OP_l_19_c_3_r_*SK_MZ(3) + OP_l_19_c_19_r_*SK_MZ(2) + OP_l_19_c_17_r_*SK_MZ(5) - OP_l_19_c_18_r_*SK_MZ(4)); -Kfusion(20) = SK_MZ(1)*(OP_l_20_c_22_r_ + OP_l_20_c_1_r_*SH_MAG(2) - OP_l_20_c_2_r_*SH_MAG(3) + OP_l_20_c_4_r_*SH_MAG(1) + OP_l_20_c_3_r_*SK_MZ(3) + OP_l_20_c_19_r_*SK_MZ(2) + OP_l_20_c_17_r_*SK_MZ(5) - OP_l_20_c_18_r_*SK_MZ(4)); -Kfusion(21) = SK_MZ(1)*(OP_l_21_c_22_r_ + OP_l_21_c_1_r_*SH_MAG(2) - OP_l_21_c_2_r_*SH_MAG(3) + OP_l_21_c_4_r_*SH_MAG(1) + OP_l_21_c_3_r_*SK_MZ(3) + OP_l_21_c_19_r_*SK_MZ(2) + OP_l_21_c_17_r_*SK_MZ(5) - OP_l_21_c_18_r_*SK_MZ(4)); -Kfusion(22) = SK_MZ(1)*(OP_l_22_c_22_r_ + OP_l_22_c_1_r_*SH_MAG(2) - OP_l_22_c_2_r_*SH_MAG(3) + OP_l_22_c_4_r_*SH_MAG(1) + OP_l_22_c_3_r_*SK_MZ(3) + OP_l_22_c_19_r_*SK_MZ(2) + OP_l_22_c_17_r_*SK_MZ(5) - OP_l_22_c_18_r_*SK_MZ(4)); -Kfusion(23) = SK_MZ(1)*(OP_l_23_c_22_r_ + OP_l_23_c_1_r_*SH_MAG(2) - OP_l_23_c_2_r_*SH_MAG(3) + OP_l_23_c_4_r_*SH_MAG(1) + OP_l_23_c_3_r_*SK_MZ(3) + OP_l_23_c_19_r_*SK_MZ(2) + OP_l_23_c_17_r_*SK_MZ(5) - OP_l_23_c_18_r_*SK_MZ(4)); -Kfusion(24) = SK_MZ(1)*(OP_l_24_c_22_r_ + OP_l_24_c_1_r_*SH_MAG(2) - OP_l_24_c_2_r_*SH_MAG(3) + OP_l_24_c_4_r_*SH_MAG(1) + OP_l_24_c_3_r_*SK_MZ(3) + OP_l_24_c_19_r_*SK_MZ(2) + OP_l_24_c_17_r_*SK_MZ(5) - OP_l_24_c_18_r_*SK_MZ(4)); - - -SH_ACCX = zeros(4,1); -SH_ACCX(1) = q0^2 + q1^2 - q2^2 - q3^2; -SH_ACCX(2) = vn - vwn; -SH_ACCX(3) = ve - vwe; -SH_ACCX(4) = 2*q0*q3 + 2*q1*q2; - -H_ACCX = zeros(1,24); -H_ACCX(1,1) = -Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd); -H_ACCX(1,2) = -Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd); -H_ACCX(1,3) = Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd); -H_ACCX(1,4) = -Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd); -H_ACCX(1,5) = -Kaccx*SH_ACCX(1); -H_ACCX(1,6) = -Kaccx*SH_ACCX(4); -H_ACCX(1,7) = Kaccx*(2*q0*q2 - 2*q1*q3); -H_ACCX(1,23) = Kaccx*SH_ACCX(1); -H_ACCX(1,24) = Kaccx*SH_ACCX(4); - - -SK_ACCX = zeros(7,1); -SK_ACCX(1) = 1/(R_ACC + Kaccx*SH_ACCX(1)*(Kaccx*OP_l_5_c_5_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_5_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_5_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_5_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_5_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_5_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_5_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_5_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*SH_ACCX(4)*(Kaccx*OP_l_5_c_6_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_6_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_6_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_6_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_6_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_6_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_6_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_6_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_6_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(1)*(Kaccx*OP_l_5_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_23_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_23_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_23_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_23_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_23_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_23_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_23_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(4)*(Kaccx*OP_l_5_c_24_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_24_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_24_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_24_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_24_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_24_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_24_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_24_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_24_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*OP_l_5_c_7_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_7_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_7_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_7_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_7_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_7_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_7_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_7_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_7_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd)*(Kaccx*OP_l_5_c_1_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_1_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_1_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_1_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_1_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_1_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_1_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_1_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_1_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd)*(Kaccx*OP_l_5_c_2_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_2_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_2_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_2_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_2_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_2_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_2_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_2_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_2_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd)*(Kaccx*OP_l_5_c_3_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_3_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_3_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_3_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_3_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_3_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_3_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_3_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_3_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)*(Kaccx*OP_l_5_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_4_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_4_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_4_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_4_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_4_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_4_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_4_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_4_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd))); -SK_ACCX(2) = 2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd; -SK_ACCX(3) = 2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd; -SK_ACCX(4) = 2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd; -SK_ACCX(5) = 2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd; -SK_ACCX(6) = 2*q0*q2 - 2*q1*q3; -SK_ACCX(7) = SH_ACCX(4); - - -Kfusion = zeros(24,1); -Kfusion = zeros(1,1); -Kfusion(1) = -SK_ACCX(1)*(Kaccx*OP_l_1_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_1_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_1_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_1_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_1_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_1_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_1_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_1_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_1_c_24_r_*SK_ACCX(7)); -Kfusion(2) = -SK_ACCX(1)*(Kaccx*OP_l_2_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_2_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_2_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_2_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_2_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_2_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_2_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_2_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_2_c_24_r_*SK_ACCX(7)); -Kfusion(3) = -SK_ACCX(1)*(Kaccx*OP_l_3_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_3_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_3_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_3_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_3_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_3_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_3_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_3_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_3_c_24_r_*SK_ACCX(7)); -Kfusion(4) = -SK_ACCX(1)*(Kaccx*OP_l_4_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_4_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_4_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_4_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_4_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_4_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_4_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_4_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_4_c_24_r_*SK_ACCX(7)); -Kfusion(5) = -SK_ACCX(1)*(Kaccx*OP_l_5_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_5_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_5_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_5_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_5_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_5_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_5_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_5_c_24_r_*SK_ACCX(7)); -Kfusion(6) = -SK_ACCX(1)*(Kaccx*OP_l_6_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_6_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_6_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_6_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_6_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_6_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_6_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_6_c_24_r_*SK_ACCX(7)); -Kfusion(7) = -SK_ACCX(1)*(Kaccx*OP_l_7_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_7_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_7_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_7_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_7_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_7_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_7_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_7_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_7_c_24_r_*SK_ACCX(7)); -Kfusion(8) = -SK_ACCX(1)*(Kaccx*OP_l_8_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_8_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_8_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_8_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_8_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_8_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_8_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_8_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_8_c_24_r_*SK_ACCX(7)); -Kfusion(9) = -SK_ACCX(1)*(Kaccx*OP_l_9_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_9_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_9_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_9_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_9_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_9_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_9_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_9_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_9_c_24_r_*SK_ACCX(7)); -Kfusion(10) = -SK_ACCX(1)*(Kaccx*OP_l_10_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_10_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_10_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_10_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_10_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_10_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_10_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_10_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_10_c_24_r_*SK_ACCX(7)); -Kfusion(11) = -SK_ACCX(1)*(Kaccx*OP_l_11_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_11_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_11_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_11_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_11_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_11_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_11_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_11_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_11_c_24_r_*SK_ACCX(7)); -Kfusion(12) = -SK_ACCX(1)*(Kaccx*OP_l_12_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_12_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_12_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_12_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_12_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_12_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_12_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_12_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_12_c_24_r_*SK_ACCX(7)); -Kfusion(13) = -SK_ACCX(1)*(Kaccx*OP_l_13_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_13_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_13_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_13_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_13_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_13_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_13_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_13_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_13_c_24_r_*SK_ACCX(7)); -Kfusion(14) = -SK_ACCX(1)*(Kaccx*OP_l_14_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_14_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_14_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_14_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_14_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_14_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_14_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_14_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_14_c_24_r_*SK_ACCX(7)); -Kfusion(15) = -SK_ACCX(1)*(Kaccx*OP_l_15_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_15_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_15_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_15_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_15_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_15_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_15_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_15_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_15_c_24_r_*SK_ACCX(7)); -Kfusion(16) = -SK_ACCX(1)*(Kaccx*OP_l_16_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_16_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_16_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_16_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_16_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_16_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_16_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_16_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_16_c_24_r_*SK_ACCX(7)); -Kfusion(17) = -SK_ACCX(1)*(Kaccx*OP_l_17_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_17_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_17_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_17_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_17_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_17_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_17_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_17_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_17_c_24_r_*SK_ACCX(7)); -Kfusion(18) = -SK_ACCX(1)*(Kaccx*OP_l_18_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_18_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_18_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_18_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_18_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_18_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_18_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_18_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_18_c_24_r_*SK_ACCX(7)); -Kfusion(19) = -SK_ACCX(1)*(Kaccx*OP_l_19_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_19_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_19_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_19_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_19_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_19_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_19_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_19_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_19_c_24_r_*SK_ACCX(7)); -Kfusion(20) = -SK_ACCX(1)*(Kaccx*OP_l_20_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_20_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_20_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_20_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_20_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_20_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_20_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_20_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_20_c_24_r_*SK_ACCX(7)); -Kfusion(21) = -SK_ACCX(1)*(Kaccx*OP_l_21_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_21_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_21_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_21_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_21_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_21_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_21_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_21_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_21_c_24_r_*SK_ACCX(7)); -Kfusion(22) = -SK_ACCX(1)*(Kaccx*OP_l_22_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_22_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_22_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_22_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_22_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_22_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_22_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_22_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_22_c_24_r_*SK_ACCX(7)); -Kfusion(23) = -SK_ACCX(1)*(Kaccx*OP_l_23_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_23_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_23_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_23_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_23_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_23_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_23_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_23_c_24_r_*SK_ACCX(7)); -Kfusion(24) = -SK_ACCX(1)*(Kaccx*OP_l_24_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_24_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_24_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_24_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_24_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_24_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_24_c_24_r_*SK_ACCX(7)); - - -SH_ACCY = zeros(3,1); -SH_ACCY(1) = q0^2 - q1^2 + q2^2 - q3^2; -SH_ACCY(2) = vn - vwn; -SH_ACCY(3) = ve - vwe; - -H_ACCY = zeros(1,24); -H_ACCY(1,1) = -Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd); -H_ACCY(1,2) = -Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd); -H_ACCY(1,3) = -Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd); -H_ACCY(1,4) = Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd); -H_ACCY(1,5) = Kaccy*(2*q0*q3 - 2*q1*q2); -H_ACCY(1,6) = -Kaccy*SH_ACCY(1); -H_ACCY(1,7) = -Kaccy*(2*q0*q1 + 2*q2*q3); -H_ACCY(1,23) = -2*Kaccy*(q0*q3 - q1*q2); -H_ACCY(1,24) = Kaccy*SH_ACCY(1); - - -SK_ACCY = zeros(9,1); -SK_ACCY(1) = 1/(R_ACC + Kaccy*SH_ACCY(1)*(Kaccy*OP_l_6_c_6_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_6_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_6_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_6_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_6_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_6_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_6_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_6_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_6_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*SH_ACCY(1)*(Kaccy*OP_l_6_c_24_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_24_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_24_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_24_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_24_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_24_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_24_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_24_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_24_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*OP_l_6_c_5_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_5_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_5_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_5_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_5_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_5_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_5_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_5_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_5_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*OP_l_6_c_7_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_7_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_7_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_7_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_7_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_7_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_7_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_7_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_7_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP_l_6_c_23_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_23_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_23_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_23_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_23_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_23_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_23_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_23_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_23_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd)*(Kaccy*OP_l_6_c_1_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_1_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_1_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_1_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_1_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_1_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_1_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_1_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_1_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd)*(Kaccy*OP_l_6_c_2_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_2_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_2_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_2_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_2_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_2_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_2_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_2_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_2_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd)*(Kaccy*OP_l_6_c_3_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_3_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_3_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_3_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_3_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_3_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_3_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_3_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_3_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)*(Kaccy*OP_l_6_c_4_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_4_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_4_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_4_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_4_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_4_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_4_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_4_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_4_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd))); -SK_ACCY(2) = 2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd; -SK_ACCY(3) = 2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd; -SK_ACCY(4) = 2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd; -SK_ACCY(5) = 2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd; -SK_ACCY(6) = 2*q0*q3 - 2*q1*q2; -SK_ACCY(7) = q0*q3 - q1*q2; -SK_ACCY(8) = 2*q0*q1 + 2*q2*q3; -SK_ACCY(9) = SH_ACCY(1); - - -Kfusion = zeros(24,1); -Kfusion = zeros(1,1); -Kfusion(1) = -SK_ACCY(1)*(Kaccy*OP_l_1_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_1_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_1_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_1_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_1_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_1_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_1_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_1_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_1_c_24_r_*SK_ACCY(9)); -Kfusion(2) = -SK_ACCY(1)*(Kaccy*OP_l_2_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_2_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_2_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_2_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_2_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_2_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_2_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_2_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_2_c_24_r_*SK_ACCY(9)); -Kfusion(3) = -SK_ACCY(1)*(Kaccy*OP_l_3_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_3_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_3_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_3_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_3_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_3_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_3_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_3_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_3_c_24_r_*SK_ACCY(9)); -Kfusion(4) = -SK_ACCY(1)*(Kaccy*OP_l_4_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_4_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_4_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_4_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_4_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_4_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_4_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_4_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_4_c_24_r_*SK_ACCY(9)); -Kfusion(5) = -SK_ACCY(1)*(Kaccy*OP_l_5_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_5_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_5_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_5_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_5_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_5_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_5_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_5_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_5_c_24_r_*SK_ACCY(9)); -Kfusion(6) = -SK_ACCY(1)*(Kaccy*OP_l_6_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_6_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_6_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_6_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_6_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_6_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_6_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_6_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_6_c_24_r_*SK_ACCY(9)); -Kfusion(7) = -SK_ACCY(1)*(Kaccy*OP_l_7_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_7_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_7_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_7_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_7_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_7_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_7_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_7_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_7_c_24_r_*SK_ACCY(9)); -Kfusion(8) = -SK_ACCY(1)*(Kaccy*OP_l_8_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_8_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_8_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_8_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_8_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_8_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_8_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_8_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_8_c_24_r_*SK_ACCY(9)); -Kfusion(9) = -SK_ACCY(1)*(Kaccy*OP_l_9_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_9_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_9_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_9_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_9_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_9_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_9_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_9_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_9_c_24_r_*SK_ACCY(9)); -Kfusion(10) = -SK_ACCY(1)*(Kaccy*OP_l_10_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_10_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_10_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_10_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_10_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_10_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_10_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_10_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_10_c_24_r_*SK_ACCY(9)); -Kfusion(11) = -SK_ACCY(1)*(Kaccy*OP_l_11_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_11_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_11_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_11_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_11_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_11_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_11_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_11_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_11_c_24_r_*SK_ACCY(9)); -Kfusion(12) = -SK_ACCY(1)*(Kaccy*OP_l_12_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_12_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_12_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_12_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_12_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_12_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_12_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_12_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_12_c_24_r_*SK_ACCY(9)); -Kfusion(13) = -SK_ACCY(1)*(Kaccy*OP_l_13_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_13_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_13_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_13_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_13_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_13_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_13_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_13_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_13_c_24_r_*SK_ACCY(9)); -Kfusion(14) = -SK_ACCY(1)*(Kaccy*OP_l_14_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_14_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_14_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_14_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_14_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_14_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_14_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_14_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_14_c_24_r_*SK_ACCY(9)); -Kfusion(15) = -SK_ACCY(1)*(Kaccy*OP_l_15_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_15_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_15_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_15_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_15_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_15_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_15_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_15_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_15_c_24_r_*SK_ACCY(9)); -Kfusion(16) = -SK_ACCY(1)*(Kaccy*OP_l_16_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_16_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_16_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_16_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_16_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_16_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_16_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_16_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_16_c_24_r_*SK_ACCY(9)); -Kfusion(17) = -SK_ACCY(1)*(Kaccy*OP_l_17_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_17_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_17_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_17_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_17_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_17_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_17_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_17_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_17_c_24_r_*SK_ACCY(9)); -Kfusion(18) = -SK_ACCY(1)*(Kaccy*OP_l_18_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_18_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_18_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_18_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_18_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_18_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_18_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_18_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_18_c_24_r_*SK_ACCY(9)); -Kfusion(19) = -SK_ACCY(1)*(Kaccy*OP_l_19_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_19_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_19_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_19_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_19_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_19_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_19_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_19_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_19_c_24_r_*SK_ACCY(9)); -Kfusion(20) = -SK_ACCY(1)*(Kaccy*OP_l_20_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_20_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_20_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_20_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_20_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_20_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_20_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_20_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_20_c_24_r_*SK_ACCY(9)); -Kfusion(21) = -SK_ACCY(1)*(Kaccy*OP_l_21_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_21_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_21_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_21_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_21_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_21_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_21_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_21_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_21_c_24_r_*SK_ACCY(9)); -Kfusion(22) = -SK_ACCY(1)*(Kaccy*OP_l_22_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_22_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_22_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_22_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_22_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_22_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_22_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_22_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_22_c_24_r_*SK_ACCY(9)); -Kfusion(23) = -SK_ACCY(1)*(Kaccy*OP_l_23_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_23_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_23_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_23_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_23_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_23_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_23_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_23_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_23_c_24_r_*SK_ACCY(9)); -Kfusion(24) = -SK_ACCY(1)*(Kaccy*OP_l_24_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_24_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_24_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_24_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_24_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_24_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_24_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_24_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_24_c_24_r_*SK_ACCY(9)); - diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Tbn_312.c b/EKF/matlab/scripts/Inertial Nav EKF/Tbn_312.c deleted file mode 100644 index d19f7b1b6a..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/Tbn_312.c +++ /dev/null @@ -1,15 +0,0 @@ -t2 = sin(yaw); -t3 = cos(yaw); -t4 = sin(pitch); -t5 = cos(pitch); -t6 = sin(roll); -t7 = cos(roll); -A0[0][0] = t3*t5-t2*t4*t6; -A0[0][1] = -t2*t7; -A0[0][2] = t3*t4+t2*t5*t6; -A0[1][0] = t2*t5+t3*t4*t6; -A0[1][1] = t3*t7; -A0[1][2] = t2*t4-t3*t5*t6; -A0[2][0] = -t4*t7; -A0[2][1] = t6; -A0[2][2] = t5*t7; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Tbn_321.c b/EKF/matlab/scripts/Inertial Nav EKF/Tbn_321.c deleted file mode 100644 index ca56049298..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/Tbn_321.c +++ /dev/null @@ -1,15 +0,0 @@ -t2 = cos(yaw); -t3 = sin(roll); -t4 = sin(yaw); -t5 = cos(roll); -t6 = sin(pitch); -t7 = cos(pitch); -A0[0][0] = t2*t7; -A0[0][1] = -t4*t5+t2*t3*t6; -A0[0][2] = t3*t4+t2*t5*t6; -A0[1][0] = t4*t7; -A0[1][1] = t2*t5+t3*t4*t6; -A0[1][2] = -t2*t3+t4*t5*t6; -A0[2][0] = -t6; -A0[2][1] = t3*t7; -A0[2][2] = t5*t7; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Tbn_quat.c b/EKF/matlab/scripts/Inertial Nav EKF/Tbn_quat.c deleted file mode 100644 index 0618dd168f..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/Tbn_quat.c +++ /dev/null @@ -1,17 +0,0 @@ -float t2 = q1*q2*2.0; -float t3 = q0*q0; -float t4 = q1*q1; -float t5 = q2*q2; -float t6 = q3*q3; -float t7 = q0*q2*2.0; -float t8 = q1*q3*2.0; -float t9 = q2*q3*2.0; -A0[0][0] = t3+t4-t5-t6; -A0[0][1] = t2-q0*q3*2.0; -A0[0][2] = t7+t8; -A0[1][0] = t2+q0*q3*2.0; -A0[1][1] = t3-t4+t5-t6; -A0[1][2] = t9-q0*q1*2.0; -A0[2][0] = -t7+t8; -A0[2][1] = t9+q0*q1*2.0; -A0[2][2] = t3-t4-t5+t6; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c b/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c deleted file mode 100644 index 89e0283708..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - C code fragment for function that enables the yaw uncertainty to be increased following a yaw reset. - The variables _state.quat_nominal(0) -> _state.quat_nominal(3) are the attitude quaternions - The variable daYawVar is the variance of the yaw angle uncertainty in rad**2 - See DeriveYawResetEquations.m for the derivation - The gnerate autocode has been cleaned up with removal of 0 coefficient terms and mirroring of lower - diagonal terms missing from the derivation script raw autocode output of C_code4.txt -*/ - -// Intermediate variables -float SG[3]; -SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3)); -SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3); -SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3); - -float SQ[4]; -SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1])); -SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2])); -SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2])); -SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1])); - -// Variance of yaw angle uncertainty (rad**2) -const float daYawVar = TBD; - -// Add covariances for additonal yaw uncertainty to existing covariances. -// This assumes that the additional yaw error is uncorrrelated -P[0][0] += yaw_variance*sq(SQ[2]); -P[0][1] += yaw_variance*SQ[1]*SQ[2]; -P[1][1] += yaw_variance*sq(SQ[1]); -P[0][2] += yaw_variance*SQ[0]*SQ[2]; -P[1][2] += yaw_variance*SQ[0]*SQ[1]; -P[2][2] += yaw_variance*sq(SQ[0]); -P[0][3] -= yaw_variance*SQ[2]*SQ[3]; -P[1][3] -= yaw_variance*SQ[1]*SQ[3]; -P[2][3] -= yaw_variance*SQ[0]*SQ[3]; -P[3][3] += yaw_variance*sq(SQ[3]); -P[1][0] += yaw_variance*SQ[1]*SQ[2]; -P[2][0] += yaw_variance*SQ[0]*SQ[2]; -P[2][1] += yaw_variance*SQ[0]*SQ[1]; -P[3][0] -= yaw_variance*SQ[2]*SQ[3]; -P[3][1] -= yaw_variance*SQ[1]*SQ[3]; -P[3][2] -= yaw_variance*SQ[0]*SQ[3]; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c b/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c deleted file mode 100644 index 91567af894..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c +++ /dev/null @@ -1,38 +0,0 @@ -t9 = q0*q3; -t10 = q1*q2; -t2 = t9-t10; -t3 = q0*q0; -t4 = q1*q1; -t5 = q2*q2; -t6 = q3*q3; -t7 = t3-t4+t5-t6; -t8 = 1.0/(t7*t7); -t11 = t2*t2; -t12 = t8*t11*4.0; -t13 = t12+1.0; -t14 = 1.0/t13; -t15 = 1.0/(t2*t2); -t16 = q3*t3; -t17 = q3*t4; -t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0; -t19 = t7*t7; -t20 = t15*t19*(1.0/4.0); -t21 = t20+1.0; -t22 = 1.0/t21; -t23 = q2*t3; -t24 = q2*t4; -t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0; -t26 = q1*t5; -t27 = q1*t6; -t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0; -t29 = q0*t5; -t30 = q0*t6; -t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0; -A0[0][0] = t8*t14*t18*-2.0; -A0[0][1] = t8*t14*t25*-2.0; -A0[0][2] = t8*t14*t28*2.0; -A0[0][3] = t8*t14*t31*2.0; -A0[1][0] = t15*t18*t22*(-1.0/2.0); -A0[1][1] = t15*t22*t25*(-1.0/2.0); -A0[1][2] = t15*t22*t28*(1.0/2.0); -A0[1][3] = t15*t22*t31*(1.0/2.0); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c b/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c deleted file mode 100644 index 9fe4d7db47..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c +++ /dev/null @@ -1,42 +0,0 @@ -t9 = q0*q3; -t10 = q1*q2; -t2 = t9+t10; -t3 = q0*q0; -t4 = q1*q1; -t5 = q2*q2; -t6 = q3*q3; -t7 = t3+t4-t5-t6; -t8 = 1.0/(t7*t7); -t11 = t2*t2; -t12 = t8*t11*4.0; -t13 = t12+1.0; -t14 = 1.0/t13; -t15 = 1.0/(t2*t2); -t16 = q3*t3; -t17 = q3*t5; -t18 = q0*q1*q2*2.0; -t19 = t16+t17+t18-q3*t4+q3*t6; -t20 = t7*t7; -t21 = t15*t20*(1.0/4.0); -t22 = t21+1.0; -t23 = 1.0/t22; -t24 = q2*t4; -t25 = q2*t6; -t26 = q0*q1*q3*2.0; -t27 = t24+t25+t26-q2*t3+q2*t5; -t28 = q1*t3; -t29 = q1*t5; -t30 = q0*q2*q3*2.0; -t31 = t28+t29+t30+q1*t4-q1*t6; -t32 = q0*t4; -t33 = q0*t6; -t34 = q1*q2*q3*2.0; -t35 = t32+t33+t34+q0*t3-q0*t5; -A0[0][0] = t8*t14*t19*-2.0; -A0[0][1] = t8*t14*t27*-2.0; -A0[0][2] = t8*t14*t31*2.0; -A0[0][3] = t8*t14*t35*2.0; -A0[1][0] = t15*t19*t23*(-1.0/2.0); -A0[1][1] = t15*t23*t27*(-1.0/2.0); -A0[1][2] = t15*t23*t31*(1.0/2.0); -A0[1][3] = t15*t23*t35*(1.0/2.0); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAWGPS.c b/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAWGPS.c deleted file mode 100644 index 0059bd3fa8..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAWGPS.c +++ /dev/null @@ -1,38 +0,0 @@ -t2 = sin(ant_yaw); -t3 = cos(ant_yaw); -t4 = q0*q3*2.0; -t5 = q0*q0; -t6 = q1*q1; -t7 = q2*q2; -t8 = q3*q3; -t9 = q1*q2*2.0; -t10 = t5+t6-t7-t8; -t11 = t3*t10; -t12 = t4+t9; -t13 = t3*t12; -t14 = t5-t6+t7-t8; -t15 = t2*t14; -t16 = t13+t15; -t17 = t4-t9; -t19 = t2*t17; -t20 = t11-t19; -t18 = 1.0/(t20*t20); -t21 = t16*t16; -t22 = 1.0/pow(t11-t19][2.0); -t23 = q1*t3*2.0; -t24 = q2*t2*2.0; -t25 = t23+t24; -t26 = 1.0/t20; -t27 = q1*t2*2.0; -t28 = t21*t22; -t29 = t28+1.0; -t30 = 1.0/t29; -t31 = q0*t3*2.0; -t32 = t31-q3*t2*2.0; -t33 = q3*t3*2.0; -t34 = q0*t2*2.0; -t35 = t33+t34; -A0[0][0] = (t35/(t11-t2*(t4-q1*q2*2.0))-t16*t18*t32)/(t18*t21+1.0); -A0[0][1] = -t30*(t26*(t27-q2*t3*2.0)+t16*t22*t25); -A0[0][2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0)); -A0[0][3] = t30*(t26*t32+t16*t22*t35); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/calcMAGD.c b/EKF/matlab/scripts/Inertial Nav EKF/calcMAGD.c deleted file mode 100644 index a9d71649f0..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/calcMAGD.c +++ /dev/null @@ -1,46 +0,0 @@ -t2 = magE*magE; -t3 = magN*magN; -t4 = t2+t3; -t5 = P[16][16]*t2; -t6 = P[17][17]*t3; -t7 = t2*t2; -t8 = R_DECL*t7; -t9 = t3*t3; -t10 = R_DECL*t9; -t11 = R_DECL*t2*t3*2.0; -t14 = P[16][17]*magE*magN; -t15 = P[17][16]*magE*magN; -t12 = t5+t6+t8+t10+t11-t14-t15; -t13 = 1.0/t12; -t16 = conjugate(magE); -t17 = conjugate(magN); -t18 = t16*t16; -t19 = t17*t17; -t20 = t18+t19; -t21 = 1.0/t20; -A0[0][0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN); -A0[1][0] = -t4*t13*(P[1][16]*magE-P[1][17]*magN); -A0[2][0] = -t4*t13*(P[2][16]*magE-P[2][17]*magN); -A0[3][0] = -t4*t13*(P[3][16]*magE-P[3][17]*magN); -A0[4][0] = -t4*t13*(P[4][16]*magE-P[4][17]*magN); -A0[5][0] = -t4*t13*(P[5][16]*magE-P[5][17]*magN); -A0[6][0] = -t4*t13*(P[6][16]*magE-P[6][17]*magN); -A0[7][0] = -t4*t13*(P[7][16]*magE-P[7][17]*magN); -A0[8][0] = -t4*t13*(P[8][16]*magE-P[8][17]*magN); -A0[9][0] = -t4*t13*(P[9][16]*magE-P[9][17]*magN); -A0[10][0] = -t4*t13*(P[10][16]*magE-P[10][17]*magN); -A0[11][0] = -t4*t13*(P[11][16]*magE-P[11][17]*magN); -A0[12][0] = -t4*t13*(P[12][16]*magE-P[12][17]*magN); -A0[13][0] = -t4*t13*(P[13][16]*magE-P[13][17]*magN); -A0[14][0] = -t4*t13*(P[14][16]*magE-P[14][17]*magN); -A0[15][0] = -t4*t13*(P[15][16]*magE-P[15][17]*magN); -A0[16][0] = -t4*t13*(P[16][16]*magE-P[16][17]*magN); -A0[16][1] = -t16*t21; -A0[17][0] = -t4*t13*(P[17][16]*magE-P[17][17]*magN); -A0[17][1] = t17*t21; -A0[18][0] = -t4*t13*(P[18][16]*magE-P[18][17]*magN); -A0[19][0] = -t4*t13*(P[19][16]*magE-P[19][17]*magN); -A0[20][0] = -t4*t13*(P[20][16]*magE-P[20][17]*magN); -A0[21][0] = -t4*t13*(P[21][16]*magE-P[21][17]*magN); -A0[22][0] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); -A0[23][0] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/derive_air_data_errors.m b/EKF/matlab/scripts/Inertial Nav EKF/derive_air_data_errors.m deleted file mode 100644 index 1eae4a2f70..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/derive_air_data_errors.m +++ /dev/null @@ -1,97 +0,0 @@ -% This script generates c code required to calculate the variance of the -% TAS, AoA and AoS estimates calculated from the vehicle quaternions, NED -% velocity and NED wind velocity. Uncertainty in the quaternions is -% ignored. Variance in vehicle velocity and wind velocity is accounted for. - -%% calculate TAS error terms -clear all; -reset(symengine); - -syms vn ve vd 'real' % navigation frame NED velocity (m/s) -syms vwn vwe vwd 'real' % navigation frame NED wind velocity (m/s) -syms vn_var ve_var vd_var 'real' % navigation frame NED velocity variances (m/s)^2 -syms vwn_var vwe_var vwd_var 'real' % navigation frame NED wind velocity variances (m/s)^2 -syms q0 q1 q2 q3 'real' % quaternions defining rotation from navigation NED frame to body XYZ frame - -quat = [q0;q1;q2;q3]; - -% rotation matrix from navigation to body frame -Tnb = transpose(Quat2Tbn(quat)); - -% crete velocity vectors -ground_velocity_truth = [vn;ve;vd]; -wind_velocity_truth = [vwn;vwe;vwd]; - -% calcuate wind relative velocity -rel_vel_ef = ground_velocity_truth - wind_velocity_truth; - -% rotate into body frame -rel_vel_bf = Tnb * rel_vel_ef; - -% calculate the true airspeed -TAS = sqrt(rel_vel_bf(1)^2 + rel_vel_bf(2)^2 + rel_vel_bf(3)^2); - -% derive the control(disturbance) influence matrix from velocity error to -% TAS error -G_TAS = jacobian(TAS, [ground_velocity_truth;wind_velocity_truth]); - -% derive the error matrix -TAS_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]); -Q_TAS = G_TAS*TAS_dist_matrix*transpose(G_TAS); - -% save as C code -ccode(Q_TAS,'file','Q_TAS.c'); - -save temp.mat; - -%% calculate AoA error equations -clear all; -reset(symengine); - -load temp.mat; - -AoA = atan(rel_vel_bf(3) / rel_vel_bf(1)); - -% derive the control(disturbance) influence matrix from velocity error to -% AoA error -G_AoA = jacobian(AoA, [ground_velocity_truth;wind_velocity_truth]); - -% derive the error matrix -AoA_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]); -Q_AoA = G_AoA*AoA_dist_matrix*transpose(G_AoA); - -% save as C code -ccode(Q_AoA,'file','Q_AoA.c'); - -save temp.mat; - -%% Calculate AoS error equations - -clear all; -reset(symengine); - -load temp.mat; - -AoS = atan(rel_vel_bf(2) / rel_vel_bf(1)); -% derive the control(disturbance) influence matrix from velocity error to -% AoS error -G_AoS = jacobian(AoS, [ground_velocity_truth;wind_velocity_truth]); - -% derive the error matrix -AoS_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]); -Q_AoS = G_AoS*AoS_dist_matrix*transpose(G_AoS); - -% save as C code -ccode(Q_AoS,'file','Q_AoS.c'); - -save temp.mat; - -%% convert them combined to take advantage of shared terms in the optimiser - -clear all; -reset(symengine); - -load temp.mat; - -% save as C code -ccode([Q_TAS;Q_AoA;Q_AoS],'file','Q_airdata.c'); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/fix_c_code.m b/EKF/matlab/scripts/Inertial Nav EKF/fix_c_code.m deleted file mode 100644 index 0eb2874843..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/fix_c_code.m +++ /dev/null @@ -1,92 +0,0 @@ -function fix_c_code(fileName) -%% Initialize variables -delimiter = ''; - -%% Format string for each line of text: -% column1: text (%s) -% For more information, see the TEXTSCAN documentation. -formatSpec = '%s%[^\n\r]'; - -%% Open the text file. -fileID = fopen(fileName,'r'); - -%% Read columns of data according to format string. -% This call is based on the structure of the file used to generate this -% code. If an error occurs for a different file, try regenerating the code -% from the Import Tool. -dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false); - -%% Close the text file. -fclose(fileID); - -%% Create output variable -SymbolicOutput = [dataArray{1:end-1}]; - -%% Clear temporary variables -clearvars filename delimiter formatSpec fileID dataArray ans; - -%% replace brackets and commas -for lineIndex = 1:length(SymbolicOutput) - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '('); - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ','); - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')'); -end - -%% Convert indexing and replace brackets - -% replace 1-D indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf('[%d]',(arrayIndex-1)); - strPat = strcat('\(',strIndex,'\)'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace 2-D left indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf('[%d,',(arrayIndex-1)); - strPat = strcat('\(',strIndex,'\,'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace 2-D right indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf(',%d]',(arrayIndex-1)); - strPat = strcat('\,',strIndex,'\)'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace commas -for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')}; -end - -%% Change covariance matrix variable name to P -for lineIndex = 1:length(SymbolicOutput) - strIn = char(SymbolicOutput(lineIndex)); - strIn = regexprep(strIn,'OP\[','P['); - SymbolicOutput(lineIndex) = cellstr(strIn); -end - -%% Write to file -fid = fopen(fileName,'wt'); -for lineIndex = 1:length(SymbolicOutput) - fprintf(fid,char(SymbolicOutput(lineIndex))); - fprintf(fid,'\n'); -end -fclose(fid); -clear all; - -end \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m b/EKF/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m deleted file mode 100644 index bfb0bc6108..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m +++ /dev/null @@ -1,8 +0,0 @@ -clear all; -syms spd yaw real; -syms R_spd R_yaw real; -vx = spd*cos(yaw); -vy = spd*sin(yaw); -Tpc = jacobian([vx;vy],[spd;yaw]); -R_polar = [R_spd 0;0 R_yaw]; -R_cartesian = Tpc*R_polar*Tpc'; \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/quat2yaw312.m b/EKF/matlab/scripts/Inertial Nav EKF/quat2yaw312.m deleted file mode 100644 index b71bb48b68..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/quat2yaw312.m +++ /dev/null @@ -1,29 +0,0 @@ -% define roll, pitch and yaw variables -syms roll pitch yaw 'real' - -% Define yransformaton matrices for rotations about the X,Y and Z body axes -Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)]; -Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)]; -Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1]; - -% calculate the tranformation matrix from body to navigation frame resulting from -% a rotation in yaw, roll, pitch order -Tbn_312 = transpose(Yrot*Xrot*Zrot) - -% convert to c code and save -ccode(Tbn_312,'file','Tbn_312.c'); -fix_c_code('Tbn_312.c'); - -% define the quaternion elements -syms q0 q1 q2 q3 'real' - -% calculate the tranformation matrix from body to navigation frame as a -% function of the quaternions -Tbn_quat = Quat2Tbn([q0;q1;q2;q3]); - -% collect the y,x terms required for calculation of the yaw angle -yaw_input_312 = [-Tbn_quat(1,2);Tbn_quat(2,2)]; - -% convert to c code and save -ccode(yaw_input_312,'file','yaw_input_312.c'); -fix_c_code('yaw_input_312.c'); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/quat2yaw321.m b/EKF/matlab/scripts/Inertial Nav EKF/quat2yaw321.m deleted file mode 100644 index e5729f35e3..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/quat2yaw321.m +++ /dev/null @@ -1,29 +0,0 @@ -% define roll, pitch and yaw variables -syms roll pitch yaw 'real' - -% Define yransformaton matrices for rotations about the X,Y and Z body axes -Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)]; -Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)]; -Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1]; - -% calculate the tranformation matrix from body to navigation frame resulting from -% a rotation in yaw, pitch, roll order -Tbn_321 = transpose(Xrot*Yrot*Zrot) - -% convert to c code and save -ccode(Tbn_321,'file','Tbn_321.c'); -fix_c_code('Tbn_321.c'); - -% define the quaternion elements -syms q0 q1 q2 q3 'real' - -% calculate the tranformation matrix from body to navigation frame as a -% function of the quaternions -Tbn_quat = Quat2Tbn([q0;q1;q2;q3]); - -% calculate the y,x terms required for calculation fo the yaw angle -yaw_input_321 = [Tbn_quat(2,1);Tbn_quat(1,1)]; - -% convert to c code and save -ccode(yaw_input_321,'file','yaw_input_321.c'); -fix_c_code('yaw_input_321.c'); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/quatCovMat.c b/EKF/matlab/scripts/Inertial Nav EKF/quatCovMat.c deleted file mode 100644 index 02fa81c243..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/quatCovMat.c +++ /dev/null @@ -1,59 +0,0 @@ -float t2 = rotX*rotX; -float t4 = rotY*rotY; -float t5 = rotZ*rotZ; -float t6 = t2+t4+t5; -float t7 = sqrtf(t6); -float t8 = t7*0.5f; -float t3 = sinf(t8); -float t9 = t3*t3; -float t10 = 1.0f/t6; -float t11 = 1.0f/sqrtf(t6); -float t12 = cosf(t8); -float t13 = 1.0f/powf(t6,1.5f); -float t14 = t3*t11; -float t15 = rotX*rotY*t3*t13; -float t16 = rotX*rotZ*t3*t13; -float t17 = rotY*rotZ*t3*t13; -float t18 = t2*t10*t12*0.5f; -float t27 = t2*t3*t13; -float t19 = t14+t18-t27; -float t23 = rotX*rotY*t10*t12*0.5f; -float t28 = t15-t23; -float t20 = rotY*rotVarY*t3*t11*t28*0.5f; -float t25 = rotX*rotZ*t10*t12*0.5f; -float t31 = t16-t25; -float t21 = rotZ*rotVarZ*t3*t11*t31*0.5f; -float t22 = t20+t21-rotX*rotVarX*t3*t11*t19*0.5f; -float t24 = t15-t23; -float t26 = t16-t25; -float t29 = t4*t10*t12*0.5f; -float t34 = t3*t4*t13; -float t30 = t14+t29-t34; -float t32 = t5*t10*t12*0.5f; -float t40 = t3*t5*t13; -float t33 = t14+t32-t40; -float t36 = rotY*rotZ*t10*t12*0.5f; -float t39 = t17-t36; -float t35 = rotZ*rotVarZ*t3*t11*t39*0.5f; -float t37 = t15-t23; -float t38 = t17-t36; -float t41 = rotVarX*(t15-t23)*(t16-t25); -float t42 = t41-rotVarY*t30*t39-rotVarZ*t33*t39; -float t43 = t16-t25; -float t44 = t17-t36; -P[0][0] = rotVarX*t2*t9*t10*0.25f+rotVarY*t4*t9*t10*0.25f+rotVarZ*t5*t9*t10*0.25f; -P[0][1] = t22; -P[0][2] = t35+rotX*rotVarX*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarY*t3*t11*t30*0.5f; -P[0][3] = rotX*rotVarX*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarY*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarZ*t3*t11*t33*0.5f; -P[1][0] = t22; -P[1][1] = rotVarX*(t19*t19)+rotVarY*(t24*t24)+rotVarZ*(t26*t26); -P[1][2] = rotVarZ*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t28-rotVarY*t28*t30; -P[1][3] = rotVarY*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t31-rotVarZ*t31*t33; -P[2][0] = t35-rotY*rotVarY*t3*t11*t30*0.5f+rotX*rotVarX*t3*t11*(t15-t23)*0.5f; -P[2][1] = rotVarZ*(t16-t25)*(t17-t36)-rotVarX*t19*t28-rotVarY*t28*t30; -P[2][2] = rotVarY*(t30*t30)+rotVarX*(t37*t37)+rotVarZ*(t38*t38); -P[2][3] = t42; -P[3][0] = rotZ*rotVarZ*t3*t11*t33*(-1.0f/2.0f)+rotX*rotVarX*t3*t11*(t16-t25)*0.5f+rotY*rotVarY*t3*t11*(t17-t36)*0.5f; -P[3][1] = rotVarY*(t15-t23)*(t17-t36)-rotVarX*t19*t31-rotVarZ*t31*t33; -P[3][2] = t42; -P[3][3] = rotVarZ*(t33*t33)+rotVarX*(t43*t43)+rotVarY*(t44*t44); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/rotVarVec.c b/EKF/matlab/scripts/Inertial Nav EKF/rotVarVec.c deleted file mode 100644 index 7eae50465d..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/rotVarVec.c +++ /dev/null @@ -1,19 +0,0 @@ - t2 = q0*q0; - t3 = acos(q0); - t4 = -t2+1.0; - t5 = t2-1.0; - t6 = 1.0/t5; - t7 = q1*t6*2.0; - t8 = 1.0/pow(t4,3.0/2.0); - t9 = q0*q1*t3*t8*2.0; - t10 = t7+t9; - t11 = 1.0/sqrt(t4); - t12 = q2*t6*2.0; - t13 = q0*q2*t3*t8*2.0; - t14 = t12+t13; - t15 = q3*t6*2.0; - t16 = q0*q3*t3*t8*2.0; - t17 = t15+t16; - A0[0][0] = t10*(P_l_0_c_0_r_*t10+P_l_1_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_1_r_*t10+P_l_1_c_1_r_*t3*t11*2.0)*2.0; - A0[1][0] = t14*(P_l_0_c_0_r_*t14+P_l_2_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_2_r_*t14+P_l_2_c_2_r_*t3*t11*2.0)*2.0; - A0[2][0] = t17*(P_l_0_c_0_r_*t17+P_l_3_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_3_r_*t17+P_l_3_c_3_r_*t3*t11*2.0)*2.0; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/temp1.mat b/EKF/matlab/scripts/Inertial Nav EKF/temp1.mat deleted file mode 100644 index 28375e502221a8012cee924945ba108ddafa41d1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1043 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X0%nwjV*I2WZRmZYXASajnphc|TNxTE7#SFvDG&>^85tme(bJb7$npi^iaC#y6B05QlFSsI zF(f&%aU`^SD*n{nEaV-eEFl>oQ8V{%v!vt<$u8|;$sY|xavr}b7XFG@SYx(AvL~QW zAfD;ap3^NV?1ve47;~_I4D};ms3h@*CNnean8U#aGW7uy$f;LDyo+u*2*?KZX-6KJ z8gp>aafG@9uJe7VlibAZY3f$|d$OQ&g;ZhgXu-3~%?QYQMw zon5{1`sKTe1H<d;ua{kU z|D$2urvkSS%Z~T6cH3=kwp+it=F#uEPZM@u)%QN6@%7Em3bW78%*{h>D+BiLe;@e% z-JPS`f1dALe*f>+<$t0?SBI*2zY?{d_9tlW`w7i8y$1`g%cQE?{*g!do;6Ke!IG>h zkS@@Fgz;IBWModFNRD==&Ot*K=2V;Si5$w;+kE*iPvOvA&))0lvhYT3$(__yocB~E z7l%k2b4E>`-74tX{J^ZG^XX^T85d*q7hd_+BF3_NUGABl;5B?@!${*y6Ik`PtDnzDd7aRosh@of7qlRazl(Nb6#9xs8s+-jfSUL#~)k z)e@`qevzFfb}a5zf%Rj9Pp>~*6W=I4`_diOW}*WthQum4BaFw0%zItc)D?$Nyf diff --git a/EKF/matlab/scripts/Inertial Nav EKF/temp2.mat b/EKF/matlab/scripts/Inertial Nav EKF/temp2.mat deleted file mode 100644 index 698457b1fd12b6f7aa8c9f999f938b675ca6b9af..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2434 zcmV-|34QiWK~zjZLLfCRFd$7qR4ry{Y-KDUP;6mzW^ZzBIv`C!LrFF?EFeR2Wnpw> zWFT*DIv`VZZXiuzc_1J$ATcyLGdDUhG9WTAF*YC|ARr(hARr(hARr(hARr(hARr(h zARr(hARr(hARr(hARr(hARr*p00000000000ZB~{0000=0001Zoa19)U}ykhHX!Bz zVnGH7V1$Z!#QXRMM}YV&48G3(!Hf*d48@hX416FtBz?S4eXLME8`KO&s5po(0b$-{ zLXv}F069bky$=8Y0H_E60C=3OSIN%gND;1XHHa3&Xn2J=fqd}3jf6zG%Dc;Um3Lo8 z-d*0yyM5xwBk%}30wBagt5Bh}M?(>+7az=4oHg}%s$$V}xH`Q_no`07`O!_S}k z7f-xByqBN7@K;a#{fqbdoxFbGhgag|9slx)-+%Ej{`F_#Kj6Ln&3A3DUVJVm&G?Y# zpTGQ==hrX(ul}vyeCPX)KZkqZ?p5OSLDwbyx8J_R|K*eTkNv&pZ+Jj3NcWxtw3IISSF||)<&00qypAAqg@@0S=Ri)4FuQ z>vLY$O5L&M%^_`{hGGnf+dKY%JspVwD;VKw8N8iFJms8Uqmo>h>B&k@#}RbDAH7!-IdoRe}F5IhjA@k(L|tvdr@CgU0*WnlpPc1 zd`(&a<;?nc7DOS}P#|>LW`?x0sSMloA~dhDrqxMQ6F%)t&~3G`s;p9> zEWyBs#9|e&MmeVf=B&ELRCgu77ZH%Ady{my4xC~Uz=|y!O0MaUtoQD6cH)5tt%Qar zWR>nGF~SK$3pq`Dk?i<1ix)B7v7#n-N4)Y)UdNTr)(a|lq#PN8$CsTaTyh|5(z*1> zQ(^d%4zMhJ>~B_bA%j#Z<{jjHATA(m;>v0jRW`&kM2)%CUfLcK-8l=<2G(&}TFo9E z)xf3dnM6wLIP-$d0{zj3@19>qyCaz0!w)rmT29RtG0o9h+9Y;`>sX|cPiWYzo)?44 zPZ51P-EdhF@3`t{lB$3w4wst8<7V$`Ytm;|1spUqigG4_6^O2l6lH}(3fiV3mU)S+ zhQ2qu2-Krk4o)jtflNVo=<8=gj0%e>K$n>(O(PKXl2Y|JO*nxqIuwu{1!Xp{-6~1; zNB9AFB<^7N^BTG%z}OpPG7SwMo7H+_ob$EcxGUli`_yf&rnO4N0(r4RBJl94U^?*NY>O+ zfVP31BN9hIQ>kO0z;ZfL0ls2%?mi?KBpS&u@kz19p~G|3+sPFlm#dAHK#1hh^=`@FFU)D$iDPTlW3v>A`XBX zAqOXJ-KIQvBL>Z7)A4pzm{6;tqKi#*vGgubWi%bm#aMCWzykFKjArC*HMIa!%z+8b8X<~AQ^3hPx3})qg;hk zb*-ex7BLe)N@M~hsWxqW>Xk7%IJh|X$Jp4J&~*UXZd=A7u4xkinLBXADRM(y91=x> zc6}B?IFd|j<{Jvz5oI5YmBx9Xs9gf%VpDJiK$Z7A9-r%+#+Ew542s=I9jXWqNsd-~ zKC=aN?&P{5n=lV34T79Gkau1JwTVYcP|WEU{;=~F%;%P!(AaFGh57io|bj>K)y(^2_Q9gz)f_Yu}SM$SU$nTj=Pi^D#+(?LvT1*AU|aD`yapp=&v{O%Yg{zFh}dPHOo&)ZZaT0uwo?KuI0gvU`L3_x zdCG?pr>P`YLz8IsYrffnGV?`zasgTIn+ZC*!6o%!V;s?GfpgR@<)%~f{@l1gpOqAW zD4DwW!F~x(t;Y8I1HO(U#Q^GLsfx}zuIJjvj!LR3DUO%EfN7XFR5ic=VxO`~DciLR z$-FGUkY3a?ylvPFCzl6_#hK6~d8N7w#q3lUIqAGvnsHhvv$g&$2XTdHPKSOW`g zO{!nHjdhVTp5i!N-yy`Jjv!y^F^kR|3->!*Wt@RxrY}Kwt|z(I^*xv@3dKDSBBCUG zbOa^!aYoh2ac$60>39q=X@jfs8UvMyGQO)I1i{4-iXow!7(4;xE4sN^=C0M~qFv81 zh`c(bPRCO05L_1dkt|UA`g|^YLGsIBQ~{clOL0t@`K;g8bsh$BNio!qK;@{f9_cA$ zcIyYM(xxn(%m!GSjY5%g*hco$xsRyk(y8kPpO?t9pGwkhjjr!fiJq$fNTAv;LzTRkYGrW!vB7|*fL`v>5)ps&c_|)o`o~|Jg62V2tP;#=W=UvK7NAQ?#SwL55 z>zsvIp=?lWmIb-A7zq-xrqXw8+X;C}hu*!BT|BhrmW4co__SnlV|AKIMdHTLdG5+S z&{f~d9LrwAFhifu2bHmNIhrE-20bZtE&XJ^* A^Z)<= diff --git a/EKF/matlab/scripts/Inertial Nav EKF/temp3.mat b/EKF/matlab/scripts/Inertial Nav EKF/temp3.mat deleted file mode 100644 index 24e0cc9cf4ddc4966f1d59bca6987f20beb5aeb5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2443 zcmV;633T>NK~zjZLLfCRFd$7qR4ry{Y-KDUP;6mzW^ZzBIv`C!LrFF?EFeR2Wnpw> zWFT*DIv`VZZXiuzc_1J$ATcyLGdDUhG$1lCF*YC|ARr(hARr(hARr(hARr(hARr(h zARr(hARr(hARr(hARr(hARr*p00000000000ZB~{0000=0001Zoa19)U}ykhHX!Bz zVnGH7V1$Z!#QXRMM}qh)48G3(!Hf*d48@hX416FtBz?S4eXLME8`KO&s5po(0b$-{ zLXv}F06C`yz7GHZ0I>)F0C=3OS4qz7ND;30)SzyL7Tf@_0(s$iYN;1?xm+&K<)J)m zTAt^5DBCNR9DyTn6vPQQ21lS?_ussx=>`^r6iR)O5s{IKbo$HBPp8vwetA0m^fi9^ z($7xc$WPwzyO;j`7vGE@Mi~+KmQp2alLQwC*P~%ga6-7Kfdoj zf6xE@o9+FZLLc`1rhj=BAkZ<%pK20A_413gZg# zXQ_}d{#+?6I41hh1avw&UGr3R2>{%3J<=sXOeo85o`I~31hSEQ-}Nd4FbEr|b7B#R zE-H9%+mV5_aXYb`R1IU*CC>AZKR7x}9prEb06-ejCmKenX%zd)OsN zK^5pZ*5c|ns+OJx49u7>XN8RAY>HixEnqdk$b~kh%UObmC@B|(#bK3MGr){kV-uJg z#$7L@5u;9XX_#!G#5l6#M8bP=!LAzA0&UiTrtAybGIUyV-qRQ&+0aX&g1lFJTf621 z;!F$f+Gnp^^unEJB+f^M#je3)zNKv0HH1XdO_kyUWF474T;)4r34z&XH%XB~X+-0I zlc=i4Mr(O?^n7h>*vh4`e)wX^CFwOnA6|cEAW9*p9*d9$J9z&n{PGS}BO_Ki6yY>g z>K^1BssZlV<<19E`D{arRA?Z@4q=;;m48mvBIa3zZj0W7IGE zY>^(#@7J4&k*odK^&}=Ahofy>Yust0Evw+(KI>^l8bS7o^ZeB8c0BY+ z7DC`Ey8U<_agw%>hlP~~|Ma2?CiTU|0ze8Dx3d9B5%s~{^~P`fc=jlXl28_BQ(%Wt zDGZizz@ajO2|dntuLOsi;mJHTRaK>I6qsSd^$tJ|9(T~K z4($=^!8lCf2N-ea;RRdZuuER07pQWihN35K8M<;+>Q}Z<^*YePkoW8B3hZ~} zuIHRl@3&);wdOJ_GmG;;U}MQWB_b-hdFPfFCL4%3kqWu6yk{sU^twZUXPtT73!2>! zYw_HnL8>B=&Mx>8q(MhkH_XLog2-anAb|ND&kwyudRzi)@ zY$it@09)mjMWZ5ioy%F}N2Hs!A}uD3PVi@KfG7tSQxk2V3o9L*x~ouEYolhdfzj6f zG7xIHtP5{@oEu*;1IKMe>n?#KTSLmg3&&ZQhG*@28q1qKtZpAZE-DP7}*!dpXc7rXDv zJL2kM3!r#L%Qv3J04a(clsPyZLAfEwbIe-F;MoDGl0_H3ClEPNNP}Xo23P8>XW!=i zC>JPZ`AkdN9o3|P-7nLab93{BYiuei`R!q;O!-$ zqD-I-(h=A(C>IWB$N}_FFH#&QW>F$mBfgIoIh`LcnwF0)j3UPF$#J^q;(1+VZ*2{G z2uDYvwTKuZ;`Pm(y)*Dw`!Z8o`?*Mh1P3u4YOcHuPY#G9ZW9aY_2*0~-Hde5SvCgn z=8lGZ4;+MinzD9 zrP77U$cAtP(!wq)mAX@jXkI$+<3Ogtm~d@E00kW;JP%CxKoR1lA?+Dlkq50~LMi}W zIfe2yANTm_8~*%UgO-zhGQQpHsGoaiM9r~=<>&_DIQQ6Pig^(Zzc<6PPO+Fw?*zcq z=QdPkiVi+T6Jf0E@kTrD8p=z47c14y9=7k16Euy~oKg%OPCAo)`Q>ka z|B(lN;HN*vkKdpEogY)D|IF)8r+?-3|L^!ep8oT@#6A5hKX<=Q`0-sL{Fn9r*LTSW J`3H8nV3{RdmY)Cs diff --git a/EKF/matlab/scripts/Inertial Nav EKF/test_output_filter_gains.m b/EKF/matlab/scripts/Inertial Nav EKF/test_output_filter_gains.m deleted file mode 100644 index 40e654e9c7..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/test_output_filter_gains.m +++ /dev/null @@ -1,35 +0,0 @@ -% script to test complementary output filter gain structure -vel_state = 0.0; -pos_state = 1.0; -vel_demand = 0.0; -pos_demand = 0.0; -dt = 0.01; -buffer_delay = 30; -vel_state_history = zeros(1,10000); -pos_state_history = zeros(1,10000); -time = [0:dt:9999*dt]; -vel_correction = 0; -pos_correction = 0; -omega = 1.0/(dt*buffer_delay); -prev_vel = 1.0; -for i=1:10000 - prev_vel = vel_state; - vel_state = vel_state + vel_correction*dt; - vel_state_history(i) = vel_state; - pos_state = pos_state + 0.5*dt*(vel_state + prev_vel) + pos_correction*dt; - pos_state_history(i) = pos_state; - if i > buffer_delay - vel_correction = (vel_demand - vel_state_history(i-buffer_delay))*omega*0.5; - pos_correction = (pos_demand - pos_state_history(i-buffer_delay))*omega*0.6; - else - vel_correction = 0; - pos_correction = 0; - end -end -min(vel_state_history) -close all; -figure; -subplot(2,1,1) -plot(time,vel_state_history); -subplot(2,1,2); -plot(time,pos_state_history); \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py b/EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py deleted file mode 100644 index 20d6dbefba..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py +++ /dev/null @@ -1,20 +0,0 @@ -import sympy as sp -Wx, Wy, yaw, R_TAS, initial_wind_var_body_y, R_yaw = sp.symbols('Wx Wy yaw R_TAS initial_wind_var_body_y R_yaw') -Wn = Wx * sp.cos(yaw) - Wy * sp.sin(yaw) -We = Wx * sp.sin(yaw) + Wy * sp.cos(yaw) - -Wn_Wx = sp.diff(Wn, Wx) -Wn_Wy = sp.diff(Wn, Wy) -Wn_yaw = sp.diff(Wn, yaw) -We_Wx = sp.diff(We, Wx) -We_Wy = sp.diff(We, Wy) -We_yaw = sp.diff(We, yaw) - -G = sp.Matrix([[Wn_Wx, Wn_Wy, Wn_yaw],[We_Wx, We_Wy, We_yaw]]) -b_wind_cov = sp.Matrix([[R_TAS, 0.0, 0.0], [0.0,initial_wind_var_body_y, 0.0], [0.0, 0.0, R_yaw]]) -i_wind_cov = G * b_wind_cov * G.T - -print('P[22][22] = ' + str(i_wind_cov[0,0])) -print('P[22][23] = ' + str(i_wind_cov[0,1])) -print('P[23][22] = ' + str(i_wind_cov[1,0])) -print('P[23][23] = ' + str(i_wind_cov[1,1])) \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/yaw_input_312.c b/EKF/matlab/scripts/Inertial Nav EKF/yaw_input_312.c deleted file mode 100644 index 0a8bea9a56..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/yaw_input_312.c +++ /dev/null @@ -1,2 +0,0 @@ -A0[0][0] = q0*q3*2.0-q1*q2*2.0; -A0[1][0] = q0*q0-q1*q1+q2*q2-q3*q3; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/yaw_input_321.c b/EKF/matlab/scripts/Inertial Nav EKF/yaw_input_321.c deleted file mode 100644 index cc5158702f..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/yaw_input_321.c +++ /dev/null @@ -1,2 +0,0 @@ -A0[0][0] = q0*q3*2.0+q1*q2*2.0; -A0[1][0] = q0*q0+q1*q1-q2*q2-q3*q3;