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 zcmb8r^H(GQ!}jqB8#cQ(8=I}owwr1*Hrtx4$+p|D*{-b(lWViR*`Dy8`@Fy2&pFR? z{Q;lzJ(se$mXf#x1rIAbg|fI7ik01vh1t&WPFU9|M(=dSlKNxvs3;+P22mk=QbN9?m zOsoNBycCB3GjRkY6L>Qniiao+7gU5>JwpGyw@@OYOrn^tZbVOFu5vHlMK8c2f8GkB zSTz?+W7vW2@h}?1K>)5Vg3

hQ)mH`xD8J5c=U_0x0=M%g1Sf18DB!mU^I#B}9N|#Oyr-^lTY;k9Z6E=-E2{*in7B zZhA`qJ$q@2zBjx%1rk9g*`{Zx7ujw^$)CH(L67fCt*`apH!wiGZ-;ELAEwaj=_Aqi ztZU=P+wwioJ^1zb$@qf`WV1p3f%ZIEa0I&lxE2a@=z9w#ds*9pzAg2p-Q6x3d|=tfL`@Y8f-9lNK@6ZlBZ~alHEmqQ4E;>+F3yf?eK!G zmP|-EiY#MtSXe_zDf({S`VbrLhw3lNQ)wDC) z!}%A5sH-Pf)le|Mk|JM?YnY=U@mWci1jkWc9X)N=;*t1o6hvD(Loi3#vWCLt*?rTo z85VnaJx+ogQ+l)ud6-g>euKg9ae~c^JTFgEuroV}+PW-m23AM5zH__@`b#ycD?#YR zZRmZs`wg%nkh(pi3^KuosFuL%1Y8dx3C<};i$Mc+Et`|sRPO-B3OdccCfW7sJjP0N zei12#u2d>qU3Lnzi&n<;bs2`Sn^V-N#JV9jE!c801yosmwIQk4Uyxb|Aznf2Ioek@T=O^j8EfF z-&+l7iS1lqs`%1wq+E4ZOdkGxE>|7`PjinRYyx-BIy|w#w_``w2QyVvAbCku%ayQDP6LaX{It6m?*Cs+_IZpGu`Hkswn#3yK`g9Z4XKQj^+8!IrEQNeQFKGVc;aJ$Le$q{pL z1%K}QN;F(8Ou1iuvlCm*KBg=E>FCz2&IMFqdVqX%I#J@xt)&m%JV40>+cAlz6D{7C zuLZmS@}Qk5x7RB-mGG-DW@j@_?k$P|l}c63lFY_##nR2l%Oe$-@@U`8*4#`sUtWwS z2*6}AGa8I*L<*{62wDvBi%~0rcxR6Sl)rA~H!By#9sDP$Z7eQ+9XZpq{#5(Sd9!gl zD(*sWe?6V27~D>~4Qa*tV$)-3cKxYv%`f8jLw>2bqa7D_4H;<I=gB=w}Q?rK2xAMMY;FoWy1=}#t z6>O-OVVXpZcfj&J_o$A4KElu{eK#YvaymHWAkf~rfOA1P=LHSQ1i6{%dZd>16<=0k zo)O^kK0jXtnyH5?E^ZVsBn*7=@=GM-i|4<5ul=DdMR>qf`;>0sVp4kD9h*9nZY|~0 zMT3@!UH*sgd$Ia{B_LRF!{4_Uwo}^3pWUHSK!A^>U3&)Ce2Rp*_f%+#y6*jk))M1^ z@6<3xI(hBb8;r#IFUmv%$~*TOzZ^|*c7KjVo7dymE>KAyZWF8qa=pTx5YCKi*{`{M z7*_B9iDDj|m1=bY`3*B;p$9?DXqxQBMaY^ekX|FwPdeC2X5c#{W~QoA|GN@F+tyQ& zs$nkLBtleCq@=3$ZK)46F_bm_(~YE_QRj_T#9pXtu*&Yd9mtKguEk_KPg7rpi&~wV zy08N|Tww0htiP`Xdnu>)%$Rt>?Cu@6F9Bd%R-+lXALLrAv!!;UTT%KVybhP>A?l(c ze~_8<%G>6xB9sy>Y4rY?hgDT8Rv1@_z?$-&QKIatThO0VnSi!w0>ACzD}9?c|1n2a zMv0Va=T`OKWszOoOX;zu6*y5V@ui(a58&`)fv+(6t8iNX3BDZ4?3s1-@-p!P;;XriZba?W#k|9UB67a$tD?*Z@qG6 zPAX`4j3vr&03IOU>0uZnEDbktNqFcWuiSCpE)XGV5uNwR$4HnaoiBbF8 zyS-t>#IUu>%oqYgam! zX~O(^TG!FR_a%Dj(P!q-N6JMl&GXDfc8+`$LPVDY-L^34a(12;8!u4hGr9Q6J71CI z*qR-L;-D{&7~;N`8gC1Geta{`amj5b$+9RVReKQ#$iNBqOW93 zOIs_K)s)#Xe#QTUd&YWlS<>qXFX z`n_%+yM--;qI=VNCR{?HKhKPhhNhr z12KWyxj zxvnC6bViDRU>-V$d*ru51mH<;uDoihWn;LD%23#->XBpgvrV)ckVhD;p;1ROa1~zm z=Z0Bi1*Mn-mD{7WnY^4g`sukT^!!9`b|09l2UI!!qn|rHOJ;21U7vPw4T#QbW2Ue!BcgpU{iPIO)&M_`*M8fF5f@GT1aogm+D$z z0maD(^*+9EGHF7UT=xE4Z1H9wtsOovL>XNN6v8lb5EBo6A-k;`@+E?uxoS4Qx15;` zrRBSEza1a|Y69w)8Z*Sr0R7*4d2bm@E&mpz#dB?W)Pg0-zSDivKFuGCf;Vn1$wq09 zN>9LY>9DMi!Skr4JJkyby8dP+22LHPJ*6x)Ogn1j`8nKri^(zX|K*{RlQZ}$ve>kN z^sC8F3@0Tm91H~&%x^ju9#v5^GikN6*XUwekA&EV7TQv^f9e0}g(=|$Si@(?X3BD^ znS|5lIO$+*6tmWhuS#1ihio*r)(iZ$d{Tk~Ke)(!V`K~hGF(Q~Of=JRFZ8&!m(^>P zsm9nvHE7MA5lEOuiU_I?#@(loL{c5Ui1>hHMF}~%LzoLRw1Tia(=QfK*T1!MWF(V) zZ9eeNhY$u#&lc+JQ1aXi3#KEBN%M^$K`c`E691&Q`Dt(O<3C&cusje~o$zn@;{+fB z&upcSclMEjJHreR_?k3;h{PUTm!_uh@O_$Eg!<0|j$lcg`luj1-Dxi&^mn4~-kK#e z0)3qPbK>ILzzS|TwWSl@5L^`+io*3U)UqUDUnx&+I zS%QOPL_YB|vVdUS)sSk*{^8!{f-D6QrjC0EPMqoBWEIny+CP!2lns-^ol4A5t}kiy z+S7=sZl(PsO~&1uR)w>Gx%C7U9K{^u;F&MuFb4EMx!;lXDMZf;>Zz96+XI>OUem!1 z;TV&AaAt0DkcogMRHKNc6~7FXBc9v8KI$VKr&*Su_Gf_h7<>M>Qt_V5>T}u^XP$eFc=@K|0#G}res>$H~ zL;vr7whr@Xvtp(5f)_;enoA)&VzZOgt;|H9U(!abk&y*&raM2Oalr>)m|?NA``Ku- zZtj5{v0BVZQ!Ym?qHzmyEQxFKiGS=R=_*8HKNBEx+OtDb*?(15)zzwQU_5;$ES~I1C9pe}#Tah6z)MBd(0wnyj~gkFkPNN^ zW>C5zPPkw8U;P$(ax98iAN)bqWDmfC;_d>Z; zK=&QNugxGi?9br-szpy-nEMkgWeZ-@80L;=JWNu^mTyT*l)ERkY6>p>@38!s3Y=Fd zlUddD+*C|!r(Ec_(arCI6*pYb*>boLRFm@CB)HIIII*|Q&{VN6IafE4(M(6MB=%e7 zJ6Qh3gH**-SM?R73>eEUcni9IHnsGVpRdY)GATE>FWJ%kzNNkltoUYA}k$?*>35J9>2p{uC)63pEV{9 z;}K6T_qNEm`Xpa4v)osFO*D5<1rtvituM6~Z#u%Y52kgG*&QSqx%r5tb16i!N7sb% zSgI5$?a%21N!;x;uF&F7A3|nMDL)W{^2D4vjlz=o6of9EQit3sOZV7PgI35O>96ql{vtZIHfDd(*fCcos9^e@Vu&2^oWn zF5%J4A&ibuDPb8Tfjfd9x*Z@Tq4TP>tS0J@&mmD)&2|EIc$AiGgj=P*Icoyy95;$O z+|KJX49bqWREce2hxZa*cleEAGNhpXKSZwAjUG_xstZl!q}t6{ddW}ZAT&HfbI0a! ze}dt~Ujry?EIv!|`!2Tx5^yccwTo$wLQhh!8>4dW#A$7}{l7WYm+CIJWi^b#)h2QTE)l zF-#6DO~2ak@FZ;6=rp>#e@iqx_aOB!z^VcHeV~b8ovqpIj4V1|FL;R=$wKE3YVqLU ziLxE?qn*>Hl_{NOh%hE?SxvH2bDZnxBbd$|n zX*1N`??Fw?-wJjd&co}B&YM6kZ#j$u30vOtVCb%_(?XGagz{@`#*3%7dnJtZX-6k7(yHa24RXr%>eoEFXu%u(_#KwOB_o_d=5Et;Os5n(ydq^ zIyLso<0H9v(U=|lp$q5F5MSCxNo-7IIovZ{Ew8Ay#hEyPxL7JjWOH;>D1ePoAJG}P z_G8OPvA3s7#VMT746qp_h*apHflc< zh%G9O?Jk@4;q4Ry%Rd9rEAbvx;?D$-^${Z@rz8Mu1Ns3&jm;SdzA7=6{m0^k#5nA9 zltv?kEmZ4EsL%lG}nC^|W-r_U-vp})z ztpg;CsQG&3lV1~B{$oQuYUOBA@T0{EytXvBRkS{H+c;TG;a!4o_FNvad2DdzQ4Tfm zmw@sMJ7uKR$a<}Ds~f`pb2QcZ^K{o#866!aMz7fogJ8eyfI(}laf^ohxu%^TP=QT< zG^zuXt-iZwpMPv9FfTgfq4Q(jJZxD+sXMW5d=X`9tcwc5^ulJZc6CjcvF1>f#p|^W zkCrG=qZ&n`8IaMJhF0>gr#Yh&)zUmAoxx49@^)j1@X);9xo)>1g=<+Lp?}aG2Bqz5 zjaUCB%U7sFY3DglP3f!ssYvUX`P$~xebYI#!Gp|T=ZC>%!+}Axk9{-&D{0(AjbB%W zQg2M2`F1buc=$I}x#`e{3!fm}rk@{u007|Qqb6N;>TVb-LKHdK)W`G*RUfh)BG3$A zvx?YfkEHxT=s97N$)&no^5icP{4C$My%hm)`Yu)ij4Q7~ZtObckO2vh#A5frrd_pU0dXE+z z_7o_d9e#ptjZi(^R6?+Wg+rcZkkquX_8ru1Up+xbXep#SUh-riohXn_Cg&K*d2#EZOd_0f3OY3h2KJn+8Y zA%pF@eaQj%{^};!+`@wGEMgcegG@kO2s7&U0lPF?KXwjSf6X(4W43~H=57UzY~3z%pK1smL7{9$F< zEnyxl;uySr3#yYej%b5~=5>hWBKu7@QHGU7)Ha<{7mCbjUD_1gWB*1o;YK}MxJDya zQ8;n2ms!|4_g!Vv48Ed}#TvT%!jb&b3M&m#v_g_~B#|bxi$|npJysQKIa@}f$>!N0 zUf=a&f5SB9e0Z~=0b+oIEmJ9r;smG0JMl{!`nkFCksL?zZ$c(6e=8{+(Yj!?xV^n{ z7ktao&rqM?Q)S57E*93D?TB4J9M-^`3V3~B{g-JK2F|LN_(hXpg@9oG3&{~1d-uO{ zqzBG&jgHT03gG6`)DafNliXyA3+cBN}Aw{}Jip^RrBFChrDszf`KG?g2mgc&rEP>!Yh z>9{e+6wt`+o->Tt)r&DSZBj$jh7Y(nj?9W$Nb&*o6yCa-cj2TqN%+M!IsYmGXCZ2n zn6o~Zo1o|9$yC4+rqPV3-w)=QR6>fqEI6jDuO1*r_F zRiDSQEt}JEDG>G$Fhy(O2PFxnwI4rK(UjRr`8t~<)P{B2oLTS znnnxv$y5V5lC+lo45|2Zuh8zBZwF;cH)?bWM+4XWGjCAr~%s^hAXNr3B?U1@I(?sF8U zzIe`4hdO(`%-`gsQCW01P4#i7sHG{=J@r5*={?7TNjc$ldPM(2l zN^Nk*MJv|>8CG$b&K&vQ&ISnDik8u-MNt|#G_V5qSy{f^XI=52^BiINWoU9R^U7*-?bhc>a3{Gr z8+TVy-U!#j710u@;5NU!4uy+Hy6Utv&EOZ6N!;x4GOhO3{5OTmt575%?-;o(6{Zt; zbYl6_s-EQ3{iIktQR`1 z4WwrqL^f=VwU2xSC$ePH)2^wp%(U@)3RjwHY=hNNr~KhBt|thcr^i+H%6J4TjaZ~Jfbdd#N6hi+y20V_MZ zK9-e+`bSQ9tx1fYj9Wj>a`sNQeu4-mnvX$b8hhKM!@57ycU6SfnNBUK4KZ}TM}gS; z;YiQ8SaCVI$3cfC80{|>lEr`0ej!&^=1tiLB1e9&z;~QtooQnGQtfyD$uRpJZpc`N zizVxvV$UGh{}PZwkh{_9?Aj>ahIm8{7BX8b*0{ynt_z)cDhbBrMuckUivz|Efrs3&6G>DeT!98_?j@NMTJ1s4w5dPsYhE zJatS%)ZF~?FTKNp&2dJ5qsUqO!vnU!P)Ipm?h4TBgI&XylR|4wJ_%u|ZfI655(`C= z13(deC)6)^-w%QO(o#4n@miB!b~NctluI1XWES`T4%|z!1ofhDq;n}=!h0FnqawjD93d{O zZ!G0cup{naXdL%i52-@u!5>dgZAPz$eRsQU#)pA9ANyH#Pfj3NwE0cE=`*LCW=H^@_k5>yL?q5nU*tKtj* 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 zcmb5SLv$`sw_uwuw(aD^wr$(CabnxHZJ*e-ZQHh!H~w4AcdP2wYOKa?%rVCtyR~Em zRb&K(2wCYE2xSFTXw58bOlb%eYz&>v>>X{n2xSF@CD~YL2!$O@4V_I*2<`2-2$fyz z2xSdD2niVp8Ckek7`PZ22$>id*$DrqLkR-(zXl*FivR=!Bn1Qn{0|EEojLl%IjEdTGO{}mPVe;!pC0!V7+f0_QD zqyGO)|MyYD7J#Vh!Snw|QUB+vqks~C3<~)3f3SHX|M{rZ>ErNMVyU^L&6OQ6pgxS82 z>Gf;b{hYq_^IoHV33|>)(_Z|3_FqT8dwYLdul?SzKld-~&n6#xHu7%Y{C;dQpCD-+%1ic7J2uYQNvve%$o0H{as^vj}=NTJQONo9z$%ewTY6O=ge!k>0yi0`O>?zCzp98z@^t`%L&=P6m{8E6vJsl%f|H{(5 zN;Hi{`!A1kAVX!geB97#(*P+xds1#c3QEhjY<+T_1VE(oz-ksc?J4ftQ&LCe`skcj z<};${fNF6)v<(kEl-TG=!_)d)!#gC*Tb=& z=4>k|40V0Q1DMEvuRsNiJz}}Gvv0T_&SjR~j|ulp`vPDzCiig;O#;8Dl*3lKH@!(! z(v9}V*3tBltB9%kb@f>>-Ic0sR-Wu^VR2s}>zGDO#b>t)NG>cjPY&npx41`~db{sV z8Z#m=>`qasgx$}U%wgz9`eR}$g9D&#zM%{SB#=#5p1~|5y58=2@ZC3Pc*6rop?_xL z%@!=Z-`4O~&+V*~sBNPQ5Czbk#mz#Wf>qNPTq(u8kKdj3?9R;ZpANn{n038E+?`-T z#r%w4P%jgZ9rHCjXKgc9ohNdM9>@wMWzDSd<*tFs7HSB{p)e~TvbS+99b*9L`sZ;W zt#wu!6v6~fU#z-XEB8J#H4V$XQ}YOh#fLs8TY>WFw|gz^1|T_lan25)8625zF})ZW zW-Q3tpD~J3mPuFs2e76FMXz4*dIqEQr~dU@1Mn!s_R~}MhNncJ^pGV)m?6ci#O!Z8 zo#)ND%pm@5$||b1XyGiA!v@*#RO}*S14c`eb@zoXh&gnFnip}~Gf1T7R6_nXpOAbt ztdnic!scWGW)#uU2(oc@tm^KuZC^}>EZmvjDS}Ahiam+dFZ1|{E&Z?-b6bhls(X3L z2g6V~wk4DTc-k|j(*R!+ppC{eW^b?gBDBB#F#d@ML5OPp{j;ob%V1E|ar6_Y(&K>! zZ|B4-CEOZk8v9X{RdYnC$`1cvy|OJL;uz8hq2a}yXd4xNM4o}VywA@(BcCu@ZFe|^ z=X>mY@3#5%tk`J~DzdX(E^2HdB)B~ugTr$!ONL_9v1>i0rmu+~hcLhNn#p1D=54!H zH&g?qf$@UDk3qqBKd}n$} z#3_$ePW~bl1t96CSu9**pjOq5JHDqc<`0xz`*f<|AElbG9SmJ0H)ri1-bW+Z zJ%LVubGesrR8+HUH%oTZXbLH{<;%;V)#BMe1Sc4@{3A2=F2vDj)c629r-Vk8?aQzE zC6LI@p~^`I^7K+CLkdr_r^*Q+E5v^hbSc|i2-$0-uOiEEDXiopXQR9g2h>3{edtw# z^mPxNf{ybF%y660Iq|7g(I``JD!#KzuMWVx3}I`yc&i>OyFtm>euq&tsv_f&7{qjgy8h@M!r%Ha9Jc zbnHeqhoqb1s}Qs)H6d?|07R^UL@x)!q(|O(U#;=oogbT=RxCqO=FF51xmK(&DHH!~><%X|EUp!FyWMLGCD{4< z{9e+z)h&P3cDPKY1q6h?3@@z@&oR44l;$rsuW7%=3GF%Z1H{3alRL}v53bNPz&4*M z3EW2rwq!>a;yEelg#$r866Q|+#=r_!0ibDG*#&%LYEvgCg@thf;%}wizyhP1wk*;V z!;T9cBk&6AMqdz>>>(XcmgJWjBmnMhTP^W{>Os$ladc?+E9j#zfOB5}!L<#in;;}C z*B8u3AOhW)4_ICW+T`ry@a1%2*mUIdht(C<#4^*{`#P`b29CNiL)nw`h6OkImLR-S zK;_wl%r}A2H%YiX%LoC3al;c~kJ|Id5bZEw`IFv0Os}O0hC`U5hx&bfo|5(KtCen=rJpXgtwGYg_srCo05XF~GdJoZ-OHl}J*zcT zo5$GqTCP0H!4gz@7>K#q+nMC^HQD>M==WYp|3k3#8n^p*m;Z)RH%)%2KkkW*61lRge1dB&VfaiR@2*Cr^wrgUJNx#I8fyR z${=s3g*e%h3H*g}z${1*$@-k_X;Jm`qMf)FrElKJS~)z?Mw$l>K&rY&iZ7j}oJ*CZ z9CSnmKXy$V0{#p;5AmodL6hKyy5-T71{b~)E^@t~0oxt!dv3yWmak4LE+uyIWN73$ zy%Vlkk*u%Upuy%5P^DyGSP;wm{up6sXK!V|L+_E}R2K3tIeI2Y{0yfZCew!#0>Jij z_-%H6bgd#4Sxb_kWWMiQ#rwy-cuk{ho^v$OKLjuX%6IpZt3Fx?*E}EXj$Ws%TbPz+ zIb7-}uzf8k4r7__6e`SZSR%!+Vm1{x>J-uBFDH$j00HEg61NoUPtBraW0T%-yY4cz zZR*wcHb)eN!Kf>Hw!jc6pJqaCorp~nA-9t8E$t?U?vk^knfgd-PL+ZWu$n-QJwqi` zZK4BLfR#dzp7N%Im=Lw92~T*2z3r61@$cW*@k)m7D2C9%Gnd~mV6Wgq(gEbKxw7HP zw*6KDc7d7J{2nmHUhwY6ZA9p_E*%3sl+BQ3r@Hor(WzS$O`!++N6M;aWnYP~Kraf< z_i{0m_(jWWrCmaWY(n|=ytrAg&ko*U+p}b;8nxm(c9wvpocvhi5|W_yFpo~rHD)Wz zPw6dYp=poS#J{X+d^7(U1yDGZ6PDl!sI0@P$Pyzg2zbDhsr6C& zpzXqe+x&6gNw14w(%q?K_yWcx)sAbtjd)U=hz&9ln-ERg-}7lS%e^7^20&+xQBm?; z4m@sitM(0yZqG8=5?$H+wno@SF3op@Cv=`LS-pJsc~8;rc7I8gR909bW^ir%P$txpd+!{88!fW8U5=-zLp!%%{+3S;G4Wzu<#TjXq zkIt!;;e?tcxfEa(;X6fw1J*ACrUcsB8(O50@K4(A5?BvAW`kp~^x|33BChWt zLqg#WW0t{4>iBWECpa488mRYBHOp9mRyFUBfoFt%Ng#*ZHzheg=i&*!I!9JWvdgGfaLij>Hi zz7v;WwT!<#7nE6Y#@^-`%8}j(}sr($1>SNr5}`1md%|o zz&5*vc2J2#XuQ6=lOO_le&Ns64Tsltmy038O=`Y{%=CiCg-)UclZnKaAG2k>ST&m zjpTHJ^#@8@gR5rV%$sk)wWte6BKlZ%L>{_GmyT43zHey05Asp-Y@%V2qL(HwMgc70 zs|(`-4pIh%6XBVjJU+Pnl_VX$;D)Dkh&2%FBh;W?7_>nCuW=i0sH)JWC!BUl(V}8j z*i)61&mIV-aop0(bOfmBT4e{&rdC4G)Y23!jOhVVN*5;z`T89(AzAHUX})nYr&@Oe z`9>od$b=4^sHFUkAOxJ(30zVba0vV}98g&JqIg~ed4_B^o=Mj`Cqjgjf4OmAYIoPC za7Q`A8F+eL`(y-k6?48$1k??=9@%8a948#08!Pr}8ue7^TpLo7%2S5!0geM>$M3SN zr>Uk7*mlUfJ7Yb&!kLv3$P6cvtE1p?aVgndrUcrbDb-Es;HkyixST?iB5Bs$p;A2c z%~v2`GYD55#P%qq2(}UO@pnrbB<51gY-43kbMEC;&tnwv8LY~SRf%4W1OEN zMRq8AXTI5Z`gZpd0kaLytNvZ4EJ)uvZ{*X*EOML9tR-`{Y8x}P*tw5UG~j%1x3~@< zj)P(0Ld8FSmS7{#Kjf+c{)O=vA7W77k$G=hg**TSsR_ohD;c|0C7(DH^BDCu2Dku$ zDBweas;vb%pPVgZ#ez9QVsvp+GXTq=LDUx*jvm_vQ(oW$DtYmw4MOvA2{90J6545s zB|QZ;z^pZd(dv1K@XmSSvpJT-J1wAg3H~4Ne(A*8#CDioR!M{N&BlphEVXQPU04zF`hc_)x%y=F&+v;g74FJ*V z4vo}tb97c3$_mVgji&iAvre_R&EQD#+j%p%&NPIoU?r~U5+_F*71)cWA;DzjI-3oG zm`sxpwA>fX(HQ72?)2bI`x|f`h zr2*9Q1O}6sRENb;J-q}cTckc0CM;ub$g=6!rh4NpFjEH&SHbOERjB9O#0W7VS~7bL ziPghl)f#QNHg!5(0e2BTNqrXIF&ov^;~Fgpt~N-RyyVP2vaXW?oxYM|t1ukGKo6*d*D{}JP6g&poRVV8DqcaBBP*7^ zFZS`+@is1AtV3S6T`Wp~XCBzap&F8@$s{xl;TWpWUN(yhn8#rhRO1N=#eY#6ObTe# zDiI1obz4$`i;FA%kqg{`>5r!p)RnJ(UH62_mA;f7y}d_D1luOp2Scqj!2{<0FrPi$ z*m2*bQ2A_)w*9-2vJ#DW6ihc5p}OMg8bS`+9PMTW%IqVC-qB5(yn{nmB3#1gN+~WV zKiBlIxzF$|^_NaZ$_a=F_d+#|V#+WV3=bHkhaVessy3N)V z?4is|tFF_v0Z;Id!M;gm(i!;jI`K=hqFyZ($ws`RxVD z{yrH&^}l9ZP&0d&3`&JH9G5`3qp~AS{84HzLahu%Cx0e)j{wYs{YSTanq@pOFmb4W zxN>hxO^pu1yb5v8r>u?hrmX9no2gF`Nela}I`Y5cdQHMO$Tn%&5A9^G!AbeSem9sN zYJBAX8p)%}eK{?(*8L&6`oUd+Vz>kciA{suVDT0KHnQRLyQd9KIGmB_hOawxn>7=V zJevymZfA3~PoYfZG1i~!E=x{!e}oI59Ono$Ks-rpi8y;dV9qXK9hJd4flZVGn62CM z^^bM!4%3jh4sQ1TEJzU|aIRz>Vc-KC^cXF@-44$8xJ7LFYl&Mkr@);{9xL$DV%?J7 zu;2-DKF;Ij5+t)oci(Wbr$^`-P*l{6r1&_~hTD+oS~)ac;7J@xh1bgrng5tHskYOW zFY?PwxT73-7^+Y|81T4p2PpzO*5HuhLb^qi`s@HR)h-0lo;4buFh&PG7O@?2nAZ`yEw?f~nsq`|7+<1qzyJM-%*5ca3V!`*<%} z3lJC*H+%v9+S%n2O?>3~(!izhi6WBHa0u`5hr>4iW|0$EZKyUTD+{_AL5(;$!j zR)E5WglBbtq~h5Nh`%MK{*htrA;*0v#|C>s3IkQJFOm0*dd53ZjK<}b*7^O+(sB2bZ|F61(q&735Xsu%rgDaEWMucn$)?!T@0^2`gCkOeD!Egb&R6PcX=pS}Uu}lvyKUkp@@Q zPzNDx2IQ&Gu@#vEU@L--r@JG*ng!Ujte){8-c=iN_3j=H!bv_vDLde=$~-PF5k^Yl zZL$VDVD@hh{M{E`h=8I@K8Y2Y0>~F^>bqHNe;3Bk3EAq_6lg;yvm!e$uW7rcY*x|r z1t|#RjJ@}aot+*3OQZQOa@n||Fb$uPDKuc}1eIyxf>e_Y!iPgK*3zEY!sH`Zx?#su zWg>&NY$CZ-!+YdSJFs|0C;Bzl++j9TG{eHoCLIG|mS-kW3T%xN@3=v8uP=+&zVBcW z%*I587SYI1(b610$?rI$cPJ{>G(Eib&6wZosti+~lbLmg-{v>v_wjqGZj4@w3~z(y zrIFq*Qa@R$@m#QsTa6a!mFI);irnh}^aEIl66?-%LVyD@6f(U*aY|@LskEe&Ihh&t zZ2Ua>nBeyJw^fX6|NRC%0H(6<;Q|Vn5vXI3qKH)Ko6V924`?kOsA+UiUgEs~wvc0< zjLLK_-=p0@0_@UO&*DaL(=#1IGyq{=J#`)rvLNZ+WYko7xz1D z?DsoKQ0w=m{N)z6-uqqj>+n69cW`_WTKqFe5b1H2?eg<-eCwA+-AbTv7Ym)bs&B`{ z=0hC51zS^TC0om&LnTRwz5CiUOVzVg`0h<@ly1zW{AMD)_Z`F$Yj2pq&5&RaftXDJUbc6bZK!!Xgv zvi%@Hd_<@|h!fQ>@*$IvJaYToBd5ge8pt%CdULD+>6{@cgkCP!U4;u0h4xoB<>SHA zH+0{9VfU@yfbhkUS6YiSE=_4}Kn+`2ag3eiy~V-bbtKaP%Fw1BihaEQ~Z3?eJkd6wOxKwGhCQ z#wbve1_IvQaHP-?Km$OHHrHOhJ^f+#oW(}G zvj)?<%-XHjmHhx&Z^7p}AWT0&nT0_{KYS~6{T03@>N3m_GNTQl7}boqJ&U5 znvyN7mKs|dgH@#5nZAj`xlJtd-Oc{$u!&%(ze(D8blo>c`+%+k6vfGQ&80lUt~mi`0cySkLe7Tw$B-KI&BlGw z_81hLcKQ_s4u=(cd@d#h7;tGR*g7J zNB|!)oh?yGG2NAAf1eM;^L22|K)1dqYC}-^2i8Q!-;-6Dq*TH}rD4pJp-nU9uIc+D zhJj9w0phEa25r{XzIrRTIahiprJ+s-Cw9 z@{tds^ak`Ft3i&Iy@J7XIDAaHg{^>i88&-j`-Vb2{RXhT_42B+4WSk+_7` zX!S5_nU}0h;a^~9MV3zj_Cwax8T#9m(6$ci0C%apOBp3-DnufoL)~3$UJB)N#WT_B z)BKRr;PLb1S*U5Kzl3gLL$cjdL`_oqe#zAms0I6G=}Jf6-vwpiGP67@BT)s??ela- zS2n=TTnio2laRFxZ$zMsyQ@B%v4;ygS8vIC@eVbjlDSr8 zgOS42PW{3u%$^3wuK-cEd+}JZcQRFH#K`URQ0q)C`tq?kw!!MHFdb6PnlBgLE)>6W zjtqEP!yDgTAfY|SkOC%n92QHiOc;OdOqu)cVbtp0^{4FFF(K8z97y0TFR!vXc9&y* zXUTBM6C>5w?nvXw_k^%W3t>_+afgsoMCS5*yJ^9!#@}ef&oArK6t}w~B=jv`kk&T! z^+aVDv03*#IR37O+CxI42CLjna^?)7nWVq7k$Vs13j!;Kf#ZZiQ#y1d8lR$`T(UMRv4Ex_F=<jjN-zn#qC%`f>w$X^Rmhwc>L}nUkZ>g7erGY+xTWN zz;zrB0YP6-gqL;of3)Vl05 z(e!6G{YlyjJd#vMJKd^SJ7QoKD>J3!mQ4HPC+ zG^I(3^{9n>GcT9e0wubsNSDKJ)eheA`APD^c@&F1=JBV~`fs%dGoipE7RY25L*HPY zWA$pZ+YY$AoJELh2%I&*TMRuQ)hcsqXV_=$H8pe(r!nHR`-nM|3p2PpEPVOcDez}D zy8z#p`&F?}6CN+@$Jkz8tY4;!9_wSB8h;Pz2x*@PrUz3>xPUazIUk(JjE)bcdG`;L zR9zpl;G;c%(^%YP82)>$-^cFrnBUi;{$&Ee{qEeK^{V#*aho$u-^&dy^KlA8&LFLK z4n;1emdeP;yBFxH*mCtqj_Jb`7s?4apl*`8%DjtsXlxbe>?z%K^a&n!1_k9-_HjHJ zR;+4MYEv73r9zO~wfuvfJgxo?CYla`LoaZjZM4>Rif-K^b05z0^=nuespectE_y ziKKNM{EX=CGZlw7QcrQ~pyP zEpofb&6y9FeWPbhZaiylH-{!j!Gc+dtMpxQyI`AjFzGZBH>GCE{&ngQo1*b*^izK< zU3Zu!?z%x#M@n6OAx&Tw#yG3;^>={S@)f1D-oh&&>t3$^TVYU-8lKDJ>?3bmc5ofw zg)?RtgT@&BJz8oHgMKT)#r*;*Z2;nxahgS^7t z8ly)b?j?S=xdIFFt`*jp(D3FeD0uRAA@XhI&=00(paiRl({$3^E6p7mb6CplC&e>eU!}3w0+}`Sh=gl(kybANzbNsg4 zlhQKMXoSdv@!y}Z!A;=wujmt)BJ9PK2Lmr3hLPkki!0>W_VMWuEa2R?_Y=_LP12%_)=+CED~3ImXriI2 z72P!e#EabmL^eBc(Y194V<5_T=)gU{i{B6njtK(@@4ytF`bn44Mmp6B?EgdZ1qb9t}M#XtWBB**o75dd9tRxdtWQK}o z!Y=>$TC*uHi}V^Jj8+j4swa>miSa737`C4*VK-^7|CDui4H0kTTOHERp?efR(V-o; zkG}-O3r?{pIrHX@AD%TAcuYq2RQQAzDPOKo2k4H4o}XWbE$w6~>io9LjnC+FX3g2M zfu-e-$GB3uJox$PRzY4~AbYa5h{izSBXDrW>_E7JqRWWlh+CiM&#;4iWZ{?Omt_1h zeE*B#1>K4Cv-1YomMtC_Hl$9F*K0rJ=$vNvv+qf_H*xC-w zi#J`W>l=pq4cFlLk~{~=*Mkj|iC;cIj(hxf|4Xz;#-fsKx%jopq2 zzC{P_`N==BNcFBW1l>WyCI3&c+-e7sKHlXBOFQ(gLeIFVN3H4I0kB)8X(AEoXQwY* zX_@Ihrfrn3G4D#?r|X)@PQ+lLvn)-mCfk{91>HddZEAWW*_i7Xu~-REq$O8&1R zF0$G|FUk>CHA_Tp!RA~o9wrD&>rAcZOk|(C^k$2GRyJK3-f7L)0x`hViAfIH{`krt zZVOhH(O}~P<> zJ(i>5BrjkbWULJyR&S;Ly=&Iqk0={<*!Zi9Me?JW3jBc04!NZq{RjCUa9U*M4WFW1 z_l%J85r__Gn|FARaUN(5h#IDA#r>Vs#P8?q?icItC(>`t?L7b6 zF3dwg`3`oUA!vG5zM?9P(6f-Ln)@?$@x9g6lBc>5oL5!Wk+c!6#qA%4rQ&Q`Lq`s# zhfSmq?rnG|nsup!6TAmf(E~f5Wm%|BxCN1DwJ%@CtkOOV|JQQ4h6G}5zR_2A5afM~ zeQTwVipURt$KR4%GoEO=RyC#AAPoKs9?qmw44z>=xxVW~ZVp_TBjEwM56_hJ*lFAJ zS2CzLzEFRul>oro+{mWhNgd#UITh(@ywSN%{u+Vqo-_{aufieEHd6Gs6}(uzH`j&= zinem|4o{a#J+3mJc@p%Y51*lR^(C=Dk>C&rJ?KefL0Aqr!OrV*{9lq)B0qQiJz4NP z0sV#Z;XCeWD%v<`p!T5c-eP9EDd#x8v~22ckYQ*_lgq(RN55nsV;=!|>>ldGu4aO+ z*&t1i5Mf8%Kd!5$Kmmth)3i1gE)buv9KVXj0G8hV9&=q$1kOjIG8wH)+Px+!x74HM z@OqPMi|xr_uEbidQ34(@gsGJ)LX3hit*%u*1Wbdg{l~-d8d}VO=&5NopF;Yx&)SgR z`+g-G-jDNb5~A+YIZC@?>?7SoCcx7h4yWn pxrTNyNmWm~7Z_pf} zy24$WgkAfQA4HEX#0;+O3);c4&xmS_kL$H8$h1Fwm<;O~jLG<#L%D2&17REqXY)r! za^{JsdY}a+yux?BkFn8ZW}D;vWoK`z`YpcmIs(I?zeR<#OZW71P_?~hh!+Aa?Luz< z$BQ`ns-7Y@vH|*zS!w71*Wfx$H$KrA2uWU4(R@@U$JiG*SS>`CF_2bgahT;;}lB3-5w13MQAv0ovCo{-ab@AzpG_-t6~}gsrWB0C6mrGA{L> z)u)dPHq(0DN3nFGoM{1kOi9%Oov!CRkz)g#2Y;2j5o|jA{GMZu9D!e?+9j0EbOqXEWr zPAovApUAWm^F+!FWuWj;r#V>{mX*;aJM2PqaR5+O-p9M#;KnK6+ZWuzSl8&~dc?ak z%7(Ab3zjiR%&1L(-9K4pWCTo)qB1|;Re&S4Zd;pS(IUX7KOA2Vzz<1@vrX|P2kmUB z8xkHVXljS$>#YSd#)C+eBR1JK(M#;P;xaoWMkhyOa91+l(vf$L#sVp)^qPKTkp$$U zeG1h~Cg480H!Z^^2+AikC1TSVk9Uz+hmX!V-};=%=i%e}68>&FoHd@|ReK%q=^~7G z%QXT|CpEGutpSi-YmzfhmO_4H_p%Y|4)s3AwrIpV+u@@pw@nI_PH0%sWWO@vPTw>( zkngLYO)U8{D=h#_KE{_C`ICk@qJxL_=eC{X?93uD*ru#>JF5&4UXu*D=JhBDWMQ1; zZ0?!+FmUirMGbs=lsF?2nHlyXfrIf*Ew{io9mZ+sX=o9x@iks0Rw%%+wi~4U$)it%9f=Z+8YNKGBK`A|P)^XcY>0QjI#HPDV0~tY#F&|x$ zgr6h#;FCkJ_FKHfvwQI#^bL?2EwjKThNiUmM3IL>Oki`R*-WD%vSO>$gIQW}B2H`H zeU6}y&$=^9MV7?LMW%)-WQQQUTHdA9i`iWBH8zPfIw>(3Dmg=zJ?<4Xf~YZjG? zNOZvZpX916Lr| zb85}Ze_{lxO9-2XO)hi0lHyWg<36SYD<=m?T8%pLdu5=%PnHqHEd9sGPdLJ$e+84> z0xBq7l1lPV;@)30lOQ!7OCxb?4`es)0n+JXxD+msh29gLh9s>`I80o=;a*C97OB5F zxC13!qQ7>OWbnYq!&x~{%ngpUL{%Q@?SV4BMYt7O+7aKg9hYvz}>{srouis9je;3#yJ|ZE2dekg-&b08VpkqZNpE zzm+qbesXXJ55d3&zP+iU#aE7<376rUSiR%KQ=fW0UldoDsR2@boBcCI{*oPg6hdaS zXlyph10Zk8BdCukkN_+)4w zb)2&4oS9fX^m-!V_!5vrFREM_fH^+Vr=;#NWjc=@FeL){m+_lSRte94h=`mX7Jlu0 zsJ+mg%5B63nubmJ9Geg9WuE`eJ=D^JfyL9sh+M`3*IW~`^qgh%&*u6uBYMhHph?Pw zZ)O=3h=sRrT-*R3Y`@<>`eDn3a}fWW4ja;nxN%s^clS+bPGhhLbQIEjq{|jl)8486 zlB2^A)_1)cVmGnd4L~+YW&J=!SaL=zfwlnISJIvcs+4sM&*0-Sq6v4m zDY+oeW>t{tY#AF>f)Ac|zBiUQy?KbNZHxb()ouWn@*Na~7j__%wPq@HRh3)>=@r>O z!LA*sQ0IQ^8bT*L|rL^EV_m=DTPixl)}evuy2+kZ13K zv!Esp3)wq$oE}ozJNV2Na86F;1g==qW^I`OS;u9n4cCWF+huK*;xPG(RwIzbgU4fh z<`S_oG(UwCfQyB-ot8#fw|GtL+uPD~`O$b8@b1GYi%IuT3)&^-pg5)w*~g)UIymhL zMFlRtvzc0~#6zM!O$V^%Cj(}|ywgMHkC1m|-=dBD>1wseOpx8YHkz@WwfG#Cse_)Y z6}V!g_kpp}ExHab5z(z!_4{|fXF6FDB&-Z2zT!u2;#wxCrDRvpi1`Ah z_v7nDBP&vQK2lo3y~sB*ywlux_wgFwt1#) z?VoM>g-1L-S?HgRTS+gipQYA10f7LfksBT&hDLf{B__@~Ak!p(D}z#2So$7Oyb`o^ z35FZww`H0QOZQ~p6>@G#U=c8{T&-JGe8F$=f%MXWcwG9F>Z|`FT**@?hIH8Mn!pKM zT}L~g(fc7HMf(yqRb33b&j;iDW$|$UFt@BAgPDMO3rClh-q#!gW|%(VM>NCApR@MR z8lprtR0~sDazAzndu~cz@b7YZTWMcD5^0Ua@QUVJg$DyW=gtmtKYcq@jB zuZDe2Z}9UfY`PvQ1NNbfajcjif$-hw@pgGiiqeelQX{UBdrDfz@m_MUJZv~%B#CYE z?kN^8(vC~>&9lvR%Vt!&R&A9*{CoH?VM(u3*MgohElHDz?o15U;1sBnp5wxF1T)mj z0iqK#%5pV}>)oNnm_uc0TF9q6Q5Md9j>ZuiZ}8hZ-nX!Ur^wzpXQM@g*~t`y5Ii?= z&ZCq=Y8vQS6N3d>DEzFu0tvSPE1quS^lI5ATY7m4%X~gg*wakVKTlS7$O`7dt;*IC zJQAp>955^RgE?k~VWSp6Mw|r6E@8RjwU&kL%^}BXBHNfy#mr(MRh*uTqVXoPd_L&s zW|3-{Lh@-VN9R~l0`(h7L-%xiXb|5QXlRiw-JIP+^CCka^2UdyI%g2pEUVN7^}=WQ zNc`_b`O|~{(gwZaKnj!=TUG^QJso@6d`0q77%HRZX3M@jN{ySFm}Y|-Z#(HP6(baH z)5cI4YpVTm6C?CI_XZneLOACFZ$b_ABCuq#gyh&aHhPn}pwysQJ|rpy3XEsd5~!6& zq5SD926{?m9(l<09~N>FfyAJr;{Qf{59;;4@gGubNd^D$I&ghnJALS}0yS50RYOg0#)=%QF;Iy7rnud11L}chRVS4Ed zFn4^jJg)ht1KCE4h$X!ee@ADEEY9BX_1(o^{W1KD6t_m|ip2-aU;Q@x8(puHdvsFf z(fdYlzrc6EycNxV^`GYu5ji{Qx53=rH~s^9_hQyzD2!MeA?a(!6lloZ%4!`=^odEH zc;%m&Dz$JsLj?q64@lgco{{XK@X}W0lvJ)5Ao9bJSyj0}|Cz>j^qo?6zloXK@ZMLEW zqZ3zB_-qZ5F-fI1RcOMTwUj3%ydT)O*zRucU%_3fB%}wU3 zn!-ptdwCzl72UN2-PcCcZ*Av`7cp{RJ9}w*)HimVK3vZiMC~|+8KjTrhW{NI!4Sm3 zBXikpa*Ot?lh^j%E_|DS*4WNv3&eo-s|Nm5=+>8S+b!^3v&< zkRE$1!-ICSFP3IdGC=N=DY+%?Hs)9fbDhBHL__9~HZQoyUAN(vdAP%p@LSU2Qlihf zqCoS0pHo}%LVRS8P)N7eesA-Gd3U)=3(wKTAh?#uesOQ}@LxTzN{O_fuacqY#iI~Z zP{3!bR}rQ^q2LUNPQ$|;&bX7th!V)JM@C`z(r+TXZ$-Ia8tW|yHYeYX4XK?HZERas z!m%dhwC_&??hT%gE!*xZ`W`kUE6jgWB3k1Y6+*$rhUE zk5fnNFnfR{gFaTz2Db%;kSJL!<|{ScoqI(eQHeCPB&^32H15piiU_`>!YUN zjdwYO3he4z#iQAxMaG*O~p8lsj>wyVsr|&a@9+aYw}KJf$!I>z3_FDfF?d z012M&3Ha+Pl6x!hGbPbz$q#+f(N>~kRZBlk9$B*@QQN%^$cBX95u#@)2A9#QgRs9S zh2CKmjD4))n1WTaxc~Rch*X?-Kz!o>J4^~_8It7oh*vGD0kVJm6&!_&udU){ZVsG#oEWI*c^eRe1w1S!D z``+eie#QaWEMiSw=T3Zz#)#$Ff=C9q>~hbHq2{aXO(kh4=NEl(DAh}?XIk7#Lfo+B zo@0@&yb<}d6vA*`B*MWTYIY_F?l3~F@#@xT(&}aRs`g&??$=hXPz@e9&JoZN3-q+Qq!_PBg8R(N$xdrlU?PE~X~W5U$!GdOqlBxw^nRBsj+Ps{(4s)U6qJ$bTx^BUs zIBcsmGsOVpeTC61|y{NsWJG@1svTstq%3stGhz%<#k$C#G~6dy|P# zc`~~7Tai_8l`4H`o$U;E(-2TM_q{$Mt~MIeE*_Px?Q|m6HARYKFpSplNhP5k#>I{?&A=j@hpQn>@bwMT`1t` z(^|Qq?>V=bS?W3|pXOYa%;!|3VM@^?fbX|RxDhgr%hit%d}DN(O*fdkWQim6&0Lsr zh$rD`(WJ?sV9jH+<8B?R&llf?(s7d#ZPeS$u|}+2aJIUoi?na<2vzSplt#1OEEE-R z#i93?$C_ua216nJ0p>SUmrr(baQJJ0c7=?T?br^6aGvCsV>#jSF&9Ki2 z4&;Vydn>jewDf5455)u6Y+906QuM-K%)nS5&##g-F7fwaHkb1A950h@xazBW7aAW%-@F}DeO$W35MK;XQ|6U6j3yIVEg7Z>`<^^;dozAQ7 z4IMrpZa15eI=Jh%$<{N7H=sk9RSF_x8Z~RL#H-nAS7@&pDkGsG^{tte*a@0ZniM4L zgRpWNE%kPi@($l*GCdrC1hLM-=QO}i^8nFG*`Jl5-&1f}IDhGRPfUHq;NK+8zAW0J zh=>IfKbm(iW3l?}SOquJqaVEESonlt!Tr1nc@)4tb0IxF%tcAGxhZ^+w`o%Jdiv;1EJwrbE$#xJYu#1gVm}t_{!~6WAR#;Q$#T#Vu3@ zajM){#3z}lUd}pm+c0r?YQ?K-RmX`u3AGe$!eGIPe99lWGsv46uUZB(sN?l5IN)K^ zS@HIw>?E_T^;6xDQ!C@IYdz9T?@sI)XK=K94im7(+=P!sNMPeXD{}`o(_II6@d1)H z3BF$QzvRRVj{o=bPu5{&#VCUGmeb3Q;bl4=aM9Cw@?+XlNDfymFcC40a+Gk5lMSP@ z=X^<;Rt8EdTNKt=>~3Pw8_p^?GkHv9c_H`Y8SLdX@hg5M+gZpVydL}5Jfzi_!rr!fe dKNA-H$o7HkXNB@Xw6C*&Fw7hf0068p8;@a2E*t;= diff --git a/EKF/matlab/scripts/Inertial Nav EKF/EulToQuat.m b/EKF/matlab/scripts/Inertial Nav EKF/EulToQuat.m deleted file mode 100644 index 9b57a20b5d..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/EulToQuat.m +++ /dev/null @@ -1,23 +0,0 @@ -function quaterion = EulToQuat(Euler) - -% Convert from a 321 Euler rotation sequence specified in radians to a -% Quaternion - -quaterion = zeros(4,1); - -Euler = Euler * 0.5; -cosPhi = cos(Euler(1)); -sinPhi = sin(Euler(1)); -cosTheta = cos(Euler(2)); -sinTheta = sin(Euler(2)); -cosPsi = cos(Euler(3)); -sinPsi = sin(Euler(3)); - -quaterion(1,1) = (cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi); -quaterion(2,1) = (sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi); -quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi); -quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi); - -return; - - diff --git a/EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m deleted file mode 100644 index 9eec7cb79f..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m +++ /dev/null @@ -1,519 +0,0 @@ -% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run - -% Derivation of Navigation EKF using a local NED earth Tangent Frame and -% XYZ body fixed frame -% Sequential fusion of velocity and position measurements -% Fusion of true airspeed -% Sequential fusion of magnetic flux measurements -% 24 state architecture. -% IMU data is assumed to arrive at a constant rate with a time step of dt -% IMU delta angle and velocity data are used as control inputs, -% not observations - -% Author: Paul Riseborough - -% State vector: -% attitude quaternion -% Velocity - m/sec (North, East, Down) -% Position - m (North, East, Down) -% Delta Angle bias - rad (X,Y,Z) -% Delta Velocity bias - m/s (X,Y,Z) -% Earth Magnetic Field Vector - (North, East, Down) -% Body Magnetic Field Vector - (X,Y,Z) -% Wind Vector - m/sec (North,East) - -% Observations: -% NED velocity - m/s -% NED position - m -% True airspeed - m/s -% angle of sideslip - rad -% XYZ magnetic flux - -% Time varying parameters: -% XYZ delta angle measurements in body axes - rad -% XYZ delta velocity measurements in body axes - m/sec - - -%% define symbolic variables and constants -clear all; -reset(symengine); -syms dax day daz real % IMU delta angle measurements in body axes - rad -syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec -syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED -syms vn ve vd real % NED velocity - m/sec -syms pn pe pd real % NED position - m -syms dax_b day_b daz_b real % delta angle bias - rad -syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec -syms dt real % IMU time step - sec -syms gravity real % gravity - m/sec^2 -syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances -syms vwn vwe real; % NE wind velocity - m/sec -syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss -syms magN magE magD real; % NED earth fixed magnetic field components - milligauss -syms R_VN R_VE R_VD real % variances for NED velocity measurements - (m/sec)^2 -syms R_PN R_PE R_PD real % variances for NED position measurements - m^2 -syms R_TAS real % variance for true airspeed measurement - (m/sec)^2 -syms R_MAG real % variance for magnetic flux measurements - milligauss^2 -syms R_BETA real % variance of sidelsip measurements rad^2 -syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2 -syms ptd real % location of terrain in D axis -syms decl real; % earth magnetic field declination from true north -syms R_DECL R_YAW real; % variance of declination or yaw angle observation -syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes -syms rho real % air density (kg/m^3) -syms R_ACC real % variance of accelerometer measurements (m/s^2)^2 -syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt component of true airspeed along each axis (1/s) - -%% define the state prediction equations - -% define the measured Delta angle and delta velocity vectors -dAngMeas = [dax; day; daz]; -dVelMeas = [dvx; dvy; dvz]; - -% define the IMU bias errors and scale factor -dAngBias = [dax_b; day_b; daz_b]; -dVelBias = [dvx_b; dvy_b; dvz_b]; - -% 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 truth delta angle -% ignore coning compensation as these effects are negligible in terms of -% covariance growth for our application and grade of sensor -dAngTruth = dAngMeas - dAngBias; - -% Define the truth delta velocity -ignore sculling and transport rate -% corrections as these negligible are in terms of covariance growth for our -% application and grade of sensor -dVelTruth = dVelMeas - dVelBias; - -% 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*dAngTruth(1); - 0.5*dAngTruth(2); - 0.5*dAngTruth(3); - ]; -quatNew = QuatMult(quat,deltaQuat); - -% define the velocity update equations -% ignore coriolis terms for linearisation purposes -vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth; - -% define the position update equations -pNew = [pn;pe;pd] + [vn;ve;vd]*dt; - -% define the IMU error update equations -dAngBiasNew = dAngBias; -dVelBiasNew = dVelBias; - -% define the wind velocity update equations -vwnNew = vwn; -vweNew = vwe; - -% define the earth magnetic field update equations -magNnew = magN; -magEnew = magE; -magDnew = magD; - -% define the body magnetic field update equations -magXnew = magX; -magYnew = magY; -magZnew = magZ; - -% Define the state vector & number of states -stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe]; -nStates=numel(stateVector); - -% Define vector of process equations -newStateVector = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew]; - -% 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, [dAngMeas;dVelMeas]); -[G,SG]=OptimiseAlgebra(G,'SG'); - -% derive the state error matrix -distMatrix = diag([daxVar dayVar dazVar dvxVar dvyVar dvzVar]); -Q = G*distMatrix*transpose(G); -[Q,SQ]=OptimiseAlgebra(Q,'SQ'); - -% Derive the predicted covariance matrix using the standard equation -PP = F*P*transpose(F) + Q; - -% Collect common expressions to optimise processing -[PP,SPP]=OptimiseAlgebra(PP,'SPP'); - -save('StateAndCovariancePrediction.mat'); -clear all; -reset(symengine); - -%% derive equations for fusion of true airspeed measurements -load('StatePrediction.mat'); -VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement -H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian -[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing -K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS); -[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector - -% save equations and reset workspace -save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS'); -clear all; -reset(symengine); - -%% derive equations for fusion of angle of sideslip measurements -load('StatePrediction.mat'); - -% calculate wind relative velocities in nav frame and rotate into body frame -Vbw = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; -% calculate predicted angle of sideslip using small angle assumption -BetaPred = Vbw(2)/Vbw(1); -H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian -[H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing -K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector - -% save equations and reset workspace -save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA'); -clear all; -reset(symengine); - -%% derive equations for fusion of magnetic field measurement -load('StatePrediction.mat'); - -magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement -H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian -[H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG'); - -K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector -[K_MX,SK_MX]=OptimiseAlgebra(K_MX,'SK_MX'); -K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); % Kalman gain vector -[K_MY,SK_MY]=OptimiseAlgebra(K_MY,'SK_MY'); -K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector -[K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ'); - -% save equations and reset workspace -save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ'); -clear all; -reset(symengine); - -%% derive equations for sequential fusion of optical flow measurements -load('StatePrediction.mat'); - -% Range is defined as distance from camera focal point to object measured -% along sensor Z axis -syms range real; - -% Define rotation matrix from body to sensor frame -syms Tbs_a_x Tbs_a_y Tbs_a_z real; -syms Tbs_b_x Tbs_b_y Tbs_b_z real; -syms Tbs_c_x Tbs_c_y Tbs_c_z real; -Tbs = [ ... - Tbs_a_x Tbs_a_y Tbs_a_z ; ... - Tbs_b_x Tbs_b_y Tbs_b_z ; ... - Tbs_c_x Tbs_c_y Tbs_c_z ... - ]; - -% Calculate earth relative velocity in a non-rotating sensor frame -relVelSensor = Tbs * transpose(Tbn) * [vn;ve;vd]; - -% Divide by range to get predicted angular LOS rates relative to X and Y -% axes. Note these are rates in a non-rotating sensor frame -losRateSensorX = +relVelSensor(2)/range; -losRateSensorY = -relVelSensor(1)/range; - -save('temp1.mat','losRateSensorX','losRateSensorY'); - -clear all; -reset(symengine); -load('StatePrediction.mat'); -load('temp1.mat'); - -% calculate the observation Jacobian and Kalman gain for the X axis -H_LOSX = jacobian(losRateSensorX,stateVector); % measurement Jacobian -H_LOSX = simplify(H_LOSX); -K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector -K_LOSX = simplify(K_LOSX); -save('temp2.mat','H_LOSX','K_LOSX'); -ccode([H_LOSX;transpose(K_LOSX)],'file','LOSX.c'); -fix_c_code('LOSX.c'); - -clear all; -reset(symengine); -load('StatePrediction.mat'); -load('temp1.mat'); - -% calculate the observation Jacobian for the Y axis -H_LOSY = jacobian(losRateSensorY,stateVector); % measurement Jacobian -H_LOSY = simplify(H_LOSY); -K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector -K_LOSY = simplify(K_LOSY); -save('temp3.mat','H_LOSY','K_LOSY'); -ccode([H_LOSY;transpose(K_LOSY)],'file','LOSY.c'); -fix_c_code('LOSY.c'); - -% reset workspace -clear all; -reset(symengine); - -%% derive equations for sequential fusion of body frame velocity measurements -load('StatePrediction.mat'); - -% body frame velocity observations -syms velX velY velZ real; - -% velocity observation variance -syms R_VEL real; - -% calculate relative velocity in body frame -relVelBody = transpose(Tbn)*[vn;ve;vd]; - -save('temp1.mat','relVelBody','R_VEL'); - -% calculate the observation Jacobian for the X axis -H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian -H_VELX = simplify(H_VELX); -save('temp2.mat','H_VELX'); -ccode(H_VELX,'file','H_VELX.c'); -fix_c_code('H_VELX.c'); - -clear all; -reset(symengine); -load('StatePrediction.mat'); -load('temp1.mat'); - -% calculate the observation Jacobian for the Y axis -H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian -H_VELY = simplify(H_VELY); -save('temp3.mat','H_VELY'); -ccode(H_VELY,'file','H_VELY.c'); -fix_c_code('H_VELY.c'); - -clear all; -reset(symengine); -load('StatePrediction.mat'); -load('temp1.mat'); - -% calculate the observation Jacobian for the Z axis -H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian -H_VELZ = simplify(H_VELZ); -save('temp4.mat','H_VELZ'); -ccode(H_VELZ,'file','H_VELZ.c'); -fix_c_code('H_VELZ.c'); - -clear all; -reset(symengine); - -% calculate Kalman gain vector for the X axis -load('StatePrediction.mat'); -load('temp1.mat'); -load('temp2.mat'); - -K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector -K_VELX = simplify(K_VELX); -ccode(K_VELX,'file','K_VELX.c'); -fix_c_code('K_VELX.c'); - -clear all; -reset(symengine); - -% calculate Kalman gain vector for the Y axis -load('StatePrediction.mat'); -load('temp1.mat'); -load('temp3.mat'); - -K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector -K_VELY = simplify(K_VELY); -ccode(K_VELY,'file','K_VELY.c'); -fix_c_code('K_VELY.c'); - -clear all; -reset(symengine); - -% calculate Kalman gain vector for the Z axis -load('StatePrediction.mat'); -load('temp1.mat'); -load('temp4.mat'); - -K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector -K_VELZ = simplify(K_VELZ); -ccode(K_VELZ,'file','K_VELZ.c'); -fix_c_code('K_VELZ.c'); - -% reset workspace -clear all; -reset(symengine); - -% calculate Kalman gains vectors for X,Y,Z to take advantage of common -% terms -load('StatePrediction.mat'); -load('temp1.mat'); -load('temp2.mat'); -load('temp3.mat'); -load('temp4.mat'); -K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector -K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector -K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector -K_VEL = simplify([K_VELX,K_VELY,K_VELZ]); -ccode(K_VEL,'file','K_VEL.c'); -fix_c_code('K_VEL.c'); - - -%% derive equations for fusion of 321 sequence yaw measurement -load('StatePrediction.mat'); - -% Calculate the yaw (first rotation) angle from the 321 rotation sequence -% Provide alternative angle that avoids singularity at +-pi/2 -angMeasA = atan(Tbn(2,1)/Tbn(1,1)); -angMeasB = pi/2 - atan(Tbn(1,1)/Tbn(2,1)); -H_YAW321 = jacobian([angMeasA;angMeasB],stateVector); % measurement Jacobian -H_YAW321 = simplify(H_YAW321); -ccode(H_YAW321,'file','calcH_YAW321.c'); -fix_c_code('calcH_YAW321.c'); - -% reset workspace -clear all; -reset(symengine); - -%% derive equations for fusion of 312 sequence yaw measurement -load('StatePrediction.mat'); - -% Calculate the yaw (first rotation) angle from an Euler 312 sequence -% Provide alternative angle that avoids singularity at +-pi/2 -angMeasA = atan(-Tbn(1,2)/Tbn(2,2)); -angMeasB = pi/2 - atan(-Tbn(2,2)/Tbn(1,2)); -H_YAW312 = jacobian([angMeasA;angMeasB],stateVector); % measurement Jacobian -H_YAW312 = simplify(H_YAW312); -ccode(H_YAW312,'file','calcH_YAW312.c'); -fix_c_code('calcH_YAW312.c'); - -% reset workspace -clear all; -reset(symengine); - -%% derive equations for fusion of dual antenna yaw measurement -load('StatePrediction.mat'); - -syms ant_yaw real; % yaw angle of antenna array axis wrt X body axis - -% define antenna vector in body frame -ant_vec_bf = [cos(ant_yaw);sin(ant_yaw);0]; - -% rotate into earth frame -ant_vec_ef = Tbn * ant_vec_bf; - -% Calculate the yaw angle from the projection -angMeas = atan(ant_vec_ef(2)/ant_vec_ef(1)); - -H_YAWGPS = jacobian(angMeas,stateVector); % measurement Jacobian -H_YAWGPS = simplify(H_YAWGPS); -ccode(H_YAWGPS,'file','calcH_YAWGPS.c'); -fix_c_code('calcH_YAWGPS.c'); - -% reset workspace -clear all; -reset(symengine); - -%% derive equations for fusion of declination -load('StatePrediction.mat'); - -% the predicted measurement is the angle wrt magnetic north of the horizontal -% component of the measured field -angMeas = atan(magE/magN); -H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian -H_MAGD = simplify(H_MAGD); -K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL); -K_MAGD = simplify(K_MAGD); -ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c'); -fix_c_code('calcMAGD.c'); - -% reset workspace -clear all; -reset(symengine); - -%% derive equations for fusion of lateral body acceleration (multirotors only) -load('StatePrediction.mat'); - -% use relationship between airspeed along the X and Y body axis and the -% drag to predict the lateral acceleration for a multirotor vehicle type -% where propulsion forces are generated primarily along the Z body axis - -vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity - -% calculate drag assuming flight along axis in positive direction -% sign change will be looked after in implementation rather than by adding -% sign functions to symbolic derivation which genererates output with dirac -% functions -% accXpred = -0.5*rho*vrel(1)*vrel(1)*BCXinv; % predicted acceleration measured along X body axis -% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis - -% Use a simple viscous drag model for the linear estimator equations -% Use the the derivative from speed to acceleration averaged across the -% speed range -% The nonlinear equation will be used to calculate the predicted -% measurement in implementation -accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis -accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis - -% Derive observation Jacobian and Kalman gain matrix for X accel fusion -H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian -H_ACCX = simplify(H_ACCX); -[H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing -K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC); -[K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector - -% Derive observation Jacobian and Kalman gain matrix for Y accel fusion -H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian -H_ACCY = simplify(H_ACCY); -[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing -K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC); -[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector - -% save equations and reset workspace -save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY'); -clear all; -reset(symengine); - -%% Save output and convert to m and c code fragments - -% load equations for predictions and updates -load('StateAndCovariancePrediction.mat'); -load('Airspeed.mat'); -load('Sideslip.mat'); -load('Magnetometer.mat'); -load('Drag.mat'); - -fileName = strcat('SymbolicOutput',int2str(nStates),'.mat'); -save(fileName); -SaveScriptCode(nStates); -ConvertToM(nStates); % convert symbolic expressions to Matlab expressions -ConvertToC(nStates); % convert Matlab expressions to C code expressions -ConvertCtoC(nStates); % convert covariance matrix expressions from array to matrix syntax \ No newline at end of file diff --git a/EKF/matlab/scripts/Inertial Nav EKF/H_LOSX.c b/EKF/matlab/scripts/Inertial Nav EKF/H_LOSX.c deleted file mode 100644 index e2223cc9e4..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/H_LOSX.c +++ /dev/null @@ -1,8 +0,0 @@ -t2 = 1.0/range; -A0[0][0] = t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0); -A0[0][1] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0); -A0[0][2] = t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0); -A0[0][3] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0); -A0[0][4] = -t2*(q0*q3*2.0-q1*q2*2.0); -A0[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3); -A0[0][6] = t2*(q0*q1*2.0+q2*q3*2.0); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/H_LOSY.c b/EKF/matlab/scripts/Inertial Nav EKF/H_LOSY.c deleted file mode 100644 index 306eed204b..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/H_LOSY.c +++ /dev/null @@ -1,8 +0,0 @@ -t2 = 1.0/range; -A0[0][0] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0); -A0[0][1] = -t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0); -A0[0][2] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0); -A0[0][3] = -t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0); -A0[0][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3); -A0[0][5] = -t2*(q0*q3*2.0+q1*q2*2.0); -A0[0][6] = t2*(q0*q2*2.0-q1*q3*2.0); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/H_VELX.c b/EKF/matlab/scripts/Inertial Nav EKF/H_VELX.c deleted file mode 100644 index bb6df65746..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/H_VELX.c +++ /dev/null @@ -1,7 +0,0 @@ -H_VEL[0] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f; -H_VEL[1] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f; -H_VEL[2] = q0*vd*-2.0f+q1*ve*2.0f-q2*vn*2.0f; -H_VEL[3] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f; -H_VEL[4] = q0*q0+q1*q1-q2*q2-q3*q3; -H_VEL[5] = q0*q3*2.0f+q1*q2*2.0f; -H_VEL[6] = q0*q2*-2.0f+q1*q3*2.0f; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/H_VELY.c b/EKF/matlab/scripts/Inertial Nav EKF/H_VELY.c deleted file mode 100644 index 2b79748c65..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/H_VELY.c +++ /dev/null @@ -1,7 +0,0 @@ -H_VEL[0] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f; -H_VEL[1] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f; -H_VEL[2] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f; -H_VEL[3] = q2*vd*2.0f-q3*ve*2.0f-q0*vn*2.0f; -H_VEL[4] = q0*q3*-2.0f+q1*q2*2.0f; -H_VEL[5] = q0*q0-q1*q1+q2*q2-q3*q3; -H_VEL[6] = q0*q1*2.0f+q2*q3*2.0f; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/H_VELZ.c b/EKF/matlab/scripts/Inertial Nav EKF/H_VELZ.c deleted file mode 100644 index ed7c569072..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/H_VELZ.c +++ /dev/null @@ -1,7 +0,0 @@ -H_VEL[0] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f; -H_VEL[1] = q1*vd*-2.0f-q0*ve*2.0f+q3*vn*2.0f; -H_VEL[2] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f; -H_VEL[3] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f; -H_VEL[4] = q0*q2*2.0f+q1*q3*2.0f; -H_VEL[5] = q0*q1*-2.0f+q2*q3*2.0f; -H_VEL[6] = q0*q0-q1*q1-q2*q2+q3*q3; diff --git a/EKF/matlab/scripts/Inertial Nav EKF/K_LOSX.c b/EKF/matlab/scripts/Inertial Nav EKF/K_LOSX.c deleted file mode 100644 index 5044d0cc6e..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/K_LOSX.c +++ /dev/null @@ -1,117 +0,0 @@ -t2 = 1.0/range; -t3 = q1*vd*2.0; -t4 = q0*ve*2.0; -t11 = q3*vn*2.0; -t5 = t3+t4-t11; -t6 = q0*q3*2.0; -t29 = q1*q2*2.0; -t7 = t6-t29; -t8 = q0*q1*2.0; -t9 = q2*q3*2.0; -t10 = t8+t9; -t12 = P[0][0]*t2*t5; -t13 = q0*vd*2.0; -t14 = q2*vn*2.0; -t28 = q1*ve*2.0; -t15 = t13+t14-t28; -t16 = q3*vd*2.0; -t17 = q2*ve*2.0; -t18 = q1*vn*2.0; -t19 = t16+t17+t18; -t20 = q3*ve*2.0; -t21 = q0*vn*2.0; -t30 = q2*vd*2.0; -t22 = t20+t21-t30; -t23 = q0*q0; -t24 = q1*q1; -t25 = q2*q2; -t26 = q3*q3; -t27 = t23-t24+t25-t26; -t31 = P[1][1]*t2*t15; -t32 = P[6][0]*t2*t10; -t33 = P[1][0]*t2*t15; -t34 = P[2][0]*t2*t19; -t35 = P[5][0]*t2*t27; -t79 = P[4][0]*t2*t7; -t80 = P[3][0]*t2*t22; -t36 = t12+t32+t33+t34+t35-t79-t80; -t37 = t2*t5*t36; -t38 = P[6][1]*t2*t10; -t39 = P[0][1]*t2*t5; -t40 = P[2][1]*t2*t19; -t41 = P[5][1]*t2*t27; -t81 = P[4][1]*t2*t7; -t82 = P[3][1]*t2*t22; -t42 = t31+t38+t39+t40+t41-t81-t82; -t43 = t2*t15*t42; -t44 = P[6][2]*t2*t10; -t45 = P[0][2]*t2*t5; -t46 = P[1][2]*t2*t15; -t47 = P[2][2]*t2*t19; -t48 = P[5][2]*t2*t27; -t83 = P[4][2]*t2*t7; -t84 = P[3][2]*t2*t22; -t49 = t44+t45+t46+t47+t48-t83-t84; -t50 = t2*t19*t49; -t51 = P[6][3]*t2*t10; -t52 = P[0][3]*t2*t5; -t53 = P[1][3]*t2*t15; -t54 = P[2][3]*t2*t19; -t55 = P[5][3]*t2*t27; -t85 = P[4][3]*t2*t7; -t86 = P[3][3]*t2*t22; -t56 = t51+t52+t53+t54+t55-t85-t86; -t57 = P[6][5]*t2*t10; -t58 = P[0][5]*t2*t5; -t59 = P[1][5]*t2*t15; -t60 = P[2][5]*t2*t19; -t61 = P[5][5]*t2*t27; -t88 = P[4][5]*t2*t7; -t89 = P[3][5]*t2*t22; -t62 = t57+t58+t59+t60+t61-t88-t89; -t63 = t2*t27*t62; -t64 = P[6][4]*t2*t10; -t65 = P[0][4]*t2*t5; -t66 = P[1][4]*t2*t15; -t67 = P[2][4]*t2*t19; -t68 = P[5][4]*t2*t27; -t90 = P[4][4]*t2*t7; -t91 = P[3][4]*t2*t22; -t69 = t64+t65+t66+t67+t68-t90-t91; -t70 = P[6][6]*t2*t10; -t71 = P[0][6]*t2*t5; -t72 = P[1][6]*t2*t15; -t73 = P[2][6]*t2*t19; -t74 = P[5][6]*t2*t27; -t93 = P[4][6]*t2*t7; -t94 = P[3][6]*t2*t22; -t75 = t70+t71+t72+t73+t74-t93-t94; -t76 = t2*t10*t75; -t87 = t2*t22*t56; -t92 = t2*t7*t69; -t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; -t78 = 1.0/t77; -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); -A0[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); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/K_LOSY.c b/EKF/matlab/scripts/Inertial Nav EKF/K_LOSY.c deleted file mode 100644 index 8abde33643..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/K_LOSY.c +++ /dev/null @@ -1,117 +0,0 @@ -t2 = 1.0/range; -t3 = q3*ve*2.0; -t4 = q0*vn*2.0; -t11 = q2*vd*2.0; -t5 = t3+t4-t11; -t6 = q0*q3*2.0; -t7 = q1*q2*2.0; -t8 = t6+t7; -t9 = q0*q2*2.0; -t28 = q1*q3*2.0; -t10 = t9-t28; -t12 = P[0][0]*t2*t5; -t13 = q3*vd*2.0; -t14 = q2*ve*2.0; -t15 = q1*vn*2.0; -t16 = t13+t14+t15; -t17 = q0*vd*2.0; -t18 = q2*vn*2.0; -t29 = q1*ve*2.0; -t19 = t17+t18-t29; -t20 = q1*vd*2.0; -t21 = q0*ve*2.0; -t30 = q3*vn*2.0; -t22 = t20+t21-t30; -t23 = q0*q0; -t24 = q1*q1; -t25 = q2*q2; -t26 = q3*q3; -t27 = t23+t24-t25-t26; -t31 = P[1][1]*t2*t16; -t32 = P[5][0]*t2*t8; -t33 = P[1][0]*t2*t16; -t34 = P[3][0]*t2*t22; -t35 = P[4][0]*t2*t27; -t80 = P[6][0]*t2*t10; -t81 = P[2][0]*t2*t19; -t36 = t12+t32+t33+t34+t35-t80-t81; -t37 = t2*t5*t36; -t38 = P[5][1]*t2*t8; -t39 = P[0][1]*t2*t5; -t40 = P[3][1]*t2*t22; -t41 = P[4][1]*t2*t27; -t82 = P[6][1]*t2*t10; -t83 = P[2][1]*t2*t19; -t42 = t31+t38+t39+t40+t41-t82-t83; -t43 = t2*t16*t42; -t44 = P[5][2]*t2*t8; -t45 = P[0][2]*t2*t5; -t46 = P[1][2]*t2*t16; -t47 = P[3][2]*t2*t22; -t48 = P[4][2]*t2*t27; -t79 = P[2][2]*t2*t19; -t84 = P[6][2]*t2*t10; -t49 = t44+t45+t46+t47+t48-t79-t84; -t50 = P[5][3]*t2*t8; -t51 = P[0][3]*t2*t5; -t52 = P[1][3]*t2*t16; -t53 = P[3][3]*t2*t22; -t54 = P[4][3]*t2*t27; -t86 = P[6][3]*t2*t10; -t87 = P[2][3]*t2*t19; -t55 = t50+t51+t52+t53+t54-t86-t87; -t56 = t2*t22*t55; -t57 = P[5][4]*t2*t8; -t58 = P[0][4]*t2*t5; -t59 = P[1][4]*t2*t16; -t60 = P[3][4]*t2*t22; -t61 = P[4][4]*t2*t27; -t88 = P[6][4]*t2*t10; -t89 = P[2][4]*t2*t19; -t62 = t57+t58+t59+t60+t61-t88-t89; -t63 = t2*t27*t62; -t64 = P[5][5]*t2*t8; -t65 = P[0][5]*t2*t5; -t66 = P[1][5]*t2*t16; -t67 = P[3][5]*t2*t22; -t68 = P[4][5]*t2*t27; -t90 = P[6][5]*t2*t10; -t91 = P[2][5]*t2*t19; -t69 = t64+t65+t66+t67+t68-t90-t91; -t70 = t2*t8*t69; -t71 = P[5][6]*t2*t8; -t72 = P[0][6]*t2*t5; -t73 = P[1][6]*t2*t16; -t74 = P[3][6]*t2*t22; -t75 = P[4][6]*t2*t27; -t92 = P[6][6]*t2*t10; -t93 = P[2][6]*t2*t19; -t76 = t71+t72+t73+t74+t75-t92-t93; -t85 = t2*t19*t49; -t94 = t2*t10*t76; -t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; -t78 = 1.0/t77; -A0[0][0] = -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); -A0[1][0] = -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); -A0[2][0] = -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); -A0[3][0] = -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); -A0[4][0] = -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); -A0[5][0] = -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); -A0[6][0] = -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); -A0[7][0] = -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); -A0[8][0] = -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); -A0[9][0] = -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); -A0[10][0] = -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); -A0[11][0] = -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); -A0[12][0] = -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); -A0[13][0] = -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); -A0[14][0] = -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); -A0[15][0] = -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); -A0[16][0] = -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); -A0[17][0] = -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); -A0[18][0] = -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); -A0[19][0] = -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); -A0[20][0] = -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); -A0[21][0] = -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); -A0[22][0] = -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); -A0[23][0] = -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/scripts/Inertial Nav EKF/K_VELX.c b/EKF/matlab/scripts/Inertial Nav EKF/K_VELX.c deleted file mode 100644 index 6dd6d862e5..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/K_VELX.c +++ /dev/null @@ -1,116 +0,0 @@ -float t2 = q0*q3*2.0f; -float t3 = q1*q2*2.0f; -float t4 = t2+t3; -float t5 = q0*q0; -float t6 = q1*q1; -float t7 = q2*q2; -float t8 = q3*q3; -float t9 = t5+t6-t7-t8; -float t10 = q0*q2*2.0f; -float t25 = q1*q3*2.0f; -float t11 = t10-t25; -float t12 = q3*ve*2.0f; -float t13 = q0*vn*2.0f; -float t26 = q2*vd*2.0f; -float t14 = t12+t13-t26; -float t15 = q3*vd*2.0f; -float t16 = q2*ve*2.0f; -float t17 = q1*vn*2.0f; -float t18 = t15+t16+t17; -float t19 = q0*vd*2.0f; -float t20 = q2*vn*2.0f; -float t27 = q1*ve*2.0f; -float t21 = t19+t20-t27; -float t22 = q1*vd*2.0f; -float t23 = q0*ve*2.0f; -float t28 = q3*vn*2.0f; -float t24 = t22+t23-t28; -float t29 = P[0][0]*t14; -float t30 = P[1][1]*t18; -float t31 = P[4][5]*t9; -float t32 = P[5][5]*t4; -float t33 = P[0][5]*t14; -float t34 = P[1][5]*t18; -float t35 = P[3][5]*t24; -float t79 = P[6][5]*t11; -float t80 = P[2][5]*t21; -float t36 = t31+t32+t33+t34+t35-t79-t80; -float t37 = t4*t36; -float t38 = P[4][6]*t9; -float t39 = P[5][6]*t4; -float t40 = P[0][6]*t14; -float t41 = P[1][6]*t18; -float t42 = P[3][6]*t24; -float t81 = P[6][6]*t11; -float t82 = P[2][6]*t21; -float t43 = t38+t39+t40+t41+t42-t81-t82; -float t44 = P[4][0]*t9; -float t45 = P[5][0]*t4; -float t46 = P[1][0]*t18; -float t47 = P[3][0]*t24; -float t84 = P[6][0]*t11; -float t85 = P[2][0]*t21; -float t48 = t29+t44+t45+t46+t47-t84-t85; -float t49 = t14*t48; -float t50 = P[4][1]*t9; -float t51 = P[5][1]*t4; -float t52 = P[0][1]*t14; -float t53 = P[3][1]*t24; -float t86 = P[6][1]*t11; -float t87 = P[2][1]*t21; -float t54 = t30+t50+t51+t52+t53-t86-t87; -float t55 = t18*t54; -float t56 = P[4][2]*t9; -float t57 = P[5][2]*t4; -float t58 = P[0][2]*t14; -float t59 = P[1][2]*t18; -float t60 = P[3][2]*t24; -float t78 = P[2][2]*t21; -float t88 = P[6][2]*t11; -float t61 = t56+t57+t58+t59+t60-t78-t88; -float t62 = P[4][3]*t9; -float t63 = P[5][3]*t4; -float t64 = P[0][3]*t14; -float t65 = P[1][3]*t18; -float t66 = P[3][3]*t24; -float t90 = P[6][3]*t11; -float t91 = P[2][3]*t21; -float t67 = t62+t63+t64+t65+t66-t90-t91; -float t68 = t24*t67; -float t69 = P[4][4]*t9; -float t70 = P[5][4]*t4; -float t71 = P[0][4]*t14; -float t72 = P[1][4]*t18; -float t73 = P[3][4]*t24; -float t92 = P[6][4]*t11; -float t93 = P[2][4]*t21; -float t74 = t69+t70+t71+t72+t73-t92-t93; -float t75 = t9*t74; -float t83 = t11*t43; -float t89 = t21*t61; -float t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89; -float t77 = 1.0f/t76; -Kfusion[0] = t77*(t29+P[0][5]*t4+P[0][4]*t9-P[0][6]*t11+P[0][1]*t18-P[0][2]*t21+P[0][3]*t24); -Kfusion[1] = t77*(t30+P[1][5]*t4+P[1][4]*t9+P[1][0]*t14-P[1][6]*t11-P[1][2]*t21+P[1][3]*t24); -Kfusion[2] = t77*(-t78+P[2][5]*t4+P[2][4]*t9+P[2][0]*t14-P[2][6]*t11+P[2][1]*t18+P[2][3]*t24); -Kfusion[3] = t77*(t66+P[3][5]*t4+P[3][4]*t9+P[3][0]*t14-P[3][6]*t11+P[3][1]*t18-P[3][2]*t21); -Kfusion[4] = t77*(t69+P[4][5]*t4+P[4][0]*t14-P[4][6]*t11+P[4][1]*t18-P[4][2]*t21+P[4][3]*t24); -Kfusion[5] = t77*(t32+P[5][4]*t9+P[5][0]*t14-P[5][6]*t11+P[5][1]*t18-P[5][2]*t21+P[5][3]*t24); -Kfusion[6] = t77*(-t81+P[6][5]*t4+P[6][4]*t9+P[6][0]*t14+P[6][1]*t18-P[6][2]*t21+P[6][3]*t24); -Kfusion[7] = t77*(P[7][5]*t4+P[7][4]*t9+P[7][0]*t14-P[7][6]*t11+P[7][1]*t18-P[7][2]*t21+P[7][3]*t24); -Kfusion[8] = t77*(P[8][5]*t4+P[8][4]*t9+P[8][0]*t14-P[8][6]*t11+P[8][1]*t18-P[8][2]*t21+P[8][3]*t24); -Kfusion[9] = t77*(P[9][5]*t4+P[9][4]*t9+P[9][0]*t14-P[9][6]*t11+P[9][1]*t18-P[9][2]*t21+P[9][3]*t24); -Kfusion[10] = t77*(P[10][5]*t4+P[10][4]*t9+P[10][0]*t14-P[10][6]*t11+P[10][1]*t18-P[10][2]*t21+P[10][3]*t24); -Kfusion[11] = t77*(P[11][5]*t4+P[11][4]*t9+P[11][0]*t14-P[11][6]*t11+P[11][1]*t18-P[11][2]*t21+P[11][3]*t24); -Kfusion[12] = t77*(P[12][5]*t4+P[12][4]*t9+P[12][0]*t14-P[12][6]*t11+P[12][1]*t18-P[12][2]*t21+P[12][3]*t24); -Kfusion[13] = t77*(P[13][5]*t4+P[13][4]*t9+P[13][0]*t14-P[13][6]*t11+P[13][1]*t18-P[13][2]*t21+P[13][3]*t24); -Kfusion[14] = t77*(P[14][5]*t4+P[14][4]*t9+P[14][0]*t14-P[14][6]*t11+P[14][1]*t18-P[14][2]*t21+P[14][3]*t24); -Kfusion[15] = t77*(P[15][5]*t4+P[15][4]*t9+P[15][0]*t14-P[15][6]*t11+P[15][1]*t18-P[15][2]*t21+P[15][3]*t24); -Kfusion[16] = t77*(P[16][5]*t4+P[16][4]*t9+P[16][0]*t14-P[16][6]*t11+P[16][1]*t18-P[16][2]*t21+P[16][3]*t24); -Kfusion[17] = t77*(P[17][5]*t4+P[17][4]*t9+P[17][0]*t14-P[17][6]*t11+P[17][1]*t18-P[17][2]*t21+P[17][3]*t24); -Kfusion[18] = t77*(P[18][5]*t4+P[18][4]*t9+P[18][0]*t14-P[18][6]*t11+P[18][1]*t18-P[18][2]*t21+P[18][3]*t24); -Kfusion[19] = t77*(P[19][5]*t4+P[19][4]*t9+P[19][0]*t14-P[19][6]*t11+P[19][1]*t18-P[19][2]*t21+P[19][3]*t24); -Kfusion[20] = t77*(P[20][5]*t4+P[20][4]*t9+P[20][0]*t14-P[20][6]*t11+P[20][1]*t18-P[20][2]*t21+P[20][3]*t24); -Kfusion[21] = t77*(P[21][5]*t4+P[21][4]*t9+P[21][0]*t14-P[21][6]*t11+P[21][1]*t18-P[21][2]*t21+P[21][3]*t24); -Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24); -Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/K_VELY.c b/EKF/matlab/scripts/Inertial Nav EKF/K_VELY.c deleted file mode 100644 index 6351f09759..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/K_VELY.c +++ /dev/null @@ -1,116 +0,0 @@ -float t2 = q0*q3*2.0f; -float t9 = q1*q2*2.0f; -float t3 = t2-t9; -float t4 = q0*q0; -float t5 = q1*q1; -float t6 = q2*q2; -float t7 = q3*q3; -float t8 = t4-t5+t6-t7; -float t10 = q0*q1*2.0f; -float t11 = q2*q3*2.0f; -float t12 = t10+t11; -float t13 = q1*vd*2.0f; -float t14 = q0*ve*2.0f; -float t26 = q3*vn*2.0f; -float t15 = t13+t14-t26; -float t16 = q0*vd*2.0f; -float t17 = q2*vn*2.0f; -float t27 = q1*ve*2.0f; -float t18 = t16+t17-t27; -float t19 = q3*vd*2.0f; -float t20 = q2*ve*2.0f; -float t21 = q1*vn*2.0f; -float t22 = t19+t20+t21; -float t23 = q3*ve*2.0f; -float t24 = q0*vn*2.0f; -float t28 = q2*vd*2.0f; -float t25 = t23+t24-t28; -float t29 = P[0][0]*t15; -float t30 = P[1][1]*t18; -float t31 = P[5][4]*t8; -float t32 = P[6][4]*t12; -float t33 = P[0][4]*t15; -float t34 = P[1][4]*t18; -float t35 = P[2][4]*t22; -float t78 = P[4][4]*t3; -float t79 = P[3][4]*t25; -float t36 = t31+t32+t33+t34+t35-t78-t79; -float t37 = P[5][6]*t8; -float t38 = P[6][6]*t12; -float t39 = P[0][6]*t15; -float t40 = P[1][6]*t18; -float t41 = P[2][6]*t22; -float t81 = P[4][6]*t3; -float t82 = P[3][6]*t25; -float t42 = t37+t38+t39+t40+t41-t81-t82; -float t43 = t12*t42; -float t44 = P[5][0]*t8; -float t45 = P[6][0]*t12; -float t46 = P[1][0]*t18; -float t47 = P[2][0]*t22; -float t83 = P[4][0]*t3; -float t84 = P[3][0]*t25; -float t48 = t29+t44+t45+t46+t47-t83-t84; -float t49 = t15*t48; -float t50 = P[5][1]*t8; -float t51 = P[6][1]*t12; -float t52 = P[0][1]*t15; -float t53 = P[2][1]*t22; -float t85 = P[4][1]*t3; -float t86 = P[3][1]*t25; -float t54 = t30+t50+t51+t52+t53-t85-t86; -float t55 = t18*t54; -float t56 = P[5][2]*t8; -float t57 = P[6][2]*t12; -float t58 = P[0][2]*t15; -float t59 = P[1][2]*t18; -float t60 = P[2][2]*t22; -float t87 = P[4][2]*t3; -float t88 = P[3][2]*t25; -float t61 = t56+t57+t58+t59+t60-t87-t88; -float t62 = t22*t61; -float t63 = P[5][3]*t8; -float t64 = P[6][3]*t12; -float t65 = P[0][3]*t15; -float t66 = P[1][3]*t18; -float t67 = P[2][3]*t22; -float t89 = P[4][3]*t3; -float t90 = P[3][3]*t25; -float t68 = t63+t64+t65+t66+t67-t89-t90; -float t69 = P[5][5]*t8; -float t70 = P[6][5]*t12; -float t71 = P[0][5]*t15; -float t72 = P[1][5]*t18; -float t73 = P[2][5]*t22; -float t92 = P[4][5]*t3; -float t93 = P[3][5]*t25; -float t74 = t69+t70+t71+t72+t73-t92-t93; -float t75 = t8*t74; -float t80 = t3*t36; -float t91 = t25*t68; -float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91; -float t77 = 1.0f/t76; -Kfusion[0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25); -Kfusion[1] = t77*(t30-P[1][4]*t3+P[1][5]*t8+P[1][0]*t15+P[1][6]*t12+P[1][2]*t22-P[1][3]*t25); -Kfusion[2] = t77*(t60-P[2][4]*t3+P[2][5]*t8+P[2][0]*t15+P[2][6]*t12+P[2][1]*t18-P[2][3]*t25); -Kfusion[3] = t77*(-t90-P[3][4]*t3+P[3][5]*t8+P[3][0]*t15+P[3][6]*t12+P[3][1]*t18+P[3][2]*t22); -Kfusion[4] = t77*(-t78+P[4][5]*t8+P[4][0]*t15+P[4][6]*t12+P[4][1]*t18+P[4][2]*t22-P[4][3]*t25); -Kfusion[5] = t77*(t69-P[5][4]*t3+P[5][0]*t15+P[5][6]*t12+P[5][1]*t18+P[5][2]*t22-P[5][3]*t25); -Kfusion[6] = t77*(t38-P[6][4]*t3+P[6][5]*t8+P[6][0]*t15+P[6][1]*t18+P[6][2]*t22-P[6][3]*t25); -Kfusion[7] = t77*(-P[7][4]*t3+P[7][5]*t8+P[7][0]*t15+P[7][6]*t12+P[7][1]*t18+P[7][2]*t22-P[7][3]*t25); -Kfusion[8] = t77*(-P[8][4]*t3+P[8][5]*t8+P[8][0]*t15+P[8][6]*t12+P[8][1]*t18+P[8][2]*t22-P[8][3]*t25); -Kfusion[9] = t77*(-P[9][4]*t3+P[9][5]*t8+P[9][0]*t15+P[9][6]*t12+P[9][1]*t18+P[9][2]*t22-P[9][3]*t25); -Kfusion[10] = t77*(-P[10][4]*t3+P[10][5]*t8+P[10][0]*t15+P[10][6]*t12+P[10][1]*t18+P[10][2]*t22-P[10][3]*t25); -Kfusion[11] = t77*(-P[11][4]*t3+P[11][5]*t8+P[11][0]*t15+P[11][6]*t12+P[11][1]*t18+P[11][2]*t22-P[11][3]*t25); -Kfusion[12] = t77*(-P[12][4]*t3+P[12][5]*t8+P[12][0]*t15+P[12][6]*t12+P[12][1]*t18+P[12][2]*t22-P[12][3]*t25); -Kfusion[13] = t77*(-P[13][4]*t3+P[13][5]*t8+P[13][0]*t15+P[13][6]*t12+P[13][1]*t18+P[13][2]*t22-P[13][3]*t25); -Kfusion[14] = t77*(-P[14][4]*t3+P[14][5]*t8+P[14][0]*t15+P[14][6]*t12+P[14][1]*t18+P[14][2]*t22-P[14][3]*t25); -Kfusion[15] = t77*(-P[15][4]*t3+P[15][5]*t8+P[15][0]*t15+P[15][6]*t12+P[15][1]*t18+P[15][2]*t22-P[15][3]*t25); -Kfusion[16] = t77*(-P[16][4]*t3+P[16][5]*t8+P[16][0]*t15+P[16][6]*t12+P[16][1]*t18+P[16][2]*t22-P[16][3]*t25); -Kfusion[17] = t77*(-P[17][4]*t3+P[17][5]*t8+P[17][0]*t15+P[17][6]*t12+P[17][1]*t18+P[17][2]*t22-P[17][3]*t25); -Kfusion[18] = t77*(-P[18][4]*t3+P[18][5]*t8+P[18][0]*t15+P[18][6]*t12+P[18][1]*t18+P[18][2]*t22-P[18][3]*t25); -Kfusion[19] = t77*(-P[19][4]*t3+P[19][5]*t8+P[19][0]*t15+P[19][6]*t12+P[19][1]*t18+P[19][2]*t22-P[19][3]*t25); -Kfusion[20] = t77*(-P[20][4]*t3+P[20][5]*t8+P[20][0]*t15+P[20][6]*t12+P[20][1]*t18+P[20][2]*t22-P[20][3]*t25); -Kfusion[21] = t77*(-P[21][4]*t3+P[21][5]*t8+P[21][0]*t15+P[21][6]*t12+P[21][1]*t18+P[21][2]*t22-P[21][3]*t25); -Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25); -Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/K_VELZ.c b/EKF/matlab/scripts/Inertial Nav EKF/K_VELZ.c deleted file mode 100644 index 8b4269e884..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/K_VELZ.c +++ /dev/null @@ -1,116 +0,0 @@ -float t2 = q0*q2*2.0; -float t3 = q1*q3*2.0; -float t4 = t2+t3; -float t5 = q0*q0; -float t6 = q1*q1; -float t7 = q2*q2; -float t8 = q3*q3; -float t9 = t5-t6-t7+t8; -float t10 = q0*q1*2.0; -float t25 = q2*q3*2.0; -float t11 = t10-t25; -float t12 = q0*vd*2.0; -float t13 = q2*vn*2.0; -float t26 = q1*ve*2.0; -float t14 = t12+t13-t26; -float t15 = q1*vd*2.0; -float t16 = q0*ve*2.0; -float t27 = q3*vn*2.0; -float t17 = t15+t16-t27; -float t18 = q3*ve*2.0; -float t19 = q0*vn*2.0; -float t28 = q2*vd*2.0; -float t20 = t18+t19-t28; -float t21 = q3*vd*2.0; -float t22 = q2*ve*2.0; -float t23 = q1*vn*2.0; -float t24 = t21+t22+t23; -float t29 = P[0][0]*t14; -float t30 = P[6][4]*t9; -float t31 = P[4][4]*t4; -float t32 = P[0][4]*t14; -float t33 = P[2][4]*t20; -float t34 = P[3][4]*t24; -float t78 = P[5][4]*t11; -float t79 = P[1][4]*t17; -float t35 = t30+t31+t32+t33+t34-t78-t79; -float t36 = t4*t35; -float t37 = P[6][5]*t9; -float t38 = P[4][5]*t4; -float t39 = P[0][5]*t14; -float t40 = P[2][5]*t20; -float t41 = P[3][5]*t24; -float t80 = P[5][5]*t11; -float t81 = P[1][5]*t17; -float t42 = t37+t38+t39+t40+t41-t80-t81; -float t43 = P[6][0]*t9; -float t44 = P[4][0]*t4; -float t45 = P[2][0]*t20; -float t46 = P[3][0]*t24; -float t83 = P[5][0]*t11; -float t84 = P[1][0]*t17; -float t47 = t29+t43+t44+t45+t46-t83-t84; -float t48 = t14*t47; -float t49 = P[6][1]*t9; -float t50 = P[4][1]*t4; -float t51 = P[0][1]*t14; -float t52 = P[2][1]*t20; -float t53 = P[3][1]*t24; -float t85 = P[5][1]*t11; -float t86 = P[1][1]*t17; -float t54 = t49+t50+t51+t52+t53-t85-t86; -float t55 = P[6][2]*t9; -float t56 = P[4][2]*t4; -float t57 = P[0][2]*t14; -float t58 = P[2][2]*t20; -float t59 = P[3][2]*t24; -float t88 = P[5][2]*t11; -float t89 = P[1][2]*t17; -float t60 = t55+t56+t57+t58+t59-t88-t89; -float t61 = t20*t60; -float t62 = P[6][3]*t9; -float t63 = P[4][3]*t4; -float t64 = P[0][3]*t14; -float t65 = P[2][3]*t20; -float t66 = P[3][3]*t24; -float t90 = P[5][3]*t11; -float t91 = P[1][3]*t17; -float t67 = t62+t63+t64+t65+t66-t90-t91; -float t68 = t24*t67; -float t69 = P[6][6]*t9; -float t70 = P[4][6]*t4; -float t71 = P[0][6]*t14; -float t72 = P[2][6]*t20; -float t73 = P[3][6]*t24; -float t92 = P[5][6]*t11; -float t93 = P[1][6]*t17; -float t74 = t69+t70+t71+t72+t73-t92-t93; -float t75 = t9*t74; -float t82 = t11*t42; -float t87 = t17*t54; -float t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87; -float t77 = 1.0f/t76; -Kfusion[0] = t77*(t29+P[0][4]*t4+P[0][6]*t9-P[0][5]*t11-P[0][1]*t17+P[0][2]*t20+P[0][3]*t24); -Kfusion[1] = t77*(P[1][4]*t4+P[1][0]*t14+P[1][6]*t9-P[1][5]*t11-P[1][1]*t17+P[1][2]*t20+P[1][3]*t24); -Kfusion[2] = t77*(t58+P[2][4]*t4+P[2][0]*t14+P[2][6]*t9-P[2][5]*t11-P[2][1]*t17+P[2][3]*t24); -Kfusion[3] = t77*(t66+P[3][4]*t4+P[3][0]*t14+P[3][6]*t9-P[3][5]*t11-P[3][1]*t17+P[3][2]*t20); -Kfusion[4] = t77*(t31+P[4][0]*t14+P[4][6]*t9-P[4][5]*t11-P[4][1]*t17+P[4][2]*t20+P[4][3]*t24); -Kfusion[5] = t77*(-t80+P[5][4]*t4+P[5][0]*t14+P[5][6]*t9-P[5][1]*t17+P[5][2]*t20+P[5][3]*t24); -Kfusion[6] = t77*(t69+P[6][4]*t4+P[6][0]*t14-P[6][5]*t11-P[6][1]*t17+P[6][2]*t20+P[6][3]*t24); -Kfusion[7] = t77*(P[7][4]*t4+P[7][0]*t14+P[7][6]*t9-P[7][5]*t11-P[7][1]*t17+P[7][2]*t20+P[7][3]*t24); -Kfusion[8] = t77*(P[8][4]*t4+P[8][0]*t14+P[8][6]*t9-P[8][5]*t11-P[8][1]*t17+P[8][2]*t20+P[8][3]*t24); -Kfusion[9] = t77*(P[9][4]*t4+P[9][0]*t14+P[9][6]*t9-P[9][5]*t11-P[9][1]*t17+P[9][2]*t20+P[9][3]*t24); -Kfusion[10] = t77*(P[10][4]*t4+P[10][0]*t14+P[10][6]*t9-P[10][5]*t11-P[10][1]*t17+P[10][2]*t20+P[10][3]*t24); -Kfusion[11] = t77*(P[11][4]*t4+P[11][0]*t14+P[11][6]*t9-P[11][5]*t11-P[11][1]*t17+P[11][2]*t20+P[11][3]*t24); -Kfusion[12] = t77*(P[12][4]*t4+P[12][0]*t14+P[12][6]*t9-P[12][5]*t11-P[12][1]*t17+P[12][2]*t20+P[12][3]*t24); -Kfusion[13] = t77*(P[13][4]*t4+P[13][0]*t14+P[13][6]*t9-P[13][5]*t11-P[13][1]*t17+P[13][2]*t20+P[13][3]*t24); -Kfusion[14] = t77*(P[14][4]*t4+P[14][0]*t14+P[14][6]*t9-P[14][5]*t11-P[14][1]*t17+P[14][2]*t20+P[14][3]*t24); -Kfusion[15] = t77*(P[15][4]*t4+P[15][0]*t14+P[15][6]*t9-P[15][5]*t11-P[15][1]*t17+P[15][2]*t20+P[15][3]*t24); -Kfusion[16] = t77*(P[16][4]*t4+P[16][0]*t14+P[16][6]*t9-P[16][5]*t11-P[16][1]*t17+P[16][2]*t20+P[16][3]*t24); -Kfusion[17] = t77*(P[17][4]*t4+P[17][0]*t14+P[17][6]*t9-P[17][5]*t11-P[17][1]*t17+P[17][2]*t20+P[17][3]*t24); -Kfusion[18] = t77*(P[18][4]*t4+P[18][0]*t14+P[18][6]*t9-P[18][5]*t11-P[18][1]*t17+P[18][2]*t20+P[18][3]*t24); -Kfusion[19] = t77*(P[19][4]*t4+P[19][0]*t14+P[19][6]*t9-P[19][5]*t11-P[19][1]*t17+P[19][2]*t20+P[19][3]*t24); -Kfusion[20] = t77*(P[20][4]*t4+P[20][0]*t14+P[20][6]*t9-P[20][5]*t11-P[20][1]*t17+P[20][2]*t20+P[20][3]*t24); -Kfusion[21] = t77*(P[21][4]*t4+P[21][0]*t14+P[21][6]*t9-P[21][5]*t11-P[21][1]*t17+P[21][2]*t20+P[21][3]*t24); -Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24); -Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/LOSX.c b/EKF/matlab/scripts/Inertial Nav EKF/LOSX.c deleted file mode 100644 index ea7b36c36e..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/LOSX.c +++ /dev/null @@ -1,160 +0,0 @@ -float t2 = 1.0f/range; -float t3 = Tbs.b.y*q0*2.0f; -float t4 = Tbs.b.x*q3*2.0f; -float t18 = Tbs.b.z*q1*2.0f; -float t5 = t3+t4-t18; -float t6 = Tbs.b.y*q1*2.0f; -float t7 = Tbs.b.z*q0*2.0f; -float t16 = Tbs.b.x*q2*2.0f; -float t8 = t6+t7-t16; -float t9 = Tbs.b.x*q0*2.0f; -float t10 = Tbs.b.z*q2*2.0f; -float t17 = Tbs.b.y*q3*2.0f; -float t11 = t9+t10-t17; -float t12 = Tbs.b.x*q1*2.0f; -float t13 = Tbs.b.y*q2*2.0f; -float t14 = Tbs.b.z*q3*2.0f; -float t15 = t12+t13+t14; -float t19 = q0*q0; -float t20 = q1*q1; -float t21 = q2*q2; -float t22 = q3*q3; -float t23 = q0*q3*2.0f; -float t24 = q0*q2*2.0f; -float t25 = q1*q3*2.0f; -float t26 = q0*q1*2.0f; -float t27 = t19+t20-t21-t22; -float t28 = Tbs.b.x*t27; -float t29 = q1*q2*2.0f; -float t30 = t24+t25; -float t31 = Tbs.b.z*t30; -float t32 = t19-t20+t21-t22; -float t33 = Tbs.b.y*t32; -float t34 = t23+t29; -float t35 = Tbs.b.x*t34; -float t36 = q2*q3*2.0f; -float t37 = t19-t20-t21+t22; -float t38 = Tbs.b.z*t37; -float t39 = t24-t25; -float t40 = t26+t36; -float t41 = Tbs.b.y*t40; -float t60 = Tbs.b.x*t39; -float t42 = t38+t41-t60; -float t43 = t8*vd; -float t44 = t5*ve; -float t45 = t11*vn; -float t46 = t43+t44+t45; -float t47 = t5*vd; -float t48 = t15*vn; -float t62 = t8*ve; -float t49 = t47+t48-t62; -float t50 = t15*ve; -float t51 = t8*vn; -float t63 = t11*vd; -float t52 = t50+t51-t63; -float t53 = t15*vd; -float t54 = t11*ve; -float t64 = t5*vn; -float t55 = t53+t54-t64; -float t56 = t23-t29; -float t65 = Tbs.b.y*t56; -float t57 = t28+t31-t65; -float t58 = t26-t36; -float t66 = Tbs.b.z*t58; -float t59 = t33+t35-t66; -float t61 = P[0][0]*t2*t46; -float t67 = P[1][1]*t2*t49; -float t68 = P[4][0]*t2*t57; -float t69 = P[5][0]*t2*t59; -float t70 = P[6][0]*t2*t42; -float t71 = P[1][0]*t2*t49; -float t72 = P[2][0]*t2*t52; -float t73 = P[3][0]*t2*t55; -float t74 = t61+t68+t69+t70+t71+t72+t73; -float t75 = t2*t46*t74; -float t76 = P[4][1]*t2*t57; -float t77 = P[5][1]*t2*t59; -float t78 = P[6][1]*t2*t42; -float t79 = P[0][1]*t2*t46; -float t80 = P[2][1]*t2*t52; -float t81 = P[3][1]*t2*t55; -float t82 = t67+t76+t77+t78+t79+t80+t81; -float t83 = t2*t49*t82; -float t84 = P[4][2]*t2*t57; -float t85 = P[5][2]*t2*t59; -float t86 = P[6][2]*t2*t42; -float t87 = P[0][2]*t2*t46; -float t88 = P[1][2]*t2*t49; -float t89 = P[2][2]*t2*t52; -float t90 = P[3][2]*t2*t55; -float t91 = t84+t85+t86+t87+t88+t89+t90; -float t92 = t2*t52*t91; -float t93 = P[4][3]*t2*t57; -float t94 = P[5][3]*t2*t59; -float t95 = P[6][3]*t2*t42; -float t96 = P[0][3]*t2*t46; -float t97 = P[1][3]*t2*t49; -float t98 = P[2][3]*t2*t52; -float t99 = P[3][3]*t2*t55; -float t100 = t93+t94+t95+t96+t97+t98+t99; -float t101 = t2*t55*t100; -float t102 = P[4][4]*t2*t57; -float t103 = P[5][4]*t2*t59; -float t104 = P[6][4]*t2*t42; -float t105 = P[0][4]*t2*t46; -float t106 = P[1][4]*t2*t49; -float t107 = P[2][4]*t2*t52; -float t108 = P[3][4]*t2*t55; -float t109 = t102+t103+t104+t105+t106+t107+t108; -float t110 = t2*t57*t109; -float t111 = P[4][5]*t2*t57; -float t112 = P[5][5]*t2*t59; -float t113 = P[6][5]*t2*t42; -float t114 = P[0][5]*t2*t46; -float t115 = P[1][5]*t2*t49; -float t116 = P[2][5]*t2*t52; -float t117 = P[3][5]*t2*t55; -float t118 = t111+t112+t113+t114+t115+t116+t117; -float t119 = t2*t59*t118; -float t120 = P[4][6]*t2*t57; -float t121 = P[5][6]*t2*t59; -float t122 = P[6][6]*t2*t42; -float t123 = P[0][6]*t2*t46; -float t124 = P[1][6]*t2*t49; -float t125 = P[2][6]*t2*t52; -float t126 = P[3][6]*t2*t55; -float t127 = t120+t121+t122+t123+t124+t125+t126; -float t128 = t2*t42*t127; -float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128; -float t130 = 1.0f/t129; -H_LOS[0] = t2*t46; -H_LOS[1] = t2*t49; -H_LOS[2] = t2*t52; -H_LOS[3] = t2*t55; -H_LOS[4] = t2*(t28+t31-Tbs.b.y*(t23-q1*q2*2.0)); -H_LOS[5] = t2*(t33+t35-Tbs.b.z*(t26-q2*q3*2.0)); -H_LOS[6] = t2*t42; -Kfusion[0] = t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59); -Kfusion[1] = t130*(t67+P[1][0]*t2*t46+P[1][6]*t2*t42+P[1][2]*t2*t52+P[1][3]*t2*t55+P[1][4]*t2*t57+P[1][5]*t2*t59); -Kfusion[2] = t130*(t89+P[2][0]*t2*t46+P[2][6]*t2*t42+P[2][1]*t2*t49+P[2][3]*t2*t55+P[2][4]*t2*t57+P[2][5]*t2*t59); -Kfusion[3] = t130*(t99+P[3][0]*t2*t46+P[3][6]*t2*t42+P[3][1]*t2*t49+P[3][2]*t2*t52+P[3][4]*t2*t57+P[3][5]*t2*t59); -Kfusion[4] = t130*(t102+P[4][0]*t2*t46+P[4][6]*t2*t42+P[4][1]*t2*t49+P[4][2]*t2*t52+P[4][3]*t2*t55+P[4][5]*t2*t59); -Kfusion[5] = t130*(t112+P[5][0]*t2*t46+P[5][6]*t2*t42+P[5][1]*t2*t49+P[5][2]*t2*t52+P[5][3]*t2*t55+P[5][4]*t2*t57); -Kfusion[6] = t130*(t122+P[6][0]*t2*t46+P[6][1]*t2*t49+P[6][2]*t2*t52+P[6][3]*t2*t55+P[6][4]*t2*t57+P[6][5]*t2*t59); -Kfusion[7] = t130*(P[7][0]*t2*t46+P[7][6]*t2*t42+P[7][1]*t2*t49+P[7][2]*t2*t52+P[7][3]*t2*t55+P[7][4]*t2*t57+P[7][5]*t2*t59); -Kfusion[8] = t130*(P[8][0]*t2*t46+P[8][6]*t2*t42+P[8][1]*t2*t49+P[8][2]*t2*t52+P[8][3]*t2*t55+P[8][4]*t2*t57+P[8][5]*t2*t59); -Kfusion[9] = t130*(P[9][0]*t2*t46+P[9][6]*t2*t42+P[9][1]*t2*t49+P[9][2]*t2*t52+P[9][3]*t2*t55+P[9][4]*t2*t57+P[9][5]*t2*t59); -Kfusion[10] = t130*(P[10][0]*t2*t46+P[10][6]*t2*t42+P[10][1]*t2*t49+P[10][2]*t2*t52+P[10][3]*t2*t55+P[10][4]*t2*t57+P[10][5]*t2*t59); -Kfusion[11] = t130*(P[11][0]*t2*t46+P[11][6]*t2*t42+P[11][1]*t2*t49+P[11][2]*t2*t52+P[11][3]*t2*t55+P[11][4]*t2*t57+P[11][5]*t2*t59); -Kfusion[12] = t130*(P[12][0]*t2*t46+P[12][6]*t2*t42+P[12][1]*t2*t49+P[12][2]*t2*t52+P[12][3]*t2*t55+P[12][4]*t2*t57+P[12][5]*t2*t59); -Kfusion[13] = t130*(P[13][0]*t2*t46+P[13][6]*t2*t42+P[13][1]*t2*t49+P[13][2]*t2*t52+P[13][3]*t2*t55+P[13][4]*t2*t57+P[13][5]*t2*t59); -Kfusion[14] = t130*(P[14][0]*t2*t46+P[14][6]*t2*t42+P[14][1]*t2*t49+P[14][2]*t2*t52+P[14][3]*t2*t55+P[14][4]*t2*t57+P[14][5]*t2*t59); -Kfusion[15] = t130*(P[15][0]*t2*t46+P[15][6]*t2*t42+P[15][1]*t2*t49+P[15][2]*t2*t52+P[15][3]*t2*t55+P[15][4]*t2*t57+P[15][5]*t2*t59); -Kfusion[16] = t130*(P[16][0]*t2*t46+P[16][6]*t2*t42+P[16][1]*t2*t49+P[16][2]*t2*t52+P[16][3]*t2*t55+P[16][4]*t2*t57+P[16][5]*t2*t59); -Kfusion[17] = t130*(P[17][0]*t2*t46+P[17][6]*t2*t42+P[17][1]*t2*t49+P[17][2]*t2*t52+P[17][3]*t2*t55+P[17][4]*t2*t57+P[17][5]*t2*t59); -Kfusion[18] = t130*(P[18][0]*t2*t46+P[18][6]*t2*t42+P[18][1]*t2*t49+P[18][2]*t2*t52+P[18][3]*t2*t55+P[18][4]*t2*t57+P[18][5]*t2*t59); -Kfusion[19] = t130*(P[19][0]*t2*t46+P[19][6]*t2*t42+P[19][1]*t2*t49+P[19][2]*t2*t52+P[19][3]*t2*t55+P[19][4]*t2*t57+P[19][5]*t2*t59); -Kfusion[20] = t130*(P[20][0]*t2*t46+P[20][6]*t2*t42+P[20][1]*t2*t49+P[20][2]*t2*t52+P[20][3]*t2*t55+P[20][4]*t2*t57+P[20][5]*t2*t59); -Kfusion[21] = t130*(P[21][0]*t2*t46+P[21][6]*t2*t42+P[21][1]*t2*t49+P[21][2]*t2*t52+P[21][3]*t2*t55+P[21][4]*t2*t57+P[21][5]*t2*t59); -Kfusion[22] = t130*(P[22][0]*t2*t46+P[22][6]*t2*t42+P[22][1]*t2*t49+P[22][2]*t2*t52+P[22][3]*t2*t55+P[22][4]*t2*t57+P[22][5]*t2*t59); -Kfusion[23] = t130*(P[23][0]*t2*t46+P[23][6]*t2*t42+P[23][1]*t2*t49+P[23][2]*t2*t52+P[23][3]*t2*t55+P[23][4]*t2*t57+P[23][5]*t2*t59); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/LOSY.c b/EKF/matlab/scripts/Inertial Nav EKF/LOSY.c deleted file mode 100644 index 260e50c048..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/LOSY.c +++ /dev/null @@ -1,162 +0,0 @@ -float t2 = 1.0f/range; -float t3 = Tbs.a.y*q0*2.0f; -float t4 = Tbs.a.x*q3*2.0f; -float t18 = Tbs.a.z*q1*2.0f; -float t5 = t3+t4-t18; -float t6 = Tbs.a.y*q1*2.0f; -float t7 = Tbs.a.z*q0*2.0f; -float t16 = Tbs.a.x*q2*2.0f; -float t8 = t6+t7-t16; -float t9 = Tbs.a.x*q0*2.0f; -float t10 = Tbs.a.z*q2*2.0f; -float t17 = Tbs.a.y*q3*2.0f; -float t11 = t9+t10-t17; -float t12 = Tbs.a.x*q1*2.0f; -float t13 = Tbs.a.y*q2*2.0f; -float t14 = Tbs.a.z*q3*2.0f; -float t15 = t12+t13+t14; -float t19 = q0*q0; -float t20 = q1*q1; -float t21 = q2*q2; -float t22 = q3*q3; -float t23 = q0*q3*2.0f; -float t24 = q0*q2*2.0f; -float t25 = q1*q3*2.0f; -float t26 = q0*q1*2.0f; -float t27 = t19+t20-t21-t22; -float t28 = Tbs.a.x*t27; -float t29 = q1*q2*2.0f; -float t30 = t24+t25; -float t31 = Tbs.a.z*t30; -float t32 = t19-t20+t21-t22; -float t33 = Tbs.a.y*t32; -float t34 = t23+t29; -float t35 = Tbs.a.x*t34; -float t36 = q2*q3*2.0f; -float t37 = t19-t20-t21+t22; -float t38 = Tbs.a.z*t37; -float t39 = t24-t25; -float t40 = t26+t36; -float t41 = Tbs.a.y*t40; -float t60 = Tbs.a.x*t39; -float t42 = t38+t41-t60; -float t43 = t8*vd; -float t44 = t5*ve; -float t45 = t11*vn; -float t46 = t43+t44+t45; -float t47 = t5*vd; -float t48 = t15*vn; -float t62 = t8*ve; -float t49 = t47+t48-t62; -float t50 = t15*ve; -float t51 = t8*vn; -float t63 = t11*vd; -float t52 = t50+t51-t63; -float t53 = t15*vd; -float t54 = t11*ve; -float t64 = t5*vn; -float t55 = t53+t54-t64; -float t56 = t23-t29; -float t65 = Tbs.a.y*t56; -float t57 = t28+t31-t65; -float t58 = t26-t36; -float t66 = Tbs.a.z*t58; -float t59 = t33+t35-t66; -float t61 = P[0][0]*t2*t46; -float t67 = P[1][1]*t2*t49; -float t68 = P[4][0]*t2*t57; -float t69 = P[5][0]*t2*t59; -float t70 = P[6][0]*t2*t42; -float t71 = P[1][0]*t2*t49; -float t72 = P[2][0]*t2*t52; -float t73 = P[3][0]*t2*t55; -float t74 = t61+t68+t69+t70+t71+t72+t73; -float t75 = t2*t46*t74; -float t76 = P[4][1]*t2*t57; -float t77 = P[5][1]*t2*t59; -float t78 = P[6][1]*t2*t42; -float t79 = P[0][1]*t2*t46; -float t80 = P[2][1]*t2*t52; -float t81 = P[3][1]*t2*t55; -float t82 = t67+t76+t77+t78+t79+t80+t81; -float t83 = t2*t49*t82; -float t84 = P[4][2]*t2*t57; -float t85 = P[5][2]*t2*t59; -float t86 = P[6][2]*t2*t42; -float t87 = P[0][2]*t2*t46; -float t88 = P[1][2]*t2*t49; -float t89 = P[2][2]*t2*t52; -float t90 = P[3][2]*t2*t55; -float t91 = t84+t85+t86+t87+t88+t89+t90; -float t92 = t2*t52*t91; -float t93 = P[4][3]*t2*t57; -float t94 = P[5][3]*t2*t59; -float t95 = P[6][3]*t2*t42; -float t96 = P[0][3]*t2*t46; -float t97 = P[1][3]*t2*t49; -float t98 = P[2][3]*t2*t52; -float t99 = P[3][3]*t2*t55; -float t100 = t93+t94+t95+t96+t97+t98+t99; -float t101 = t2*t55*t100; -float t102 = P[4][4]*t2*t57; -float t103 = P[5][4]*t2*t59; -float t104 = P[6][4]*t2*t42; -float t105 = P[0][4]*t2*t46; -float t106 = P[1][4]*t2*t49; -float t107 = P[2][4]*t2*t52; -float t108 = P[3][4]*t2*t55; -float t109 = t102+t103+t104+t105+t106+t107+t108; -float t110 = t2*t57*t109; -float t111 = P[4][5]*t2*t57; -float t112 = P[5][5]*t2*t59; -float t113 = P[6][5]*t2*t42; -float t114 = P[0][5]*t2*t46; -float t115 = P[1][5]*t2*t49; -float t116 = P[2][5]*t2*t52; -float t117 = P[3][5]*t2*t55; -float t118 = t111+t112+t113+t114+t115+t116+t117; -float t119 = t2*t59*t118; -float t120 = P[4][6]*t2*t57; -float t121 = P[5][6]*t2*t59; -float t122 = P[6][6]*t2*t42; -float t123 = P[0][6]*t2*t46; -float t124 = P[1][6]*t2*t49; -float t125 = P[2][6]*t2*t52; -float t126 = P[3][6]*t2*t55; -float t127 = t120+t121+t122+t123+t124+t125+t126; -float t128 = t2*t42*t127; -float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128; -float t130 = 1.0f/t129; - -H_LOS[0] = -t2*t46; -H_LOS[1] = -t2*t49; -H_LOS[2] = -t2*t52; -H_LOS[3] = -t2*t55; -H_LOS[4] = -t2*(t28+t31-Tbs.a.y*(t23-q1*q2*2.0f)); -H_LOS[5] = -t2*(t33+t35-Tbs.a.z*(t26-q2*q3*2.0f)); -H_LOS[6] = -t2*t42; - -Kfusion[0] = -t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59); -Kfusion[1] = -t130*(t67+P[1][0]*t2*t46+P[1][6]*t2*t42+P[1][2]*t2*t52+P[1][3]*t2*t55+P[1][4]*t2*t57+P[1][5]*t2*t59); -Kfusion[2] = -t130*(t89+P[2][0]*t2*t46+P[2][6]*t2*t42+P[2][1]*t2*t49+P[2][3]*t2*t55+P[2][4]*t2*t57+P[2][5]*t2*t59); -Kfusion[3] = -t130*(t99+P[3][0]*t2*t46+P[3][6]*t2*t42+P[3][1]*t2*t49+P[3][2]*t2*t52+P[3][4]*t2*t57+P[3][5]*t2*t59); -Kfusion[4] = -t130*(t102+P[4][0]*t2*t46+P[4][6]*t2*t42+P[4][1]*t2*t49+P[4][2]*t2*t52+P[4][3]*t2*t55+P[4][5]*t2*t59); -Kfusion[5] = -t130*(t112+P[5][0]*t2*t46+P[5][6]*t2*t42+P[5][1]*t2*t49+P[5][2]*t2*t52+P[5][3]*t2*t55+P[5][4]*t2*t57); -Kfusion[6] = -t130*(t122+P[6][0]*t2*t46+P[6][1]*t2*t49+P[6][2]*t2*t52+P[6][3]*t2*t55+P[6][4]*t2*t57+P[6][5]*t2*t59); -Kfusion[7] = -t130*(P[7][0]*t2*t46+P[7][6]*t2*t42+P[7][1]*t2*t49+P[7][2]*t2*t52+P[7][3]*t2*t55+P[7][4]*t2*t57+P[7][5]*t2*t59); -Kfusion[8] = -t130*(P[8][0]*t2*t46+P[8][6]*t2*t42+P[8][1]*t2*t49+P[8][2]*t2*t52+P[8][3]*t2*t55+P[8][4]*t2*t57+P[8][5]*t2*t59); -Kfusion[9] = -t130*(P[9][0]*t2*t46+P[9][6]*t2*t42+P[9][1]*t2*t49+P[9][2]*t2*t52+P[9][3]*t2*t55+P[9][4]*t2*t57+P[9][5]*t2*t59); -Kfusion[10] = -t130*(P[10][0]*t2*t46+P[10][6]*t2*t42+P[10][1]*t2*t49+P[10][2]*t2*t52+P[10][3]*t2*t55+P[10][4]*t2*t57+P[10][5]*t2*t59); -Kfusion[11] = -t130*(P[11][0]*t2*t46+P[11][6]*t2*t42+P[11][1]*t2*t49+P[11][2]*t2*t52+P[11][3]*t2*t55+P[11][4]*t2*t57+P[11][5]*t2*t59); -Kfusion[12] = -t130*(P[12][0]*t2*t46+P[12][6]*t2*t42+P[12][1]*t2*t49+P[12][2]*t2*t52+P[12][3]*t2*t55+P[12][4]*t2*t57+P[12][5]*t2*t59); -Kfusion[13] = -t130*(P[13][0]*t2*t46+P[13][6]*t2*t42+P[13][1]*t2*t49+P[13][2]*t2*t52+P[13][3]*t2*t55+P[13][4]*t2*t57+P[13][5]*t2*t59); -Kfusion[14] = -t130*(P[14][0]*t2*t46+P[14][6]*t2*t42+P[14][1]*t2*t49+P[14][2]*t2*t52+P[14][3]*t2*t55+P[14][4]*t2*t57+P[14][5]*t2*t59); -Kfusion[15] = -t130*(P[15][0]*t2*t46+P[15][6]*t2*t42+P[15][1]*t2*t49+P[15][2]*t2*t52+P[15][3]*t2*t55+P[15][4]*t2*t57+P[15][5]*t2*t59); -Kfusion[16] = -t130*(P[16][0]*t2*t46+P[16][6]*t2*t42+P[16][1]*t2*t49+P[16][2]*t2*t52+P[16][3]*t2*t55+P[16][4]*t2*t57+P[16][5]*t2*t59); -Kfusion[17] = -t130*(P[17][0]*t2*t46+P[17][6]*t2*t42+P[17][1]*t2*t49+P[17][2]*t2*t52+P[17][3]*t2*t55+P[17][4]*t2*t57+P[17][5]*t2*t59); -Kfusion[18] = -t130*(P[18][0]*t2*t46+P[18][6]*t2*t42+P[18][1]*t2*t49+P[18][2]*t2*t52+P[18][3]*t2*t55+P[18][4]*t2*t57+P[18][5]*t2*t59); -Kfusion[19] = -t130*(P[19][0]*t2*t46+P[19][6]*t2*t42+P[19][1]*t2*t49+P[19][2]*t2*t52+P[19][3]*t2*t55+P[19][4]*t2*t57+P[19][5]*t2*t59); -Kfusion[20] = -t130*(P[20][0]*t2*t46+P[20][6]*t2*t42+P[20][1]*t2*t49+P[20][2]*t2*t52+P[20][3]*t2*t55+P[20][4]*t2*t57+P[20][5]*t2*t59); -Kfusion[21] = -t130*(P[21][0]*t2*t46+P[21][6]*t2*t42+P[21][1]*t2*t49+P[21][2]*t2*t52+P[21][3]*t2*t55+P[21][4]*t2*t57+P[21][5]*t2*t59); -Kfusion[22] = -t130*(P[22][0]*t2*t46+P[22][6]*t2*t42+P[22][1]*t2*t49+P[22][2]*t2*t52+P[22][3]*t2*t55+P[22][4]*t2*t57+P[22][5]*t2*t59); -Kfusion[23] = -t130*(P[23][0]*t2*t46+P[23][6]*t2*t42+P[23][1]*t2*t49+P[23][2]*t2*t52+P[23][3]*t2*t55+P[23][4]*t2*t57+P[23][5]*t2*t59); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/M_code24.txt b/EKF/matlab/scripts/Inertial Nav EKF/M_code24.txt deleted file mode 100644 index aa51c6ff7e..0000000000 --- a/EKF/matlab/scripts/Inertial Nav EKF/M_code24.txt +++ /dev/null @@ -1,686 +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(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11) + (daxVar*SQ(11))/4 + SF(10)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(12)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(11)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SF(15)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) + SF(16)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) + SPP(11)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) + (dayVar*q2^2)/4 + (dazVar*q3^2)/4; -nextP(1,2) = OP(1,2) + SQ(9) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11) + SF(9)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(8)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(12)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) - SF(16)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) + SPP(11)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) - (q0*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)))/2; -nextP(2,2) = OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) + daxVar*SQ(10) - (OP(11,2)*q0)/2 + SF(9)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(8)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(12)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) - SF(16)*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2) + SPP(11)*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2) + (dayVar*q3^2)/4 + (dazVar*q2^2)/4 - (q0*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2))/2; -nextP(1,3) = OP(1,3) + SQ(8) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11) + SF(7)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(11)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(9)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SF(15)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) - SPP(11)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) - (q0*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)))/2; -nextP(2,3) = OP(2,3) + SQ(6) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2 + SF(7)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(11)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) + SF(9)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SF(15)*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2) - SPP(11)*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2) - (q0*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2))/2; -nextP(3,3) = OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) + dayVar*SQ(10) + (dazVar*SQ(11))/4 - (OP(12,3)*q0)/2 + SF(7)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(11)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) + SF(9)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SF(15)*(OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2) - SPP(11)*(OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2) + (daxVar*q3^2)/4 - (q0*(OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2))/2; -nextP(1,4) = OP(1,4) + SQ(7) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11) + SF(8)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(7)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(10)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(16)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) - SF(15)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) - (q0*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)))/2; -nextP(2,4) = OP(2,4) + SQ(5) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2 + SF(8)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(7)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) + SF(10)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(16)*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2) - SF(15)*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2) - (q0*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2))/2; -nextP(3,4) = OP(3,4) + SQ(4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2 + SF(8)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(7)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) + SF(10)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(16)*(OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2) - SF(15)*(OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2) - (q0*(OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2))/2; -nextP(4,4) = OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) + (dayVar*SQ(11))/4 + dazVar*SQ(10) - (OP(13,4)*q0)/2 + SF(8)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(7)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) + SF(10)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(16)*(OP(4,11) + OP(1,11)*SF(8) + OP(2,11)*SF(7) + OP(3,11)*SF(10) + OP(11,11)*SF(16) - OP(12,11)*SF(15) - (OP(13,11)*q0)/2) - SF(15)*(OP(4,12) + OP(1,12)*SF(8) + OP(2,12)*SF(7) + OP(3,12)*SF(10) + OP(11,12)*SF(16) - OP(12,12)*SF(15) - (OP(13,12)*q0)/2) + (daxVar*q2^2)/4 - (q0*(OP(4,13) + OP(1,13)*SF(8) + OP(2,13)*SF(7) + OP(3,13)*SF(10) + OP(11,13)*SF(16) - OP(12,13)*SF(15) - (OP(13,13)*q0)/2))/2; -nextP(1,5) = OP(1,5) + OP(2,5)*SF(10) + OP(3,5)*SF(12) + OP(4,5)*SF(11) + OP(11,5)*SF(15) + OP(12,5)*SF(16) + OP(13,5)*SPP(11) + SF(6)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(4)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SF(5)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SPP(1)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SPP(4)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) + SPP(7)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) - SPP(10)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11)); -nextP(2,5) = OP(2,5) + OP(1,5)*SF(9) + OP(3,5)*SF(8) + OP(4,5)*SF(12) - OP(13,5)*SF(16) + OP(12,5)*SPP(11) - (OP(11,5)*q0)/2 + SF(6)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(4)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SF(5)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SPP(1)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SPP(4)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) + SPP(7)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) - SPP(10)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2); -nextP(3,5) = OP(3,5) + OP(1,5)*SF(7) + OP(2,5)*SF(11) + OP(4,5)*SF(9) + OP(13,5)*SF(15) - OP(11,5)*SPP(11) - (OP(12,5)*q0)/2 + SF(6)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(4)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SF(5)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SPP(1)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SPP(4)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) + SPP(7)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) - SPP(10)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2); -nextP(4,5) = OP(4,5) + OP(1,5)*SF(8) + OP(2,5)*SF(7) + OP(3,5)*SF(10) + OP(11,5)*SF(16) - OP(12,5)*SF(15) - (OP(13,5)*q0)/2 + SF(6)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(4)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SF(5)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) + SPP(1)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SPP(4)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) + SPP(7)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) - SPP(10)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2); -nextP(5,5) = OP(5,5) + OP(1,5)*SF(6) + OP(2,5)*SF(4) - OP(4,5)*SF(5) + OP(3,5)*SPP(1) + OP(14,5)*SPP(4) + OP(15,5)*SPP(7) - OP(16,5)*SPP(10) + dvyVar*(SG(8) - 2*q0*q3)^2 + dvzVar*(SG(7) + 2*q0*q2)^2 + SF(6)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SF(4)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SF(5)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) + SPP(1)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SPP(4)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) + SPP(7)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) - SPP(10)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10)) + dvxVar*(SG(2) + SG(3) - SG(4) - SG(5))^2; -nextP(1,6) = OP(1,6) + OP(2,6)*SF(10) + OP(3,6)*SF(12) + OP(4,6)*SF(11) + OP(11,6)*SF(15) + OP(12,6)*SF(16) + OP(13,6)*SPP(11) + SF(5)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(4)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(6)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) - SPP(1)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SPP(9)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) + SPP(3)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) + SPP(6)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11)); -nextP(2,6) = OP(2,6) + OP(1,6)*SF(9) + OP(3,6)*SF(8) + OP(4,6)*SF(12) - OP(13,6)*SF(16) + OP(12,6)*SPP(11) - (OP(11,6)*q0)/2 + SF(5)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(4)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(6)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) - SPP(1)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SPP(9)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) + SPP(3)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) + SPP(6)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2); -nextP(3,6) = OP(3,6) + OP(1,6)*SF(7) + OP(2,6)*SF(11) + OP(4,6)*SF(9) + OP(13,6)*SF(15) - OP(11,6)*SPP(11) - (OP(12,6)*q0)/2 + SF(5)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(4)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(6)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) - SPP(1)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SPP(9)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) + SPP(3)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) + SPP(6)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2); -nextP(4,6) = OP(4,6) + OP(1,6)*SF(8) + OP(2,6)*SF(7) + OP(3,6)*SF(10) + OP(11,6)*SF(16) - OP(12,6)*SF(15) - (OP(13,6)*q0)/2 + SF(5)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(4)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(6)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) - SPP(1)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SPP(9)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) + SPP(3)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) + SPP(6)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2); -nextP(5,6) = OP(5,6) + SQ(3) + OP(1,6)*SF(6) + OP(2,6)*SF(4) - OP(4,6)*SF(5) + OP(3,6)*SPP(1) + OP(14,6)*SPP(4) + OP(15,6)*SPP(7) - OP(16,6)*SPP(10) + SF(5)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SF(4)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SF(6)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) - SPP(1)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SPP(9)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) + SPP(3)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) + SPP(6)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10)); -nextP(6,6) = OP(6,6) + OP(1,6)*SF(5) + OP(3,6)*SF(4) + OP(4,6)*SF(6) - OP(2,6)*SPP(1) - OP(14,6)*SPP(9) + OP(15,6)*SPP(3) + OP(16,6)*SPP(6) + dvxVar*(SG(8) + 2*q0*q3)^2 + dvzVar*(SG(6) - 2*q0*q1)^2 + SF(5)*(OP(6,1) + OP(1,1)*SF(5) + OP(3,1)*SF(4) + OP(4,1)*SF(6) - OP(2,1)*SPP(1) - OP(14,1)*SPP(9) + OP(15,1)*SPP(3) + OP(16,1)*SPP(6)) + SF(4)*(OP(6,3) + OP(1,3)*SF(5) + OP(3,3)*SF(4) + OP(4,3)*SF(6) - OP(2,3)*SPP(1) - OP(14,3)*SPP(9) + OP(15,3)*SPP(3) + OP(16,3)*SPP(6)) + SF(6)*(OP(6,4) + OP(1,4)*SF(5) + OP(3,4)*SF(4) + OP(4,4)*SF(6) - OP(2,4)*SPP(1) - OP(14,4)*SPP(9) + OP(15,4)*SPP(3) + OP(16,4)*SPP(6)) - SPP(1)*(OP(6,2) + OP(1,2)*SF(5) + OP(3,2)*SF(4) + OP(4,2)*SF(6) - OP(2,2)*SPP(1) - OP(14,2)*SPP(9) + OP(15,2)*SPP(3) + OP(16,2)*SPP(6)) - SPP(9)*(OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6)) + SPP(3)*(OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6)) + SPP(6)*(OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6)) + dvyVar*(SG(2) - SG(3) + SG(4) - SG(5))^2; -nextP(1,7) = OP(1,7) + OP(2,7)*SF(10) + OP(3,7)*SF(12) + OP(4,7)*SF(11) + OP(11,7)*SF(15) + OP(12,7)*SF(16) + OP(13,7)*SPP(11) + SF(5)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SF(6)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(4)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SPP(1)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SPP(5)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) - SPP(8)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) - SPP(2)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11)); -nextP(2,7) = OP(2,7) + OP(1,7)*SF(9) + OP(3,7)*SF(8) + OP(4,7)*SF(12) - OP(13,7)*SF(16) + OP(12,7)*SPP(11) - (OP(11,7)*q0)/2 + SF(5)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SF(6)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(4)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SPP(1)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SPP(5)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) - SPP(8)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) - SPP(2)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2); -nextP(3,7) = OP(3,7) + OP(1,7)*SF(7) + OP(2,7)*SF(11) + OP(4,7)*SF(9) + OP(13,7)*SF(15) - OP(11,7)*SPP(11) - (OP(12,7)*q0)/2 + SF(5)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SF(6)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(4)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SPP(1)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SPP(5)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) - SPP(8)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) - SPP(2)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2); -nextP(4,7) = OP(4,7) + OP(1,7)*SF(8) + OP(2,7)*SF(7) + OP(3,7)*SF(10) + OP(11,7)*SF(16) - OP(12,7)*SF(15) - (OP(13,7)*q0)/2 + SF(5)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SF(6)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(4)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) + SPP(1)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SPP(5)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) - SPP(8)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) - SPP(2)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2); -nextP(5,7) = OP(5,7) + SQ(2) + OP(1,7)*SF(6) + OP(2,7)*SF(4) - OP(4,7)*SF(5) + OP(3,7)*SPP(1) + OP(14,7)*SPP(4) + OP(15,7)*SPP(7) - OP(16,7)*SPP(10) + SF(5)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SF(6)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SF(4)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) + SPP(1)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SPP(5)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) - SPP(8)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) - SPP(2)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10)); -nextP(6,7) = OP(6,7) + SQ(1) + OP(1,7)*SF(5) + OP(3,7)*SF(4) + OP(4,7)*SF(6) - OP(2,7)*SPP(1) - OP(14,7)*SPP(9) + OP(15,7)*SPP(3) + OP(16,7)*SPP(6) + SF(5)*(OP(6,2) + OP(1,2)*SF(5) + OP(3,2)*SF(4) + OP(4,2)*SF(6) - OP(2,2)*SPP(1) - OP(14,2)*SPP(9) + OP(15,2)*SPP(3) + OP(16,2)*SPP(6)) - SF(6)*(OP(6,3) + OP(1,3)*SF(5) + OP(3,3)*SF(4) + OP(4,3)*SF(6) - OP(2,3)*SPP(1) - OP(14,3)*SPP(9) + OP(15,3)*SPP(3) + OP(16,3)*SPP(6)) + SF(4)*(OP(6,4) + OP(1,4)*SF(5) + OP(3,4)*SF(4) + OP(4,4)*SF(6) - OP(2,4)*SPP(1) - OP(14,4)*SPP(9) + OP(15,4)*SPP(3) + OP(16,4)*SPP(6)) + SPP(1)*(OP(6,1) + OP(1,1)*SF(5) + OP(3,1)*SF(4) + OP(4,1)*SF(6) - OP(2,1)*SPP(1) - OP(14,1)*SPP(9) + OP(15,1)*SPP(3) + OP(16,1)*SPP(6)) + SPP(5)*(OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6)) - SPP(8)*(OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6)) - SPP(2)*(OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6)); -nextP(7,7) = OP(7,7) + OP(2,7)*SF(5) - OP(3,7)*SF(6) + OP(4,7)*SF(4) + OP(1,7)*SPP(1) + OP(14,7)*SPP(5) - OP(15,7)*SPP(8) - OP(16,7)*SPP(2) + dvxVar*(SG(7) - 2*q0*q2)^2 + dvyVar*(SG(6) + 2*q0*q1)^2 + SF(5)*(OP(7,2) + OP(2,2)*SF(5) - OP(3,2)*SF(6) + OP(4,2)*SF(4) + OP(1,2)*SPP(1) + OP(14,2)*SPP(5) - OP(15,2)*SPP(8) - OP(16,2)*SPP(2)) - SF(6)*(OP(7,3) + OP(2,3)*SF(5) - OP(3,3)*SF(6) + OP(4,3)*SF(4) + OP(1,3)*SPP(1) + OP(14,3)*SPP(5) - OP(15,3)*SPP(8) - OP(16,3)*SPP(2)) + SF(4)*(OP(7,4) + OP(2,4)*SF(5) - OP(3,4)*SF(6) + OP(4,4)*SF(4) + OP(1,4)*SPP(1) + OP(14,4)*SPP(5) - OP(15,4)*SPP(8) - OP(16,4)*SPP(2)) + SPP(1)*(OP(7,1) + OP(2,1)*SF(5) - OP(3,1)*SF(6) + OP(4,1)*SF(4) + OP(1,1)*SPP(1) + OP(14,1)*SPP(5) - OP(15,1)*SPP(8) - OP(16,1)*SPP(2)) + SPP(5)*(OP(7,14) + OP(2,14)*SF(5) - OP(3,14)*SF(6) + OP(4,14)*SF(4) + OP(1,14)*SPP(1) + OP(14,14)*SPP(5) - OP(15,14)*SPP(8) - OP(16,14)*SPP(2)) - SPP(8)*(OP(7,15) + OP(2,15)*SF(5) - OP(3,15)*SF(6) + OP(4,15)*SF(4) + OP(1,15)*SPP(1) + OP(14,15)*SPP(5) - OP(15,15)*SPP(8) - OP(16,15)*SPP(2)) - SPP(2)*(OP(7,16) + OP(2,16)*SF(5) - OP(3,16)*SF(6) + OP(4,16)*SF(4) + OP(1,16)*SPP(1) + OP(14,16)*SPP(5) - OP(15,16)*SPP(8) - OP(16,16)*SPP(2)) + dvzVar*(SG(2) - SG(3) - SG(4) + SG(5))^2; -nextP(1,8) = OP(1,8) + OP(2,8)*SF(10) + OP(3,8)*SF(12) + OP(4,8)*SF(11) + OP(11,8)*SF(15) + OP(12,8)*SF(16) + OP(13,8)*SPP(11) + dt*(OP(1,5) + OP(2,5)*SF(10) + OP(3,5)*SF(12) + OP(4,5)*SF(11) + OP(11,5)*SF(15) + OP(12,5)*SF(16) + OP(13,5)*SPP(11)); -nextP(2,8) = OP(2,8) + OP(1,8)*SF(9) + OP(3,8)*SF(8) + OP(4,8)*SF(12) - OP(13,8)*SF(16) + OP(12,8)*SPP(11) - (OP(11,8)*q0)/2 + dt*(OP(2,5) + OP(1,5)*SF(9) + OP(3,5)*SF(8) + OP(4,5)*SF(12) - OP(13,5)*SF(16) + OP(12,5)*SPP(11) - (OP(11,5)*q0)/2); -nextP(3,8) = OP(3,8) + OP(1,8)*SF(7) + OP(2,8)*SF(11) + OP(4,8)*SF(9) + OP(13,8)*SF(15) - OP(11,8)*SPP(11) - (OP(12,8)*q0)/2 + dt*(OP(3,5) + OP(1,5)*SF(7) + OP(2,5)*SF(11) + OP(4,5)*SF(9) + OP(13,5)*SF(15) - OP(11,5)*SPP(11) - (OP(12,5)*q0)/2); -nextP(4,8) = OP(4,8) + OP(1,8)*SF(8) + OP(2,8)*SF(7) + OP(3,8)*SF(10) + OP(11,8)*SF(16) - OP(12,8)*SF(15) - (OP(13,8)*q0)/2 + dt*(OP(4,5) + OP(1,5)*SF(8) + OP(2,5)*SF(7) + OP(3,5)*SF(10) + OP(11,5)*SF(16) - OP(12,5)*SF(15) - (OP(13,5)*q0)/2); -nextP(5,8) = OP(5,8) + OP(1,8)*SF(6) + OP(2,8)*SF(4) - OP(4,8)*SF(5) + OP(3,8)*SPP(1) + OP(14,8)*SPP(4) + OP(15,8)*SPP(7) - OP(16,8)*SPP(10) + dt*(OP(5,5) + OP(1,5)*SF(6) + OP(2,5)*SF(4) - OP(4,5)*SF(5) + OP(3,5)*SPP(1) + OP(14,5)*SPP(4) + OP(15,5)*SPP(7) - OP(16,5)*SPP(10)); -nextP(6,8) = OP(6,8) + OP(1,8)*SF(5) + OP(3,8)*SF(4) + OP(4,8)*SF(6) - OP(2,8)*SPP(1) - OP(14,8)*SPP(9) + OP(15,8)*SPP(3) + OP(16,8)*SPP(6) + dt*(OP(6,5) + OP(1,5)*SF(5) + OP(3,5)*SF(4) + OP(4,5)*SF(6) - OP(2,5)*SPP(1) - OP(14,5)*SPP(9) + OP(15,5)*SPP(3) + OP(16,5)*SPP(6)); -nextP(7,8) = OP(7,8) + OP(2,8)*SF(5) - OP(3,8)*SF(6) + OP(4,8)*SF(4) + OP(1,8)*SPP(1) + OP(14,8)*SPP(5) - OP(15,8)*SPP(8) - OP(16,8)*SPP(2) + dt*(OP(7,5) + OP(2,5)*SF(5) - OP(3,5)*SF(6) + OP(4,5)*SF(4) + OP(1,5)*SPP(1) + OP(14,5)*SPP(5) - OP(15,5)*SPP(8) - OP(16,5)*SPP(2)); -nextP(8,8) = OP(8,8) + OP(5,8)*dt + dt*(OP(8,5) + OP(5,5)*dt); -nextP(1,9) = OP(1,9) + OP(2,9)*SF(10) + OP(3,9)*SF(12) + OP(4,9)*SF(11) + OP(11,9)*SF(15) + OP(12,9)*SF(16) + OP(13,9)*SPP(11) + dt*(OP(1,6) + OP(2,6)*SF(10) + OP(3,6)*SF(12) + OP(4,6)*SF(11) + OP(11,6)*SF(15) + OP(12,6)*SF(16) + OP(13,6)*SPP(11)); -nextP(2,9) = OP(2,9) + OP(1,9)*SF(9) + OP(3,9)*SF(8) + OP(4,9)*SF(12) - OP(13,9)*SF(16) + OP(12,9)*SPP(11) - (OP(11,9)*q0)/2 + dt*(OP(2,6) + OP(1,6)*SF(9) + OP(3,6)*SF(8) + OP(4,6)*SF(12) - OP(13,6)*SF(16) + OP(12,6)*SPP(11) - (OP(11,6)*q0)/2); -nextP(3,9) = OP(3,9) + OP(1,9)*SF(7) + OP(2,9)*SF(11) + OP(4,9)*SF(9) + OP(13,9)*SF(15) - OP(11,9)*SPP(11) - (OP(12,9)*q0)/2 + dt*(OP(3,6) + OP(1,6)*SF(7) + OP(2,6)*SF(11) + OP(4,6)*SF(9) + OP(13,6)*SF(15) - OP(11,6)*SPP(11) - (OP(12,6)*q0)/2); -nextP(4,9) = OP(4,9) + OP(1,9)*SF(8) + OP(2,9)*SF(7) + OP(3,9)*SF(10) + OP(11,9)*SF(16) - OP(12,9)*SF(15) - (OP(13,9)*q0)/2 + dt*(OP(4,6) + OP(1,6)*SF(8) + OP(2,6)*SF(7) + OP(3,6)*SF(10) + OP(11,6)*SF(16) - OP(12,6)*SF(15) - (OP(13,6)*q0)/2); -nextP(5,9) = OP(5,9) + OP(1,9)*SF(6) + OP(2,9)*SF(4) - OP(4,9)*SF(5) + OP(3,9)*SPP(1) + OP(14,9)*SPP(4) + OP(15,9)*SPP(7) - OP(16,9)*SPP(10) + dt*(OP(5,6) + OP(1,6)*SF(6) + OP(2,6)*SF(4) - OP(4,6)*SF(5) + OP(3,6)*SPP(1) + OP(14,6)*SPP(4) + OP(15,6)*SPP(7) - OP(16,6)*SPP(10)); -nextP(6,9) = OP(6,9) + OP(1,9)*SF(5) + OP(3,9)*SF(4) + OP(4,9)*SF(6) - OP(2,9)*SPP(1) - OP(14,9)*SPP(9) + OP(15,9)*SPP(3) + OP(16,9)*SPP(6) + dt*(OP(6,6) + OP(1,6)*SF(5) + OP(3,6)*SF(4) + OP(4,6)*SF(6) - OP(2,6)*SPP(1) - OP(14,6)*SPP(9) + OP(15,6)*SPP(3) + OP(16,6)*SPP(6)); -nextP(7,9) = OP(7,9) + OP(2,9)*SF(5) - OP(3,9)*SF(6) + OP(4,9)*SF(4) + OP(1,9)*SPP(1) + OP(14,9)*SPP(5) - OP(15,9)*SPP(8) - OP(16,9)*SPP(2) + dt*(OP(7,6) + OP(2,6)*SF(5) - OP(3,6)*SF(6) + OP(4,6)*SF(4) + OP(1,6)*SPP(1) + OP(14,6)*SPP(5) - OP(15,6)*SPP(8) - OP(16,6)*SPP(2)); -nextP(8,9) = OP(8,9) + OP(5,9)*dt + dt*(OP(8,6) + OP(5,6)*dt); -nextP(9,9) = OP(9,9) + OP(6,9)*dt + dt*(OP(9,6) + OP(6,6)*dt); -nextP(1,10) = OP(1,10) + OP(2,10)*SF(10) + OP(3,10)*SF(12) + OP(4,10)*SF(11) + OP(11,10)*SF(15) + OP(12,10)*SF(16) + OP(13,10)*SPP(11) + dt*(OP(1,7) + OP(2,7)*SF(10) + OP(3,7)*SF(12) + OP(4,7)*SF(11) + OP(11,7)*SF(15) + OP(12,7)*SF(16) + OP(13,7)*SPP(11)); -nextP(2,10) = OP(2,10) + OP(1,10)*SF(9) + OP(3,10)*SF(8) + OP(4,10)*SF(12) - OP(13,10)*SF(16) + OP(12,10)*SPP(11) - (OP(11,10)*q0)/2 + dt*(OP(2,7) + OP(1,7)*SF(9) + OP(3,7)*SF(8) + OP(4,7)*SF(12) - OP(13,7)*SF(16) + OP(12,7)*SPP(11) - (OP(11,7)*q0)/2); -nextP(3,10) = OP(3,10) + OP(1,10)*SF(7) + OP(2,10)*SF(11) + OP(4,10)*SF(9) + OP(13,10)*SF(15) - OP(11,10)*SPP(11) - (OP(12,10)*q0)/2 + dt*(OP(3,7) + OP(1,7)*SF(7) + OP(2,7)*SF(11) + OP(4,7)*SF(9) + OP(13,7)*SF(15) - OP(11,7)*SPP(11) - (OP(12,7)*q0)/2); -nextP(4,10) = OP(4,10) + OP(1,10)*SF(8) + OP(2,10)*SF(7) + OP(3,10)*SF(10) + OP(11,10)*SF(16) - OP(12,10)*SF(15) - (OP(13,10)*q0)/2 + dt*(OP(4,7) + OP(1,7)*SF(8) + OP(2,7)*SF(7) + OP(3,7)*SF(10) + OP(11,7)*SF(16) - OP(12,7)*SF(15) - (OP(13,7)*q0)/2); -nextP(5,10) = OP(5,10) + OP(1,10)*SF(6) + OP(2,10)*SF(4) - OP(4,10)*SF(5) + OP(3,10)*SPP(1) + OP(14,10)*SPP(4) + OP(15,10)*SPP(7) - OP(16,10)*SPP(10) + dt*(OP(5,7) + OP(1,7)*SF(6) + OP(2,7)*SF(4) - OP(4,7)*SF(5) + OP(3,7)*SPP(1) + OP(14,7)*SPP(4) + OP(15,7)*SPP(7) - OP(16,7)*SPP(10)); -nextP(6,10) = OP(6,10) + OP(1,10)*SF(5) + OP(3,10)*SF(4) + OP(4,10)*SF(6) - OP(2,10)*SPP(1) - OP(14,10)*SPP(9) + OP(15,10)*SPP(3) + OP(16,10)*SPP(6) + dt*(OP(6,7) + OP(1,7)*SF(5) + OP(3,7)*SF(4) + OP(4,7)*SF(6) - OP(2,7)*SPP(1) - OP(14,7)*SPP(9) + OP(15,7)*SPP(3) + OP(16,7)*SPP(6)); -nextP(7,10) = OP(7,10) + OP(2,10)*SF(5) - OP(3,10)*SF(6) + OP(4,10)*SF(4) + OP(1,10)*SPP(1) + OP(14,10)*SPP(5) - OP(15,10)*SPP(8) - OP(16,10)*SPP(2) + dt*(OP(7,7) + OP(2,7)*SF(5) - OP(3,7)*SF(6) + OP(4,7)*SF(4) + OP(1,7)*SPP(1) + OP(14,7)*SPP(5) - OP(15,7)*SPP(8) - OP(16,7)*SPP(2)); -nextP(8,10) = OP(8,10) + OP(5,10)*dt + dt*(OP(8,7) + OP(5,7)*dt); -nextP(9,10) = OP(9,10) + OP(6,10)*dt + dt*(OP(9,7) + OP(6,7)*dt); -nextP(10,10) = OP(10,10) + OP(7,10)*dt + dt*(OP(10,7) + OP(7,7)*dt); -nextP(1,11) = OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11); -nextP(2,11) = OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2; -nextP(3,11) = OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2; -nextP(4,11) = OP(4,11) + OP(1,11)*SF(8) + OP(2,11)*SF(7) + OP(3,11)*SF(10) + OP(11,11)*SF(16) - OP(12,11)*SF(15) - (OP(13,11)*q0)/2; -nextP(5,11) = OP(5,11) + OP(1,11)*SF(6) + OP(2,11)*SF(4) - OP(4,11)*SF(5) + OP(3,11)*SPP(1) + OP(14,11)*SPP(4) + OP(15,11)*SPP(7) - OP(16,11)*SPP(10); -nextP(6,11) = OP(6,11) + OP(1,11)*SF(5) + OP(3,11)*SF(4) + OP(4,11)*SF(6) - OP(2,11)*SPP(1) - OP(14,11)*SPP(9) + OP(15,11)*SPP(3) + OP(16,11)*SPP(6); -nextP(7,11) = OP(7,11) + OP(2,11)*SF(5) - OP(3,11)*SF(6) + OP(4,11)*SF(4) + OP(1,11)*SPP(1) + OP(14,11)*SPP(5) - OP(15,11)*SPP(8) - OP(16,11)*SPP(2); -nextP(8,11) = OP(8,11) + OP(5,11)*dt; -nextP(9,11) = OP(9,11) + OP(6,11)*dt; -nextP(10,11) = OP(10,11) + OP(7,11)*dt; -nextP(11,11) = OP(11,11); -nextP(1,12) = OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11); -nextP(2,12) = OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2; -nextP(3,12) = OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2; -nextP(4,12) = OP(4,12) + OP(1,12)*SF(8) + OP(2,12)*SF(7) + OP(3,12)*SF(10) + OP(11,12)*SF(16) - OP(12,12)*SF(15) - (OP(13,12)*q0)/2; -nextP(5,12) = OP(5,12) + OP(1,12)*SF(6) + OP(2,12)*SF(4) - OP(4,12)*SF(5) + OP(3,12)*SPP(1) + OP(14,12)*SPP(4) + OP(15,12)*SPP(7) - OP(16,12)*SPP(10); -nextP(6,12) = OP(6,12) + OP(1,12)*SF(5) + OP(3,12)*SF(4) + OP(4,12)*SF(6) - OP(2,12)*SPP(1) - OP(14,12)*SPP(9) + OP(15,12)*SPP(3) + OP(16,12)*SPP(6); -nextP(7,12) = OP(7,12) + OP(2,12)*SF(5) - OP(3,12)*SF(6) + OP(4,12)*SF(4) + OP(1,12)*SPP(1) + OP(14,12)*SPP(5) - OP(15,12)*SPP(8) - OP(16,12)*SPP(2); -nextP(8,12) = OP(8,12) + OP(5,12)*dt; -nextP(9,12) = OP(9,12) + OP(6,12)*dt; -nextP(10,12) = OP(10,12) + OP(7,12)*dt; -nextP(11,12) = OP(11,12); -nextP(12,12) = OP(12,12); -nextP(1,13) = OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11); -nextP(2,13) = OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2; -nextP(3,13) = OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2; -nextP(4,13) = OP(4,13) + OP(1,13)*SF(8) + OP(2,13)*SF(7) + OP(3,13)*SF(10) + OP(11,13)*SF(16) - OP(12,13)*SF(15) - (OP(13,13)*q0)/2; -nextP(5,13) = OP(5,13) + OP(1,13)*SF(6) + OP(2,13)*SF(4) - OP(4,13)*SF(5) + OP(3,13)*SPP(1) + OP(14,13)*SPP(4) + OP(15,13)*SPP(7) - OP(16,13)*SPP(10); -nextP(6,13) = OP(6,13) + OP(1,13)*SF(5) + OP(3,13)*SF(4) + OP(4,13)*SF(6) - OP(2,13)*SPP(1) - OP(14,13)*SPP(9) + OP(15,13)*SPP(3) + OP(16,13)*SPP(6); -nextP(7,13) = OP(7,13) + OP(2,13)*SF(5) - OP(3,13)*SF(6) + OP(4,13)*SF(4) + OP(1,13)*SPP(1) + OP(14,13)*SPP(5) - OP(15,13)*SPP(8) - OP(16,13)*SPP(2); -nextP(8,13) = OP(8,13) + OP(5,13)*dt; -nextP(9,13) = OP(9,13) + OP(6,13)*dt; -nextP(10,13) = OP(10,13) + OP(7,13)*dt; -nextP(11,13) = OP(11,13); -nextP(12,13) = OP(12,13); -nextP(13,13) = OP(13,13); -nextP(1,14) = OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11); -nextP(2,14) = OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2; -nextP(3,14) = OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2; -nextP(4,14) = OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2; -nextP(5,14) = OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10); -nextP(6,14) = OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6); -nextP(7,14) = OP(7,14) + OP(2,14)*SF(5) - OP(3,14)*SF(6) + OP(4,14)*SF(4) + OP(1,14)*SPP(1) + OP(14,14)*SPP(5) - OP(15,14)*SPP(8) - OP(16,14)*SPP(2); -nextP(8,14) = OP(8,14) + OP(5,14)*dt; -nextP(9,14) = OP(9,14) + OP(6,14)*dt; -nextP(10,14) = OP(10,14) + OP(7,14)*dt; -nextP(11,14) = OP(11,14); -nextP(12,14) = OP(12,14); -nextP(13,14) = OP(13,14); -nextP(14,14) = OP(14,14); -nextP(1,15) = OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11); -nextP(2,15) = OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2; -nextP(3,15) = OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2; -nextP(4,15) = OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2; -nextP(5,15) = OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10); -nextP(6,15) = OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6); -nextP(7,15) = OP(7,15) + OP(2,15)*SF(5) - OP(3,15)*SF(6) + OP(4,15)*SF(4) + OP(1,15)*SPP(1) + OP(14,15)*SPP(5) - OP(15,15)*SPP(8) - OP(16,15)*SPP(2); -nextP(8,15) = OP(8,15) + OP(5,15)*dt; -nextP(9,15) = OP(9,15) + OP(6,15)*dt; -nextP(10,15) = OP(10,15) + OP(7,15)*dt; -nextP(11,15) = OP(11,15); -nextP(12,15) = OP(12,15); -nextP(13,15) = OP(13,15); -nextP(14,15) = OP(14,15); -nextP(15,15) = OP(15,15); -nextP(1,16) = OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11); -nextP(2,16) = OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2; -nextP(3,16) = OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2; -nextP(4,16) = OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2; -nextP(5,16) = OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10); -nextP(6,16) = OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6); -nextP(7,16) = OP(7,16) + OP(2,16)*SF(5) - OP(3,16)*SF(6) + OP(4,16)*SF(4) + OP(1,16)*SPP(1) + OP(14,16)*SPP(5) - OP(15,16)*SPP(8) - OP(16,16)*SPP(2); -nextP(8,16) = OP(8,16) + OP(5,16)*dt; -nextP(9,16) = OP(9,16) + OP(6,16)*dt; -nextP(10,16) = OP(10,16) + OP(7,16)*dt; -nextP(11,16) = OP(11,16); -nextP(12,16) = OP(12,16); -nextP(13,16) = OP(13,16); -nextP(14,16) = OP(14,16); -nextP(15,16) = OP(15,16); -nextP(16,16) = OP(16,16); -nextP(1,17) = OP(1,17) + OP(2,17)*SF(10) + OP(3,17)*SF(12) + OP(4,17)*SF(11) + OP(11,17)*SF(15) + OP(12,17)*SF(16) + OP(13,17)*SPP(11); -nextP(2,17) = OP(2,17) + OP(1,17)*SF(9) + OP(3,17)*SF(8) + OP(4,17)*SF(12) - OP(13,17)*SF(16) + OP(12,17)*SPP(11) - (OP(11,17)*q0)/2; -nextP(3,17) = OP(3,17) + OP(1,17)*SF(7) + OP(2,17)*SF(11) + OP(4,17)*SF(9) + OP(13,17)*SF(15) - OP(11,17)*SPP(11) - (OP(12,17)*q0)/2; -nextP(4,17) = OP(4,17) + OP(1,17)*SF(8) + OP(2,17)*SF(7) + OP(3,17)*SF(10) + OP(11,17)*SF(16) - OP(12,17)*SF(15) - (OP(13,17)*q0)/2; -nextP(5,17) = OP(5,17) + OP(1,17)*SF(6) + OP(2,17)*SF(4) - OP(4,17)*SF(5) + OP(3,17)*SPP(1) + OP(14,17)*SPP(4) + OP(15,17)*SPP(7) - OP(16,17)*SPP(10); -nextP(6,17) = OP(6,17) + OP(1,17)*SF(5) + OP(3,17)*SF(4) + OP(4,17)*SF(6) - OP(2,17)*SPP(1) - OP(14,17)*SPP(9) + OP(15,17)*SPP(3) + OP(16,17)*SPP(6); -nextP(7,17) = OP(7,17) + OP(2,17)*SF(5) - OP(3,17)*SF(6) + OP(4,17)*SF(4) + OP(1,17)*SPP(1) + OP(14,17)*SPP(5) - OP(15,17)*SPP(8) - OP(16,17)*SPP(2); -nextP(8,17) = OP(8,17) + OP(5,17)*dt; -nextP(9,17) = OP(9,17) + OP(6,17)*dt; -nextP(10,17) = OP(10,17) + OP(7,17)*dt; -nextP(11,17) = OP(11,17); -nextP(12,17) = OP(12,17); -nextP(13,17) = OP(13,17); -nextP(14,17) = OP(14,17); -nextP(15,17) = OP(15,17); -nextP(16,17) = OP(16,17); -nextP(17,17) = OP(17,17); -nextP(1,18) = OP(1,18) + OP(2,18)*SF(10) + OP(3,18)*SF(12) + OP(4,18)*SF(11) + OP(11,18)*SF(15) + OP(12,18)*SF(16) + OP(13,18)*SPP(11); -nextP(2,18) = OP(2,18) + OP(1,18)*SF(9) + OP(3,18)*SF(8) + OP(4,18)*SF(12) - OP(13,18)*SF(16) + OP(12,18)*SPP(11) - (OP(11,18)*q0)/2; -nextP(3,18) = OP(3,18) + OP(1,18)*SF(7) + OP(2,18)*SF(11) + OP(4,18)*SF(9) + OP(13,18)*SF(15) - OP(11,18)*SPP(11) - (OP(12,18)*q0)/2; -nextP(4,18) = OP(4,18) + OP(1,18)*SF(8) + OP(2,18)*SF(7) + OP(3,18)*SF(10) + OP(11,18)*SF(16) - OP(12,18)*SF(15) - (OP(13,18)*q0)/2; -nextP(5,18) = OP(5,18) + OP(1,18)*SF(6) + OP(2,18)*SF(4) - OP(4,18)*SF(5) + OP(3,18)*SPP(1) + OP(14,18)*SPP(4) + OP(15,18)*SPP(7) - OP(16,18)*SPP(10); -nextP(6,18) = OP(6,18) + OP(1,18)*SF(5) + OP(3,18)*SF(4) + OP(4,18)*SF(6) - OP(2,18)*SPP(1) - OP(14,18)*SPP(9) + OP(15,18)*SPP(3) + OP(16,18)*SPP(6); -nextP(7,18) = OP(7,18) + OP(2,18)*SF(5) - OP(3,18)*SF(6) + OP(4,18)*SF(4) + OP(1,18)*SPP(1) + OP(14,18)*SPP(5) - OP(15,18)*SPP(8) - OP(16,18)*SPP(2); -nextP(8,18) = OP(8,18) + OP(5,18)*dt; -nextP(9,18) = OP(9,18) + OP(6,18)*dt; -nextP(10,18) = OP(10,18) + OP(7,18)*dt; -nextP(11,18) = OP(11,18); -nextP(12,18) = OP(12,18); -nextP(13,18) = OP(13,18); -nextP(14,18) = OP(14,18); -nextP(15,18) = OP(15,18); -nextP(16,18) = OP(16,18); -nextP(17,18) = OP(17,18); -nextP(18,18) = OP(18,18); -nextP(1,19) = OP(1,19) + OP(2,19)*SF(10) + OP(3,19)*SF(12) + OP(4,19)*SF(11) + OP(11,19)*SF(15) + OP(12,19)*SF(16) + OP(13,19)*SPP(11); -nextP(2,19) = OP(2,19) + OP(1,19)*SF(9) + OP(3,19)*SF(8) + OP(4,19)*SF(12) - OP(13,19)*SF(16) + OP(12,19)*SPP(11) - (OP(11,19)*q0)/2; -nextP(3,19) = OP(3,19) + OP(1,19)*SF(7) + OP(2,19)*SF(11) + OP(4,19)*SF(9) + OP(13,19)*SF(15) - OP(11,19)*SPP(11) - (OP(12,19)*q0)/2; -nextP(4,19) = OP(4,19) + OP(1,19)*SF(8) + OP(2,19)*SF(7) + OP(3,19)*SF(10) + OP(11,19)*SF(16) - OP(12,19)*SF(15) - (OP(13,19)*q0)/2; -nextP(5,19) = OP(5,19) + OP(1,19)*SF(6) + OP(2,19)*SF(4) - OP(4,19)*SF(5) + OP(3,19)*SPP(1) + OP(14,19)*SPP(4) + OP(15,19)*SPP(7) - OP(16,19)*SPP(10); -nextP(6,19) = OP(6,19) + OP(1,19)*SF(5) + OP(3,19)*SF(4) + OP(4,19)*SF(6) - OP(2,19)*SPP(1) - OP(14,19)*SPP(9) + OP(15,19)*SPP(3) + OP(16,19)*SPP(6); -nextP(7,19) = OP(7,19) + OP(2,19)*SF(5) - OP(3,19)*SF(6) + OP(4,19)*SF(4) + OP(1,19)*SPP(1) + OP(14,19)*SPP(5) - OP(15,19)*SPP(8) - OP(16,19)*SPP(2); -nextP(8,19) = OP(8,19) + OP(5,19)*dt; -nextP(9,19) = OP(9,19) + OP(6,19)*dt; -nextP(10,19) = OP(10,19) + OP(7,19)*dt; -nextP(11,19) = OP(11,19); -nextP(12,19) = OP(12,19); -nextP(13,19) = OP(13,19); -nextP(14,19) = OP(14,19); -nextP(15,19) = OP(15,19); -nextP(16,19) = OP(16,19); -nextP(17,19) = OP(17,19); -nextP(18,19) = OP(18,19); -nextP(19,19) = OP(19,19); -nextP(1,20) = OP(1,20) + OP(2,20)*SF(10) + OP(3,20)*SF(12) + OP(4,20)*SF(11) + OP(11,20)*SF(15) + OP(12,20)*SF(16) + OP(13,20)*SPP(11); -nextP(2,20) = OP(2,20) + OP(1,20)*SF(9) + OP(3,20)*SF(8) + OP(4,20)*SF(12) - OP(13,20)*SF(16) + OP(12,20)*SPP(11) - (OP(11,20)*q0)/2; -nextP(3,20) = OP(3,20) + OP(1,20)*SF(7) + OP(2,20)*SF(11) + OP(4,20)*SF(9) + OP(13,20)*SF(15) - OP(11,20)*SPP(11) - (OP(12,20)*q0)/2; -nextP(4,20) = OP(4,20) + OP(1,20)*SF(8) + OP(2,20)*SF(7) + OP(3,20)*SF(10) + OP(11,20)*SF(16) - OP(12,20)*SF(15) - (OP(13,20)*q0)/2; -nextP(5,20) = OP(5,20) + OP(1,20)*SF(6) + OP(2,20)*SF(4) - OP(4,20)*SF(5) + OP(3,20)*SPP(1) + OP(14,20)*SPP(4) + OP(15,20)*SPP(7) - OP(16,20)*SPP(10); -nextP(6,20) = OP(6,20) + OP(1,20)*SF(5) + OP(3,20)*SF(4) + OP(4,20)*SF(6) - OP(2,20)*SPP(1) - OP(14,20)*SPP(9) + OP(15,20)*SPP(3) + OP(16,20)*SPP(6); -nextP(7,20) = OP(7,20) + OP(2,20)*SF(5) - OP(3,20)*SF(6) + OP(4,20)*SF(4) + OP(1,20)*SPP(1) + OP(14,20)*SPP(5) - OP(15,20)*SPP(8) - OP(16,20)*SPP(2); -nextP(8,20) = OP(8,20) + OP(5,20)*dt; -nextP(9,20) = OP(9,20) + OP(6,20)*dt; -nextP(10,20) = OP(10,20) + OP(7,20)*dt; -nextP(11,20) = OP(11,20); -nextP(12,20) = OP(12,20); -nextP(13,20) = OP(13,20); -nextP(14,20) = OP(14,20); -nextP(15,20) = OP(15,20); -nextP(16,20) = OP(16,20); -nextP(17,20) = OP(17,20); -nextP(18,20) = OP(18,20); -nextP(19,20) = OP(19,20); -nextP(20,20) = OP(20,20); -nextP(1,21) = OP(1,21) + OP(2,21)*SF(10) + OP(3,21)*SF(12) + OP(4,21)*SF(11) + OP(11,21)*SF(15) + OP(12,21)*SF(16) + OP(13,21)*SPP(11); -nextP(2,21) = OP(2,21) + OP(1,21)*SF(9) + OP(3,21)*SF(8) + OP(4,21)*SF(12) - OP(13,21)*SF(16) + OP(12,21)*SPP(11) - (OP(11,21)*q0)/2; -nextP(3,21) = OP(3,21) + OP(1,21)*SF(7) + OP(2,21)*SF(11) + OP(4,21)*SF(9) + OP(13,21)*SF(15) - OP(11,21)*SPP(11) - (OP(12,21)*q0)/2; -nextP(4,21) = OP(4,21) + OP(1,21)*SF(8) + OP(2,21)*SF(7) + OP(3,21)*SF(10) + OP(11,21)*SF(16) - OP(12,21)*SF(15) - (OP(13,21)*q0)/2; -nextP(5,21) = OP(5,21) + OP(1,21)*SF(6) + OP(2,21)*SF(4) - OP(4,21)*SF(5) + OP(3,21)*SPP(1) + OP(14,21)*SPP(4) + OP(15,21)*SPP(7) - OP(16,21)*SPP(10); -nextP(6,21) = OP(6,21) + OP(1,21)*SF(5) + OP(3,21)*SF(4) + OP(4,21)*SF(6) - OP(2,21)*SPP(1) - OP(14,21)*SPP(9) + OP(15,21)*SPP(3) + OP(16,21)*SPP(6); -nextP(7,21) = OP(7,21) + OP(2,21)*SF(5) - OP(3,21)*SF(6) + OP(4,21)*SF(4) + OP(1,21)*SPP(1) + OP(14,21)*SPP(5) - OP(15,21)*SPP(8) - OP(16,21)*SPP(2); -nextP(8,21) = OP(8,21) + OP(5,21)*dt; -nextP(9,21) = OP(9,21) + OP(6,21)*dt; -nextP(10,21) = OP(10,21) + OP(7,21)*dt; -nextP(11,21) = OP(11,21); -nextP(12,21) = OP(12,21); -nextP(13,21) = OP(13,21); -nextP(14,21) = OP(14,21); -nextP(15,21) = OP(15,21); -nextP(16,21) = OP(16,21); -nextP(17,21) = OP(17,21); -nextP(18,21) = OP(18,21); -nextP(19,21) = OP(19,21); -nextP(20,21) = OP(20,21); -nextP(21,21) = OP(21,21); -nextP(1,22) = OP(1,22) + OP(2,22)*SF(10) + OP(3,22)*SF(12) + OP(4,22)*SF(11) + OP(11,22)*SF(15) + OP(12,22)*SF(16) + OP(13,22)*SPP(11); -nextP(2,22) = OP(2,22) + OP(1,22)*SF(9) + OP(3,22)*SF(8) + OP(4,22)*SF(12) - OP(13,22)*SF(16) + OP(12,22)*SPP(11) - (OP(11,22)*q0)/2; -nextP(3,22) = OP(3,22) + OP(1,22)*SF(7) + OP(2,22)*SF(11) + OP(4,22)*SF(9) + OP(13,22)*SF(15) - OP(11,22)*SPP(11) - (OP(12,22)*q0)/2; -nextP(4,22) = OP(4,22) + OP(1,22)*SF(8) + OP(2,22)*SF(7) + OP(3,22)*SF(10) + OP(11,22)*SF(16) - OP(12,22)*SF(15) - (OP(13,22)*q0)/2; -nextP(5,22) = OP(5,22) + OP(1,22)*SF(6) + OP(2,22)*SF(4) - OP(4,22)*SF(5) + OP(3,22)*SPP(1) + OP(14,22)*SPP(4) + OP(15,22)*SPP(7) - OP(16,22)*SPP(10); -nextP(6,22) = OP(6,22) + OP(1,22)*SF(5) + OP(3,22)*SF(4) + OP(4,22)*SF(6) - OP(2,22)*SPP(1) - OP(14,22)*SPP(9) + OP(15,22)*SPP(3) + OP(16,22)*SPP(6); -nextP(7,22) = OP(7,22) + OP(2,22)*SF(5) - OP(3,22)*SF(6) + OP(4,22)*SF(4) + OP(1,22)*SPP(1) + OP(14,22)*SPP(5) - OP(15,22)*SPP(8) - OP(16,22)*SPP(2); -nextP(8,22) = OP(8,22) + OP(5,22)*dt; -nextP(9,22) = OP(9,22) + OP(6,22)*dt; -nextP(10,22) = OP(10,22) + OP(7,22)*dt; -nextP(11,22) = OP(11,22); -nextP(12,22) = OP(12,22); -nextP(13,22) = OP(13,22); -nextP(14,22) = OP(14,22); -nextP(15,22) = OP(15,22); -nextP(16,22) = OP(16,22); -nextP(17,22) = OP(17,22); -nextP(18,22) = OP(18,22); -nextP(19,22) = OP(19,22); -nextP(20,22) = OP(20,22); -nextP(21,22) = OP(21,22); -nextP(22,22) = OP(22,22); -nextP(1,23) = OP(1,23) + OP(2,23)*SF(10) + OP(3,23)*SF(12) + OP(4,23)*SF(11) + OP(11,23)*SF(15) + OP(12,23)*SF(16) + OP(13,23)*SPP(11); -nextP(2,23) = OP(2,23) + OP(1,23)*SF(9) + OP(3,23)*SF(8) + OP(4,23)*SF(12) - OP(13,23)*SF(16) + OP(12,23)*SPP(11) - (OP(11,23)*q0)/2; -nextP(3,23) = OP(3,23) + OP(1,23)*SF(7) + OP(2,23)*SF(11) + OP(4,23)*SF(9) + OP(13,23)*SF(15) - OP(11,23)*SPP(11) - (OP(12,23)*q0)/2; -nextP(4,23) = OP(4,23) + OP(1,23)*SF(8) + OP(2,23)*SF(7) + OP(3,23)*SF(10) + OP(11,23)*SF(16) - OP(12,23)*SF(15) - (OP(13,23)*q0)/2; -nextP(5,23) = OP(5,23) + OP(1,23)*SF(6) + OP(2,23)*SF(4) - OP(4,23)*SF(5) + OP(3,23)*SPP(1) + OP(14,23)*SPP(4) + OP(15,23)*SPP(7) - OP(16,23)*SPP(10); -nextP(6,23) = OP(6,23) + OP(1,23)*SF(5) + OP(3,23)*SF(4) + OP(4,23)*SF(6) - OP(2,23)*SPP(1) - OP(14,23)*SPP(9) + OP(15,23)*SPP(3) + OP(16,23)*SPP(6); -nextP(7,23) = OP(7,23) + OP(2,23)*SF(5) - OP(3,23)*SF(6) + OP(4,23)*SF(4) + OP(1,23)*SPP(1) + OP(14,23)*SPP(5) - OP(15,23)*SPP(8) - OP(16,23)*SPP(2); -nextP(8,23) = OP(8,23) + OP(5,23)*dt; -nextP(9,23) = OP(9,23) + OP(6,23)*dt; -nextP(10,23) = OP(10,23) + OP(7,23)*dt; -nextP(11,23) = OP(11,23); -nextP(12,23) = OP(12,23); -nextP(13,23) = OP(13,23); -nextP(14,23) = OP(14,23); -nextP(15,23) = OP(15,23); -nextP(16,23) = OP(16,23); -nextP(17,23) = OP(17,23); -nextP(18,23) = OP(18,23); -nextP(19,23) = OP(19,23); -nextP(20,23) = OP(20,23); -nextP(21,23) = OP(21,23); -nextP(22,23) = OP(22,23); -nextP(23,23) = OP(23,23); -nextP(1,24) = OP(1,24) + OP(2,24)*SF(10) + OP(3,24)*SF(12) + OP(4,24)*SF(11) + OP(11,24)*SF(15) + OP(12,24)*SF(16) + OP(13,24)*SPP(11); -nextP(2,24) = OP(2,24) + OP(1,24)*SF(9) + OP(3,24)*SF(8) + OP(4,24)*SF(12) - OP(13,24)*SF(16) + OP(12,24)*SPP(11) - (OP(11,24)*q0)/2; -nextP(3,24) = OP(3,24) + OP(1,24)*SF(7) + OP(2,24)*SF(11) + OP(4,24)*SF(9) + OP(13,24)*SF(15) - OP(11,24)*SPP(11) - (OP(12,24)*q0)/2; -nextP(4,24) = OP(4,24) + OP(1,24)*SF(8) + OP(2,24)*SF(7) + OP(3,24)*SF(10) + OP(11,24)*SF(16) - OP(12,24)*SF(15) - (OP(13,24)*q0)/2; -nextP(5,24) = OP(5,24) + OP(1,24)*SF(6) + OP(2,24)*SF(4) - OP(4,24)*SF(5) + OP(3,24)*SPP(1) + OP(14,24)*SPP(4) + OP(15,24)*SPP(7) - OP(16,24)*SPP(10); -nextP(6,24) = OP(6,24) + OP(1,24)*SF(5) + OP(3,24)*SF(4) + OP(4,24)*SF(6) - OP(2,24)*SPP(1) - OP(14,24)*SPP(9) + OP(15,24)*SPP(3) + OP(16,24)*SPP(6); -nextP(7,24) = OP(7,24) + OP(2,24)*SF(5) - OP(3,24)*SF(6) + OP(4,24)*SF(4) + OP(1,24)*SPP(1) + OP(14,24)*SPP(5) - OP(15,24)*SPP(8) - OP(16,24)*SPP(2); -nextP(8,24) = OP(8,24) + OP(5,24)*dt; -nextP(9,24) = OP(9,24) + OP(6,24)*dt; -nextP(10,24) = OP(10,24) + OP(7,24)*dt; -nextP(11,24) = OP(11,24); -nextP(12,24) = OP(12,24); -nextP(13,24) = OP(13,24); -nextP(14,24) = OP(14,24); -nextP(15,24) = OP(15,24); -nextP(16,24) = OP(16,24); -nextP(17,24) = OP(17,24); -nextP(18,24) = OP(18,24); -nextP(19,24) = OP(19,24); -nextP(20,24) = OP(20,24); -nextP(21,24) = OP(21,24); -nextP(22,24) = OP(22,24); -nextP(23,24) = OP(23,24); -nextP(24,24) = OP(24,24); -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(5,5)*SH_TAS(3) + OP(6,5)*SH_TAS(2) - OP(23,5)*SH_TAS(3) - OP(24,5)*SH_TAS(2) + OP(7,5)*vd*SH_TAS(1)) + SH_TAS(2)*(OP(5,6)*SH_TAS(3) + OP(6,6)*SH_TAS(2) - OP(23,6)*SH_TAS(3) - OP(24,6)*SH_TAS(2) + OP(7,6)*vd*SH_TAS(1)) - SH_TAS(3)*(OP(5,23)*SH_TAS(3) + OP(6,23)*SH_TAS(2) - OP(23,23)*SH_TAS(3) - OP(24,23)*SH_TAS(2) + OP(7,23)*vd*SH_TAS(1)) - SH_TAS(2)*(OP(5,24)*SH_TAS(3) + OP(6,24)*SH_TAS(2) - OP(23,24)*SH_TAS(3) - OP(24,24)*SH_TAS(2) + OP(7,24)*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP(5,7)*SH_TAS(3) + OP(6,7)*SH_TAS(2) - OP(23,7)*SH_TAS(3) - OP(24,7)*SH_TAS(2) + OP(7,7)*vd*SH_TAS(1))); -SK_TAS(2) = SH_TAS(2); -Kfusion = zeros(24,1); -Kfusion = zeros(1,1); -Kfusion(1) = SK_TAS(1)*(OP(1,5)*SH_TAS(3) - OP(1,23)*SH_TAS(3) + OP(1,6)*SK_TAS(2) - OP(1,24)*SK_TAS(2) + OP(1,7)*vd*SH_TAS(1)); -Kfusion(2) = SK_TAS(1)*(OP(2,5)*SH_TAS(3) - OP(2,23)*SH_TAS(3) + OP(2,6)*SK_TAS(2) - OP(2,24)*SK_TAS(2) + OP(2,7)*vd*SH_TAS(1)); -Kfusion(3) = SK_TAS(1)*(OP(3,5)*SH_TAS(3) - OP(3,23)*SH_TAS(3) + OP(3,6)*SK_TAS(2) - OP(3,24)*SK_TAS(2) + OP(3,7)*vd*SH_TAS(1)); -Kfusion(4) = SK_TAS(1)*(OP(4,5)*SH_TAS(3) - OP(4,23)*SH_TAS(3) + OP(4,6)*SK_TAS(2) - OP(4,24)*SK_TAS(2) + OP(4,7)*vd*SH_TAS(1)); -Kfusion(5) = SK_TAS(1)*(OP(5,5)*SH_TAS(3) - OP(5,23)*SH_TAS(3) + OP(5,6)*SK_TAS(2) - OP(5,24)*SK_TAS(2) + OP(5,7)*vd*SH_TAS(1)); -Kfusion(6) = SK_TAS(1)*(OP(6,5)*SH_TAS(3) - OP(6,23)*SH_TAS(3) + OP(6,6)*SK_TAS(2) - OP(6,24)*SK_TAS(2) + OP(6,7)*vd*SH_TAS(1)); -Kfusion(7) = SK_TAS(1)*(OP(7,5)*SH_TAS(3) - OP(7,23)*SH_TAS(3) + OP(7,6)*SK_TAS(2) - OP(7,24)*SK_TAS(2) + OP(7,7)*vd*SH_TAS(1)); -Kfusion(8) = SK_TAS(1)*(OP(8,5)*SH_TAS(3) - OP(8,23)*SH_TAS(3) + OP(8,6)*SK_TAS(2) - OP(8,24)*SK_TAS(2) + OP(8,7)*vd*SH_TAS(1)); -Kfusion(9) = SK_TAS(1)*(OP(9,5)*SH_TAS(3) - OP(9,23)*SH_TAS(3) + OP(9,6)*SK_TAS(2) - OP(9,24)*SK_TAS(2) + OP(9,7)*vd*SH_TAS(1)); -Kfusion(10) = SK_TAS(1)*(OP(10,5)*SH_TAS(3) - OP(10,23)*SH_TAS(3) + OP(10,6)*SK_TAS(2) - OP(10,24)*SK_TAS(2) + OP(10,7)*vd*SH_TAS(1)); -Kfusion(11) = SK_TAS(1)*(OP(11,5)*SH_TAS(3) - OP(11,23)*SH_TAS(3) + OP(11,6)*SK_TAS(2) - OP(11,24)*SK_TAS(2) + OP(11,7)*vd*SH_TAS(1)); -Kfusion(12) = SK_TAS(1)*(OP(12,5)*SH_TAS(3) - OP(12,23)*SH_TAS(3) + OP(12,6)*SK_TAS(2) - OP(12,24)*SK_TAS(2) + OP(12,7)*vd*SH_TAS(1)); -Kfusion(13) = SK_TAS(1)*(OP(13,5)*SH_TAS(3) - OP(13,23)*SH_TAS(3) + OP(13,6)*SK_TAS(2) - OP(13,24)*SK_TAS(2) + OP(13,7)*vd*SH_TAS(1)); -Kfusion(14) = SK_TAS(1)*(OP(14,5)*SH_TAS(3) - OP(14,23)*SH_TAS(3) + OP(14,6)*SK_TAS(2) - OP(14,24)*SK_TAS(2) + OP(14,7)*vd*SH_TAS(1)); -Kfusion(15) = SK_TAS(1)*(OP(15,5)*SH_TAS(3) - OP(15,23)*SH_TAS(3) + OP(15,6)*SK_TAS(2) - OP(15,24)*SK_TAS(2) + OP(15,7)*vd*SH_TAS(1)); -Kfusion(16) = SK_TAS(1)*(OP(16,5)*SH_TAS(3) - OP(16,23)*SH_TAS(3) + OP(16,6)*SK_TAS(2) - OP(16,24)*SK_TAS(2) + OP(16,7)*vd*SH_TAS(1)); -Kfusion(17) = SK_TAS(1)*(OP(17,5)*SH_TAS(3) - OP(17,23)*SH_TAS(3) + OP(17,6)*SK_TAS(2) - OP(17,24)*SK_TAS(2) + OP(17,7)*vd*SH_TAS(1)); -Kfusion(18) = SK_TAS(1)*(OP(18,5)*SH_TAS(3) - OP(18,23)*SH_TAS(3) + OP(18,6)*SK_TAS(2) - OP(18,24)*SK_TAS(2) + OP(18,7)*vd*SH_TAS(1)); -Kfusion(19) = SK_TAS(1)*(OP(19,5)*SH_TAS(3) - OP(19,23)*SH_TAS(3) + OP(19,6)*SK_TAS(2) - OP(19,24)*SK_TAS(2) + OP(19,7)*vd*SH_TAS(1)); -Kfusion(20) = SK_TAS(1)*(OP(20,5)*SH_TAS(3) - OP(20,23)*SH_TAS(3) + OP(20,6)*SK_TAS(2) - OP(20,24)*SK_TAS(2) + OP(20,7)*vd*SH_TAS(1)); -Kfusion(21) = SK_TAS(1)*(OP(21,5)*SH_TAS(3) - OP(21,23)*SH_TAS(3) + OP(21,6)*SK_TAS(2) - OP(21,24)*SK_TAS(2) + OP(21,7)*vd*SH_TAS(1)); -Kfusion(22) = SK_TAS(1)*(OP(22,5)*SH_TAS(3) - OP(22,23)*SH_TAS(3) + OP(22,6)*SK_TAS(2) - OP(22,24)*SK_TAS(2) + OP(22,7)*vd*SH_TAS(1)); -Kfusion(23) = SK_TAS(1)*(OP(23,5)*SH_TAS(3) - OP(23,23)*SH_TAS(3) + OP(23,6)*SK_TAS(2) - OP(23,24)*SK_TAS(2) + OP(23,7)*vd*SH_TAS(1)); -Kfusion(24) = SK_TAS(1)*(OP(24,5)*SH_TAS(3) - OP(24,23)*SH_TAS(3) + OP(24,6)*SK_TAS(2) - OP(24,24)*SK_TAS(2) + OP(24,7)*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(23,5)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,5)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,5)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,5)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,5)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,5)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,5)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,5)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,5)*(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(23,23)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,23)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,23)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,23)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,23)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,23)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,23)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,23)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,23)*(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(23,6)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,6)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,6)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,6)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,6)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,6)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,6)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,6)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,6)*(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(23,24)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,24)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,24)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,24)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,24)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,24)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,24)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,24)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,24)*(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(23,1)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,1)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,1)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,1)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,1)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,1)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,1)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,1)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,1)*(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(23,2)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,2)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,2)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,2)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,2)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,2)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,2)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,2)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,2)*(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(23,3)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,3)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,3)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,3)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,3)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,3)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,3)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,3)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,3)*(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(23,4)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,4)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,4)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,4)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,4)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,4)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,4)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,4)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,4)*(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(23,7)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,7)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,7)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,7)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,7)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,7)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,7)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,7)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,7)*(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(1,1)*SK_BETA(6) + OP(1,2)*SK_BETA(5) - OP(1,5)*SK_BETA(2) + OP(1,6)*SK_BETA(3) + OP(1,3)*SK_BETA(7) + OP(1,7)*SK_BETA(4) - OP(1,4)*SK_BETA(8) + OP(1,23)*SK_BETA(2) - OP(1,24)*SK_BETA(3)); -Kfusion(2) = SK_BETA(1)*(OP(2,1)*SK_BETA(6) + OP(2,2)*SK_BETA(5) - OP(2,5)*SK_BETA(2) + OP(2,6)*SK_BETA(3) + OP(2,3)*SK_BETA(7) + OP(2,7)*SK_BETA(4) - OP(2,4)*SK_BETA(8) + OP(2,23)*SK_BETA(2) - OP(2,24)*SK_BETA(3)); -Kfusion(3) = SK_BETA(1)*(OP(3,1)*SK_BETA(6) + OP(3,2)*SK_BETA(5) - OP(3,5)*SK_BETA(2) + OP(3,6)*SK_BETA(3) + OP(3,3)*SK_BETA(7) + OP(3,7)*SK_BETA(4) - OP(3,4)*SK_BETA(8) + OP(3,23)*SK_BETA(2) - OP(3,24)*SK_BETA(3)); -Kfusion(4) = SK_BETA(1)*(OP(4,1)*SK_BETA(6) + OP(4,2)*SK_BETA(5) - OP(4,5)*SK_BETA(2) + OP(4,6)*SK_BETA(3) + OP(4,3)*SK_BETA(7) + OP(4,7)*SK_BETA(4) - OP(4,4)*SK_BETA(8) + OP(4,23)*SK_BETA(2) - OP(4,24)*SK_BETA(3)); -Kfusion(5) = SK_BETA(1)*(OP(5,1)*SK_BETA(6) + OP(5,2)*SK_BETA(5) - OP(5,5)*SK_BETA(2) + OP(5,6)*SK_BETA(3) + OP(5,3)*SK_BETA(7) + OP(5,7)*SK_BETA(4) - OP(5,4)*SK_BETA(8) + OP(5,23)*SK_BETA(2) - OP(5,24)*SK_BETA(3)); -Kfusion(6) = SK_BETA(1)*(OP(6,1)*SK_BETA(6) + OP(6,2)*SK_BETA(5) - OP(6,5)*SK_BETA(2) + OP(6,6)*SK_BETA(3) + OP(6,3)*SK_BETA(7) + OP(6,7)*SK_BETA(4) - OP(6,4)*SK_BETA(8) + OP(6,23)*SK_BETA(2) - OP(6,24)*SK_BETA(3)); -Kfusion(7) = SK_BETA(1)*(OP(7,1)*SK_BETA(6) + OP(7,2)*SK_BETA(5) - OP(7,5)*SK_BETA(2) + OP(7,6)*SK_BETA(3) + OP(7,3)*SK_BETA(7) + OP(7,7)*SK_BETA(4) - OP(7,4)*SK_BETA(8) + OP(7,23)*SK_BETA(2) - OP(7,24)*SK_BETA(3)); -Kfusion(8) = SK_BETA(1)*(OP(8,1)*SK_BETA(6) + OP(8,2)*SK_BETA(5) - OP(8,5)*SK_BETA(2) + OP(8,6)*SK_BETA(3) + OP(8,3)*SK_BETA(7) + OP(8,7)*SK_BETA(4) - OP(8,4)*SK_BETA(8) + OP(8,23)*SK_BETA(2) - OP(8,24)*SK_BETA(3)); -Kfusion(9) = SK_BETA(1)*(OP(9,1)*SK_BETA(6) + OP(9,2)*SK_BETA(5) - OP(9,5)*SK_BETA(2) + OP(9,6)*SK_BETA(3) + OP(9,3)*SK_BETA(7) + OP(9,7)*SK_BETA(4) - OP(9,4)*SK_BETA(8) + OP(9,23)*SK_BETA(2) - OP(9,24)*SK_BETA(3)); -Kfusion(10) = SK_BETA(1)*(OP(10,1)*SK_BETA(6) + OP(10,2)*SK_BETA(5) - OP(10,5)*SK_BETA(2) + OP(10,6)*SK_BETA(3) + OP(10,3)*SK_BETA(7) + OP(10,7)*SK_BETA(4) - OP(10,4)*SK_BETA(8) + OP(10,23)*SK_BETA(2) - OP(10,24)*SK_BETA(3)); -Kfusion(11) = SK_BETA(1)*(OP(11,1)*SK_BETA(6) + OP(11,2)*SK_BETA(5) - OP(11,5)*SK_BETA(2) + OP(11,6)*SK_BETA(3) + OP(11,3)*SK_BETA(7) + OP(11,7)*SK_BETA(4) - OP(11,4)*SK_BETA(8) + OP(11,23)*SK_BETA(2) - OP(11,24)*SK_BETA(3)); -Kfusion(12) = SK_BETA(1)*(OP(12,1)*SK_BETA(6) + OP(12,2)*SK_BETA(5) - OP(12,5)*SK_BETA(2) + OP(12,6)*SK_BETA(3) + OP(12,3)*SK_BETA(7) + OP(12,7)*SK_BETA(4) - OP(12,4)*SK_BETA(8) + OP(12,23)*SK_BETA(2) - OP(12,24)*SK_BETA(3)); -Kfusion(13) = SK_BETA(1)*(OP(13,1)*SK_BETA(6) + OP(13,2)*SK_BETA(5) - OP(13,5)*SK_BETA(2) + OP(13,6)*SK_BETA(3) + OP(13,3)*SK_BETA(7) + OP(13,7)*SK_BETA(4) - OP(13,4)*SK_BETA(8) + OP(13,23)*SK_BETA(2) - OP(13,24)*SK_BETA(3)); -Kfusion(14) = SK_BETA(1)*(OP(14,1)*SK_BETA(6) + OP(14,2)*SK_BETA(5) - OP(14,5)*SK_BETA(2) + OP(14,6)*SK_BETA(3) + OP(14,3)*SK_BETA(7) + OP(14,7)*SK_BETA(4) - OP(14,4)*SK_BETA(8) + OP(14,23)*SK_BETA(2) - OP(14,24)*SK_BETA(3)); -Kfusion(15) = SK_BETA(1)*(OP(15,1)*SK_BETA(6) + OP(15,2)*SK_BETA(5) - OP(15,5)*SK_BETA(2) + OP(15,6)*SK_BETA(3) + OP(15,3)*SK_BETA(7) + OP(15,7)*SK_BETA(4) - OP(15,4)*SK_BETA(8) + OP(15,23)*SK_BETA(2) - OP(15,24)*SK_BETA(3)); -Kfusion(16) = SK_BETA(1)*(OP(16,1)*SK_BETA(6) + OP(16,2)*SK_BETA(5) - OP(16,5)*SK_BETA(2) + OP(16,6)*SK_BETA(3) + OP(16,3)*SK_BETA(7) + OP(16,7)*SK_BETA(4) - OP(16,4)*SK_BETA(8) + OP(16,23)*SK_BETA(2) - OP(16,24)*SK_BETA(3)); -Kfusion(17) = SK_BETA(1)*(OP(17,1)*SK_BETA(6) + OP(17,2)*SK_BETA(5) - OP(17,5)*SK_BETA(2) + OP(17,6)*SK_BETA(3) + OP(17,3)*SK_BETA(7) + OP(17,7)*SK_BETA(4) - OP(17,4)*SK_BETA(8) + OP(17,23)*SK_BETA(2) - OP(17,24)*SK_BETA(3)); -Kfusion(18) = SK_BETA(1)*(OP(18,1)*SK_BETA(6) + OP(18,2)*SK_BETA(5) - OP(18,5)*SK_BETA(2) + OP(18,6)*SK_BETA(3) + OP(18,3)*SK_BETA(7) + OP(18,7)*SK_BETA(4) - OP(18,4)*SK_BETA(8) + OP(18,23)*SK_BETA(2) - OP(18,24)*SK_BETA(3)); -Kfusion(19) = SK_BETA(1)*(OP(19,1)*SK_BETA(6) + OP(19,2)*SK_BETA(5) - OP(19,5)*SK_BETA(2) + OP(19,6)*SK_BETA(3) + OP(19,3)*SK_BETA(7) + OP(19,7)*SK_BETA(4) - OP(19,4)*SK_BETA(8) + OP(19,23)*SK_BETA(2) - OP(19,24)*SK_BETA(3)); -Kfusion(20) = SK_BETA(1)*(OP(20,1)*SK_BETA(6) + OP(20,2)*SK_BETA(5) - OP(20,5)*SK_BETA(2) + OP(20,6)*SK_BETA(3) + OP(20,3)*SK_BETA(7) + OP(20,7)*SK_BETA(4) - OP(20,4)*SK_BETA(8) + OP(20,23)*SK_BETA(2) - OP(20,24)*SK_BETA(3)); -Kfusion(21) = SK_BETA(1)*(OP(21,1)*SK_BETA(6) + OP(21,2)*SK_BETA(5) - OP(21,5)*SK_BETA(2) + OP(21,6)*SK_BETA(3) + OP(21,3)*SK_BETA(7) + OP(21,7)*SK_BETA(4) - OP(21,4)*SK_BETA(8) + OP(21,23)*SK_BETA(2) - OP(21,24)*SK_BETA(3)); -Kfusion(22) = SK_BETA(1)*(OP(22,1)*SK_BETA(6) + OP(22,2)*SK_BETA(5) - OP(22,5)*SK_BETA(2) + OP(22,6)*SK_BETA(3) + OP(22,3)*SK_BETA(7) + OP(22,7)*SK_BETA(4) - OP(22,4)*SK_BETA(8) + OP(22,23)*SK_BETA(2) - OP(22,24)*SK_BETA(3)); -Kfusion(23) = SK_BETA(1)*(OP(23,1)*SK_BETA(6) + OP(23,2)*SK_BETA(5) - OP(23,5)*SK_BETA(2) + OP(23,6)*SK_BETA(3) + OP(23,3)*SK_BETA(7) + OP(23,7)*SK_BETA(4) - OP(23,4)*SK_BETA(8) + OP(23,23)*SK_BETA(2) - OP(23,24)*SK_BETA(3)); -Kfusion(24) = SK_BETA(1)*(OP(24,1)*SK_BETA(6) + OP(24,2)*SK_BETA(5) - OP(24,5)*SK_BETA(2) + OP(24,6)*SK_BETA(3) + OP(24,3)*SK_BETA(7) + OP(24,7)*SK_BETA(4) - OP(24,4)*SK_BETA(8) + OP(24,23)*SK_BETA(2) - OP(24,24)*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(20,20) + R_MAG + OP(2,20)*SH_MAG(1) - OP(3,20)*SH_MAG(2) + OP(4,20)*SH_MAG(3) - OP(17,20)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + (2*q0*q3 + 2*q1*q2)*(OP(20,18) + OP(2,18)*SH_MAG(1) - OP(3,18)*SH_MAG(2) + OP(4,18)*SH_MAG(3) - OP(17,18)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,18)*(2*q0*q3 + 2*q1*q2) - OP(19,18)*(2*q0*q2 - 2*q1*q3) + OP(1,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(OP(20,19) + OP(2,19)*SH_MAG(1) - OP(3,19)*SH_MAG(2) + OP(4,19)*SH_MAG(3) - OP(17,19)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,19)*(2*q0*q3 + 2*q1*q2) - OP(19,19)*(2*q0*q2 - 2*q1*q3) + OP(1,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(20,1) + OP(2,1)*SH_MAG(1) - OP(3,1)*SH_MAG(2) + OP(4,1)*SH_MAG(3) - OP(17,1)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,1)*(2*q0*q3 + 2*q1*q2) - OP(19,1)*(2*q0*q2 - 2*q1*q3) + OP(1,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(18,20)*(2*q0*q3 + 2*q1*q2) - OP(19,20)*(2*q0*q2 - 2*q1*q3) + SH_MAG(1)*(OP(20,2) + OP(2,2)*SH_MAG(1) - OP(3,2)*SH_MAG(2) + OP(4,2)*SH_MAG(3) - OP(17,2)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,2)*(2*q0*q3 + 2*q1*q2) - OP(19,2)*(2*q0*q2 - 2*q1*q3) + OP(1,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(2)*(OP(20,3) + OP(2,3)*SH_MAG(1) - OP(3,3)*SH_MAG(2) + OP(4,3)*SH_MAG(3) - OP(17,3)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,3)*(2*q0*q3 + 2*q1*q2) - OP(19,3)*(2*q0*q2 - 2*q1*q3) + OP(1,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(3)*(OP(20,4) + OP(2,4)*SH_MAG(1) - OP(3,4)*SH_MAG(2) + OP(4,4)*SH_MAG(3) - OP(17,4)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,4)*(2*q0*q3 + 2*q1*q2) - OP(19,4)*(2*q0*q2 - 2*q1*q3) + OP(1,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7))*(OP(20,17) + OP(2,17)*SH_MAG(1) - OP(3,17)*SH_MAG(2) + OP(4,17)*SH_MAG(3) - OP(17,17)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,17)*(2*q0*q3 + 2*q1*q2) - OP(19,17)*(2*q0*q2 - 2*q1*q3) + OP(1,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(1,20)*(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(1,20) + OP(1,2)*SH_MAG(1) - OP(1,3)*SH_MAG(2) + OP(1,4)*SH_MAG(3) + OP(1,1)*SK_MX(3) - OP(1,17)*SK_MX(2) + OP(1,18)*SK_MX(5) - OP(1,19)*SK_MX(4)); -Kfusion(2) = SK_MX(1)*(OP(2,20) + OP(2,2)*SH_MAG(1) - OP(2,3)*SH_MAG(2) + OP(2,4)*SH_MAG(3) + OP(2,1)*SK_MX(3) - OP(2,17)*SK_MX(2) + OP(2,18)*SK_MX(5) - OP(2,19)*SK_MX(4)); -Kfusion(3) = SK_MX(1)*(OP(3,20) + OP(3,2)*SH_MAG(1) - OP(3,3)*SH_MAG(2) + OP(3,4)*SH_MAG(3) + OP(3,1)*SK_MX(3) - OP(3,17)*SK_MX(2) + OP(3,18)*SK_MX(5) - OP(3,19)*SK_MX(4)); -Kfusion(4) = SK_MX(1)*(OP(4,20) + OP(4,2)*SH_MAG(1) - OP(4,3)*SH_MAG(2) + OP(4,4)*SH_MAG(3) + OP(4,1)*SK_MX(3) - OP(4,17)*SK_MX(2) + OP(4,18)*SK_MX(5) - OP(4,19)*SK_MX(4)); -Kfusion(5) = SK_MX(1)*(OP(5,20) + OP(5,2)*SH_MAG(1) - OP(5,3)*SH_MAG(2) + OP(5,4)*SH_MAG(3) + OP(5,1)*SK_MX(3) - OP(5,17)*SK_MX(2) + OP(5,18)*SK_MX(5) - OP(5,19)*SK_MX(4)); -Kfusion(6) = SK_MX(1)*(OP(6,20) + OP(6,2)*SH_MAG(1) - OP(6,3)*SH_MAG(2) + OP(6,4)*SH_MAG(3) + OP(6,1)*SK_MX(3) - OP(6,17)*SK_MX(2) + OP(6,18)*SK_MX(5) - OP(6,19)*SK_MX(4)); -Kfusion(7) = SK_MX(1)*(OP(7,20) + OP(7,2)*SH_MAG(1) - OP(7,3)*SH_MAG(2) + OP(7,4)*SH_MAG(3) + OP(7,1)*SK_MX(3) - OP(7,17)*SK_MX(2) + OP(7,18)*SK_MX(5) - OP(7,19)*SK_MX(4)); -Kfusion(8) = SK_MX(1)*(OP(8,20) + OP(8,2)*SH_MAG(1) - OP(8,3)*SH_MAG(2) + OP(8,4)*SH_MAG(3) + OP(8,1)*SK_MX(3) - OP(8,17)*SK_MX(2) + OP(8,18)*SK_MX(5) - OP(8,19)*SK_MX(4)); -Kfusion(9) = SK_MX(1)*(OP(9,20) + OP(9,2)*SH_MAG(1) - OP(9,3)*SH_MAG(2) + OP(9,4)*SH_MAG(3) + OP(9,1)*SK_MX(3) - OP(9,17)*SK_MX(2) + OP(9,18)*SK_MX(5) - OP(9,19)*SK_MX(4)); -Kfusion(10) = SK_MX(1)*(OP(10,20) + OP(10,2)*SH_MAG(1) - OP(10,3)*SH_MAG(2) + OP(10,4)*SH_MAG(3) + OP(10,1)*SK_MX(3) - OP(10,17)*SK_MX(2) + OP(10,18)*SK_MX(5) - OP(10,19)*SK_MX(4)); -Kfusion(11) = SK_MX(1)*(OP(11,20) + OP(11,2)*SH_MAG(1) - OP(11,3)*SH_MAG(2) + OP(11,4)*SH_MAG(3) + OP(11,1)*SK_MX(3) - OP(11,17)*SK_MX(2) + OP(11,18)*SK_MX(5) - OP(11,19)*SK_MX(4)); -Kfusion(12) = SK_MX(1)*(OP(12,20) + OP(12,2)*SH_MAG(1) - OP(12,3)*SH_MAG(2) + OP(12,4)*SH_MAG(3) + OP(12,1)*SK_MX(3) - OP(12,17)*SK_MX(2) + OP(12,18)*SK_MX(5) - OP(12,19)*SK_MX(4)); -Kfusion(13) = SK_MX(1)*(OP(13,20) + OP(13,2)*SH_MAG(1) - OP(13,3)*SH_MAG(2) + OP(13,4)*SH_MAG(3) + OP(13,1)*SK_MX(3) - OP(13,17)*SK_MX(2) + OP(13,18)*SK_MX(5) - OP(13,19)*SK_MX(4)); -Kfusion(14) = SK_MX(1)*(OP(14,20) + OP(14,2)*SH_MAG(1) - OP(14,3)*SH_MAG(2) + OP(14,4)*SH_MAG(3) + OP(14,1)*SK_MX(3) - OP(14,17)*SK_MX(2) + OP(14,18)*SK_MX(5) - OP(14,19)*SK_MX(4)); -Kfusion(15) = SK_MX(1)*(OP(15,20) + OP(15,2)*SH_MAG(1) - OP(15,3)*SH_MAG(2) + OP(15,4)*SH_MAG(3) + OP(15,1)*SK_MX(3) - OP(15,17)*SK_MX(2) + OP(15,18)*SK_MX(5) - OP(15,19)*SK_MX(4)); -Kfusion(16) = SK_MX(1)*(OP(16,20) + OP(16,2)*SH_MAG(1) - OP(16,3)*SH_MAG(2) + OP(16,4)*SH_MAG(3) + OP(16,1)*SK_MX(3) - OP(16,17)*SK_MX(2) + OP(16,18)*SK_MX(5) - OP(16,19)*SK_MX(4)); -Kfusion(17) = SK_MX(1)*(OP(17,20) + OP(17,2)*SH_MAG(1) - OP(17,3)*SH_MAG(2) + OP(17,4)*SH_MAG(3) + OP(17,1)*SK_MX(3) - OP(17,17)*SK_MX(2) + OP(17,18)*SK_MX(5) - OP(17,19)*SK_MX(4)); -Kfusion(18) = SK_MX(1)*(OP(18,20) + OP(18,2)*SH_MAG(1) - OP(18,3)*SH_MAG(2) + OP(18,4)*SH_MAG(3) + OP(18,1)*SK_MX(3) - OP(18,17)*SK_MX(2) + OP(18,18)*SK_MX(5) - OP(18,19)*SK_MX(4)); -Kfusion(19) = SK_MX(1)*(OP(19,20) + OP(19,2)*SH_MAG(1) - OP(19,3)*SH_MAG(2) + OP(19,4)*SH_MAG(3) + OP(19,1)*SK_MX(3) - OP(19,17)*SK_MX(2) + OP(19,18)*SK_MX(5) - OP(19,19)*SK_MX(4)); -Kfusion(20) = SK_MX(1)*(OP(20,20) + OP(20,2)*SH_MAG(1) - OP(20,3)*SH_MAG(2) + OP(20,4)*SH_MAG(3) + OP(20,1)*SK_MX(3) - OP(20,17)*SK_MX(2) + OP(20,18)*SK_MX(5) - OP(20,19)*SK_MX(4)); -Kfusion(21) = SK_MX(1)*(OP(21,20) + OP(21,2)*SH_MAG(1) - OP(21,3)*SH_MAG(2) + OP(21,4)*SH_MAG(3) + OP(21,1)*SK_MX(3) - OP(21,17)*SK_MX(2) + OP(21,18)*SK_MX(5) - OP(21,19)*SK_MX(4)); -Kfusion(22) = SK_MX(1)*(OP(22,20) + OP(22,2)*SH_MAG(1) - OP(22,3)*SH_MAG(2) + OP(22,4)*SH_MAG(3) + OP(22,1)*SK_MX(3) - OP(22,17)*SK_MX(2) + OP(22,18)*SK_MX(5) - OP(22,19)*SK_MX(4)); -Kfusion(23) = SK_MX(1)*(OP(23,20) + OP(23,2)*SH_MAG(1) - OP(23,3)*SH_MAG(2) + OP(23,4)*SH_MAG(3) + OP(23,1)*SK_MX(3) - OP(23,17)*SK_MX(2) + OP(23,18)*SK_MX(5) - OP(23,19)*SK_MX(4)); -Kfusion(24) = SK_MX(1)*(OP(24,20) + OP(24,2)*SH_MAG(1) - OP(24,3)*SH_MAG(2) + OP(24,4)*SH_MAG(3) + OP(24,1)*SK_MX(3) - OP(24,17)*SK_MX(2) + OP(24,18)*SK_MX(5) - OP(24,19)*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(21,21) + R_MAG + OP(1,21)*SH_MAG(3) + OP(2,21)*SH_MAG(2) + OP(3,21)*SH_MAG(1) - OP(18,21)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - (2*q0*q3 - 2*q1*q2)*(OP(21,17) + OP(1,17)*SH_MAG(3) + OP(2,17)*SH_MAG(2) + OP(3,17)*SH_MAG(1) - OP(18,17)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,17)*(2*q0*q3 - 2*q1*q2) + OP(19,17)*(2*q0*q1 + 2*q2*q3) - OP(4,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(OP(21,19) + OP(1,19)*SH_MAG(3) + OP(2,19)*SH_MAG(2) + OP(3,19)*SH_MAG(1) - OP(18,19)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,19)*(2*q0*q3 - 2*q1*q2) + OP(19,19)*(2*q0*q1 + 2*q2*q3) - OP(4,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(21,4) + OP(1,4)*SH_MAG(3) + OP(2,4)*SH_MAG(2) + OP(3,4)*SH_MAG(1) - OP(18,4)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,4)*(2*q0*q3 - 2*q1*q2) + OP(19,4)*(2*q0*q1 + 2*q2*q3) - OP(4,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP(17,21)*(2*q0*q3 - 2*q1*q2) + OP(19,21)*(2*q0*q1 + 2*q2*q3) + SH_MAG(3)*(OP(21,1) + OP(1,1)*SH_MAG(3) + OP(2,1)*SH_MAG(2) + OP(3,1)*SH_MAG(1) - OP(18,1)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,1)*(2*q0*q3 - 2*q1*q2) + OP(19,1)*(2*q0*q1 + 2*q2*q3) - OP(4,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(2)*(OP(21,2) + OP(1,2)*SH_MAG(3) + OP(2,2)*SH_MAG(2) + OP(3,2)*SH_MAG(1) - OP(18,2)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,2)*(2*q0*q3 - 2*q1*q2) + OP(19,2)*(2*q0*q1 + 2*q2*q3) - OP(4,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP(21,3) + OP(1,3)*SH_MAG(3) + OP(2,3)*SH_MAG(2) + OP(3,3)*SH_MAG(1) - OP(18,3)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,3)*(2*q0*q3 - 2*q1*q2) + OP(19,3)*(2*q0*q1 + 2*q2*q3) - OP(4,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7))*(OP(21,18) + OP(1,18)*SH_MAG(3) + OP(2,18)*SH_MAG(2) + OP(3,18)*SH_MAG(1) - OP(18,18)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,18)*(2*q0*q3 - 2*q1*q2) + OP(19,18)*(2*q0*q1 + 2*q2*q3) - OP(4,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP(4,21)*(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(1,21) + OP(1,1)*SH_MAG(3) + OP(1,2)*SH_MAG(2) + OP(1,3)*SH_MAG(1) - OP(1,4)*SK_MY(3) - OP(1,18)*SK_MY(2) - OP(1,17)*SK_MY(4) + OP(1,19)*SK_MY(5)); -Kfusion(2) = SK_MY(1)*(OP(2,21) + OP(2,1)*SH_MAG(3) + OP(2,2)*SH_MAG(2) + OP(2,3)*SH_MAG(1) - OP(2,4)*SK_MY(3) - OP(2,18)*SK_MY(2) - OP(2,17)*SK_MY(4) + OP(2,19)*SK_MY(5)); -Kfusion(3) = SK_MY(1)*(OP(3,21) + OP(3,1)*SH_MAG(3) + OP(3,2)*SH_MAG(2) + OP(3,3)*SH_MAG(1) - OP(3,4)*SK_MY(3) - OP(3,18)*SK_MY(2) - OP(3,17)*SK_MY(4) + OP(3,19)*SK_MY(5)); -Kfusion(4) = SK_MY(1)*(OP(4,21) + OP(4,1)*SH_MAG(3) + OP(4,2)*SH_MAG(2) + OP(4,3)*SH_MAG(1) - OP(4,4)*SK_MY(3) - OP(4,18)*SK_MY(2) - OP(4,17)*SK_MY(4) + OP(4,19)*SK_MY(5)); -Kfusion(5) = SK_MY(1)*(OP(5,21) + OP(5,1)*SH_MAG(3) + OP(5,2)*SH_MAG(2) + OP(5,3)*SH_MAG(1) - OP(5,4)*SK_MY(3) - OP(5,18)*SK_MY(2) - OP(5,17)*SK_MY(4) + OP(5,19)*SK_MY(5)); -Kfusion(6) = SK_MY(1)*(OP(6,21) + OP(6,1)*SH_MAG(3) + OP(6,2)*SH_MAG(2) + OP(6,3)*SH_MAG(1) - OP(6,4)*SK_MY(3) - OP(6,18)*SK_MY(2) - OP(6,17)*SK_MY(4) + OP(6,19)*SK_MY(5)); -Kfusion(7) = SK_MY(1)*(OP(7,21) + OP(7,1)*SH_MAG(3) + OP(7,2)*SH_MAG(2) + OP(7,3)*SH_MAG(1) - OP(7,4)*SK_MY(3) - OP(7,18)*SK_MY(2) - OP(7,17)*SK_MY(4) + OP(7,19)*SK_MY(5)); -Kfusion(8) = SK_MY(1)*(OP(8,21) + OP(8,1)*SH_MAG(3) + OP(8,2)*SH_MAG(2) + OP(8,3)*SH_MAG(1) - OP(8,4)*SK_MY(3) - OP(8,18)*SK_MY(2) - OP(8,17)*SK_MY(4) + OP(8,19)*SK_MY(5)); -Kfusion(9) = SK_MY(1)*(OP(9,21) + OP(9,1)*SH_MAG(3) + OP(9,2)*SH_MAG(2) + OP(9,3)*SH_MAG(1) - OP(9,4)*SK_MY(3) - OP(9,18)*SK_MY(2) - OP(9,17)*SK_MY(4) + OP(9,19)*SK_MY(5)); -Kfusion(10) = SK_MY(1)*(OP(10,21) + OP(10,1)*SH_MAG(3) + OP(10,2)*SH_MAG(2) + OP(10,3)*SH_MAG(1) - OP(10,4)*SK_MY(3) - OP(10,18)*SK_MY(2) - OP(10,17)*SK_MY(4) + OP(10,19)*SK_MY(5)); -Kfusion(11) = SK_MY(1)*(OP(11,21) + OP(11,1)*SH_MAG(3) + OP(11,2)*SH_MAG(2) + OP(11,3)*SH_MAG(1) - OP(11,4)*SK_MY(3) - OP(11,18)*SK_MY(2) - OP(11,17)*SK_MY(4) + OP(11,19)*SK_MY(5)); -Kfusion(12) = SK_MY(1)*(OP(12,21) + OP(12,1)*SH_MAG(3) + OP(12,2)*SH_MAG(2) + OP(12,3)*SH_MAG(1) - OP(12,4)*SK_MY(3) - OP(12,18)*SK_MY(2) - OP(12,17)*SK_MY(4) + OP(12,19)*SK_MY(5)); -Kfusion(13) = SK_MY(1)*(OP(13,21) + OP(13,1)*SH_MAG(3) + OP(13,2)*SH_MAG(2) + OP(13,3)*SH_MAG(1) - OP(13,4)*SK_MY(3) - OP(13,18)*SK_MY(2) - OP(13,17)*SK_MY(4) + OP(13,19)*SK_MY(5)); -Kfusion(14) = SK_MY(1)*(OP(14,21) + OP(14,1)*SH_MAG(3) + OP(14,2)*SH_MAG(2) + OP(14,3)*SH_MAG(1) - OP(14,4)*SK_MY(3) - OP(14,18)*SK_MY(2) - OP(14,17)*SK_MY(4) + OP(14,19)*SK_MY(5)); -Kfusion(15) = SK_MY(1)*(OP(15,21) + OP(15,1)*SH_MAG(3) + OP(15,2)*SH_MAG(2) + OP(15,3)*SH_MAG(1) - OP(15,4)*SK_MY(3) - OP(15,18)*SK_MY(2) - OP(15,17)*SK_MY(4) + OP(15,19)*SK_MY(5)); -Kfusion(16) = SK_MY(1)*(OP(16,21) + OP(16,1)*SH_MAG(3) + OP(16,2)*SH_MAG(2) + OP(16,3)*SH_MAG(1) - OP(16,4)*SK_MY(3) - OP(16,18)*SK_MY(2) - OP(16,17)*SK_MY(4) + OP(16,19)*SK_MY(5)); -Kfusion(17) = SK_MY(1)*(OP(17,21) + OP(17,1)*SH_MAG(3) + OP(17,2)*SH_MAG(2) + OP(17,3)*SH_MAG(1) - OP(17,4)*SK_MY(3) - OP(17,18)*SK_MY(2) - OP(17,17)*SK_MY(4) + OP(17,19)*SK_MY(5)); -Kfusion(18) = SK_MY(1)*(OP(18,21) + OP(18,1)*SH_MAG(3) + OP(18,2)*SH_MAG(2) + OP(18,3)*SH_MAG(1) - OP(18,4)*SK_MY(3) - OP(18,18)*SK_MY(2) - OP(18,17)*SK_MY(4) + OP(18,19)*SK_MY(5)); -Kfusion(19) = SK_MY(1)*(OP(19,21) + OP(19,1)*SH_MAG(3) + OP(19,2)*SH_MAG(2) + OP(19,3)*SH_MAG(1) - OP(19,4)*SK_MY(3) - OP(19,18)*SK_MY(2) - OP(19,17)*SK_MY(4) + OP(19,19)*SK_MY(5)); -Kfusion(20) = SK_MY(1)*(OP(20,21) + OP(20,1)*SH_MAG(3) + OP(20,2)*SH_MAG(2) + OP(20,3)*SH_MAG(1) - OP(20,4)*SK_MY(3) - OP(20,18)*SK_MY(2) - OP(20,17)*SK_MY(4) + OP(20,19)*SK_MY(5)); -Kfusion(21) = SK_MY(1)*(OP(21,21) + OP(21,1)*SH_MAG(3) + OP(21,2)*SH_MAG(2) + OP(21,3)*SH_MAG(1) - OP(21,4)*SK_MY(3) - OP(21,18)*SK_MY(2) - OP(21,17)*SK_MY(4) + OP(21,19)*SK_MY(5)); -Kfusion(22) = SK_MY(1)*(OP(22,21) + OP(22,1)*SH_MAG(3) + OP(22,2)*SH_MAG(2) + OP(22,3)*SH_MAG(1) - OP(22,4)*SK_MY(3) - OP(22,18)*SK_MY(2) - OP(22,17)*SK_MY(4) + OP(22,19)*SK_MY(5)); -Kfusion(23) = SK_MY(1)*(OP(23,21) + OP(23,1)*SH_MAG(3) + OP(23,2)*SH_MAG(2) + OP(23,3)*SH_MAG(1) - OP(23,4)*SK_MY(3) - OP(23,18)*SK_MY(2) - OP(23,17)*SK_MY(4) + OP(23,19)*SK_MY(5)); -Kfusion(24) = SK_MY(1)*(OP(24,21) + OP(24,1)*SH_MAG(3) + OP(24,2)*SH_MAG(2) + OP(24,3)*SH_MAG(1) - OP(24,4)*SK_MY(3) - OP(24,18)*SK_MY(2) - OP(24,17)*SK_MY(4) + OP(24,19)*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(22,22) + R_MAG + OP(1,22)*SH_MAG(2) - OP(2,22)*SH_MAG(3) + OP(4,22)*SH_MAG(1) + OP(19,22)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + (2*q0*q2 + 2*q1*q3)*(OP(22,17) + OP(1,17)*SH_MAG(2) - OP(2,17)*SH_MAG(3) + OP(4,17)*SH_MAG(1) + OP(19,17)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,17)*(2*q0*q2 + 2*q1*q3) - OP(18,17)*(2*q0*q1 - 2*q2*q3) + OP(3,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(OP(22,18) + OP(1,18)*SH_MAG(2) - OP(2,18)*SH_MAG(3) + OP(4,18)*SH_MAG(1) + OP(19,18)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,18)*(2*q0*q2 + 2*q1*q3) - OP(18,18)*(2*q0*q1 - 2*q2*q3) + OP(3,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(22,3) + OP(1,3)*SH_MAG(2) - OP(2,3)*SH_MAG(3) + OP(4,3)*SH_MAG(1) + OP(19,3)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,3)*(2*q0*q2 + 2*q1*q3) - OP(18,3)*(2*q0*q1 - 2*q2*q3) + OP(3,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(17,22)*(2*q0*q2 + 2*q1*q3) - OP(18,22)*(2*q0*q1 - 2*q2*q3) + SH_MAG(2)*(OP(22,1) + OP(1,1)*SH_MAG(2) - OP(2,1)*SH_MAG(3) + OP(4,1)*SH_MAG(1) + OP(19,1)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,1)*(2*q0*q2 + 2*q1*q3) - OP(18,1)*(2*q0*q1 - 2*q2*q3) + OP(3,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(3)*(OP(22,2) + OP(1,2)*SH_MAG(2) - OP(2,2)*SH_MAG(3) + OP(4,2)*SH_MAG(1) + OP(19,2)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,2)*(2*q0*q2 + 2*q1*q3) - OP(18,2)*(2*q0*q1 - 2*q2*q3) + OP(3,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP(22,4) + OP(1,4)*SH_MAG(2) - OP(2,4)*SH_MAG(3) + OP(4,4)*SH_MAG(1) + OP(19,4)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,4)*(2*q0*q2 + 2*q1*q3) - OP(18,4)*(2*q0*q1 - 2*q2*q3) + OP(3,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7))*(OP(22,19) + OP(1,19)*SH_MAG(2) - OP(2,19)*SH_MAG(3) + OP(4,19)*SH_MAG(1) + OP(19,19)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,19)*(2*q0*q2 + 2*q1*q3) - OP(18,19)*(2*q0*q1 - 2*q2*q3) + OP(3,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(3,22)*(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(1,22) + OP(1,1)*SH_MAG(2) - OP(1,2)*SH_MAG(3) + OP(1,4)*SH_MAG(1) + OP(1,3)*SK_MZ(3) + OP(1,19)*SK_MZ(2) + OP(1,17)*SK_MZ(5) - OP(1,18)*SK_MZ(4)); -Kfusion(2) = SK_MZ(1)*(OP(2,22) + OP(2,1)*SH_MAG(2) - OP(2,2)*SH_MAG(3) + OP(2,4)*SH_MAG(1) + OP(2,3)*SK_MZ(3) + OP(2,19)*SK_MZ(2) + OP(2,17)*SK_MZ(5) - OP(2,18)*SK_MZ(4)); -Kfusion(3) = SK_MZ(1)*(OP(3,22) + OP(3,1)*SH_MAG(2) - OP(3,2)*SH_MAG(3) + OP(3,4)*SH_MAG(1) + OP(3,3)*SK_MZ(3) + OP(3,19)*SK_MZ(2) + OP(3,17)*SK_MZ(5) - OP(3,18)*SK_MZ(4)); -Kfusion(4) = SK_MZ(1)*(OP(4,22) + OP(4,1)*SH_MAG(2) - OP(4,2)*SH_MAG(3) + OP(4,4)*SH_MAG(1) + OP(4,3)*SK_MZ(3) + OP(4,19)*SK_MZ(2) + OP(4,17)*SK_MZ(5) - OP(4,18)*SK_MZ(4)); -Kfusion(5) = SK_MZ(1)*(OP(5,22) + OP(5,1)*SH_MAG(2) - OP(5,2)*SH_MAG(3) + OP(5,4)*SH_MAG(1) + OP(5,3)*SK_MZ(3) + OP(5,19)*SK_MZ(2) + OP(5,17)*SK_MZ(5) - OP(5,18)*SK_MZ(4)); -Kfusion(6) = SK_MZ(1)*(OP(6,22) + OP(6,1)*SH_MAG(2) - OP(6,2)*SH_MAG(3) + OP(6,4)*SH_MAG(1) + OP(6,3)*SK_MZ(3) + OP(6,19)*SK_MZ(2) + OP(6,17)*SK_MZ(5) - OP(6,18)*SK_MZ(4)); -Kfusion(7) = SK_MZ(1)*(OP(7,22) + OP(7,1)*SH_MAG(2) - OP(7,2)*SH_MAG(3) + OP(7,4)*SH_MAG(1) + OP(7,3)*SK_MZ(3) + OP(7,19)*SK_MZ(2) + OP(7,17)*SK_MZ(5) - OP(7,18)*SK_MZ(4)); -Kfusion(8) = SK_MZ(1)*(OP(8,22) + OP(8,1)*SH_MAG(2) - OP(8,2)*SH_MAG(3) + OP(8,4)*SH_MAG(1) + OP(8,3)*SK_MZ(3) + OP(8,19)*SK_MZ(2) + OP(8,17)*SK_MZ(5) - OP(8,18)*SK_MZ(4)); -Kfusion(9) = SK_MZ(1)*(OP(9,22) + OP(9,1)*SH_MAG(2) - OP(9,2)*SH_MAG(3) + OP(9,4)*SH_MAG(1) + OP(9,3)*SK_MZ(3) + OP(9,19)*SK_MZ(2) + OP(9,17)*SK_MZ(5) - OP(9,18)*SK_MZ(4)); -Kfusion(10) = SK_MZ(1)*(OP(10,22) + OP(10,1)*SH_MAG(2) - OP(10,2)*SH_MAG(3) + OP(10,4)*SH_MAG(1) + OP(10,3)*SK_MZ(3) + OP(10,19)*SK_MZ(2) + OP(10,17)*SK_MZ(5) - OP(10,18)*SK_MZ(4)); -Kfusion(11) = SK_MZ(1)*(OP(11,22) + OP(11,1)*SH_MAG(2) - OP(11,2)*SH_MAG(3) + OP(11,4)*SH_MAG(1) + OP(11,3)*SK_MZ(3) + OP(11,19)*SK_MZ(2) + OP(11,17)*SK_MZ(5) - OP(11,18)*SK_MZ(4)); -Kfusion(12) = SK_MZ(1)*(OP(12,22) + OP(12,1)*SH_MAG(2) - OP(12,2)*SH_MAG(3) + OP(12,4)*SH_MAG(1) + OP(12,3)*SK_MZ(3) + OP(12,19)*SK_MZ(2) + OP(12,17)*SK_MZ(5) - OP(12,18)*SK_MZ(4)); -Kfusion(13) = SK_MZ(1)*(OP(13,22) + OP(13,1)*SH_MAG(2) - OP(13,2)*SH_MAG(3) + OP(13,4)*SH_MAG(1) + OP(13,3)*SK_MZ(3) + OP(13,19)*SK_MZ(2) + OP(13,17)*SK_MZ(5) - OP(13,18)*SK_MZ(4)); -Kfusion(14) = SK_MZ(1)*(OP(14,22) + OP(14,1)*SH_MAG(2) - OP(14,2)*SH_MAG(3) + OP(14,4)*SH_MAG(1) + OP(14,3)*SK_MZ(3) + OP(14,19)*SK_MZ(2) + OP(14,17)*SK_MZ(5) - OP(14,18)*SK_MZ(4)); -Kfusion(15) = SK_MZ(1)*(OP(15,22) + OP(15,1)*SH_MAG(2) - OP(15,2)*SH_MAG(3) + OP(15,4)*SH_MAG(1) + OP(15,3)*SK_MZ(3) + OP(15,19)*SK_MZ(2) + OP(15,17)*SK_MZ(5) - OP(15,18)*SK_MZ(4)); -Kfusion(16) = SK_MZ(1)*(OP(16,22) + OP(16,1)*SH_MAG(2) - OP(16,2)*SH_MAG(3) + OP(16,4)*SH_MAG(1) + OP(16,3)*SK_MZ(3) + OP(16,19)*SK_MZ(2) + OP(16,17)*SK_MZ(5) - OP(16,18)*SK_MZ(4)); -Kfusion(17) = SK_MZ(1)*(OP(17,22) + OP(17,1)*SH_MAG(2) - OP(17,2)*SH_MAG(3) + OP(17,4)*SH_MAG(1) + OP(17,3)*SK_MZ(3) + OP(17,19)*SK_MZ(2) + OP(17,17)*SK_MZ(5) - OP(17,18)*SK_MZ(4)); -Kfusion(18) = SK_MZ(1)*(OP(18,22) + OP(18,1)*SH_MAG(2) - OP(18,2)*SH_MAG(3) + OP(18,4)*SH_MAG(1) + OP(18,3)*SK_MZ(3) + OP(18,19)*SK_MZ(2) + OP(18,17)*SK_MZ(5) - OP(18,18)*SK_MZ(4)); -Kfusion(19) = SK_MZ(1)*(OP(19,22) + OP(19,1)*SH_MAG(2) - OP(19,2)*SH_MAG(3) + OP(19,4)*SH_MAG(1) + OP(19,3)*SK_MZ(3) + OP(19,19)*SK_MZ(2) + OP(19,17)*SK_MZ(5) - OP(19,18)*SK_MZ(4)); -Kfusion(20) = SK_MZ(1)*(OP(20,22) + OP(20,1)*SH_MAG(2) - OP(20,2)*SH_MAG(3) + OP(20,4)*SH_MAG(1) + OP(20,3)*SK_MZ(3) + OP(20,19)*SK_MZ(2) + OP(20,17)*SK_MZ(5) - OP(20,18)*SK_MZ(4)); -Kfusion(21) = SK_MZ(1)*(OP(21,22) + OP(21,1)*SH_MAG(2) - OP(21,2)*SH_MAG(3) + OP(21,4)*SH_MAG(1) + OP(21,3)*SK_MZ(3) + OP(21,19)*SK_MZ(2) + OP(21,17)*SK_MZ(5) - OP(21,18)*SK_MZ(4)); -Kfusion(22) = SK_MZ(1)*(OP(22,22) + OP(22,1)*SH_MAG(2) - OP(22,2)*SH_MAG(3) + OP(22,4)*SH_MAG(1) + OP(22,3)*SK_MZ(3) + OP(22,19)*SK_MZ(2) + OP(22,17)*SK_MZ(5) - OP(22,18)*SK_MZ(4)); -Kfusion(23) = SK_MZ(1)*(OP(23,22) + OP(23,1)*SH_MAG(2) - OP(23,2)*SH_MAG(3) + OP(23,4)*SH_MAG(1) + OP(23,3)*SK_MZ(3) + OP(23,19)*SK_MZ(2) + OP(23,17)*SK_MZ(5) - OP(23,18)*SK_MZ(4)); -Kfusion(24) = SK_MZ(1)*(OP(24,22) + OP(24,1)*SH_MAG(2) - OP(24,2)*SH_MAG(3) + OP(24,4)*SH_MAG(1) + OP(24,3)*SK_MZ(3) + OP(24,19)*SK_MZ(2) + OP(24,17)*SK_MZ(5) - OP(24,18)*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(5,5)*SH_ACCX(1) + Kaccx*OP(6,5)*SH_ACCX(4) - Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(24,5)*SH_ACCX(4) - Kaccx*OP(7,5)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,5)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,5)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,5)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,5)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*SH_ACCX(4)*(Kaccx*OP(5,6)*SH_ACCX(1) + Kaccx*OP(6,6)*SH_ACCX(4) - Kaccx*OP(23,6)*SH_ACCX(1) - Kaccx*OP(24,6)*SH_ACCX(4) - Kaccx*OP(7,6)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,6)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,6)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,6)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,6)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(1)*(Kaccx*OP(5,23)*SH_ACCX(1) + Kaccx*OP(6,23)*SH_ACCX(4) - Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(24,23)*SH_ACCX(4) - Kaccx*OP(7,23)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,23)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,23)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,23)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,23)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(4)*(Kaccx*OP(5,24)*SH_ACCX(1) + Kaccx*OP(6,24)*SH_ACCX(4) - Kaccx*OP(23,24)*SH_ACCX(1) - Kaccx*OP(24,24)*SH_ACCX(4) - Kaccx*OP(7,24)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,24)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,24)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,24)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,24)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*OP(5,7)*SH_ACCX(1) + Kaccx*OP(6,7)*SH_ACCX(4) - Kaccx*OP(23,7)*SH_ACCX(1) - Kaccx*OP(24,7)*SH_ACCX(4) - Kaccx*OP(7,7)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,7)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,7)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,7)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,7)*(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(5,1)*SH_ACCX(1) + Kaccx*OP(6,1)*SH_ACCX(4) - Kaccx*OP(23,1)*SH_ACCX(1) - Kaccx*OP(24,1)*SH_ACCX(4) - Kaccx*OP(7,1)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,1)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,1)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,1)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,1)*(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(5,2)*SH_ACCX(1) + Kaccx*OP(6,2)*SH_ACCX(4) - Kaccx*OP(23,2)*SH_ACCX(1) - Kaccx*OP(24,2)*SH_ACCX(4) - Kaccx*OP(7,2)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,2)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,2)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,2)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,2)*(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(5,3)*SH_ACCX(1) + Kaccx*OP(6,3)*SH_ACCX(4) - Kaccx*OP(23,3)*SH_ACCX(1) - Kaccx*OP(24,3)*SH_ACCX(4) - Kaccx*OP(7,3)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,3)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,3)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,3)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,3)*(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(5,4)*SH_ACCX(1) + Kaccx*OP(6,4)*SH_ACCX(4) - Kaccx*OP(23,4)*SH_ACCX(1) - Kaccx*OP(24,4)*SH_ACCX(4) - Kaccx*OP(7,4)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,4)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,4)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,4)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,4)*(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(1,5)*SH_ACCX(1) - Kaccx*OP(1,23)*SH_ACCX(1) + Kaccx*OP(1,1)*SK_ACCX(4) - Kaccx*OP(1,3)*SK_ACCX(3) + Kaccx*OP(1,4)*SK_ACCX(2) + Kaccx*OP(1,2)*SK_ACCX(5) + Kaccx*OP(1,6)*SK_ACCX(7) - Kaccx*OP(1,7)*SK_ACCX(6) - Kaccx*OP(1,24)*SK_ACCX(7)); -Kfusion(2) = -SK_ACCX(1)*(Kaccx*OP(2,5)*SH_ACCX(1) - Kaccx*OP(2,23)*SH_ACCX(1) + Kaccx*OP(2,1)*SK_ACCX(4) - Kaccx*OP(2,3)*SK_ACCX(3) + Kaccx*OP(2,4)*SK_ACCX(2) + Kaccx*OP(2,2)*SK_ACCX(5) + Kaccx*OP(2,6)*SK_ACCX(7) - Kaccx*OP(2,7)*SK_ACCX(6) - Kaccx*OP(2,24)*SK_ACCX(7)); -Kfusion(3) = -SK_ACCX(1)*(Kaccx*OP(3,5)*SH_ACCX(1) - Kaccx*OP(3,23)*SH_ACCX(1) + Kaccx*OP(3,1)*SK_ACCX(4) - Kaccx*OP(3,3)*SK_ACCX(3) + Kaccx*OP(3,4)*SK_ACCX(2) + Kaccx*OP(3,2)*SK_ACCX(5) + Kaccx*OP(3,6)*SK_ACCX(7) - Kaccx*OP(3,7)*SK_ACCX(6) - Kaccx*OP(3,24)*SK_ACCX(7)); -Kfusion(4) = -SK_ACCX(1)*(Kaccx*OP(4,5)*SH_ACCX(1) - Kaccx*OP(4,23)*SH_ACCX(1) + Kaccx*OP(4,1)*SK_ACCX(4) - Kaccx*OP(4,3)*SK_ACCX(3) + Kaccx*OP(4,4)*SK_ACCX(2) + Kaccx*OP(4,2)*SK_ACCX(5) + Kaccx*OP(4,6)*SK_ACCX(7) - Kaccx*OP(4,7)*SK_ACCX(6) - Kaccx*OP(4,24)*SK_ACCX(7)); -Kfusion(5) = -SK_ACCX(1)*(Kaccx*OP(5,5)*SH_ACCX(1) - Kaccx*OP(5,23)*SH_ACCX(1) + Kaccx*OP(5,1)*SK_ACCX(4) - Kaccx*OP(5,3)*SK_ACCX(3) + Kaccx*OP(5,4)*SK_ACCX(2) + Kaccx*OP(5,2)*SK_ACCX(5) + Kaccx*OP(5,6)*SK_ACCX(7) - Kaccx*OP(5,7)*SK_ACCX(6) - Kaccx*OP(5,24)*SK_ACCX(7)); -Kfusion(6) = -SK_ACCX(1)*(Kaccx*OP(6,5)*SH_ACCX(1) - Kaccx*OP(6,23)*SH_ACCX(1) + Kaccx*OP(6,1)*SK_ACCX(4) - Kaccx*OP(6,3)*SK_ACCX(3) + Kaccx*OP(6,4)*SK_ACCX(2) + Kaccx*OP(6,2)*SK_ACCX(5) + Kaccx*OP(6,6)*SK_ACCX(7) - Kaccx*OP(6,7)*SK_ACCX(6) - Kaccx*OP(6,24)*SK_ACCX(7)); -Kfusion(7) = -SK_ACCX(1)*(Kaccx*OP(7,5)*SH_ACCX(1) - Kaccx*OP(7,23)*SH_ACCX(1) + Kaccx*OP(7,1)*SK_ACCX(4) - Kaccx*OP(7,3)*SK_ACCX(3) + Kaccx*OP(7,4)*SK_ACCX(2) + Kaccx*OP(7,2)*SK_ACCX(5) + Kaccx*OP(7,6)*SK_ACCX(7) - Kaccx*OP(7,7)*SK_ACCX(6) - Kaccx*OP(7,24)*SK_ACCX(7)); -Kfusion(8) = -SK_ACCX(1)*(Kaccx*OP(8,5)*SH_ACCX(1) - Kaccx*OP(8,23)*SH_ACCX(1) + Kaccx*OP(8,1)*SK_ACCX(4) - Kaccx*OP(8,3)*SK_ACCX(3) + Kaccx*OP(8,4)*SK_ACCX(2) + Kaccx*OP(8,2)*SK_ACCX(5) + Kaccx*OP(8,6)*SK_ACCX(7) - Kaccx*OP(8,7)*SK_ACCX(6) - Kaccx*OP(8,24)*SK_ACCX(7)); -Kfusion(9) = -SK_ACCX(1)*(Kaccx*OP(9,5)*SH_ACCX(1) - Kaccx*OP(9,23)*SH_ACCX(1) + Kaccx*OP(9,1)*SK_ACCX(4) - Kaccx*OP(9,3)*SK_ACCX(3) + Kaccx*OP(9,4)*SK_ACCX(2) + Kaccx*OP(9,2)*SK_ACCX(5) + Kaccx*OP(9,6)*SK_ACCX(7) - Kaccx*OP(9,7)*SK_ACCX(6) - Kaccx*OP(9,24)*SK_ACCX(7)); -Kfusion(10) = -SK_ACCX(1)*(Kaccx*OP(10,5)*SH_ACCX(1) - Kaccx*OP(10,23)*SH_ACCX(1) + Kaccx*OP(10,1)*SK_ACCX(4) - Kaccx*OP(10,3)*SK_ACCX(3) + Kaccx*OP(10,4)*SK_ACCX(2) + Kaccx*OP(10,2)*SK_ACCX(5) + Kaccx*OP(10,6)*SK_ACCX(7) - Kaccx*OP(10,7)*SK_ACCX(6) - Kaccx*OP(10,24)*SK_ACCX(7)); -Kfusion(11) = -SK_ACCX(1)*(Kaccx*OP(11,5)*SH_ACCX(1) - Kaccx*OP(11,23)*SH_ACCX(1) + Kaccx*OP(11,1)*SK_ACCX(4) - Kaccx*OP(11,3)*SK_ACCX(3) + Kaccx*OP(11,4)*SK_ACCX(2) + Kaccx*OP(11,2)*SK_ACCX(5) + Kaccx*OP(11,6)*SK_ACCX(7) - Kaccx*OP(11,7)*SK_ACCX(6) - Kaccx*OP(11,24)*SK_ACCX(7)); -Kfusion(12) = -SK_ACCX(1)*(Kaccx*OP(12,5)*SH_ACCX(1) - Kaccx*OP(12,23)*SH_ACCX(1) + Kaccx*OP(12,1)*SK_ACCX(4) - Kaccx*OP(12,3)*SK_ACCX(3) + Kaccx*OP(12,4)*SK_ACCX(2) + Kaccx*OP(12,2)*SK_ACCX(5) + Kaccx*OP(12,6)*SK_ACCX(7) - Kaccx*OP(12,7)*SK_ACCX(6) - Kaccx*OP(12,24)*SK_ACCX(7)); -Kfusion(13) = -SK_ACCX(1)*(Kaccx*OP(13,5)*SH_ACCX(1) - Kaccx*OP(13,23)*SH_ACCX(1) + Kaccx*OP(13,1)*SK_ACCX(4) - Kaccx*OP(13,3)*SK_ACCX(3) + Kaccx*OP(13,4)*SK_ACCX(2) + Kaccx*OP(13,2)*SK_ACCX(5) + Kaccx*OP(13,6)*SK_ACCX(7) - Kaccx*OP(13,7)*SK_ACCX(6) - Kaccx*OP(13,24)*SK_ACCX(7)); -Kfusion(14) = -SK_ACCX(1)*(Kaccx*OP(14,5)*SH_ACCX(1) - Kaccx*OP(14,23)*SH_ACCX(1) + Kaccx*OP(14,1)*SK_ACCX(4) - Kaccx*OP(14,3)*SK_ACCX(3) + Kaccx*OP(14,4)*SK_ACCX(2) + Kaccx*OP(14,2)*SK_ACCX(5) + Kaccx*OP(14,6)*SK_ACCX(7) - Kaccx*OP(14,7)*SK_ACCX(6) - Kaccx*OP(14,24)*SK_ACCX(7)); -Kfusion(15) = -SK_ACCX(1)*(Kaccx*OP(15,5)*SH_ACCX(1) - Kaccx*OP(15,23)*SH_ACCX(1) + Kaccx*OP(15,1)*SK_ACCX(4) - Kaccx*OP(15,3)*SK_ACCX(3) + Kaccx*OP(15,4)*SK_ACCX(2) + Kaccx*OP(15,2)*SK_ACCX(5) + Kaccx*OP(15,6)*SK_ACCX(7) - Kaccx*OP(15,7)*SK_ACCX(6) - Kaccx*OP(15,24)*SK_ACCX(7)); -Kfusion(16) = -SK_ACCX(1)*(Kaccx*OP(16,5)*SH_ACCX(1) - Kaccx*OP(16,23)*SH_ACCX(1) + Kaccx*OP(16,1)*SK_ACCX(4) - Kaccx*OP(16,3)*SK_ACCX(3) + Kaccx*OP(16,4)*SK_ACCX(2) + Kaccx*OP(16,2)*SK_ACCX(5) + Kaccx*OP(16,6)*SK_ACCX(7) - Kaccx*OP(16,7)*SK_ACCX(6) - Kaccx*OP(16,24)*SK_ACCX(7)); -Kfusion(17) = -SK_ACCX(1)*(Kaccx*OP(17,5)*SH_ACCX(1) - Kaccx*OP(17,23)*SH_ACCX(1) + Kaccx*OP(17,1)*SK_ACCX(4) - Kaccx*OP(17,3)*SK_ACCX(3) + Kaccx*OP(17,4)*SK_ACCX(2) + Kaccx*OP(17,2)*SK_ACCX(5) + Kaccx*OP(17,6)*SK_ACCX(7) - Kaccx*OP(17,7)*SK_ACCX(6) - Kaccx*OP(17,24)*SK_ACCX(7)); -Kfusion(18) = -SK_ACCX(1)*(Kaccx*OP(18,5)*SH_ACCX(1) - Kaccx*OP(18,23)*SH_ACCX(1) + Kaccx*OP(18,1)*SK_ACCX(4) - Kaccx*OP(18,3)*SK_ACCX(3) + Kaccx*OP(18,4)*SK_ACCX(2) + Kaccx*OP(18,2)*SK_ACCX(5) + Kaccx*OP(18,6)*SK_ACCX(7) - Kaccx*OP(18,7)*SK_ACCX(6) - Kaccx*OP(18,24)*SK_ACCX(7)); -Kfusion(19) = -SK_ACCX(1)*(Kaccx*OP(19,5)*SH_ACCX(1) - Kaccx*OP(19,23)*SH_ACCX(1) + Kaccx*OP(19,1)*SK_ACCX(4) - Kaccx*OP(19,3)*SK_ACCX(3) + Kaccx*OP(19,4)*SK_ACCX(2) + Kaccx*OP(19,2)*SK_ACCX(5) + Kaccx*OP(19,6)*SK_ACCX(7) - Kaccx*OP(19,7)*SK_ACCX(6) - Kaccx*OP(19,24)*SK_ACCX(7)); -Kfusion(20) = -SK_ACCX(1)*(Kaccx*OP(20,5)*SH_ACCX(1) - Kaccx*OP(20,23)*SH_ACCX(1) + Kaccx*OP(20,1)*SK_ACCX(4) - Kaccx*OP(20,3)*SK_ACCX(3) + Kaccx*OP(20,4)*SK_ACCX(2) + Kaccx*OP(20,2)*SK_ACCX(5) + Kaccx*OP(20,6)*SK_ACCX(7) - Kaccx*OP(20,7)*SK_ACCX(6) - Kaccx*OP(20,24)*SK_ACCX(7)); -Kfusion(21) = -SK_ACCX(1)*(Kaccx*OP(21,5)*SH_ACCX(1) - Kaccx*OP(21,23)*SH_ACCX(1) + Kaccx*OP(21,1)*SK_ACCX(4) - Kaccx*OP(21,3)*SK_ACCX(3) + Kaccx*OP(21,4)*SK_ACCX(2) + Kaccx*OP(21,2)*SK_ACCX(5) + Kaccx*OP(21,6)*SK_ACCX(7) - Kaccx*OP(21,7)*SK_ACCX(6) - Kaccx*OP(21,24)*SK_ACCX(7)); -Kfusion(22) = -SK_ACCX(1)*(Kaccx*OP(22,5)*SH_ACCX(1) - Kaccx*OP(22,23)*SH_ACCX(1) + Kaccx*OP(22,1)*SK_ACCX(4) - Kaccx*OP(22,3)*SK_ACCX(3) + Kaccx*OP(22,4)*SK_ACCX(2) + Kaccx*OP(22,2)*SK_ACCX(5) + Kaccx*OP(22,6)*SK_ACCX(7) - Kaccx*OP(22,7)*SK_ACCX(6) - Kaccx*OP(22,24)*SK_ACCX(7)); -Kfusion(23) = -SK_ACCX(1)*(Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(23,23)*SH_ACCX(1) + Kaccx*OP(23,1)*SK_ACCX(4) - Kaccx*OP(23,3)*SK_ACCX(3) + Kaccx*OP(23,4)*SK_ACCX(2) + Kaccx*OP(23,2)*SK_ACCX(5) + Kaccx*OP(23,6)*SK_ACCX(7) - Kaccx*OP(23,7)*SK_ACCX(6) - Kaccx*OP(23,24)*SK_ACCX(7)); -Kfusion(24) = -SK_ACCX(1)*(Kaccx*OP(24,5)*SH_ACCX(1) - Kaccx*OP(24,23)*SH_ACCX(1) + Kaccx*OP(24,1)*SK_ACCX(4) - Kaccx*OP(24,3)*SK_ACCX(3) + Kaccx*OP(24,4)*SK_ACCX(2) + Kaccx*OP(24,2)*SK_ACCX(5) + Kaccx*OP(24,6)*SK_ACCX(7) - Kaccx*OP(24,7)*SK_ACCX(6) - Kaccx*OP(24,24)*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(6,6)*SH_ACCY(1) - Kaccy*OP(24,6)*SH_ACCY(1) - Kaccy*OP(5,6)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,6)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,6)*(q0*q3 - q1*q2) + Kaccy*OP(1,6)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,6)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,6)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,6)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*SH_ACCY(1)*(Kaccy*OP(6,24)*SH_ACCY(1) - Kaccy*OP(24,24)*SH_ACCY(1) - Kaccy*OP(5,24)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,24)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,24)*(q0*q3 - q1*q2) + Kaccy*OP(1,24)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,24)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,24)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,24)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*OP(6,5)*SH_ACCY(1) - Kaccy*OP(24,5)*SH_ACCY(1) - Kaccy*OP(5,5)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,5)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,5)*(q0*q3 - q1*q2) + Kaccy*OP(1,5)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,5)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,5)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,5)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*OP(6,7)*SH_ACCY(1) - Kaccy*OP(24,7)*SH_ACCY(1) - Kaccy*OP(5,7)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,7)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,7)*(q0*q3 - q1*q2) + Kaccy*OP(1,7)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,7)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,7)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,7)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP(6,23)*SH_ACCY(1) - Kaccy*OP(24,23)*SH_ACCY(1) - Kaccy*OP(5,23)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,23)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,23)*(q0*q3 - q1*q2) + Kaccy*OP(1,23)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,23)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,23)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,23)*(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(6,1)*SH_ACCY(1) - Kaccy*OP(24,1)*SH_ACCY(1) - Kaccy*OP(5,1)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,1)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,1)*(q0*q3 - q1*q2) + Kaccy*OP(1,1)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,1)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,1)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,1)*(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(6,2)*SH_ACCY(1) - Kaccy*OP(24,2)*SH_ACCY(1) - Kaccy*OP(5,2)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,2)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,2)*(q0*q3 - q1*q2) + Kaccy*OP(1,2)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,2)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,2)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,2)*(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(6,3)*SH_ACCY(1) - Kaccy*OP(24,3)*SH_ACCY(1) - Kaccy*OP(5,3)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,3)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,3)*(q0*q3 - q1*q2) + Kaccy*OP(1,3)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,3)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,3)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,3)*(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(6,4)*SH_ACCY(1) - Kaccy*OP(24,4)*SH_ACCY(1) - Kaccy*OP(5,4)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,4)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,4)*(q0*q3 - q1*q2) + Kaccy*OP(1,4)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,4)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,4)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,4)*(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(1,1)*SK_ACCY(4) + Kaccy*OP(1,2)*SK_ACCY(3) - Kaccy*OP(1,4)*SK_ACCY(2) + Kaccy*OP(1,3)*SK_ACCY(5) - Kaccy*OP(1,5)*SK_ACCY(6) + Kaccy*OP(1,6)*SK_ACCY(9) + Kaccy*OP(1,7)*SK_ACCY(8) + 2*Kaccy*OP(1,23)*SK_ACCY(7) - Kaccy*OP(1,24)*SK_ACCY(9)); -Kfusion(2) = -SK_ACCY(1)*(Kaccy*OP(2,1)*SK_ACCY(4) + Kaccy*OP(2,2)*SK_ACCY(3) - Kaccy*OP(2,4)*SK_ACCY(2) + Kaccy*OP(2,3)*SK_ACCY(5) - Kaccy*OP(2,5)*SK_ACCY(6) + Kaccy*OP(2,6)*SK_ACCY(9) + Kaccy*OP(2,7)*SK_ACCY(8) + 2*Kaccy*OP(2,23)*SK_ACCY(7) - Kaccy*OP(2,24)*SK_ACCY(9)); -Kfusion(3) = -SK_ACCY(1)*(Kaccy*OP(3,1)*SK_ACCY(4) + Kaccy*OP(3,2)*SK_ACCY(3) - Kaccy*OP(3,4)*SK_ACCY(2) + Kaccy*OP(3,3)*SK_ACCY(5) - Kaccy*OP(3,5)*SK_ACCY(6) + Kaccy*OP(3,6)*SK_ACCY(9) + Kaccy*OP(3,7)*SK_ACCY(8) + 2*Kaccy*OP(3,23)*SK_ACCY(7) - Kaccy*OP(3,24)*SK_ACCY(9)); -Kfusion(4) = -SK_ACCY(1)*(Kaccy*OP(4,1)*SK_ACCY(4) + Kaccy*OP(4,2)*SK_ACCY(3) - Kaccy*OP(4,4)*SK_ACCY(2) + Kaccy*OP(4,3)*SK_ACCY(5) - Kaccy*OP(4,5)*SK_ACCY(6) + Kaccy*OP(4,6)*SK_ACCY(9) + Kaccy*OP(4,7)*SK_ACCY(8) + 2*Kaccy*OP(4,23)*SK_ACCY(7) - Kaccy*OP(4,24)*SK_ACCY(9)); -Kfusion(5) = -SK_ACCY(1)*(Kaccy*OP(5,1)*SK_ACCY(4) + Kaccy*OP(5,2)*SK_ACCY(3) - Kaccy*OP(5,4)*SK_ACCY(2) + Kaccy*OP(5,3)*SK_ACCY(5) - Kaccy*OP(5,5)*SK_ACCY(6) + Kaccy*OP(5,6)*SK_ACCY(9) + Kaccy*OP(5,7)*SK_ACCY(8) + 2*Kaccy*OP(5,23)*SK_ACCY(7) - Kaccy*OP(5,24)*SK_ACCY(9)); -Kfusion(6) = -SK_ACCY(1)*(Kaccy*OP(6,1)*SK_ACCY(4) + Kaccy*OP(6,2)*SK_ACCY(3) - Kaccy*OP(6,4)*SK_ACCY(2) + Kaccy*OP(6,3)*SK_ACCY(5) - Kaccy*OP(6,5)*SK_ACCY(6) + Kaccy*OP(6,6)*SK_ACCY(9) + Kaccy*OP(6,7)*SK_ACCY(8) + 2*Kaccy*OP(6,23)*SK_ACCY(7) - Kaccy*OP(6,24)*SK_ACCY(9)); -Kfusion(7) = -SK_ACCY(1)*(Kaccy*OP(7,1)*SK_ACCY(4) + Kaccy*OP(7,2)*SK_ACCY(3) - Kaccy*OP(7,4)*SK_ACCY(2) + Kaccy*OP(7,3)*SK_ACCY(5) - Kaccy*OP(7,5)*SK_ACCY(6) + Kaccy*OP(7,6)*SK_ACCY(9) + Kaccy*OP(7,7)*SK_ACCY(8) + 2*Kaccy*OP(7,23)*SK_ACCY(7) - Kaccy*OP(7,24)*SK_ACCY(9)); -Kfusion(8) = -SK_ACCY(1)*(Kaccy*OP(8,1)*SK_ACCY(4) + Kaccy*OP(8,2)*SK_ACCY(3) - Kaccy*OP(8,4)*SK_ACCY(2) + Kaccy*OP(8,3)*SK_ACCY(5) - Kaccy*OP(8,5)*SK_ACCY(6) + Kaccy*OP(8,6)*SK_ACCY(9) + Kaccy*OP(8,7)*SK_ACCY(8) + 2*Kaccy*OP(8,23)*SK_ACCY(7) - Kaccy*OP(8,24)*SK_ACCY(9)); -Kfusion(9) = -SK_ACCY(1)*(Kaccy*OP(9,1)*SK_ACCY(4) + Kaccy*OP(9,2)*SK_ACCY(3) - Kaccy*OP(9,4)*SK_ACCY(2) + Kaccy*OP(9,3)*SK_ACCY(5) - Kaccy*OP(9,5)*SK_ACCY(6) + Kaccy*OP(9,6)*SK_ACCY(9) + Kaccy*OP(9,7)*SK_ACCY(8) + 2*Kaccy*OP(9,23)*SK_ACCY(7) - Kaccy*OP(9,24)*SK_ACCY(9)); -Kfusion(10) = -SK_ACCY(1)*(Kaccy*OP(10,1)*SK_ACCY(4) + Kaccy*OP(10,2)*SK_ACCY(3) - Kaccy*OP(10,4)*SK_ACCY(2) + Kaccy*OP(10,3)*SK_ACCY(5) - Kaccy*OP(10,5)*SK_ACCY(6) + Kaccy*OP(10,6)*SK_ACCY(9) + Kaccy*OP(10,7)*SK_ACCY(8) + 2*Kaccy*OP(10,23)*SK_ACCY(7) - Kaccy*OP(10,24)*SK_ACCY(9)); -Kfusion(11) = -SK_ACCY(1)*(Kaccy*OP(11,1)*SK_ACCY(4) + Kaccy*OP(11,2)*SK_ACCY(3) - Kaccy*OP(11,4)*SK_ACCY(2) + Kaccy*OP(11,3)*SK_ACCY(5) - Kaccy*OP(11,5)*SK_ACCY(6) + Kaccy*OP(11,6)*SK_ACCY(9) + Kaccy*OP(11,7)*SK_ACCY(8) + 2*Kaccy*OP(11,23)*SK_ACCY(7) - Kaccy*OP(11,24)*SK_ACCY(9)); -Kfusion(12) = -SK_ACCY(1)*(Kaccy*OP(12,1)*SK_ACCY(4) + Kaccy*OP(12,2)*SK_ACCY(3) - Kaccy*OP(12,4)*SK_ACCY(2) + Kaccy*OP(12,3)*SK_ACCY(5) - Kaccy*OP(12,5)*SK_ACCY(6) + Kaccy*OP(12,6)*SK_ACCY(9) + Kaccy*OP(12,7)*SK_ACCY(8) + 2*Kaccy*OP(12,23)*SK_ACCY(7) - Kaccy*OP(12,24)*SK_ACCY(9)); -Kfusion(13) = -SK_ACCY(1)*(Kaccy*OP(13,1)*SK_ACCY(4) + Kaccy*OP(13,2)*SK_ACCY(3) - Kaccy*OP(13,4)*SK_ACCY(2) + Kaccy*OP(13,3)*SK_ACCY(5) - Kaccy*OP(13,5)*SK_ACCY(6) + Kaccy*OP(13,6)*SK_ACCY(9) + Kaccy*OP(13,7)*SK_ACCY(8) + 2*Kaccy*OP(13,23)*SK_ACCY(7) - Kaccy*OP(13,24)*SK_ACCY(9)); -Kfusion(14) = -SK_ACCY(1)*(Kaccy*OP(14,1)*SK_ACCY(4) + Kaccy*OP(14,2)*SK_ACCY(3) - Kaccy*OP(14,4)*SK_ACCY(2) + Kaccy*OP(14,3)*SK_ACCY(5) - Kaccy*OP(14,5)*SK_ACCY(6) + Kaccy*OP(14,6)*SK_ACCY(9) + Kaccy*OP(14,7)*SK_ACCY(8) + 2*Kaccy*OP(14,23)*SK_ACCY(7) - Kaccy*OP(14,24)*SK_ACCY(9)); -Kfusion(15) = -SK_ACCY(1)*(Kaccy*OP(15,1)*SK_ACCY(4) + Kaccy*OP(15,2)*SK_ACCY(3) - Kaccy*OP(15,4)*SK_ACCY(2) + Kaccy*OP(15,3)*SK_ACCY(5) - Kaccy*OP(15,5)*SK_ACCY(6) + Kaccy*OP(15,6)*SK_ACCY(9) + Kaccy*OP(15,7)*SK_ACCY(8) + 2*Kaccy*OP(15,23)*SK_ACCY(7) - Kaccy*OP(15,24)*SK_ACCY(9)); -Kfusion(16) = -SK_ACCY(1)*(Kaccy*OP(16,1)*SK_ACCY(4) + Kaccy*OP(16,2)*SK_ACCY(3) - Kaccy*OP(16,4)*SK_ACCY(2) + Kaccy*OP(16,3)*SK_ACCY(5) - Kaccy*OP(16,5)*SK_ACCY(6) + Kaccy*OP(16,6)*SK_ACCY(9) + Kaccy*OP(16,7)*SK_ACCY(8) + 2*Kaccy*OP(16,23)*SK_ACCY(7) - Kaccy*OP(16,24)*SK_ACCY(9)); -Kfusion(17) = -SK_ACCY(1)*(Kaccy*OP(17,1)*SK_ACCY(4) + Kaccy*OP(17,2)*SK_ACCY(3) - Kaccy*OP(17,4)*SK_ACCY(2) + Kaccy*OP(17,3)*SK_ACCY(5) - Kaccy*OP(17,5)*SK_ACCY(6) + Kaccy*OP(17,6)*SK_ACCY(9) + Kaccy*OP(17,7)*SK_ACCY(8) + 2*Kaccy*OP(17,23)*SK_ACCY(7) - Kaccy*OP(17,24)*SK_ACCY(9)); -Kfusion(18) = -SK_ACCY(1)*(Kaccy*OP(18,1)*SK_ACCY(4) + Kaccy*OP(18,2)*SK_ACCY(3) - Kaccy*OP(18,4)*SK_ACCY(2) + Kaccy*OP(18,3)*SK_ACCY(5) - Kaccy*OP(18,5)*SK_ACCY(6) + Kaccy*OP(18,6)*SK_ACCY(9) + Kaccy*OP(18,7)*SK_ACCY(8) + 2*Kaccy*OP(18,23)*SK_ACCY(7) - Kaccy*OP(18,24)*SK_ACCY(9)); -Kfusion(19) = -SK_ACCY(1)*(Kaccy*OP(19,1)*SK_ACCY(4) + Kaccy*OP(19,2)*SK_ACCY(3) - Kaccy*OP(19,4)*SK_ACCY(2) + Kaccy*OP(19,3)*SK_ACCY(5) - Kaccy*OP(19,5)*SK_ACCY(6) + Kaccy*OP(19,6)*SK_ACCY(9) + Kaccy*OP(19,7)*SK_ACCY(8) + 2*Kaccy*OP(19,23)*SK_ACCY(7) - Kaccy*OP(19,24)*SK_ACCY(9)); -Kfusion(20) = -SK_ACCY(1)*(Kaccy*OP(20,1)*SK_ACCY(4) + Kaccy*OP(20,2)*SK_ACCY(3) - Kaccy*OP(20,4)*SK_ACCY(2) + Kaccy*OP(20,3)*SK_ACCY(5) - Kaccy*OP(20,5)*SK_ACCY(6) + Kaccy*OP(20,6)*SK_ACCY(9) + Kaccy*OP(20,7)*SK_ACCY(8) + 2*Kaccy*OP(20,23)*SK_ACCY(7) - Kaccy*OP(20,24)*SK_ACCY(9)); -Kfusion(21) = -SK_ACCY(1)*(Kaccy*OP(21,1)*SK_ACCY(4) + Kaccy*OP(21,2)*SK_ACCY(3) - Kaccy*OP(21,4)*SK_ACCY(2) + Kaccy*OP(21,3)*SK_ACCY(5) - Kaccy*OP(21,5)*SK_ACCY(6) + Kaccy*OP(21,6)*SK_ACCY(9) + Kaccy*OP(21,7)*SK_ACCY(8) + 2*Kaccy*OP(21,23)*SK_ACCY(7) - Kaccy*OP(21,24)*SK_ACCY(9)); -Kfusion(22) = -SK_ACCY(1)*(Kaccy*OP(22,1)*SK_ACCY(4) + Kaccy*OP(22,2)*SK_ACCY(3) - Kaccy*OP(22,4)*SK_ACCY(2) + Kaccy*OP(22,3)*SK_ACCY(5) - Kaccy*OP(22,5)*SK_ACCY(6) + Kaccy*OP(22,6)*SK_ACCY(9) + Kaccy*OP(22,7)*SK_ACCY(8) + 2*Kaccy*OP(22,23)*SK_ACCY(7) - Kaccy*OP(22,24)*SK_ACCY(9)); -Kfusion(23) = -SK_ACCY(1)*(Kaccy*OP(23,1)*SK_ACCY(4) + Kaccy*OP(23,2)*SK_ACCY(3) - Kaccy*OP(23,4)*SK_ACCY(2) + Kaccy*OP(23,3)*SK_ACCY(5) - Kaccy*OP(23,5)*SK_ACCY(6) + Kaccy*OP(23,6)*SK_ACCY(9) + Kaccy*OP(23,7)*SK_ACCY(8) + 2*Kaccy*OP(23,23)*SK_ACCY(7) - Kaccy*OP(23,24)*SK_ACCY(9)); -Kfusion(24) = -SK_ACCY(1)*(Kaccy*OP(24,1)*SK_ACCY(4) + Kaccy*OP(24,2)*SK_ACCY(3) - Kaccy*OP(24,4)*SK_ACCY(2) + Kaccy*OP(24,3)*SK_ACCY(5) - Kaccy*OP(24,5)*SK_ACCY(6) + Kaccy*OP(24,6)*SK_ACCY(9) + Kaccy*OP(24,7)*SK_ACCY(8) + 2*Kaccy*OP(24,23)*SK_ACCY(7) - Kaccy*OP(24,24)*SK_ACCY(9)); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/Magnetometer.mat b/EKF/matlab/scripts/Inertial Nav EKF/Magnetometer.mat deleted file mode 100644 index c302dd2ae05bdb092c29447191a3b9b759b5d237..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 19484 zcma&qV{9&97oh9fwr#uJwQbvWcWv9YZS2~%ZQJJCcJiH*Oit#^WWM!xC3misJii_} zAyru+VIp=0CL%c@ReEzPTQfQ$MOz~mbBDin+(dFhB2pY|bVMS5&5T^kOo<%qxrtO< z?TO@!Jc)>yiI~~ASvk3x{t&S+F>?_8w}S-=^#2a9lpG=u5ReQI5RgaCk%^(9C6F-( z(J7Fz5CoJVn6Wz1Ys8zY3*?g)mQU_?FgA7ub`-5M_PgL;$xn~MPt8IfjtcA;MJFT! zkiqV$5HiRiAb21Mng4S#BnsSn_=~$U_?-@}clKu>25uT|B(($1NAM3KnQynEZ;c{9 z?lPQs1y=+ku+grWa8ht1Fnt3=!GDrc|5I7<|52J68r{de#R4t-|9UUoiZB1^X)=Pb z3Vxs22u))hXmpBe^rA0 zmrC{jC@&yz{tG48f31X6fds+}fpGu7Dnb5Br5hDI(2X_({(qqa`LC5@z@@-=?+{-9 zi2@}5^Db-LClId3oo;U2WLj+{@4vw7pZmejqyjKao${htJD9mJCJuQqIv&qFE)*Wl zJU-TMtJi8_Th!%=5?q5aYlyJ1on{GpeLfz?4yV%hcAo^cU%MCuV)uMT_I6(PJ`|qf zkG|^Pr`(%M3GZvmechwr`* z>%C9^ua(}H-*1ht)n7z|o3+EATZi?xz5Tu~h~InopBc{gw!Qtn-^V`ApZjP}z*zp% zX?XpQ!Rc52&%@c-_gVe>Y#6cK&vW@-0&a`ktDl!Kf9jMeOkxiSPh6H$*XF8|Mr{O2 zrevg7R9a4Uwr+_InuDewQdVshcmk$K#CCy; zJ*joFIs=X?!!zbBVkW1()f^7Pf_WH15a zy%lceL(7yzNcNnsRDD^86j-cI)M6zzNsbW^3T+Cc4M=PK9z0vB=g4H6ysAZjDf1nQ z-A`1%EpMP4VO|`$5H1xO2rOR|VZ8kZ$Uf?xtIvTo%WA+k>Qor;AU=rX;j50ERn;J7 zhC)Qwu~4@8a^PqGusR1Uk-ycl!wSo~g#h3X3A^ips=Q}C@X-gf6ZA0WL zTqrs+?I)c9XGv;8YYM>B!{IcQZEB%6?lM_JN!Da|CnfVLQ$2BU@g=HX;k|PffkC%f zTBRuP^kqO3ZZs6^qnbrj*d|b!fWl8y3g1NF{$Sidc2}i_Zr~8 zE4rxJ;X36*bZvgLVLmF_wVHQa3O;q>J^$g;>Gya{+Ol;He$nJjW-YQujJtI!2x-Lp zL*IC7E7u$Llt(#W1Ff)~t=0|I^5Dq0nbGjk(rAbX?M;NI3SVO({aVc1r(RZ1I6TD{ zRwLZ9BcdeZ*%8-)>R^jC1!j6wF?E;MP>irNVVuvZaCS{VG{aHL2T|Ge)Fw@6$Pygj-aB5w0o6O-c6 zj6(WB)cyDGCAN}Ph?gp%&I40D0xB}g!b-_V-r+OA1ts5)v&}0Wf*x`}NIGf|HN1*o`?;|4 zE>bGt!h;Iy0;d-+hGhQfK?OY+$}(kBbn1)`veI;|9YHdZ)O!fEDK^CzJPlliGSWIE zc1`zaJjTIPRXk{u61oYU5Q;NFe9cXPs8<^^zNv=SuztW@xILYNBQB_~I#*5ohTlM; zX)99=2IBr%zTWx@fzB;U0V@z9fBb==9*LFAHjQlk5=+>zaZ}~!@R!^R>qpyza8k(h z_=6!)O+(g!uOi640n*I*>&1DE0aR0l0a*ywLNOt$ic>LvMtEr7Y@^NOX(WNZ(3VTM z33v-CQ>B|jS<2r}$1{Q$+2}e#iIs(84@Qh9mBlvlrg0cdT@pf(5{m1xc(J*+_*vK%R~t>=7-U@# z$|HP$bTgf^;&hD=xIi+J55{J$&XyP|8}JI1^mmXI{@IBnk57~?*K!R_JNF8k<%0!a za{)>eEZtY;x3eerlY4^JZp%fLaXUcF97b{(kQIdX>Z~q>G7UGQiIl)mcG&GBuA06+ zf&Lcowa2!K1Q=?q(@n6rdjnOk&YckX{mGLT7WA#ZAE6y-Gz@*!B}~~N<7ceunnTIF z1M|&?3mtw-Y6*68x{6&?ETotFowWtAQ>RFSfH(@EoR^tYnQ5`VojOlj zLdY3|PqY=TUUSYTI7r}+SVfP?Co};XE9ty;B{Rl_m$3RFtBGEcr5O?H-i*#50d}0s z&As|l!(Cpey&|R>h7mjH6ot~vXsnqH43SB0*JEe#s3u64CApZsP9`BlXrP^+o>r9q zU*Fo{E3Ip-Sp-lo!2`HWQCs$vHnSQiVl>8D3UJ3Zw2og9oUG1 zj*ABU#nd=qwIkub#VR4Xbx7bL05}#S;|cKh=5ve6V27I|PSLU;KcT3Hb^2*|h*6MQ zNj;l7$HFe-#t7yf_%Z5?*7^CG<{z(yC0-KYP%o$ z?#Pdg6&nqM<^)nFl=@SM&BQiA4C=zy&dN6{x5JM^in8}8iJqwtTY8hN2Rq$)e;rs; zbgfnb=@WsN3}4yUJ>{swPRu?Mo8>iHBhY{iF^dUHNV@~@AMouev29^Oj%ZXD0AtJF6^!EkfRthfQF_=| zrthSY6+r+$g%dzzawB5QT!ZZSs5vopG&^|{?acfj>RK>5E|W)!3<W%XR~qiU_2d< zTqntAVGY1jB^jiAj!I5fq+sr;%(^y9;(-ztuGqDrD4V)_!b^M7|EH#bx5IoE`MV1x z1tIFa5)_3e^AW7QgKBHLd_EXmeqV}Hb}y7zr%a4d$6WA3oZs~-zFVs~m9mUYob8EC z4H92f+HOnRlkE*(-Ditmqep3jR@RFh#ZlGhC8{WQS@^pGABDMrS#UUk!q+vfKnGH3c_DIsema>(xZM z8?9O4!I!Y4#ZoR(+af`x&)-}IN|WJmxwD>dni9%&T*jHo5h5q?bb<)=XJ2-M(UE5MQ@+YFm28AYRH~R=hE?6_9ylnGo`jb^y`u(1 z3r65z32`P39i&(V3!J_24A~6Hz2a=1JvpZ*1g8($0yf{WE}%r}XG-efTnMw^f3;R5k|Ag{0>MF77Fu$T>i7LWk z&3b+w5C1TO6JqB@>wGv$>4Q5gnScn97NJ}-p4B+vzHvAb%$)iZqeW1T$D)IZTM|c9 z`>YuEM?hB0LF((dQMyxG?EF4u_7cyURKXnG$AN3_EXr zPX>f^J2=SNWbvk~Qxx!GlUt^MrSn{RI|WH9!y1Dq1pUyM4K@^G0$lV4;pLhjbM-KW zJFrU<+^PUt{2DO*1r~Bd<5?Abpms)J%#E9e==k|}@+w(^&1-l#9L$38h14e%pf5wX zRBTtjFsamL=y-z-$~pimo@$&W;lD^uB&cIdKd;UL2<4m4d2w#A3hWo?cmxwwMO)#Z zKi8pLxgLuSdw@ZLj^PpHt`>&m>USD}KPQEd*t=yHKy?G#{!T)GgENNU<4WHar3u}R zSg?^1c)HXo)-~E`PTB~MGatwBSO>FC0sUBvJ7zD`5rC$J?jvwEhWYaL(sy^?^EnVn zn}eK{R+Vq&BNW_Z&KIFmb<7Gch#6$+?(>p^)59PazG79-bo!TZaS4<6%%Cf8p_3$C zy1$+jXc3@(NE$;)svh9kOp%g{Rl*ncxabRIz0GM57XLU%mh33^41a%GTCVSp*i z?*v|Kx#Q6oK-YC_&Gcj#afh}2N0&~@v~MKxQ_F~aEp78nhp$3c>MUZhjHug_BO(+D z9pe?qQQsV6WYcd7>3SUK1{pZTz{COWYLZzd@-}Cn#BQI`4T;08^yk^9nlJ-?m13=46_2_9`;xz)7x`%jB+5d{ zwNO;<`ACR0K?lM3QbPh1CqbQ#!>Jv;6j2Fbb9{>B?N%5~6Zk6l5{O4M2>|ck^amOZ)%H{()D`m*na2Off!e{BjF;wVF6e*NmW5kUe;##=LSSQs6X#hoVC0hsEA@)L z;}>sc;4m|s{mX!R08hj}y>if&_3yHY@4QxkGqxDKbJ|Ta7|we-R+-m(>`V?+VWKxD zvTC_uWM1=uFS>+CF_kC=^o@9XY8Q^=w55y)@Y=*&2f-Y(?zpvzJG{9vpJ2-B{5Gw?qJ8gIQ9+t`iH0vhfYhi&T~%$|>`T^k)3| zNdre85{T-YyQGrzUMTJL8)Ali0Jcu0v3@@T_jD~;s!j_OBcXw!nsz=G0>1P3rG1By zW}qePPjAI-Zg@-WXNm|GV|=JDjtHXY(4wQR*J!rgwt*+>50-6(T)hs^Of5(a=D3M6 z#x~0y_)aTXNuGd>Wn;;ARgdLAyhE1P(C%s^o3E@WK356Q^pBt)+wt(jQq|ndc3KW2RZqS;| z1(+!&tnRq$yc~|(ScbW=;3u%k8)9nMT_?DCe)N^;g%lRtHfZ%3c9Fp!W>8XUTEV4y ztWa7WfUcnIJDV7c!q(t|cD6D43XyDjk+<&fLThoSd@gQCnQ!w9tybAXRzn8ZSa|ok zW={y+MW#n0idJ#}iEPCr{X2R^NR{?rW3LG<*`azV5E@L@|ptHF1l+2#J zlF$-go6uodNHn<>;CI!lTL6?j-cInsPQB!T$lP(UR)A09FnnHwxY~eY zy8E?=l0H}|x!~&$g9$xEv)ylQE{_1T2@l zRi5HlpV=~M!7%&AtcJF8gpfl8s zwzWLx@8}5%kn`8_l?!r5F$!uw3K<>@r=2^jwNO+Nr9gy8g)MfataO1QeB?Mjr;nwT z=oH1t7DdlSXsnN3UAq(GaukIZYlz!99e0FMH}?e$YG?$P#1H2+d5>VAdzDC_EJHBz#<3!~RsI;3_i3rG|+E-$+(Z?|x6&Kbfk0M*4 zY~x(rZS^9Nl!0_TG2MCSjU*1@(UO2ar%CLSv{{!dEiq=mW10@*u9dqQ%Y0Z2n4i7= zpB*cm8*%1{No$gB*Av(py6o%&mYKJ8)W?D0TR(ZRt$W^sy*L2fSI{c;4&d;gSD1$o zdB??>X0lSB492yY8YtMRI*dt~Ez#3V{+D^H2F(Y8NsXv!bSgZN9uQZqfWm&K(4oH7B-`^BZDTJ%>v>i@ETnHF54|w(_WYAYVV0I+2MpT|g zN(_Vw^SWZZO6npAY??>0jWjh5%4v;S~FbGx2R_PlnV9(iaw=c9e_>z^HWa zd1V`X>fU&+1yMen%Fy<#M<_<<_<%Ts^u&IUwl#5VW|>88;4^CWbL8LRt3~Gkeh&hG59}7jSY^xlv3&s#x;Rwg( zE8?bpc-0yq7e>8UGa>~k@ z@-Hwj-KpZ|1)!-`%SBfm*@(Cs9}D-F1p|M`Zzjmku*1bjkpu$ubBfe*l3ryD5x(gP z73yEX?#!GgRT#4FA*ydJR3oD93fu$6a&FH<`L^A@ZLOQgz9NbC%s11B>~j!%*-b(z zDQ(U2nUbE?mS}H|7E)k^*iNJ?J`bvAo@Le5^DNZWG&rC@84~=B1>BwdZ;KA8iGwzY zqelzH^C{n8A9~CmMj?2d#h;zs>M4hY5-v9K-}5kEkQk&NO6F&-=cj|1de%G+1%!4D z>~CKf8aO2^A_fS*^@IKDiF=NI!@S{_d&0gbj}^fM0XR5ZVhl%ajlr^RW4$gOcFuvc z-URH%qlJ23Z%^@=MJ1x7F+0*&ntHR|KT*GayB?+-Dp#L#SVvRjaEma%em@gxI*kT> z#v*VKR)%FEj4_EYo#FG)b^~N6R=3^+z&<5Rrt8U4`J2_^g|W^@uboOLFCj5B(EQ~U zbu`ajK2QH%!1`Wh0JZM@zRUfd&7HrBD}>#E%j7rRLpg5Qe;hF2%eSG^yWQrxu>N%yY5|8>B<0~q?*I1{?C~Q41do8v8 zmz2Ll))Wz7y-J(9>N8bmV9=vQ^p7Mkwvdg}xgpWsj>|}e`GnPLY7ay8SGxa4`R_ye zp8oI4=r16=4{#!|clUcT>JRwyo&Ni510b{qTnPYtejh#^`bM8NUY%a%esdIjZRk}t ze1E?L%pOm?SeuOMsvw-tYyN9Kb<*Jb0Y;h zKzht@V#N;tn$S+tyduP{c`2{`Y_*S(K02s8*@TD|;y94TI?P7?Uz+;uHfvn}Q>vR&`0@~fjlYg^bVRCf$m7WiIl-F?T@trHQUWrg`1W9lE%Z{6%VY5)W7%n z&X75%HR$jPh&s?YpjZb=?H(GiT8-p<$Ucq8L&SLF^R-`A3u#bL`sjgjVBVnf6ieByLF;cRB#{QYZ zIIN?A;>no-*Y7kvDT-a=YsL29uu+GOmK$+hwSJIT&MeyLk`$&=zCUuu zLV2z3=o6_5vP@7e`Ko>`IGJMO{a!L*4itV|}niC9#uvw^CJDjxsT{3?pE z@c09B;o&X*o#jLDoU5`{YjF?^50|7$P^XP+6gWw50qsK!F4@!7!kG^=$;&%=t?zUro%!ov`=Q1taZBNZ6Y3q@W1CBPltzsDsTTW zz|dmkKQ$GgAt^JpQo`^Z))1~!!6bC@5AmZKpMq-7CxvTVdI^kJW`%!LEf1m#Ai?@s zMAF&&B(d&CN^8PZ!k2e@(4(o^Kq>cy&Ws2|qG(MDt&&z)T;~zF?}Djc%}F{=PMzjd z{42G}z9}?u9jl#?qDMj7wgRs%^T>qp-eN8Zyx{svCG3~?L^)4J{78l|$|BEly4w5sE92z1k9_^T;J8rW1^VQ`#EG;6`qK(Zg^;S{23&PPFro zayOm9!)?4zbEjBPvfOodJggH5)`jp=Z{a|Pnzyf9g`0L9i+Kux>OQCuWv;_~nS<|w z$>bDueGlDJs+Q=5J}qtKTDnrBynzZIHUl)2iE7Y7hPH2ky}9^g$4JN3c_63o-iM`aBq!h@zgG*S+&RR&@Z9#9i zonP5DKh%l#btBTo+6RKLKP}-V=#Aq05hF60o^`j~{U;19cjXXn9H^;_Yo2WTZk7h; zv{w*@8N?pXFYh77khxZZ;zf=07h+%zS*frRyK z=BGGw&wE{+Ak)KCyEOmTi$k#b`DYG=cc?}uLf(_6eM1-IJ3|{Ce#t}*^>bFDRr^$w zM;oqf4q}wg(BOo!vK6>meY()?1OMtQUC=ht2Dh%OfO7V)>$4A4bG;t@H$wI` zHsvdf%43NgU_@5}-HS=7r`hjWF9rd58mXZYiW>S=%xUyno(ra6G+g{(Gsjd+MrT9I zREjRbGIe(*RLUgncIr727hvPS6gXP`mGUQz?`ih|M^`MG&kvyRO$IyqI;yQzki}G_ z9g%zs4bp6#WIEvS8HggTdy#Q@=f-ROe$aO~0$M##IGe8THlm+p7P0tNYa>n6QH}T5 z9J~WAL`&Ru*+XK^ZmJw~JEd?_n=V*0lp$PBcZ5M%5M2$WArYX-I_nah$Cyqc)`YNn z_{I78v-k7xTX$FP576=bH39f9zU4;Y4AVNgBlNa5WY5qlni@$QrGw9wpY2JSP-||o!?!V=2a@O7$141|N|Ssgq3WSkkG0~iM|-dBLJ$lcJph*f z7I)fjvh>P$EOFDj4@eh#B4no$Q*pXiM&_JL*|>|XFM5_{xON#YfA8ii8TH1P=-a|G z!IMqtu(N;RwHv#=#kYT=`GD*zVhg&EZDFrdy*Mhq8nd>1_lOQ`&D;JSV}qxY7YKkf_B)NZ z{=r_Y)XjCE#Bi$UcfuKy@mi)A&Q9u&Dt=WDurN)4SlOgIF|ueI0UMucXqj>e9bqIZ zy$$gUOt9@hLH{>ba|i_b2GzDHoPqkc6_J9CU0B6I#f_h9)mA3yY@D7+-XqT-v2^s` zQ9dK3-8x-coFtFB4o#BB`Urher=%P{uKXouFK(2!L2RhgdXRb*%$j!Lj#pO|U3_9c zMSce)MsXBKaUd4~(B3+3q6D5|yxWRZec7(d+4aB34ZN!@)?&s`ic>i5Xrg}IU7Pl= z(Qc!c0%@>*Q9oDUFi1Ifg5O(pQb23~$!RzrLuOW{(?H8+vQTllI2Lty}j* z&QZvW<|V#DWyZcL;O$LXQ)(_ra9rKu`@K$McDzc_ z?nu50XySj)?YibaaIROk5&7^&M@0udP`RvbVf{38kwQzUgV*31TOiBRgKQu10nmg( z>&lR&kzyZ&2=RLS@(QetWwh;DD$7eL-u?Lc3y}wQFxbWczqt7`--K_I|X215IT<0+f zNnYuUUa>&Dp`X^eOvr{I1cE&@neI`@WXRjD2JmL8rbxL({c2`RZfhGvx+a;P!jg~X zHay>XTBLeEt{Frpl}VlTO3igVF5IbAqSdi(u{sk>RdYo-W%#EJ zhG4Q$qZ4(kQ(lkrsn<>^o#|GMutU!BJM#P~1yOD0Wr&;@Nrz}$7b3`~XJFzc>aqkJ zG)~N2tNO#yEPcAYxx=bE&^4fzkVW7+XQ@luU7pr$g|56&?u=63noT)LiNxm9ug)oo>@4@6-&k4v! zx}0=rlsht$j5Fjt)-{nK3RDiG!exD`l+Hxf%E@G-wvm%}zU~V%V8ok0$Zv<**g_Fj z-#jTFqrAa!AofOYzmbkIoF|fKph)j0DLg4sa zh9asscBcC8%)_2A#985wvIwh?iWk*ZWx-=^i=;#~8_pP$Np*&c>c~c$%U6e`W6WXD z6QkZDp0jq)T#|>1+ap(np%BlR)8HJso}=I!dR4X%excxd&v$86=Qg|ncSVpQav?lB zYbW1coad|?eG*^I6I{mwod=55lK+H835Ugae}e9A+}2B|c&@~uV}U1TlJ!5kj(mLZ zUtHxY2^wvBqpg|<$LXW#fE%l2vR|E(=H>H*rIwB%KnilPRiBie89@H|=NFLn=$qFC z+)&t5Rntiv+B8&*4A!6(mEx28mPJ&^#A$YtvYc1!hBUKSZIvRc;@s?1k?P|$`V-BY zyTlKbir=OENj(+o+`3yEk(z6$*udUf5pWbOT&H4pOcRW2VB9l{=OCMnR^jD^Ym`>H zpeRB6gOB;P#X`uRvziPaA@5b7?(sJ)g--t`o061q1&8Lt=4U7QW9#f!fH$+K(zU+j z_i_U8@$rkO0MT`H`V9!Dd^-Ej61&w(>CXPy|IPFV0QlupfascA_sksI<}5x-e;d5`b0j&H&x`%Jf_<^mzpD7&MTz=%WOR{?RnL=7_q#~wq{&*xoF#>n zWr0zF0^pL%;{3&tTmA1kU06hUam#zNrUPsL+aXVOcFBOy0LOWV(&|e4E_i4++y?-x zn;XvuRqfR$dO|uw+x`La=KC3n4)%7ld$*;>hJU%EJHSc6sn93V^8s_W^*zM;;Ly98 zT~C;|YvA;h>bHBy<^1%_la$Q}_z65bjqdu;X5dt{K0zVxTgg>ZOV6$b zJu)U;NIKFroQuX^j_x@llw8K{IihpV(VQT6O|&^6)lD4^I|XErYkz}Ul$X%C39l#c z{pq|5fbW<_B~IiGz7?*~?R-4*yYc@P0A%?8@(7&n{R#jwKIy$hj@v^??R3VidsGxm z999xbbwuDb8cf<}YSoV*rI}@77+@cJPXgflm&;8PCun*#&$H{2s8%f{1oZC?GrRR= zIC~npH|rb`?)=PLcp5Vp(Iz=aONV7dWS*R~ttaH#L69pS9pT(adky|jeltEB8#Qus zIp0Jrk8)-4hE5eRO`DU&g&dkjK4Ac@cV%C;+)$PDdH1We%M!T*;pi=5rc%XnC7FVm zuB|sv=nvOrlBgX2wQ_uBy2J!=Xs|F%{32N#=*RS#mFL~QorONd=f$;TOI#8vr(JHnPYd?{Ili*r}}t8@kjPfiebhiw2lN zzk9i}t-66vGs#RZ^>6Y@%y6+@Yfa%Ros*J9zMW@Q`B}XYis9fyhtj9>^=S@`jEQb5 zjucaxPkOnQus~pe*wsC~z0HM+v~i{U_g~JvH$}&U9YaurZ0+Ffonr!9cQgjyxiWG_ z)(PD^jJinNq{72&5`F}Z&yT;61@B7Bm)_r+^PYy$c)hK3=H4zun{5b#s*yR`nj)Qx z=^Z$t#$WVsTF;u;EFF0CkV`MTd;S}&uLzWdswLt<1Sb<`OfUNwRwXMil^y=s=s+kR z09%0S8#n~Dp+Dbr(P09k|8-$w)9wM zLzKsCg0>u#2`1bVnbFQn?$0S0Oxv{(pL`QjSaa#Jw3k61AEUG0WR_xjvx~@!Y{m0< zuVCGe6zbXnJ!THy5}=tRNQpzvWRi-YRXNQYo+rFhX5(Jpo%g$+;t}J(yU9e1D;DZ5 zYKqZAhoa!$b09{#)`WrbVTi0p?Ud0tZ>HOvIjytx>VbT!8YHvfkSxdExPWbZ@wWnD zZn!7>_dBU+naHey64byajB`4GZKRmA&iTB$1Q~H5nPQ_Rn^??JCMxiwmV_pa>AD*i z+R&xQ&@Emv>Slg$vo5XA+PR>~8O2olZB2=kA4}%FMH*ZXuX>63jk@<+Dky37lmG~| z@uS|?ASF%Bh530!U)2HT&4U`&p61NZL2E3sQr2da`9&yMDuxg=cI@#CInP!Ng`qsB z-0?F4gETIbj>kfxi>1;_+5pR!z&7;7Ql)#Rz0M8}y|8W##unWlrVp1&WXT=}H~FNu z?&p6RNNEUpJLMDBk#D;6rF@ptG8a$gGBI&fZzoqO>p73W+Cj=6*hsCQa~)(3S12f& z9T4fJ=)#=tTP3ARcq6+%+d$=mr@D<5PwC?Lb_q%Jib=Z;-SIfG2T1%+*3fjwbB8=; zs*0E)nIiBm(dT!edHs1#-tWHnXe&OjR z2q3*Na!!|a2=g3y7u>8cjc6HI1ar(^NZ#Is7RT2~UgCURT*}LPc?d}reRz^Zw|p@e zSBfT$p{POe&)*<@9(+%h(qLI56?z5!Z+}g!!Ih9tzgp_@iCf zqkSt$v66QYp+CCY>8&xyq%Q^D=0LTakX9I87iG6o#;-uPU6l;ns>&3;qY#-Dd~43W zLhHdgfDG8t&!6TETxKX=D%>qS{fT$vs~+zwOest0UZ7WuGsu}<{TNeL^c#DzC2=?v zA2rG-7yF@|{fDUzYnFVoT}BX` z513s@vhKgKwxn)RiH913O;dyO^EdkX4npeNCRXPIc^Brun#=1IDv0)VcOaz>lkC=t ze|jsoGD=-6;wE<#@xBL~ z!}u2%&0o`>$qi&AkD;fFS^co@dDuM{mbgo6=zrk#?RNu|--JuLGQd^|$cPahZA%Ya zvm81P3~NRZFU&7uWlvuzp%XPF6^Zt^ZtBQs$nJK?eK-t8A5dbuD2xlu+4dtkwc*al z6B;l>OmMAUC!Sp(YC+76ixqvI;NEPZJyaFwvO^QsTMoLaP09PQR4aR?pm9z0)hB#Z zutk~Z`A{I!(E@bn=PAb*34k{0vf5)*8C-HSdvm;h__F*$j zzH8#AOspk`_04m}M5-UAT9&2aKJlh(9kW0gUUMs6i1xDXl#egs!l8$lIfTKEZT7O+ zh+<=?%94w^4ek4&v*G}k6!#P!cVV#e9)^FL>{Vg@keD>g5Ks61(h3MZK66yOqJ`3+?kNqC_mO}jgnk&XnmAu=PzLRwYlm^1R^@NI7$t!2|u6d`u~3C|2=v2 z!uL*l_kaxF{yHm*6II7yRt)<-vAS6ShSgAe2&Z2wwGi1<_IdG+M#*}y88f58P`1HN zB-lgNNPVQc^K%N4Gzm=D&)4&v>9=m4lTk-oIYm9N#YQUBUzFQditAv~vp7>R)Rv-H zf3@nv*jso>aEdoA$Ij3q{k`>h@7O=<3Ndb{^h=QFAM%u@(N zl^Pk(Ea>upvB%K5!dA{+VTlu)_tbZ?dS2`T+mE&{75K-OQ!AYungW_CX$PYv{zzHK zO)3U3W*)0OZ_ZxlOG^P#=$ot%Ng8K19Uh0?vky%(@a`&+((LXw)}Yfgs-LFS)=I!0 zBVhf|f0pW`LK(r_$Qo4vh}OCZY|dHVI0&Bsd~Md7xgk|T?!hWU*=9x~b+%D+s@oga z(Wzx=|G+7za98CH9RPPkCN=h(mkdo>UPDrzCxoT8SmZOH2H`sMOBKZU9AuHpZ8{*x z zKilOBrFHm}iK>vn83cw!@hPyV_^Xd&XtV(ca5W)dXdj7AaPm|&#F=%Sn;00Ddv@BS z*JE*V=7GQgriR4m{EX3N0NgNpT*rgQKaA<}8oz*1=vuD$!avcx3?(iv2sgi%Y_PU;D_Q?|P`O8PtsWa$&(O zvXi2K-{T2ipw2jLN7IHXlYBHpdn=obfgj-&+3TI)OBsuhS-^^!U)f-+{1dq}*jK;) zEv}!XEr^>)$6Cpi8br@O`=a?tgSe9JH42!7%t=i=HH7F0Qm{vBnu18*P94D>QaGW~ z#;(4&fM#9~TzSqx%Iia^ADP6J-CTU8RYZP8R=GAtj%vCCqc$8aHnvf0CsPX71on@Qr+(Sy5|PC`Cws zCUsioa9;+jwT@!)%=5ol)CQSt`iDSJ_t^L?KCA+om|}7zMsyZ~EAqR^?3!Y^?K0H!m^Vg!jzSj$0@bS4wKPgpEig?LU z-V{)s)^-c&h%A@~C#Yf&6D7RmC!Df<9CnxH5GlJlveDtv)zyAQ>?fq4e`6A6<|TV< z=Rex@=|F((x%Jlqz0;21deh(Q*yVwtU60T1hH*JOjR4)ADu$8=oF@{%_|B*PNj!Gnqm~ha)dOk6zWKJUxPdDqP)cp!>W=kK9E~~c4e$PSK&E3 ztjQMA?ikpRJZ}f?2&rw_S!WIEM3H|)hP;SwSLu;1t$ZKm$0DXTzW>-W4Qegmc++j$ zbw;R$IeNO$gVwIv$$rM!OEI%3Gi!Junk3@tI=0+C$79yj)&asic#%M#w!%wTvuMG_ zN6`G=+!3s0hK37L2>a)p`jscmX)0F<^>&UVR~3QAL~H0_y-WGUu5Zd} ztC(I6c@vi+(5%jG$<)=ce#3?u0kZMOm+PK`I-5EcH5ITbuE#4szczssGuq(gX8E(3^8)-bzdqBn{wmI@-4px5ys5|O2U2_lK7i_EcV9Wyl4>ikg*8 z3^#Eo9yO)MPP~ba+Pm*PB4;|{my0>P!309cvF=m`CZA8Hd+^$M@-F{c zgun68lh@%t*bx8P#V#QDolDl-QldK!Ma=Gt{0n*bC?hcL`MJZvzlcRc-f0)!Z4#D(!Fh zK|7bT(?*Xz@kM_R#4ZV5*q?|`k(#TR(w=ik7HioS?oU)`g)%~e9>NowE(|{TpNKPo zGN$-3k1xs?j0Jlp{ecZs0;)YZsy(rp!ysIr{QGx^S7DYC)c=AveQxWMgB#5)*&s#K zN4>EpCst;uvIaFC{IUfTF~?{BgqNIe>Km~syWmS4m4CI{3v+`P^;Uja%dc72M1%zk zmf69No@ijSFvjl^1p5I?Od5V)|MGXQ_Lool-~aIc_i6v@AO7@dfA#-gKFwdy9q=Ew z$3DQxOp6!ZGqS4cXA*;8`E`i;@jO@iuB>1nWNeeFG+k?pu0xdN*h*m--nyp9lP{6w z*~|pm?*$(^Xt?qsZ2HCWvaAhtB?fy|vef0(&{fa&;B`-d@7#kAu!WLP2#oD~3TL68eMg)Y9(R)^oYcgwDwivzp zl;F+iYtpfzee%da(qbmA$%}5$%V3EHZwSOtB#ML`J{AS%%kaXnBfe$FC{HjWhphOL zt7tNqa30p0e10L0&JVKYjG+$Yt0yH13f2wCv_Vj#QoCrN07)OEa;G5sYt=1eQVV2^ z_bYPL*;#}q!Qt*?E*V!D5#>t(`qU7#f0;>w5(ExK$~)AnHkA4ZEbcOp$GHa$)YwSD z*4oD$)E>e*oAFJKb;9));rXHPdE@%6^x}$rDor|s-Wo)TJoaJR#pqMe&_eh0+R8l> zBvQO9iPRvQle^N}`_Y#`(w>C4FgG-BLJU+o`)T~qiy5lh_Z(53Zq*ZPm?;0nczy3t zeaNzt5ZYk|s_DxX$zl4r`)~A7$=jgNQ_|2?x`_0_Y?9;=iO5^OK;J5kMzZhB-<@{o zBf?SYTdlPhJY}p=8NYh|o*1xeyQWrOmzf`bxr@{F$0J+TFl{IX)fWP~Yv$|}BEL_C zX4Kk-;9cpI{8vYmxYeS`N}{hTXv7f>imv>kd$}D_O@D8{B^BrTVY+Qkv^X&iO2UbA zlPSET9L#4gTDv*|Eb&^dhWP1Fw4mCj@5Vqt$6$OI{o$~mhXr@oQ;TplB3vv1DvAn0 zaiHGbNmIQgtXf_{qnNw1zI)0xaiZj3i=NO@_}z2@1yOpPfv4q@)wvMAAMTZfa&q%T zXyBU)&tk}g^oqqosncvUt{83`#W%d6 z(13g>zPMG|9%(z4lh{FcXzekG;JhVFPR!03E<7;xT>zr&fb|vegi#Q7xwg?0WrK#! zW?{m$RwkB%=72MU2XMibldnxu0v?7G3NoR zi1&!tnuUTOfuQu)<`H;U%`i5_U#2hzLHO&(rQj9+F>qIKR!|YE6jjRm;NAM88ER_B zfL|+OHYUnO>~$BPfn7+dJW2HnvJ?Ju8kC9-rVCz+izhTsL{3dOkUeuJgq$az)V5)s%g9I1qx8hbr!OjBpWul>rFCuJ4#; z3ODrZA-?maVU_qPXt%9JELgYSnw9T5N9Iu40LI-MgXM%~URaXey&HRz!BmMJ>v1r@ zg7XUG7Q&wUOdHS*FDXN?FyEn(DvG!Kn?j}C~?CAyRD9TF!-xl_>7yS>L5i+x0U#M@%ltuSf}?Az?D zeUG|EoOKl8eZVJE*1qtc>HdeJ{hIf2qTVy_Y`|w+p$i=f#_GFwM6??spw1N}V-$Q4 zdd7*ANXhR+fg}(^tFgagzlt=0IB>=sW*t468C()rd|JY}Yd;9b)1`L?%bqCBg=MCk zb4hlP!f?)QuNSzJXo0vSNC5*qep&0!Wzkx=iIB-GmNS;<>(HK9;nTDaDVq4l0`b^) zF_Rh$!BwQRF|cVcBT4^%00030|E*8SuB%2AyzeP5N;dh3>>?J;)C|jOgTc%;1~=@? zXajEM!F)kJC7&7JmwT0~C{i{#QmIv?s#B*On~g6HvF9Qwrwmm*fNpH3#G(yJ*-giU zD3ASZwQx01*217|RCAC$E*r7yS#^6udY0}6&q(NGE(Lr}JUbctHhdUY7u=Z{Xlk8`-7tfy!wj_5sX_B>p0^~N84&I4_O>Uo3g-aBkoBi!k+LoJ zU<1>>xvmvo?jX0hg~?JEglc;=SU3>-URVW&CIZ|a#i4>0Emw)FUW8Vd*bAw}$v}zM zvF?yg%p9|VeQ_e2IfQO7X8!FAz;6x~e(1ck=pD?@43?4+)&*+}(C6?H>^!n-q3 z>OQbV8{7t#cm=)rh-fmqYcLtllwWw9!=meYzmdM&1k~M=vxYwfjyf>HhL25U+>|ik z{KsS3Qi^V~-MDAWGc!fgydj2g?d1!Zms8m_4T1OT<68p;L~!# z1!f9$i5~={x*?WKb|jnQjclrfkd%4;ARC00;S&j`O&Kx+v7=Q> z4@>aCWSHvam@^u=umm*cWZ4Oat~Baqng=?}ySu?|s;6i^#W8S>6(bOmk?No@`#5nM zwy`ZxC%QUqKsdz_Mj3h?8z%Slkku;65iV{;s5iLwr`4co@nB$!Z5}>5Muo}7%wCWm zEVi1}y+@9;5!a>A&Lu2lvu=^k2;smu3f&&VPA1Q)(z6IF%XKuP#cIkqQWQ4CvQjq) z>5r?}FDGG@GO52Nfls>yKlQd05Rm!Rv}{tR0VPaXn8l)*xN4208ny z631Q9_w1A_sF|m`S%-MOdtSHZV+|sYD(1(T@RL;FF5QX3TMF_u&Ualw9W%|QAh!h4 z(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 zcmb8r^HU}OqxSKP-E13M-PyKn+qT`EZQE|MZF95TR(Ec5XP$X~eP-Tsu0P;2-?`+4 z)#QXlNZ1*eNaTgp=q;@6%mE}ycE)ZNjxP2*B=W+d(j0665>Xd(V>fd%5=RFf5>;JIs6#w}&Hgf~%N=n0Oo(9h<(Q1&GB){VxcveM8v~=OEP_gXs z2JIdF$Tx53Bp7%0PO0e!E}&mg`By_zR7!V_F#h*2s;?Gr;J=gUjKHyezt^10w=LN( z&~(CeL;s~A!{5SIB*{fjxjpV1}w;AEy*qLGA zXLnXZAm3NSjlZDZcbwll=;od{)|Dvc`Jv%Exyq7O{ehO0i1eX((nx&ek8?wzfgkbRUkz_vikEzvR^=jo zZdOe##X#e$+=#wWrJpTEM`z3}+i~h#ed}GcSVj1`+W>WAaJNgM1W9$Tv?BaR{_-Lx z#2hZT8ObAG*3NQSsRpe`%#nwpo>T-T@JF+dZezyAFg9_asVcdBx>LJxW+CE@>CX17 zatR+0ljcAH$0;+PVa3X&1v~yCz5plYoCH=~I2t`;Crl3IDv>H&Gz~oFwgbFZUqic< zGwCv^gMKl&!v)kxuD_G*o-3vw81p-@qy8MI>}pLm1#S1^?!%9z!cPmKMFJmlQ@)&u zW|=Mcg=@+5SgjUJao}1B)kugeUed-Hla;gxPE6MK_mP`m0?iUZw`SGRnBB&#eT(s zhJi6?IAKDbYF6#kM_;xKpPb!-gwjAncA5GSv0uWGIAygLAc+m z!{`k(No`Meg39y?pai^mAZNEjg2pVTP;_bYLB0r>_PVET%dwB?nO8f~?E2ot=}>Z_ zupeMaUIc^=CjxGpG;&m}NzX#7FDzkl_J8bnc@KfTguG{=l=mJHmVAdmj8wCbzmpl4 zByjg$EH3!*1wQQ@*$fJmjHx)$=PqI{QU`^sKyw=pnLAauU zwn{ll|6plD4V4yvdfh#J`Ry2pMNJnJ0z>vX39guvY#FuX8Z#c!bYTwZnyHko(QkBe zU^OEIOA4Hnit8O;-a^Se!r{?C=c6svl8fI6{K4JYcQFR`sKsv;D(`E@wG&+AAQ9V& zFB&>x0*d@jB4`;Sj%<1>U(?Noc~HVhZ0(GT=i^%}tIL8M-We*Xv=3r|Fz#LR1`?qM zYPcsVVR*%=L?Stn(ttcF#@d6F<>!lC-;p>zNc)v?iGXIAur_!JFL_7E)z+s&FzVX& zJF>%8Zj%lz(VniVJTc3v>rPT%rZYdiiX-0Ry*405kuCW)Z4gNXQi<#Z2O;5lW9@@V zQUDSaRp`ry1Z|hMa;lX_7MJGAkfjRcoIk9uIWu4fiskp3x-pH#qx|1cZYzNM%Rdnd zC`(1yXAP6~b<`@k>E&74Hs#8$Vm@G9>KlA zfJ1V<=LY4|vBt$mzD~#tKc@>%LQjtA4TH@bPyEGMxFIsPkb;3?yb1MBI%qj=yri~g zHCgF5*8r}{Bnj&41es~M3}C*f{Y*+Lb^w^($F;?XzN?X1j-!w;Ems-Rj_0ZdpwVVF zrWruO9NZ=lX545~#6xb6y@tk-61;;E#6mPc3LdyoW~cwfv^bdWnnkc+qFr0UQ29V{ zWgaVJame$@KZg>YiSth^qrFPis&>L5;n2AFz+Q1ImH&Fqm*j7rEYONT##J|^Ct$q! zZ>RB=Ktw0kvJ1S=;l*HkcHb~xgZZUeqSGIlX(L6MH!OVn0m>%F1b-+htLvrn17oan ze@V2kiz;mT?K1UjB-+uUql0wLb;N2R;09#<aMKR{5&`aFT zWqxjU%CGo%oCep$c8Z$ajUVa}kZ}Tn@f_}fb@NSC*!|r0t7DE4n77VIz8?XE+b$*N z@`#IbUjssj&uLP6Tm87Vh#PkKq+q=}$%?k%M;uiTFgYxt+ZsS*=uJ}tRvxeyXBYb1 zT|TZ>1LlGIoOALhB(!-Fzyiu~etY2i_hm0}OZD=m};NMls4etBo;^=tPMCBMA z1Pb15=*VKU`bqTmSlsdF2jMVbWz3$DXF-^S1CFa1pg4<4VRCELj?JCZb>D$m-*ca; zss$Z_Wi4n1x=ZjuM`lxPD!r5784gO~#Bc~#%7@I496gnSG;uQzCM{m$owmL44#1l< z5cpQ4EVV@&V!u90z{QQxpX#vTB+-VMKUQ!8 z(6@*oW9!f~YR->(=XWh{?0BRtyZmpRR1o356_Ux;9cTI9 zV|gfHvS!XG8K^}(*s!|)-LolQJuk6z=Dve#msV#FHKOizlriE64PTAq>&gf#H7)A( z87Hdr*rpSbdN|&2WbSW=doelXRF^%M2t3ldC9g6ZG}`4 z7k*BEzh+^7-+T|m%FMp}6M0Te!_^z2X=tF1jX^3MhtUva5F| z4_)caS-D^{M#FXfPVIbsz$OsQ|1XIgnBWlrOkercsKyii7Y7|tuB!Cnx2=(>ba@oeW8 z)*Rdu1R1eIr}2e*?mC)U1Aft^MIg1PR;iymb>k@Rizw5tN{l)7oWp}V3X?J9d-~|A zPdgcRlTRo-f)03Mh~Op_D;Dp>=zn$X^6Bthu5qQ!q2|aaLjR+yccbLy&l-(gcdNhZ zmT_ffHSxk(D+p;)4_`!@N3VrYsdr~V(&KTQXJN_WBHR&XN$W^Hv9}>D08Y4&oeI}E z=FxOgcuwS7Tp1B)MG#YF^?;eQ`$O6@vDEJ-hUZDNwE1ZSV+Z2C(RBrqq@IQu*m`sC zsLJ}7TNa_mpiQ&(d#Z<3W%?4&En0%gJ_pLKmW4Z&lB9>X%NX|aR6FhU=nK!&{-m)6Gt5Zjp6mb%2Ot9)oAav)t!O{bZ zQO}a`4Nw$9wUaUpqGHfxfAmy(Kpam{s9Dng3Wj+w5pux6c=NC%GeJd$8Wz*6z4PmU zCT}C@NUY$ti@jPbBvLN!^+-K7&6bG8(SGu0c0{v$ysZZqEwfnC_qq-xRCtWdiBoSx zoE0^4EzXko+KuoFI_^J_l3&=V5Y;1Nm)sX|cif%w>&0VUji<=K3{+q+*Dt&LPQpbc z_AKbFz#s@t8Me^dc9AhS;mXZZx36Iiom-Q`6~21*UdKD>Y4W`2 z)Altsrc|z@Hx-qVTMocznl#g$EXyeCyDIiK!+a8xK z;c_3k3Bk+9VOp#S6}v`Q>2z*juz#Z|K8wP5R_#=hMKfRGI&%@uwno)>Q~xWa;tQRz z37fI)R~W6PGdBPlSnJ_*s7`>5@J2QggMAl+t@u7s-V-qT5cXYRUL-Kf`8&0CP$O0h zlZl7z>3MFdZd4!>a&|GR0{W;fZX@%OY_#eAX?x1 zcv*|V+U&h1ydAR-F7CIaok`d0{c1_w;^#<54*t!L>NIu(AIP=gcC6Vu5c%OV4?AJ_ zQmaWr`kqsM72Rs;Vvuk9^-Nyq7n+-sAZGyue(t&Q`3Q)Pl@12N+}qAS8d`-@i(krt z%Gf{EyeO-IHHZhz1m^OUEFvi)3H^2?9|sGWKPc-r_D5XJVvEY?J=G!v;N@!OqwfI! zMkGcIG(8$)ZH}vyDTwq@y{!3G)EL3wPU?yJxIf+vj(8m|Q4@2@c#v8Q0*ve8NQtIn6eAp1o_xnfoI}gGHyD( zq8#iMb}IUo<$GRE8MBe97xli8;Ky~ z{8rLBBM5@c_T@rO{t$&B2;!qK!G#@()W<{@BF(l{(59Jk@Cn0^%}|3j{J4v@;2Sew zwM&218hb8WR9p&qx-JYkQp+R(7vj?h5y)e>O@a1_S(mB77BvMmLv8D4#WWUYgjz_* zudu<6){l@`C`d<-SCA09zc^a)t7M>za*LjZg^%E0`$`AGFvC2oIInaNRv+>(ghyy1 z0TF&IRxlScW^M2!7Q?XiyjxJZ`<`*_Qa73;X2FS0wc>_;JF5ZIYh6(6p@g*GOrJ() zXG4b#E7}N~T-Lvx;H@T_g14yX{S!YY8Jc|PmAZfXO>WfIKiZEovjPhBWos2w=@w$b z&f<0cs7K)e5y@Zx%R!ty**5-IV=Sh2@Vc;DGN(mjhcER>j*KypTZgG0MNjxH+u;2o z!6Av}-OaAz`r^U-fiEd}mvEz4U2}Fbu?E=UKY5K@BaeMN(l&uGLm`Lkj8W70v3N!W z$(s(}&$2|H*Ee4*319tzFQ!DF<==P-Z7^mjLt9&l+xo-lb^Zm@&>tW_qJNaW1QcEk zA`Cgk?_;w1uCJvUQlJ3w6L$x{o}6iK1?y{!Y?k>rS#`Lt^tGjf2+U+<hN2gYpJ7Tdmrv?j7BZ>Ns2QbjRxDxj3b zTJSxtL00AUn3O=gi! zCrL~b!O=W()##dYgUA(CPL|WpIE2KaI>hAn$D)>NAIeZBs*v(~{^2e$X4H!OTJ2dg zD4Foww287fJRv%aHDz3;#ip3KZY#Ed7{yD1yv4zfu zE%K7g#nvRkNr|CiAUnAH(UKWRGO{s9ENgt*MtI=bBGsrjP$lbCvYd zMZAOZp4EhjWAv~P0d?OOI0R?CUBZiOq+r~JFeOU6qgwqjSyGqm9-uf43mP>k(!1IE z&R5r6^nTql8;CXP6#0HcT^ z(lc<~)qNEeX8_7OS#5pY)l0WK|@a{t6UCzb<^F+G5Fs8s(dv$VK zd-Q7b#axX*E;gd|*7~}|?qC}X%0(=Q?Ck>24&6j^MN=$~HP(QQQpzriI|gN8?Gb1R zYOI_3JliMKJIH&&7&Vz456#)f2ikt{BobHGAp17ctdPNi#uv~C{SxMQ^N5e#;!_4l1KU$ ze&@_ses{{Tink~{6XJ$$7Jk(y)JB7gQkP&rVt)81!5xmo%5^bbM}{g~FdeewDW!II zhmTX3iSR3boAg^h+$fCwKI^g`76vLf!Pp$#fXv*5HaGA<5%sAexs-kQn_9)NqEt88) zT>Oci;46nd+Q6cFjMQA)QNRXS{^5Y7T3oBxDZC{k1!T!uE#vz5(-kHvw;?t!?IPII z98T~<8I0ZGU|rpWQ_jwxd+)ZOG|Se-WhF73%x3>5mJ?$>H&h}1Owtz-s{_`Otl&{e zi#V5~Km&%ksWG}c1&lx=^J5dKg)M64i(d+Zy`c$q=8JCL6jwY}wxSZy@USuX(7rA} z^4G@cV_JEs#_?`WQAv-qAn81bQ2eIZjY+YP9^ts|dCImc_G}SKDQ8ZrCh3Xht^@WZ z3nVKo%-7AN)}}=dWWEHD!$=f_I<{ut@3kM27z=Jg!G%GztMPZ?=@Jr^F}Xw>&Dzf_iHrRyXXg1$d+LS*2G9`uiz}5 zzL%}j2y_wro_42}QJ+86~EHG-o{(Pa$3 zAW4ZhH$H&bS^E>GW~Rd#ywH-`-P1W-cyBm(_7cWI8s^)5(B%>K6XaB}2)cRNd@vf6 zIC$Sb!N9)1>oO+mYh^sb71OE#y0qE>;dmP+LVMcxK}0MUlTa*{Zo(Ol{EGYIw+ji@ z&pT(;pjMgt5vihtmsXQH5))L!;f*PcyHc{nf|V25-^p^&IxNPwnlGra%pO>1pil2e zbdy2{Hwxu@x#wqoAZ^E`21fY5J2rXfd`B`fV4>g9j&5FcW{yMJ~P+5$seZ!BdNm9oj^CuNarCftBR=hRA23WXdm!VT5so{xKS*3t` z5x?aq5OfEnsOnQS1}#p4@o$p}^#0f;+Ry^5yi#Zuh>jE3e5TUTm>S1r=}P`piCG=K zDVl9EOewKiV`QDkR3IR+U$t_lk(B?SLR0+Ic@s%-N*7-`mhdY0JkiV)=@!^OYbl(z(O^(`3~&hkiP zbbSdx8u7RTbpmg_VCO=BghElazX8q6!Pt#sw07-Xoi^P9r~FV=2j!AWBSTCzgCSlK zdkATw{ufzyax>p7MF^jOJiIOFwCDOgx4?d+9gEE328q`ja~R4Nq-g-b{!9}mVdm04 z#v;jUu)m)Mi|oC5kU8$a?Wi&VzV7KB#YuUS{C=>9ilpB)c`y9$2G#EOHDxi7VVH*Oom6`1co8H~Qe zpV5=wbGyF-<7~?^V8&(FOV|;^@W>12U>bA1E zNPNhH{QKB}xh{`6qqI`-eqAsbQc+1#$Ob2IvnL{|XzHq)FT;q@uAHYoB({K|DBNB{ z7~Aniqkn8d6Qcc)wgW$&_5wccKOWS-E*S*_K88mFd!`EdPKX1)w7-YdtNZ;PE?+YW zo-VAqI^O;T!kVsmJ!rMP_J6T==LHND(lKz)W4Ye4f+0&9m@TjetBTmwG0!&Zg&Wuq zJ6vQx7j+}yzun;Wq;cerBjn+mO|6m8=%^A1UdHAzy5ScmDaaHmsYdJgijHtE?y>Ge>=ne3-6I4>i_mO|2iiAT59EiZj zLbGU&98Fy@t5%sD=e0W&h@t$5(zv>`>IPS8AH_l*#t8-S+p|1$7Q~~cRCXc1T#bQ* z3(u~I6hVy=zb`S0&X&=Z7*5zpV8IV|g<7^A0Pm+sV`f|+&O)^H5thDO$P_eUoU&@E zJheqJ6jS3ir8@GOleCnR5E7-IafBi@$R?x+uaJqezZiCk9Ih@ z$vFsWKASH00tdh;9{ez3l4H;BN?Pb>j8Jiv%G$VpU!?RvS3#B2oVw&vM&HVcPU;rh zsVGz(vvA#>ggb=7;bx(#A$7D^`{|m0?#V4Lxph8#iJ1E+mKPN*fQlZ?MrwPN>%|y) z*Jp5>lec?)PP`0tW_2uhh#d$fK!Q>>W(ZN(bkNqW*Fo6q=Zl-kKC!^87?Ei6Fw8=T$Ct1fx-G_w3Q?dwE77Azi5?6NhSywFE<4$(fH~ zQgw@QiY~u;0a+Dpsw46a8?=2AkA<+nBf=yf@1l-2meKvzc`qo8!!klt0{=u?o!jBE zG~IRk9_PV=*e9;Vr+QBY4PLjKJ|buswM&t}g?M31`I!H(8@fH&a1c-X4k3_ z(;5e0$v(*AdKXtFwOH?lH2FEi%lw`$?eMX^xB-GLodLnVVpx9*Trqa_8k1v!2sxa0 zeGrsiT>6fzckKkQP5X@#S&ElET2s3#He)8`1thb`^)mU^t!itr zx<=O1M)O&LkHtDURFaji;u{Js`8D%V5w+6)-dVCVgY#FU+b@c1@^e(F>n2%77Rdz^ z#JX;6?yNZxQh3|xnb8U^d)&pZ`GfKXCgu`2>FoU<|G1BZdU|`dYE`W1CNTZ-XtF?o zn3Exp@p)ZvK{(bUYF|LcRfjb1_p}SiBOeXELk&~?JuQD*xQ?B|DG!d=%o@tX!0Hn! zZwXb9D|FimKt2Jpmig(luJ?Y2pKP9g24HZ2WMyQUdV32TvFZcNofetD}G>&361 z{<+mqWIhn0?KnbDKQL`!Vry2@sg*~B^5fjFv?ZVa=h9#P8~iZb=89C^r(znytVam% zBs)Jz;)hqUzfKCC;XCqmAW6i5bVr}Rq^0&C&j%>R9$k|Sx(5bB`oh?Nkeyv|Qbx$` zZeE`xQ7BeTiV_ZG^MW%|BZc>H=g?adjR!8gP4#PDt***#uz0WyFM9|I7U}jk3O`v4 zNPM6vLUQikYd_%i+aHHD`39F;le=P8SJvQ`g|ddt1?EE}}Vrt9Zi8@j8~0#F}tE$c$9 zHPki_A%A?QhV6mq^gOgjPs;-@0;bF*a#M>eSrKoEqQkLu^HZ0bq>gaG&&$7A2m&J|jIHa=!=$qpuh4Kt1eexVH^D z;Ctm%STb0bAJ;n6d^g3=rJtj7dPn5gE?12})idRX4e1nH@MaV*Yp`=f+%nxNNb(2s z%{&aTsaNj(yU*53hg{9+;IYde|M=m|@{V^D`?CRwn!H>kd!ES6Rq+W|!#KR&=`0=Q z7>xDfRqs9hb%+0?Kk(x^|32c|UGeou@mlk9I{!89llXhC_jLT`iebn>j*jf&er$oJMw$W%kb{|qxnl*^mmWpryu_}q$pa2|Nj6X^z&%| 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 zcmb51c{r8b9{2aQ+3cHbC{qzakvT%8%qnDPp0+6@4eT;!mU%3cdQwtJ)03h>gCQj< zQySGyTb@QDm7#WcueI*;^q$`LV6F4dIoG+aKHqh(d#&I4-rs%SKMocvms(gV8S3aM zIasXJ-sHQ@TT97#n@8X#|DD_CD>+zL*&5B!QnK3V?GfnhrR487Uui{VHzdQ^@~NZ5_lY6v`qBg%XFMr%in7s2=^bUG1zpJ9 z%YD-l+jEpwEs@Hmky}bpJG^yhiq1_m+l9I-7i#7<)1M085^=8Nj}OaZ*or1n-=3lK z0ovkmX#9w@yL8GsNjg8kEg1*Lhlb0XVn?IAi=xxV=gm5cc(IuRb~MW84*JxIc!-yq zDP>2a^w`to$Dh7gXA!Sg^rR@I=Q3RZ;B3d3zK!a0C=bb7n=aBIQoHY(NQC)3G1dN? z&i&_{P36|=@*9}{m?ZwrWv1%9V09+tbdkW;34r{_Kv8uj<&3z%_6dM|$UsK`3>Wa9 z07yp$>Hy$7fgR%mAJCA2e(Frh?J|MV@qzXN$Up^kCZ*b3;LHR-Dl*Vjok^*BI}xxR z{gO@9nUq@r0wv>*a0&V)N2xO@l@x*E34rLAOjT!6DvC&;!|$E4`M;_l|Hjoyj<)BL zo%p`3lZ`k$ynD)&15;8p1EvhcY*bkCGqmvM+QJBX$ROrF616}8e=&HD>>@gX+6+7&HwfPT~Is6T2Ry? z>M2#h9x%?O{}UUZTBN!I9ySHU_PJAn63r)~* z0%jr>Gp?1gWrv{i1k401W@0O4%WJ{q<1%ws;az)dD`jV<;HC+fc-NlLO4<2KaPtIA zyldaxN(tB^xCJoBv_a~c%``L{X0Xq|T)D{-RrqJ3^sUu>jOdW~wtO z-G}J&CIq5hvI78GP7M5lddb@AOv>kaa_5b?Q-o5**r=DhU7bn!}kWD2NUfETT>drN;s2Sf`;tt;w~-xpBy+ z;iIHhiq0zfg9!)PCx(9YK9nM={95{903Ex0tJ_l1%59Ul9gWhJMqdbcV=kg4whm3G z_gqDtNg<@@S>w;-jq>@t8048WNNlBOwNE^gS%D(xx7>%Xh&t^8{ls_!oi}f>*Sme_ z2FM_xm9pGjP-g<>CM@ReR?6}wK|R16`;OnQ7eu_{@^&;zw-ntP@W$IeA%Vdd%aU3| zWzMHZjSn4gTpols@p8gF$9sjrQ{klT_TWjQs$!Sxj zto?r;&&G1oKFlI&(J>*Baj7-S10wLKv?A)c7$KOsj5pT9`dDKXE}|a$DkM13So86y zf<@HREkaWOb<9GduH~SGT1djxnG{(8dKCbUx$cE*?;{%3CSWXI_0iPc0P!VnT$_yR2E zF|A6~#9ISpk6@)HVL;d3NhRH?~cTkQW1h%e=jtsnO! zRO#dUgik&RU)zpunr&HY5y24prZYJ^Uoe+W>D)-X1;hE@KYa098tXOIaJ#w{8|teY zoLL7SR0~ReGhf~;ysDXaTNc}Iu1CxLPmK00zLk>nO<)ZS^gaj^c6GeE|UMsdHvIE6~j#G;GY z(I_4o40s%(j6DU5t_bJ{8PLd}&@*u8Iutt^Wup+|JnGD(VbQ^Z?8Xqr1yp(#7F`I? z-!b41Z3r`SH5rR8Ye%Dau3}t59Q)ODEIL1+-(g$<^sy`7zdJM2cn%mmV&!A0=h7)V zhZtOBI2b!BTsD_Z3E0GVgA4~-6%{@gz#lXIL572!iVEk?rBebe8SjwcV56eKRRR10 z;{!4r>{C>@WG%u3R(;3~!aIi~J;UWP3lL3FxLRsgRaMrn2E}as* znehd6*I`k0a)iHj4ot2&dB+|Qk=&4j10%wp~?WB#^^S8JWmxrD9sG<%M%79`6|N zISXavSoTGS369ZKjN9OjqR_GIiw<}w*m#Fgi%Q3{FEntB8Zhcm>3H^KGN2!0+(V_~ z*%x6u8pTV30e^}>n0HM3A_wR(j7G$ZVRY-80 z;-~=Ndl@|Bk>eCcQve@j3?svFiX#=&ukDOcWH?T7oDSeG2`Vxi$G%8_qtuq5A;WR( z%WMF@N(dmsaqJ5tmreJK_J6%cBmnMXA|((tci|` zOkDeNo(_)bV~pou=qPkB=g9fXd62<-N-$mk`b6d^6Vn`hIy#5q8N=Y9jvULroCj}G zdG<13=7usaSoY;S`1#Arm3&Y~p<~&X^WY~iuSW7o9fgi%U(SP{!MqockLn0?Jo|DU z{1oPWmV8!6p<|i#^T4ccnu_+Y4)zk3ebEFZ#*vtWQUxF!$G*q`_)S6*8IDsNO+bm! zAf%DuIK>gf@xccOS!6g)aa03GDV>1dX-srxz&XdUFERigNWhOakl;A>#TdZf5b$*Z zBsh+J5d$W`nNUKVb1eI!3*eQ6GBO-zoh#byzL`Vy z1P2g`$Z(7u%CQFU4q`eo9Ak%aRMnZ35GO(f8IG|-IbHx>LCgT~i3)lOu6^+%a_JP$ zMlv`;p<~$>KaE^E#cKgtasCLP%v`Ar4%vkdqd#YE;P1=AdzwDP5RPi4TbDNyDG z%f9&0bLkZC6!JkGg^p!k{AL3B5c#BzLdUW%e$u&g$|fK3Q5}JfXJ7n`0R1WXtd2s* zvM+w3xpaz;4cfyxSdTdNg~I|RCXCQTsR9s=Qydon_gfR(YWH^R>;YAOA*JuaFzr3N5TYk{>&>H@Hzoj$>aqJHb-aBjDFKNN}7TN&xr~ zVlL{mVA&TtuoOjz`N(jb9jXD~y9oHAAJVko?9eR${(*pBR83USGqCLo6Ig&DGB`q^ zf~q&9>&pig9uGBC}Nk-0Br(h4 zWC&vsOOfF?_67832)j?fcdU_aD2{yry&87SCE$n5NN^nc0{S-WI!VAUno;0b_678C z2$v*QqRu&%eF6O(!sCe5sBkR%0(v`yZXleH;W#@K^mz!qN5I!~QRW=WzF2^zXiTg? z9y!hqoetnd#42Ps&JOheOK~!>2EZpO=xMn2rAG)H(kf(dghI!%FFoLG#!cC1$q^Vk zmVM~~uQU2k2{qK2!Llzs;C)6PHZcp8j+MXkfEOBlUXzdN6PcqlOmpOD=}Xz{NIt2f z+*2(3(gR*-++0CEsw2?x>`M=LqtRE3d{#%HW7(G;@JgfaA+(2eupY7O%Q{fMnh6(_ zDgfa)#c>`eF$)PdWH^p}2?Ox6gaSCj$>aylZtRp!WS8iV_!hSitvX7{Ag^VqZBMfEc*hQS45Z*urC-A9A}+_Mivo& z6BB$^M7~@PMvi4)KvRpbL<07IK^ZxgeE|(F!oCx*5eyO>XNQ7j7rQnRumcGa9A}4u z#uvL<2tU-Nh-F_u6O3?Ef(_sk74$4@`vT*1M>04ZQ--!Sr7eIj#|g=vlyEPN@x zAIL{_gpp&~7w|^omNn$FItm@nzJOO6x7mm`0 z496L{AXtiu#BO9b&JJA+;2A^$G8|`zssT8UNJ56=>`))D6n%*l0H2_sUrolgFECCI zBZDIpI-Y$2Z!>QGfRr4;vE$hn@H(UK8e$#l%;4D<@IIsOU19?&9nZdi7aF%1l8@>r z^Nwd)+86Lj#Nk#Zg`b(*I!`K&E5(7x~G69<-eC7cw2gz{ndwdNm^-nU3LL0m5yU!rlGWE?`Ke&@;IXVsW*)$AoXUW${!#E)UM4dO;!_x~a~BF8%gkgJK>QE`wm3$?W4Rf} z7=ZWmVZesRD0nP80OM=$aD-_ zlP?13A?wIKaVYbSV{7uYAia(Oy9XlCan?Ju!l8zY8>lmbV{7skfUyUOj^A_YU*iEP z0gkQ7=Y#YJvgZ`a%;4CXd}T=g#3)Ce8JxX33DASrGOi=jarUY{q~B%SM4%hb`8T0K zY=M+d06P2a_+8Ncb5}_^PL0$6^c^e4?}GLR9cSLBLi#PF6-kaAXWr)n`c7S>6-m-@ zY9t-f4O^;3$QysqPJPiMwjn;qt zIh#k`@hz0y9%L`Hbz?k&{1-jb7#f=YuZ!uJZ9^5n@OL+n{m{?^z(FLeb;1t;x4NU;-T6BTwKp;Vi?2U#d05&eVfCLnXV|)io2u%R&U37ujP@tDF zfGz;GFS-B&2qdx?L&yTq<)919f&%v#KhXt%(LooO0tAwb7{8DOpw~ecmGUV1K)Z ze-j|>~7m1GJe8|Awknsqaj^lh_U>|s#@dTBQ<$TD%UV`xqnU3>x2m||| zXhs{LPq*&f4QBT7X=Eq0v2N^d^OKHgtpYsbTPX2EWGgka;p5dy7~3a!lkL>d z1n}x5Ozsm}$(Cwp0(kWjhWCkPWLq^f0X*je^ZUee1QUG;;W-}|;U`TY#LxxsoR1hN zK1qp$1hN38^U;L@--yZR0(j0x5eOu^6H>?mn9j!x2qajL-^?PdB0T5AqCkQ34e6A9M`o!y+?%squTS{XxfYJ}j8&2ks_&uZ?wt{XxfY zJ}j8&2lkQa01_R;`LJN7ALK?x14wiX=fi@Teo!Ns3!u<(oDYl4^i9Z60Ev#_d{{8k z4?c~U2>k8{jO=_^FxF4FOg3SgupU4GtfJ3?$$p|T*@$hd`T1?FQZcPnnooQSB{7Ta z#D+F}ym|@4{ls5nH#Rf@ta{0U`F@fw*^v!R0L%HXV8oyFl$eQh>cTsW<$PE$V$^f%eI zZLAaK4?2$XfuVn}-1yPJA9Nh&0~7z?6fzn>8as~jfpLHEAejrG(6O8k%=$yT$xr}^ zj^lh_&>!*$F%f{hXTcweL3ci6(Epn3-!@@AfC6|WlMMQol0Dqe1n^2G4EmF^pwPw?D>?<9uN5AN-8$_%_x7@CO~o z`M}UW#DZ%ckmxw;9R~fO z!-$ChY!!^>d|=R@%phC8O;``00AA52gZ^N${TrG9Ude<(e{v_;0uD_8t7LNc7YQ*b zDb8dYxUuf=Z)=r;X{`>`3l&mRD#$)?Xv4>HJ{(mr{8SBMEz+qA?=Y70;dnuTJYqe% z0G9LNNCJU0eu9N8faiQTE1|#+0{&%Wqv%e37mI z&-rjffIx~C`JFGC0M<6lmRvT)C3aM!;3HU84+VJt5&lWKJ zv|9xH8Wl|d&-qLR0_i#g{2moa0Mq%bhXRKP_(dw30Ny5;4+JuV2>4AZk^rXj*#!hr zJ`y{TtQFYCn9hd|1X5QMf#?Ew+jtQaxJ@?nLYpGIZ9EeQr0J1Oz0d^kw(%w?aF}fD zHP$x%ea?VxkT}jq8qh;J$jAzbj^lh*1A3?vVqOKtj^licAiZMzK68J}436^&2J|pZ zvdtX+ z9|b@UUxt_nfE9`9d|ZJ*steh0Zo+y11@MZ#DHOO*Hl0Hgz$=+iKp@SSY&?f1fLAi9 zP~ZgFd=5(2{8POwL~Ve0H*U<1_f$~J?H{>n_xB&$kZpYkp(cF&ki7vHkn967r?s$1{8=P zGSCI^wy`Y~_)PW!L)rwG&Sw@7NMBF(149$Q+s3|7ppNVbHr6iteG-7qpg7KF3ZRDx zkKdr~4?2$XSqJH1hzFA0A9%qhXQHD6?6f-k|_uUh6vbQZK7j~oFcrExflp!dK0k08kzu}^HGBWtpx0` zh9-dLe0+dFmKgz?TA>Nx#Xd4n;2eR}*Xj~(Un|~yp+f4ODMStO62f#o#$fn+5{Wy= z0+`Mx1PEmK5EbYGc-vSE3OpvN&;{_eu@ex;v>IT_|v#xQ#A=w~hUQK$aX) z3j`)S2{8Nla1;SO+>MN^kmxwh#|_dO(ef%db{ywp2Ivtc#6IMi!Erv(kbauT1N5;P z1pjuNc>R1znV;CQKVMu>U_OPGuM8R+8Bd-Yr65cQxkjeoRz^a(?azG%_iP3~EfK@T&O`yO{ zMhm(CR>hPT1OiF37_I06SQS%V4GQdMJVqD5GCuM$Kp>gMc#1B7Wqjm~p#Yol99;m* z_{fVvfme(d$O3q&kGw7rNU>vZfWTPi%D{7_1X`#wTA13hZROM4uw8ZJaL-1zs~=p$lMb<9t0Jki3-f zdb}m{?*Xij8TsT30Xq8`(qA(n=@`Z*UmehQ*f3yZIM(w2W9%5lCtnuQ%NQ^(M51FD zpZvLizH>SQhJ{FUjL0XSAJVfJFeyZ$V>q9DRY)IZAjO1m&0sj6d`UnL*vdGMx*~C$ zPyQT8Z$oP;4#tk-eDXyAJ#aDERD7a}24@B@^2yf%0=rj`6(3Ci%lYI(&%C=Dt>}|i zfaQGh7l12>*Jf~0DjF$(S2Fpaz+YqwF|;YdE1AkbAVHAqB8Db_<$UrdL4iQBkrRrUHQ^_3@jE{cElC zaYL&WK5;FScn0G=@+QD^K6Ehr_+Z9IbOF3=JQE6ZGP=+O@V2ou5J+%ld`1_*+r~yv zpn}m01SUKqF(aRRQ9$2GMf!s$#*mKVd~_h)p8+F7q_N{T9|cH%#ejJs5*^3+m;!o$ zJp+b?NOTh`LlpPd?Z@edr|#ScBH}o)o|< znF3JY4qDfHQUI@H&Hw_5v&k-EXg3+p`A9*5BV;2nGyy#4V+aJ2gvnlFXaac7hY1Bj z$#!B$0+`N68w$KdYc>vVGN$vH1_Y9qqct0c0(h}c{z4#-U_emDD|5fC6>e%3;2zgP zNjOUIA^8SSh%lWG%>5IY1V6d}-ZqBOf8s8JjxK<=jbZwq_?{3N?*{zNO)y(H=fm_r zK$8rHCp>`x9mn~==s(~fnF=G(ahwm#{R0KbNEnHZ<9uM~9~eaD!ANu*=K~Y}z|Qf* z;6Lsvj`M+Wf6y{A7eE?2j`M+8e^3<}3ZT%joDU58gSF9`ih~u2<$PeuAAATg4uJi4 zv0iY_hbezTFH(EEx1B_MmmWCk3!d zCW{REH;`S#COp>RO~!IQEHdb?CmV^O31B%N78&##lD)*x1hAYB3ugVv$Iv>B!zsdZ zJ}embr-+dq#gMK5&-t)m;-3!-7G7$Qrb!;$Y3-I3E^F`9o?E z;{e!y7t8su$dvyfTGxA00L%HX$dum{t?NB0faQEx^WoF_S+uVAqyS#Ygh79@ELzul zQUI@H!k|Anf$SnS;i(I6GM@8+L4WdBvXK~?0G{)KL4S%H*-H#f0MGfrtUsjzt3bz@%=Ek@dN^%o17k&76+Zd+*DJI1H@rM7~`GAh0IL?Pm|D(ys zYQj?t&~cm(jQ)c@Bj#0rj^lh_?jO8?AdqJU$N9j}KlmOYf=tJ8J}~hQF(#t{qRjR{xnatPUEBi z-adede_9J#r*SBN>3m@5pL~(9M>^TTD#CO=F!xVUARN#I@OCnc{!`KjCv*Y4oeb0e zlp$i-cxU8qYlYj4y=zlk3nkT?SOMsZDHKX*ey69q`zgvsBc*qgjTUqvcYezdy~Y}D zSGQtAeRYE~>)?ZGLCJ6C%bSH)H4|^kV*AbYXt}ynk9Kd%>@E+0ixhxgEZO}}oR|AX zpS|XKABe&8;8boUMT@#5gpC_i;f77}DoU^IiT3vnv^B(CUPn`pKRbyqIb%x$JW6}~0n z{GU1WgAq;RH9FkEz0;_a(EPwBL6=t9$eQ=@7GA$`=0Dlg^bx>o1nuCCI-$uy}`d z+B2H&M%U!N$*UXA%3NiCJoC+_C|`G0Mfd&7?#=PDC#&t__;9xjo~|7k>RliFwq5Gm zmfDO%vjblIGjD}Y^ARJT`;tw%Zl#}oo#}n>-vWdzn)~Yct8NnA~QPQz6`TBfCPb?M6<8U|V>0 zg-ly`PK8(-FS|mqjh7QiX%ozj6mJvEi47xzUZPil`9Ma!>E7-cqaOzx>MLipsqKw4DD`(7(5^Q<*u5b; zQnS?GVPJcGrFPp@72beXf#txodeeQ~CfSi{rM*iBe7#;LbU(URsoCbUC(^jI*Ky#W zSAosI!um?JHlLhG-O}diJXUG1!$4$xrEc4x3U9=#VDSL0-n5`wxIRj$O-z}0%gb%a zfK`2zdYhOEufxkMu6uFzuzIPY!+>XflvZ1o3a`)0EVf%Mdswx!VCle$`Y4SyCl%gD zFE{G}gZikMZOzKOHZQZJ?)TZl3rY)|288RQrnl)T^J=}S;=A**hxJR{90xS(qh_>~ zDD$3pRV8*m%O0Lz>gF^cS0AO)=C90a^r}kezM4I3RQlR+z@$D(t*uv?_sXj(se3Sc z*tGPu(|}<8h*F!PGViumbbNPK_VDadGsgk7`VrN(0%hJKujs_?=Ir5lrDje8GW8?M zZEniE2VT($-6h$>hNV@G1IG0uGuvcxhP6wp90o$_M|9d=tMGVUqcPoz*~2QO(Mt!M z>PKd^nW^yJdyU3*yJZh+mPR`a_}7nUw^ga|2E9gOyUntP)k;T~4$Sa!vm5aC8cpb~ z%8s1Z7OleT^%~vX9i2U_TRQ465M5t6>CLEu#n)r?-QLP&c^M6^I{M*1>bs?s%hsef zxX#iKe_P+ZO1W%JMuV%iet3U3@X`8insS*~ zdV{IDK95`9ZKYf$meF9UrO*3P->srtmX+RMs-e&OP~YvTT$Yt_?#`$Hg)%zIx*sB| z#5e73U$|R{sIP76z>$@yCogdhhdr9lX+Ly`(~&PXkZ~+|Wv+y8o441c%S(JWFSE`W z;D%K$aH_0&|KnF*RKe>QDo4SBzMwCWzsHI;=n>0163cUq<+aE1Ok;VKvAn2Q-k0h~ zYp$t2TT!Y!OO#W`-@_B^5#;X?r1uC4^au*}c+v)~_y(apD< z%XLLLdHg;0v_bD!-riW=vRK~N>PQ=IR3JBMDK}~xw^E;Nro(oVDpyo2FA?QDftxl!z*HWGh%r~vAkm1-~+zFX8xW6CdW*sJld2!YRQcX<3`zVqrACMtGH1?+$aZb z)OPMjZS}ByEH5aQ*B{Gc#_}>^d3m(KLwtjWXoDO01~vApfEysNaqUjClfOiq4++^!y89LsZw+Z3;27? zn4DJylFjuP~=qj1w(WZYExCCR;vg z${w}ij?`5TJH_$}XoFS!Jz@eqVuC%%WEU$_-ILhBM%H)j7 zl#8iz9@Pz1uwS2JE3PdU)8H8Eas*|1Hc0ktknY(a)#IeWxvIk{V3nH{v7@(ij9jW2 z{+PfsO5hm~gWH7%eWZI{O7%2ra0*_tP1UuTdW2CfZ_-z7DlNekh-BY5$adRoJI?0aS-cjkqMSRT*d3-<*k&i#W|!Dz=h%vC%CmGiYLY#Zg$HdDc>mNy+I5&-W*4k3 zchcqfNcI>A53Wn#b=O2Xc9>pe7py67*5$}Z_9V&lESBsM79O-o;JvAdwC$+8#CBU< zuDGsTaaFmlE@zEo&tjRLI?0|o=^jnt!A@duRRXUsf%mB<(xIdB3fpZ>d4Y3z!P@eI zmE{Gj@`B~%1?$TTR+pFPa*QQ=sX?Bby92_7GurbRqRzP=9)S~wgXG}+1((v3rp--9g}Uv5i-8( z@TH`DmS5tbPU-R)ehC{ojmowClA>>a?&~e5^(32gG?feYq~7T0DHrTX3GZkx7wbvm zb&P~k6p{ryZiNadq$YKAgz_t-Ebe$1Dx#29*U=ZscO+S@<6bCnB(=EXV<`Pdicd#d zsOXWj_Z>e%sjkT~9krnh*VMd@x1j>ADQh~Ogfd;zo^|wx(%O@aI~qfU+f%Q0d=3?C zPYLOG6)M)AHrO#dL@`Yk>$p86WSW}Q(K*C#nlf4){bt3`9Mj#+ZvuuERL1DOu^-Z} zj4OGwb!dKNtpA%0Lq?Umd*6f)=|(7;Rg3k#9+HSkFW?prO^M2MFq6C{L@qbCJDGUa+pVj~$UuQM;4E-k8s?R(p@VGoP>c&T00Re0p*1NA|9K zs?VK+>`nOsKDBM^;C$NqJJ;BL`GW6jf3Txi6q!2-Y!4Q{Ol>WD2a7N7P7&LeMbE2! z%MN2v*WB61_GSsJseQr@V$q)6xy;_q5`0$M&yM6!jPGQyJvscNsfXTdDxc>UyW!31 zaznq}FW&?Y8FLgyGflXe<&%5TZ*b3-%k^Z2bB~nE^kne3mF0>(S%TcyP;rIyByM4- ztU~5u?%q&Ig^W7x#ZY;LEH!RQ=%gd*#oW`OQ;%f&a1Vw`AIW&ny%wr)Buj>y5GvuC zp2sZ;o#L9ghPy9R$~EH|_j2eo*DPahM(E`B^sC&zL*?2tL%4@SW!f_axfP*`?O9^n z-9zH0=~>+4L$aosPTZU!v0rMY2~M2`Lt3Uu&7HzS%9ZiDoti@$m5C*tazisK6Z|_( zhIA^EMr#K9XurfqGZZ`Oh8R()1s(5)1fo*hI-U(Nqtae?3=YvolFd4rhlEE`t2%n; znjalCQ4)=(bt=tO7fsyVDLz+Kw7+DO3U+Vz^L74{mdVC=YR%UZwxl5sm0<_a$?B#Xi^swEy#9Uh_4cU-O;^Z>LUe zco#IfySIu?F$4OP6UXi8Cx7rT|zq4H(fkZgo}@NG42 z&X-w3b0X*p33c4EFLNVAO~u?|)ZE1)B#y*aTo4<$oyUE^rW}de{Y7d>V*%4`cgbyI zcZTRKkF|*^7x)%;9Ab-P-ewF*pR2K*EATqeFqD2Ip|^T|e&Ty>*j$=V$Bi!whfIG_ zqJ+%i0@w;V)xsC3G9Ht4Zb`Eex0myou%*u5O8YX2m6X>(=frPd%elsyuw|k|%wh{_ znz`@42oKHpMMN>HVqe$vatFT%4ypWNjEF?XR@J=b_I?o?@@ZDyyF_(TiuM2fuS+kO zqah<&n`h#v?Iuc3>;KE(<*5y;0v>O_v*!LfHLp#_ zb{NPQA5TloNjZ7gdB(|4%QtMbUvZ`A!8(VhkGIUdq*t;wVV-v@`&Z?uV{!(m`>#f9 zxu&;QeU7GW+m~JUx6C*w{CrE@qwSmb?=#A?OZ{8s_UGkK3iP|zH}w3~?yT(jsWg`m zRBo)k6uyo5*?aia)&;HEPoKILjkuOd`r53HEHR^iRls>};eSX^Qt*r8i<5^>MkXG) zvZB@U>dEDz!ReEain?!^wA2349dV^OE*g_emx!5s|JGfeHAjijx@%p?f!qc5PW7Cq zUD5XjtPbWBu9hV9cP8JKo)%$zqQlF)x%ghfi=vJ9PY1}I*=Fi{Ag8X}>7-pi)0>eY z{f&nb=f+LH@q0mfVZcKCcba#vJGn5mLe*1Ash{SkZn zgr?^{tlq-6Gh^Vq!j&eczvhm~5&y`Wt!y2z|yVX|9W*k#rH03@sVmIXo*PQw`ih*N5#jp4Ta2(RZ|UcZr<% zf+|>aI0vqYc0b>dSVDbypuWXGz4TTLZ>Bl5jq?|;UDcq+q>Ym&xZ|jK?2$!0f5Q3J=t^uY12seRO#6T7=QE=KY20c^6r!{)J*T zRX#V0-_1@6(hJ-W&hpx^-)O;eE%8s^XDEebc^>1wz2ZK@@TB3aQzteQ2h1Z3*B95V zJnPa@u|DW?-ijHQT>Ivpp&EZR@-i&b`MCRXx&GxNc!+ukt-x45qZyw5F)q_IM#TzIO}6)@Z~_no^@ z@0@u5n6BbohOK>8if_*Kj2>r2|D5uyAj**S+2xp4*IJc2S^37^PQ@zy$c0s(DCaYdqz9Ht}FYoXlKh6x$do^$9`4*Dv|%ieAZQ5VVG_9Gl%8-(zqo_Q72&P zvH48d4$WsH_ilfE`Q}sQ`_i??f0{fCsjBXO)m&c5t19<%+xE;QK0<5T6{D*M7qfm` zeAcJ(x7p;%`JT<&XB&4Hyyzemmr2pLGPu2UKDYAN( zMf$JJlq@-+c6nLF#m>k9VqodAu;lgi^NO7VTM9o;d6{KDdFk>ZH>>YG!YymO%f9Wc zx#N4ytsupst?nD#yh#j3XfAzAVl@=eRPPnMv{kA;!_gl2fyw9ru0tLa{ZpvZ5^(&AD+B0 zaztmwPK%Vw)puqD_FcajKeg==p`c>a*;}ipn&T(%!Z3Ae>b8Cf=?+JZRBXjo!_>Gt zr8%?(v;zB5oAn?5dDCR3ze0w_0+_RL{?q z4>a$2hdsJ&T5PKytsmw%d-{{F2i$H>mFilS@?mqN`g)c=FTK{9n3C50v7Inu}d>))Cee0=l3!Wk645OpK7mB+SN9)4PO zC~@uapv@75{<8{tR!bKL)txptHE0((oxPE|Irge-jCTe7RM1g}){pbwZ|~cuU9Ea2 zY|{&YE2;~ER(YqFsyJ+qbD95Z<(`M~dTkZmts6=mpVErYaQ8)B|I3f|a`WT*=Le;T zRr7bptZ;hr<)TXL$|>iql~x;N$AtGSoqc=r>|K#MyFWc#Kb^a=UvIHji@Txqo8z*3 z?q|lVJN!~f-|AV6@Zu+n=>IGalsII6a@+DZB_a#`>rFb|N>-fRLfJl`;okoy`_QLf zMb;~wljAO@zYCeVe22AwdDztEDaNJ-+tn5(d@2hyYg7qv`SDLZR4HzA;C=JolQ!iAsO33`Ie!{D$hHXca}6So%L$y~VnubeAp znp1pE2l=J43gw?JT$Q8LSiAT^+>|AE^yk`ATW7RnTzcSeLor>(n;QOYN%&QZw}Ex%hl^RB`thu1K}&yrVdWO`hDTamc-2LCG$EPh=4X(h_OwT08l zWN(}Aa3bO5rLfM%op+{uXrgHZPyW7I^Vx{-gsZr1O_kZ(lfjZ0qcU^^aLD+F`52m##g! zvoUb$z0HqKF8&_e9MWrlUDm28*!Drh>fS@@t1|Wng$$bobAxAeX}RyaeoN`|KMy?uyjGuDtTz-XSsmUXaGa>E zDgG?AOog4BJghae=C1>b1ve1&9m`sOYDwO$o%h`6OUkKR_b!X;&i}Y_()=z$xow^5 zi#tbX#is}dC8xB987f`4@-yvkm!*_z z9=1+OCN&Q)Sj}`;E?`jjQtzWC!+Tpy(U$4ov=_uQyz5e%q9{{s_rT6%&@h!9SpGsc%z>Xplk9RyD?ISFCClML)=|c>l`9DSIVr9x34Nbvm8wmA z3Uh}<;@3oeUb0T*NqcWjsD#H}iOmn5=wu)N@*{TXsdpM>vf2H&JKni#|C}m&WMkCh zisK*kv|5Z*jcLNub2}679el%|>Rb?fYsb4g-q-T0R|Seq)0@6eeW`P0x}}OjlB4cZ zpL?PeO!0Nm&61l`L|ytB$KIyzeR590GGLv3T6&OkWdE+%dD*#d3r`l(yf!;fj!Rm6 zS-bzo`di(-9uJHgVia0W2%0-qXB{J){I%mO3YC{<>V{@cKA>J>VVyR-r|DPOBNwHp zwg24Tz*i7@qxj`_Z<&ZstDM)HCO>ddFJ?6OnbStXPVEwVp`La9(9Oce2%&X>H7lqu zzfXQvy7#DjtA?=bj>*i0rK_oL^*Z}}W_Ql#94PP83fh&YeEqs|#M-G%ralc3$pt|_ zni}nc2BK1g{}#-aa_*xr2RGb%aZ7yLk-!Ezy-t2nadAbJ664k5(6#q_w07wOPLAB{@O7tU@4m3pzwbcAgYY+rA1qBZzbr7+=1!}Acj0)( zDzlUItq-K~_XxDwoqA#}+drq-(|_|#(SZFYB;xhTR;3&cJ!xdMBPm39shgtN9p8;Y zivsjm?j_$;t|?efzCNol@wi0F(79RBEbF!N|xRMCavbrSyz36-*L{S)0%6K?QVVa<7ShzR*xbc8bPOE%c!iTd9kG(q=!k9gDZq4)0D;mttpAbB(IoUVu;O(Tjb^A}%m-@ai z79Nzyd^pc~`T4|UKWESAU7WN$w@+V3T9B$D_Epwb-+Ld?82*pig^EL7PN{;|WEScw ztm8)K$FN(R4D{-xxBm01=F-y0#1GdgcJ)5j_-px5A!DX;r@Hd2NHx2ooZxv{ zeEHMQmWn&YOxkVmc&Az17vU?fwH!9P%wMy!<^IdX&)Ib@DS9E4{I65})~NO!+!hc* z)e9(z_Y&O8Xt1LlZrLMebZ;O|bs+SBMMoXK%U@4ThqoSjKsCvs@d;d<$zM6Mlkm{2 z$b6zCRDJlA*uhFOOWxj}C!cHV+pqeUb&ZzR&u06r*6xg?^tmbq!7Ji+ZD*clTb(vu zC*GB%rxdS$V#ZqY+aZGTwfr>)qz->p^E3!{yjDp2m3L>gzw`I76xr5z>cKIdl_)>r1S_=yh+Q!3g|Yo0AF z70+n7X(nc~;kB%F(H;5`{TAhp;S=RQa$G%~dae}9?7JNI`R&hylY5S+elFAW_!WHX z!kL9@(pAlm)oi?MlzjL?@XqNm^IdCE}t0B>Jp`x81k5IFx@hY*o$D>qkH69eowVFDD!R zW~XqgL?c_cv+(ZsQ}mgS3YhdWSJsNuZ}V~qJbyUBxIGY<)`0vs`KDcdH zNPY(Y;U$Hc%1v7)%{pLf6B9ZtE57;W&o#rTo(q^WK5R{W!dv3oaY=vM&MnJ>z3y#4 zeoWT;@SY&z!?_FeS8q-OH`E3IzMSTu>MXEfDY|9UOo(c4}%^mOrA z`N#KATZS%LdJ5+bn|zJa%!GWxBibwa#U*Zq`#JE zT%yjfjaaH$FzfSGH?#61XKE?l+KNrP52Ph;w$S*tPk7R?2kxGK`5OjXqzuQMwwHYq z89uAuvuysaUl#-lRzJ$sx;}aHGc7swE0>*CE)wi}%x`44UOwsLGe@T{M5IpR8sDNv zhi=SlwaW>Zxq5eLUrD7zMA9tcx>BciUQhKpx0i>P9#Pu#Dept`idVjc>Ng~q0uI>; z4Uz)2)+_5gqo!P%YIJnc!$)`52A6v==cT1ce$@GD7fo!+9_8~1agIO#`Y`!=hRBP0#i<(`n+p5(MV;6f zHQy%xDy{hZg?U@F&AH!&kIrcKpzIf$f7Gzh)IP6^)tpzdojP-RQ(39$r5P4Rb24V1 zJ#y*$l;Dpa7FsL{ss8@u>&p-IGhaF8pSi7ceD~fS%a_|OKVNWwnL1~FQp|(;OCJK1 z3RfOFZ>{wD*J8e&MuWF}$KN)~5dx2!dgQ1&Pt)y}CTc%5R~?ybwS1wtodP_ZT5oHzzaeY>mhZb?q^}8oyrE)^t65Cv*;7uhr5hhv zYZsij>*v#6v{T$IRk`8YGgslX2iM9TY*JJ2|7BTzwDgjUpm5$|!{aYs6$wemPTgC- zS;2xf=Z0v1#uSmwnvQz%foAtrG`sF52-$7mSash~DVp&)@$72${uleNXsx{O@yfnT zsC9khC5?l7tt)4$oi@Lcn0T;1!CUP36lXW?X0tZ#l~dUntNFjIH12)WobZ4@{rX0^ zWmOFoz8A|wban{OUv#B^%0Bn4`#wD1bk#*vqp49*I7Zm`+g^nj&$6(YRpkyF#M;mM zWbf|LKeo~Ac$3HR{F;oFa~n%$?%n(1o_I;y(ytwf+Z;B}m+C6sdfP+dOSN62$v&}b zW_e+u-&3ewt}do6n~Llk8R6-R`CsvsS{;Zf3tOw@u+{MGt$~#_c2UlM?NXe%@6gp1 zr`$80A}(1RbtqAbUH7Sa-ef(I8(XhSJUZBLuJOsEqlxZsgwG6G`do~=;u3r}C){DT z?vyj)o#Lym?=-a3Iq(0lHP%Z^i&~*eJ^k?5`Yj3aZedqa>mTJR95XRF?5X}EzGhWaH~tQzijX9*cY*< z=xN(+H>rQV%>5i8otpi1>UOzM?Ja@C!kP!J*B|FL@v@hQNvDMD;8xE&f4lY7l=KDr z%Ig}4x!3!wr^ocYEEzhTe5&NZ;a0}Mv^kGu-|A<7@;uRhh%w#x%!esn(d-2Zdu(!6 z-fQ?^+H`l3>&lx3<@r=41By?=$iab>fU;*hL5@6_cZ zbqWp}YvXR1@LyQfdqZ*S`DgPqZfriX`TddHov+tt|wJ zUw16HmpJr8^>RVz!qMZ*d2hQk`#7Qj$1S9t>K4s^kW`l3(Dl9Fa;I^$^M=T`4>ze_ zd-`ee1{JD%b@$dSU-vKnmGAP{k@+rGEKFSC+L=D_%h~o8g&#JXm*qb%cqw^qP2a7d z*Kz*bs%qn|Xt&C1>cb^{4k8?tT-nH^!spKl&2G3Cc*xy7cz(Z^m&9&IqAFbIM{2oU z=s#Rd$+&l>d(K#V@|?S=Z{{(5wU*4StP|!PUBfxEp0uwzVxGEVK4+5O+ZM`kEomQv zx!0+;`L@sQ>B z!ZblSrLP_9)EgGbUsqUett)Hbzg8t~|Czn31-c5~DWp#dP@Sc#?6P=q{o|M7i)O`f zFD@AD&z^U3hi-5~ylLyUcyo*Dg3?SKf6d#=iZ(8{^$WFKen9>BGV2GOx#!;g^?RgRJo;a_LOn&JytC;zBl(m(ssi7xjd+y>_E^QBk=OrwtxW;1B{Iof_n!8O;95~-AtN3hxS&2ZI=^MF2 zD?DEP6(F*8`6t>QznH|N&4CNHv}c@j|8~OjvfhxUd*J@hDlw{o&$4&?{qwBW(vezg z+gW8+O=81|X=!QF@TuTk$1ZM|msR%e3!grGiO9T%}M!3*=cyC*d| zsh55wq|_v1f1P~1NjW`u-LtSubK0#1>|2E_*Zw#;)3Ma~#_8*wkE!1(N0r``>hT|+ zC#!RjyUXL5)#jQDCu#!&m8IS$evLjWWWVNtldjyOy#stZ+23y6JutH=t(oTCUa?fV z|Kzpeu<6}L&J@15FIQ-P$XIZ1$DG2BPyYuDK=Z$kbORZW(p`>6G>#n?cXkGpJNU47 ztN+IS_FeaFb4XPfJ?=&d1;9vH6)a{2(+arIgr)9qjZezRmg~M&48MJd7H1DnFNp+n z<&1YN>+dhvV&UVc#84kGwd8|Kh~XQvusdX!iQMS`I(IJ8UuKCu;*xwtRZ%RL!)C_bD!5pW5_D%MW>!123L==)Y`0 zIOr<|=G+sVTmnqbwP0DT0yO!VcG2g+HqnwoRd{F+`TayBqn}g_)yJ;JRe*D3Q&&O; zC^5Vuz3^iQzhQe~X+rO_rB=dD**}L^<7;{sv&?*$BxFV+&Z+ z7P_FZFl~|@1W6q5D2Jk#2wj@iU;WzFdcw}^fIvxXDS>b4Z-cOn-hTBc2*8tSABBEi zuzlgaE%#cZC5a}HD38kFZCpPDmT<^Sax!OkHEg!dlu9>}28#u;;fz8;s3KZ|1vHv^ z;4Ehl#=h~@y&jvhOU8{o;0F?0PoU`Y5;3L)t>9N%170alhmV9{JOC2j;7RnFG@=ym z(J<|y4};XV>MTaO(Y&rn|GVXj2tw{v`uKMMz#B#j!5iit$Z-eh-KF%9W-dcve{;!#R!_ zTASACnvS3zRURe`4viPE6ON)cMFXh;;7=|$V)xZs)&*|RSMDSp!Txi*w2+SvQrz+5 zh|i&iIU@J%a`g;NahRFE1-UsgaJV-JnP3Axlh<*%AIuXpq=4l5A^3qJf5rRKwz$hR zmVr=1EG5|Zyj%*#&Q_ZmW(|}Pc!{v_(Yum7%$%6rx78KYr)`JGwo0nUK2=??i(SYf z8lcD4a2i`@B4LAl;FN=JwL}F5zfvMzzU z#i-nK9wg|mk$NjdJT^f1Lgws>hvr8#TF$V)|YnhX(XwoA@QldODQ0X!9jYM_K*gY)s3w|$R6Yr;U z*!9Ai;Io37QEw&|slwIL*q;qPy_{~+ar}(kQU2&Y%Fm@mW#&ddeQtvIB3gp&Ds1MxaOxk*Q_a*eGyNDW6ah z0N%GURp13N%$MLpHgj?K3#h7UCCpcqfu*RDn(Ssk?yxF~WdY{56zj#kT%@&-?It7* zu$`2k%lr)G&`ThlcG1CKOT_1kf^f%zEmJOA7bu6b$T^F8oeX8=lCxxc;G=r7_i<#O zK8dlB-1(M4iGt_B5E-#i*;GP3V}eMh;DW#|&l^ym5GTWY&)sB#0EcM;!^jHF~p1}nHTq2Q(KVoT+Q(L2q989(T?6F_cB zB2(_Ek95^jM$9mPeig8Pp!7l*luWWr44crYK@^91u;4=S=pi@`JZON!bIqXEauwLm z^H_U{^4Tt&4^`{j+7+Ru9@7cY2(l)H9NopE){VC2a0lgER4sO*zp6Gmc_Vn&KzMHS zuFL&w9SG%F!AEi^gsTVSoI)ip;(QyFD~iH^K_Rmjjn6j2GEG-&4#hqflyPSqkC2pu z1B7(~5=o;jpes{}scl)xVy3?JL#q}s8msWs9?risx7gIbjV2;MK)P(I2LwUQVW}WL{2V&I5g8QA5cm>3T(9zc=HPTQD?nXoOkaqchZgc4f73H%m%6{ zv@FW#e8jfpa#$gv>A7PN7}yukw0G>1#j%KzpvxC4j$qh~lTn0y&9y?WLHTBNgt3=Yv6d~&}4(p6Rj z;ka_3m*)mJLIYDp0j-rBYokojf8?_Amhy&S=$CWtXA8|&z%rTlsHY4&wZHN)e175- z++3Qz-jJhc^+3Cdvju3peUZ01+n&0qgNHEqR*7 z^r=y)LvqftE{aa@I>pL541Spl|B|k7p2LEX*nRy zVr;n)a(l3PgnxB5l<)v1tse-}>!d?Wf=Gfm(A$@yXa|D{_)z`_u$mO$PY1^3U9yud{rus>NOf}|u*YQJHNtXl~O7N=8AJ>@=i+?hi|WZoQI+qt}>15FH$!MY!J zNJpm=rMXN?bhowV)O^c@GJIh~t1e2*_?>czvRQWnw+fN<&mkpoAsoIPZ=;VErzs>9 zoJaaN8tGYceTlp~dml-#JL*8LG2Jr79i}Uo1XN3N>vxmY_R@{95K;}u zdu%QqO!!3ZtI_(AQRMFSQ%X!Zzz{Wd^91=^En{|OpYXG$xN2qJq#r?DFig!|tZ8#Y zeAX&D-5CwPVBbO+!Hp0RA9cfq@MJKLid@x+8kx^o>Y}t*_m77VElAp3TQaIS^zUe? z->!5Uhd&;Fxm_1LyV5R@p&N1NT&NBjK$FBACp*#r0Cxkw%%*uv?B{V zuI)01@K$Pzc;%rs-4YIWaF$XNi-nX}fo$Zvae*_$yN@LKa^^a>>;)^f_FDFR`mdCKE0m5<-iT3p1dTo*JlSIRjU zIfBJ9$H!;;LM}P29vcd^?1ay)Kyu2TQml#QfB_6hlsaB2d3lZ%j7wtmpNpWMHYdcY zpn-^@JRVr6%a+DTdBKDOw5THzjjH9!3`sD4cKqmneww+O$MS9!hJV1v}ar`FJ z)LpvAJ^^%7zVHHw%K*w7St0j>!mSP*%?*!_l>4Jfi(ljl^LMzZ&N^9&Pi5rSl1z}V zEN15h$rypRfw8tnFMx3OEli(9ZmWeEF~0> zC@uNIRBx6?X5IkYoHi;$a2mecs2JFkZ76D+ajNHS2dw~1+#+zYYQ0 zfwFu3a}Sn1Xe|SB3;->&MYeQJx>n&r8`i7Bj0pAuAhLAvRIv#Z4t5Y*CulR?i{ zZD;rbbM?A~`0XK;aQn#DiuS`r$K|9xfT{z8pzVU%!}WC?A14Krc1Wpf*zGldV{o!v zYzV6YfA6nF23M>-2qiUQsJwc=xsUoWCP_#!Uh>=9i+vzD4)yBsycPdv!M&R|v0_FjKFu0*aCdh>j=H!JdSp zuH_dDyfmqDnjD|3oH>Zb%8%rGqnV6ASdywcj7MV@cxYE!vQAybm}{t~D)Rtl6#<~3 zg5zcm(CA)8vl=j?)u$@$Y##Z6YID<);^ho573;@5BcYvB!t+IE`brEOVY1K%Xob|2 z@p%+-S)tVzGhl*}!EtyQK6#`TW`64~^KOh#_I{u(b*SI;L}q?LDRX=7D5IO7D3!x8 z(ZWw)b#7sMr%m@VG%togM=PFCy#!nJew~PKiXfMcZ!Dy2&;Vp?*F(kv7|eu#RDq@R z7_)=Zo{411=NC1jys){AA*=wWrA2RxECs?v>_dZs&oEl!>VN*j|M=zC@V zzx?Ot`dohbkH5YA%YXWP{2!k4pZ@N*>yP8V{ytu={+l-c_>blM?cH_$4Ap=CheEmE zV!Q4l^v0rQYYXSaCiOea?00d;(IOTMZGLu9|vizH=IReUZ=qZklXf!n#z_3 z=S9i2Rv*|B3BU`?Y*Ulhl;WrCq&94jQ1T8MC{e~(K&6$w_qEj1v9E| ztvxSVn$`}MTGv`C$LY^VWIVIfv)oiB#(-uK{jNDQ(uf!vqx`Tp^Q9t9AK0gMQn=Kfg?t8EW2vY z=jC$R<8?Y#Yz$EZHJtY)3Gzs2EMC?|)O|e4Q8D0#hED}i6%ApuD21HQhAKE@Z-Z1_ zGxtjC6$9t6B*xIoh~&G@YZYQVOg$pIib9My0x9k8t4(j-2F}vZYuHWsfFPEuR_Pk> zmuVmfeDF!T1?(Frpf~fTF!I|Fl@r4XGr4#A&iCm`|H7GPgwGa*Pta7BxXt_Aq!1uF2ssT(Jh%~w|#;U9%IWugN ztMZXnbn!7%cq>XzLlID2MAUXOiN&$Y-KvTl*~gtG7w?2o|6EwGFCY=D$V1M*aEaqH z6ti7pT+m%EGd?WcJO}HxTO&Op214dlIA$IW+o!4~R`NPTnVyR(s7Zf1WoWFTTozWo zf3epDTWf=qynJfSMk`25+`GD_3@Nc@GCxsHkdaR*2G^c59)6%I7Db$yG;Y0YY+D1v zmCu1it+$-_mI+Jf7n(qW7d`C4%uJwC&tucC(iZF{nooc-;jPuj!_=^=$CbJEPm3SV zcwZ|cfZfkZpj82Tr4RP}rs2rVNn>?Ob%!%Mv5jw1HlEcSyh%)q`=GUYq!hp%ZY~w@ z;Cb-_V2AILIPH|O*73kT;a84&aXoxuH(}xRS&NTTLLxSMv+W+w>d~;h{Pxnf!1Bhd z$nw;c7^RV#wTE$`rwo59bYDCZ3P{#g%PaXo_1l8wr7p9}9N3oj2l13%2tZ~nr1XIl zy#U=nG`R_1L2epBG%>dT6u!UX7A+Q*X!)VwscLC9P+@4Rvz;hst;H&kLgCCaU#tdd zn%?|bMyHz5mp9QuhNK+q0*s#{3qmXV{PJ8+NNcSurptC_TD9)2j5L&`YtvdP|B3Jr z8C!h;ClKrHX0&28T?SUH57y=q<{BX~xFr4eh4%~RiAjjQ8Q4R@)C8_Huu^Xo>4WKO z-VpIN#$F#fcR4vc*RII{uUyURBLkdF!*j#EC5Sfr0;|V?HjC0u0xef>DbszkJSo}} z$p8XIaxHsaz`P<0vKVd<#-tb^(*ZD8&xi`RlGrxV;tASAxsR}JZi^-?nl;B4y8x?e-(?uSM)gw?h~W5eO5)X+V4umz#;ympJNm z5@$HyqUN3Zgr65#xdP~_qLk&18Eb{UO(+-?_c$<-1 z1x+#0aye0qYzYRmDO=UL3i{ckSmu`Jy`3SBMkX0n^EZE2c!TD>jz%#H*K!OBP*@nL za2S-n>YRv8RjMw;`aCo6Ymqw>^d^s#wKi2wU0xT;x6KnFL5^t~FXf~#D0IFqy57)Y zI`2!e+?c*t0#u?VybDWidr>K6juRpsX4wtwP1lEW*g;qmsWO!2;Ult@N?WSiz>R94 zg^iHBe?^=lVSsZI%__eoiOTc~-rqWO!~{wU0Yq`R0*#$jFxiiA9@#F*gj7xlyAb## zRd)9AaN5cxP(j~VS=NfbY#X;jV|In_i(G3S0dvMGAgitGmCtgj!*qdaf}MoAfuZo# z_%>kZhVP9hXE9`JP{%_DGKY5hC}9scfWb#wS%ENkqkX{{n*b5vUB69v$4~K&(0iyt)}B z`N?&p9Qh6=MZilv`p=V%!a~@LV7#m#*T_9%(%Xld@}M!;;FoVvfDChXutKwjKz_(F zE=(fUv$u%hoxZ74Pr-{Wtxwj#3JbcB3s_c$gRsK8Zf`8^*9MKahpGL!II2_DYKAEE z07e_TOO*DkWd6FH9d>IS3FCVg!4|`|rHrl9=5B5n(dJGB-L^Y=HYW78$*L>UID?H) z&N636Wwirz^dkz!+3poN6uHW!2PZvH(6ih)m|WZPn@8JOj$w<^1&(g42#Phsq15l- zGKtz;J2D(tOHsxVo(phlcF0dky?(${;pR^-uW*;+F9GDJ7MMzHp4N3`-neGrS{XhC zTfMQ?#K5!P^|frQ`2{4J>Q^)>L4;ES+jl4K%6Bp;O%=r&w-)Wb!_4T~?X{ZZx5H=t zK#AO<2Wam&MNCN*7)9*`Xl&bMN}6yZ0X^QDw>2c( zjU;f`e>jGUDa&LFeHEj@PS8yXg2^dl5G)$ZgjJ^gX z1rU}wH^eU~lf*RL})@dqj8~9t# z*(dQ?YHa<URK&vfhZn4Z&ZzXPvs=+%x{_p7vkegPYL!!#xs^{2%wQzq_Y{e|Jy(hkIDPfb5&NF8wgron53cG-Kx3p1B4EZr zceZX8?ceGKNEw5p-`IOoR+WL8B@856HEp%*9CUIjm>=O1urQu$t+7OPn&!~{n-vwd`X^b zg4Vgz)=k|Si|W`!cg1+wE}nZ{yWXMzm@)$7=eMK1ar*)a4rWksQ)T(W1AfV`a@7>t zu052d$<1n{7s{HdINprOus!Y^hisx7ueoGIsI+l-tL4C}>fw3rVA!-i1@SCgIQmqp zP(c0Rp8xju|GsPcD7=C-DLAwN4<@p>M@n z=o8-arZrFwZS*hPi8o5gCi|6#HiE*9Tk#GXM*cjj6x{p|HcctvVz!mcNQ#*jq>2Q! z#eF4ak{Y`c#(}Q37IsjW+cNV&JIsz%Gj>u6fJgghHOnyG7wRIE%Z2oSL2CtEMjJBM zukw>v>B5Or5ZjYV@YZ|pVfd8*(2Zd^a(r=Mc;4u)0J+@6wA+1{(Q?5Q`?#&B@%=LI zAuHEINwhx@C|&0 zLa1K*RkJ|7`AGYMfkd08m@ca5`!uh2RmH4@n%%& zTj)=mx}Ke<07ldoG;tSlRU}ju5iJkBv!K!V9+s*P(B3GK1GmuHxUw36iKecTgj@xH zCU3Qy?7;L#b-Cd9cmtz=(MYityvkem32VJM!*1Zb5d+TQ3%^{jkN7|Jd|-n7Q_shb zfyX{BME}(D3C}-zJ{C(2_tbrSzA7Q-r8~1rzi0^=g~~Zy$st}^s;Kv=Y8il(ab`EK z%}jAA^H!7U$-l% zB?BTULr|!?E{Xd^$qIp(9fr!QN~q^ZTd;baCq+iQV zMk!!5pCID0{0BcC1C@S2ooWBzrwz+;iX^XVipl##O`DX)<|I${;TFCSMVfEVz1GB! zH2XD&U!6u0`gO#B7U7o*Hs6qTRibCsn3~?bhlpYLreUuyqlZ690D9gk;s+yEydhF;{N*c} z4qkGI?_Ki-y}b_Z&kM{W9GY`0lv?;l+ou@^ST1%u?Po`$CEN0g@}SmGAbSk#P#4r? z?IIrTVmGZVZ2n9IKGVQDR}Qu$B?XWR<=wEAnO2X+x>tTtDJ)gAO7RpimO7b@f+_6q zjZW0}#)e6bLvgp^H+g@?H}aLy2zl72r~o7EEIp_hxO;hTZl$0fUt}_QYAisQCQPg^=2ccUQ2F3F2&mOv&z3FdkBXl~kT_CPM#g`s( zWd(N7IqxI#GIN}#2uiGyvPCI~GWGC&iOe&U>RqIL#3Z>zrr1==^3opj52!DXan|P?H!;+n4-; zxCv~)3hLIzg}e;eVZxUU9UTa4)!p;L6hnr?1^LGk3_mtfG-@O>W-f8>Wh zP-ETTn>3Ln>Va@y)2%62BxH+gA#A@br#)+@{D3HVC(hR)f}PzKgqOS+S|j8#o?4Z83k+ zU52@dQZ@}#HaHkdoW=z7%bZ#EATAuDv8%*b_-l}t;o))8_%S*9a}KFw49j3YY`00i z`w&M^FC(S}Bm-=?1=nPBu%Vp!wBCyKQn$krSXcp@H0$P!!+N(Y(Z?p83VQ4R1^@v6 z|Ag4d4uouW9^gMx64_ZqxdyKiv0$DodBF_DSj@0vEarK>#9WeB=I?Gxb`~8Zgs252 z;C$zO4r#$$br6|WNmZEsYYOJE3xz;$dNYPc6N`el-LxMki*>{c{FY0vHMC`|pv<{! zZtxJcLqwHVAz{EuK44v(Kc4qmR5jdhsiggR9FO%^d0Vgnh&1F7yR z>+?RDk2k9Kp33cYkZfQOjBJSP#Vq=xYPu6d)l(Lyn`Fp(9ah&e!6;HPs1@;fw{O8M z8+`X|PDEI;3_4ncq_4)pp!|tD>av1_N!0E<%=(V925`>s(F;gB31f=wv9_40@$W$l z&FeUn_`R-X5z9>^nxFF1=D0+T;*hf+3973bZQa zn!UqSCkmTXSvUIH+T4}x7pM+MXhZLsqWO!FWW}-w1+p=cU`9b=2767UcTp1E?-)Td z;jQE=QhtELi=DPHdXxso1rNt=NfOut`-TSA#aAD$=vaO5U0IpPV&c!JK{jGf8o+=8L;O2=yCg4l(gQsgG@h{u>e zF_qt{Xsgbk0!hKfT*Jn0wQyc&=XM*C5CkzE^81O4F8ZPf|XT@$DK;o1N^N-`U}`Z)WkS(SCBd^C85_$tw0JoMmPm%U1*R;In~f{9ts==y-IeWGQOf1f25OG=W(TLVgW;y- z9|8yQweYK8-Yn%s&~D4-Z;DI)u_0@X@Yr>M57iC)1+w}%#E_D1@x3AD#k?FozU0^%% zqM8F8+BW8nGkYM__=`|HTsMCZ_bVM}o>HkP`C?8C2>WcRIH;+VEKia=fU3k9aB>2~ zv;PLI9j__vru6|``m`GH&vgP9`b=W^DGx=(n^U#Q1I|(gsQso4V+YLa%_w|G&ZrrV zq3`&Xx%c5~C``H(nbmp_8-x>@Zyf zcx1sLe{Pb^o<7!=fARn`z7n)|8W7k0uO$j6Q@V1&HH`jpIKWaOD5D%D*{%eHl2M~`_M{q%K2zlZGpKm>3K927tIF1y$LYen>tTQaDh*R1fZ{@6&r$&p z*#mqeOB4KYnF6lB=Fuc?N>sHw>~{D;b3f5BQwxkgM-W+XAwW-ch$0T{+JUq?E(COI z{ZeN2Ncl2MlUvV0@|DjuQ&d{$=BMZ;+nTTyt_VmaN^nJY;Mtn}sZoo%Z!ncsDNLp4 zvzm7r`*L)gM`jp8h{@*{yfq%-BBhkdORrcEIcpue)@x|3vSRoXymjV*x1eb>LZr_| z8{o9~faoF>wt;QEK%kfexYq0~1L$=yGz~Y8;=X3&!%gg}51taM5ghSP|L2dtZ2$A! zq5tv6AOHP5{=>Wf*T4S!$AA0V`G0x$fBnb59{=3_*Wb?9yC2i$pZ~dB#@^YMdieeC ze=0tYFSXl&3N~ge+tfEW_f=uexmUIFapLDF*rt9|Cmx+PwZFj z#*WH!TrTiTE$o9?*hpayG;!@OU<41xmQxQYE!Bs) zzC8}!CBDGfhynOYJG*n-Z_Q3+zVg%1`t+?0Z;uQ{A(Dp*=tcTkE@VHNqfFDBtZ&Q^ zW^jVM#dz%QMAv`TVemRn@Hq|HEqb^{7KIIVE;0#*j zmE@DAAQwZ$o3xI{S~3xq{MO8FvJ%TfsS#kQ-+4BoxL??1Cje*JknrH!Q_Bgan+Hi+ zp#Mo(s7BXwg-vV403YL>(M8?e(lHANJkMhsZb2VQUF)z;F~iOzF5mX%DB@hclhCF$ zd8`&hGigwr+-c?_baOWZ5B5_%))iRWrcF%E9bq{k2>5m+Ggd;B9#;5`-cvKr{+Vec z^r&Z!O#?+wj-*`|I?ALVhLIHpgt-0+K$D_XF zXY{^ndw$)9y>3%JJ@jj}-p|?-6pPO~?pt-#AMr$MN#i>l;P@FnGY~yvxq15Sl?f1& zy7>R}mOTV7~r7^rWK|W%c9Jofr1>JlFHE-!6+ExololhGvFchC ziw-iODm@oN?HD06O}x9OLwFS?CGTBw28d2VK9_i)*iC&|Mrr^(QOxTsu7-1L0!Z$sR)RgcI!o|srn zKmWmP>``Lg>o%db*uvui^w9dpUJ3M4ww>-#QKTb24&E^*Paim}Td|xF5W?x-mT7>1 z74%+Tmr14{x+9ClHB}g=z(xk}i2hL&pcoyz7(c6Uzwl~C&3R2zN5f1G;T`fEYY#UF zn%c+s%g2hh=bOcT?gPUk#vN{{YBFsWj^79(Wp==Ca=}m>I-n~q%SBa%E*MbX1xt^A zMDr9p02gvETGUE}c*PU~Omp}Z8o>EMM~klxl(@`~?42?AJo9r|7KEP|o4W1C02)2_ z>9Jss{)iAnoZb+SbrH?`OObP|(-8%Yo~a@J_9rFHBza`G05baF^h7p{6B?%JGkDLRuSbnX8H|d`>%hSx zgRUmB{>^E-lvAM|9QBUZvJqx1%DR{!KN~c9R?`qCiJXx*pn&V8zJNEfKj^5ENZlac zCV$8#gR5r@Bm~o?+^O!E)TdNHfHujZK+pzGYSxSSv#aJj!4DyH)9bhC*YM7U7A+Cs zH8$&x_e_fn_D{(}ceVmN@GXsu1|a4+bYu`)m^TUcAhlBfT%AcIae`l%TswI((JwSp z{pA*YgO(8{Oh~Y6O;dmt5SCGn%kOz3BthpP5hwCuv?Vdcqn(_BCr_;k4vW8qkTPG|s}dzv6e%{6 zlIKyq;17Th1+ z>iS44xW1egLJZRkQsUN^3&0PLvF~VfQQt)DiaGC~RXl=MX2~HAx1SW~)H$^E#X?%E zA9Vz)g>k{z=REg_SEK(lOY(`&25fc?|;#Q4yj zPfZwFfWhwY`}CWCMqVdZ@KOoE9nzzV0K*<03$_#jh3J}3YOX+ej44J+k3LfgW2*%y zrV;QSY6a0d?7+$U1p1f%=*B<(@$b9w|Ng^&cH^Jt zf6t8))GN|m2ZSokTPc}C6vB(PlkRRPqt1Mw zug)nBk?pd;fphhkPGuOU4#j- z4ctOC(BSUb$y_G8wH|5X+hqvM@zfjt5f|oN*!3&*q*IDX6=8KfC8YP5NQp-a}0Qv>X+7%<*q|8 z!8hxLQ2P>LTV}?my8*`9ys;%Owu6_Dg1*lXkN}E0oK18_o0-V zHal@2H5c()rX&-d5#amN%u1@K%zD-pkp-~L*gbBLt0<>VnGMOs)>hh8e8vM2=#-a? zHw=D(&=rZ$rG{E9?*KTGvf(nX40zJ{jWqjBRq`$Dp~GErXX5kb>C8SQlRjxhQ9g| z@|{1XMQzuME6ap0_)cKCzj>C5C}rmgZ7Ha9wY@yfb(+=Skh zPFesKrP@52Ti-njYg80&UuB4?VOb7u^lEOQfmb5nR|}n|G3Bm-3Nv^GZ#X+RLQa1a zmPKBB5L&n<9F8329Jt$|02zxls4JIT_uKVQ--_uUNq(l3)=VfJ7$IT4PqQjK zn5yhA-v|v85KB|xX}B4jO{cTM)rpv_&A#+(iDvg?6AnFRXd8bG3}cnwC7BBRcJlCXyTh%_ zxp$+e_68j~XWGtqzt&*FY#(s6KqSqVdB8yGv`61~1#i4B@VaOmXs&mRgjj zve+{>Z@Gc4(bAwZiC;VXCS*S{BN~nQ4Co=U0DBQZ0T{I8q-uz{>7m@V1?c$3H*WKp<$J2ifCW183TTjgJ||vTkPs zq(6t4hC-bdFVs@*)gsnA?(jIp%l$M!?TOW>cS#O!NXB0mTw=4rmYx70>mv6zs7a2q z?XnKb$r;BrHJ&(v5TiifM{CgM`;6C}`+|J}KdDb)>hdnYcE6s%*9PM!KzOUcJ<$vH z6q`%Gz2F;m)D>^aT@`RI1dKv3UB^ofOft&~%IixqB6*Di7SFQld_e1h8tb88jYC&_ zru|~NorGBX;Uh__MRDxNlaHeU!@${l_yGGx4Ip0F6d1c(O}TQE1c3gh@IwZ$-f6@=k2nzVvTTKH4{FF3vvxL+K&ZXEgKY%rvaCr7Y#jkkvauh=-SazvOVgi8G>RKU(yg zKDa47eYm3r>2KjEK`6zZT=@jz>rK_5wV@8b#txIB0&wcV@jWIFNjK^e9_R@j;uhp5 z7e+HQ!{WYSSs4XWdh^r50kB6)u+qB~o=dRLUkc9AGWotdU)4#E2kr7U(1y2MP0LvH z{9WcAg~NRLYW}A)VUkgSynhtmPBIp7@B$cS@JnVc?e>UJ^M2v+HSy6;dw;;#J zIueHoZ=9{o2sz;n`POqFqvF65tqvO7cy4~9RLlBJ(eJXlQvFTP=$i~b=}6@>To2^? zNi-s_1msY&hs#Fl(@=&yLy@M~7E(zv2gSp>_7X%QF}VM%Iew^+BOFYS5EAH-Jcn=p zD#FoYQw1$c2=5^zAd&J*oCgl&!{(k!6X<-4YFp4Y@Fim?MNQZ8@X! zv&14RNURnNSC8iGc}al7=r9uUVDQslF5~=x9;Fmb1>sw<3Ji#h2H&Ds z-p*N4u9#fOz4$6v7sn15{VoDL z`acs))=ZfT1J+0I3?aH56);?O@b}T_h6Xu+ZD^zBif!5owbkB{YmAF^CT*(!TyjB| zwMTq*PpRK{$jbq!$h^3|60My_ac*_-aA6Dpu}wqE+AI?|q@w9Bpn}|1Qnqwk%%8rX zXZGrL2sD)tGkXH(NCNh{;po2ToISkwZvX%Q|NpF*$qK~WR)v2-MHIwmaHM3AzPq4M zsq}rNPiE=+zR%N}>%9{PI`Y8baG(aM?7i3eRxVL&f51n*Vb4Btj1$*6uaOuQK-Sqx z185Xir3_RtN=iJ)q{iGRcF!ITPA!Eh(~;pW%c2DS7SF~I;$nm6CgR)VIOu?(u$`_6 zW@%0oHcq(JrY*>6UN+bdoj$$ZN?R-yu+5o6_z*p=%iyG7^l!#MnQogK%ao?ivV^Vx z7d3ak&^@yypLBP+Ya%Z{%029BM8Ns=P*oM4Fuf5>u>+q@${Jl6Nj0M_O-R_UOCgzL z(8ZH0qmI=f5!+xz1t?ZjH_|ROEo3ly$_0+*n+nc>$euc^!6&~n&qeY1(EZgu{0Co$ zb%yBS+ScI!ojVig-v|DEML7Nmw=A2c^?cwt8b>;ZHpHx{A4j9ioUW=O9HP2n^(JF3={AG;T`3|pujMzu z4X$nBQi4PWEb;sJJtBjq1oVXa)CwJGdczs`7Qt7>+%z>zzNK*KKC5*)K6%GRPKz;-rgraAT2rt!Cce1;#$#9;C)~!@4Ed81?z$- zftCegi}NfxpN2pE0@^P4j=%}P#5(wyyLkuA+EXIHR>wIHN?4Qx;HRvKl7hf|!dxHG zh5^#g1(yI}rhdZGO)pXu^GH?V>a~fgl+t)e4q)Jsv5@Z5E^TjzuZ}m;TP6QcUeF#pb0IV=Y9U;U;q7&KmPfTKmM5h>5o7D{yqQU-GBSXU)JCK_5L?heN|9h z+Y&AA8r&hcdtl@4Y#=~z3GVLh7Tjgy!QI`Rz(#|+ySu(R=idA3y`MF@dw#8|Q9Z}# zTDAJ}V@~*e|LyiAt!HDggDmMl$)6#$uBD}jblm@SQP)o(e;p0-hAm;;?d(Kao-{T(dWs|yE71{^PJ|-%lEv%aIf-vuv zbcnZ4V3Y_50CC$gUE{kXx{4=RFFKM>PpUqL4%^Pgfa=&cBHu;&@M{GNG>6Z8VXVeCqHgu1Ta@cV!Y2QS6fS z@q(6uNg}>EHm~m8f@H+iTH^atm&52pKd~jhGM*TdR+|$(2DZH#fEa#N*_pK~Z~ju! zZGksNPRpDVqTKcFOww+JbT>f3=cZj5bqva%Ce;Z5RJt&`H62#Kq56gYMlE6u*ySX| zI+BLWD{)SXsk@LkECxJz1s@{D%x^ce(3%vWtvNSX?AbiWFr9IH_r1Pd0iT^R%P|U$EbFTmZj}y0?Nmw z9m&q3J)R1;O3RAQO_(NBxZ67pcZDNPi71O6ocS#bWc$fTb?ZhgrOdj00rW@DE!FGf zZ`>q7%uNW7+<-Mp&Zw3M4&9oKzQ!wesLS@`m`ktylp#c5Yoi!hZKTvPwg~NT!2~u$ z(ns(*D=14H%&1ExFR(>yUZ#;9D2=|nW8n+x{B}gMhV@=Esu$RIl3PZ=<07f#;*x4! z=Vy1ggWEb6^C|UEenJc^fl7uB{#Am>%{dZq$KRjxGsc7sBE#*FGs?oxv-PZwEJsp@ zgB)+{L3>G0NhOi&l(1by_<9A&E3tI4Sdl6C7Bc-M(LCc+h9m`1LRYiCJt{r}I`GFa z^>-p&q-7@xi%zpEWu8iZZ4qVP2|GSR`}gLy-0^1$<%Q0J$Nbz-n3{)Z&!5nLuxGbI zo9!d+Ggn#ZVvWF;?OSxxl#X#KXAEu2mtRvMh=s8>IWmth zTm!$g#U6E`fF~RuV%eYbK^Kf5y2Af8ELs<@#QJGO+JN^`xlzjX@_4$S!K9OB017MZ@0wASheH< z5TokqToukDuR0Q~5zDdLzknC|Obp{0R|P}7q^>BjE?5W2r64QV=%*bO=}QQrs-$}B z@gw`eaFat_69fHO*D(zj)mJ$xKi8t6Ye2~<8;aTA% zXti)3cLN;!rfLWx7MT(ZjQ*=F03nj&@qb`5aXi~^DB{raA!?HTG?mCp* z7J`3=gLdYSfBRHEf#@WC1iW7O2UFFNl{|$eryrMuYz|hMd(##FRNcS^;nBOnoS>HH z<%d`mkL&gj^MS(t}X9X%NJXO7MAi+(>hkt>bi1|5r1)b`P!QJ9i z*a?)Sk+JHTtA^BD3aOQ%>yGUjg(5p}rFb@+RkWPZ{qb!y9)Aa)>m(>8`%hv>nGizw zsYnU~8cbjiHi}!|&Gkust*8buUz}#170?NI>NtJGrzj=kqsp6Uqlxs`FArVt70*>S z=cK2>wyI5!;ezCy-Isj_Bx_22~0#>|(EdlZFREL=Y)R9+7{Cd<0Jiyk%&8*V{Wy7wC1C67q4b zVf(gTC8|f0_m0 z7q2%6lld4}Bu>)lQj`^UDeS`uZibqmA4KJurH4T{r@u{G;<>>Ui0bBz1**dpN*Yi+4^EuCH|jXQ&l*-AZVV;b(| zgnrNNCHMPZS93Rs;i(pMUcg0nN)41Yw%&E%+4`p}gbVU#XVfh?=}PCmNR zmo2yN&-s|U_{r}s-ZADZgnQo>rx_~eiqlP0$#!O$CtL-+&25WJk|`` z?G+a>r4~{UHG${A+6p%pUNr@SGNc-)JT+)G_v(4Mr=ii!=qn;5#6bq=6$!6?jLSms zR{18Bm{;VwzKUu>HqfkZe$7iNif5OMIyi4UfC7-II;8(PZ3-UvKBy>w8~KgUVzU8 zB+Mw|o`rx~46YoW5kd1szLg0EqM&;-5zOpNroZ9-d8v5xmHv+Sgk@h~Rl|s9vK_ur z{RKmO0oGFfk|o{a;`xQ>odNY}wpuOGOOF4VR|kNO`Nt`vja;t{Ii^lWhgIwc;relHeV;<>x8Ek z8Gdup9VjYQ6504cL>#siLc?=fw>zCGxdzOONZZpZ*y88vzfbT&PL63-yHT@ji?a^9 z8EQ8}+ecLnzmhc!%yUr`; zF)3rp;bgvp3HOXR6ech+==bRK?u2-KQn}6=#-F+h3?5)c1vo44QO>tV7 zI)a}iEy zf0NYxYd=Kw`+jekcr~wcWS{wBV>o=NRwdAr)MTV zl+4aiQ7hg&tcnU6X;qU?#rk>trQ;Zh*2iTy4ry?duDAyK)*X*Ygjg~4`%~bYrJ591rF~evFCb- z5R1))FKH6a8{UG;y}0}hx7n>&nI5D6``QMi!|e?kir)cOh=T@R$nT=BQbz5rJZC$F=MeuBU2WJLNN}^#f`^n(ok`32y6&VfoCFY0xdsc(JDFd@?OHDW1-Kt^3Zp);ILDzJoEk@d8@ zR@ZFp2d;&6bj52RzNO92otW`w(;&{YusqV4c}i20kmZ0&Ky&TD8FU{PjiAGE#womP znf3nlk5!%k?ci5Jx^JqzfjdR9;W-ue|j20fig)oWYQlWJpdsv<*PA$=VO<#MtM}4LnXY5 zcTOi}NFR?LNI~m9j8}k$!e{q0kRYx-d?!ho2_+egZA*lapnKRO&lp$$viSt2eetT* zWAW2~`CthAwJs_Gl*?u4u~; zNCw8@<8!Z(_-v$!6(dZ;96cAl>Ly{s^wOW&eRx8eYA%4kaJ4>c={rvp>Udy zK>-TXzmQss4nDk88$`m$w@MK;Le|kNuH$5c?FGVKE3`9rB%*wUr@e^CP&_C2pfI?+ zl&}_=5@68Mkt8s*51m<~y~lmb#Wn}mls4SobJyPEK6%_f4~0LJEQ8yGbB&>j`9-%J#nix%w&C~b8$23sZ2#jHddxB-7wVv~BOpf%%2j?CILgE3Phy zI${T|EDBMwmm}+AFt6mS1(srxU(r0osA4b3$RhRgqg@db_;5B7l$8dTNS^kgN*4NV zb-o!7Tl&V3?%?JONVGz@Z$mVxG0iEgE2IYp-$S`#w#aXhs3LwX1F}OzSY=hz3P&XW zMI!E^PtN3TBip|eGN1YOX=sJyVm79_Mg;Hm=pT?j5J_xOoY&pVsG! zw;ay!viXUc$I&h4#OHq4wnK5f%?Z1wGL2p7*BgO-dZJDib?{`RX2kfI_*!jyhAQ%# zqI!fl_+@Kv`SKjMyFH?#q_6*YVQE20$PgdJxwj{e1ZWBdxZXL{(LB$ z-}oGg{3DV2;Km}TKYTUHP$hr|d&H2dtGa2=Pi*G-8u@6+^5O;5lWRIB^fgUHx^9tb zJ`_a6q5Q)yaCPCkx5%Weh$N8juQeU8Ai6qFQr7%w{?(+{u}@wm_Z}*LL;)h(Dn)d= zN?J>TS!PfpiW9>yQVII64JACxR1SZe6z|i(#Uj)j-hXzVX^vIj$!IfEiBw{|BXvP_ zB1}PlL7oCmW#n}AH_Tb`XXl*58_g&UCQA1`)@<7=Yt;P=98YxJPriucJ>=iSsJpRY zkF`wg$*HLvHS|(>|{?#pNvF&B+`50XG(fzyDTBvKn+t zc|cX`J16#);{Xi<#Nr&W=r31c(mahAQ6_ zSB?+x!w|rK*@F49+|mP>tYU_%m7Hcr{1-)e<}Ko`w5iV7WH=>*bJ4tsZ)(A8JW#mSl9~8 zHfFSeZ}arA19y8@&}OH-;14d&)*RS1`C zHUYcruFZRp(IQBCo3W>q>qnAjcVT#zSvm#U$=EjYkwF~*iA!I*ppAKiG|@oOGZMYE zXabKyhS7z3yJ*q+!MPOcMjiWK2>4&Vo(y)hTEtgoCE~advHxh|Q$@??W>vTnCKl~Z zCNb>R2ATjw?Xmf%(lQPdsC-7LzPkT}5IWoYN^L`os+5#hOPSupk)a|8)#9W^C_3l( zG1|ITl0|~_HozjB8mU#im#$f#_=IMf*xzDyvkdXmkjNw?s5`LPKiW`4i;-4>Fle7a z&j33nK6N>#5@kIJRIV{=Lh+-uY3UTZDJ`^Q+LMRz7<(p5Z)Fu8*(mBK0k=6Cr=h?w zySGMkDCC1GaoRyqGLLpPH&k`C_x$AoU;7hPWlk4yl)D&AE!ZV^Ue2y<+$fbFA!dRB;})Q|8_i zB%@>F!j|`iP$*3nlSD-1PwN2Js&>h|L>8>>tv%)b*U5z`+c`wKg!cJAZ+n`)U*{}Z ziMZJ@Z4m(?jFdf!fPu(HQQ4Ny>C?SpP>7?19eEe~y6Dsh3I(yb?vYO^S5N`O|>F;Knt}&nnn%-P}M}qXkGmPUpOd`1yr$VE}dt_gW7Us19 z27WlPo5H`N^glYsW);N z5G{F{_(18Z;B+(KX>H^T;ZG$!-HccaL(AYqIR3N5M2<=$R7!-PmY6p~260RJNYBS9 z+2?UM*~dpXncq{~>HF&Gd&lYfDVd+o$3e+uPy2@d=kev@ExNF;lJp}m%l|pe$;TZm zAoO}d_NiIwjpeS3GiH@inj#>hNA*gwxPcP4enIr7GbaaQGqhvoR+m(6iYb1&fYL$h zCMw)TDYtdTMC4EE0yYwgT{NTZahhZAp^v}N`-!SNYt3FuJ8k98efPaNx6@Yj*#d)n zqe{cJ9}m@niQ2L2Yv^Xr`*zFo`z_fAe^1S2&&1`OH+mBe`!9Dh=7|WtLMv}f9)ms2 z7;>1u-~r+~x91rF!%$&7>s`CS;L{1>Nw!oDaO{zBN|u^z>B-s9kJm}($zUVrh?F6X zIuZAjgD;Py?G|!0Flr(5>HH<<78by2nEF(BSpDd!!Hap`v&kwsyO97$KBME%IOJJU zH|p4L(!RE62!mJM$EZhEhR#JI)V?dNw(P4RDzh0Ax>a20u<-@`;aZ`E)7#M=I&XwS zG6IoCP6Ax4Hs-?%$X>npG#(hHnn>RIngmj;544B*t|hjyx>dMG zIGFeymCE)`q)<@k>=cXzc;1-OtGrST>^aM{hXWlYI3|{=FP<%evR`Q+S3=T}hbY^#5yShE z)9V28Bw{w9)y&bOnEH{W-4`&U8gd2S714q)w#+PJc8WSzBwKCjw|aS>C%{zs`Ee;d z_QN9IS_;(GN=qCQb%C>}kg63xEk*UBG+aTG$; z*phA>?Ev}^gY%t}9|04Yz?Kb@hzIZseR}x$<2c(43e59bS0EmpU0*B22r)F zdwWs*=p-Ta#OZD@&ND@jvKU+m=AyZHXyv@B)r3`&)n(AfZC?dy{DiY&o;)Se7^#DCw>?YJZq|kA0@(?7)F?+!qEXdhrv_txb0JM~5${^UQ zgC!+cQ5nK{OKVww+zO8tm+f-n?ffp^YK=ee>`tylk}_I*D-qQ@BJ3 zeZ(-2@KU`PAio^Mya!5BIHbR0SaFC_M9sJ@X+EoIesl|gRH6qg5L(6vO0jIF)pO!* zUvWjt4f&D2m;J&?IMOYFLF$4k%tKB#606iuGhDwlMJ0cA5o93GBRH?84H{Y&dtF!9 zKOho{bOO6tj+Y|Iy?%sx@i#Lu$FC2oNR{4?Euy5TX{4TxUco}Fp_XJ$x97v-_#ccl z3rPS)r>rl2Mm*02Co=}z=dCcsJpT2eK4eaNaq&%jj`EfG%{=2}^PV8oM+lCCoja^B zWDkwO{g|0g>pjmdR-NYvO6c|S=b8UY8D4LaeuDK|kRf3bw0}ClAhNKoqLu?c`!cWm zboM?JXvzjRsXmbG(F{Y|AVbBff|Ng+{3ROiJt8C6gZ8jAOd?7)B9u!Ib?DMrdx4lw z$yRWuz=2XI-qu$LE(dQ{jB07#W?x9R9#?#V5tAlR6Bu!fnMY;MjSdz5fo%@Y$4m z5?@pJ=SlRSs}Uo~G@eTZv=sr2aGDCf4y>WUE^O!m!{5Z7&pfl z71O?FREUgs;|qgh&4p6E)RfxF5sK`&GYsR~dK~k?h;d@aJW>rwDM#XTn!E_b31*SS z+Z+Xmcl`{^cR&&mPMl8(A)?YHlI$G~mF8R>XHju+?xF;i^?nHNq%StVmQwq7Uxm>Fq27Zl6z@OgXL8;b@lENZxJ6iSPH!-MTl59ewDpQ=qoi zFX`a-m$-1pqI#<IQBT z1%OOXGtc%cwtNkwxw@>`_6?1}R~1A5g=xm=mdDd;5~V7dE?#m%d!x^wSZ=Wrd$&%p z3BS;%q-@um=bqR{0OEu_4$S8Tq`vv-Ds?b}{sSe_?QG~J%0n13^s$~pLQVa(5}*1Z z4~R_AxNol>>*q$0TZil7%Ph|Tg3Y_>M&EQXZ4z=COQK`ZwwdtO&;7CU-CE((L1y8h zJ!}7u05&{KIrBS%h1W~Rgi*b=HX+~corXK6K>aTg+?8h8Vv0q^pT z6s%LD203k?djusNo$o}h!;%`Pm8bTDqDZTko`3w0Hm@2sbEjc>fSZC0d`$j5U;Byt z)(Tt%#bkoBsefOD|65hbW7MF!Zl0e2TAA>kwi2^_NeY>VlB` z&#Bh#YIdI;3dSH-u256#lHg>YB^S_B5t{9>4qjW1>n0yllWYzK6|SSggSvUozKM;4 zmFuS*RE=s-S2|-f?>^(0UBV5GVC6WytE+$2#Sj-hCD!;uiE~qQ1*vfN76|#`iyor; zX%G?E;3Nm{2el;EW3E7tVQ=j{!$4Z~O|2&i~ z6Y3rPlM`jUnf@3d5UpKrGEcqpPceC{`LziAy%f0K!hE;$MC-Yd=-RseSigzxxm^ny zx#SU!H1IHz+IsW5Reb^9lJNFyzS;P1NB#=DHGh6K_}`j(yDSlGI71DqipqiP~Ii-JysU?~P2ASH+u?2r)W|HKFC44;Q zwrE!!TKQHlXf+8nKTMSje26!F5O35>r49IX$aA_o`*wjF=DkKKF@3tOVw@TjMgj{7 z`o<9&`DI9Kb7Ha98)Ze2Iie|b^uDKL8gX>%iR?KZy;vH|^ga+M^L(F(alfB?O;J$q zSzJyZbK#Dd)H}Qml$sG}MW<+9O!|uboffpBYhP= zy(^l7sFOT3ne~J&5INPAJ0zBXuSoiPqa4LcTdPgF@)e9&nHugE;S5!=PXX%)G;9)> zmrB993C7aTHf;^NgwDd4eU$>JOWA|`Kev7O9EG}6O9&DrWfe=ov!Ncs-e37gZaiZ3 zvN_E|?)|xca~?2>pLX)|d53VqRyosWleV}tg1cQ9*807Lw~y3p?6$jdBVBOU`dx(A z+iw-^z2lsKfl~H5c_7uinN>FxF#YtAEj-;JHt)IZDwdk_c%vn`2fE@I4vTUh;6 zh?Ko`;TdQC%xbmrL&o%xEi?ww5A&`T=1+5=$A1>cFZJ1FiD1BOsaaLzlnaRit9)vW zMTQcbAa?8W`&lsR!?G0F4Gm>$b#?`m(R>R?U+Jg=>$CQzjaNkpwVoaG+6>BISi#6IZkBcjaD}y=YurzL&CpnHa-|IuD$k|YwWkEqiD~45AGiz|~N_~qq z)uH`{)@HXUpv;lJ^9{MgiX)n^`SoaY_7vQo+DCWav@3kAe=v7Lj*5CVcA1*`|4>lO zh=a-N8#)e83O}?J6h8ITo67Ri|7MWeX|-uEf7|um>A%r**ZT8RX>{Tn=yU# zZfT*W^U2DucR&moGtCzaUY-jmw#Y_vCKN#y0N1;MFS6A;10&BEqP~L01{YXs`Od+B zyNGi^w~wdb+l$EC3o_Tty822ctdq|?!QUIfn9BgdbpR4>on&LrqV>TVCj4dB9e+tF z)`<9Nayln9FmM4eTlITH@S~sE1VOyK!lUyfe_Y+r!wP2}sWxqzs!+iMeO1zaa!wRS zNh6S@WJ38Md0}in8Bh+o8Ei8G%jfYo0{cNi)6m-HqadO@=tf``fHUu_cF7coTA83? z?%;uf;5w)GzPXEiM=A~us#w<0bzoxDU5@c#KN(Z6<1x^qh}3IYIk>hg1q zK=-kc;D(06NBF75)tfiEsneTI`l&yYi|{qgkF4Xjjqh)+BzBi+r(9W*B_+3#shJg$ zbZ_p`8?_N2wMScyyAtu-H8HECQ#B>H`B^8*MWK=Wzs}J)qC;(doPpE(4rfJrp#l5s zY-agllr8r|@2SyV`XwYYCS2;q?RLVIACnnpAX;dlW=w#&`XL`14eV?AOwPBLyg-K; zKHFm|a-T+^JRf}oe$`wG5KAI#!BVaS)hKcSkOD-Ph+P;f;)I)#I=%aSixv{JF!q}h zKEJ?~Z`KnfBkb=O!|gQe4j@qdARqHT!2T~#z+Z!U8E&VE|AMZ(KdGi(&1+YM`Ekt& z3f`SJ$9{7jDu|vkxdB6+u#9<@dBbNj92*uCyFZ7XHJyb%N`QnGN&R`sAMorEJeSxe zJ%yC~xl@XrQ>Gex>L9_S`S$b3mI)gnw^wE08i^b|66S4ESI_%8;RsjGKCGq^0ROVHVJ**GbPZu28B>)%12ahk51Jh|BzeeE+l zTxuA9mO6@85NhvjxZOp6J@nI=n)1@;sV@9nJC-j&*UfYFnk9hREO3RJ&46T`;nO?r zgxna~UE>0JIQ_UrlJbqia4N?R4_|X%>SrSq*(P=jqj3vzasSxQoPQQ=j9cSTf)9dnSA~YZt0%Z%p*frTX0Nb@DK4EZ+c<$!PE3HQ{VMVj_T@83zT}~D@h$89 zj`hFVqAxa6gc!~tvr$n=G54pE?L)wMf2v)5xZ{0XE59A2<6zcz7?`lCc39s$!85G; zC29H}C(*0o_KLT(8The1TU48m8HJtMTJw}edWlMwJFYZtBgJp!@1`iSy<6hxr-k0{(UpQ3v&0%~!l)U|@6pEX?yzrdstoi22uA{3v zktTUjorV~(6{y6D>gq(^qk#Fr065kL0gkxz-1(!qOpVo6~`KlwD@{NVZ+_YNz>V`vGA=My`C{Nf|H>VWh{!A0wQDDEISm zcR*V*o=3!vKnDrZ?}+7~s}6rR>5dNqvX_zz&PC~Y;kfKrhvsyhJl7xahi#L((H&zO z%XnZMBBK?>*lc$r_=Y8n%Xbih`FNPeWeLlJ(9KBo^E)dzoE9AZIZyv!*#u9}*Q9jW zQ`op_H8J4AYF>Y9(V~}sMjxYkLQj+|SmY=tC@H?IN?0`QTUyy1u{|2;dF~xtF$o(%rCjXHf@K^s=aya#tp9fLyLj+Q6?U=B8pz?@6 z*Z-6yX&Ls*BYu$(d{B}5zNZH$UoqxqjKNs*(~e31M5#$qx2^NVwfxd%Roz3#Nv(hB z+Wu&6>wB7meyI59*HUp};pvxC zeQMRNg~#OWow+p0QQ64Ec5d_2{D4_45Kz%L>*V7q-+W@B{^TX&vQsO6K1Upt-v9cW z5KZ9<3r|K31!T$;v&*_Kq4d&Kqzc3xA78$IAo`BoaORB0DBQJnVDm&|KAH_X4^uHm z4S%>yx`bQBxdo=ECDksO%KlOdsck;-iWNX_k*#|1hR_r>(sltl2yPftWy3a5|N8+S z3_L}y1g7zG@{#1Zep@S3o)SB4L%#ha@x~%I7G36u1az;hiM93$Z#-^b;`9AT3+QyepR>~ z+i-(!iu+4{VLOE}h1kFGgX2FyNST8**u^X=KpWw$6XgWN4^GvO!P&^Yfx++plsnve zr}yr~_|V{^y;K6g;O4pBd?c$HvTM^$F*+eKy;>hC{eKj3(D|P#^@_+^f|Wjjj2<3F zpp7@GwNE!ptr=RfB=`K6A_po_rddO$;&HZcmS}5FmRge&J?mCYe(Hbi&U4~kA1947 znF}f^_W9)PFBd6Lg|eS_IWkL}*ob1G-HA~&;=y5aYCyI1M5My_7e@ z+@Me&yXf&#c+|ap87$ipE_*7y1e$(Ai?{c)+lvliwfpaXJIXgS?#(VeJN)f?IR4Re z(i%1CT}z)0fG$ zg`-Z0dt50Vuik1powfO29wjNnAj(n9`=q2-Eq>Sk1&X7{oxl>e zm3a3nzN+KCwt&uGisy*Qe?-I6f%x(^YtzxhZYME$*iYl#f0Hy)R;tw)H{@C_> zo#aGJTSbz&Rd52WMseht8#&dl{UC;vZ1YcC|5%=Q6^MT3vOJiBb>uX8<^q6iTL5GQ ze*OH*IR%vKst9STnWYcd8{uX&MgN0l0`m1GNHD)-1blzbleqR*McR4(ly`uf+aNT72o>;0NU@@qU>td z4s^{GuSQkUS+;Wz%_f)iWpv==3}2Z8hasf4IXOtjFybL<4}^G#hF{S26_C~sfC#gw zkpU|4diP?Svd!SUO`SAK>1M(+w_u_?Doi|}D^#Qz+`gMDqH>a#% z>9Oah9zNGOwiu#fORv&+>P?~5-$)R|s zl7#)Agog}?6Wh!G01jPGo~xUe4v}T9?C%Y?u*NB>@Ajw0LbNVks}Q$<{JGWPQZV9I z_l58FI$zH@t;<@f5*{*|=2vr!&p7{kr=yvaudr#gP{JAq$ki}9Seivfs9unq$siiYLA)V^A&6DIj z`K|GK3t#j3eQR!G_dY*c_`K8l(3jsnSO4)T(el7-lYWJ1mn2OGEY&pV13fhR(|_YDjS z0Z3W}?*|UP`hqMUo59WGqU<*zd+=_UJnmBd?QCzKe-Mq4b9={yEe~w#f7Mtp`D{}7SkXC++KI*2N%~{eGksW@1a>s79IV<#wGq_L=x{AgW)qqMH4{i%8pUQ zjS#V=?&Bc85iLV7V#cUAPC1C&i+0%o82b4F#U%Pi7WNPVzhHyjQL3NvNXD0D`M&uC zp=^pOv4w>4K11TBIZeiwCZY2j<=T&_`A64zW zmgiwoCs5(~@N~dh&&7#e!|49^Rk*tN>&oM+XPakJHupgJ~F^D+l+z^ebx78;_&7Zt#Z@EKigHv z+g5O7Z!Xhhf#*uS+F`z&+OoqZLIf2bBl;#ZyP?Ec=v^cHj40W8OCmNktbb4mbYC5d z6Gpat=(IFmPqQIkUdH1&X%o&bi^`*?;6+kcOD<#m4D8Eq8~AF>(EwFCxlUj> zfF4`06PN~&r+e|p{+7jPt~}}il0TTRK3rQ@vcqn0lM^xeXYybQk(lpV-Fi!eA>l0P z0Bm!Hf|=391R-Cz1d?Ym200IBw}@a&;`ALmo5R3d>Sj2YqY)Im(enyktciwk_#Y^K zn0Olman*1Eb2N~Cdlos~bk>V!Y7o*+F$(_nEb7b>WuN^Q=#`PH8s=8NPrQnjv@o+@!e?(h zpxpXRAKMD&pKyM93NMpV<-cqu9Re>eH-0Q6JY?iduWk?<@!Wo2IF zFDQVb@jkm*BtHcq==F_GZQT8DplM zUK=|y{e^H*dUJ-EtuJ^KKFw!K`YfI8sNW-ptJIylYIm0+ZwMT4^^-Y8tD$oUj7Svu zQD3b*eeHwr^fNLqS>QKaf1UVFLaO-jzqnjA3aQ<9l)I~U)tsE)UIq`Ge<-T=G;dD6 zw61Sf&D!t@3#0#HB4LTrlo*i~wFf-T0=X_83S?PNHpKWY_$+DJnTpfg%33@pO3hVQ zzu;w)u?&77bi>eFx(2>egY)-5K^6u3AQ>=01gVmW{s5Uj##5d!G8~p3*oeiJ)|7&Pld|N}^BQWvmIHx6VikjB z@7%Fe)k0_h=!A3(Pfd~qqL)okMYW(NyeJfStit@8FvtI!P=C*r#kTj3nm7M4wA!rltPrZ| zwfZd-W&ceU+g?5%%qVf)Oe=GUQ-5m99p$1=(XrvS`;2j)()IW(Q1kYG$6o)ROF192 zv-(5z9UX(?8@WSWm(Noe`>@aX$|@J{eRn|g`^Wdo6)(?{>YJyp*IpXJ{#~p~3|;Lj4nHRBJ;in(4-@=k z(n7zy8VjP`d2|N%lktZpTGJ%RcOBjWMn39{jvlnH&2Dpzr%RFTB+NF@h974`4onWlr)SBV zsFc@JPX=0hWCM5a0#Q+ z4XpK^F4cjl7TFTKs(HSRU}au+5zPCPSZ5Z{e*#Vna9l6OeM;;c4j7Y7!nbHQn((j) z3dPq17^{r6j5g>`URII z@2^_3aN$%v&KF$+-ai)-yI^DA2B#zwd~@fmjmnfM$p$<}%Sc55ZmWy?_Q$!f{j-$? z0sr;SYX0Jlzdy(c4gTSy?*{H|NBZJWC;h>GPtBD6`Gx`ZZRhKn(52fYdR!TZrMlWo zt~fYFZZMKp+{I4B!Xz8o<(ueOxgRqCh{@;Ki0VP+ZrIMFqB z<~aE7h<7<()ho}l<@v$C6qz7jw~SA^$QdF^xmqoIVD>opYcivHi26~M|IPHaPk55SSKQFZJ8=4W1Ii6D9&{@NVTt7FJ@;`erX_u^w z4S$tc>S*c{R_&rbUp7R;lIKy(7MPIlGZ~wYlGf&MTvRnJq>Lw&AWtSa3~ckTHrg2{ zPQ>~@05w3$zjE3A0_2?YcZ}@4vaw4iQK@~S++Mjw5#*cz_17mYVTbdK)+W2nkMslg zw3~mmKwqEE)H(vyKWh!mIdXv4pZ4GE{kmTE^`o!neZA=G>HXuqXS?&0zP|15+w0Zt ze0fvVtIhH8x~VJgswr6hdOx$e&9c(-cxvT|l}|6Ail;%Pm!|D&t=noJY2h17*Kloi zqMt`lq#?Y^s>8_?{A>a4s+icD^k9}=m(IUB{qn)-0l&oS3-2*MX9Zlv>kJ2H1C92Z^ZQ?%carn&e{~u&b7o#JO*67iPwK4^+k2$p&^ImM z6z%s)gTXn+JReXo2c2P_52%=fVpQ|NYH*URGmg5&7tP)}3t%x?r!Sl}uoSH`70zp5 zIa;SDoC+{ReMNF6!ubIhqP`+I!@;R)z!3En$(ae~6d2yVvJg%i7^1yXaxR6_1BP<% zB=>vgF!!0j5%m?>8IE&4M14heihadHx~~u^w~dhQD@4j|Q&juPqyKZ9^C9XhkN(ec zl=C6(DNYPU=7ufMHD3xH9({Txnc@TD6+EDAa?cDOhl5VOMHt zOX-xhdOP?)F76ZErH>*+?8vZiXUu6bA>HwXlymy;Rw|Xf#o4ZWTsy1#t;PAb`dnUr zR`>ZlPDYdK>v5LEy{9as{^5bwVv}K9_Xo|E@3j2p4B|nvAJ?avL6pKQl&dHt>d;h8 zgEs1qHiWJPr(S$6HGz0tFh`+eOx&vOK2O`KcUGGi)3zf=#=Q3OWQJkiPXma^xKRZ3 zyyHcQMqm6++ zXELGN`lx!Q;QZ2UV@OcG zcQT>dx=7zsn9yy5pXizI00030|J>L?Q=32(2XH%6J3V#!89cS8#2A%krUw#y$Yw|+ zaUsc#q=XP)5uyeCKpy-=9{nhS#$9G}@M!y(dBec%H_YtTDZuJ7CD&25~_*PYHXOXBf$7BK(Efk%q2T*EqzYcA&7CKZsd$|e?rcDIhSy2$ty4LJ%)sk(xRXF;qIHIp z_mu$B4PK|ioecAw5Q=$dBhLws?>O%z=l#Fx)cQu>xMHfRdCJJWsf_5pqnLwQsOEz? zcaq9EofM-B)vW%rDzFf(lM1H}EJ5pB2&V}QQ9qHKW8oYFL)1?srz@OOV2Jik$>|8E z2Mp2PDLFmi41wY8DjBhp4aYoX@D1^C9jlJh!cKK9qSTk#gH1(maz$x$Pxn z7FuU5x7}c#6Kc6_fq71-<+iQ=aZY#!00960jM>Q!#LRjY;2=Si1PQTZomG$*o~KAz z*zMWvsXcq=d7fuq5EsOXH5b7e|H(ll2u`q6YE{cptIDqWp0|3l>e?&kN@(5Ub5;Xx zzo{;lcX?H|M0i}`7i$Q`VVjWZ*Tj{xBb=I{_Q_~ z{TKi4PyK)Mw*T^{&p)pJ_^1AQ_hZuh{ofbM*xTDu59QzeZLVBR)U#zMDrKyc;w1E; z==E$TYc6wICquWj%S9GvX~T!j0-;WQ0m*wD69qT_306{LT9blmx_3I4qfn!DdQ7$2 z4L#fc&p0_%R^@467_MO0`21sxe~j-x{mq{r-+%w>e>uKC`v3RwJ)YQZhX{F?q#yct z^L)&AiQDlbt;=6Q6DuL|zR+aLlguN01G}4(Ac)X~ys^uO%Ylf^2v4bDe@SxA=_F*c`vjGxGC?01c>)^ z;@AN{^9QH{-KN^wB@lzG^7oT0d$WrVsQEOz<-hIt22s4^_&p?Zg(ja#q9D3kpUsDh zI?42hm>;lD>O%8mTy&@aYrM1g%_h?oW&V-oFQ_8K*Tp1QYk?nHCYh@i!In@;C9YVV zAE$=dWx31OXi{i&?EvgdO=AX$JPEkx4sCOOvdL7A5(9s7vQTFmo70(2gGhdtu_7%~ zJwEHYV09aiwoz8529{%MW~3%W{2@QjwtsLr+xF(jr(FX4&7x|lt+EwKokMUZ7iO6a zcF1iDf4S<>OSe~3^l5%X-kzNgi;(pwz*ks(9G!9Ez`P-JB5%*^OCls0km`e9!U_uZ zPi6r3u*MqRVgMF28?y;NteGmxP`+k526%5K!kCCIUqZ4(7jovxmjLXGE`N@&7$lD) z=d`kM%Zs72pe_8NR=?*l+yx&w{n z4R_zJIPm?67CEi-oZ=ow%5)txgLc1rXtc`%8-;9UcJ$Kho4*fNyhWJF6bz2Q-(`H>FC&DB6L_hF+V9l-5VFR;A zfmvDZ2_KU!+)s=y-}kw!GkPgfw!nv-5GUrgmq=hJIuMk^^CF03bs-A$PV~K5$5CSJ0iLhaqb1Dn;8M1oPn)4O#3I=(k zHHmBlaaj}n%roi3eqQFA@MU%~iI+Hq;|ZE0OvB@)nhJ>ZPX(d29N4tM$co~LDzxJQ zelJ;vF5)@ruP7wf9>BW*aVa|Ai|~BJ{oztrC79nsPFOy`yw*yfroi50HYVwBQ#e8$T+vFF{7o-u_oK*bLJFa%8-gZPMk23;)MkBS@r?5a3oYilYbD#H57c0A z(h32^7rTYIP+-UEL0jM*LO&)($P`4SUXrqzOLZ{ddyr|~h88eXB%|3(FO4DA;buEx zcOd&?(k99PT+5L?^Nm>vFL^l@5gDS%JmYLpjR zpscG^>66Pcg179zo!7U6@6 zCt?>f;Sk#`GG8BeO}tlU^*2qXQ+M~)f?MBo55Qe(>?O6|V^peK5W>@~^kz#?0FUZ;!VLwqb8LfVJawj-F%K-n0MA=@Gk2&T%+ zb8uKl#b9cr@Hda^5Bb~i$TK$4XJVzcp`?j6HR(YoD#6PN8?esAlWg*U0;PbJrOXdl z0le#Z5biIqC(WWJgIMrx%Myj3AFSw1nc0PcFCMexl~9P3C~hFp105!c zTGooJqK>oZBQ~F#1Fi-GdC-ml0X7pzV$#l!J+!mpLJXUr_HT+=Y9>#A2txyO*Z9<2 zMA7x+ed!-USV#yF8<>h%b5R?HJIW1(&v(D$Ir1U+f|@G!Dg8eGgG4gE&Yz`on8Hwj zCWZ70o|(k>=Bb5}EsJ?SQuUPN(&V8{PVDI*Dr?3>70VmxKp(Tmm_TN zI%^NWGDlF&;s3dRslWd7KK#r7!-s$QXMG6&;X^2Fj<&s6YoKzNNPwf$2n_7qTeHCxP)+_?VHg76yB*ts$KhNw&8!ODK) z3|-Repu3g>Heu#O&5+ByeKU|i=en?wk#4bS=xATlU#*Ej**B`F%hVG&R=e{ss5{IW zz&_1K%c1X$>yz>uYm*UPN(Vw2^027cz!c!TIVP8)2#*nqpTRH?(>kNg zwA8c*bT>DoZ|eJy1}aCi?AyxxfI2A5fv^(T*CyE!cJM${zyp*R?dqdgSUds)9bePq^ zS&}qs3oXl69F}YE-){bcjW2fhxUy7UWkSvTYF>KtXT>M5$9(V zcy6t-kK>tj(SwLU z3ngn8$ba$?<<)c(5jC>ef*^@Emjr|yKUs#fkKyUxXgOP!2#2TzrZkJb+b92S046nR zE^p6@QVxd>kOw2U{ETflKe*qViMRKfJ(8@nl?N;6$!I?#>q1tEWE?6Os_lS^iLSiO zVoohKH=oB+3lL~noi=G6e!h8wnqSf)x2vi##|PKbXHAKjGxaFaGDrW{SHuBka)Ffb8rTu zD@I}jqzQUZUe~~>xd=Li6Zv@OVs0Kl>fQ$&_7b#{0bVh=6b!;3orWf&JtZ!>f4D;xd)YY!`z^9-=C%EpcG*q-c9dzsrXg^0 zF9WzSk;Elq_KuCi$a~HczOrwd;IXglg#YZGC>H!I_v1H}w8BtO?hnVNk`r$2*9ES}B z0+tuGhYWgh4!Mg|0y`n2)q_zO*7(x!kZ6}+0?0{A`Ubg?j`{=oo-kxu@Ov+;LUe?& z-exkWovD$7mVr7Gv=;u-asm6iO0KSws0y0Jx;GZG1o53Xb{*k`@+Pqzba0<7f@Ow@ zEX|(r%^Q2pi*9zIpWvk-`qJ5jlXLL)Bt%Bw`{!kp4^dV8xAh!n}mxU~LoVIF2 z0Wf3)$gbHTh9Ui}!2_%-J+v+Ap~>ZFAyhhauHd=7rjF$9CMDTcu_aR0Fs6g^79csUYQa<#enAmt2kza{v5NZ@$i}MY9WzlNC<>Rt?JnQj-Cu0`49bV?(Y;$@{9b{E(Wu^*u&`2znF=(kVT8!tu@P^!R8>^wVhY!?Ehj z0eZ?kCIXE2%%EwnE#w|v5IL?@2wzcyg6-CE`LdW(e4Eeh zoB{(`(#ocENpj)uyRB~e*tHJy)_#^m2nnf1c0ncx%Ch_PqNV;lF4Qja)R|PuH}qzX zQ1VrN%?*Z_8(y&o|?A422#xSGI${C50e#@4V2AV6>RC%*$PkkEwFXhPC|(H93hcdRVeJ zd9uxIIh9Ap+EFKNF$XP@0nUIF>&>0|bvOu}JMf3fq~P7^)(ZHf$I7NwLhzP*;$QJi zRMxd^e7pM8)#fTeVM%0izJ`CMxuC^&CNT1Q66}%anV!Mf$QW)6;uL&;^ZIHPb>2*v zy3l&779aJ9irxycy6BVCA}ZjG$TdHEnR6~%{d9;FB+sN?tO)E!|9)BHtcW1p5hqKj ziq#Ao4AyXqHH_@z?X}>~q)1{jaoMOd(Z7$Fb>8{9@rhnTOeD!*{V1Px*Wp7Ze<(I} zqYMU3UpR(FIeAbO3{DM~ZoUH8AeHj&p3TBzD;}RGjj#{+ANAz3rXFf1p<~B5A z+2%tWGLvz)THRHczF5!(5dwH_C4TZ2;ZnQ`;;U{er)Op(HHlFlkfdB}q1-TfCxstR z+k`|c&CPLW$Q|{Ou4+rg8NOaU6>r2(E`&kJcwNPy4yp>2@Qud+E+CH$dh@pn^>A>l zDb$#b0()5&E%2v!HueuY73bEh2sL#iM2Nbdwh82G4jwj6xYcOWD^^}M*oll~ueb6R zGydkv<`7<&^Le+1JH}qb{tC}zMrh)@SLtt!QNi&U(8Fy1SQ25=RXbMoMb>qS*{K*u z)wWL)7~rv;4}Wd~fxqkMW&!w(uz_q2cNwYsp?8jyvQc`J{2I6uzF!Q8Pcs;S2gOR$ z9nx`NkHq%a_6!QQn^j@R3lWO+a8Jn3T%s&!43UeMtDzD!@}rsQrDhP5Ok3p~&h+NuT? z&fE=KW&&@YQ}iBd(2Ia=wiwnc5}u$gm~$Wl+406^%<79BfN>VNnP}4Y&N!U7?w-omL4)NQz;6!rE`hX5?nSIX` z$x#o3x1AdHOj{Z4?z!_M0J$Htdo&SX;`j{}Jxu`&H#T&dW9uc6rY6xRyIVC#2| zuoFO2@oOib0XJ$OMNlMj6WjccaeF2x78a9eyuu-6i9@1OYqWV`?;?J@!5~)5zuQ9q z8}+V-?^)gb3$QR((`$rDl!JyPOX8O5CUpfX8a;%(cF6=;=;3^0^jqN5B?Ar-VdnVy zCdA=Y_2K|A_UQ_ZKKwIpPC)^Fh!1p-=Pr=9Cw)*Gi zNq_h6|GbC($KU)*5B<^qe|V@9q3uBVAEqzc)X#RyB*EO1L@5g%Bb}<^O}0CI+Mn&3 zNo__}miz1RJGf;*;KPRyey&dIuo+7(zY2n9zRK8*QzjQ2~IEGDJE+ zR&xS@pI87a0PPbx=_t8c3%vDpxCT9?F5%akcHagN*aVF0DyIogjf$ei*9o)p%G1O} zVz&3a#y}M6E5bBXV1zk#2THFB|7I?z!-+4#jGKCGR*i@$r~~&ys+08$9haP9;fa3 z8M*x0|7jM#Wj- zCbvnGH1pt#lDN4L>>|-CF_jwsJyYV8e#d{$ooWgsbmKUK7HJ}egMx~Qp zFrag>3U_ZZO>=?B&2zdH%79GMci2oh)Q`9-lA!IfA#-=%>5f1Oa|YS#Kx1V799>=Q zX60G>b@o_AZ>&XF?)LU5*|GO$l-{;?E?Uh(O5dPmvX~MW1Ji3Mg~?9;sPuD(Z+iL3 zSo$E${oU83tTj)+S?c!7Tl;v*j%*+7-7cz?#gOkIjey$K)u^L zL)-I)#w>;vJ@8!dzKLG@Zz5db`jptB>!&JN3~|`02ct3XbPBt3`t_ZpS#V0U!-gm6 zy!``qOuF9Hpkgd3e8fy9*;U+uDDt=uhekH#lZtrzAo60^+T_U?zWD`qpUJsN zn^`UiVmVcq25&D*B|y{Ju7|d+ImOCn+jxE-cg(e14jJg1kg5e22JN>wi# zXNI3!)P$V0k`pl*!W=i;*qPaNVtRq>GN6Uzs0nal-}H5AR?Avxj%~a$k*7l@w%rlZ z_p_yOdq>&Ge0y)E2jI!u=`tK9y(`JTJz1ym5^ka*`b}y6&yGl;-VD60tw<64qZ*1( zKJ9xIJUq(OJ5!&1 zZDnCtnMo61d-aY|9bA3-@f_@MFXbRNXztr|_c%{x-0woKhlz>QO~KnOzX{IlkiE3- zcYJYu0@*ZK-vw4wmq&rw*lTA9j(LPVD9Gq>rbrdU#8f7srf1y6j#(USeT3cg+g-4F z=u;??%%$5qvR?Rtzx!y9v)3jZE@nRTLtq``p~Hd}^H%)W{Owt3S$uPgs1oXhOLTf8 z$JzQ6M^3JS)Y5=#jG#0nhZW8x{P7vqSkXxPEprREhcLY&u(=hg?9nAsA$DJHt-|nz=aBcdysrXl zCar}oNw{L(b2>ahs^3-2EoPiA(yY9P;aneV)FLAHDyvr_>zi9lMjU;4NWnTZPVC!) zTE@bg)}4nyR1dhQIKbWJvmB}HxppirJ#J`=phm7(|DO4iH(tF89r7~CfCW=-}Y=M>cd&57i?tvxTpnrHZ zbabF2%0G15XC4=UCOtGZ2NmJ~OYOw?&mIBiq>O`$ow!($dw#Ep?n6@H0Dvadq@ zJu>($Ied*t4?oCvQY)V)y?#|&DUnPW3g-7oe=;wuW;Nj@PcvM=uW=0HI^=GS`Y-e_nX4ZqCLz8(IL#)PqAfe<-|2BHk^Q?e(WB!_-5de(EiD>7MI5DO899dkqOA_dhA$#s zEfyq|nJsTFCF8>W+lbH1lV6#Mv?Mo}e$i8tr zW=PVEZ;1IC+}KLxsn2$ZHh%OGl=AgPhs{G|2VRo}8$ME@x~V=$)@{TzKWk{KGz6Ot zVz67l@9aj>m>aK3Qcs8p2aP8+T6-cS$yStmvp1cyH7{_89s1Gzz+)9S%AHh&;-CNZ zk3atLkE{K|AAkJq_4>Q(`RhOatNqP?_y6j8{^Rfe>-}f@r~mHHXZOpSfBQE${W@=% z>i+#N|6FM(RKCkbNiJnUA2(GR6EotMNs`cts)2 zWz>@Amg_tN|MxB=r`_qIG9KVou1{6{k}ZGxy|w#`9s*l2H(^<}c+0aU9mB~EsDA6K z>F7A`_D-LxuY{Nk#_*kp8`hPB{tI`sExb?B?tVHo2IynOaugKif++Y{1t4*L91q*D z5OcYv;w8wW?9a_0goE zb-*C7MGK7SCno9KEcGu3+w3`=x>qHVMZE>^Xh1gnX7?Z|rq+@tqS8{7;HfwaQW>JW z05$rK#+~-@*Xkv5!6}$mx3yTzKzmL@mf7LltOb0HwC(xt2Ue8hT_7*DwC-z({KT}n zWr}uzR3)J8ilJPrj9&bBx6GR9Kz1WZDjQwNM80IHIQ@O=IPv3W{LO0|-7n+NQ3iQ~ zt@7+y4lcUx0k}cVb9wcvvDkiS_YKva_&5?=XKN|X56zNE>!sfMJ}=cDZ3OC1HZCT( zsmzb&#soXC8zT^07Abe&Ojs{uhKcXP(_Ohd+a|GkP^#I>>o1nyyiGw;ROfZ1yXN;n z->xi;27L`d$8+4S=-84}wN-z&ZT?22b+Ph7Q=IYqla+x*tH^tdE2lBKTn6DZW2w*8 z*Cw3AyIu{E*{PZyh;M?AEaRJw_F(LQ?LP3N=nwvTadF!k=!aIbtx(;(bt zub)CnGMw#m1l4|+gzfw7(m&&45UXLL`l&D0?XmaAMxGDq&C(rn>D?UebnFn+gs-cg z>eTPsH=i{eetdc=nXex;zaq;#+NHEeGC@>p9u>!T9{#qGCP)}Ns$?o8TTkipaoo2# zBD#tlj)Owg)U={;gtAvnxOb8nI?c^K9W1b)3ZVw>l*FcfDibRreWV8mNZ?!Xf_?Ms z2kA-BXmCsyIWckb$29j`SRQ?mvWGf~X3t&qA#OeORy|EK$ESgbTsO~+b!}>Vm4~#@ zJ17d}faDTuV>pfAgVcM)#)ZZTzfx@&aYTjSapaglfjcz%I>n}--t&%W+f!(TUu_NB z6v5Y#NT6-WLn#BSoj;`EpB!6pMo!-3I2K-$tJZhTrUJ&zP7{C56@Jj>{zLYsv6mO& z3kOdSL%&A2z*8B_$BwFAqroSUCa3~oqV#}+A@q#%=|5pbb{Qqu&=AD(BJuTR&FwpSsknI(dBvd%3W&R4`I3{`j3;-AjR%%JFgFao&(ZHb7F6Jbra^s3Ih^m-5(KE&IHOg|WPOCuheweuvnL1m?A(XcX})_L!7G*TcD^L7Wm4tgU7(?+lRW9A*%*LF48J> z632k`Plwpe__9V6g6g-#ZVzc@j?~fXrpnn<+?+>3+adV0@KR8^lL6;rH$}e}3p9KZ6Z!GVOz;XcVS7b8_ndAE!4%>eIYR1w z`5hdN(o>shtc!sUCwz{&Gl1lKS0xi5LI7oa4 zP!&nvemimHZXcJr%zDgLSa1a4o`qgjh*gnc>I8QS6rPSp}=@s##Z-W~Ty~|ns z`~Pt8C;b08m<|6o2mjgszc{#e)+Cifg2RPaEW^@vc=vFs`d7bSueBR5&%W+u@q0+L z<9PSyYGhTCW%tWb3l7dV=+=0}4`;LXes2P)4oS-}d{*PrNPTI;S6Z;2Taz~X zbo~^hHKf8Orp5)w%I#7VE~1ld_uY1mJwFn%-MH;p>i3}z_TJa@%vXIRDC131+HVxh#U4$sbT~+TBL>BIr zk-tR#6gULFpv~pzmT)!?w_a=Wx4!w@e8tm)eV>6Y&y|yV;GVx4WT1|CzQi&(ija=; z@hAyWO;0Lks64u^R|z6CjP-I>J(>rtzs8;kwpDg&a4Ehilogwd>B=$*N<58QI#_^0 zH0&7Y-Hjn2HT8SEcQ&^I|C@H@N@dN&YNWb~K3&8Vi6(zYc)L9a-y^{wXwqhL&38-a zU!~<|RNFsg!9Xzj{&CH*M4PIvP&hZA9G}`zl$>_5kejcC;Ph1yos}v-TvpoPBD*a^ zm*F?OH;fxmft&T_1ia`=Tvz}l)nrAcDUDrZ#}<+684v#YTe~Im&yKf&x?lLh3JnM5Sgv6#9GoXBDOjNBugJS=|#)ux1^elO1R z6w&~$;bBmH@;kVR-3qZ(OS>=U%4Mjyr+B;fd3=WAD;h7T&jG{5LK8i*sxtas>9Xs} ztf@xjfWpqil6m4j6Smzm(<0Tmy}>UpFej62u*l`4eo(J7BB2^SOF`eR^rnwxg*L}c zU!6HJumZATc{==ZLM5cJUnhE< zSsR0%Mb|p53y)cgGvAc1up#UCSXJ3E7BFnqE7-dfO|UK@+ld%_e%ySDhp=TRY@p0m zz)(H#F=6Xe+U7p$Tcub!hvExp0!g;k57!Guh}AY;4{z{HVc8dG{C9 zvldh>Fl{)8N(0ifEHXk-Fw@0@ubV{ju2%miN7DWrnOSM{KN$8nCJ0Ne0H_>DH2@Gr$n)O2c zbxc{dJ<_17-1dOBY@IJTX9qGOb@MAet94}FBNtkTFHMp_d8aBI z;IdS%g2@*yL=d&DS$lwC16onY3x_}VPv}rpNpMPUHUObN({TI7_eEa2lJp?Sp7*<1 z7s^|UYe2E4qDU8eF`WG^_z?Rr-N19uDxIcW1ZH|%wqYg(f4wIhY`1tK#P51iEL#1k z#Hw@kNpAT4_L!&#f1n`ce$iA(WmQGAtF!aaFlC-0P|xD#uTnog~h~9R+D9$;6w1ez1qRRprIK`R8BNZ zGnUz|lGL}&*@V-%@=R%qThqJ*KL^A)SLK%PSTMm~HB_6$lNpuQ3r*ltgb5d^k@jlt z6*jWorq5+p5;Px>4=R}Q&4GRSj){y}x>YBe91Wc{Q^%Q)CDkZ=#PIcbNxXo5bt+$1 zAuS6T-#5VXfh~$$g@el&0Omrl?Z$EEhGJ$?%t_0EXQXGoE1c*XY^vKT285gVc64Uu z!~T0xGk=K`cQfAYbZlh~c#Vp&sMyl*rSsiy!A`q~&h-uO_IWA$_w*8Ax58IPWy%M9 zNHfrn1xo;{b~)Y{I@W;;n+~3^+_IJX{j`z#eDSi?ZR+F7b5Sl--(7m1viVooC;0Kg z?!KK{EY4-ijC4o9<72VP=bl|Og(1|kh_ExS**G>%lKtq)_8!l}z|iCxS*PV^OrAdv z%iq%H311lS&kHppoxW-XW=mXS2*!t~+OGXxmrI5foG4zx2&U&$&F13QMv)2Cph&^* zvPSoyIt8H(u9*{REq_i5EVv};z>v|eB;m2ifG`?>`v~wGFj$qn%OR^_}fw- zC%%_43I zd>H&MCq(LHl9ziO-V;Z}>owfZ5oh1dh)Y7kSNb&WBBm z8dl0L^c13`Xa5pcd&T8$!k(Oe8Sh===f8~K%trsqcpdH78|0o(_9*c%oX=}W_Saru zIG)!32><~9|7>~3!m~P%>;th7AmWmYG10~*>`czsWO(|~_uqSFH5+UYZgsa>)m0WC z+h#5>JSv`rU74(ZtOoex_TjSrqc_3l=^TK4K1dyWEcfeb)WCl!sDQ=RqK*IFSRE{e z9u@p}dK;33yD4r6NJRchGm+oUHREFSM` zlfZd1kBz+@qGDw^@4`__mJM`UPg$QFdu;=!S1x#$&K=k}%h$;!TX_1*vyuH^8!oV;2(oZhdAl_kT@%@OpE!`g zJ*!uj*Q9WQtMTRWT=Vb+B%ERpJisY~%f^Lc+-xzsZ%DfvpPLU%4H^B8XEbfAMMh1D zbAIBmdcuO-ahlqa&t7vyIsBUPo9^8ZOEbUUlY`Tj?@wdvFeEm5bu)-(`xGi^ysAYS zHq)vmX>qZz<|}{Q(o5J9+(6mpUe9EuvqE!Mn!kU)Y?Uvi0P;JDZ~B&-B*k_iG+4>a zy}g|(7`)8&dirg+FeDf18%LRQ3iKA_dsK&;$wrV&QXp-4NFLxvPEgnuvUn^Pspbra zrw*n{Ne}M_gn57XI><5I!uVUWz4i%8d}W*yEAj$J?TWXTZaNlMfm-&54UTgzDr3;zTHMTmuah zcNw($*Q7Gykiepc>HBR27dtb>LN#wG+(vp5HEP^OxJgx6Wz?lKDXt~oW2UUwkdjUe zsi&%ehkdut(!|ZGS`BtpT!BH>ZH_uq$DW0&aD1z@v2vW@UEVxL(F*T%w-3MAZ=BLt z6ihWIDIbe3_H5}^7TTeW8L*W|66n$sbop4HY|fZ9s=6{?=IXIHbd>HW2t|B2Ye&R*`!HLK8~59)xRH@l2_^Qdnsni&dc_BV`<~Y@&9HqZi%h;v6fR##|MI-Z z{X3~djVS(dkV;po^lWy{k{FWHez(bry)S2Z1>4{Fb^;V&2|uea!qsKSRB`tFxpWyU zs6=H{?a!xx_2}unmn`zB;6IcXFX0X4QD|+biZh(QRoR{N(4nE6n&4fsV@%QL;EF@y zLs-f%C6!t%TQIJZ7UHKHsSN$^GVAL;YPk| zCXt?OifMq;i=}-n4=|FPZ*c9Y^M1nize!gsNJOrZkJy;|d|}prpky{U(3kGVxft{L zU9_;fg)U6gAi*1UMDq6ZV$MFceM>cgC1#)6Hl}hL%(rBiD7r%*&F*Fn8f&$}<~MY+ zNQKIKxWNIu5HkGdaR7~(>4&q4es@z@9Ij%hj>%6clre8xhi)LMzhtMP$=!86XRMv*IOfg2`)x+!qYDq*6H*w!KzJc zWS*11gELb+TO+j%r$^l+nD^4_SYR*7CS_Ie0{4YmRaK)l_s%ym-57jwyjezQCGk63 zq-EvpB72vyJv;+XdDlj!y${fqU@-gUeD9$)z!sG;`-p7iAH$3aS2l5;e-;Bf07 zU&-;_o!^FRDShpQ47=o_#`Xxo`m;5+o;T||U&|!!)5^gi$0E;p`mENyq*D6$#NxDm9A^i0d^U!sHi&(q_&!%egQ@0*Eo>+7Al7m*^ubG9&( zHA;zJ?%s;Ahrjb#CmZ36Hx1sB0_M0+|JD~X+x|!t`9sgeE?d&DG`L^XAynhI&ogDk2E z6R&OwbHAQ$+YR=tmtisQ7s?|P_#G{1NG3uCpus)DIqXSkxcAq`Emw|2(|IBy3uv+h z-B88zmT$2LY)QZO#m7hQtOCpIBE;q=a>)7j&RB0zh4G#tT>p~H%{Hde6{=fXD4HGs zY8`XZm5e4MDOau~!DVOshLM>w7}K`>L@vz6qZ|nn<5X5AB+OjNz`s}Mz8_jJP>9Yj zls%DCoaamUB2Q3!=U0CXWXx0ROG?2;?)+ys=~*$&K_8)XvjXm3k>zxMoKo{Nj@pbL z#`5f%_b1r-rIya&x)<55x;v=>sWDM}8D)tsKVm)VS@yS<2GX1c$-T8`Ev136g15Vm z4B1d_h6#|tlRqbWd=`D6c1>5bk_#iz=QP9VDJ4tjKKCv!RoCp6pAqT>w^{KepN(+F zlZz};l|6beap`S~j6eGsJ9^)C#lh=VJbpYZ)!3>>WX!9C)sc0hEbrx$<$gCR?z>A* z+(fJM(Krk7(&-LmevOLhBBw`6J8afb=Tg*ZPJNm_P}0qIN~}@&P+!gC``);VGmq=` zv5lD9Qs&H4^3jr+Yi<`tr3)W>2nOGUKzza?OGnf0DsrQO!aFj|>0ugU`q18wsUTmb z-4{@v2*;gyM_&m9F8riIMw>B~UP^YGIj!TbW&CB}hgZ${fgiW%eP9k}0ZO=;zg?UU zte$$&j{*N4yBS;Qf;O)kH_?X;y3lYPFMfc?+;l7kITR<|>7J(K$XwjmjgU6{AULJn zSBx(wj$G!}uGzJ(usSRC0*8<~5(M(2YreqVYPFXywakV}x4QP31GQCGvqIl(*#>KT0WM(I+l}wIF;wsORS|Tcjx4) zc$w|qAQQ!O2PtrJyO+ov{ib-g_js@jE1rqSiul4?`fmR=+x!&BHzmj4G`7t7yamaH zlAHIUbai{xfDV`An!6%5K2bh4e{AA6nlJpe-pnIESyn5>?t0aA#KqU6;gul5G^_-7 z<#xlM(hJ5!-fJs-CshP^rI8Im3zZ+IOuVllS{s}!3e}|?W$Lbh#kBR6R3SUQ_`L|f1>*uC781*%LynAY zA55I6r(3cP=~=MjuF9QGP#1Uj-kXgiWbD1WIlPJzME36Z3kB)cV+k(eT-tDL=3Np( z9*RAO`V#zk4SRA=c5bMg<;IE&t5E+&4$DpdzROP0tXrYb{KB}_*L{cswxwVHb&K2y z-ng^Q6mD*z4{#eB+SfVqBIaja^1E38s-3U~wm30)vdYux^~+;%l@Bfd3FaYJDiGgG*s^s%;aS z>~_IRMaPVkw&~P0d_a~r;YAN9AJY7TqkFtN6VtdQdw?~6Q^Bkz?-xq^eRvg=b$)pj zE3C0N)DA)z#No-qR1A0U+twz^LS}voNUtTUIJqshvh2m(fDU&;#1v~vG05%bSWZ9T zOYizhvA3B%RGL!r?EBj?5lg7i0&G*%rMVgoOT2QOfN$1$#eVYY13Vu;FslR8DE) zN?q;}%COzU!?CX{NR15AJx>uL)VXgY6Q4u>UC-nMt@Z0LTRpa1mB?YRySS7VPFrHYMPTGVytFw8@w9cg#6Q$HR1sP zt8fmCDszfF2IiG}8(uEeo&#ob3swQN=3lw6p5Re+rnbFY!t7k`9v4mZgM5P)2<_851dmrIZKAjP$eSQ5Bmy- zTI=c%^M%)C(N-;XeEUG)T(6wVaqZnG`2#_rifqxycjrp~@YIi{Sk7JkE0Bq6k+PQ4 z*DDe#cqRT}v=|a+Ls;ittaL1sy<=-Y zI>N_P9v*5v!ErmeI5fak%TS{pkg{9-k+W7CG&dx~_g-f!iO`*cf^WAlg>YVlc}k@X zQ$AEo#|8mD2KB)5cj3h7v(M5=2QFd=VRPvU=^1@EB-M48hC%NargKwakrIXUN|o(< zByGb@7p~3LWU8gS2-LTH-=|Y~hk@ox+)ZTV`YVF^Q3aigeNxT$|e|gFActkC{wYh?^QQ9<@y&I}AzWI}u!fKVr)XPq; ze%h=oUbMMQBfCkK_JJ z9NlPKCH>X6V1>8nye`1qxsl3Ixu|A&z-V`oZf~NCg zlc{#v@za^5DVP=&yYqS6-iocThZ&LmNmNX@{uIh(2p50vYjg3}f6FIH0Zfch~I zC1oFF`_N=F)UWZZ5~{n56ijm$w$J!-gH1pcAMV7tJ(|7A;^Q&gf2+`BMcGeAn{G&% z?`AQfo@*>S7h$(g6!CWYRIzNY^t%A#5C7mF2NG2Gncd!@icQpX<$UOBeskKTGNt_! zlurjrF8RE{G(tH}ypc`AS$Y)a&}>)hMPmb=oFr}it+TJLuJ0j5{c*ng6;iKaT0fc8 zxO1m60dFzPWQNO7Ho&c)YQWWzeFUyHIORQVsWg?68c+wMT?72Bfh5%zTUK)50EnIX~{UFv3y^06eHgD6MR<_f?OI5ELUXrbE^`|uZn!Y z79jdg-V^EWAbq9fj$q-svss&4E(79!dUBg~P=wJv-u&$zmpnoJ=R2Dqd?mxyv?_CD zhDTM4i92Y%+!ViB`p?$9w+D5>L7n$i;h6%or8?t?GdlPmFQh01saJSNmr*AB&5Tbf zgzqba-K{F) z#F^#N&mG!)-MUL6Vb46lmY_4#Vky3o_h2`bOVrJKtNO|jr(K(p!+jJ zdm)ceup8*b(a zfcd&5kB!QvKunN= z3xKiIl)5=P!IdY-Y%#4nHdK^+&!$z)x~ZAI)=1MMKJIiDf|v7xq+izhovf?Y=h1Y|NY_yUg{hUP;@?2WdkZkd}G?eGKrx+F4m ztpYWk4uq0bO7N74G&0Yk9knP;e_{2D*D&1kCO@G$T^f0}q?{kl18Cv0c+-ai{qy9< z#3Y;Tc?PEf%1fJ-;;3&E&=hO2ZUx=mN$!>&c%pOK3t9uOD?seBsGxAveC1WE4VP;4 zZmX_9F)_-XA1BTC(MI74aFE(Exliiv;)RIXz^Tnc~F zGk=qH>?3Ye>36V}f${D^BdjO1 zbF*~*V-=Sec)8*7W!NI}6gqPcRwU(VK8Q93S2kx@kj=VK0LP!|Az~>{%T&o;o3YWQ zerbro4E*(X?zk43w4n>3V==vpZBfhfnLZ0zlHu$XgAy3@i?0E+{?0XNPMIf_ZwDmseg7oj0Gx?o6VA}^B0*!oNXl*OHb!!>hZ3~!fC5PM- zqq(FmpxYqMV*#t{3Sl5!ES-4Fkl#%)A?6L>c3?qCbye6Vs_s0Am#6XH%bz#-0gR23 ztF+=SQU! z_|h4X_KxeXBjYAi?bKvCKpW4;Mf^hELmmG8rQTOz7!E?}CZc$US@ur|gX~zEF4+fd z`;APKXs*WPSlLXOy?*h}`Nk2!a+$orMep7>@{2MI!ntE9bAthsWd4^}$Pp*W#-EDy z@g-IbJy$=WKTNj0Af$9Tz)SY9-c@4b7ne9YB}Z%ywWuFNq2Fmyz~H1|Y_ZMwYLeOj z6OQk1d3<_1)1KPQhGZFx4nfrh!6#6fmzBvx1aoT2n+|j_oYS+NUV%}$E@Wc;%6PK* zVwlx8&Ijc2?^)yld29c__A^)~X|I-Ce?Rg$v^zU|#rgP70NlVW2C8Fpx5Ctv(qhM9}Z z@Xu6kuhE3^2O>3;WZ3;J+K|rw2LJ&7|0Gz)ma9q({2&&jCM`|IhiUd*p(q-6j;^&Lx%|9O%)hnYfLUo#8Y575#2 zeO1uHaCZUV&ICsp7DgfOii+ec)5bw^r2?NBN0rXU&Fd$bdXA z{q#P8h2*fl(}xC7)DyWdA=a8b^?RCCfh(KYL>BAI*JXPSyc>R_A^iB#=agHJPO&+U zY05z1Zes16Lcs?v))3aD?_ClM#TF=D(g6JO6)TsMb-!dE_M!6xNkLMf^gdQV&vh%=a?;#MMq(D@+hIRoSBqb-qwU|gf-+ChOv)!L5^U|oqKN|MDWZ(`Wn zgxH~18M`H^DA0x^$y-?yUP#G9e1}8}yXh(6U-%u8@Vk-VqJ)Avr|}xH(lE%yc}M(G z8!#W{yW)#;3_kMvZttIxWquL2FKc>(aE0xRq$8wdnHxyui$;O9_AiRuUC{1(#nKQJ zt{=>z`|D-Rt>hPuGq>hB*d5--JSzV9wX6hl#I-L3N?omgw0`g`)`k-5zw?>bjdUI-Xh)fR*^neG&1x@ep@L7ZXX?_#$#IAwL``a0yJGc&pyLMFfHp^&qpz}+p7^0R z-SEkAirvnNE*~{$-Nx+^#;j;nz2cM=oRtRBTfshic*jJ!OJrs9xH*PeIWjvFEmR#fitcuChCb zKRS4eua+j9i32VK;m#Z0KB-b`Q?@<72YIBBz$FF^2rrKrc0_yK==X`UKIMd}&O2b= ztJh&)58&T=esN)6QGjDTcyq26WHwd-EAL-8MH7Ag;1yt+;qam7LW|6oEm9`&U!-2X zzbArd*^ZlgA1b&6F`*9;Js{8{4rSQtsER_>#p+d~ll5%1Wnc5wU6I+!P!#@d(cx7U z*+%z1{C;;?oIuYi(h+vbI+ui-^12sRmBj|u>vKE@?nOts%Hj>&Kg)3(88FaAtN?xK z9Lt^yIE>bF*vD4pEBB?##VdXRo8Q$=2Zyx>8he6H5qRgZt;E&{*duBR;W;0Al+70t z9-~|sP;`u9Z&#!)wav?9kdE*8E%91=4-A#4_#F-uaA+GTzNq7FIq^j-ZuVT;8Rsk2 zj!a7yJJaCIzrCsMv9&f#d;uRS2+qtn>S~V6G=|qJn@o`LeOJgY=XJ=11$~Z6F8U-m zB4uM=_h}`N{8|nqgCvA!abk|W>W~+cP3$K>6AJ$9ck*vDuS!1oCL%l>-(~-9opLaza$$kGubZF0TcvQQmf&Q8!ueln zTcLCOmtmWlHv(O`5u@u<;0W7MrcZ1VSy%YGZ^iCll3TiAp$GE z@@B#bWQbsdMEqfBXU(p= z@dV=&=VcTxtl!_Kgq@KNuzXNKXn35CcKz`2Gw z)d>gsKq5ij9rA``1~K#CnZfG_5cA)0_;XVilfX76@)tqm|i zt~BOEN(+YlEXS66&4hQuC0-p-ku{E^pJ`bm+avhn4#PM0)d?TZUTkt0)xz>m7U>eK-IOJa&uN(*%Gbb$H^}4qAxB#|iL08#0oGA(S&CdhJ#ZVS_JFoAf?>XlPve4nY{k80w@*Z}^ zI+fm$1c31IU46oz`1f532?BF1{y)3O_hhByxFX_R-F#DXNB54I^6$IC{~AKT z2a}JB>WRhp7fY!)!hp3>2HqY1#4F~5Fuw6t!s%!4DG#4qN%2c8#uF!GD{l5tn!48M z7s(vagIka9`_FfF(BnVf$K#ug%g6W4US4y`mHs}PM=m^h!b0G+FS$~D4>>;B_4Z}? zx~prp0QaNPpn9llb!`@F?m++D(CfDqr`25DAv8bg#8?U$S*`3!urzzBO+4&%W(ew#XT- z{Dh@w^|=|PuI|27macE+_3zBe=ZtT*tN!X2HrvAyI)=M)&~ot$ z$4|^q`md~{k%opoOlXY3Xc+7yiRd3=k}6yYSP|wNJ#23xQV#Q9m}qd_oUg zvrNpdmeB;T#?mRJ35yu3UXRyVVBK|~HZBu%I`R!OdUv?{GxvmdDFRt~n}ol3bSQ!> zDD4^Zh890Sxu1p%3nmhK^)2CNfxtRs6OdUFP<@}SZozM6;JfV7tlzC$V7qOL#+O6Q zy-rritRb!4kLq#)z&OSQtC|%+I!!}AH1JzcZ;xU76M5Ri?Mq< zT^2j#Qh?a!S$QSlcO+Xlj0C9G$vjgY8-oGa8@-B+a-TTobShC=Wqs! zYZw6F9beku;+k-1{(Z@3b>9eUXm|(ZH3F6-+Xm!>H!Uxqgkc}(y5JGrIGLuLB(vfY z%eN&t#5%R$XAWvHOy{ys0A^;Wwk19j##leY;y$h@Pf%uny*xw%OrHw)wyLxb8j4ZpPmW=dYF zHZ#kyp*Z1SWZqsvxi5RWFI<4z{Kxtx(=Z^=7*m;&C*9!gfG5{}2m5E~uci9N;LH#9 z?;jfHX1glWv1(TJ$Qd+yg>hw95KKXn%MZuHdesm-x+jPtCa9+bMn;8xCgfZGZ|(f_$nNEgOB;V4$(o?z zV1h<6K^ekE+5)}_3ZNWpw@EZ}J%KnijWFT|Ob- zyo>96M+SXgl#(V-r{pM)!F58yEQ+#xILk>%^7L?3?ex1bh4AjOlFIOVXr>8P#1>1u zD6~?)L!#_LC67DFTpX9Yv;#BvS=ssIl0Rqh2wHP#=rY-hW#f6IB;2>_0Iaf$aFjr- z#|}ppM|f@N$?W+#0hM-c(>ff`elxtD8`HU&CA(m|z2pPc>tV6#pDPtMq^)< z0v3K5r>5CuR;*inrs%~_AhFV*QLgK~cOeqeT^i?C7>X>6(wuW#hYKB2f}Q+JS8hm& zGpMiejV1{>+YT!`8;M~RQ+c~zg?GH})a_6DZySTlw2ndb9e(zivy zyuS#Hh5tZ;61qj1rB7d>mk=@Y)7~QH z@BMt{MnW(=a|4I4;kr}@KTC+OFJ;Mj>od~Pp|Q^Wd-Kz?hb#IG{b(JHeIT66^gNrZ zCIcuas(p!Jw zuojQcmYBoXkWu;mY;;b4j_GP~xKQ1F@tHGu?gB(lX?Gh|lGHe25*kn5rNEpcE?QX_ zmJ+@zHj_(xSf-Fcu0aUz7NQU(QZ)Bbt{1j@B?6r^ix>r%MJfqX|2j7dC#)r{o;@CZ z0FA!_Or_J|+R(h|JK50bZiGF_YC-|&R*4;O|Jkp%W6&?nN^8**|mzdP97_ey_J1-0HfhmmS%B7GT`XE zni+(%C|4hLx`;-#Z^zoC zkZmO=6cf+8H2kq%%5->mWrUzFC>vV3_sCQ3iRGzF)%TqxgqQQ=#$ssLO3$c3DFOPC z&$*O&hfX530P>EXnuj*^^`JYO1=Oy!nLHy1y5Uetu@%EXnP{y;4iy8ph-i|eV2jQdMoxF<$j&<}*9 zYf7d-Mx!@F-i=r@V&B8I4leAx0ytmxv15YTpJ zBdmY^$l{+r*$hz1*!|_hcqc^|@^i-+2Wnx~^xC>;0i)$a`c&A*6i|Npk2_ z(xX6LTGR{cgAFr9rnjXZ@<%TqFUt%p&N+pIQocHT9_Z*b`t``;xE}BI)(C`^k z%c&d()nG6linwSv9;%5H$NL}-@fuE2)@gh@_9MZ@`_kz4ek2X8he~$QHP8>9rfBx# zA0?u0A{AHgL*`$^46fO9yX2(MK$L|WWrk%FkMu|@Qy%3*xaQk}?C+?YrF9tJF~cAq z@;|%t9;gq-aFX}k*p}qz7HWBI8|jwwzZuk3JtK*p_zY6RK3n1|mWIzn1f|vaPm_#1 zL>?ToapI8XBg^9>N6%HKf6sh=e>#p4q;N_ej$UB>YCk zGpX;|oo^3+i?dV1)5W2;pu{7SsN^8zy*T5G{g5_QO`bfY!RYx=nAT6x?k}_}C1y5- zH9bz^Q6N8mMzUDX8`>ovBD?EQu#(!v??Itu9z|3!A@?lSVi8BR>1xL@DzP^j5z-cg zmyfcNHAhLQ(J01!&<5q8=MFKdpzjIBNdM;pQv6q%5bhB374D~L<)L=8jr`~offP3a z(c)5)-Ma`nI(ioqF+Eb&7%2ZmVYJ6h!^md%T2kcIGh)7`oIh%uxh%AD}oOejC&86j*h)7b*9nyKcAO zC+GQVQ!GtM7P%8dodVU=@6)pm4Unnzhm(eW^8wGCI0cQFf)Nng(y-rWGG%Onu%2d@ z$GSN8w>q`w`MwwDfOd%vpeJFsIs_u21e;}d_c(;&Q#$$OOVwWebcYw#>yVC3$3u8s zqwGd#j*saF}>mBeOV=UxP_bX=c7^T#4fB8w! z$J9LO9D8I~lgu%dohA-`S?&UTpMb|7#l9(R1D{p{tPwMJTf>fMjb$yMBlc*IbOt-( z4|GLRN|2PCV!~BYHo7h!ZrK11296O;C07>J7Z$$hUJ1B-7<(1~g{Aj!ua4m3H z6ypB_00960t(Qp-tXgu0Z{Z6b;y?$cav0;tvjk5Y1`@qqe?M{zbu0>i^R4m9Z()tW^eek0Nf0`oGhipm{LZU#A?01O(;3J8=$ZhRY?8$ zib_Mw5izGF^H;iy+v!EJGivnglqK{!ZJi?4{%eb5#nES5bcFjE3-=of(LD*Y+dhEe zDLEDlR8!PyL&FLeEA94fK5n}U*V)+2^@8Y^w@cEiY*O8t>59PK+2IWa^&IRwc5)Ql zep8B`O$}Wx4E26N4!8qCPF+{T4pJpst<{u7ISQo?KnzaAoLu*o$;JN6Tui90tlr>) z%MZ>Q$P^T6&~5szb3d>JjB=%Gn91OIeX$GzH6{E3NM~|EDU23ZxKP6IT#mvVX(cPS z!y4s=uI-cya!8FqrCeMX5S+bStd~G`abba}JGJcxTwo!~Db$}ICL4V|1QlQ`H06N0 zQBiO6*l5*OHCdww>MOV0F@EP?pQnAXYWciAkL_2y-hfc36BWIbmK=)&{6JXu-9CbX{o6SU%;oIl4~VgPC+u>Lqp%1>4qg>v7usho^>o+Em-5zo(U?^ zMdNi`b1OQK7>}yOs?D8KTB*&{`l*0;Ck|LU^kK^AhtB>KhoOJP!CY^MF};k#jqCjizv6Iiap+#+ zdPw8B`W+hqLtg1Y>zeci<*^$;N^5`~M2WBM+_F3fRv%LXtc0_hJ(6?a#zMPQ@q&z^ zdL}7Pd3sf(g*is_DBmc2^5y^tUI1c=5|P*nvW?~;HceG=T5^gBR!Q0Dy1ak+!GXW# zRgjz#&lSIA;;<*h-~QKV7L?A-j*0-P7bxs`SqX5zZ5+7!>%L$gG{R@+i^C+*UZhMp6dy-a@6qCN24baV8-H(F|yy7>(s3ed}R6k z!UnpBZeNQe$|?O2cHU~4?AFhSsV`Y?ZJjU}{)HB+oe_t1z-5U!>PIbI)p$eH3k%!i z*d(Z$({@HWiiX?(5L^wIZcZ{&>H?Lg1y_lE<0Jrw6myU8o1Z#Boo;qkWHei&K;P`T z_-YeXRO=ea1jzSqdI4YD&6dvFe(Aw3B~_5Y?>PLJ>GrYF zz(zPFKpX)j6BDfvHUw_3k2XcPu0G~AN3pI?xIrq999t|_&gEV@VQs(bNT`(uP5H$> z6WTJs;&BVc_sqlC8UgQ&#^@==GsJvheD_(fePMe{5cLC9&rFm!fI9C5*0DZrpho$2gN!cEzJio3+Uv5rFxmSxOfv)CPV7;-DWmgC$8P~o;joWTS z{*_uHWWlP^#1{>xf7|qPtxkD+SfBM&NK9+LbGDtT! zs)I=}&gyL3+`e$xLsu>V#k}}o$N95}-d7LYcDj+~y|v(ZI}m8kqu5a9Mwn!8dlTMr zu%kN$H5ShvcxmcSsF?s4H+QW$s&DOoQo{MIC0r)$;+bxqV&>6+eYP*xb+t^?mc8Rr z4Kj2k=6F0I`Ar)$AhOR8LIKcUZk<1uv9wkA5pDCgzBEO|8OzFoAFd@FJ}R2&em<`f zkmy?_(8K;TWm<+ePWHjv!Db9!SMGV&kUJD}5-oS%BHvmw4uIotR$UZ(*QUDBje@yj z^eoq3th&F2idamG=r7J~_t@j++Wk*^?00d!8>GnFO3*B8$Fnr?{BDg5l?BK77MTGThEF`oq68Lls^HH&O{fb6xs_3-_#|M<^8 ze*FE9A3y%{&p&?r?S1{b`~1_7U&r74cK?U_{P#cqx_*y;`0ai^`*+&@%fFP>17;6TQ-jD1COL9O{+7d^QNg_xJWCqm%ATDw#FzHXnk<@=>cu9t*8 z0(6s4CSU|>6o+{_Q+*!Uz!`V>m7)ZHFRd+Suo&wON zpr)fI&$>#Z;wlVU1SkI;wLPHJpryFN>IwXG@ns7oPW7co#H5#1T<0gfdWt`XL>_y!ma<*1kCgpR*K z=3BG4I0fi;k0kE^DD51(H4&>B04nSbGjGb3Uce(?MH2=J2Wt=XEMm^*Q!rHCR>A`% zR$EX)%NQuTT3m6%OB#gqrGR6LjQc}RXg`Q))e54p`cxIw79lN0aLAo_{pN5ib(VRd z9)wNnv_Ph2#M0@LG&%q(-qf?>nUt6i^2RhJOeFenIQNugaYPcXEAzCKjX|x#gPhU{ zU4gnU_Wt^SNUCR$89K3cx(hcs#nXqY1MrJ&;sB)n;$-y*)-Y+0wlHxF!Dqtz@VgD#|C8eurJrD0!D1hvwlG z(5+0qM>TE=W%I6X;h0k6dgUWlqj_`~rq4lv#e-Ouxd z<;H=h_C=YbnjbJGTN9f%K%p8#(RQj7Edvf_DIQ|ohuXvsxDIg#9%M>4%D zpm({VBqZO#bg1pATJD~_gY6!(VZJT{m0pX>X+GibH!Pk46|5AoMXX9d z_=nKL$!wvaGfD?wr??aM>5xAzybc|8BIxD%M6!Q?1z1RJ1R3hc5tF-9FnrLwB^#6c z@qvbucw(e@>d-xMc@J~A(+;XklohOpllpKTLcPJ$EfHm_vJ;J@)b?r+mP?TBYc5YD zff1_my2WUuAbM<08Nf&lI(+zO8|sCh9max|`Qjhqn)Fl< z7t7QwgxnLChX_jcRG%ogtmKVAOjw_rh%zBM)v_)wuHmb|AqT?j+HuAvcL-ec> z(;s+t5F|8w8g~0FF#$ISOBbA_7~`Y^gr$;6l_vz|n#oQWp{WTyd6DBC?{s^vPcJ(&7{ucOyH|ZN-n)pcTP`K-4+8$l8-QAI;ajv_5@{)WyXONCH}Xh(S{+ zqZ7Y8%olyPIsCcQPpzWxj3$G71BOrwEC*3F8>^VxXpVfS>L_?pm`S<7lNo5;F+i~l zjy}#=NR$I_%0|<@G6@eI}%e50wD1# zwnc~F$gRo76&1kBapE3giHN0puHi+1Drjg?TfCl)7sxzM9%B>BYFVsNMx$y;3hO6# zGl`!8{3IUkM_yKZ2K<;;idm7sGryN^I5MU*9v*WH=Z2q?Ir%PX9Wr*{J9JV`yfVwD z&Z4iDq^E0FZT%{WSF?<5cIp7N8pkY(8Ag zN#4Z>-cf=@WzN(Jf)rR%^Wh=wC`|Lx&A*{Gn4iH+ZRveQ;c3A7(Yj|oh>uq?qS*pX zFRaZJ(3;HyUr$|!%O{Xzq1?S@#@bnL#ZDB#`FfPf0k@Tw0wMESR!QB)^qrlt?>{l3 z*0YcG=H=b%4cB^tk;W6-hJP9E9&`v5AY)TPq2Id@`cbe?I6INJ1Dh0V*Zl*{6}+<` z3*XV)JY;B+Fcf8HEnga>%A^PBU|3VnFVWeR43|gqINx4v?e~Tq5@Oxj(W(R!CB{b7 zRqlZ}OnnyYxHqY4`0O^9f=Z3nh>}8`$Ey?*o}3jgGpA20)oXq4POZ;$3nT;_BLkI$ zXlEarsvbfQC>}4(YKP0Jee>n)PMH~Oe)lUaCd%Fk3>53J<-q8A--_|0)TZ~n!x2c= zC0^VzsLNx&`xvl0I@+Lot9+T0DuDik)d=tF^E$;R2=KHjYxwr2ww;^`l>!%)s@>GCzY+G=vkgFLo`c(}{?`x(lu5A~C?+OktlPE$@( z%TQa}aG`lCqpq z)oU(M8r1|U8piL+MF)DQ(`0QMXlsl$?2J_pJf*+pzbjx{!JWACFk_8SGPUahoeR*+ zCZTa7(jL$$AEwc{0dDmqI4(3?mXtY-@*$c|pPZ+15%FTPc+6>(ukG}iQ}GZlH*2~% zjnZpDm!5q3R4yW3uh3Z`O7C^L48X1V%k;S^Yg3++w|0-fV}bhzZwW*$F%VYxamV(@ zC0mnO8#TDKmi!bG`QS89!5~aokaE6|XWJA&Ze*a4vLNMx2+xiwfLzExYXIEM<39zE zjto=>zz;lulLH^okb!>6f|Q!8Jf)KZEqIWDGRlIKswF%ZrT|irfzHZ;l*+EDfOY7X zY@#ekxf{%LdGZmiM!)1}WkE^>g{No=Ao?X!l?5r~g(T4G_fGNtuPVsxwn5H%&81Wu zu0h8ck;lgpq@<2WrK<)@jl{aito{{I@M~j1r0zMXJ+^@&E?m`j9wjO$@fA{+E}}Ps zQBDfog9GI*q%J*4Z=DjFjsq1aq%ISpw@(38xrlZ7T!qv%<@6s@La{END5RRJ(ub#n zVqHGHkZPVw{|TUft&^)&(3evn*g;O@9=G|+atf2P)-YFlq;{VB+yA?ug8sIksD;#X z3cS5woR#QqrUD*hYbp{!(DoIsH@(>XsIdW&8FgHd1_Pyw+1N zld+ibjTGNNUfU^{Nm$I}Mv8AM@488u2kh~#J+6@wl*Q{g1rzVulNu>OqrBczF!8QE zp^*~o%j*l6f3-o%s!wTXHpp7WoJRR(OE;N>Z~lYDbIn0 zoZ4rsR2HQ0?4$=z3FJluDgs~!J#_@%i124 ze6F$}g_usyo_r?DScaTfP+>5J_I=h5^X0m}A7?`<#IEZe5 zw2~Sr>s)!&r(k+wF%udo>l%0$0_NZE_`^D0#5*o+PNVcpr>_9K$@WipNEpVlq!v;I z4e8O7Lx-#_t&1tho6?I>NHv^6A3%U|qeFQLsfLO4K>)S>-+k}5_0oflQR~Gsq@*_f zKaXdBbJJdeh19}Rd;*hFtJeib;!$aZ)Jw5^Fmst~tj8B&jg`NUdg>b=?^I*W!=v&R zQqMo*lLFMg77}GOD>c+YvRhe@GJ}U+34niH_k3#}A{x~OU@V&l=*ob%?)Q!V_{U?p zwVZX}V+&!G`3ve}Gp;TiG34-SEn28G&EdnMe(HUrz266yy}GjO=XTqxGYUH7{wJnv z5H+V!Xm|N8O+jW@JYisx8Rj$!zY<@?6y!BeRMrTnS*gXLj~S$w0&RTNfc*ETN_90W zwE>F-yvR?L-CTv#ybj(;pMJL}4_YkXMKp>I%7PTZO8TUM)E4D_bfBKHAcfzbUI;GS z?k~&04=#i7hn8wX77J3+|M!LP~ z2fKe39!fwySNEqhQo>lg@X_dRuIJ#vqF*@7^|6hV;Q72T*8Tg!YOt~#euT5Iint1? z`=ojAPQG3ntSrNyU}1?uYSv-iTT^4}v9R<)YL+1aHoFcB%U4J}IK*2wmhU z(=@$3HuR2D5heQk374J=sX_j8PMmq0b4%YwW43IRZQC!Oy=TPZc zSae}?8fD9T20RW?#-4^nmj(1(1~f7#^h_MOI>nquapPkYqs~kQ79Bjux`i`JQ0duN zbUr};z<@urAweU*l+H^qH_cKJ;n_{|9j>8&(6%!(*}b_tb9!6 z19VEz2%`-d4#tiOpK*Xr3HD@kAj82{MTP4F_)EsW$Z)VzQQ_PN=#-FUj1S0guu)Or z3IJZh_=pS#`xF&E?Esw;D#Q4U3Zr8t}M6&a4RL*)THgVBc! z$JwD`0REE!!zGl}vIu8~E&@x@ivdHXsg8?un=C!Nf)3sk+7ixy$;ebzD;?8ntt-%` zZ27=|&siuV$FeW#g5VgnXVic@ibBV-FY4f-!0jI6J}Mp0zRP^9rQ_KbaX>%C zs7Iya*%y9u8pU0d0e^}>n0HM3G7HdS8BY+$j%iaK2k4Y=MFPh1C`%E`zNi5Beu9BIaxD8Ib%0KZpc4G3a4h>` z0M_|C0=~A3GA&s4g$FFf)dYNp7YUBDLlpu12Em6sEjT-LI#`Md1c404*`YcBzK?*v zW=(ZmWZ~MEVmdgcPchztp`*~joFnHi#UO*q@bj0uGx?y7LdUW%#o#9}_b23&Itm@jz7&I>!90w}M|A`` zo_#3>KZSW*B%jq$=vZdG7?^d>nP?B|U@u|W7gbPVtO+rcDgfa)_GK1;-yx0y3}iS?aa00FDV>1dX-suyz&XdUFOmQrLcotUkl;A>MGwF` z2>3by5*)|A2m=#fOUR+lIhK9V0PqSz9vP0a&SekKDLd5&1yneeeK7?~k%yR#496Hb zn_F3s5*AFzBEvCuD0>Bfw-a-a;TSuVt)MJO3AZ8UBEvCuDBB&t%LzpQpQ@mz;o28J z;sBkp^$8gqq0q7Hi{JbMbc(wXT5<%&4kjJh9Qn-z^otDm8O2oQC=Jsbon7uj@tDbg zUsIsW3zmKHqaUDCJkrPqbrd?5ees(I=p*ElItm@jzW7NTpi?}($VYVqI-Y&;(*^X` zo9m~G>2_2wQyjG$;tb_H4V_(=zP-1oxswh;6-DBd`>4>iVF$&H4YLS zXNM91euB_Pofa(nVh)y~0AYv>$JwFt0elw$U-Uzo7MvaG3*a9K_(j!J1w9knz6b&f zFhT}LD0DpgvH%>UUTDb?7(1SQnGNW#2_bM#QN|8t9o-xmgM)M>Ap+=AnWIchbELb( zhvIpSd{9RjIi7u)3l8bIv~)j%Q!^0X+ci zVI8bTEc>zn)UPuH{E}j-Dgfa)#Zebj0Wo3~O8tUx9Qy)#FGR!=tC8V2#S!#ji0CEE zk>NPS5%gr($s{b1;W+jM^k>-lkbv)4Bi&FO`vQ73?9wOThs;QD9Qy+LHtae}z%QCn z;8^wr^l;ccjj%(Vb1eG;`Z?^5CpMtMvFr=z?GUk`>6>A)=muuj!)9IhK7{ z3YMZCu^xHkI6HI>fEN<>$Z(t;x&rz+?fxb~%&4;<2S$>0cuj%8nZ!P|_U z`_PgjFm^2a(hFW^^r8|*}7rfEvqeecfqtLPJOD}k((dQW2!#Y@xSoXyc z)UT(66G|0;aGc_307{H8;erguu`fFT{35Xh8IDsNc|iS=BDNyKaf%~oMzJe}@IZ#+ z*cZ^4V%HDC3mJ}MUqF+J-CGGCWH^p}0Szm5KPTWvV^bZaU@2nR7tp*S(v*OG!I0oM z>l`$)i2R3`;X z$Fncsbw+PBVhQTZ;Mo`OKBMLBgeEa;EhIK2l81Rg^p)mz$=Zu56~Xg!Ft59FY7`5`bC7HQ~?Ob zDeGFG#CQ`C$Z(wE7!2UAiCxHWoZ`q2>emV)5*dzTUn~H;lz?qMrYh@j&T;IEDmY5z zh*;#2s_20ut6$FVQ70DP24L>)Pnec7xmNQv?#l9Ay!>wGbQza>&p;aK(s zbnw`9g@{9jIIgf50M7oQxx=@ zso3@f#_2oB;0T3|XJ5eEjNTuSk|Q{FJo^G(XY_F(98qTm&%S{78GRlQn^Ea__65Aq z=&M6Ms-w(1o_zstH2R()pVd+5c=-!>rE!}$`LI5PIl7sOX^x_o`%t#Ulh5i1BgeEa z;FZR014s|+a6Mw#7Z*?k6baZTZHfblgyR%PQvlyjA8^pFXIR@9mBs!ivxP-I>u3CI);If)`IjZMjkR9 z!@)@N1A3S`;{+-l$HGXfLV6zKBr+Xiu}jYa^l*Z43YCsyVx$)XdT=M>7&0Aa?7V;; zvX%k+IsUDJ{r7suu`$w0kY35iM;<%+R^2Bp3Fx5<7zN05oV}_C>BktS0o`HpHZ3>z z;5wPC6wzfg%9e`^7~G-Yv8+tiVSsm=$$+i?QSew^CMz4_(-^RG7YZKB%w*+5{0IZK zI7Y!^xf$zNfZyuHfDMmP@K|=ndN0JkX6!^=Iaq$iIu+pES2A`Z;_(t0>mv|D>(2!gz{G z0LKn98dSOvpoiEqswUe_(f{-FyLPG#U3BHZl0XT%i`RtfD!ONK$3rIF_>7XUh3tjq_?JhJ|DtE!wKoknZGrof*geCy? zF1mmw6zF3Np$mZRi!Q(b0?ACq2(kclIp_i^P@tai3ta#h9drRHAdsTV7)2I+2*@f>VjBsz}sAp?6I#tURRj`M+meaLCXD^xm`^C1I!QN|l& zI?mG}4D3T=7)^ja<#zoY<)BwInAs<0ke$^2c4L2=pFNn?DtK$+GfLtJ*-8y<_;~da z#`Z}bWIHu90laz%ll!DbvZWfD0A9U>;eE0x*;Wlr0MGfr{66^-A&9<&@SG2f@KdA+ zVRQjJ=OYY?Pf9W&iY$QXd^DiIAR&$}faiQ4!cca{&}Oj`LxXnf_uj z6hNY5I3Fg=^ux|0CIY`Z0wX&gCXDryu9HpJrmP200ITRTVX~hrPc~xv+x+~tR_U15 zD#I)B86`QJ?8JsPe7t%I!~Nt@vKt$k09L(Z!hAo)hwR9PCV=I9m@wi`c}>hiI(6Y4 z#&SMPnDVDCC*U8OL%RYj=fi|Se`*OaANdOKoDUOb{b{oaHFN>IO#tKmv~&XgtxTjT z!gD@MnD{4eBR{uA6TrIy82Tr_CBL{u6TovmF!xVcNq%&TCV;n-Vf3F;N`85ZCV;n- zVfvpcJNd`Azfa)*v=T;jK4kj8b@H}tf6#H9k2>&GADlm2&=L4hvP}9lVw*4`7 z9Onab|ImNPwrzhqVg8`wI3F1Lhs~Ni8u){b<9uM^AC^W&14v`XaXv8a4;vuAklH04-ERlUmzv|u=gzZLow*ihYb2#$^LCq)&nSjS2D?<-;(U%h9-bl zGGWl4a*OQa_O~1G+gfE|TC0n$iO(ph^T=moVi|-A^`iLleMrJ}~7^qY$P@ z$2z>jc+Ll={AoLgrRV~9&IbnlY43>T$O4$o2WI{0X2eQ#0lZBB=%30(eyocofVYid?w{&Uey@urfaiQ*^q(zdB?Xu#*X8BVD2CG zhV1zEw*%l0I*#*!p?~;FG8#al<2WCf_=jJeJQw(5>^RN`#{Cg<$Xozr>{!kRX8jS_ zWGH|{$64<%=#LmfOax%7U_9pogZ@+o+4^nDdH@CRiar_ihmq~y&;;;GCJg#hJINMs zXaZOzlg+(~Pmq#kOSXag+a3OGtWe0Tw~f_+KsufL+80d#%lWXk zLxEuOgWtc^iQlIfXkChN60oHKJ?!S>&Eo!`V>ll+6VSsI5%VfAb`0mkHiGp1lQ)a| zW9%5thaCy%5md5S9MZgFI3G3_r2CW601_R;`LLHkdNY{|Aki_L4_hA4cUnvy3jFuH z<2WC-8>HVNLjjbr<2WC-7@+S`LQDi;BVa7&!(Imj(zM8~aZ}accLcgt5CZ@I8e4np?nqf!4 zZ&A^PkLP@R!SFNg67XwOGyy#4GZP5xQ77Q{s7L~s&Sw)8I7YxPQqctPHi01!$mAp7 zH>pShn9gSx5J>w(1R+@~u#GXD4;=`kZy-X@1@N}RoR0*chqsfF6%rlC`D_662phz_3XC1c`RGD=`Q&}( z{+JmY=Mx6#J5|X(b4W9T<9vi5J(r9Ikmxwh#~RRg@sha!5*^3+s6%=v8494#v7C<| zq<2o92>kb5#d1C}fWCVzVj=)mB&PFm1_J3$WW%{B>j4zNEBYo-;33&`4ov{BWJUvl z3_Y^(9GU=L$)rMoGi38QGy%MlxfBTO5hWYYp$Xu{K69WzG?5ODt-tMn-_|Mv(^@Tl z%vV6&^M%-hbST0lgy(!j!SFLT5?ROsn9gS{6u3|9MHj%^1e!n~YZ0*zSpd`d1OkB! zaUug<0PhMIP#~7bL>Iu@#%rL!SF#rv(k8%kJ}N+9&nB`T7@7dyHuix6wPa7QzwNT$ zCjsaTisO8w0DUL_3p2v@cRhZtqN&=FrAMv5Xfc{uwfOF0H*Va0s?!Ci8IJkgz0=} zP~ZY_7F_^u8!ra}nUcggbOF3=JQoTi5*N?~@V4<*C@?@=0s?>A1phhTuVMD{VT%L$ zE*&zmLZah1pY@P_8ZEDaW5;nmT7bS=l46AbAwWGi|kI*#+&Z|6g^uNXwgGyS9+?Or(Q%y5EJ!aziv!?_#B@HJfxwp}BF_3xpqNyjig()^JAn(Sde<%xpd&PQ4m2qbkd9wU_lD1cQlrDp+w zWE(~Ux&T(ilwJ%4?l7LA3t&}DXjPH2|VMICkF+B81K=i2x}YXi9mr? zMl-qq);7*t2n13s8Lg8oq5mAfi!dXfJU&1Vc!TuUOh`J0@ySyL^uU!27#aR;`TsF? z4C9kG1JbWDU|xts$1py5`hXrZhXKPvBsxaqlgAC|*$kKzBGECNPo4s#k1>#9Lbzry zoKM~~Ko8!=C`Mh8IL;?e8`7K5nu>$5<2au@0YDEiBb$m(Rng$g;6*-pYCs@iJz4S5 z1hAY>9`wu!RcJ+@yaFuelV=33AaMbs4W*)y0(d2p3kn=1TZo}e5njoZ2LegFWEU|s z0W9Z}Ck6#V$VOsl0$9!`ZxIw|CwqyZ31B&&JOT(LTcb4_hc_9|`Q*)q0%d5;#z_IZ zeJ~RUq$p3`RO~<2Y7uT|wcacK86}ay=tkZIn9hd|hMyS5_=GNiw~gmPflkI3bOF3= zECB?PY#CqC1@N}9E)*zd^Z|h>&q&P3Cr=2_gQ!S<@I(*Nah#7jr28{qWQa6&9Ookg z>CFt77b4MdoR0~h2U{>;ScpW&aXwT?zr}z_Arc+O`OE?I5G4juObG8Pj`IUd5M!0-w>k-jf1& zCDQ;1BsrjUy(b0mN+u5!xQEvDo)o|P9aDr?kh9-dLd~|?7 z3P0IP3{3#f`3ORR2(q0Rk^rXjSpWsP(3*|In~dpvW&?rLb!g4Tp#Wa&lV=PBlC%iQ zWM%HRwZcuUf?eaEQIbv)Tu8nF6e3LL19SgmL4q4y0B;+^=s$TEK}Q$B+r}{cPwpo8 zCc6Q@a}&%K&UrBX4^}0E;VDmGK*w=DF!~S9B~xJ}I*#*!xqk>R83`lNahwkf{X;^@ zJQ#_N<9uM^AJRE_82raw#c@6`?hjo{<^o7#$8kO|>kq9YLje>zmh*u@f7k-Frs7~l zVmTj}@`oKmj00f*U91}*O86H&;+oY50eb~b;w>~XaZQy zhY7R()Kh4k#^Ds^aFk$GQ>_kW*o$O#0 z;l(~onENL`BxX%^vj4VL*v;78_6u@#mOfuy+LF;-?3Sc=OrXhS+@G-(ta-#EM*2Wsc z*KHnsTQ$svzPVpsGL-IqdBxDfq*pN$rE8OV?CYH~dtTK$C-mslJ7@GKY_V^!a>r{_t1OMK6Q`j+&bf9hM3dv?^fWc7TlZ;9)fUEh+{ zbG*JKsmH0lC9|iwz9pf@xV|N$r@X!;r6;PsCA(*=-Xyk%R&SEplTdGx*t5LeWKT~` zy-9q}+nkLhl=epLD80OVD9zpFaL<>VsHjpKc@Et@-*TvQ zB80Qbz4b`X`nn26lb#~S(>>pO=t+GAPt#2W&Ib3^qdo8UMr|&Y+#fZwsc3)H;!=N` zq1kmN2|Zf%6?2=yNmJ(uHFDr++&UKcIjWFyb1cQ;MyDcU!tU7Bw_q*WI^uj#2gr^($krKfw}m{DoI z%@BWG^qeLQdCqygX)UWHb6&W|B=>E=p zjj0W>mk#+aiyjSc}}al3w6LH zTW;rC_o1MirblL;mql{R-Q0)FbDC7mJTHsomixL7`Q|iTH1oVHnp^JaKC~&PX`7kn zW%1l{Klh8JpDy-%eS}>t;%VdW#;KGmRs)QKIECxbj-}tUo^Me!+mH&PSa*H zPk-^;@*VC&VL46j%{===a?7{64_W6lEjIJ)6U!~%=04tmhl)L-T+MFgc zGf!FZ+^FsDLxDL>wPqVWOn|B~!F(7lA`&t?Ij}`igqwTXd{xz}$9FtxyjHNt&OPCw z-4LBuJvp6=IQX@ExbfkT%%-el51;o#*RC~Qdil^)rtEb0?%`XU@~2~Tfwl)cQvDNp zCIR^(EE@rKwjev3pPentR^sVZ;_lr~8&-+qysV1a)MjEFkT1%5D!}&Q>AgxD7N-sG z=NjHm8#ajJu&bh++e}OYTtrwJ0&E)w+eVNr$cYhs-m3QDog@eMOgU~tb7?(z9=hSisdiBeoC;P3bCIu*iQx7Px;w;JiW(g z!wcd#udBvJtH$E1#;U8LHn&BuZHo?Ut1u2|6=n4aur(NL4MBD|Pj4A*Sdyn#Lx}An z#&VHhRcZ&sIJS*{s~Vdg$5EvXJMi>g7Gk@Ivnq81Vw~E>e^reM#&KrRhV^)Q{e{@A zVysG?fSAo~<3m+rv*S36X~W?>y?sLLR&iFPUO5S7Ut=d z6=Iu;v0S8BG1>tW8{5VQs>a0QI7+l(8=l^LA-1VFD@He9g4s6yvucb#jw4AM*5K)N z5n@-0v0SFJTxPRkbOI(ewMARCMek^fc4~``Y8(GiH8v}bqemO|=jqks?rjxfSBkS* zXRu=Q0w$c>qSv=Y2e*xnRgLk+afE5ZvOK+}LhKkZ)`WIIrDDK@W83&()tF=)M}s!( z!qZzR#EubXP3Q(pIJJ#)s>X!lII^_i&9q@tp57QCwm(1HUzj~1#+uLxnAqGFZPpg; z)iyp{HC9|TCL6~wr47gM^oH~H4s-Vo(|ad`*c0Nc3B3SWTUOEV;Ba-6YrDz009huh z&z>c$!cLLs-7VQ$LkzbkaZ;4;abUF z8)bI>MwTp-WqLj!Ca8VC#|D9-d`O5htpXe?6>6PlDJc2o2zVA)@lY=O7qt~2n=9K3)j{%0+!|RDLq&d zur5zPsa7SxJdY@P;1jSZPpGI?Dqz(pe?y{Hr<%TML-N&5X?>-Jq~J~?eT{~c!A>rg zoO&XEr#wqpJvpOOjHRHSWYM{ZrKXoX+#=WtNJFHn+z42uoeyqj4 zal1R5S$e$*oQ`OgsZ1R1!8=tO)` zhfjpzi8!wg#|YgM3Edq#BTSrQB|BC|XgbFq?(m2(aE^26u#eDjPI%K18e!ZLtJh%_ zq16(9vtviZ;+DAZ4yOpcmW1Jss0dS&SmBQ4BMVL9vpcqq=$ph%+>2@7KQhfE^J&}F z5owcbjkb&tvGG|h4;=dKBL!V*_4=(M`CA`^_d7)jx7G^xFCXPIePGkSepJA;R-@ly zl&E~*-@kcOsIpeJ-)xi-^T4IwVN@`t*0eu{(?r!@C={R2;i|7M6t}#?R$p5vp{651 z-zY0~ZpUizr$VMz&dVyhn>ET zb;65|5Pjo@Sgj71fO&aRUUwY>b@OI-R~ZDV=gsQAyED)vPgb&uCvXmPhUDFqftpO| z!&QobD$JRO?|K9pFl8L7rUxoAr5x_s2kJ0qzp2s*T)>?5=5A=9F;iBrN-%IPd&We1 zOig9yJeG=I^07|S0DiXE&FX&wXR#%2)_e`*Ws8Pa9}kpdPYj8Ua={``AI{XUWO{QKSgJ0qzs_YU{B zjx4H(zud8H#IPdHzhm==Zbd?0$LO zpFN|w<=FL^xGK2`D3y6W^D zzSeUN1beqg}q8v;N1!b)}8l z&Q5>P{ZG-+PwPr@W=5nqHQxw+y}@eRj92G2Z{u0yd3v_LzycQq#}a{+4J#6SHCUe= zj}$l!xxYE}?y%Q3haGn;a$Z#!t}pxc%l%FI=g(KKt1fPuzRsyL->DK-Yxdixe#aI{ z^Y&Z5yKwxh@p7B@Dn14Fo6qwue!vjnU8tjSt5D?%L*$U!L2;QgDYJhWE$+>s|4g{% z{QCHzw~aP+dM3Z3k_P(DJok=N`x3Ra%%o?i@8Pd2yEl9p^qYt}-X;9v*s*%~r=wl+ zCk8Z%a;_g0(yFV;x^E`Y7g^)@ma*u_(M0N}H37%(2?!eho8vgkq@^>$sH%HySV8#0 z4CNsix8s(zR=Y*>|(J}zv&hlSu_(#{Vn@%OuNdjWFDlY!lHkrdhqF= z^Qr3}`u=6e*!19!i~ z`j1mKOYlIT;M*zBru~G=v0L2>PZ@G$UD0*7$l3X# zp{x5~#;nHm9VSxC-jr|Mc2nSFiL0y(M|x{t+`*Kg_ZK248pfxxU-nO%xp#F4_w$$i zm$X05*<$?HXA-fp|l?(=ZIe~ugx zd0B5P$G+vkweX*&M&y#~IdV&=r!F-4GWGqDx@TI-rFUaF^OjJX*he`n3R=C3o7jhW z*Pg73JF%)@nQ0cMLB59e_H@veex zY8HNR`t(7s#v@kiZd+@x$E8(KG-kL|$!qm5!DG+#Ke+1lX(#%}ektPkp6N_6yyw_F zvH8~ch*q)bxG(En*$T(kS5{?d>K|>bZH@8xb-N;H?58AaykD!>bkv-+Y=hoxVI32L zjcJ#Pqi8>dm%A9>+-Y#GkMq!7aA;QBFO}BTj%)+I(p@8h0nHxY0@l~{CR`ZXMylyM%O?8>?P7EK!_Bp<6Ck9#zb^ zuli}t4mc_NSm~kytxVh|m-GFDvF6{`O$7fc@ z7N1vYr734-*-taEx823#BU$g-;nVnl%hTK3ZbM=c_wmR73@kXe`@CQh*Zf?fYDuaK zN64&__xAi{;%jqr>9Xf~-@iMhIOCyqNcdUz>y?5|#})4-Ma(NdZBnCrCT!;eb+?R- z3_ksTtColr>|DF=p4p1g2&HDju<|b|zTCDSM%#>14bt{XoRQzmaoRI>c$sKk{l@$i+deVZ~*$zAFBm-4i# zd*AjP3By~gL93%{A}#NoTCeNpqk8w|5w+7=i}J$Shw^-ydf&G(+)h2%Sl0K=Jjk)- zd&yIueUI}kPpr3GeckATO0$ef5`2Iotq;)_!lw)PvnNh zEnwI<*-M@7S-t+4>tP{Ny*h89BR$SxpB6YOeTiFd-*0YeZ*?RT9%faS z8g_`}7Hq3ZI_*1`7Nl}J^@-woU6lcc^oq4S}ul;)Ow*v&C~XpFFulE98H6%;$L+h0o$O@9`V7 zGDX$!H~;={N)eP*`sG>Fay~%BB~||MVD|>b)knAXKRU~+96D;rI%%Y*#LHh~_A24M za}uBEjG6oDTzf9hJf|o4*|+o6>CBPjc8A65WE_u5swdhuw#m5F%BVb9^FIIJo~thn z^;Qi3|%rNKiG1$KI)g4UO|-Wto*fnH`b*d`bY15*U8b1V{>>}t~!37==!?H z=QUREcz0*O)1X5jq?Mp8M)jb%=nm3EKOSaU&*SGQ* zH~Z?ku}=?=v3p;)lvgc=kwhigBVkwMovh%2a%Q{+9=rGLC;ecjVxx z!xnERk;8tk@6e=zBRdL)xOPO$W>~J{w>}@c<*ohm>qjizeT$0^-I>ofPtEq0r(vf1 zHJ=xAca?RQ4J$f{%1fUjh8>0WW^CK0e^aiBI+)S&;f>PQm4&v0ULl^J%UN|ihwGM{lglqaW0O>BqT)426^ zJV|AmGj`UkO_5S8xmgppjMAxhXhb=!xoG8MYUO;$ExIi)ELbQQAAZ z-TUpF{qxSp=RVtUh%uJ8RRA+yAD@W=i=+5E1B9)-s_nzi#>owur6uehq? zY9W?*--bvHyr7BaY&gWsOgd9$RPV zJh6-1=GpFhE1s?B8Lrg5tQmcG+u6$d+xEEZe^WJ) z`9`hM`Lcn^*TZ+_aTQb3e-z)heH`?$cHUdFXWVNqsPIS~U2!?5I^qY_Ma+vTEFDR& zzE}}<=2DJt-|(6{sU>xM*SdTq>(`EqE}3T+e9%##{#ScIaEU~epn#30>F1W`Au}Yr zXYD%{mn8Bqu;yROK(3YNY;Lvb2X~Y*t=s0`P7yk?*L>PMWtUY2qS?!fB)@36r?k*h zFI9RGS4C5be{nw#dQj)PLAc=7rNWgD49`{UDzB1W^YEMPi)YkD*XG5pO_z&MdldPP z>(FpX*DTK^!aj{OIi)tUwMj)cCGMWI6;-K~G^%A-PHUGtbw7Hw*LdzHbrDmwpsodR zHg}p|?D$p6Z}MZl@&eg=UFrv3aC_>Vq)r&wmGka6VAgEa^32mDQO&UZeC&>2Y1^7s z+Y0#{R5U zc}jLG-*$VH|J*>&v0PlmT6})-o->?X9a~C`Pg67HYwsV=q~mRv z&hsVII;~cx&0wg;*Q>d%<5HS=_2qQ6=Sx3*igfOdZ$B*jyyM^YdX=2md9u&Xo^AGs z6O>?QE&KjTW^M56vcwsY4^(@GR6OMz8WS20rd_-LxP5uvRtr1#Chf1sk~n;|AEo70 z9BP!uj_eygyrRGT=jK7J$ZLum)^BRPME_&a zj3Zk!bb_L{Y&u)<;r`>L3HjcV<>A|>TdCM49;4?3PRlt^A+BiaS0*h%ZP;t?n5wiS z?XHyXE!m|bJn#Jv&AX<4T-Ct4>+9U@tj^*UM;248cM9HA-8W)+<5n5v$SsFur3rQ1 zo`rGN-gSb<+;=Y(+imptWdGK9b*pXpzpg*&ialOWjf>wX&gHT{FsbCAbFHrkcboOg z6w6lGg0a<6>$e{?w-B?E-y$I~)?KJ0Uy`_dRbGx%naa}g8_9O8f|tkCQh%7u5LqVG zD5iF@y^NOS#A+W~!(MXeBtb)~}7!9=@6xZ4)5V$FiOio;B9;-idIdCPg#i>0L^na}Q+3a{MU zI%7rQJ^G17&*a<3&ai&&ciw8#d!tD5;Pv>gUB8mf?meOK^{VQY(XhKE7mOYDC@eWu z?RH%^^>|5G(41I9=W4yV(;|dIHm%Q}d!~d;R@OxOwzE zeqGY|#`&zUVt-u0S_}URT>>Wy2#NguBNZYs*`K1E*D#j}+pIrXx$8;gSp1vOPhQT0 zMH3S)gE74^Pbz63oK4Kt6T0_q*O|+dOgY5_$AnX&GoCG`5_M;Xt!zBMpRnpXq?L1W zx=LQ5K;K72o46IR`cZ~*&-8ydMBNt>JN3wQ>rsE5u%&5Z@#igObVTh|`LXq?;pk`y zPyU7%2h?thd%sberF`SMjom8VftTF6I-8_ZKE1KF=_jJppE&pwzBpDkuhD#e@VpHP zr305ML?csFh}&|V9*29Y99`ZYw>%-|`T6k2r|X-23Y5!41$nIYB|VcrhsmV3PrMlXgnio%8`z{ssDYaK) zRe06+{%`L;*3E0S&bv?}cRC@bciH>x*WVf)5lq)MOo@F|ckN@aT!G!O;uUhOqh?&a zPqezYPIo<>Mew|A=$%DXf4#@TGI_!4B?{x>%hwr;Ec7R4t&sRA6WD&K>}?`5U0rcD4)gLi8`{?&Ze=Q7th(WN<4Y7VnP0y2Yy~Hg+U@N z>GF>U-#GJUJi2xDk*AXKkI`kUlcm=rdHD~U>70JwTnNl(PMx>RQkr&|(2qPXnE3B_?IweB( zZ2zJ)|L*Jg&#yf3E0ea%J7y|Dk@Wj8*O*f4IcOOjc$$?$?=QAmY=3pmY~e`op{i@&;vLQ0KD`)9_SPKPpV~S9r<}z8ff zu}kNjZvUp2AE)g~JX{`fUgh|(VvdZOSYJm~Tre}GuuN;l&iR4axzhteI@6X>I&ZoR ztQTGDa(Ho}_##Q4bD9r56kgS?{-P(o%+Kz*R^518gppjvyz~2VpGW0oW+n#d-dpsK z-H4FPQqSSc49kXDu7#nek9S{HkZ^6TT$Yy>=6^R!fp73!^2%vW5{h@0ud!F-S5sFE zUt;8QWsAr)P5Y|5CerPX`6(A?oxOhSiV*M8$n#$l2VdSQr}e#h95S8B(HW0+^@!1} z=2~4vopy1R6`#E`g@x#^p#?0Nnsf27^`2h-`?9v@APed5jgbR=x= zUWNXs`lXhQTH_`a6W=r@Mr>k6C%(i~3|CH!$;ON}msLEHjp4|;ObnQgmrfiT9yg7d zko}VXX5!bQ=<*bocjJSmhadR|+$fZ~I5C!;^lilL=4|$zM;(bl=gyJ{R)drsw{t_~xy3fVu5Q+CMpzg|0(7Iy)^? zq8rqGi|B(@DMozR%vTYqo9YWs+J;Isb$s%-1kc7u``}hY!)`%^a@5ZNnS0C?EBM#tUS{^^kIbX@ikk1KsIl$& z@+{w}+mSKPZ+M}6JN z{vYFY2YmbH+sNJ5pY6xE8RQ+iRwCo7lyr0Kmbe>p3THd=6-euuUlvfZP2ANL-}yL6 z)m$gx*vAue*QGPpZaNuMxqfM+b%JxH|J$qtL6M*RYSTRD3ONa@oRZv=t9MCssl}_+ z7vq1}1`DrBGuU@PGN<_TT=&mA_)bq-+Q0DxXIH)S3zp>xV|5>%?k(=|F3VdcNp)?LuKYkaP9%IF5gJ*@LtCw3Oan;#ftG1Xbr zn-YEx4=$BmXZa?4PxiJ)O>g2IWd|~!oGb~aww zG-Gu7Lc>QxqD#bXZ>wi*7_*m{H{o{b%z>sk-6_#E_wTRdhmQh-0ml7CrE}_+ZVU~H zSZ4h&=eFqWeL;fGhl^A0(B4Xig{6H^n&!AODP25$N%VSOEzwnFm%bQnn(2QkUeSkp z&ZWrp;!2L^n(dkcKgaHDjS&fSoDu(ATHx@;fbxax9@P;OMh}A7;1X z_LzNAubn%yHY;QCV%o#mk#Eah#cYk?6mxDEoSDBzCVXxZm#e}hLgH$?4-eD3dR;i{ zfs%*&qfi&s9l z8$vn0Sn`v`)fKXVYbs7%P(I{gs%CUgf0d-jnp=jeq`LF0^JNN^wHEqI@KUexWpMih zhwe9T=1h|;vDvZfOjnB0!#K&OM&Dg@<)pt)rv>+>S*ugec5f=Vw1Ikb)UxbjugmKa~rE*NfFr@E!6M}yU# zRVKQ&ouc1$on2slDzDcfhPH5Ud!l>F6&^KTey3*}kLcD5#VZI!99aI`B2nGy%jM`D z+l;q(g)KR$@P7aeK=Qw~l)$(2w?WuOZ@+pJ1mH=vk3v5$*uHSzmV2$yl0=h8lt<<8 zHm)B6OE_dEIhnJ&8a7*JN~Ie~gT;c_a7G~^R1qz~0vb&{aF#O&W8e7dUXRV$CF8~( z@B@jhCs6cxi5Sy@R`9E>0k4#&!$(3e9smh%@FaRo8c~Y(XqfiUhe7IaP9@y(Nx#@t z=u9NC(^`l?2|Yi?Xb$Sm>%1yOGqRqyFxtk>AQ~mM@SKBR5x>mn)9a~ncR?D4Ag9~9FfJ$`}o>=rO=L#-@}&qk<3W? z;Vq%SqMsrrLDbPg0x=Xml;yL&$RC@N5%HKp+=ssBY#^#3-@+%7=r9fU;9k;@rn95e zLA$|jkojxA`M>`S-~8Ku;+uc_FZl-klW(9u`37(p9{$YGq$*!R^+HuvIQ9NX)7{xw zw5f_T|B?ZrA|$Vm;#ik$#rUQ^zlXzodtxfB6O-E3VDi0F|hsF!o z2}jYJqJh)^@F$lWvHR*R>jF3ED|Zr)VE?&YTFA!-Dem}j#OKh%9FhBWxq61CILyr7 zg4`S#INTeAOt1l;$?Le>59SFPQb2P35d1)qzv6vpTioRu%Rs0hmJ)1yUM>Y=XRA#O zvj)ltyhPae=v_%3W=_oR+v*DH)3!roTP4+FpQ_{Bm41!VdQW+=H!>if+1-}=uiTBev?0R8M@L564s5g@aGXBRA zNCpbi*6m7N)umxROE<`S-{;duI5^-2eyO%Jo3Q~tJ(qVt9IDlR%C7J@(mAzU{N^gz zQ~cQDF1a)D*>G@XAFB4oVkW(^c2h1kQOh^IPZkV^9+;D&3lcuvg>u+G{)ktQ8l~Sm zC{lzXkD5ZX1&riGS)WCRT*QIvK_gVU@~}>Ih$*~y>zC4kB8<9--koUztNl$6t1@F` zXHWp(_^c>>J!Ow+*@3s$P>r-0BTyuU$kZ}yY!o=ClusxL0PkCwD)53B=1cG)o4Gjr z1yohF66UMQz*1C6O?ERNcUTq0vHspTyWm?tIIjM8Wf5h>X~% zY$~CiF+rqLa6w>~=MAV&h?8Ny=Wa4Vfmov;wbSH8Q0=sxdUVeN8g;i4IWeFZY5<=K zX1E~oyNGW{Mp7~)gB9GFQ1H@qv88gu=$+=lj34yc2_Uy5ktz4oN4n}MBW4&tzY5qt zPqc91xP$U7sunxZUsW5Oyb-)>AUrpE*X4e;4utZo;3GK{!qo$E zPN9+)alQ@86-8mdppe;%#%G&hnWn2Xhhm=#%D6L*M@Y)S0m3=~iKI~%(3Ppg)V3^T zF;n0Ap;e0*ja7K+@*GLz{AiLxlEp_Y2=Y-(~Cq5k9 z(0>mTljd=qkbJ8UD#tB|>{-rdti-pi1Y;;5cR4wDqoU0LH(a;K#>#yyW)*ty3pZS4 zJWe8}U2=g$e1;tAf;e0nsBxRIl?tWY$~66Kjyb?=oc8n>(=1GR>O&)7#1NY^$Kf1* z#d9o$oUn!h@ap2D=nqp-FDFw~GB2k_cfq&OjrhIY50gSXh7j0>@#bY&g@z@*e#QyqRV*?d=6-g{yzoN65O?qQW3&Ms^<~rQ#ngVORI<(iKGP7y7YqfYs)=sLsPyWZNqL0V42K&)Ki9?+F$t?K0k2^ZZ1tMr2VxZ*cfc1EpmORa3`qZe@AvtGR7eyy{onmDj z2EWXOe@R!k(64eWD;?pdDl54iZnf;wX@k*90B{hW2M~b`5Z)Y%Zd*KG=wbbZiwNxK zft7HtAA|JQob#{$@LzMz@(+K_Ie+f|yZ1N}w|9vr%d_DB951LLemy+Kk^KHUIlkU> zRs0vuLDx0iPjvJV7=iPT|Hb5RVr1PB_$S*>39Q&!k(LAEEXI}_A-4ysNBCD~LkSOH z()xigy-qsBB#0!41HFAIigqxVfDh$=0INv>{&Y}W$U>?*?EFDghx(0aXDdQVBO*qC zJzIF;W-lC$_v!)#&EacZxj#WzQ)71G2p$%jN-e*n5WEZQzl2uTv8kkmK+7<3{8p(F zLY(m~u>sZO?tczPA}d86OgL<2DE-iiDORKkpb|kpKg_At%x(SoGiwI!pPL;sGJ`t3@$aroo$m)mv0vn%Za z8M+aN&V}lr0W?X>ak3)~0C2~^srZfyDe|2N*8FI4&Q+l>uScnCSc0JNEgW`|#!VPl zlRub=K=dn0ZMz>WBv2n3gi#)?zM#JpDF8krZ0o%iPCK%|an3v z%TD;*3M8leDaD#-4j90IM5*JYl9%UL!MG$=|G5bIX>&rX3L1zg%Hx5Bx@>8jlow1m zK#N*)2`TTIb=4X~*+S)eK!g4DR!NhI<~Zqu?E52x`-nuacs1#I##)Y$vqosH;3{ZH zx0D1FC@3}X&p0|ur%&GA3Um@sVcQv{S){j3&I5G#6vuBeP2Ht?>=QsYxD24Y zkri@3DBSA6(cJL(NVz|%wD?7?Fn@=e>a3Hc_*6!IEy)D=%3^kIkc<&{D;ShIn<+a`ZaymJ*SPRSZ@k9|F3!5!_t-Q2M&9rWEz{O~GbV2Ul@mdY=*=bzJ;t@79 zQezc|&ac!nJ_mM~BoEE}1EX+*sYv+-oEQ%G#q60nEk^3$FX%w}-q4$oE-Fq4HZJ9a z&b?fgfRpp*e|PuYwnU+`Fb$ePSm>`8h?UY41>R#@r(w$qgLsDm9)uQVK~@9D1q1eT zsmLDHi7uqK(u+wngv`>m3oog4xDc`ZMQV;W$W}L`wnOZZ9VLT{4`Rej0Yk!Zr8~)6H{1!^B+_O z;r{^u0RR8Q*~bpF+L|8VZOOK}Czdb4>X9u5R+TEj;{Bd)m2}f1Z`X?@AgHAQCySm;W9Rq+^Ubz|D0XZzltkZp^4_-0d}l6Yy)h*cjC%9_-8_gDch@gpwHv zR9#~cnKbZfgC3F#puew_Fs0yorU1S-8(I>zk>G8!zjn3;yA-xcc=xqnE`pezag#ue znNd{Ox9CCriq4eiUeiy;6~SvQ%oOucKv5C_(eXq&+|y{%wc>&SMw4o%E%2{Zum{mv z#gTq)G?x(wOEYzc@p#HZ5A7RE)~U;w3JvvCWgfx2CIB>2aNH~a8r|!7Rzr5OhD@cM z%_HAXV{TenGR_E7iGIv864?bMJRfvsOlsf=n@2uCE2O4O\B8m&K=0TYxAP9tOZ zu9hmp3_u?grenfoQB%-y-8tZu&JOpc~hi{7EtyQS@&Ha*DDychx< zuXsug5^Od5bt1kgf_ydwSVY;N0m#^{N2~>~mq8rI_G-@5_d3jr2U)z{4$@fxoX2F*WZ?#o+xCIl+LlL$q2xwu4s3}9fB|#c)Z{gz zUf=wrHf)qo@(x=lRi;EhrIo(-wbHYxv;DW$VO)hoS8sWl2@-Yag894v;SJnIYKH~k zMw>jm3rdZnrUsjHcT=h$qYHrt1TD{{j*6e7^%!`&~je}*@wN}b;`ZJPS z5ZLRSO0`|Z^tR)?mrUDLSjcixlFSi|Hf|-q>A4>{MGWr{s*H;HU@Fm3t@sAdVl?*` zYz`aO-Si(DMAs{aDqz` zkI1jG6l0!1D!cz^-{55fXJzOO>}Ic!AeO69=?3tZ??@2%=#zE_*f&r@0UJ;l z`DuyTNnnNjx_1^7`}a+tdQR7Z)nV@Zov#Li^?)6a%YAZYT4~G>DTm8xoO5VZk>A)F zmYShjhz598HBn}~T)b@H`&lCO2xcur8hw;tb>5S_8FlGZ`$#9c_!KEGE6z?s5l~%3 z)NV70#j&g1s>=e|r=2Dj?}SnRTsUwjArY*}LoPmWh2t}naD8iB&|NJvJ}&({hwHXm z6FngYLKby2dq_${eCh&t)Apr2jo-XsY8v7S`9maMu^M))uK=@~O2Otsot7 z@9LVdq{Nx&{G^;9BkxKKuf1SB{6JMKj(Ia}-DcUit^vlYm_v(NZ@CyO6PC~q^aYKV z=wTOT<^q*@9+!QTuH-h+d;-dZw@#l9Q^T$SSLVh)t=DuW`&t_z?7mk5txMRed~kSe z8jjq&G}X7%^fCE%1Tn*GTz4^UNPCcWH_oam#$#~cW zSie9PgjV*M@mx>H8m%h7m+j27YTa8EYbZxIrnS`mlkgCkSVIY?5a;b?ykZSqg;t`E z*5(rC8Y42eq{D~7hlTgVG(rOw_K+wufh!BG%v(kJV29>qh)$kH9ugt?dfm?(% zDHh0e01VeNrb50VwvDu21nr^xTUa-@LsJgT+hZ=BvEM($Eu=Mq1u$Qr>3p?JZ7uG- z7HRXn;h}f?{YZvu5qr|@5mrM4!h~=d5TC;3<|6eWj%NLeGhA#^Gj~P}peZR9LgLXS zy^!ZiFLiyTAXIh+OWEH<1u2*75ehUc|n2rY^+#oHOua zkvkg><_oDB?OQudbzSJIYoCM!dA4i4jF+OY)L%{64~CY|#ZZye#t!8Yq7wDR`>5hK zhRP^&`XbU{R{hA`baOa|9Yzh2DPv_GJ|bJ0v}L*t+^7ax*aXS@N5pv&26!jctm;#d zs7y2P9_Y}K5GXB#5XBb?H1$@==D)^yY`Y{IQ3WCFLg-gi)!PS*!d4-H3L0Ql)hPb5 zZTt>R`4t5hxi&8ZELf|AtgdO+S6)y(rVCUX?j+QYEQPPerv)Q73bq$HixJm=IvzQY zId3A{ zQ{Q^bQYthurZ^c;R!7Nh&mQW0IeH@W^-L@6ZedAq_Q-&X9kgjk^?l%1`~1Ih&+Grf zJ?7tVkNWSq=hNAGH_C?(E<1dsgQ$SLzjOc|#S}PFLkcxIdGu)R7_XU1j*+iGCMvrD zn~@z~*24G}MNW&~1UQJ|T=&Gy5U7fj0Uh1;K&&PMyr!Kb`KfiJ9QzI?#Q>uo!~3a5 zQ7LRjI9*m)Xyl%=+3mw^b3s_c`hp^JSZf`1YW`ic&!_?ti9M!2BHAfVB1mlg{B}#i{^6R>t9d;WX ziIRI4!4AW=rAn;R=5KzS(DqIR-L`vrHooX>lXYKZNe&y4oafG%$!ZVi=vx$wv)gNO zEDM#-4qkeopy!2gu!Xi2_Y3W2Ie{%o7kIk0VkprJhf+VI%cg33?Z|jw9Yq;ObS}WD z+0pB(H0vA86n=gW@(T9_&Ilk+b-=gM=I^?$Y=CPPu2qpB-Oa{XUlyMIerRN4EiNF{ zRKKQC2_l>Z*giXP*S?cWS*9q~v~_6z8E014Zm-iMzZ>848%pI4JwSWMDViEF{C&?y zjY79b;$lIYnv0b}g#X*_5q@(|_?vsEpWQRHKe?yPez<3aroV8{8X3dm9(YFK4gK9c z3GDys9@1b^{uQFTt=Y61#9iti`nE6#=P@i^#3O>^7N^Jk3Fcjv1_`=`YYn%JBoy>H z)*b0s?VOAo!%|o_<;KE}M%j2Fn%H)gktWwg!mRR@`oRGFcL$z;DBZ~@=YOSV3uD9d2IH?V!OlkE2PhC zOGe@o+Sc&Qpvf-3WD~b_R?&l4-d)9CNyL+bJ z+|&K5dvFt)eszxor2oe~+#l}g;XmAy{BRGamyo`KJ{tewo?JhFb`L#^_%eq-+`~NX z@qc#D>xX*+0?mLw+{66l9vRIh4(>)5B$tU&FRjj~B$Y+u3|?cPoY@kRplzWQN6WdA zlUx)qG`IKtmKb8t;GpQS;xMq*wqUXH##KEHX^hlC1k8Ep&ezSN{af7tDQ9u?lXwAT z)j6nH!boy;+ttg?Lnp6-#gQzwd0F|8H}t%PKn2|&wsMS%M`ucHp)dt)5ZxPsoy%|W zsb}~@r0WL@0e9lfisEp;(IB`HE^fz2NHf*-V_~4$4B!1*5*Fa?^M?SqWP=D<1zi}_ z3rT9vagLv+7$j}N77@%jBP6Ps>Ne7Ob&*`S5ej-+c&^YP&X;wX6B2a?j%X95M_^N) zB??(!P7bo<)2;6u6=uG*;1va?xn+sfX_HUXpWR3$m*lx7Xq`)A-PEmdsE&Q{++Q$~Q|{PeUpZ683v!5k`Ys;U?~d@aRQt(rpHjfc`Sxmm5mpq#0S)6JSJ zH{i~3$R@hIG?$DCl{OAL0ilZ(hf^= zTjm$gjk9AltesW@;L-kB&oYb;rMd{!av=j?&{_#s@rKL|Q@v9wTX>NQ6Z@+YFDuvw z7-mucx-qOkjt>ru&lCL>BA1(*cDIi+S}mAjAGZ~^zF!psWEI-rwjI*X@d;UrPx5sC z;HT1*20Uwsc%|xi?Bac4z6~Hpb?YUfIj}w{&gWeTuNJ-{AyTjXYB-?T{HLB?L^9?? z)B&yslxT1|Pcg4sqApTM5k4-TMe9Cv#Hfnp!Ht)01guJZO8v>H>$&q3z=(&EChk(M z%ap2PqI*F>9=6(RfTj8kbT>*Az%6t(uB--NqM7TY5nltK`LeoAc3}Fgx_o%Ny^&SG zc%nE9UgfQSM~&W|Q9ttDgaPOHff*MZ;@6+nd|-n7)0&T;0FQlKi2iBKCpv$u`B)q^ z-rxT1zv>Ed7~Pp&nxQ3R5^CpkC5K?NOi}Ms)p7u-lH6{Y&CGBq_g0e{$kLMQ0Pdw= z!e9w%u@mj*Q`PhNY_OlDJ=#tPqIV zW2n5UgnCZIl}*SqiTlEP1NyYNDHgmuS|;}BYRIhoI3k@LRr%S!)%D;9Ad{wr#IY|G|&PLX{s< zXEr?e>7uIo#*){!<=1=1ZI@QY=A=*c;g?q_iu9{H_eK-n((E@pevzh{M$ibjmF3GX z?V38l8f6GDl=bN@$e~5{@J+vW9fIFhIFPNRz;NV$4xg;8ixy%iyKx(E;vA_$K&eK~ zd-Fmji8K{pJvY9CCtiRPC0OF}^W>oPaz3GJkMVB+J^hsrXQu=L`gJ6L7UP!-wx5W0 zRbpV*g!%@9hlo)W(6BdD@xz}q1U+vR@q-oXmmyMJ@_E%X9Wru+?|mD9-rj`wn*oa$ zhvveHqz?Xd*{7XGST1)v8)iqNCEN1L>Y&z0AO{TWQ5V!@?IIrTVmGZVT=85Ae5QeQ zt{rSiD+(YN%DZ7JH?09pbg%lLQdFsEo#7c`EKNEaC0p82fPSgZjg4P9iNxK81M+@P z0rHX27H5Zk3=PwCK$;UK4 zkS2|DdrjyR775X^vc5o1%mcN;ZvdVvRG5o?9|b+wIvu)}(95t>+fqr1%8kkjNU1Ok z*q%Ddu}eZ}#F)fp=D^8qLgFzcF5(>pjMk=0iQrdlST7FQ9EmtzqF^*a*=-ZAYN%ZAh~P1imOkoAPDGQH zG{$_9w#jg{({b?7dgr-a^w(xPpj58ODXan|P+u|pZXfak;wG>GEAZ;=i@;3K?N6nM zv<<<$qChd2v;kKXA?HM|s6L|0NWt9=bKx8y#H|RzBi3W6jVV_j{`3VeLsTC)DH&cB z{Aq=o;y)T_Z3(7dgig^VXS#(2hUHU-8Nr~x<0m*C{>Tr1pw_y*aGcmS<_#Q=p0?z5(tVD(sZzBqRJAymNW8{| zG-J*jcMum2(b!dC9Q-lJ%kc0dZT*Ct{JDTsGJ$1q7`NLbgCW8Z)XRzQ0+Jy%-hykg zI@nUqd|H8GG3s_$0tYK#o8|qS^Vr~475dnuS3z&hO6Girv1wIQgXwi!GPhI81bWe{ zG2WU`mc->{!)LNsS2#htng^|=Eo%WK&SMLMhp-3uP0>VzLr*s6VIz#kE!5GeLh&w- z9{~=YPkMabQC<-0*lZVBCRl`x&yN^MbyL}Z56I$kq59yf-1ZLB6%4}<8zEcy6@rgy zdJ{ywr##74(UA1V3gEt>GmZd~df0@|yD;+B=z~&-RIn;!b+%w8E50 z)&AJc_Z8>aKjqOeJgb)&DX z&t2Jmf$D&SHuSD3n!gxHRxFE9AR8kIW)vi5u-8O-7bVgCjuA8y-b%hAr3xHg?6i&1 zqck`!csO=TlE5C=H#D#=zV>iM$9@Lim6eGsCjN{XWFz)O-kgQ0xB;o4UjR99Q+okU zuf-EplA2{GW}bo<;|9_em$KaD77UG7I#weW#4hxdA~$(QJjSfXRDNrstvQ1RG)44) z#r);Vlt55tHi+i3s~-gca>rU?yqhz^IObs{dnFRrxT&}sVr8SH!n}))LOaC)QAZyH zMCKf!C!!!q4C^>t)*vz%u97E$l~szzokrIK{H;X#3)n=|$2bBv=W;fgrRbCy0JM{G zi0?0lTo_iYUGu+PaVwB-^P>GN>B-EkZP#3^PvQy|SIFsbNiIZ#q(ubbgaIcHp zb}qAt?|#@xAp=~xbfR;xSV7`z1jzQO#w6jUg^_SX;g56%5=%1+%{#f11J7I;ydF0& zgQI&*kSB~EOxMbJu}vy7n?dZ3xr>+x#^5z{d&qR0@@qkGC)Nj;9a9{_R~p74em!hD z#YzijXHpho;f{bx!X)4AmkO4~`^zaikY?fSD>PSRx-V82*p9qt=0JzGjk)8@9!NF* zA`}nT%^$@5N(Y*!G-^t|m=goSKARd2>KY}>lOzwICUFLwoB;9czd>)uYf8ImeL$B! ztw#KFoxsIsCb9gKhoa)msaoX$XDI{Je^Z9B17`MS6h0(p)C|YacYMp-`*5(QlD{?Z zOIvFXF;QGrn)(683J~T}XvGpd)%Fvex9(#V!Yiw&qG{1Vuj&BqU>-y-m{kGWwfoZ2 zKJS>~m^vfz+EH_2vCs~-g7!`a;=2E}MB!vgS1!1Y(O(V+SV{zCl%pium4LSv2i1_}>R1{1Vfi5$bvkEH zs04FHinz(=w)!5^0?;0kOW zP4cEhRlCD(haWWe6CE?P!1!|nkp&k5^i+o^;?S-iNW0@gK)2Q}Wmb=rFT*ss^&BK$ zQLmYz(&A&PMK{^jgspHzKpIhkE4l;E*6dG>TGV}mskBOADn;sQ-f8U1(QzJ`VF)26 zpI`9Sd4!9UQYx=c#e&FLf3R!4hSn-8hCjjk!#wa7G>uM(^x5bGoE9pGE>d9|*wzaK zib;TL&E7JAUI#qb7@#IE_^DX}`i5&!gm{`kxBKi?DjAAkJu-`~%Fc+da( z*RTKhZ-2Y~FYo!U|M=I>Kaclrt85|<$H6mfP;^*nK+kFHXgtT|0?*XKKIqlO zlo)T|43_Ok`POYQWe9Am6!t(9*Zu-V@NjH7^^nq1sm%55aquqj1w;6XZ3fmCvO~a{Dy{ zFeH~TAX}3u87v*gs z6Jg13&Fm&Cu{@L-0haomXCsRHgb|!K8wl_x+=klF|Hm%EJwII4lgWBXyGZ&$o zyCHb6pW3mmz}hx#V(RV)%Lzfiw~sQKo#DmYL+)cI!B&wD#+Wi2sH1?@a}3V*Jdkxl6L`Gmw=g{R>>VE-Wo z9KXy9peAY>D0*@v{kqUmCIvB!JZW5v&4z#^O!@|085M9VL(!}xY(Dk)K@i(18{0rf zdxdHx?91Pp+fI?}yuHFASTPDn0JOpngB%UqGr4` zo`XK6ud*+0WCxr>r_(%KYP>jVr-*@2DAO?#4H1@pJ^xE%RHElXsNcWh6e$e2RFpRA zo?&Zq5lc`my(#DmYVj6bkwisQu}qKoLq$y-Q&CF5ty^*|hNi|UrcSEZ>~4VZP~a)hf3?!U z-8~(`t1u~f?~*e>bP}Ro;(=l}^<|AC3&`FoeeWv)SJ#*oaxZ2_m|{w1`^R1h^isB+?om;sBR&oyKF`w!4(nDdCj^9W`nP2|AYcW(*Vko|sY-WbvACuN z;}qD)03OjliUJg)gBRmx748>a&8RuAY3gX0$sxQ$o@4Fd20>H%7=QU#@%DVP*w1}n zc*MBFEj3-H&BE~;L8QzM_)RVtibDr<#bvpurqBfges;n7#6O~W3Lbz9xfd;JB|^Mn z3IV1${0a@=RMFAms{(WVcl&b<5~b-70N6hKkx0EN{?v{rX!J}C z@wY!IX(q`d!v&Dh5BEuA!#JT~n$*F2{(L=JJj!5HR9*)T78!Ikk@as*+ohZe_26iC zyq1kHV^P+{1gUP&=vhreoFsBa;(!9Km-+(U$o}v_l|oalB_*WUzlq9=fv? z;DK*xWHbOV&+$P9v4we)a1T;H1;G6X8L)#Mc<%hgb5Q8>{`Gycm5+Oz~(Zr{Kv`uY$wkZy}_N+@UZ2&=nPg*Y?E8o}eHL zAp2$MN#?3=65Bz0nxp-RuY#Yd9CH$yNt?pjAAAS7yl}4!55a=+rs1^~FM3uODp$tA%mF+2=g>h*zUm znvjLl(137B6uy9q5k#0CPn?VOhMLDs@z+6&fgHU5r<={h$xp1KqNV45c0GcfM z#{iu3_h7`#3=WnD7)k)K@K=JmtCp0%v4gS#L)yAJnHVbV`P77=1sLoOzfZsUXXJHq z1uvBl+#x->2r%s7v0zIfP>8Piq~;2g$CzTI^yo8f_yA$_O`GYE~)&) z>x7XulvJp$#B*EBje148>wwUtc`GGzh(dVLchcPrW&AKI^!;#(Lu9)wu;A~5wtRJy zF8GsgPV55;rOG-*;sG9Eu}%t?#t(_chr=M*NL9W*#}8x+5@8MVE~vp&eJ7wYP{8n@ zr`g-yM!Z=l%I(H(5RYBCP@~HhS%(SM|MU_s;bXFp ztSxn)mfJaW%;NzRI}?0B&@5o=tL5t-=f^9F1IUH>nJ&VF*amK)8fb9$>|`#J-TD*h z6R*Vvw|KW6QJm4E`Epb%*tz;g_Emg<+*lI5;LFu^zLg;4tvVOwU#r@H~h z+PtwZITuDEl!L?hUXRe>R7}z#4=MbEFlb)RBLUXG8~34IUfA1xR0Tc#uvo)O^t z)6Gh%r_6fR6_Ew7&DcF|kgF)CPMHnK#nx8ZHGIYc5$Kecj5iE^fzTC+(4~f2EAId} zlCt44uMBw7`HeLDO*Qf@?4iS5a%bZ6=IP8nG`;#btBLgXtHMM>OK(r!=J|xJE_>e4 zFE#Ye`she;A%wh~fwC)xBqx2+il!n(tU~mePvq^}u0L6*^(nGv~TP`K>3zrHms#cC1MM`Y3{}stl%W&+OkIO_Ymh z2r4-_Y4zXa9%VSEuc2;9)*2Q&JQ$Y3(kc5SmzgvX0}pEbCFMJROpDsC7gv@EU+|s4 za)0wI6;aC075Y+8>1ummRl`0K^nCcG_)@$Z_$^XtWUmPlBnT_AI* zxrk`7%a#NQXK3bv!j`W9FMq7c}h+|Ov9Hio5?^Kw#H3B}oNfgc3?a=E<)Zx<&k zrYhjiZ=`UkyJ%xK9zFXM7l{s|k0bqR6DL|kw6zn2Cvij0BJQLIU{Pw#qr3Ipqp(gz z;r3OAm>QPl@J6rZ78-aZ0)F+-c^Xsh8mKUXSMY|jgCpehM`2mywFjYv>%!s4QO<$8 z9SV@KScA55$#uV75B05>{*k0QrL=BB>A(mH^M%seZeK5=aK=<+fB8men1EQC3Qxn$ z;A|f{E8GteleO8Go-NVro@~PL$r;+lUjxHgn)TP)bVVfkhMS4oG5$9%e)PY1aaZN~ zPcM>x^&))|pKZD#Np}0aqM$=TZ?uim8upS*1AaSs__*ESR_5HhQB-?_4xKY?XS`o) zFk!Y2IC>zG=F2=_AV0K6-*^RYyf5${(KyKKkc{7rB&1o2F-q^Bmb~sy_Ms;VLYcZh zUOdg2rPhv1vH-GTqXS|-y#(~W_=uqqcN`{lD$QoR6cnpH)cIbpw!oF6Zve14Cdn3=y6-vhM&-d^-so@9;eor zu7UC|4&?zWK5de~Dhv+|tAt(LI7lgD1AvmhxKG9H0+UY+qbK>ZB#`g@kjG@}cCQ!+ zK3FD*6;||Tj6N%G)bI39U-2#T)@Q*%_W0JonYLi#V}`M;+t~o=&mpFv(5A%;^^|+H zi1m&;JWla)KMhcOVs+|WlEWL4@z({H*sQRnCjiL0$o&m!lH+WQk7uybG}1uV?V}!T1Rf-s*5q^nyLb=F)F3_=X*I#hY?h z1>6e(qY&)#!AlNIGRq3e>q{~sd5r@W&$8>Npmjlw^-!?Rp({SqelgunLahCuO44dk z96R#l<7mJzaP}TPz`oJL2!QN8X=T!aakd~rtcbyy>{3(|`m{1(l8($<5pcP@6C1WK{o9j|_D!0Lv(JC|PY$~NKMoTApZzoO@9v+$ ze>wxi3|CN0*~Jkh~NU^z#3&y} z6rOB9W$?#Vs=ORf=SXr-x?}MGCGl4SIl1vz#}N*X)n{mP@tP94(3`^t&Sm}WgOk=y zKvAmHCxrTpX8xR+hIO`-rCb@ZdM60+@RIMB9PT%9M%3&_i+NE{}-ake%iDj#NIw^+3LAq7!)~AcvMcTsBf_ zLmBc6MY>{JNF~V}6c6j#OAv{~;Qq7b_@P3Ma4@hj!$`=3LAAfiX(T8UZp!ss##x0Pr4&sC;ajl^42X;d-=bLGmO0JPM_+q4y0tG^@H7#HhI+ElMzazU51N2t4})NefGY~wJ+$Iy}BI&O(n$4p1?Vh zfW2-wx-U9s4=-M_+0_9 zI6OZYR9UVHcX=MC@Vgi{mXH?*JU1ENCf9wrio#B|rkMS498ls2jaYKC@1V(>n3{>g1xv^Yp-q|~)E5JjoJtz&|YRR_?x!pC9R}dFI z#+fm2az3i-(i3Gjf@w||(CM#6S60!jc*{}}4(gwjPAcf(>6O3L=aPtH^0Ee$D{7co zms$3kvpwa#lRuY9?t!Rp)~q2@+_~?e_Kws?uWz@4=;7Md;Q-w`m*_78e>oXW zzS6C#*1rJ&0RR82n8ymm+*&|?K}8hAXK$d%uJ=wH=*SJ5 z&4vs@zP46O>-oTQG>&u*ZHQS@KaO%cq`Ds}9~?6(ArKUK^MIY~_~yeQd|7h@0~c9C zU-ZmOy?*$i&O{Z#L!1(viRo&8m;~H2VtP%aN5c_ym@R!`z_P8E=QrJ6QQh%DAGD3* z8uyn}p~{`yz02wlv#9S~H@UUZhTfj{6W^~kkg=afXzC#n>9ioUW=O9HP2n^(JF3={AG;T`3|pujMzu4X$nBQi4PWEb;sLJtBjq z1oVXa)CwJGdczs`7Qt7>+%z>zzNK*KKC5* z)K6%GRPKz;-rgraAT2rt!Cce1;#$#9;C)~!@4Ed81?z$-ftCegi}NfxpN2pE0@^P4 zj=%}P#5(wyyLkuA+EXIHR>wIHN?4Qx;HRvKl7hf|!dxHGh5^#g1(yI}rhdZGO)pXu z^GH?V>a~fgl+t)e4q)Jsv5@Z5E^TjzuZ}m;TQU$_<#Tw zB_IdLfl^iv?;gzO*0BlNXrm}y;DGuf%b55qh{6&qCAZg#0Z%UVpyF{%b+$40T0Ig( z69D}NQpl+&`u!YEvI29e0N;13;rE#D&vG}V09Cg2G~4gozD;1SP6QcUeF#pb0IV=Y9U; zU;q7&KmPfTKmM5h>5o7D{yqQU-GBSXU)JCK_5L^S{?kAG<^1#dm%rZEyC0M0Pk$weFYa2uWl(IW-ZbXlk{ff)VuCTO8r)1Wy?mIgjB;y~^VqTuthmjd}w3 zEaJrt-^s`0$xcbEF#p zk2vbjngZ}KY*-4|e6bkJ@f}CF<4BT{3~rRf_6M;CI}mN+Y`4khIgy5_ZXty&vkg&# z{waz?R@BElT+cM_19SJkI8_!~-OO~Mz89akZV$#5biGwjoz3#TP4M6v+}&Lk?vlU) zLJ02e?(Xgm3wL)2?!g!Ct_z3Y65wC&yZ5g8)mQbM%-wfCM^klG_cMJk!^%_UEA#u& zgQ>*<=CTK$-3r*8xDP8?AlwS&tZ|}sZZ)lrO86H|p6IJS)QCAZ?Mythm=c7Q$apgn ztmu4w4?m4fSTSgBuQVz}@(W@%JGy#9q`6{R{IUmWab`6Y(&VL>i9lr2!hkgxEcAET z`VFcra}(iSPAwxfY_eR*FtM(ln!d}HFVlNsC)c^aPX>l)g3?IYW|NCa?C^#iSKq3`1>X{4eA;mum`X0Fok9r88cf?Hx_9Y5m1g?K9p*Fu zSLN%uz!x^dQsf10RY`ur({aBB6Sy%9oFwDgEM*&i&mg&hMi zYYZ`Zx7kD|^Ahe~>Dawm<@Sk}kRE;wn|O{NrZi%*S_o)ZTEy6Nx*P2rqxP+bJ}Ljn zzs81Ef}=-($5W!RGK>3k;vOPANH}Hy#d!fX$C>?mic7(T09`Zpt(f;0PUE4(jK|bY zft|)jb3pz}#->Z(){CvPXzHzGezoV+m82;0XT1x8+X47AhT?u?+ii?961(1f{wP52 zSx^yiERL!1$RV~(*uy9{9Gfe zY_oq?)hk1IPj>tJfmQ(*!nPM!u;F_Au2@P+;Z{Yiyg#E6Y$BxG<4DfQRo7}_q>hC0 zNkepK=>p(%`#(YE)f;80XCuVfp-1x(?~*1G3>6pr*w84-G*|mJ`kh*UV*r!!i4ojz z@(yA%uk1yc|p`}yr9vxh#-7{SeDcnl8x{A*qg*S$j(%MX6O_$7%K-9dl8<)nmZJShv(%| zFqc35u%J)z4P3;c!1s&Vx**_+oV%#`MiuleDWxo8=`%fr=!Vd_Ljccp8ClvYLy-iO z#5}cxR*1y6p|bGiN6ZyPnN98&PJc5a%)1^LO_#rg=(f9X$bA(7QrztLLSNp4{1}Qc zwfRjx2Kpd6!i5-p);0?8(nYP<5S&qEw=UO%GQX1j8K+}MeRz9I zRr*VM6$P>CqU5c#Y?q&xGVnlQQnX`aBpl(6>j@BHjz>bAK>Kakslfl zL!=n?_Gjm}DO#(wJjQP=!+J&@47rwD;eOj_*`=~!bjdYPDcdWcL<0lp{mksqD2XM2A`u!8*4rdBl5I+{2m<(mN8$f(T-9MVIQi4S1$c z=8Yz(r*ZS$gm74VV%-MWmcp5aIAN1K{Z+K!cp5?mte@uSP2?Dt2`(|v;3Q0PkL)c{ zkf$rNmD1q)oa)jowo78;!MPEaqt6Rd8_964?+_d{LGeI-FMt$id| z7q5-@t+?QQ{emjLPe#9UD>%u0w_Ea}2WQlE>d03f0$AzTB;NS2IahKpZnXW2*shcQ z44wpkBm7F<@02*K_^AAwKA67G_~O2_(BC1rx?;+cf-IGpz*F{zfN@_lhvtHfWA#n~ zmi7*7?$!JV7w@sw8xu4D;R0DCvdhp&iNCj~ja>XE)n^Y$)d1WEXxW}MFXsFt!Qzmy zvn{C0X*i-ahKL&2auoK&wV#dKNHhyd0+QbDSur$3EgcwqVqRije1%cmD}4?| z$$hAw`csd9nhDpZ?IqRc${KHtJINbS9bKdqe>G2RjIZjAlfUj?7(yxvMc`^2I1q!t z%^3WUgEUr7`6&>q;cpSIp@@D&Yi!T25%neFK$&%wvS;YyU_X=udyQ zl74cC5BIbBiD&~a1(Lx|udo+LZ9_EXk+tzYihs3(kHWYmzi-o>gkHf7D7n_}vR15_ z8BU?E>-Hfss9OcFZz%Gr31M@tC;hVo8gahI>QAhr zVzDLzD?4GT;p{*0N#e?;eF?-<6-ITw$D9yDhCN!+XelDpW_yFsFiJ7cmznLyk$qcu zJ^TfizqFeDvCUu?#CyBWa&8c+z7gG5l3P`%QRIYVD4&kMe4c3_i8Q^z175LM1^e2-8jekDOgL>Axo zblz}wt0|hQZlut#hsT+%82j$~gka^FIPiiOUXaMB!frZ>x%t^#^7{n}`7W$rW5$6v z(77q2KAqv(zO@Fa=2+~BDPMQ2w@>4cdkIayJUwU0ueDo?XEzD+_il8RnW$8jt1)Jf z4m=|DtKZ3M8MLtP3A@W)REr>o>y!QCU z%85*GdRD7Mf87Hszp^WtsFMK_&y#MrRDvEvW%SvA(9tJxygiN(DFu#EqC%o+TQgWk zAuJLAO&LmD61{~`HhmMBjqIn0m?gM!L!{NnfDwI&|F@KT5PY(=CvJA{<^Cz<+Nc-N zL?6%BQmL&-sme%s7!2EQH% zsLJU#@oTVt=2_s)ylrM_-E#euyduJ{CB^MRf5WUWu3N>|GJZGVEvtu%w`7_5xF%GV z*a*hJ$1E<)@RG_uMqjyMR`yiI$)vKpg`em4S-ph zqz1!_w;w1;9aZ4O&-IN4j#d_-50X+&_=Z>9df#eBoc&yVLw+n`tS>TN4`5Ql?GSHEnPUP9XW}XM{lWBI9QYjS27Kq_|Ssq~6SKE%(h5uF|1K<*kQ3z}g?+ zaGJmb)-k$X<+<4XWcB4tPUQNE1ubNv0Ug%t=dy<>r!IpqkHwUZuVE={^A0m}62lsi z!vXP*`P?f_VS~KYF)dC3v1^5GKq75O7~i2Vn5w9VS)s%hn$}Mwnk<->%_)Hp?Re<4 za%>i7)jye;2j0yq74cDIUF>L7Gtv5(5gzm&hN2R2vrFkTBR4mfZ-Eo;5k(1$(@e#W za6iluf&eF4997;T#;x+Orc=U<;$0xqwr+KKW_n@7j!**)$)T#to*4<9>Yg@k4Eh+z zs`ayw@A++`N8iE}P*Hzn>|l0R=tg52Z8&rYs47E37Ahe?5ax&CCLNkrR8}k+iN3Ue zTMd_orl*YCC2NZxLf55-m=D!hEV&Hh#HX2J2aPpL&}!e_w3#53GWLLOZ-rF!Ztcb` z&PcOR$sFBLr9jsg$_H1Ss%*k5XvN)TpX2zM1av2& zlFItkXO5Nr8Wy0t?X&1NbV^8DJNY%JUS+X}*(~IUX)QbjEjI=yk+=2Q(M`Kag9Dj~R!KWUzf}FdahL zhbn&1w%s(sLWlClac4XeJdE%@iX{TIGAzra8O1i|$B$Z#6IHrB1e`K_QP*Ap`3>4X zyy2iP?7w21pnH4vF_fb$VmEZCP!fHE`r6<7DIeD(Vooh~Zil)+9k-0|*@CZlzxjoK zbb0#3I^A+70A;Rd`A#dlZ4l*d9Tu~b1Eyk}Dd`bonEtxC^lbn zXuRC;7st3iy~sO9FjOWDesnr#BY+P28$E7x3J_(EaPV#3B^X}lfS5KMsg!fyp(OYU zKs_G-B@?rahlz0R=|5rOVu;XaTuc24GfEgBcU0ekjiSF(5<+T|9=(kl*c)Bo8PL+D z{e#q=X5a-i9k9vKF}ypo2Z+1e@O=BZWyX7XXcPj&b@x+;=p?O2cgwgLs?qf9^ta|$ z?vi{?9rjt=Z8UlW`jKONZ;gnBT5*x&BwJODD5*I7XZc&^zw(R{BMvwFb2hp-1&=Hc z9Vuv4Mno-Im$YhK&903LW#mB8UJM$x5`t2UUX#CnV(^0=DyZC=;B-q-K5Z^QMGmUM ztn)Rn%ULq^+?-??O7RC6eN|(ztA!1sd4g5oHF&Xv3?L)6&p@5&VI)+TsC;T$*wC-z zK{i2zj>~e8UbzVP@)2=&d&;q)zc`0O_x=1eaB2O?VAOaCGa!G~JL{KOB(+1|5AvWH zfjn=+4b>w->=#03QP1srf2T*$_w9|hypJc*_SfmhkJ!$)G!f-X2pUe;qjUMU%Qj2& z$x59Q{*C3m=dmoCZ*`lTGHbtTBOD`1Ll%O}terIU9p6f~Ha8^_){V zD}dgns`i+X>o@iSCI&L~qQdqPS0o>1uiAZTxFPPSS|#2V(Hd9k8HB;0>+c2eV#i?W z>rPQ9qj9r(S5fhZ=Z$Fm}`OT}$_Sl8<5Nd^+|*WB{oWKxPfOu%hxSQ(VKb9}1$*?RQA$Gv4*(t{aWX=y)%dt5se!YTx`!WA>Tbo-o~P{M_ZM2gEvr)ma%H~S96w4kqjJvk*_8qkhLV!1zth^X zK&tFaq@0)xyeEuGrY~Z$P?;kTxfdaD(7DE%Gw&8V=D-#!(=t`|I8*tef{gVibq@tr zV{cxJXMOZje-i{)i;4DL|JnjI@|eu_?M6#?v(ytwrPD`PK4YWZjWqUq^Bb6o*pi6$ ztM8JnV>^Yu_C3AV<^fAnDQS+)ugje}Y-ZmRO`)Xa^WT0E6=wREKl3i|D-AgH1W~~@ z6JXZeI_{M=-F)fCA=T(Wz`iPgunSdN#xufKBa)T-8DT(j^Mm%Q8nwLdWd?rz`o`rL z$u5#p^);PQ0$rhQfQCg9jL4P2L3aDo(^C!o1TJM6j9IL9f>VVF?}Hpj`PYe1)672DDtEO9j{t4&p?m_t3Ta9&sc*{zN}(a4G=Kv~k{R^76^B}lpPMCqk5I+Sikgeq<8u`u)5 zi5|`=aGjVY&ItJ9F6jWX8D5G>0j^zT%V7R@G`^ws)-i@B36+$PJ|a!c4du2jDiUb0 zCMb}At5MJ{nPAt_RUYl=Yb}KueU2u%@YHO{1$&0ypCcsvN+HLwCLP9!^zpwWv0|*t zxVo{l;mDNQq%KjfSH|97Mr^+OZlMXbu%`5;f*3keI&}~mvO0{Uxfgl^mMbHdiW4q+o=ShqEdRS4`z5>$UHBTQ(n&onXlPK!VmXuh!BW=#JoT1hoB0qwPmLqG z52RZ@LaJxf!adA9sfm`QGcnptOB+ z;gHv=Be-`UoP{O|6RIbI#dyPHZ7FA?q8>GoX{Pd>t`#e}n%5eiX(*eN$Yw_$J&xKs z-0Fsdu4Pca$TTBOT6wMl7Dsj{85CJCtsnEcQYP&}imo}#!3qO~Z%!svt~d^=)Qean ztN_A3_xSxV#TMA|g#h`R7{rL0LL2eh>x)!z?@EJ^v63yDu7tQRKDSwkt#ihARO+$M z^~#2kml?8fl;Srekw?Fjf6H19z{GLy9cHR-VWm!z^5N&2qVnRwlGMlaz$XXY`W0d= zf2&>WG3cWbQe%&UM85z_u)cZXMle)iAULVV4W>u%xrj%8=hZT8A!Jn~>|f6XtZ<|W z6`Myp#bl(TQHBNTk8^VhPyA2|5a(xt**Zw7A{1i<86xb^GB6I)nnaiH1zf>K-ohR@ zl#_9R(AXLs-&Om|Mc?a4*BPTb3dWp*B9Z-DZ$$Jrx70yz6Wlh^*#Of z1O{zfUL8!iub2#>3z3TC?WpSdEeM@jOcPzX=oHPc->f}Z?}yFzA}y9Meo>1wc3KQ8 z=~Q7kd^^&@<3y=#Zn8N^Zyg*l5ze!lP2W39ytQ7%l6mmbUoXPvHyF!qkfW?gt?t## z8a<7n?5W*;etW(0&))8RduMz0_4#o13ttyeR_&qQ{mq<7sw}Hq&9--{$NwOM5oA>A z7lohgQg>NDN!HZhq`n8{@rt6w|7IyG*<3IX8%2*}dSeuQ@#1ioW2739%35a=#vFP? zIdVh7O-ltm?qel)G1zQx@s$;}@jwtxFJ>YHtg(8&$lzo*7ywUbWEZ;i$q`wQQuZvk zi){3iLxf>tfo%t=^>g7Y8Aw7hHtbGYMhcxJoBLr$ zpGspUe>m5!CGB3ND>!d8T^U6*(w%A)tLH+E2be)CwQTruy-G+n%hz~Bhy9f~k@Qmj z9INJ;eAMH9P)q=G9X9TnSzw7ouI51er>2dF-Gt?$6oxAgr}fv{%C&-VP7(JNen!7L zU?M-TAmMeG`7Sc&ObnaYj&w=&tzFlq$nA`q)0r`Qs`XQ zjXG}0egS^EA?5^HZASIlt%YsOq;w$pj|s=?cATKl0hl;1<%_QFiv4z!axqz?1v>>1 z(vbDb{lSdCnNk%(UqY0D`|FeBU!++gwkt4kEVJYUZc(VyzLG2WCj7~7aND>odpLSPS(<|P?$g7g@}}ey zJdEs*!9XN05JOm~m7y+9UnxAT6;yHDYMft`6`+?m4bNrI6FeGQ3x>iL#hPvqV%Q8~%Me9vos|LA zp`Dp-4za?md4-GD4pX^FB*+g7ZwWjF1GjUZE1(p~Y=P{76?5!j2 z7`=Fk5o z@ee#t8%^2k>gX0v4q!Y%cxFi`&G{f015h@{JjCo&i{bkdz134pq4Dxt_9QJv*Vt{f zpiCbI@#8e2@;a#-NoeIkU^uMv*%Bci#AWlSbs{Z}T6}L3N>c(c-c8gDJTK`)4lmAV zu*vnEwFwUM=*kyKb)Uy8Xs6Qyce$W`F#*+j9HreE&&y`Hy(mla zA?;U&5f%30lcs&YR9&&c*9&_K_p_pGyM)GE`d^U0Lo5IbmttfFf688CD&|+~GbW|D z-0Q|wch(9|@OilH$?KOkxlKJ2baS~BX1yJwqfB^cPMr8S6S%)beN~ku^!@xiy_93C z0UfNKBdmZ%Cs~gjEHy`5f)N$NAnrZ;n6Mh@?t&7fG?Wp6FSj_S{3P<*Q@&uI-Tbf^ zD+Y!;`A9>BqA%8!{}m%*fie34lv;qoabGg4016?UWN8~zQ;rgu5_AJseAlvJ)!bsU z616)RtS(R&=wkauU$F2?OgM$rF~V>V@PXkdA~-M=Ki@RJ`o~$BL&D#q&*9bx#9n{r zXM;ZEX*6LwW9Cu=ZFLFDm#U1f4Qr6Qj*P^DJCxmwwSr?TUD|b}Fh^KiA=Mb6Ua3Qw zsC;BMH-?kUfFYyU;P2Mms5ZkXi{G1><0zAp9?o1r>qSa@7}ZUw7G;8SL|hHI2(awW z7-A>Lp<6ygBPB`^$WN;Q z{&~>BME=9aC{n;ZS_Ab5}`p$FU|azp@NpGFqCIXIpW&;pQiS{dPE1cir}= z!|cnM$LK+~Pi2j{v^q!+PPqub>Sr88)FkSrP$NE1Sk>8Om@N;R9t5d1t(FZ7oa5I& zzL1>ajP-GHLq$+ENbsZIZPzOXZ+V6L!b94sK5sPPtBV{q8Z{R;OYXH!Gslv~MPprS zW5#R{e7M3JFmWIm6vL>n-p1%`r0E+a`NJf)JO8?}d|>=}?skZqHVx%#(Wr6KuNXPstgUvG-o~SkDfl-3m#L z+vSi>MXZ?=z*Ta~|C6VI>?b`UVaf5hVvRhON5_X*lK z)*h}}&fO@~WRg}!7dN!!lQ{Z_(ZyaxML$rqk2J{^^chW)RR=BErQMIl)!gEDA*l)6 zI_|CZml)AZ3G>l3e;lO~_4s~KEI`@+je@q00J=^zYj^_bv%Xa8LSAe|Y0y}`J8W}4 z9GAi0e{$zC!Ja&5QOQY5wJQYQ<0u+rF5z*7(328s=M}U=#{K3$7A|?vWiMDD6^;o4 z7b(wc4t40WE2`XJw7Dk;tB{lLpI9D&>qp!!y{Gvm<0o@XzuR zEhx+APkX;6PEuxj^H<0YW&Jj{kIC`tpR;F@IBj_miTH9-s#kRvW=sLUrol|32dZIT zrxeS*cL6m%uJN>GRro`vwfNCHRlJ_r6(&}fgZ&U>olw-?(;vinjy)c~ z1I9e5{x-wkd)b1Y_RI$nZJrIcfSs{Qw75WBsSD=8#je54fn;m@rfrwKqdLH|M&3Hb zqSOa}jg=<6vqvUXBX+|4vYSR(1`@{L4pZlZ{g zvG0A%CTQzDR%Aba0+o5f{y_ZhjLdc7@y`Bf)fW9Ygy=UJY4-}VeGb7c4_~!P2aCw~ zzP1vFl!-U@hVdbd8ya<>@!Xu=|8gF~FJ){ZUMN-B zz~pk6ZOgKBnv0=@;&s`n0m?CZn}}A{ETV&7Gq>>`=fc0r=~OSZI@tWhjwe0{mzUd7 z$vj69cd!-B2v+UL0T(}U>3SXuc>JpHYE5(6kfL2vWMCuVh?9NpR;PIgtU%v2$fU0= zOzbC0p{dYLm>aFhd&dxd7N<1w_bxq35TXR4(X7vl$o^i9zan#Z`0O6V`kL)H;5zcy z^LQN%qGUX)f-LWF#U1+V8K=BZ_<|fv8vks4rmHxA8)q!zJa=^_64gUBlj=BiMvl-z zeSG72N3HdgLjG1Q+na{0BkIb(>k&-T^b&Hrgil*(?|h96PWMyGb1;Mb++HVbZZ4om zESIPFcQ#(CWYWG50hd}@mXj?2iVX#vc0WNfH*)JZeWUNcPkJ)_@$aH^m6&AM3E2zv zt7nd;s5ajfWtqM}m>%lX_t(lDN6-D`j_#+GyXl^LJlNUmua&!to?GSq;dz$)1je2h zdWF;|{Xv*+Gymwi*Yt-(>I=P^S57~r3q8)=v~`YIL056LuV{Thgg%J=dA4q2rQa@F z3rH~}B#wWtM-bD7s&{hD4@%)^!>o=byhk=2-!*_D@6v^E35s|O*R-P4r zyPs;o+>O^4^IkRne&6SkVv<*8mwptbWhnO6kUs!Wf^5o9cVJP<@v4@}4X=9?32(h}j0$5wXk#nW#8)VCC&1!e_Ge=GYeZI?#jX|RY(Go}}f6>T^8UH9iaReIF2sO)sur-*_7bBdZtq&=1 z|IIB(rea40W9R0mHHS`9g<}-@6oM*~MM0wPdp<&rBJX8DCd#TPtr<`W!e?I&O2Ls+ zk2siMF884ON2{;q1DbJ5p3W|eAzZjM&MY}1ZK%!Q0+-5--`=9T$3IBy_Pg^cT+%jv zyNGtTHpOXqkF$&m!wGcTMmi}40__FG+RMbs+DHmERCk)&E~b<(80dhj>klX?zO*i0 z>u}>D*aF=(!Hv@)K>JSdRdb25HdOI)jhu_ERtXUHT6awl1>Ilh|9~JKpDc!haLuE# zKlViMP<&Fb`6b1!5rN*91z(F`9Jsqg35SHjRFigzuoj#*T0|!8CLVVAJHvz1J_(O= z!;c`W>k$v)2eLfp)nL+f)|%}7B&X*`{m@9BXCj$uK1fl&bNY94bSZfZ7>_f(1V!^V&)S<*)qQ%V?BP&{2q`*BWxsd8GO>+gviI9W}}&gfQLo1hWnGw z6670>1zC9BX9IW2f;&GJEJJP@*-_~~#JwYO5U3iR7qXp+Gg1&OdY6eVH&l?Xx?PCM z@>M3p&v4qgN`HneeU4R4sR(`aqun@}g)4n+w-byn7<*kqHbB!V8_XdK@U+PB0iMv) zLsHgOvs6p}t7hd#DY9T(ch-Q5836|Ojy^Pt91(ZBfcqJN&+fE+u+6}9L1zQ3pLCY@ zl305pf!BWJToM~}%}(oaVZW3tAg#_rz7m3MEh_h)dm+<~PFd>@O>~~LlyuX!2*zL8 zk7!eycECIGwULy0tT{D49mtLWOnJ57L`Z8;FxPZ7sAi)z{2vu+!&L-%njP94whHQa z9#temTCaFIp2A!9p&{lDMf5q%`hD!6=jm;)ww{M7&x#|v)Wh4SCy$0nt}{P^ zv7OsMQAT4vF6i*MpjG;Oz68J>Bi9^7lf2=V16iz9-{tdcgl-!Mc$tA+^HkH#jbp;; zmJ@E)e9{MXwbF0>&vI7>I}+oFJf8Rt1txz^(@B zO-PzYI78PR7I5r=`m8-Ig81pKwMd;XA+UNAlE^;*hV71Xyr4D%&ioW1Xg&}CkO1yj2pgj1TcZ1X{~*lJ|B56HzWt#iUIB- zy(WuN78W{b!gCqbg1IFoF3J`gxjy`^%#a$CcDYB@q+$5#`?ZFmGM{Sd-hFvVVa+dw zCV$`BS=p_}yZzys)Pj4E>z4D#Ip`v9ho4AkdWHGtD9qXM<{ zt$BdX);_a=Ikg7r&#)xGI1;vpSXJ|1fO!J&TILKOBv+wk@a$Ga$HJ~f4%04k`8s6nO5DCWBgq2F7J!z(c3GY>Zjke?4UFoHK!t-_fa|P4 zaCeh*0CqD_o1bS#vKgG|kkyA;4b(mjx5Ie+>|~cseu;gs;THN!DZzOiVaPWKw4R%P zitRU@=f%~wxiiUiFJo6l$bYNQow#1f02WfUKWHVnVVssaz-hM~DZ}o9IMag0t8}oX zMQT10mGZ3uNw^>k~F@r^FOR$RhK+%n%2nQ`nyDWb1l&1NbQLK5b0gB~v?Cic-~ zd-K{BpEYxq7d=&T(mU(o^(r3m7N2WH$nxAzeOty3UHlw5w8uB}*;FxmMdLaSeL#GB zDxE}19geSGZdAMz_&Ar5<;0P1EED0UIBdTw>G1TRQJ?~RVw^_oBr!Ph=NBH13S3FH z%)8B8w6eW5?Z4}LWUsj?&bwf2)6^rHsY_FWvMnfd8)DN@qus`{?Q^P0Sto2B4ywI{ z954Z8rOwGShGRV#b@E1lh zMN*S|mGdQl+9D^>%@2N6;>=`iOsvBq$M}Lc=&oKMGclZc57ZYU{j@-vctBYXV zE8{gqw463?Nc+UA#}!0F5U(r#(NDnZipu{y>mMdPj(nb!a`Ys^Xzuf0uFl0;&vHlo zWR2TeJOfVjfQpcQyH1H!)s*>T2BH*UX+UhQQPDZWxaD06_J8c0#G-00;NH$!w`hYa zb$-Q`VO&@h5WAW7Un`m7UFt;GX^WKf-gJxi|1BR?|CWy$r@qRIAgV*CK*zrRF{5ij zyRaKm1NnL)V^L)^-$K$?dK!isN_{U^^AgOUb)_oI+r0Z)`nSbH(;unBHmW7e?>?l2 z_v1_{DW(X$EHr z(pa5Yqk$8FIYsY(@6jQ*iMP*E zIcAh>Rz?zBR2~Uub-zE}N#8DoFOz`4lcGt6nxbkj^maLj@L2-*J+7+w+UD{!)>pJIOhMl4IH?e8%R z((L(yT^4u{=yZ&Izaq=P%^STOuIFWeU}>_ut~6m~_D)^juO;ov^)QX{p)D{UbgO4m z%B2LHeBBzX!Ii$P&re9WFE6%nAfRpslMbYMnJHFI2*NDf4<*2~HTKe%mQEHUX}mw* zZH`?*Ty;gyyfl}qI&wwdd~b{%->Yk{I=7EJo4mKT%%2&1eraqUtS(+C4h^)G^*2YQ zXA-jho}bAM+ebBt{v)KSx=H=jGIg9iGNQGad%}e3el2v=eaAQf^~IRnf6USu0;p%v zf+Pus_l?6q;zjw%;2`_G2Pp!;sTow?K^$r1*3@*K-yiS9C4#Bg_{{mb3uU)`4R{`a_Yk@JJD;@FK zkuw)9bHvb?rObp@e>C>#&!6+cw~TYKliWy?xF?+@1|Sq9x1fY;Pm3@s;Ep$)EeN^C zy`v9Ni*6TeZGhYi%o8*=hDmLMvP=0&J9uq)BA}?~V}j7!{lv1&zf-iOCLCqn zvA3nFaZC>#9JJR6eF}5>C{}tk=exS)IGS(HR!U!(NIaasV{fx{5|Mh7a=8&6mg*=| zrf6$XxnL-qKRNP=cp000QxF7@?fDHjuVYx|%7m@36*uQ!P#;`PfXevuMBQVAjocWn&^QK|(uH|{(nhlV}7i;%r_ zi||5v09VadGy3Af0l6n0N8-k=9> z)=;#(P)Cpv?@g4Uoks0X!4|iyl3EdWmytrqVBV+V0(AYt!fm2JB20%g%W17}U9P{W z>s~H})ioP><(aNkHhjP=FOF~I&gu|XiB~+8pM)+%=wZ6%lWcYhazBk#S$vgT*{pS& z5KtH|tPv`>oOj16V50vBye(NmOHV&E}tAJfuS^FN7`+2OdudQ z(BGNr9Ij6Sl4mZV$HxMS@YU=d|0XKAOGfmOIDi&{Bw zxoag9&Xo?uz%xmC&=LR-2;Ce$0`RzG9`50k>8KMD8169|dmYci1Gy*NUi!P9mKqYO z|Cqm%qd;!uDtsjy&fTNhRDHNI?JQ$fxnTI{O0VI953|S}1=hJ9^~z_FY@nC`5p?3J1J`7aOsK#(#Hyg~LtX;ta;&Uh)Z zr66{>JRZ3)m(Bov5njgXCsTa*UmgUwcY!*0ksaqnqsOC*XNB6{a(0O4Js3{lk0uv? zHi`=?J;DJYsyc3@i2|>E-j_$AqN5ES-Eb0uUg%LHvL4z)2X^Qzh5p#a zz@{kl^QXp)j4?jbL!4ygi5%<0qk$<-w!?3uCwt|Zjk}IX>R&qLsrLIw+D6oQo1+_Q zp2Y=)XGv7^EMPTmjj!gxZ}$a){++kAf>)il54-o^%ZF!0cR%~9?6ds`PhZ`iv!~Y% zD(Z(P(m4(4dkP`9uo*#109+vY3RI0VD+rSvHD1;CvYOuFuKEOYisKQteDVk+^9Wr& zxd$UYy=2}_i-OvcZXa3%6xSPToJ-HlIN1m8e@9#j&h7mX{L+Uxm5ldcdUf!>pbI+A zBNw(?HBf8htaz))8S!4J3)t)K7q%vGHaSd+W3Ywp)POL2;}xsO2;qV`*#99gYycp> zW{y?SCM@}{)h zmd1Gu@^>Ky@@8rY#7=6O5P!U1H53;xuCsJbCq;-sk9&&*BRS3)?Sl!M_K42lN6l{l zjw|@bHaDDh!}idjRgk(iX9r4>4%OrCiAE#aL%JZ$-Bc(AM2}H0k4dcoxDEC8w0nHt zAAct8+p`GK^vzv{Siau&8$SLm7Kz{NZr5|0=5yEViJAuUa!;GJaZ~PA6^U(+`0=9Q zVGHY=?iL5wa5y$OR%XV1UroIci6mZZQ(LEo>|3K3-V+0ZY`8%I*b4o6(vnJ=bqa~@|@My+!Og9w$eYq%VDARlhXhp2iG5P*xGsSB76U`T=K0>%TF z=)*m7(6X6El*c_lOh?Rwkb!ljI-DlQuVPruz;J*mR8o;^ZTl@YhLp3k1NfIS66~xF zCg|JcT>yC&Q;;(_XITtm3b&uY!5jtlN+;9F{8M|-(up$!%_-%%$>Nr7{q=jXXNl`g z*LTV(rkZ3(D#={eoPehu0r?cu=HuASxZEU@{>PpV1_=G5N|lFOk}0CFJg`~!XDsHX zx68cVbht-u{Z{MAv+_e`=*+s7-0P&ov8}n7Z8KQU{u{>?LibkdKo{Aa*ShiKt9V_T zZ+PYO--}lh?*v#J)5|;m3tLkX%e)ih?hD*Gk$F5XZM^@uQzhS{|FO|;w$9HXie1gAmClfWp{Kt0yU&V9FVpn?gZJ%omv4_Y&54VjAL)T5r^Z1dVm#X0rPC$t zg~P^4ShC$pd#K3<0qw;~eTLpj=++))QLUowH=SmG0Sr$n{y*4r=TNt=21-SxJddZP zk4%>;AI%(Xds|ZbOO*|JD=OS5O#}{Kba1%DYkhMGjYxp&uCG>}zV<;xI+<%jnh*ZR zUlzUBP^4~Oche!;Zxuql6V3rcfF$523CXj&66a49aZ9V|V4mW1i=0{)EICvmaFee2 zxR^1Y^odYscy-?yu^JfO1horncu1W-N`l}<97G0fM1elCUYl2V$JT1 zKqUbfjEJMCJz_cYnC5BfulCvhl#97_*2ITQsS{hJ|A=<^i^4F2_xhJmoWpndl&v)K zP4$SmbyBKL^-(+fDwX0_q}2;sK4G$dcTweA5^klx+L_1p+S8WDhI_noV?@?4c+)YgZRem=W9fK{GR!wH&Vh6Do8wrpM~#M+!ACE?ee~GbM@BA z4w9H`&5Wh|;EDvTxBIlmMt0PztrstkkB9PS3`%F*drOkFLR1flZjA64+Z{W>2eyCe%ftEBrt^OjLPYGe~u}>pww{gCA_zKN;s;VkpT4buDncn;v7|^+Rf; zLtH?nMGo?AbHJ(xC>R3#dBv0z<)RM&4EGdLY^no+4RsoFkEzM3=S-@_uV3%|d7VY1 zd=J|`n%v+ogs@Xp4_b+KB3`4id2d95IP${GL~M!QkWHSso1NB)JpvxQaocN(jdRfn z+b{5MeP%{&%`+guGInQ%|E7=@eaIic&8h3(i#^TO#%<9*oO7SC|2Hq66T^q!`FCDU z^iccbP?~ighn|8f{Au^3fyW-Qext3J`VATZc?tzP)p#&JQmxDvY^Y=VM{^3dgTi4fER$iC2**tvQ~d zS*qfAvUUEvYv^!D z+l=hb899CFS4=VlO*sw#X21f)@=JhOFwy~DBUoj(3$|EsiNF$~E*DTDB9&PZLp{cEuPHPmwV!EYO@GR9{pQSHfa7$Gm> zV<(p8u;S%pDIhQV34STLrHaQ^=HQ4if%~nrLkv*0>Xq#(TnP2qg15Gr*haGJX?M+R z$mlb?nFl1v(6Z(Vn5JqlkxJS2ZV@s-t6+Yl%0f z-?|3iM}d&>|9hatfiqCIs;sRPlKgqX*z8J3Sb>b#4aQdFVv|9Dy6GwPS#eX z{0>J*T&+F|C4B85KrpM;7vDVt79531r>xbc+G~DoZm(JWBCN_LEGLO4B0Ky?839YN zU$0ow8kaP7Als)}TE93oH68}gWLMH@EvT52L6eYpsF^dfFQ1YM-yN;AK=s;{`Il2% zH#h!ZZpze2#d`c{2j z6BO;fYe!B~(AVdw=O+Y6_fFS`dd}J?K7slVmF9AlU?=OL2VyQfz1*VD%#yD?WOk?%l__^xKoR9m!8zy78BmnD zGuh?NZtpXIA(|_qljoH&(!8>B^1L!bnpbvCo>x4id8NId{hkleTzPdr`#m3`x$^3M z=DnT|@m%4(ZH2VYOoZOH7W+(8PTt#og6aoaXGZ#cUxu-tPWt_{)C3G!Kb;7@t&e#> z==HXjnD>KTZ=0gp4|wjJ3cc<6Z}yqb00030|J>L?PoqE-z;R8~crtzlPj2?ms<^Ty zd!WTvDWP3jr_kOgQlNAOp(;~9kOx1JM?Z>GE3=&*_O!|7kxUwT`=z1p@hAGo)@_S5 z8@DdRz5}NLJy%*lf*#o8Sh1#l&Ga8;9X&X!Ppw(UQxj`Zf4aBhco1e0BxKSoB6i*l zlGI>N;kZ$=uX*>e4ZX>=sS=m>eruMlbuJdb!e2+Yj<_O zU7nZK=khvS-Om>^9ZxSWCwWTy_j%01rz6@3hpl$#wZqmO&|zyp8#BWqT45L3 z&m<=Q*y#(W0xUu6>tJj1zKkyoHnpJT4zr<9bgJt=SVnx zU@9Kx#kw;kppu(n`G4Do4AESXolfO+Awx7*WaprA`j8=-E3(tAoDpP*=E}zNIs0-_ z$Pmqyjpws+=8z$pD;v*e=kH*GUFn&L5Cp;dMHdvj(|=)|rTXXoz`F zNW?yLEAI)fe{nub&gcKtshfsr+%Zkl110k2OC!49QS3ns^L~(sJ!p<)lp0koHV2b=xk|p2?+dOOW}r*Ns;Mo4=mm%1&*)HB}!00960+}Ocxn?Mi-;G}A+o~k}W zPVLE#y=V>y%%R?4?KJrPqStMF8H%Q=-z_^8EZa9e+n-*0IyHLMIA=MYQ zZd>d^`!R!Xe**S0>|2{vXSlkLN4kH~-B|I+QzGl3`*>%Ep%Y{`5YhET zal=l>ew3Q*F_4`!-a8A(`gol#cOFBg;B`9Oc?P+P*E!?P4&;}3oz!WZ$|d9`UZ=~Q z1ab?lGork&gph6VI$iE$kQI2H4tH+lIpNnooKKST>3?c6XDDtMYMNH&V_ImkfPi>$!_oDb8UijPuV%$cFu>W zugFfZrxZx@Od{pB$4K)`BIUMcsP>gt-)B4LL)2GZeV=hF=R@3Acy3$gd?@owBIUMY zqPYF!bK``wP&hWFI+vt zZQIT*S}JxsUHsfUeRj=Pv0k{?TyKt+%3_}_E`D2>DxMzmRaW_Xs93uMtEkLUnKftr zoSE}y%~Y8^bC#Y8>VK^R6!Jf+xwSZjLRm(kP=a#XU7elJQa0og^RGaq64HfzkGJRxswvdBY`ngb;wp{Dc1)qdT!`*qRI zSYe%xfUfE9aS|UlO;wv0pdm^*moKp!ev&2mHuD4-HXD^;N;5(SeqN$Ur3xQA&lW;Q29tRAiu|hA5@{!&tyN^h-9@ z5T#W53Kot&!sY0f9IhcsDWeFU83TxZ$y5zdN@+d`wEnYGw*FTY6xh60#m4+%ye(hP zdfCt;gL~!V4#_2I`pWg~*{rm@Kj?V>y5pgGXXTRZyd|9Ys%}4sR#O+sr!JgLdkaQ6 zDl{1fDv(cIc#PIOCNvQTDw0oKBt~l;13K*j*5&i%Q_V|hzs7`OT|SXdwa}#XjtRxO zd|E!$BAfObKu4^T%cr3)CvUKWHj#VWc7){=pkkwAq4hxPoWPI&T~IzFEhuU}^{g75 z1IB6fuh{6se6_XFQ^rU3{Yyy?yKNT+O8PUAWoHfIb*L`ywk+D85#31H&QG@)gBgp( zjB2E8_omy8!HmIT#x_#6H`7;+%FJAYckPjl6rWVO+Zar|YmaH9_zcjuj={vc_Pvc1 z-|h76fH|TK($H++N3%gTN*4T-A9gh3F?hCuh&(E=EgxxmfH%_qajCPmN~1gh7IJK# zu~b8pA{b2b9TO;k2%G|dZ8ZNefqaO-EgGT}nk6l8EMPA`B2Y#{ltQhg?HmgjD2NDL zqajKevZpb}1X2-!b2UUMgF|Bgg%ZVkP%qhELzL1vKwCKK2!#^GeNiu2R6~@~kwcp| zCJ^hebQ%iQ7?#8uL?NGsPvaUX z+G}VJ#vEw3IQrGQQ}U^b>u5&+bma1_VNOLWxAhVh{FHA=v;}}S;v!mZZe5Rh&nIh$ zQV1DZ+UPTRi#4CO2YDuSV;d=2@5Y|VG(QpaTkg)6Pn~#~c51YN&YNe({CEJ}0O`gw zQdT@G9r2DUTJTdkWN1qOZ?ydr=of&oEUEca(fPFS z(V<)$E1Loe@}_iWIAolAPSS|9Lzc z$xXY9=2P=e2#JhJty<|Dibv(or(WD61T&Y>#(HEn)>wt}sV9C2(Z?EVE*_PhPd)co zNDfd(EF>CQ)>^2ABt%1$A}dHM2fz{6y^#5RM59^{jOE*Ing-yl{Bz?!|MggIqhjOz z&{BNb^cnZ|Bwd}^H=jq>ojp@m+WzD0E^3X@zMnmdo?lt?d#Bx1+2d_0|A{Hy96JI_xw~apP)31w0kL3(HqEWQh5T%Hg(?$)XTr|4Sf%+Pv6k$(VKDhAJBP;_C zxC{dCTWN7EXT&A``$G72_5Un{@fQ4)1^o=8G1v^tN8Z?MF$;dmQfJ1f&(;SHeEOY# za4+(?ni1DX31HFTqtQsN=fHt8{W#23dm1Ue)9EnQ9eH8ZTU+%$z*$&l`0}ay73r0u zuh)8OtH4KCSR$XAnnf=e8+#86OUtLG&PTwe)nQ?U@~H>7^tv%eoAwY3E09k;=t{po zHugRiRwSQ#@G<=%z>d7rw6)E(%tbA&weRUfq3^Vb7W@_9IrAkhr~IZY@aKnN{+d7k;_vcrtWs||L;U{_h%e!Rm!3(Zv~MOpfZ_c64_^X{^k+V*YWdco{IIU#p`Ana zg9^HIkLk(=;WZ7!hpUmlOlR_E{wGH3iEgCC^$4y2w2_X|%|Zd_nYoUJDCKON;OS9k zOQKw(oUN-f<$4rDV-oc}KV>0{|J~^S!epLF?SKCZQ*Hh`;9rb%w;ffu;_(d$`pLGm zwi7{@4TSd=UX7SJnO}D?`8(i4esdF@)h6I~z`041)5Y2>O%DZT?|Xmy_#BG!5(cc< zBTR1<1$}xW8!1UWf{tLMYe!fK|9_;Z`iVM;^bro$e>nptSnyMvZ!ylI(o?bM;uicA zm+1_69HNXp0gJ8-=-CWtWKifSICO1_1wUo85TgKfW|FYz;6ZkCAmcJBJq?R41n3_b z@P{^pnJJFPqRU$FQ(V_Dt|5-SI2nsB0O)rZHvoO)%JG1`#fV5_3S4FLQZ<2^DQ>{L{^Kqigiw}|l(84floDqIb~FEc(P z!@)j9g-d7BDE>-}ugGw)O;O=<06dw|feZ(`6csK4;Qb8vix$c{zlO8UwK8dxfUS%! z)RAAqS?7uX{(|uX6+Rnh5WH`B=}NP@g^@|41WqAfERV7jvFyt<0M8&8s3XU+FLIeQN)VM0MulV9mw8~Fzb4>o zyC~CwWnTorQd~~JcX*NDI6HI-fZrg5kf#M_hsuDZs74UTaGV`F7r^%u@Yk%dj*C=W z`%*vy$MgxtD=>5vI+%0h{G|Y7@UD`K*ML5jIZDMeM_-T4p}6j0uu(^jWnT)wn^dly z44Ap0%nO!%DF8ozF&)VVbrd?5eJKDxfiWMEPwFUiEc;Rbeg@lOL_Vq`(DCd`0r)9w z%LVdT9fgi%)(e1HcaukZSOaG0Q@!~jSR;rj)tJbOee-8 z!*Pluh~one5wgf|oZ_esj#3%{ztb4&%z$%_V_zl!xE}#O+CYNi*cW{OZzJIA1W0fk z`yviZfE}TNI_FsSMF+skh)KwBoOQ08Nuva75o)M#Ec;>tmZBgr5gCp#a<+hmC?&v` zP)3Gh>`?X+0BMCSl&z*AN(r|oN7&5?&ZpkH9X&nU(+M+unb==9=klr8cM_%#K}ykOZE z4_YRTvL%6hP)DI-*%yzgfZj(wsiV-b?2E_vOd7?_oqSYBpySyW4?RGCK|ZUa(6Q`` zhgc?!;=UB^VI8bT9Q(p{03{}v&_t;M5ROwEjR5=;F#{QnQyfDt7^03G$G)&I+Y|5&Zj_PZ*cY}BSc)?V_%#j^9A}3T0DhD( zK%EvW`(go>q6je`8IH30>nG%YwgbUT25Cg2xUV-@riZ2KY#EI=O_9HG$h z?8^*rkh-HKM_}xD_GKcVzaYfGJw+Kin00h>v;Z8WO9=@;AIlu2V45R6(`^*DYvhAE z%Ea zk>l)8RRGT?)*!=icBl(jic*9j%*ffqinSypVU$A zDVBZVfEOCKmXeR^2y{IA!U1nIZqp*4)lukJ_JsppY222B_OK4tBbI$x59(I~u?eLL zKsZiuoCiwG0>TLyj$>bf0sI2tf(*wgj)I_m$q}x|aGc@@no;bIBeo#JaqJ6dOtJeH z;f@T)u`i%WMTjf04H=GOUqHi(kS7HEXl$&b6f8w7`vRI*gqjerFBlRWXPtva7NM7j zF+M9IU#?8_=pzxs&)lqvw>IAvWIl$fnV z5HcL6IQjzk3t~4i9H%%6gZj0E2t|hD*cVFxFCt)@kFm-+oO2xeq6v;t6=Dza$Z_n; z4ggOjV1pl|k>l7G1ppr)qESbVWnVUGh*H9~6S2r}oONyp;ID{yR5+G>0UbPcUm+rq z;W#6wgQciU>_vv->`*fRPa$HE;W#@~9l&`+95Nhdhq{BMxQ$2v@G%N{aXhwtfpK~; z862U|@$3tDn{n%Bq~r*W9nZdi*BQ6j6YEiD2G72L_Zhd{B{rhc@$3tDp>g|M@=+aS z-tp`Uc%yOqDe_qzg^riMfL9uKNRbcgW0<4jcuaE?zIYpDM-=(2jxcge`vP8R+|iBn zunyNFmVI#oRbUDM`=pI=AdzsK;%EZk8ALili6P-Q#W4bu7%Gv03`Z}HMT(Q7X%xQz z#=+6{kumu^O^zI+I4Vwt^!JQI$aD<kNWevTtm zu>W4~I5tL69n#AgdB|f&->Ul+Cjh$t490O}I?i6zhx8oANkF$By-iDTGOm+Jjgwr& zPjR`xfWaLK9?Qz4W&!+Wc?N9tkAlbYGO1}0pTK~fyHN00W+pWc;`DLv?nUY zZKSkBR&@?#%Vb6bfR9yfz^1~nG`SkE+}JbDA=5ERWv(ow*Dx+1(=kj44rShPY)!5fq}MWF_dp~%&U%MdIA|{87V6C4*qU4;Fm`{j(R)t)V?015 zz_B&Ce2^YW_MAeQ85~=aI|312o5$O7J{z+&M zTOj2UfbR8S^e$-sxvL}{r$$Z(bnjK8cR~A$jx+D_kY0(jBFVAi%=>&m_t8OGkt7|b zM$#bt2-1oq={PlVDxmucBdti1j#DGYLwYd!iiCy+bw%ny`X>f#VLV19fMbUl4Jus> z(EZjhDn{E);s5#heNMbBO>$|ssrPuAll!pqGMZa#+kFR`@iBREwZYqVgYeJUX#Llp zvw6BD`Y~m%3)u^8{RodB|3y#Lhlb|=bTJ*VZKx6${@!}B9~znf*stgU#y}w2knD+u zCIGf9x&RdloF)6Bp$ULpi!PuF1Y%^!-e_n7VB?|-NJ4=q#!s+>&;-EVMHiR_1v(jA zbOEq^(FGVlAl8A=hb#bH4!XcJC~%L_k1ha=4!VFG5Qx)b3?K_YuY)cy4+un0AU}LX zy2;S)pbH2>foSsUS2O{Dfz0#`$xr}^j^TV9 zV5T2%4lxn<(-9ci`8dE>Kju2wgl)`v00pp$z5`75V<(Y~*hZS4Kh`P{(^@6DM?a>- zrjec4(1wp!FJZVJJ3w}0LleNNmku!BkK0CeWJ43cay||);*WblOhr0%;T^_uJ`OPD zk6%o{KQ@PU1z66<0S5i?mx<}fSAgey9AMU;Fpkq z|JWVm=eB49cvk>J|JYaL7q@5vc+Lmr{&7pmk8aTf@OCnc{^N?sFK^KV@OCmx|KpWM z|M>RL3H(nhVN~ZsrvI*^w{822j^li^fv@s=f|yrl7(E*Ji;m-bVB#N;Kt=;dW5;nmFzyfNC3683I+pW+S%2Ub zG890f<2WA}^anmgOax%>S@4Hq(47w%^f#0J+s3R1PynxFl0m-}*~1M@0Iy`ipg*pJ z?Bh1l4ftcNQZcR71?T9;l=!J+BR90+Tj2*}Mz}NKywvFEL?XR)pI3Jk%2fQRZzKwJM z{6)ucJ}~qTTuMd*NOTX{~bVgpN}aO36NOXv4>HK5R8G{KViVUGj!-Rg*m0Bi(|<$TyHfk1*T*)?v=dH@BmoDX|86gWn9jzbf`E15n( zAW@X;9)~7?S277Gu$$~4hbDkmGA*FMC$fv&NK5FCwMxOXRvT-Cj#HCX6YyJ9wBh49 zpY34yNtFcr8Wl|d&-us$fn;q0evgVIfa!cTK!F?revyhMfVTd##4bn(oC|c7n%UxHge9=M3luiQ|061A1U98CfCGah%UuKo7D-%&WlIah#7Hq?eA~XYQ|=!ErtT zfF7(#_L)PP864*$2I<*kG=N0MaXvPHzMD?w0!VZm=c5hj{$wbCLdSAG9+2KXdLr=O zcNNR|C;@uN3dBSJtVm4f;|K&2H<1nJ#;gZW0I%p9LxKBb(>XK&ypkCX1d{a0#&c)_ zcqNkx1x}I8=gIj;)b)z#nUsglVk|9||3(CVwN6 zkq$+;gz%h?Bp80mIwBQW0Mq%bfC4qdK6C-RO)v`xq|PSxBMV?UA8#O#Bt;~l3*cP= z0}AXRQqTqPwy`-B_)hi$L)rwG&Sx4BNZvs9149$Q+s50VKrPu5Y@}WG=Oh4~L2;aq z9H0jakKUl}FFKC%Sr6&KhVL$;_xns*%MBM9l$ zWHf+8$8kPO0X<|EnF}D%ah%T-NY5if0Teox^VtIEp`v6WfJDbx?=p}cLdF4OZdcf6 z7ccW!0|b)dh-{>a1_kg+=3FT7lYmXr(5?WlWcmYvWETSVO+ypFb3UR_pq_xO)6fL) zN~SdsNHHW}_cSyCJm)h53Y;Zi12r@Oyx3Q=STacXK5 zaRzyfVmhBqaQK}B>{f*|KbX#E0T4)YAYj8PBmqq469xp57Z9hArwG&e@I!&~#A$Q^ zyluP~2&7CP&Y}z8ZR5#MAeuOjE`Ya->+fz^I=N?`tG@8WQ9b> zaXzac{Ulmm1;>u#d~^XlM3U@8k2H21=i>|M5o9ZRBsz}s5r*{dWHf+8$8kQEfF8P$ z%mt9>IL=2C((jR>016$;`RoAnFnux+K%(O~9|cIyM~egCio|q28-YOb6`}~G9*_cf z&c_f4q$m;B&;{^HW(X8WB5t4y;FU}|6zC&hceSyOEpm$RN~ReQNZmrf25V>nc+N*1 z3N#Y1#~PXdp7U`B0%;}$Y-)ujfEW8rfC3i@q`p?yaQj;E?h75K?vo>`ke3jq^U(*x z-xo{VK^DMtK7l|W#hoZc7r@)b;!xljQI0Nvw~cLqKXIZ9$cgdf1Z_gJFCWCqsHNQV)BQj$wQhr69c@sfRtJ;}{=B zT|f`miqylNq+=K#MPW#PLH4j8^F+aK=cA|z1Y+7450T0N6u_#OiV8p=)|OF^E`U`r z6%C=lZN_7C0j!FtNCyIO(-@8D0$3GOQ5^~#WIRI`z%o9H6M#TGKjQ_u0G9Dl)Q19I zj92IaSjIP)t*+s=wQ6^bdQ3?uX1qflKA!PW zGy%hpJw!GYL#h*a#wV8o1mXnI`ij9ifoFVjRiJ?z@9g zfVv`aoKNl?NN+-GDh|eu<9u>O0Nu}wY$`rhMT0Yg7y0CB0fD`%$cm39faQF0p=aJ( zfmZa%E5LF-xklg$qGvE#P%0WJfLAj4pul0Wg&5it;g!ruKp=)rb`e7pz;Zshjc0L%I0&V~Z5WG^u^0W9Z}O8|jb8?FJH~f( z0laOj2L(zQoj_pBGZHiM$rS^1A1cxxJkf`A9Ot7A>7EQ286u4x$N4Bh`dbFf3z6tJ z&c_(geJvRD}KO*R!D^GLzZd~&A&f#@)_uJ@z>Ud5M#0$a0jjHJt=@!GN%B6*jZ#3F|?bE=X_+Kz)`Z17@7c{^O*|-;)KawVrT+* z&PNmq1d;8;kOVND&kQK=0j=3Myvdl(XCe@YUy0Ui917sYKDi5kK#VRy8LiCyu~xXL zm9KNuV@k|1f)B|zfI@`nd|>V$D@q8U3*c>I82!iYCTQpac-t7J|FNG4q0w%@pWFnq zg>x=U|9v&dV0g?E7|?N?4~+hOv&mE#iH_rZVD9fnCnI4bI*#*!p}(I$nFk}$ahwlK z{QcTT4}<@@t2oXF#{K>)$Xozv>^RN`X8r!Isp7SAt{z$a0_oM(;$#j50e=G;B z>pdxeRWcpOpnoITMQqGt9o}Rt=i@*I{dHs`F*E@z=i@*I{d37)VrT+b&c^{}{qZNz zI*r3A!gD?jFz!zfCp(HET>+l+ae#?`LMU3Zad-uI&c^|U{;`_~Ii!;vtRlSF#{uU4 zvG)mu(N6Y1)(X2BJ2UDrB~G81h(3J0Z49ITxKo5Ox&YobhUtI2B%wOm{QNm1LB~)~ zNl{xklj;8&GO`-;6a#b&=i>mQf4^$Pyb91UoR0(fuFNb(H}cG2I3EWX`upcGdXVWD z&c^{J{sE$7G=Mbk7|zE5#{B^yWG;Y2$8bIlFzXNaLWTk;bR6g70E7NOd$gwFV9nq- z9|xH72i73Q0kHormh*8SQ~oDtUGGT&Ea&4uru@ceUGGT&Ea&4eA3m*LK3gDGY z81%=>qIJC|1@KBH4Ep0^$Sz`Ip1SZR<2fG~^vC}o8;PL_;5i={^d~rxy~NN2@SG3K z`V$_absC3Lgz0=>+@ClPtGJ#B8LK9jqcu=L2*9 zxSzxvbOF4b45R;e7ea5eBkYg0!fnQ`xjE`FCBB|8Kp#HdHiqecf*~<~wBi48KA>YL zj`JbY{|GX&8uJtbbR6dcqksSJhbS&osgZ`jMw5H-<&0sknnDPg8BE|u* z|1O^MAya-HTGxA00I%YcDgRcquJ@z>UcH1VfBbW_uJ@z>p7Y@c)+%8UTGxA00Iy`i zpg*CI>>@Vip$Km>p7Vi0f1)DUNDNH?&-uWhKQV#qC59$|=X_w+pU6e)G!CZ-)A_)- zKgkuX(>N)Bw+~?ApY#~5(>N5sbUrZjkH11#BAx7D6=6CbnENLv5!UDecsm(J{|QNi zExG{SPKN1!LLaeWv@`OLwZd)2{>?4wF(q*eu?o=5C=^OiZo8|q^I6JfJ(Z7?%?oKl z&H{@9XFjTG`PQNQu&&~vokRA63c7TU>BsK=0J%^sOda$wO_aGeqFRPR#>MaplkYjoW#dX zQ`JfwrwWNuE=UM=fB~-lL;U>T1Kg~wVslSRyW3i8+Wm`r{tl#OcTsDM_WkTx^y})P z-#hKD${uf1DHDhe&SIDsq%jupQJ%@^pb zUZ9!TKzkuvDPs4ZIrM-L&7%?;+`%?~fLs5%KWkOE@#5Yhw4U{{p+^Sy%E=v)OVsq0 z>ofIP-XCF}@d*#0Y201P$t--fzpv(gA-Jpjfk?@a3* zeXO;3W4$YIoI1asZUvxL|2o#_#Na!IC9wuk|2qLH^G3M-dV%BA85wjH{yO16Oz@x3$UVlG2@`}v?RLOTV_|obqE0#C^CdP`b7C26wok}+XLmg@VyL6@Y zrcj=smD3Wi|GU@Ht^(r7dq9i}nE!k6BkuuNNmV#>i7^K%fH=_KaGxz<3oYt zR2>02j5$Ux0Db|f6yyR>91XYBc6u5Z>`040xb|NMJ7NLYcwgW+b*2>^{sFtuMw^N` z+GH?VyFxlV_D7#ui=*Jwc94S|eQN*DU`L)>P+Mm<(3gPw zHRjY}j5ZOBcEv0@Y&W;?&k-Z^A2%?bCgdFRLoRZifW_XvZQ1kAg(sApV(k0+)Vx*h4M+9d?@QM=IDHtn-|*n~i4)KBTz?M<_6YA7 ze3$X5u&i82vn?!bzuxJ4lPw(G-tYc$XyYM6&$TX(PHvr1_BDR)LswD9ek1N|rq0xm z^+j2eLxPL!H7*YEvQiV7g-f{inT3nE=b44exL=ut3%Q4xg-f}wnT3nFH<^WI+A8q|n(4WW`;qCnkb98nxs>~Y>A9GDgXw9;xv9*~CER*u=OS(a zvvV1j!|YthJ<9A{%6-S|T+A(FcA9aAn98QyNT%`3EY7HoYlOg_p`~PUv`ASEkz(sLCr~=Gk!H z)*0{Z(7k7z)Nv~#OuVRJD(@&WZ!x!o*__l7k`bm{)M?2btTRsQ;AIR_ijoasR+Ek@FP~YS+>wJzrVwR_L6lV;I6*Zf4Z`FnC zH+8D=N}1(p9laTY)FPd!yuHkb_zv-mLCGQ$3vO-QkU>+P3NM2hk=S9IF(_ALV#)nf zH)Pc0q{2&LMkI9TWDLp_m0NIM)(y>X%Gx(5T2yY%Ev_5VYid^I-C{JvX_sWtxnllFJMZ?Rvd38g& zO%bZRLgsKvM|sAec+s#qx4dphziC*NN5l_@2;57WbTupCp`&7U=}sosC%x%`nOmVm zcIjp&*CM@1)6A`KTz2VpCU<*!(*-lPLdon>Hzs#OdeaUww?e7xQV%9~cY4z|GdE9( z>{1sdcUgLqf|;A=xa`txOs-pcQ;wONr(|~N7AALXdecTTH&3bT(p^k$Kzh>~Gq+BO z>{3@I*CxHm(9ErKTz2UWCf6gq>6V#Wr(|}iJCo~}-V|cy)+v=;>dEAWr#JD;+>|A< z!Rx#Yti188JSA3xF#8joBh1$u6Umc{PTK5 zU#|(B6Tx5)%d>`OcnycN3@vXN+S)R-u4O2wWhkv;FfWo<&et13=L|F0!wRfn9WP}K z_S4~>Qm^I{Ud=^b5f-gO&#J;MRE2%38r&Acdn(-PF3oAsV&|=4ncVVXZp83wW7Xi67@k-R?}2cyy)>s#iyf%N z4xG+*TEi;8=@nt!I`q0K>{C@(R@I-U-4!c)}Ev~^fljhLJbLcXh$-=!WVtAD?yw6pG`>F;X zR}EgS8tkeXjISE3s~S98HTbz|@KDv@tEw=oR^uC9%J!@@9k#kOM@qQ2o#<^B?v)~X zrG$IUV|eeY!Yo>iuY2XKW!dVmZKtzswb|~{99`kw^)bATs=>^v!Dm%rHm%0RUM82k zOiI1->{$&u>O z6YhmwZ*E+9?NR*t#;%>$cfGl>>kVgT$fhB!FZI?(b8-V0N=$tiRU9MQGrUnY+Q&&c z+Q&&T+Q(^Jw2zZ~w2#yHXdfq~XdfrZXdfrJXdfq;XdkDE(LPR6(LPQJ(LPQSqJ5l{ zqkWnsqWe3m$}(~K;`kptDJMK)?IdA#g@n@>Pc++wv-K(;_$LGqpS2uZ!Q;6 zVnS7nx1@vggzB2^5SD;;f@NzXOGG>ALF*3|UqQTP>wOkcka(f>D~nc;u%q=gODtSD zV$Zj>-7I5|Jqm5hShGB$a@yQj^E@IqwykB&_1OESEr7Lvv&XQ_hNa7iy4B{vGUP;t zv^lc$IeU3+;VcuSJ@mGPK{J)2;@Vt-43r|x+E(-}9*|T@scpFuq^Ojp-jWbB?r8Fv zmUBV!M^oKfvV+DSP5IPP5~OrAZ9+>-kfdXBR!e@6oMWne%Yh&n$CQ^X*MlZHrs=n& z1WCP1E^fILr0^~^u;ob5gm)>uEu}%q@6yCu_V!5_C#SWX?2|Q4wQb4hlQvFiXt~;_ zXq=|glGHb@EV;1dLZ5tDs%J}1-}thW&X!w!N@Z!vEpdI4;mLU|XZqyAQ=M9}`eedW znp@Jk)%#Wr$PFn*M3&!iYAx*6@7p*qaY$)+PsAOQ*1T?=KKlWMp^3wh!*?QD%e%`v zH^tRZ#p0^lg$yR8Mo(_nFqobid$L{DU}|cNYrBDgc4}OEyMTeJO|(?In!z-i*o<~* zgDEyKtJ>!n%&>`j+Ad-+xjtIAUCThTKK5$6qJesSjBmSW|cIRYQT;F6P`Ls&cZUZAF+X|^}H6uCO%2nNSj3zcz z=yq!vDKu32b{iQf>r@DLPr5CuQ)$^f`?g|Xg=Y8k+wz5#JG$rJR`RS+=$?98&a-l3 zx8CiEofU@N+P4)tD?_@CZ!0TT(7RPbWtA(DC;OFAwGoQNgC} zsEx6ap~%Lz`r7TR;Z$%qcRU0Yr1%Z5Y`n7Z$`!+chl^@BY<6zLMAl#@cUNQ?kpjbTAeOShh(SLmwlq)NzJXjef&f5Can#9!b6GWt(|>zn^Obd z)NCrJc$w!W{_h5x;~-~WxyF00!^D>rbG>IcD7>un_g>(jtY0DOJvld3p?xZAnn!F- z`*_wAkC=_^daM~9ac|niSd%%?hV9xcO-}5sb|scNCnlubn5DysF zvsym{2|6a&w>}RNt*;*W7JX`85Tx@iuD6}PPsKP|ynS+?hH-3KyHuZ=ag1=+woqDL z&8M#5P^#0Nn9(*{x#H3l(*OVeC&N!cEHpga@oJ&x>zUH@E}Pe`~kIA8|VMYge)Rx3`=D|B33wPcK}!H!0!Bhe?MLdOx{_ z&7F8>UC@k8BK}YM7r7RGxNFd8m3nH%iUlGDaO(WQ9j{N%{G;ET2)XLD9@a86~TE?#6`HLxv_Pn>h2E_|%`CqorDtYzrg@EMC zhb`G(-t*qa_FS|jUgVlaK08x)I$o$~c5+yr&1=`<9`*{ibt*o!&y50~uWaj6I>v6A z^Wg2Nvf0l*ald@2=gAWSnW~GHuWX*q?<2}B(7T~cDPfmd_FN*QC#2n; z6zBWmIkhBFQG4ws{(TqfTKD@Gy0AAUl$91QvpF{ScDP@%S2ILBqHRIgOrdHa^J`DK zMfvtsr+jv;7T#pL@$@vc%oP{sRIu8ro}XdSYGq^!3|}vCc34mrczv;jl|g}H>)}t6l0y5LybJtB^RCD9cZ z1RT7i>HO=xlm#UIh#67<&rK0ZQK|! z)ag_<(37R(6h8QFxbK}wNN4%*kHP-9!cXNPox=nDzpo5^_!ex;>wYrOHlbz?w|@5X z#lJmG8V;OH7*-DYEUvU{V|c&O;q_T9OFJFbuQ~i`nFtqC z@*1w&=(vsjspU8~?RNf+#E9`n({AS{@8;d@$uX(yxuD!N@YOTgJu}3n@!AsSiVdgI z4xaw<;k;kZQ!lxe=%>YshuqcQ zY74f$5#>JFt5N=_{N-MgR$jke@v^oHCexmK4%ZG}D|z@*;^nsG!paVBZizo=sCgb% zyJgn0Un$?5BDf~LgD*`6jbGOO7Wds$_NppJ=(j|jP^!-lqlab3K5e>wGoVQ^LGHVe zmhcT9yMymKE!KV$4!`k(U<~m*_uop4s~H@VcGiuMhhb+PYS&h%HK# zTqsq#x+I})$N{ESF*;#^s(w#*eBh48)#SMS+0F+yddbR&1F+MDxLPpS`Ze@&ke zO`Rb7VVUuyoVNSP3wBL%NQpUgWcr&KLd_?#A1$}vod_Y&dTC;iEr;?FDFkte0`VN%*XY= z>WeboJhru3zV&jf(wnQMQ==9?yj6G6Z+}b)%fQ?ALG_O4gEP)pe>^H~9xK#Xu%*zY zadlp!x}$5)*X)LAfg2CKx2e6eU@dEbuE3oV|H&U0Z+d@+Z-to6tR3rJ%iY%Tb^2At ze#=;$79+6ao!Y7dwNsy-ZglwdVZ~v;l3Pc$<04+QeqA7}v`hc?yzU?$#+4 zpj(IAQVHSX^t7%biu89?ZT+r=XLm+#Y!JNXov|x-oWzRe_V`PZ)DFSOtbEry8l4l@T>P}uu3Fc0&V(QL z%oe+F-tnb*PqI_3yH9JXDVitnVfOCueP@L9)w14On9SQZE$zk`i|pf94*U$DD^Mvn z^_-$(p8s4Vk~;2uYS4@6w_I=RiHf>>^^$0jp`+^TDNB#`&!`K?YL7QAUPNmz6}9b_ z`jP!q#Vn$A)#oEt-_z&r^3W1f68{zUq+s#R5FeEp6>r`Jw-{u-)QGXwEE4jWx7(h| zl-?~nxi@|f+sbrD#nK&O>jLJjKRVrQ(c8B2LSw4HRk?-iEkB&d1N){eIG^s8n*+X~rdFmr!KEH0ZE^*qX zSkK@4Dtn#sW3}(uGWW;VF|*vu=UY;GsajTK+*%CT{Yc^Cr>MuaoH)WkgB0`C|5M zZTxw~yFaVvFMIlR&&|qdRoAS($aXzkrL5hQbk+KjjKGBuyIYU0e;WwhT=&9Dw|yCP z)z_u{@dK=NjMr+CT4itF+0K@U5A`iKTlQPAlC1BzcP>ZWd;0 z_x3r((CwYU4ArjFO8sr?l4tHY+-e@#Q&qCS>}1R<`=HC`O3uv_dX%W<@0Y&b*ycpA z9A(NiUaXh-xzC!P)X#g&UOIOz|HYH`r>7YFiuW_A)hh}fHVPbDuB_J(bt`;jT9An`eZDUxu8G^q5leX2x{uaV*K}nu=+{ zE0`80g!5?&+P+J6_x*Or3orJdcCEalYIM|&VtI1-u+=-d7js|FRCyJrtCJe+tvf6@ z6SjJEe~@<0u|M$JFEQt4I^X=Cy|HIKq*SCrSIFwCP1&Pf@y)0NEf8mafO3t;z z=O!aBgE~?0^HTP>8%vgFWjs_lVNU&E zAMo@^SV-d2^fzbLzj&Il!dm^|ul4nLXDaW^o*QYkkNP#_kXff{DeXXZne-~V? z{a$IX1>RDRAlH?B*5q0rz8Kj>evxonFv%zGM=sG~|oh6iL2A%17-6J{;W>(IYTqXe%53 zQfl?DwB3BaBib2Nd94hNxRL6x_tD3LF1DsalWglaCNHEM_wQC=dm{!xYNhDRA<^mQ1LwShylA?0ZI3Q{cM6x77Rt zD{UkaqYOlObJ}}j#I|@JwOMYm)-1EN_S)fH)xTFumb&D<^;YI-FFZz9W$I4?ca+my|(V)_>wJier_?n z(zbi2e)1217|9yqD#_-wxG& z>Ktwln#Y^9Yv_00gW!%*|AM#%lcAfB3yRq%OqabA8rV1cyY$k`ynUUk0&fNg9{nNv zcG5s*=!aILVw3A_R=-Z(lgqEGp7Ts`I{VBE;a?5HLx=i~pU`b}UnA3NAh_w}mLs%@ zH0uEohl7;|l(g4}&YPCS3tf6lC(UR5t*~cb+*8A3Uly#o_S1L%TD8)rZ&%e{IJ{CZ ze*PhWnI24;nVT-XpBlYjiJb<$$iFGN&HQjjMzTtm0AKh{!v*|}%S4vstZlq|Wc>km z+WDG&S&QaxTUER}^P`lA1FD}L-c~ZZf!n6F#dt0jlF6R(wzhg61 z8^b&Y5_n}HOW!WJ@}Om=a`kOKu`GGDO0|co)!m`RYuakWe@Kn5Y5bUyG^3PH(`vo` z1O2Ckzl@WeeIyTA{@&jE)%x3nTSB)wkA2w9%2MEV>O4HYi}C!^>iXXzS@(Cio&8b8 z8a!h5?U(bieD`w`E^KqYTUBkjz@g}5omp4h;H;~~)6VQ~KYVPHN{O}T!E`t8D};_u zXL+*JmHQdn8};1xPc^nJ2{At+V%d3mtMA24`^8*Re2fiND!;jE`aMme!+*hYdBrff zcgxoKf7#wJSAOb(PiD_&Y4z^PSV3>Lt}B{gxOAVLqs5}9*V{q~EvkBw^)gOL=}~D) z;RAK8f-iR3^XDDZwN`hSro&oW&M%ldb+{BwC2_WsvjqBmom5^=X8%hCrN1Q{HLD3ZQ^5Ojr|T%lw@Lp zq`zMte?M*h+>PoI(z8@fO3d&*_f)nw+wk1G!(r}^0vW{g<1(V}t6v-u-=i zA-f{`#+Q0?kND+9q)KTVzw1-`L}teNY4y+J9t-sp{{E_Tx+1&7CG&Y3eIxsXWpPdb z$4vIu2}zT+T^T-Iuiaj_*sgsr^}9WtbG`0lgOc0+OOo5WAKAI}U)$g&b8GO9<7ukY zPjiNj)|8I!oA1QCwKIV>sB56=xh3ImC3rV$7&X8 zYv}Uo9=^~pKCA4Me99y{^K`(}U}wiz^V){3rxJdxF<_j1Y^%cd?h<|E{Ai7^exu9l zqJpkdtJz)kH=cMF9=fQiS)-Yz*xYJW7b3q@eHJ}9aQpnotc>-$O&qhfExo0`X!aJD zB*|ZMj3q2~M2TEHefs(mPML`OlMW$;S+heAWaJ1Rw(-7UzS-4S{z&(>;UJ&SahtQf zzx?1{5&A)I4g0#Sp|8rgXr*@f*{cQIc~j3uNm>l?r7gL-GiNdT+gs&_hl+RKcjE0m z^(~Wb8&Id@mDUrmElG8d%8*Tb*)7wYlQFebtfuDI;V?zhQW@z16@R_f@^2b~uQn}M zbGwguyuvO;u!upjr z19}DJKHSa>w$V7q44vk{cgS7ksZ=88)zN2NROLY3K;@l*^rWiCxdPu@w$56S=&YFB zU;E8t#qz}GT5$*BwC7%=l-928a8Zyp59^o6ZCA1WG$V_9^JeNn&l`g+#2f8BYhE}M z)EE}1ZM#_c)BcciSELt=UnkqX zAt!3?T@~5aMc2fADx5R)^tykAuC1zcZ9Z36Z?Zs3q5?8WZf!sjA^?}|Mbb{ zfjY<5efhDwxYrS*fAZp+seN7o1Rw5_P0@Gg7~kDuQ8I$+J$w(JLO zPhT@#-f!C0uKS*UeC_jT@8s>?I|=SCTo@pf-MR9`(Nkt@@4Vk{%E~COcX1e^-EG-l zj()gfdZ9s#xI908hLgbYP5$2Vx(_%w{CMS7b;HwLqiS~Z4c*QQduA=q`<^ppR@d4o zUskH{aWBmYJo%Mtx%Z`Qli9`J9eu2yndf6aF3QdsDp`=%HSP0~2EPtr!m}hYYHh-J zC7}OAhIc|KPt=DF1x&g!Cu9Aa1v!(aC}zc% z`X2eX>rq%;D{rr|_fn~Ok7vB-)zW;kdQX_>;Y(V~f_;1sQ@(sj8!si8?utA9aLx0g>J_>JE05*5T;0{cH}^h0{?jj|ozdR*jS5%fQtsci zFFhp_{hoTW=&eJcF?kfC<*IpWU(1t& zalc$AP)$Pm&OdSN=lDOQ+WCCtQ8{+kPCb*o&rLTS_`a&)3ZwL8FGoW!amucxII9rx?Mz^LJ93K)? zw=xOjBi?oE7`FB;P*;p7kcu{W$@rjSczjxU`DX!{W6hjm8=+x)o0&tlHpdD~S_OW# z4!&Rnz8`+bc>VtX7C`C0e>-7b|ELq5zwZRYLF(tdj!l6|jWC_ijznNzvVrE-9$>U5 zX_@$?&cc2YFv=SfO*Sub_lq zUR{C3D@k*{L!UZSeV+~_NtWbyPMH6|35*zQ@cN%l_{aaj3IFypC;Z!==LGL(PS7Po zuB12+7)nyJ$mZO%=dkdeW5y>-LFeY7`&PY;Zd15>{4BTn0m8j!dA>0L$_@jRdTbJV zi)lY0C~R*0Kxq(Xs``5KLvu6$dVA4b1n(zw!=OJ{27E1Y6`u1U?M$ty6Wlm(89|S* z7<&;|c7;W(FbiRxQA=K6lvx=dPs|}-!nnsPL(2VWaAT5C4hJAdaznRDOa=joFk zq6XJ#VU9-m4R|4D4h`g9W&zLoywZH|!UlfaOyFTYTa4@mEK;y9`}ug3L~zjEtm%_{ zv=C+kQ+U87U@m!Lmbh=^YtnMc%EvWs9#pI&0MAnQHiu7IUck_0Pg-!MH z(R_Q#Vkr!{&y0Te);xr8q#qjav)f&!By>5AJ;q0f%=UcTCDE`V6uZIpU9-MwT1|iB z1Cw%b#U;rePzkFDpInrK1nMF+Ksu+0BzzbQWi;|R|1t*$Q*FwJ`~uHr#afVJGR6Z+ z!+Au=x1Ml}9SY%zUXXl=Wo%Bf$ zj5xee=fY)?H1-pDgq`h7>L!>g(GCP!=NR~`MAcnLsM}x7kQC{AxEhE@8GIo&^C>G@ z2%nfvAGuZE)(B9Ji!Iba%fvp!riDu}(aQ);r=+rVpkWo;C8*U`S{HG)8lV{;S@U|# zQ2I#ugekOp767#yJ~>mH*K*&cT7odg)%U7>8e-bFm!IBrX zPUUVipRT`KH$&9CLxB7h)9uLHZGL!DqQcYsEMiPoo=lc0h@i-C%J2DHu;P+x7}w2* ziI?S%Pk96^MDhad+N|iGgrQ`1MKpNoPkWV+3WzWob2}P&@L7E4;Oj%uB$+sY=EB;G zE<`n{ZoP<2l#mM(O$#2cgK9D~)u8wia+d){T5dV~iRhaR>B*tE*YBl)3Bu^+p&;Gy zCDaGA%J7lr2vc8`&C0#+v+|(SY4=FjlD$2=cJo@tZ!}0?C4E?Lw)JM4uwTvzY1Ke| zk!$(}*r=qCbfJKE?p3qZAU->LN3Z$A4EGfLlqdBu^6%1BMBr}}k(Y)CU4Y!DCz`}- zJ}1L+wir|r$A0nPdKsIR7*G9~k(=B%B<2g)9UvgJviRw-`^JIM3mNAE(n$P6{qg1j)KqtwdummixeS`(Kog#Dw5 zd|{V@Jbk4J%)<{LLe!5@nlInt%sgP%kZJi7hw>M-Wq-Akg(Bd1?88@k-Er%73wWm_ zIC35$V9By>CR8;%mlbNhFaivuwl8esbl1Kcy8mX=MSH28-~8r3z3}&c`=`C|FTeUj zFZ?+EWiPBhygcla#TB+Uyr(Whq;r;gggZdOzexa5B@&ZOz89(LE z%#u8agEP@Dw#r{#BF|NLtMM?$w~JuW*FZz${Dw4fT$=PMZJh=^#IOswG?naVTlgAa z10IaZu0mKe>X0HCyKCXcWvNaWz)uoDDV=9Zh2!w&i-Kc2SrjU$!B2)TOjbvu^{)wn zX6plY!|9Q^Yd(u>d?`D5Z`ANS z7x?ppD>%qIyTeXwL;E*dmsby!E#M_$r(OcaZUMzrm~`pU*LpJhu-V_!srE$;zrsGL zlklAp&E6JejRWg-zT|#2WNXw6?e@b}JwP=o_OY;rPh*j>+t~XqVmj|M@mt=*J3%{{ zM^ti%spgp86#49RhA5?D;XlK)Pst0+uq1w2 z|9H;uDvxm83W4p79KKg7wZHN^IA<^H>)$ zR~@n#^hR+JzII#p^XZvfMRoKSNh@Lt5&CY7TG)^+yEwLYh+9EtF3A~G?hIy3`}B-F z)+%v*d|^buR#;Vj zSVS7E{6aDJ5^b`7z-Sg`RV!O0LB+$+WkMM)4DR_?!?y7Bd>W|Z@AL;6H*?32Z@Qex zc@jip9b1~K`7LCawXEJeU}*Aabdqe?N2R3W!gJjp6D`kUR6XP}$JWk%jW%wd*!4;>P%oTAd4Y6v)1~*9}Cp{rAID3i{r?VD>%AFEP?t)Aa4G9O*H8= zR4gV9Nlc{BoEGr-ftUIf?`|bK!zoP=!iLCuJ^x@o9@L~VTN?6ji?IRy831_bjO$< z$V$&2atTU(s#ZIiBx0i91r@z z8Z+B!Bp^8KLwitlcHp6r^$&m^dk=ejY=qWntSvFijHO;AW;L0pU*f)8(05f1j~O}tkYJV)96l8avFc&IUOb7 z^mk4vh;hq?g)^|MoXH&wqn-3yUQVM# zo&jcL`02+d9O~>k9X|T$T3g*)PZkMw+bvGZ6r0PkoJmz6+%$yWz+n7Eq8U;Axc^ePSMsYdueXdSGL6U`4aA zv~(;WQUZv{rFDWzvoI^zwB=qWAlUXbLx_1Gc12)81=43!$^64V{M~>5<1g2LeaHX& zn?L^e&+q(BfBeV4KK|oB{mcD-c*pvc%gv=RVS^5G&3COFx0JY<1i@d*fMIthfHYIu{Jm_0$ecQ#fn%;wOsVL2_&O<@=0nr&Aay%`tD#_+3x zX@?z*?^_T@py6TN9ftss{=lh*s16n`)ToBU4LVzeLMjVts(2KFXXJ1Q-LlM4;#6_nb8XDkQ7zp%zR|( zK*)e+C}tr^qrS9AY&h6$mmh=J3dYwjkYmL>tN$LOOjq=B?`)zY$+bt<~~QB31#6u+LCT z%bhZh)iNYX0qRn}DTE#!${uJRG|sL^Y(=X3dc~R{?do?BZ5_+aT-qTJIqrK+)(L~j zxIi7?treiH%4y;k(JRZ`rNKK-`>`NeeM$j z#N1z2#qh&p908nU4$i!8Dx-$Ab_ly`FeUIngF*RSZ}I@%@x?j_zth6Y>a1(BP}br+ zY_BcXSL{q5*7D%QcIJm|W!LI^K`Am|fevFsyx}!4f?qnt9G0LJ{1ZSwex?aN6fl>9 zjE3Es`V48kS@@hpoh<(e9LP#vx7(2Ji9589fIA$m^f_HRE*%bxo<(!M;)lFTb zK)=_995zP4+O|^bGAVS^)kL3SU}O<>-58-jSz%FoG8VS*(nm%(rF?*fh_OQ%iSHDt zWD^QERC1FsD^YrqeYr6d^m)${uZ>0>P{8 zM{Rmi6g1a{#Q1n&z|YU&o$`?M4O-Cv7e@`$VS>i=vVNDTlE|xGd2*03rd;g%-z(PzOUEpNtj7C3_H$Jqb z{`Ls}@jgZ)_Ofnm;(wF@L>c|nVXM+@?1+4})C+xxx~!EJkhA7G@vJU+6uYj7L zdoy|d24qh{)GoRT7gKs7(Q$7@po|wTNurUCdKtCOlp%=yNRxdD0zTn% z@u2BSW_;G9JIq+flqyA#4+5QX5)9yr$kV=o|LVW-t!8FWZYOIhrdQ^O{2VvxgD^h}S<*-4Znt#th$n#<*BZQdR1PcgRue-U1 zmhkfo6Sc^-qEPbkb8-a-O~ArQb{B=c(!lR98B`Z7Ib}f2#$z#AtY5%R1Ngv%9*7XO zi8S=*t_dcqvP(boZM7jKqr7pTT0EYCBd<7{ziFtY?uO8@qlA03pIMTF6dyaSG@k!o zIAi)hoFR@k@kJHBhQ>iX+q-g6j`^rcelHr_WQar)z`d6jv{yg9+ht|~nPRWmBRET% z0ol6MU+sGaBA);l9x2bgt;4|3!=Ujc(p*WX%aSg>MXi#nkB?o;?|-W3hdUKSZX#R(xMCieyS)uZra}MJ3uw z@nQ|Yr;#difVLT5&bpJtD2&eSL<-e5vtk;8Cm1z(G|maIIU>SsnD`Bz&ksIZ*h4;?{Yk zX*RQm=Lbqe1Id)n!wwXDvcCI*u z>;#F~jbgW7GP>wZpxe!b{F5_E zh(=`qIn&?IT!dzovnKh%-yFAkmSWdCzs9yOC#X*|F!EF|;X(g&Kx-Ar@Pwj9yEhBA zwwhXC{1OrWi>%wX{-O*5r~_Y2;387tv#-IZKz>12AqE~1+LbqDWt$v&@p|nJOFxK{ z3)@p+IIcy)1C?5Ey8A%H>>D|XkL^na2Sssist-4Xn$&5r^ZBQhU}e!Cy1eY19?Fh_m|Fxq)c0#lwb4#m%crLfV5*ZfFE4lx5`~Te zE+A!1gmk= z7ykK>8W4x=%z%WBv~o#B=6C2Ye*?}XB2wH;@bny_NzufyriYIYJpLdc0Mtw~zt#V< zroo0p>`E*#7z^ktO#J~7`q4P^niRhxeX;$?7rfgwmPSdXj5k@!vP;XOrCqx1m0l;N zy@=06YPfmvvBIvUNJ)-{$(+TY2$SI6^NzGdlIhTnG$9<2%q*7$Ak1PLzD|MVkFlwf z=}FW0=#?Tu=`j-HOz(Gkwvov94vPx0U;V{r$(M6c5IZDAGwq~iqbIrCK=H=ZAtfK> z7(HHf$X)(^x#)JKJeD+Prxf2Zv|hv0(7r&_7Qwn9VA41>=^F00+9u8&n}?J2HL7SI zA1a!I$=OJB_98~gOFDP{s3{x)RRWBBzQB7snn>&h2+bcwu+gosF7>9NfWA=>V#~9v zpiWoA{16&8?(V&2ryh|#lk|WFDaxq}D6Zq2`#C9^hdL>#D6#%wkU~Nm0&3Vjus=yD zbNPOrFCu=!jzovUuZS`s*?9aAIqk(HFUUL+movJ9<8LETo7ZAXE}XL(?1IhN8K=#U zk)9&`SYMO+*(4CS5&r}~iN+SrMDOqQA)V}=#1bM$fJ9)2D^L;ui6nbH5FmWb{8P2t zEi>Nr;t%~$j9h7^gppq44&W}BOJmp)Y>FD#xKcf4vSPK10jCXUwvi6vCIddtu)q9{ zTUt~8l00Gk$eaur&v^=h(rug7CQLKgO z(ELyW7~WAhnM-NR(EZ56HsmBV+@lMmXyRic^b?^VlO+jz3;z#i{NumF8Op!n4CUYC z4ERsZfIwDsm0J*CZiXgRv%8l+T$vr6{8uWl)b4^R=1fgZ z(FGVV6})_}9E!1bb@+Vz2nhlt_wbjE*RE@m+_>liJu*@!*RB#7XLJkemI3@HmSsWz z7S-y1w`Fy-GU&Dw#A7eE4lXCu@7-;y{2{Ya8Bas48+l}7PVz_8Yyev8N=ItZiyAYo zBZS4IPy;TBTL_{`_Z}8N&F?(INvi7OE~jSEXwm1%`P+^rZKa_uh$hQc zaWZLS<2hzU*vlcC?Sg_MbL;`Y^?JSrMukS^-dCjF=YVv~ zqc$sql0|XAyA)B8ybF#>4y~$Q6CVLl1C6`}afKR(L=*UhDZUZN&@hvMc zns2y`3E+?cz=x2u=Gkm(K1ItHtTT@ZV}idyN}mlN35M~CB?Bo2UhslxHs%dx?XxHhN zI6qLX;6U-0(TA^4kVWJ4s$0^}>#xruqM<}vKe@N3v)~qLSmTJd=g8>f1Nc$JU4y?; zE}n+ZTS;!xDF02wNc-A-1cMMnkr&^vt2mT|RyLl%8odTrV-iTG^^Lp`wrb}wo) zc#HuQ5F4Khj=H{0&BdfGCJ%hW8_Fy& z_Q49P5KVwn(+%ZxDPI18KfOfLowi{QX5pe<<77h(Lvb1A&qw(I2SAlVY`gA7Mb-F85*^D8v_<;0|EnL&A;Btg*EPHex_Sf%p-kM)DO z=LcO6W-GD*V-74P@S)ZU^-MrDS8M#)tNa_(a)WE3UQFiX*ESa*;D#q3+$6K$^^ZW0 zQ3I#QsF1tB!!ayH(us`$ALZ<1t;n4Huf3h^`d_`BN0I9}LKV49ShV0#tHrGp2dwmI znhg&DYn8 zwX4CGX>sbI)RZm!|C>Yp_P_kw9P&^9ibMYC-{BBca3H!i+a9y+uX@<+IC157J-&=9 z|2jJ{DnRWd$YvE=({!B3`r|~ZBk@VC`Zm?CEcf6Xz_MAYOM6A!8eL)Th&eb)(UruW z{GcyPgbzK8iQ?`C+*1I$JO_;=E7z*yn_5{8xS(HmMw@^H>v({=dw60wfA(17q#$hr zeF~zPSMw2prinmAHc$(GbxmG)-qFc2^RJaHUYh5vR|FPlNVM&Xx*)WCy|bPIjL2PD zh>HP=8~Y9ta%Bk>w6|SDR2F(v3!|Oi;c@)6473z(eJWZOsA6%+<;=2D_O;F6M)Q}& z?kN#huWg~*;~ z`wr6ftmTH)DxY|=sD9OFo5kg)#lIhr)@!7G7Y{YLFS6}n~&C@qs4)pwCx21zi z!8h%Lav_j6P`H~P7ZEIZAx6yH-H0Q;zsHvsNCxglEt0CCj|TbzqJ@+G>h3+CVtK&# z+ZxanyDoiUyG0;Ma>^+X$cr{}xPGPAm`7`yh5PqF```+okkFGt&`M12ELb{8oFMBXJX#2e}wDQE0>}=_$ zon|7XJx8}pV!P-w8c-VJ-mru_)6b(frNY=~COujJ1LVygH`k{TXFKW**$-xODZV%3 zX51AK+YX-yrv4q3G^Ar`=a0yku{B7vGO>5!U!zB?saDb2mS_i z_itNviQIVZdly=i)J?dfP9Q!APn42diN0rvg#xx_vAZzimhHUqtPz2J<+9fAQeBV> zFll7XGYdn30Qvo#lg@veDJ@g7vQ62n!96a@5-2$}cMqZyI9^K&oo2`1zK#uME# zK=FW@w{5$LQQXS-;o^JgmRChY9^3*wNQQYn^bIj@Nx99KLPGv2iHACNtWwWXliC1_6zB)Gwg&AnIQaw- zs!NSyUhU6Y8k#s5QsOf}-ar12zyA7%zyA8`fB*eofBo0@{CDs1pa1&T@n8Nn|4;7` z|NXz7|BV0kxA}SxQ`-L1Kb6ZgxZBc<)qnrTLVbLx+YSlxF;J*I^%ew}`$4m&elSzf zMMJ8^IN&Lm>t;dte3g$7?`FQA2eK>{>FwsJ#pmygd>!-X)1hxn95Du4qcrG9X8BYz zY1}(T_s21hSFu6U>Unp0*jRX2bUa3StumD21W#;P`N(k4-`mulOk~>AEk)BkB-@0%6FM_N)~gi7Ni#h zV!PXSwAK}IBEOG6B?*}Arh*ej*iZwASdsp0b&|Y$c2O@+Z3xE&O1s7K^L_uufo=Zu z&mnuS1N*D`F+D0L+V9Nh z*n#7alY8-dvmy1y4F$CA$^9Pn3TH}r#J($0=8XXO`eI}Hojw&9w_^Edg0+jbf29F{ zges`gP&pQwrR+B`_(5OwDLI-V-ye|uiena?L5d0x64C>*FRKc^=oJibG%qG0OQ}6V zMr_hl$uyN(*n@{U46R2$oZK#XV8&-<@l8+4{CB03+ou7O|M+??(X$Z7Cl{sRU)d;f zDIw8j>w_GaW9D*k#@=c+2N(a)@u57xgP$g0=1atjaJokahb2I-&FhO8Qe!>R2yu2O zkznq?e`BLYxQ+@)zuX>=q}o(;YqL5TXG%!Ct#RN>S00*_;8y;Eo+qJGF1J8nA%EDY zK8|%oKhGf371(IHfk#cf$aJHSeB%X3)WKbUL9%rn3tf?fI(xKbL1_b0Fbt}{Z+Vup zp8RH;gM|TwlD&2XA*SFT`px4wx%h3^hwK7FM0ehnKIEaXx8fWoFaWqCH1{SVh4bML z?)S7Fbughgt_*EyXtW@RDMQc#iZJ4x`uIudVgGf6zZ%L<5Oo+u7>ZAg&=Zp$_J1@V zv4;zCRCR;nOJ|e0BP$QtKR8*=fNTvqe5wV*0!&A;xdmVY!O(g=NrHhdCgRO<uXU_4!O_F3Z3hQD5~8*CCJ>H*xq&x!~Ok3yd@!yB-+UKhQsOilu$ zYwY*^eCZ`TA2kMU`~j6oqF-LH?5Gfc7?i(woPKQkx#$@%yU5@RYAWPe&R~po+K6H3 zanFtl3;%pkljzSHL>fZ_0Xa{W@XclAF%zl@Z7EWs4aQAoBo4*O$T`K`D{hcomJ_Q* zpg4vY#}Dbtl-Q_#Uot`$q@ft1babhuaaap~h6jxJp>Dd0XY1l^FhXivs&sj#W;HD- zg*<|X&&d~K344?;u!UP0Fnx&KfVo(pUMJ&}#F*jCku}8K3Df|*u0MH-c4c`?&hwE> z57_PufasQ`&M}oin_3+rVI3Uk2+5XVc}=rEmH-i$Z_kC}v>na1Qz^G)1+ioa_*-HW z2)iYYnJ>!BSqNMq=In;-8MOeqWFjLA1i3(r@FuS~K;Fh%VMG@9 z!1oaXhh*=TVp090LbU7=ERC{e?jDU?B&R%MSFNXn?WgJCi1fm>zX)9M>J2_0V5ywT z1F5?PhTw?_AJrTNl!1??ec`0!C1IqkUEW>^{gH;(>WLO#pIOA?vqZCW=P!tgL zhw=(xA{o7j2>6)LX_xoNnqtEJneEQMys9@Rz{wI)G;|HkrTs!@g#&VVrtgObl=Drv zu$}M>2+|Rce3Ovebi%y~8i0&eK0&~O%oK3rfppAWC+k2)vI5$UY^f}2z?@5cF`BG^ zmGq&8^^wfeMs&&5QgRN`l5DG%KgT$K4&MuBTYY({@6)jdDVfF}Wo7>PpZ_q~pYeyw z{@36A`&{-v|L)&%*`N7;$Yo43cNfDYwV!-@Valw|+O$7)`{daddo_c!7gEn2lp>?B%{5&?DD=4j7`=}nhcqQ| zjN7tCQl}FwL#_jyOyQt@q}~6h^%**LJ}{4i&W)dopF$caVQwfNy;&DT>J}Zok<|*iJx-DsA>CL!)J3q{xXt{DX2>iCc1>_K{%rN;}J1|9= z;8kU2&jm`Y<@Hu*@a#{Y^D*zIU%~*SDu^(~8sg|2^Lx^CVE@xRng#4ElnK9#3mL+l z*kx3%c$W$H@6vU2%mPBs_LNK=n0PgltAiG0hUXD&he@S`Lu~PM)n56T97}y~JJbVr zUo`N?>ez6@pj7H}O_#_1RYe;o%R`YLHG4tUd0cRz*-xtBBlR6&*l|#{DLX8}gyj|} zeD&E5xwQvNoWl!GWObN~78!>YRAJ5o-WCVxsE2pI(Zq3{eqyODZp$u4x~v!>T3;7O zf%SF8i+=ccW-W`m>HsPWXkd*yxh_pjES_i|@cf38d^^Es0F0P%T`jV;=K{oJx>!@e z@JQ=};p^co5g3>5!N7Su_RByK0mi`Kffx?S2%=E>T?T@@%JdOo`Uz^Fz++-|7v7)% zc{;7w0z{7irY#ztHEgXun4Cy{nKy1zr6=QP^EAua%&nI!T}9J5re!8T0!{cnQlb{s z=!hUBK0bhjMZkM=igbBz{vMH3 zXh~g@n0IXjGl81zhFEef>K#Zc5RMW8ZL$)ik}lPgohu6GObc5BPM;B1!4ZBYLC|me z{*14IFX&`eUj00um%|84XGci0R+Bt(@#QdAr-WD)=mUE>jD0x_n_mulY*9aPS`BeK zS0f#a`0|_FoRw-IF{nFC@N$6X>j?Q-T&~%hg`aVJIc!$Kn5c$&8ea|r`N0dUxP#aJ zG;HhIQ@SG&&mWPSbPnYp(9-!q7+8Vcfck^Mhsp9>KSD&6E4`dF4`9gubp_`VJ}J-| zHMcMk7sV!m$Ka8O}Zknf!j-Zl@#3G>$psV9PpTR%=OG|L)_L8*>0fX3HtsV9Zm$VoKQI|mE|%sxG>-$FwKQFKNg^`{K>=E7Ks%~isttCOXx03W9qPHYv9Cf zjhEi&NI)cC0RwLwiX8~{%9)m{8`dmR4veD9^I9EQtd^H-#<0UJN3Oe#%D0hGNL3{L zTDL*yTyJskE(In=48V3{65ie7jKAmbP8PsHY72eqN)D-0&;HK11iqZNL(a}i6~ytk zXp`iAIiAtl5Ht_i*niEpb|dbGp-RxCG5VlP2IFg~{c1HhSBlAsY)SmQ+;!HbD0PJz z$}cmK^l1{Wq_2D2t@p+-k41;}eI zP(zwZccVplO^<~f2Nz2dvx6|HPJejISuy+b{wERo^lzLYHaq`m)`j(cs***s z_F)9|G5#NDtYMI&WR}+7w)U%P56=;-+NHkq@*HNKfASu&W12Jo&N78=ZAo-}C?KIt zfL`J@mpP+zsl8GG3LK5mUKiB$Rh@c;fr#wIxMrhHVFk!ETTAR8D+3L9iAR{8CH~q9 zL83W*{~Vg+BS5O?O^e1_hZ(#Di~GgKkBJrDW=R1iC3bdRPA)Z4zFsZPgBkg@dq-)C zI%pufC9r^!(EG`yPpFBk=P<}ql4J0O8QorLgKXHF&r2m*;w4m+=;B;z!-=}&AccPr z1~tRwll#BwrUOXFEo9WZhZnM9#X+|1!=XqZ->YMn;^8=bs`$!{_MJcG>_+C!JM1H*>>g{lhEi_%GSEsQ4XQbGl=@||k>92thAWcejFCkB)ZMr@gU@P^>c53;wB z2paUZ73IOyNE?Fk0JtF%7`^V9J8%pZ?F6$=!udSzV1edsQipivHCRJ)V2ZeECdVNz z0BmTC73SF;LX3tF8u3tW7m#8CFbo>_=WQopyO&MdTSL88+35S{OKq^{4pGUB?g*<*zllL{k*e#!PIv!gR%*@% zErbBLx+UW(KL8s$VvK1-a_ma4PQ(~);}Kg_D{31Bh-Y!9;RUK*n92OgkSV4f15@Xg z_H0Q9znqF*R*ol1h0EDjqm&5oY12sa87gczBUAfLud&?~pL9(hp1R6hmLzFVB2vh2 znI&TE8V6NQV>*?dH9(Kp1irCH+rXJ#$3=ctl@#72`1^s1aP(={`jm-Ab9^xc^v#P0 z=JeXi4f?XZLyPD_0qmf$`;2+M9a$i~oxt+KZ~^o{>ds%FCn1A1T<;Do z%JZ5;lc9V$YlgLMn#Eq&9tX_BqoadK5ww@?&ykUBf{+Bd{fLbeB zKieHaZJX}Z0@@<3pCsF*czMy@Hp@yx(4s_`A16r7j+XOR?z&LIsR*u=l*6pO#jh>S z(HE#BuuaE7w}p_Lj$f4nZRFoNbMJP07si+*_zc6K#~Mc;w{DT~T5Rp)V<@Mh(*PSSH)QZL|>9q>^A%*A=!m^jM^jx{EkxXW}&I{yF zbP({PgEs{0#2!5doJl>1rm>bw#boMg%+>b57K)0i;GT=8`ZMo6qRJ$fvXyh53hl|C zIfuN9{-Ngq$*Y>Nn1)8~gT?eV6?4EA#K4lsaEdkz1ff+ZX@#FVl)+!V8)cZbX`nh* z>VjljH(10|m#;ZfqRVHbOu3LTM1-!e&liNx9suz6yc`R9@8o1g1U&K$GARs@ zrl_yS{N*qI{6GKrD-+fonZ|M=%zeST649}@I~WPXDoJcMlFh@o-fO4|^{#O8i6GJ4b9 z*3%c_80FKY?0QHWdS*DBZP_k!gy`KqmU08d0)!&b1=j%F!<1OM3&J#+Hx*?0qa$>m zE$|MDRh(xub$n<%cy!Y7G)3a%AQ@e7^3}?+?!@%Ib?}H3dgWk+SPCnn)X;<2vBBra z56*#8Krw^evS1*B^l)D8J#IS)n#5FW-qo2k!ojNavQ&nST%g%wmK3$PxKIJs3~Rym zR;h2tUdll*yVhdtbzz=$NmC|JpHb=O2ZK>}-v&GQTO|kU`Ri%p9B7}7CXq#Cuca{# zA9g(burP)N?V|rM4Xqqi0X^-hLtJ!uQzIvp9Q$;S2@kA=zuM(oyphjg;{c9nNHo&} zTy7@sF6w<1ut0SW0hGv}RizJFmr?yOvP~U`@%LG46kb?(r)~Do6{3ftQjs-?i0m^E zebTL*M(K$7YU6_##?P>d`@BW*h18r@f^rcC*e6g3jpn5t5UtQFT(hDtR7)%<)lS*q z@s#@(Sjgjq5qEL=`*MvF0!qa#v!(bVUQG54T{ z!8yPL9A5Kuq-3?dGLU4}e3UO*9L4f=YgtlZdf*4?Dg{UqzDOlgwFGTgUDh=6k@Vg~ zh{8F&1GVFn0MwQ>Wi5?;NC){HLgSd`hWpG~w8$QBRiQIl{eFS*!l3VZN4!mg-P{jg z0LvMfh}`aC77@KRV!sQed`?QUFfD@6MVO#D$cr;*!43dO;ZqF00;Ec;)zHSTqYEU< zSV)wYI@ul1p8s3>C20))X?L=i*n?-SS&hPd`%tS^Xj{t^ecy%4vLS-BSRF)XDZK_< zdRT|z}Zwv9IR4S8wlG~Q{< z+&{ON>%(gI8TrLG`-AuS>Hc`K&zWE*F#C9{?M?1rL{bl;&JgYBpPu9ulv_@cB&Hx1 zd{%@N8oLiqT}33zlDFEYuh653-(h$ky(1@4nsD^$lUr_pZIsa_aVFORL~&3f9}wy0 z85*Sk-%I|1VzcT6!2#kfSxgVxFa&x~!O+1~YM}3Uk}mC%(%j&&f`F+kMf5r#hUA$G zGQBE6B)|Ank24{~e21q)YQCWl-W09^vh)@WW1mOzr!(pd87eN{ABqnQzcHe&b4J?r zc&jEE>ZAzSXA=miVg&EyU)%)cn^uAK1;?5cI-}`qRlC3S z?%&?k-L$jjpEYsPvPM?^A?$tFb=@bW~NdUzus%gBr2VGGo7ZZs{DWTAWJm^2!K%&M zId4ic<=(P>%se|?9D1AhiwRCr&f2xQlq`{712d)jy!Yi&X7l4)N10h-3PgQ6iX0rz zwH45&z3{i~1OiZFV-k;cRD?BcPq9j+rwmfPAr=!}!t76w@v=PYWk|%IKrGW$ri)~w z!X%cdlBvApU|d_Y=IrUE+B~(pMKR)ZPz1F3vbkyf^a1fpQNrQ|1;|q+;|xIojbOaQ zr$9Rs2XoZM*xp``P&vzyDb682#`L~{rgd!j>(9c;_A_XYxnK(jO9Y%3nd{v3QhjzR z>fEWWpeDxStX%$iP#= z_yR|gUg|#4bW=j>Ouz??yi|`&z`2x}b1;i|YM+(3n;iRY0@jy4_1)?sJic| z;S@**rlOVV8EWOv7~2z>B0eNoO_AO9C@SE}2V%r3CFGTSN?}{)a-V_ntwBmC1JO6J z%SNG8?fRX-t^KRZ#i+of4>lE6b~D%Njq+OtmEm??ZxTM=al{cTBk$>J-^J*@EbmD-vvNWb@eC z{8m*gO}}kO=b{ z&I=oBqHO1#Iv6!%E1?R`jVWVQ5}kSUN)<->rI63g-gHNWq^d~nY=Ik89+Fa~O?BUS zy@|q>ka%*<8*Q~lu|dWJ!aid{A8vV2Xz3q{PE95SzF3$%t}Wb5+)xg; zgzPGd(+9M(FNtDNq4E@-@o=LYnA|s8NuF8<Dc^dWGv0gM@-6GMzL(_|5kn=*q z^eKmHx5kyE)>4(Dw*$G>k>~zKuCEOX;~;|O1YiFn7D>>PRFSl&tpxLO5vLr|NTeRD z8GlMH^tnvqPwd293gUN9ytOtF(drW#G(Y+LW$L`1+z2I0Hkj_3Xq;}t(hgC9)+Xp3 zwV-pJfd)h&t$j-5`nIzMtdE_D=H5SsOdk*7S*N&G@{4y+-X z!2cFnI%=fh?{=0z2tX&Od%s&KmR*E2s%+PMw{4Qz(ZEWSjyDayUY>URi}^6h*crH~ z=vRgCX%9_*u$(WVYTu?rf%j(rIQ>}x{@q0?gR(h}k^UzD$Q!)@FUu&9!Tz}B19&)S zA#l4Q>X9&DThI63yE0;LL|rO)Z!+Jl6(hqHC23&#>AZgZ4bvMw_g`Wef5CG1?J+)o z)-?ok)N3DtPYd#3USC!Qz$LJS;dSVjdXCcfQUs479yaVBf`fSNQF{7|+#NF3$PxK+ z+e+xYWgk74CXpoNCl5~=Q;5?-zI}22d6Pa;e>E_ap{`mFP2mvCtE*F=dkikP<83l} z<4Z6{atvMn%&%n+ej2XU7D;lfsBYCoKDs!8WHH24X>x4Qu(NL=<&LoPU}~>6OD7re zmR1gW2#nH6U#p&L+XyZ86zLs+u_aONfcjX(QM&VSQNM6Glz=2SjJRYLfkk+W;&Jy} zMzURz;QrjR@BFu5#022ACh^yv8o3oEboe^k-*FGMzKb`qbl=d+2BR=OVHr?C2R;BVt(B0N)H zRs{Svy=IAWM>T0k1Peb)F4oIx_AuY6+Nq0^QS((@oUIfLk5za5I4>>Cpd1PsWh~^@ ztyIY~xeG~ooT$NML}YCn_s$u}9^fw%U;X7YoB7v=;p&0PD#Mp}UX1{HbTqiBG7(iE z(b28dYa}4j>@d~&L}A`u`d1-uqlWMu3a+Q6T(63M=JKKCvb(__Pd45k?oP}o1F12+x9VE-YzGP$+NklA$Q>Qzsr^PtP^_?!ChcD>&v|%^ zu25xRANQzG_BAffFh6uJo!CQg7~PdU+^0%0jn}=XK+gRm!r=Es#x6@W0<*phtx>kp z^Wr_ft$RY9Z>Y-}M7pO&jC*zyzB9;MTNc%m$KmsbdgM3_SeRF#7FfTWr;gX7rAFuU0oLmX<-e(e!3MYJ2Ps}jz|=@VR6|?R6-22_5bDk|nE+5M`(%4GZsFC3aH9l{4l36 zCwFdJ_GY{!6?8py>C0jiL}J)?%JwwgAmo>C8+Bg7ED@#mlcVjR_Fc4&y@~Ke{Rans ztnr5hzF!i1X+{)&2tBvf{YWP_D0`^$-)!_qP}+Ug=eh*N* zlh>Q!NWcZW$`flaQ_;kyX;~1{^bz(svIq!mS${X2Blsef(+3L1G&DYc@7{vFUShun zbUmBAp56LB!td6$3=%(TUK51!PjneG#+ENeUqfNw&f1;?dk=}g0oD8CkD<^Cv-eL& zM&lY@D%Y%|{8}ir-_j4JqEpWCefU1kRl5uj++Z^*Ds&wU&Q3ctPPVc%+DmQvq`&sS z_X>>%yPA6ym|D+G`@(8LYA-zJmU2UL(!?PEhANZ2KeaJ;PBL_^)PLvmH>HTgOj-P` z^(hF?OKB}cJ$zWh@X?Iq%$AFyA<~3@2<}4JS7b`$5f1KqC0>aCYM@DT7y8qDVKe4e z=VovL#I5eKEZNjqV&EB9_l{yr{w&$TR~Dd554UXOx$or`6JT$oQnv~Bhn(m*uP~NlaWOkv8&OaXV*B&~X@1Z-g z_D!Q8Qek3LLd_a$DiWSK%urn&p6KTK4exuEw2>6)KCcNG0T;UA{93cwhsn>u-L!S4 zN43DtcBc^qoO(ojyNB}$i+Z<&^`Mb=oljh>>m-|B;;%OvMSDZMFr9OTk@{=Hmpm91 z#}%SFiGJ9UCV!~qqvv zHY%c%`3V}+qhD)t{{}oLh`-bS8F5laU;P*J4n-yY+ zKp4ZCcxqw5xt%jEkJ(tLS?r8)nNGM?t+0Ioq1Og$iY=L>)ueb|B1dr~`wkn1&awf$ zq7a%y1X}TE6zCI6Nz}7`pkb>e}CoYDIdz@_MwoP<+CW$WebWwHAZ}eK_YZ9a_zkMi1-M6A2xN4_ywHbiVkM zsyKS4*}v@8&JXrIR5qTX`)PUQ_k91=*$<0sM>{!$VQ2ljOT<1+G}DKvOY%Rf2n!A$ zn3HS6w3DbcKhY%e_?b)Kev9)WaAVEeeI!9wXpqGZ5iR>J zW9NJB+?3#j2CWx#m#M?>k8|XxE?&g6dznJgMXCH^611$$&)x(IS~$gAj+uIAUNnP~ zkE$bZ6rV}PkQyorZKFZI@%@1!mrrn+6K*c!-F56<-p<2v!osq2{P_Oz6I= z+n5W-XrIGq0{3AKzaI_tAk?NN9P7=<+6@2nt&1fhaADt#>E~z6FG=?-zs^hWgjQb@ zb488_0A~9oYGR^AZjg^MV%UGS3wHA*9<@Zc?c-88z$a$}`OYcPb5zwFf$uz6L|&vf zdR6J<*yv}_e<`RUw`96bR!I;4HnX+HxLYlJ-nA~;nx7aWkiJCoSz26hMm0;N>&G=2 z4X0Z+&4XryWOpaoB^kb?6n-AUyhAy+QrsrI%R<+4=kWO(>&m;M(idvwD7FwHx;Wn; z?*A*i+ST%Xe^_P%mm=K@!C|nEa_$P~e4W}Y!L|=1C0mLzZLIW@$@BtiQgY(`2ayDg zKvlVXQIXh4j^8Zz_J6X*l!EZ})22~LAyPk%ODN+cTjRQ~-z1=e6Fs6?346K&mhTtW z2Z!H+1iI(ZP8M6TND<;Bwj|}2@7!eT$K_M;!E_zT1C{qNpxL*{6EWYL)or^cK6~W& zy!oMWG-h}Jevwd@;1p#^ZJt_#I?Zd)BcP|e8Re~OeizzNx`MDqre>#O<;lD@az!Z- ziRE}hcUvKwh{9$3nm+NkOl1t+eYAQ>j}n@fdafiSboF793F6F?8*H?zDRAX3kwcR9 z^BCCEgqH*C`ALybbcr_+qD`X|a{~5P{cNBiXC3;ZOzE zGK|sL-0ZqXZYc9vkSQV2niAE0LuTjPmh!_^x`CT`XnAq@^2v|m>^7FgsUkDb(2*=8hs@W{bg{uS|^tJ(^4(CC(w~N0xHLX%KN*4`m{>)gVL_Z2+`~INGB@^5@ zQ3!aLrhK`aE)96S+MhO=e>9nYT$_8$74vI-**UxvtGW&Fd-iMhe|UJxJe1RTIZr6_ znJMjR|J)z&L_J@6q2UxQKO6IfV@9W(cD9Cpc2!Vm>uc)zvwJ!R>b>b_mWX0Jz*v;_{bDWd&Mt76T}PwtgCUjE8+m`ipb>eg9>}@`R0$F)4KTG{LU{efc%q8eSY=w?b`jz)3n&Gl-M)m^!R?zHwIf{K(qXcJLw>`&N1p*aw(^^ ziu#z}YF_wGiKnboIm5o#X}>&k_i@i@F<4VP;HZclKoE>3{Xbuw5&w2 z*E(GPMq8M`JE?>Bozen-9DjYiOUCj?ii=dobf#Xu!8Z{jXpm5h^lbT@9IR2Y&`d<~ z*T6CJZQ$1bsvq%<9uzI)pPAM=&QT*Gp;~W6Oapz5 zUq&HN@EYGPpN7TKCM~4eaQVjI=KjEc=mrW%xk zeP5oxuXK48mHF`(DM#vxvurgJKUa{fz~rzdnQ&w_X*N1{4fU5SRT4&Br0;v1(GqU> z=tW!w2Z>~_$~yc>s`=Jv#k7x6U?E8@e#rV@pB5C?gTZJCKh{e-LrHCNIIq4uD@KHs zm7`2}!Ncr(8cj$qv&X1K^lyq*UB`N~2@o@6)D^9CAHWazdtv)vi zBo3*hCZ16N)$W#tr(h4sd9WgWAD~2GIhUFq+U4 zIc%#Z!#)uV)Xy2#X0xF2p!rCh${OPVvApoEW1m*0H&)?}y%}YfqF^H%C`#1XQbUNQ z>kyzYhKt5BNRuV`+Ws2EK)|#3+-BJ3V?6jBf}h8xPqQu546phcNY4C-CSs#OM-+ti zNWmUa+arp^4aZFGQDCIyh)gx{rh`Or%r!W+8#cCWHs^kM36zL2BBcIrdtn~-vj}(fD^(jDIufkl>;+|H6g;}%C z+kA0<1zTdNi$tzbNVzlwT!QnuHjr@`&u8Ddkxer+3j`KPS9vCRQb zX+g7{KgDMUAa_%@K7DNDEQ3nV&?23jVF3&dI$seP4WFDLs7Q;hYdh=2KCJk)I@@ypqYx*vv3(lyBkDqj-X! zbbc@3CGNL<6L`<6tWpnGMu_|D1;jk{P{v8jOaO73IvYCQM=K#7FYT)b4$}70&Vps< zQchlQ#Exh8AESmnzf&VqLnqzQqPSLYJzZ`b)+#s1x*=$xU!%WkRA+mQh9&;Mbq>Zz zp7#Bw&iTQan&th9*=zxt!)F>5J;y(6$f@ZcNcIIE*jv=g9?v90ckpg8=URLv6Kr<1 zQ@ux%8|cznLSfOtOmEi66U&k0uDyGYO^q3}3V5yB6`3ibViWsihSryz<7}fv7-N>k z#S5rTZNqSsAvVrSRo31V)zkP{epo^VTVbt;rZ${KJ?J=)C{xtnZT#{qNp||h!Y_hp zz@(pq`7CdtvKJ@2KeeVb_CT|P z$)>6?dn|IWCFIwHEqwb!ww!1MvsNOv^Ai7NUSP|+W4v?AI@?EWzxvtIu?7rf@Muct zmtTtEP6N0Tde8MQt;*f&|ELFdl6`W|GT!>FbapVAx6d|hHTSgB$+(~1LCHU8VBjP1 z{i8o&dU62)a2n_bH5t*a)mN-i!Cz64vnx7laC8}Q?mct0X{hS%NLWjb)*7z!wO3K} zA?jAByrZNzW*H+FH?H53^l%npW}mJf3yif;>9zKZBk3 zC$uM;elOT4#Tlq(vh9Q) z_(lxAC+A9?oRylJ2&-IAjF^zd6TXj%_bARW?$f_m(;WG)rca5KUdhehHZHFysw|Cs zKpvddf9fb&D&YGph^{;uXc(PDXw;*$>ya;MO0Fcj19<&I1 zHP)%d*=OXW)5#j3ABCJY@iIBmBL6l^wyLY-w{0VM7)Nfh&>QjJv~A$=AN+D~N;F#P zZyrF+{^%00ST{c8Rp@_-uYvQuA|}4mWOUrxS;uBpV=Vw;U~#!$OeD82+RFFw>g`>7 zlaSZ@tIs(uC9O>n0~Xuq3n^sU-g;}Y5$bY!hJVxFk=w`8Fvn+Q@&|O?_`N78IsI%- z?LIQ{`L&KbB@J@?Myk<^Lwy1Y^6yrrEM~Az*Gyh+x_txgM_=z=F228(u9-|Z z3es3I=Flr3HsX^laKt*zqi#V~0n_WhUTYx_LX4zjh1jEu{%{Lu@A>&x0LLlB-1+I( z%XSwCkyXIYFo5M0lCXZ|@~r0vxA5rQr@iO7_krGfqm>r2>eX9F74b%3j}mu+bQD1^ zwGg-m6lhz3J(|%40xG8Xe}PU5F{)*5{&pQWg{aw{P^Ry#_tD4?`64c@+fM5a}i+TgBcO}0nD%`t2C~o(X!cSiXa^oSY z)(4GdVB-%M2Mt%$^c=Dl@Glh5I&(D`si~id%87D$b|H9`9AcEV#H|kx7D758vdI=X zNi7e#_3!ItZlCG(5#wLhQ$wA8zLK&2hymE4umsvdB!SFoyQu@r+o@P1kST#MNI~8h zb6g5Mbq-A%2ykU@oqm&E2-Y}(6ddtv6t#fWZ!(;xVC?^iY>&lHU|kCv?kT#N6HKOq zF|yNv@;ry?s+sNmzcR6ggnNwsdI7)t)uLS%SI6%KUg|i#1bVCBKVRLch;>HH2vzz& zaQv4V3la$_;`pd%pVha9?}%@T_Vd9QBaZbE`NxGblY&x<%**;ksffQ7b(LRr2r=0{K*#^$&+bu{Jj`B4ys_EY6BiEOwW+WsxG_0rDHepZUf{HVA8%wwEquA9 z+Lfn)JmF|R2T~fvkGYp9XlClKFaE(*AjZ|Gq34kwYQMkiGB10G84n!atxlLLSF#>i zL9D`y2WC4Hp5BiY@oa;X;hV`sfW0RfaRxNQgDVX7AMaA0)6&F;(LD6B3mTyiAE^T!8K^M)FGHs>r+Om5NdWtr>}uGdREuv+B8y%-WF z@OvSP7pf?@CFVVT8sM@j;=1}=3bd9Y_z2E>@o=YhcwrvuDm-%dOlh6FD3G?=9MLfG z4N~7S%M2B5aAD~@#?sBbZ`ZtOKK&*-=q9RLp?T~eglq}4%~k}~sqLoDm2cyt{%2j( z|Fe|ZK(B4zJ%@mQ|0G;q75%&cqT2pW;U&Zzn<;iOwILtHEO_2TzVa%3tir>ufEbsU8A}9+}VB^{uHOO6%|R zOX(r>&L69$@t{adXgZ~q@8IZXI91ahfk>XwbNgs;WY3_veT-7nCz7gZ0_fWck*~7? z`za041e{vrlJYfh&Psv4gT~MXi}c_*JqJiM^axiU{zU-8s{K_k;AQZB(p9uWqm&}a z*786tAmo#L?kn?9PGO5z{Bz=>=h|qObH%sd+s@Fq%gRv(c71CGh!J1!)f!|J3Z~mh zepUm_UkKsjegR|B$2@lAJff%m!B$s8PGgaI0+Tlv=c&s#?9l z7!gfzyl!4oyUA!=fw}WCg@4I*b+V^j#oJz;lj&d_>U0S7R891O;h*PF-8L&}`Sh(l zp;xWyPzyXN+gvwCzZUrI{&$ez^1_7Uk~j{^nU-BePKBrxu*y4-mk&(4Q;R&nH=DdP zptp_^e=gRc6!<3`btGRk)ts`Jz4&Jwc4vvXZRprBqkWN6*P=!d)SLKx{ddON=$6Y8 zmaI3&k1Iq!gnjvL6&NU`jjnjBl!;g;U|mD<>+UEJNwma$7>x6j{k zT=SX6h>eW0{e%}qPNVPj#$7neJ+!db>gX5VoErD_-WQLe zc;33QXCsg@MM9?QL3sMVmtTN3r3)%3!V-sM3ZCG`yVYt|Dna3&+Ak2da>r!ad;3c zqJ(p32@$7Q%7Vgs|GZBZ(t0=Qidp5>2P(zY6)|pMrnl&EeiOvko0@(*1cj|>?Qvc0 zg?fAnj+#RuJN(S2~&+T9x`qPB1ubr7`D|AQLDfgWF zYvRN=jFv6V%Yona(|w^0dHjVU2Rei+HJU>-D3n`Mybm~kC?;jBepSl1$DYLe6%&L{ zl}lLWgX=prk=Bu`c+#P)>wNm9jM)^6Y;J@;q1f-9-Y_6L%WYkhF_L~-9r-#|c z;P``OE;B^YNlE=Cqp$p`8S_6&nt#L-rn&FHvyNY>dFNm~j++?CE^mz2Ly(T-o4&TJQ7dCSUjC>Ivyz7LUP6ZeOU^HFr|y()IN{{QIeB z;m+7!Tloo^!y~fnPl?9jbFJdmGTxa#d4vVuQ;W1c>&e0OAKV`C;-TU6&nYke+`jWa z_jkF1+31wpeF&>;5u7dlq$baz@zIaJvLYo;RzZW?D={o(Y}v2Ze~9xM5Go|+_9?S1 z@wDQ8@<>{vrb^5$O2I{JfO7z8Yf876pPB_AtylJ?lv5dz;*f zX}0Zrj6Ohbzp$1gE)?C78hbGm6l^dg73>Uj#_*C)9z*Bj9c*b1dLC}eL(5@BWxQd$ z6vq|T#R49w5(K{+608$jB+`S|dz-Lib29tus-w0_mnv@FU;`Se zJEn^J=h*xU`Zh|K&>s2=m~nQdH3RE&oT*^_@|QgsD8V+KdctB%`h#jWUP&M*zJOOI4HYC1y{SfZFb==4SuU9Yc-Cec4^tU zH7(kJ?$r`re~%C2?u+$WoBeJCr~#u2(IJ{hIvZf`>}h28u=#y=AnKR6`F#%{_QIQ* zlwYXucMCW*(}xf)PV_1s#)!@fOXN>~4zC2^sHR-TrLZ03k09WX7sV78JQM<_#?S_S z0Yi(x>`&R3Dtrc|*r|Eo4<{YSZQ_TRtvpE4HZ^EafQMrs%@S)c&pMPeFY+dO{p!~x zt(vrtjI-=A043U^Mj%^LBao&|>@7T5=yM*^>BRQ=%aQ9-hG)%__kb6N!0D@Ysoh?9 zSAP9ZS?~v~D%_RRp;P?#``O88omEj+K!woNISFuSB(P@fl6r|z^(9xP1}@d1>ghyE zd(4BE-@=x7lMyuu<6aoC$?j-z@DY{vDBZhy5TRU!OddM0jRErKVbIq>$l3qFBTF0y z^-RmI^52%@c(|<2@-AgPI(aQPFJ;92QyxvIRsA3MHs)oaOHYNANc8F!_V-hO^T`x1NZ}c44$bgsOg`H zooH+_QvUAkxMmL<&xPbWw+X?8*H40s4$dz<0cohp|N|4muc>?VvhcaqL?EFJVM0T|(N8pCs1&Vf+kzK0m>KMQrozy^Y@=IdFGbsVZ?t|%&AWPqK22{3g2_|=X@ww(#(1K-k0%4 zYDQJ7lo9fl^UpIK4iH6o@Q`k`@Dj@osb-oG`2HZlCc709QgYxMIKm9iBj%L$DLuGa zr-J_$V%&D6h~}I?M{RY8wgV>mFzUL(NNjV4)0Wd|$9Yk6~~R}8SVbIJvz`7BI?+|D&iJxO{>oXJ zTThFt9Q*8S3@Ciy-?f+DKj_eJ*pNvjVDO2Qdno5A=Q#OPri-VO@b>W%-B|Utr`+)b;G*uGC({69MOp#7=6hnZg zoEESK7Zd4Osj?_WX9rE3*R&!G8zFjOrE zTMf`p)`()f0e;Bl85CwLGNWKoA4eaBD(J(PYphK^w}ZP-2@1g)8)OJde+7R-%MW^R zu0IZYF+=eF$*pb7P|!O}b(17Wj)Qm2jH5~-Q)bnvfSwOc3=QuYRL=7RvXS!9b zhlxD`C=0r1g!;Qa(5~o4royb`Z_~0hk5~;?brZLzi*i*H4%P|uL{*BGbtE#aKQOnD z(~R>(rNVNk#(9RnI#!a%Un;x)fK@Q0^66W1v>2J^V0*VZ#CwVE?KJPK4eSQyVZI5C-$Bky2V%R?c5UL06N?-F#xrhpz4Mn6e3<(o(GBu(+m^g+da`g(o* zZwrqT(XVH#2(Q!UxTVUt+fURC*UA$+-#cMA$$D>`3OlAxkfOrZRZ^O>puE;M^rS!j zda;5VdZn#T{OsE=lK&)(zUAcZ{z=#YBCqS)g#Cg|cT|pFFCqB&@9cd-&i1WVF`ul; z{}v$N2fDcI_;%o_*zu!;knpl%NUoA)z_LVU@|$xTJ69SqPbys0@-rr>EK$<)X0P2lAiOMw0t-Ae9lS1a<}UI zyU4*szoSo)KOVW6aIZhk5~Eq8(-(9V2>NXpN=22`lmT078WT@gA73?(SjoXA_cy5U zkWJq^-Ru%`z+5tEII(SvLMKl_yWFdDLL2(WP5%$?O5di1%_}^GfZn9V%oC%*(O9sn zQicI#eZrKr>jLoKLX?~U)a5-}dZKJr8??sibr05E6dPu%2$`w)&fuV%lijnn5y-S~x^16GA$)8=_ZI(x#9$ zQju!>L36x`chuthfimZ~&^CHoE`O+Z8$!kTH@>{7KM&urcmLCg1wZ)>I3FvN5bpTLUSelB@wO*?Bvi;J@z5sQ z@$kqtv2a-|Y7+LL=e~OP z(VLtpj)SuPC9TL+afkRO6~f!Dzp1nHMoB*@P=Z{-DLS%;lZh+l5b2m&HPxJg{!1H= zfX@(kTI)K9)a1S9%JaN&b`G}XGt1$iR3_Ud45pn^Txdj5cebTHxi;9Kg;BcVFK`;M z^1f*Lgl@L%D{hnXyQUOr>0CgYd7Y5MzQlIBGkH;!6`?#QvNi7Ks)d@Y7gf8%UKQHr zG7;SmhxslG0CisU7JK=jgX%90b5!!&A>@6{Iv(DSy`AV)+7nvI=u8%ddlgkz)Vke< zeu93tkM%F9*UdjYEIUPgRv~Nl*1E?5F5b(4z$aE?#(Y&M*ER|TJXmOB590l};V~;H z_`T;{BajBdRg0ou$^juYRcVAKIjN?uogj{bXF*`WD0g1wMj@JMYlhOOGx%U!H>I)Y67-vQ`7V6$t}#rhqf)%oQm;!@pJv`eehk))*VYehxP*z?najkHJn z$@V@O#*km*tGy%(fO7Mm%B2!Oxq6?M42$QQKuueAkMTWe~tv*3sA^_d(KP=W%ML4F$R{lVWBwfaCMp16k)H$M4CT{YR~ zJYEbKz3lP)0P}d%=WDh#%TBnfT{|>75XgQoQ0~(M?tn(Q#Y+%*W(T>2Q~9rh!>5Qt zYR?4~>cCc1hW`Z9seC)2;(~9{{`k?SLP?}U=M{phE-@lK2k+rx(1`TQjbcHzLH8q8 zE225GALDKIw9TjT+E{Hp5bUxM3EW*@ac!)&KmQuQzs9WWqSyU|EU?vG*7anD#x(Lm zVpJiO2O*q^vJq0}s$t5d5B~xH)Z$lxN+6#(qvrtXP;=9d`ql*y!CL!9Xp-B3i0Nnj z(k0jdUo93zY4f3$56meyi1vWhp%kLaiy=_ZHRV3yCDZVKQ`iCpUG*NKEtk=JDDh=A zN7<_TnEwQQ;F^Nw>yeMq%P>3QGj`+BWtbiP$ZO7puVSgq>1VE^(%1hXgUcyP93R!0 z<+FBaMJ}~wC02fw=CnLH)i$p9!dv*avv72jSO6jr5sEr8shV0xSuE>*ax^+wEOiYO zq#jsv_C9^>ZV}HqY_`qf*V4ZJHQRS_S4h{HLbrdr+qJmdlJ%&+lvB9!S~>pQ^ym(M zJ_%@IA6eWy`eyvSf3F#8bFm@Nywu+~ncP#GJgFE<%7kccXrnYuZMVgDa4SsvFf(fu zkz+qEYVjAj>rcP!#G4O3>(Kg~hDhMio+Ds#tLCCl4@;8XWA5U2RhH#eks>wY2CoJm50QuCcN8v@>P#6C4*tcyu?abT~t4eiYGbiBP>Dq4?u)knrF5s$XKGO?~rcP z+WL)l$RNNR+6Y4NKYZ58hGrnVpAT&#-$3{6lqX-3S67;Gf)%OA%kpsB z%QBR==3BOK&7J(^$cbNl$A|n8R*qFhP>C9K=D!01HR>EzQP7+kvuC!&)6us)opV9P zv3ueJj7BKr=+-a7r*GQi-{KblKDnNDCDj>v*w@-~KpK1OHFole^AcM)bl}l?M3~6V znS@m*-emkEN&m{tf6iZW$=W-jv&uE7apEd;bxwB4+FR1;@Fhm{68ryk9Kb$toWQRt zGoIX{A&eqH(Hbhk_y?xcZ!y@}mKE6?nA@AGmS#~e*73Zq)IUWV8DSd_Cw`M}^`jn# zJ}u(KF*aOY+LBE7`jK!41WedpF8<(qQm;G~a_9!5y3LSG4H)uAW@fc|6NE{VW@Xye zT5ZjRPd2R&6*4vv;M8F6()}dnX_<1N+%*~(s)ojT&8yRFl|T48g>Ou%B1R51W}qGw z!DZ^8InN4(X2$2M=A;kPmGM(8Ua$I}d7CP~^w<&lpxWo=W^Z;7T2~6d`WNpWyc|IH z9mbIPo!iZZc3-H@41fSg_#=2w@AWtqooSna19FW(hihD_E$AR`)&c#x??yd*CU`tW zES|gtPQGtRk<{ovY*e&x_d@kGzwi@Sh z*lyWPn%F5k0UFWpPb`P#IK%Lu`D2kt%Iit<$PUZR*#Ap~*7(Drk0|TJJrthFLAhTG zxbU3`hY`q})m@ULH4LSrGom+HPVnc4RC$7Ohw>@=SGsPSA8yDOqL4z6u|hwj`sP&t zv)9m7oP(LY#tGub?(({={fOol=@54j8n7?trt46@<)BVHK z{bgHL%sZ>@7m_CJuOds0u>to0t5;Y1Ynp5MVV^y^Ov5D!#e_bFzZwrjx^erxnhmrm$qRs+)ehqOz|*ZzALD%|O{ z2Ix|Al-9N_z-2LJ|7vHr;^TI~af*xjz9Hw8)fZGz1C8O&K=JSuu?DEE>K?k~0MS!t z?Xr8Py^wdJABIs&uifYB3DpWJ!ELJ6Xo{{)(6jxH5Z6w>eqTXSHJ3{8 zgDM=YV#OPBH4i-il;X2yYl;GLL5l5DfSr^}Kxr%`JXIUe$`t#s5hGJ zxEO!&1<~Xgshiwe1^Byg3r$x4s`0#Uhp6h+OnZ&;T&L}397HV7p&Z*bK|NX>T6TT< z+Lzt+FGV(t9Aj$kbDRTKBlZ$*L({gW588bNH}4Mb=yP9S39pN#&v#9y;OED#olh%u z5|R^TpF|rq0ND_~h>8-7Uk?O2hCf^81(*N?MZ@NUN#nMu=_d%3T%EjE?n^#%C<4JU zZ~JODu2l++_=PfX)#zKcerWw7pAUTQ@7ySsoAlo&T-c>9)}6N?mSj(iq;D(dz;hG@ zm+#i&7u;c5(At@#5Od>&a3$%Z%hk*JM2^UP8@W+C)}+-uw?N1Ex#Eca2DY*57}d-! zs5OHIDHsw7lO+7b?B~!qKuYw8thYFI8LvKwG#B;7G}p>Wjxv~x%JQ(A3|-m?%}i2t z7ztS*J>!SlS2$HMhjJDZiROCsS7@-U(xF z?(1k<8pbp*4F7)s6hQ00o+~0@2y~$iWsDjJoV=Mr$n+dF>KpGO%*K^3veupbwz>7` z5KfhhNY~}YO84Yewc3O?%~qXxnV4V=A#q@2b=wwwul7PVpeAC0CY_w3FdEP4nV zticn>FEs4T_S+sSgr$sW5TJ&tP+58H!-!So3)&!xiEb+LDlleQ9~&8g6pBuTkv8nZ zus91R4ME+wjC_`kd~|3iQNR`vQRaPgDEgb-3^HRy3%(6@H(esRjVBflFJXJWYa7C} zuAaY>gPppy6InTwylqR9vpYj-G4z6Bdv|bC>0nravnhAuE^u>c=Q2|4F!F+#2I9vH zAI0rvuyNY5ju!EtX%e-Klu8O4--Xfb!Wr{Znn}xwaz2u6eY$S#pE8mcV$lo_Y zIWBw^YWlS>j>AGgXy!2Es0u?&s%i@8b(La+zW1nXNMP(@^Kjlz-OyrGcnP&hhr6ni zZ!uh=lu3nxXA~pdO!c(A*=pByNIIk&kEXlRvfy%R9H!7#HB>!zhS`CepmU4nw*mO|)=E0I85df=iyDbBOSjf|_Q4*El*$jsD2o zWe@eMLO6q9@CI27x8-6Q0uh4^>8Y$C!u#!<`^Tpqhi8+r%qa)%W?Ru|L7E4`% zdk>wi%M4ruJY9r=hfls|O#03DmxDTlPB07lEs1I9L3C~O6*O@}1NyhXDKFT$@AOUT-4Uy-vzL>h*m_XQ5LUf5k6Fl>7QJ7G zq}$S7XO_fS_5E!QXyI6d6SN7|uUVx%y0bV3Bi?S@R2ztzib)!jZYk{m?H4DgyOdq= z`Vc2OGs4FTDQZam3jhHB|BRW(3It1QNbd%Ek>2~PO0y88_ul7I z=#f>or=>;1&Vjc>bh5+ZSx(oo;@mZ z8@fIH?V@fSmp&Ubn(o^k{$Mu#F*uxPvGF~xc?>0le<|?PfHwBjDh+$tp_RBfQS4`k`WG6{;(DBrV3$o5_o0QQsN*|*}SXijYcZJUqBv($MQ-A@iic%0H?!gnX)ojmW$_V z8{1F%Hv@>`s))$HTPx4efgr>hw>nZPbx4l-qq+1vJ3lza9mif@e1tcJGGVStnRr*? zZ4`oFXwh)*Vs=u1`btnEdz9)qfFdxnU^?t*fT^bdx2e5>1cx()usN^Q$#5Ohh%$j0#< zzqRun;n#!Orf9`X12yEG%!pmTf|hz#_8g~B9BPz_5-rG;#H3+%{5FMaIQt%Y`93E* z$=sQ&Y&MjNyE2JS?%4Io+JLXLDW%fPx3;J`X6)L=%85lO!-=!yC=n5pfi`2v)xw~S zZgnN7h^lgG;joz%p50(k&0`o9@SZ(qRIT=|+w8)wHqVJw9-y99d7}@3 zjUGv+BsWt1i9-C;9hRlB^yGZY*dZmn-HfsS7SbW)SJS##U$Ea=h;v!@@A<4ZQ9n{U zrCD9xj=+xHSkZ+!`0A(HN$2bU*$bJ`Ml_d5%~P0HDrI50>_fDByBMx)8<6$r#tgqT zh0p|c4$z*0@Ci69inI(PV_aq}h-T+D#b{5&NWGPJU%pZ#A=7{=36}qLaM0d~*8X(y zf($9$oLzCJ0T2){__g;7prfF*_9v=_JE!DAKH6sz9-Gqv2FcCZS#q`SM@j^2JSmTx z3vsYH1C3wK?-+>qv={nY7M23woWDR&pazsoI^!dcEm46_TbSJG`3DEWBg0V_Xi@KGRQ9qJh+<(DU<_^#d9X zHJ=Bu6!#O#kwP2hq-?pD`Fo=^8f5V`0=khLbn%3aAm*1T@~V`mb2`|R?Gle;lnRGr z^L})t6KMp6Gd&Rcp^+i%UFEEOOYsM?#dk-;hy3z+o*W7#1TeunpNx2*FZ)>3^qm}h zTY)4RNL|@VJ%(Oe5Q}(hOy%cuI15DQ(bhcg3kIzUSq+szp+s==MUm3Fx74(9Qr#XB z$<_4cLFp1q2^uMxD3GOltK-PH;s?|I(_JnE=mV*t#-px}>f z-_i(@-wCma?AL5>wSe$m8uk2ulwqqtLn>W30;N{Pte-*NDn1@>sHg|w!@Kn|aB=|H zTI=ylPOEz?uZQWGO30Ag52Zl6ypD}T7`5~G_4fS=U}_%cqKos&BQ<)|9FX>Zot!ES z3#rVHV>?{~GzV*pIsFhtG}3lB^3(dA2N>C-G40iZ0@=16ApiYjoR(0Mrfv5o1uF)( zGCWf3nHHZ^<>>b?#~gu?X*ok>6Tu=@nPd;-x3@p`QSDa*tyKtAGhLd<>!55$+S?ub z+bmNtv9le)M}BCtBaOQ31aG0_VhMd2DK}YJ8^e3Nx3oqvbW6@%4{j>`@$8GFm22G! zw$Bn-pyFS-E&kd9LlftkZ;eG11P}d12a$v}X_wL%kQ}=#8AL2BE z(8O7ns7h6?dG(L{>-|De0$FtdlX~@`2F9vA|A0Y}%}apr&W9}w?|2+G)<(e*%17tB zf2eiyKx7SsWEokpiF9!BfL7pB@1=FBuw`?`|cUH9Q^;1@^j zpUwMsCL!f^AftEp6FY*omc$R+1W^2p*PP*u&Zmw`1Rg5LMtfV)hj(>~D@ZJ{Ndun^ zH#L}MQ)D*LN6&6Wu+`lfIt}YweoPXRZ2Nh0XsAedB0IEOyOzKf$}YpiJPuF*v)9=c z6a#*^EyURfMJ>jxN!YZoh&F3c!`A(%YI#}|oV-pB&P+rv1zU``eu?rY(#M&H3midS z6#UEr{*Gxod^Tv+(x=2Ci$;a<{>es3b%N>=A-=MieA)`7u^rbr6-$U`bMU0t1VHTB zo(cgQP9<(vcJ95tr<`@#kYXr=l0Ds%VJ|SG09)(H1vvCDP6%7Y2$*dIm-cT zdweMOf@5favPVs%KK(UR0!Q|VU5^{ERB*E<*BsS)v&vDwy=Ydn72(C}r}6)aL49%Kz;?U(#7LnlaqJe*|5nE^c2;}p8L zG;-lHcG;=`K2Fgc!(+AHzH5=5uX02_jyPB+_3g!JEZyoEv1VNh=j?(?P0NL%Ok4l6 z(GLzo@MH1(_hEejy-+5ja+<6V_9V?zFL#+y@~KB(ocaX$chnjIPFI+^QNqHO@qmG& zBu4!#n?!j-ZkZ*V`-IRlpg<%>7HbtuZ#^jRp zNYb#|PjCr0oJjzlMia!(9tEUriK_*9-wMU7+)CEv&|T+oAu1(VCLcYif*R6KGLHa$ zX(R~xTR3Z8Req^ce3L4Cjkg*I8#btD=Y4D}1sv^}4IJ~^U9b9$L-cd+NZ?=u^fx5= zCh5qSx>?u+3w-uu)THE>@`y$uc?H?BH(nKe+>X4B5aL53emtTb0M-4_l4YlBX&MyI48mJi&MsU#NPv&DN9V&_JNSpKY9&ac}% zOG>i=-jifted&;tl%{-pHnKRW`_?4}UnEJ)IYoq#49$IqTFimu3KPX!%YfzrlM{4L z4|GdFPowfDi)l65x#aq00DL&dqd)*39>7BLGE5>t#hI;*Q8NOKIAkWRpZ_Af{ti#k zp4EqagW;y8ywg+TMT-W1_W9cb#3q2RK8*d?==2qu{)-OwT-&8*m^@J$a=O4h4uQUC zo}$}$XEtlNWsroIUUMHHR6F7~Wzg1R5AFf{CV}QaqU_2*6hn>4 zRuus5TW5JtRH_Y z{GsvKV(X@KeP}G0uJc|zQBc@-wDY)gzcLK3 z56r%h!*bv7%fA*z)?V@T8~i7r`%FRQ`w=bguYbGq_;dX2-s4~Y$-T$F{=>b;zy61O z(f`R8sNh28y3a?~CCut-HZs&bW6?dQV7sPji06K1&6!QnafRl!)-hiZ{e$jnY4+V& zf0^zGnH8-3b7_OKewTdGZl;{N>MH|^@Hsi-Eai6qCF9I-q50PHE|V`+#Ps>*Eq--1 zgSpuWjI;=9xKq$))`EA5j%N{;7imDXn=e_r7SzMVenU z^vq$AQ*eK2EPs`wQum&KCAio@<7iP%?I@{z$Ey8arTdt?TKb##enukSpJDc_DOLe< zXA$xpkS$uKS3KVLCcx^gG>*jt3%vyi^_K@u>B}7l-;(^k>$@xGO6fhXM*xlyWF?%+ zI9e1kQ7hQ#S_mvB+pE1^d8dm^rw{q#$Y)rU5nsNP7|FZvxZ)?SzvMKrPTMpNY(}QuEE|CN^3*S!x@mQyL zr-EsCAQGae6B6K34iLI(LVglC!oVS?05xIs`-Uc&PBwAPB68jmI3VcQ(EUUQ@xv#L}f16tN`~l;}d_wJgqQ zf#hT4*&bXE{9_k`fM9k*{Nf8^2O;2Rx)ODnt5_0%I#p_?5o>%)?iQ?zBo_y{&o|v@ zMEW_~RLQY9V&!@qL*E)4gqMp5dk5ED5+^SpkJUEK?xEF!mf>7ky5cGWzq{87X`4hiM z0RizbhiCq?#yl5g{R94>0}Bc8DL`N6=Z-FP$DNi6H`DVF#7~||`w#{N?0t5Hj+GkHE6}Q*6#-EU!GYdtjr<`l9K+t0E zID*;$kki=gzGH-k$0pGYx)wg#x6}$ZL|Do{PSTTQ_Bj|^ASgNNj=)0dJD^`Pj+0s> z2Wlwp(nE80p_S9Ngmb{_{Y&jeQrao4+<v)2gqCN+-td)1Hssgo^$p4L5XrkQCWUVdryD1j>0q=UF&+b>!-!G)sUtubny0F+|% zxz3l#u^)&ZVZbrqAt$7g-BlApx|C;~?y$gLR==#yYEM&RaJ@~7-g?EtMZ@W%X&JCq7k;wH4cA}3CjEhlNYh5^y+ zPrOi7p$Bpqgr|wPEB*8p^d&LiY7d=hi-+Ri7+ot4{F6HhCBJoc#A-}q$1k};4Sf+M zpi3=)qgo{>18t+l!2$k<$cHzcYiYnnH^MOI^3lj)>`^{e{w0~w3l?{QrC&ayLl({= zf`L>}FSD{H?;m#@96LT>4d@@!`u)X@2>zf(r(y4fEZdJeeoD7i~aT1~J}6dR%vjJr&9fzBt3-blkqlyyd%0{wLGa&B1nhX5A&3Wta6?T3V@ zIC z%^Jl3Q0F|?-T;kk9f+D(a}`kGara~R1;NIJ=~MIa$fnWhd-*^&14q(u1V%Plt!LCQ zO@WR;rS1VAV{hIs8t~VMq$WRNX1{NF6TF9V*2SDu#{BjFeUH2U373lhz@_4U%%$J|0{{U3|7>{44!gdV=D($`RGY`l4542$l*$gy@6gOKdy`k{CnQO$a8nO=CaaL2d5$6~p;H zYey}+RgdDwvF{(S<6mK9F+OwM_5G2?*FhI;oc&HNQ-ZSuiu%Zv=S;iC%QTB3Uogh} z`n(iw5#U zOl)q--`^r_Q6JZbtKd}LHEQQ)!JPeQHHvc`*Amx>>z9w4*cUtEjQg(zh0lBhSyT}^a@Ox{dIAQtg>!amdg*j} zRQXp`9lZpvQ24k3Iwghq@B{1ZSWXgpGh4a0?#2UaK5mPE{H1-vx0I&Cm_<51O(L;X z2AQYt@tlrLi=&he`L=`8_lif_1Evj4$z%qD=S}H|B93KaeemR)`xZVPJKpuR%Z)}O zjK8|v0H1?StLj7o*?E48=dpMgL=zAd?ivbwab&GaS7hwiW^dt<7p`!hwr%=-_hBqz z-|<43L0MO(*W+wLDjSbeQwhy*k}u}T6y*i1M%o^)5~?{BWJgDjPE``HK)P4x_BAm>heOe{X88M0T&Qvv)*$(-694CuCR#TlV@adTM z4!pCtxD;QgB$bfxE6#%?q_I9F)^P2)(LH3ux!Rf5XIi<0(~!7A8vv6I&EetBNV0Ry zX8UUt`xhtJ=xELM@QdZ}Gtq(p?OPpHxd2|!!)i*2(uZR48CfU%O?=V1QP~T%v=i$OUxn~3?7ykHlxX4e% z##h16v>*#LbCJ8SLf_1wxjV$>Ualv0%xKfL-y#baWLyk}vFb78*J`>V>$(28Obavk z23L1eUfbap=ET7FF{o-E2bE%R!zswfMRw#9uYH0uko!bYQyH(*1y^Y_vY-S@F|AKWnxhvj$~_1(h`QVBNOn8XStVYZq9<(kgimc(d8JZCP^h^lRuL;N2vh^%&my16=NnGS$6FuS;}HB>r`BD7Ly^TDyyWmq^;l{eTTH;t%vMpd80JsCe#9 zOjs3%QF}5UmlyAp8~GPKl5@ou7h1WRSwZHyE!yu&T1&wm(`OZOADWn$qPLi&II~5( zyqhrK8}Rs;>*_r;{BYM?gsf@Q4oO638C&=WTZGFxeDF|4cl3RLhRb9X1W!K*ADn+G zXbG=J)U>6HjZwCFoRNK5+q5bG}7X}HhroCoOQqrHfhRk2BNC}o!& zKxMn{gLG4LJ^Pr1shyCC=r2ahXM7p21D7k=X$h|UXlliYdqL0SzRYPLC17{^XLo$^ zvvO)sCn+cpxMU)IMqxW1g|)2DH?r{^mo;9NsZaxP{>Xl$BMP^;1~hl{qcmhri#MRd zDHAlzDd#o5fkUk`X~Hb8;?!#TX1DfnJU1UUAI9Op3-EHssgZ(51 z&O@jfq)f@ptDwh7!ZzmA*QqVNwq*#v4}riV#Ft>P{_ns2$6tT_pTGY4>tCOL{q?{0 z=l|aC|M}NHu7COG`~TSQ|L6bX`RDrAf4&cE{Y$IA{f+d$`Z4xp^ZNIHD~#>g-0G;+ zo-7upKJGfHct?BIeotpxjWRNcT~~++QjF_mdb}=&m$KB)rasrNIE_W4xkvfqP`;v% zF+`uTep#76X8bz}m%}sj6>lO&D|hDEuKUtl8SCPcT12%pj7UN~AqCtxbmr5hjtapC zI7gJi2GSw}yddDH`}!U0_l>NjE)3Zy4HQYD^WF{`7qUSL$yCXRD@f-(M1o>6HFT3k z*3*D=fqm2Lxb~mlQ3Enr&I-s$aZwD-b6o_nJ&94>f}#jhcTQD+;(&X52eOs*z?3 zWtcmH-6n5(T(E9?IKx*E=8iWk4$5R=_=1~1MRq8xjh@+v+P3S( zw8*=!oel`PSmMf`f%b1;d^S4BK>uCWu~{K;X@Q%phfm8$k5VgpyPc6Wk7&eiIu3!QTTUx=8FsWp9x z8Z=Mfjcb^SGM`E3GQJ?oMa3_krKH?W4W%YE?+G$9_wui{553d?Jr*dCUyEQ-oi)`Ku^Ydx z>4T@T)}t*N#w8#hLnVH$sInyG4^8sFFx1!e%bEF>imkz|F?s}qA}5tC##|n7hq_x) zPh2QdwuVTWl7y^Ca_Ou#snHWx!d&JSDTi_n*0g+@0#N0*{aKCm#_ci{a1B>!ML6~+{P zO!PcO*TGI(ICaUJZKYWApd{=_bDS2feSvw(V1O?}Gl^rkA;;|nuebBM=q0MVAT(l> zLs2B2ay4_SWon35!-!wEmuz2UvsW)n(LcZgHv*?_?0FibI_4Bj;EM={zPZ=oB760^ z#$A3U=KLmgzKJ^b>av_4ELM#cD9@|tZbkBnm6FKf6n-|zRDn-vL~ zgCtAuaJmeVQwnHW3K?>4XmL^_xf{{xxMl3%1IPms!3Qi}8Y4_-$MF4_n-UW3EdnC4d$DY2UXVZi^GGb4<+ zctNW_xFzaSE}~p-Y>Q;oEzB*+Zd$!?p8VLx&!%YcA}^vCZ0)p)iJqi8zA$xtWm>=? zjV?ugkqk)!bPEOP40_juCrjAdODfx*8(887U1>S!hgZ18(LgWXEf+{Bi-N?_%^wT9 zzu-A);*c2xu&zF4LMA{7@SiDyt+4t}$W-Xa%=s#KrZn-ZSzOnGwmob(%pGL9^N{JR zb_qhtChwS;(pBhAloE z-%B1%)IZ*HA50_OsvVMcO9VbF1HGyS(+e51Ou>04dnDTw8-A>K$90auE^J@t-1>!h zkmb|hD%&-#?G6)UCg=vfK$S>D-wtB0r!=t+0hhz3nJ8(fIp+)2RiGsD7*B~`EjoR_`KL6>HIvyr>SaH7Ub{zBW|>y zLt$Rns8Ik+dr1nyEzO`Qo*{{o_oH>;$n`NoFR5+JDI-Iwj=v_DexFg5Ib(+rg_GDj zKL}V-0PfJp#%MpwfPVtJF7jsYvKA)F3AuSSyYM+mDA9BRNQZ_DJcUXM*S$V5F15%( z*b0kLv{7nqBmx1~kt3#(zLQ&K`{)jsENg9l7*DqnE;Iz3E9^iwxK*E-D+>reV#s5f zN{=6n8y=6*b0kIXT$Z!UZWasc;+L|)EgbgHzM)`L3Mtg(Kfc2%j_vcX^YAz|oOAwuP&Q`e1-u-O07%?Q7@DzLep_y0NLy z`TenP*NfE3l_O8ysl4UEvkL9XRaCgV-7VVY#g!9lU)S8%U6wyK$*Vj3-6m>PlK#i$ z6y%z-T`(6l;!YBss)=7j+Q%d(Qr}seW`!d6%sw6k21wOuUnek%RG$V<-i2sK`%>aL z3rrs|r|;r<-o=D#w#efckd_74c!`Tgqlw7aTB)*&eM0QFn|i`zNV~Y~VmoSp`z(XY zG4d8lC|*NFqny+Pg1 zV90HBJ1!Cv|E7wt+>ERCweFm=s3gX-IRra2TRQ6&Q@Bm%rJ>!DiSz31}kw+mhV$} z+J{5IOMOU}FrX2;A&!!A8ndNu=vW{<7kAZ^BzSTJwsOV-utJ8n{+uUrK018mvDffbfw z?vZ-TrmO(v+ulnxPF0#N6A#?t%?H8;YY$_)G8}#!6_V@?*~Cklgt$N?dP)V zWRDGR^P7rHD&A`0i*uXgdqq?>RMPwH1b1^8dZC%gDf!%|Dtx1cSFmu>ne2vz^JmF2 zjPe#y%(@FYP3wg%`6qrCw~2p_HGGBjpFI|Td{R_DFXPtxym46yz8eqN;?9Yk?nt8e z=6}4<%6Rx4t5;}0?~!y=92gaZ#J=7RdV~Zh7xW~wW1FV8kgKFu$$f)1lD7IHfy6w# zm@EQremm>~Bl9&<(`wOf=L+BAi8pw^ywmDf zijN!)N^{vt~U4T zeY1RXhXh=MBlG>KX}cdymL$isT#~rBob&Kw+G3Sn@%l15Jm0XMlTf+?PaCY}q)4JY zzohOvztFY5_$h1pBf2V6zuO{<+CKg~(iOi(8y%|0N3mC)%MLbrwIzRhqG+GjEwA4W z6`u1f5($1EQ4Turo=3KeoJsXgmDG$EsLYbnze~U>@^OjnW#KX?!GkY934f+bw$hd= za%f#$XHNrX>d46fove+*9ZN|jA$;l7A;(N|wDEm$x&F2MwnIhaKxlojmoK*1^4EQ@ zThPCcC?8kY3G-{~rav}L$9TFbJ?;6s&}L$vogHQOt6w}&_H%^Mx#MtB)%*$6Gsbd| zzFy+q24}GS{#)IoWB*h4d)IwZS~@-?OI#zxmf!fj>y{r|#w$y@wb?WMD*7|6j#$cf zpB{yd3j_%<|3&v}iHpN|C;QrY-=h`?gW6YbCj4x~yHaKQEFxFr+yil}TVE^_$o;yL z_A|@lpLUw&%#)U-7pSwcnRr<|4mX2bvy{0 z-XJO7zVM*8@q5q7J4l?9T0s9v=e#@8x)B5C8OC}PYk-+>DrAo7jC+Po>pK&^p8%hA zy=-4MoxRMlw5M_(@zP4iuXMoZqU-_lHTQiK`#$3NE_3ol_80t+j||Hz<21XOc5K%6 z!8S8_eoy51#Nwejxtc|Ms^pP%2nCLR(DA4k`br=Ci{{ti%ka6Qfg}d)>9mIkev$(o zW`#g|!=0zN>_r}Gmt}9m?CS+-=W!XW`ouAj5*N21dS|~6J{}EY1UJ>jp<&%aCMxQ; zUe|8ty$Vr1B7U%eDXFg$X0Nviuye%&`gngTE4eT710P6tlYg?aiEh~mvZ0GaEt01* z0QXTTYZg75dzzU%uYmD<6F0_Di%?V#Pbg|-&%48VkihPkB8Wd1lO~I~6K?nAti2_p zpF-))eY%I-21m2Wijye=Rnr$zFGq4ONPMxjZN0^^SU3%TnDV=P4V|B?g#vF}$-DUe zeOxAZ@w*X~`9Z1Y`S5@lUZLgUmg{D{k2wsD?9bk1?nE7VjSiF)j8CjRPF0H6%Y|;d z>=S$UlZG`{YHzhe#{JA1Wsl+1I)&h?-YIRGa19vvh>in>3$LGi>nO|hqB}b-faAS> zo79hPJD}OVSjMyR6%w407Mh?}kLVmHKNMpi*=P9R1F~QQML1JQ!Ebs^y$$*+57^&r!HyW9Y-~vv+Owi z=W;vzXD$dA8EDZ<zlc(TY`+L16KUf_=ch2U7i*$)_!JJ_^Vf(@B zpB*643Jki|VS+re;<3qH0j`kt+unb?`HL(TbfKZDc*zdUa@&p*K7wQjo=|7<5C=G% zIu%P1MSzfmLgFwF47MdCNlyO`Opt704_S&yTj-=UT)Nakg82gSoBYrrn&Vw2lMJ(g z=tP$CbC6CsD_cJz0>iKq33xvt$eiOS2^s}`lc2*{5?R@>{S#0*y zGT9ZDPAW^_JrRand%13E3awNHecaMC9`xfYe&kdiD+%Gw2MCV#dfYZ>J?`-VXE~JD zw+YTze8YY;lIpM;ZK-(fAlKbKp4@!Mg4S4C_M8V-zJiZowsk3|4ll_{W3s8E$R6Lq ztmowrt@u7p#ooX)SuwrqSutZKUm}6V&fBlG=HDDo_jA~fET&P@>NRxQ)f1bzuAbVs zJMMeK4X^K6im5*DWb=?Ot#!Oinbmu3>_cmo8DlZe^x`&NTIcg(vDCz=ykeSNNM^=r ztPb4IpxA5J>Iwb1pYfLQtegQMX?dGM1#llrM_kkPB7(zwk$xY8NG{$&g3T8@WHi%H z)WJN9y42+OMV2buJtV_|H9D90MQtX1bbfeHP?@SG#lD+&MZs)d5CJ0HZv26Y6a?3B zy&A+w&r{UPSQ7 zo4J7DlqfXwz-i_EV9I$^i0p*Q7Cb4s~AR(OGZD;=36(_vl?;g$GK%Qa*jS%~k$`Ugw3NKrb z|2&P!B7>lbj+T(ds=LIr10ZI^27Hu)Vbrdf|pbsyNK6H+) zO1( ztq42(JXs0u^&n6QSmsD<)Pq51A@_GYbMlRi%4rCv1|FG^f5~7+kK*D`;K!I zGnIq(4KCq&f?CAeXbZZfPOKy)qMce1`%r#PDW-WKqdi5rZqon8!wgL* z%@^{NJFXiLmL8GQ(bGpt8t$6}DaWNfft@D0@I}<48+X{wDYoesHi09nHjtE;@2!s!;g2p-ETgrzc!kBN0>Y_8D>WwOX?nf$tnsVX69 z%>UYiudz+mTcaI6AuB7u7apVue)patpy}=FhXRXI$S5AI=hShOmXn)D_34seot-^}^^4uUsnsiPai;?%G2Z zOd5;%_7mai9+(7G$zmuRyrxDtVLa=Q3X1{gTQeA=4jy!n|2{uN#eTDa`b5b1NlK_k z4pfCxR_xT2-;5IN9U!64S?@vxEfjTiKcv|Y)h$P^wCPPi-MN!7F4++-fZH#k*cl3D z-d4?lRHQPs&z+Qhb%YzAt&j=3KDi10G!>(-jD3NJ6cJA6CJ4fj9Vg+T9AyWYyV3+x z;ZtC;*PgO#-Q{PME-RzE#@U4{KRAPUeP$#mY%&=jy)y48&;=Wjk%vOPS9_2t6dVIF zUq#`-FrdrpOP5&)0>{8fzoW+tV08*mt=et?Or8ZK<&~lDlm8h6#rHl^V%uxwf!8VR z%jI&6I!(4ZziAEZn9}1+L+I~{zqTxJ(v7rJiR&THp<6?C%X7IepS9RAf15N}y}I_{ zq^WFT2K~;cC2L!)(dCtIbFc0pH)LCjDXddYb$TBQrbqyrmMUqzPvz?PDkGwU^{ACm zKs5P&N=NJC)+reYkkzeT<|aY5c@ob|xz{}X^Wo>KiL+vX(%usz7D8-5sSAYY0HtCB zrqO&FMcBFJp|@7;>mS3! z804T(eob?uvxnoPc75aDiyAko9|Y@gD$i5|reZyFXyJ!5k@4&=){93rS1?tYiJPZ5 zwAs}I?CiIHo~f9MD1WI$fPZOl+|Mq_Zet@2E3QM!%D>1~#_!yBYwa#`ah3h^bF-PL ztUl{G6#H6ZGX^B*IX|_&HNrZywgN|056=9&dEE`LCdwuU>eDHe#OK4NjvbN z2Jw#lX&S2v|6?=n)*a{^W(Iuxy)V4pmG$`(VR-ZO;t_3<-8`F*(T;3x9ke{e?^xj3 zh2~#@SJedWk7aLm1fFP(S0S7WYn#j+B7y7|MVUY5GpFm`zmFT)XBr*xh1uYcJ%^J% zTDIKuBBc?Dt%79}jR9p~kI5uO9?qcdY{}!KM0&Mb#$U?P1t<>U{JpO33hvZsN2gtp zpT^dEb6rJjqWI|$;Zg!_;;LO5vWW8?PRd%&&%T+rV{~HLPqKC+K{>`9k$_)fID6Z0 znZWsQV7vM=f^3c5DGS<{!IU2qiGH}n zODd8=$Jxiz)(2*97G%Y;X?yQhSBrQ($u{aEI54VQ4?BO1FPgQ(HZk`9`!B~aO?_JW z)GsG#%yQduNS zlA|NEt;)c)dvT9wIq+8zIbrybx4U~vH)C^8a%x^?TCvW&v{9A^Mk{ajpqq9Wz*#4g zEVg-LZjRID703&pym;G!8XX^k-PW{uIVyXi2PcTHplDMoCxZNNbmb9|@m;eNJ8)#7 zYA|Ka+or)xMHA1!GTY$_cTORg??_Y=zLa@#tGdFX4`)D7Do^qIJIu{gTgEBbVsnv; zUV|f2zf1us={NDBP(HII$Vtd5{*WT{70cUQiYVZ_Pygz`v&xoBVlF)2MP~8_8u;6r zV)BOuovyn=ysNq5pqJiTIfuH5QWv**R!i_>Q%pm|qEqd4@LVv7EkuR zq(ZLDqu(#$EFVb&L<`AcA7Lab0;)x$uM|uoE)3+q_iL91o<|Wl01rU$zZw28OVGe+ z;}vH!^^#;EM!O~c`P}uAnUUH!6Y8F*b0%+$euCWAhzvQn4Z0=z7pO+`NGl9d_(bf_ zhy2ik8PQZjpXJ@E1KK`H!bKRK^MI-sL~~z^xa}^yZjf;w$QdEj%^D0fMp~Y6ZwMl| z(MRONIZ0-J+_4&WtjFQ(Nr-u1ksV@3E2Z}C)-@1_3cV(6BPz1dLkHj@B7X+*!@y7Z z@nW38h=%a*@Cx@{GA39Q%lmi`#$$p*i@lk;en*h8@vAyVI|C3Lq$v^ zQ3#i5+uM1h-JK3mbIlq!@wQwhykGw8x)O`+finPSttE|waI55L-o7Mrm*fE!a!d|~ zfih8}6~DbL9@+yM`=dM!xNd5qJ3-zCUD_=Ek9e-fju-t^jyL7T8jU;Iv&|{1hBBUF z)(Ylw+Yze1f7{r6cY!j#&K{4bq%#(M{_KkWcqY=Mk^T+XUBq@bBr=Ig24V1SY}-qe zA18waCekhROY=G}PreVfVLEZwg}qs3JHdQHpNh%ncUcW}v3N(nO9z%2y`=ZW3((K^ zTUt$0?F%AK(l2As-^X#b9*mafw_W5uB$YFDhvnB7ErMLQ%DC9G>ZlvP=8j+v!5NMM zvzQ)p>?gxyU7I1(55RJJJXQni2Rx-guNKLUjm2GtqjMPuj5^5F{802VeEs`e+b1)K zR01Vm6rV#ZSjG~?OQw#J zllgdHdJiWD8|)P&JBfMP@`1)%naLC&&I*~-yYObBP-!KC6+JuEg72cY!tYVHsjk1Qp z5u|t4Fh`i!qyus!NQot$RGlulZ%lZXvKPaVNqr4}y_)rt7o=25y)O~ap0%+RQ@0Q0 z%O;)fxi7fJ6tYL+*9ITh&7K+Z9^8qD-Vs6xmFy|Un@+JdM=nD>w#=G0C8vizNK^;D z;$_Yh@IBcS{}A6A)h%D#{F&=@zWYl{!9(Q(lC`^c<2dKbMH8LEGlX zJI(?16%?Ym7HG$n>BBj{n2rSq0Zu1)B@>Z5PcZcFU%i$2yCbo)fE(#;_zz9V*8}8H zmZITfKprGTbP0xh?(VN=2XBvXZ#S0wy6{+-&IP*=O3V+B0M3n5K1~Ycj8cXL5tT@B z@<36Uj_~h%^=GHq_8pgVIKZ2!!d^vk)6~*qzFvvyZhDV(Y4|MArAy13WvBZd-^>ho z@Z$?#8!|%MZE5ZTH=)!`h&cVkM+7Epu)!`1HluTvRj`nBl8kMv5r=#b2}YnNF4!O{ z7-Fj-*yi#X(yN;Vw31?5^`-|8BaY|$3zgdzKAzjfNTpuz5UBv^&9?ww19iuC$c%H-K%iR4Vej}Xs zL~%unitO#IT;gZ*QW?zpa;381m$%cPm$&-KlU@XL>g}xP`m>bMISQQC+TH@6f~(XN zvP=pArw`+l5!uxSSn4(h>x+wUXZ`}k*uFr3uiXa0vF+ao*Q%9S@0O*TK*?Fmq8tT8 zCFLa1yV#H=Y3^Imal=^-hfr?r%@_TaE_8Y4oCsDkcgZ`Gf{AERHC*N?DSp!OC^2Q+ zl)Ls$Gq>XpPBzokOkOmRAFV9w2$)YQm;ctfyg4+4kUi9q@`g0{QraS)Kv5_MQ+nZu9E!Y&}c`FWO z62!;~vV+U!j}e)pb@-B2lAYm}Y(Ed!*1neCW%7vb9m6+yq_t&uxi@ZRciA-`Og%>B zBF`gYCLqZMOcw40SebhQ1Z|_nzquGUtw_TSvZ%0c!NgiXEL3E}o`5*+J;~=&<)Zsu zyZ7gSLiK{HeD9yZBD{r%=9t&`Tm8N$#|E)je9x9rM$vdmr3l9J!lm|RJlxtcrGpXX znbyE;WYRQ}_&ph6zdG66a+n6u2gE&;i>)`$c z3+Io(k<$qX%H|i(_N60RO;rpvOJpXf9o{MZD&UYgrseYrg6D*HM1ta75#w_-*pXnQ}3 zS&G{$r3m!rRUzRR3JqVpR)dY**xse=lH6R5+m;3FDzVsXDK^K%`=}IR@vdD*k%fk} znMWrKLh-=aYDF!btb`2-RAM()N@C%Dwfud#hRamBZ9@3Uzk4)A$cTZjMB3!=1-tV~ zz|+?!BMh|UtHSo^hRa-2v6W2cn4hr|$|2vrkOuqb@&!Z|0`g^>91&Kt;M4%J5B(!*?W-b;c^w(0mpEYOGiM)zCcJ#RC#>3%o zZQ3Ay{GN^Mz3!P5e==7nf#hb5VQ92V^!FK|t;bOBiZ2VBVC(LzWEL@Hv2c#KQ0b#6 z5H)IMUr6EY8Xx0y4*rI-cY})Iw)0slM`cik6F>%8s&|DmTaT-dGx`fxWh&3YWUq6K zZUW_)HxBAq^tPyAQ;+2n)QgFY*I%LCe2*7gi24hOH5NO&^gi{MAqT>GL2)pC6)I;` zU6E)&_}=%ipnN!V#!{a8Q1Vjq@{yM}55h0o6B|!-*GC4pc{_QhD@KwK#bfC#pI#Jh z7?Dhs@J$;gSBgq9$_*nCbldH26(03XTy7dBytC=GM%k=;`cr02ewS zI7H9E7q9CRYpXM&UMI*Y&E)Z4q|z3DBR}_PPZ1x^97J&;UF24Kokr8lX7G)&eSnsJ z{TTsuO-wUo2aoL?y$vV2+mv#LHdt3JEs)Q>oD`tYdWi6I@Futl@7&&2LXqV>%{tnN zk1E}lb1I?~g3SIo?%Fvx{&E7TjGlW}h)@MDBeznBD5bnWc-fevN}!|v z0{{U3|Ezb%wuDM>{x{u8;ZTr;Ts~7WssxT9Q1C~_rOglFa30O5q#2oY|bat(WrR3e1`SszJ}g{C5CiV z%Hkh=?N&sE1p1eXKl9nx9pQIIyn`Qgtc)%sA5Lf?u97a=pmi`6bTeKXlm zCF+bJ2l%fbGTM97$ZcUEw+xiu>jd=j_`TYBx1{bAKcT(asPicdv}CU3X}VrGhKL@H zq2YZhZ&Oiow%gd>A}-x&KF#0Ke}AQ?$TWUo?oaT$=zah;JcQeYSk)*l?Ap$JS;^;P zmV+(`VTZd>)^oP75QImphsnhp0tsy%DV?yVh%etz?)l3F%i+|!>g0_ketONT@hnX4 z8n3XJ(csHOMudiwoz6(FdNCfJ&ofMeG*QP@-(PPMUtuj$MovUX^Z1Is|HIkZvtEx5 zNg_h6hoUR??l2-Iw{mW3*TOE7EC7pC?b@%*HkF)pzu?n$;I7w^UTB*R4vX(XPP&d8 zm5(Ii_YgkqWwqi|N@SC(`o(amopYzI5w)YT$h0mEJ$km8hdF*04cUM(nH6WfC@;Rv zlL&>IOA%%NU<+}QTamUk{6zCaznQb1WJp@^Gj0g5Z~Z``m_T&scUJuQOA@;E99ct1afv-fbvjPIs{L!)iDBmVxF|Ws)LU^}I_6RCotnYi5C_ypsZmNmFesd)<4bk}}LPeEqMf=`d zTvr?|Mdvoyzdm=?yl!sB)YT&9v0!eHAH>p2GoQdVH4^ioLBqZE-CKl@!?FVSqhQiu z%eLo2X?Bly)(ex+`)gg`W(%uv!5RtV>0@5`Z8KRXP{XLu;o8wrmp1bwd%(}5AtGf^ zH4+QtzI9<1^t>MsPGXKU6`XrzAUG2jb7SB~voI1>kLB>EpK+x9Q|XL@IG*7%grq?$ z(eir)Da1We2w`HkHA4;Qkkxv9s&Cms6ZMWJGFvO8ia(!YbfT^Ljh2Ebi+sE?FxL%B z%>3fnM(1KD^*+PM%BjfNjWLWt;4N}c@^I{^LScN}ipp3nY@wO{g6e0_`~!CB@9|8( z!#`vO#SbXnV9t)Re2NU1D;}a${4YaZuaXA9CWfm-2l_^qY+|fXIjmzxGy@muz%V>vWAm_* zQ2DtkV$@*+k}B?nhInvFM*Pv%C$ zZ6@9R4xcZDvwa5N<)Uy4m_LFF&jtzN=yvmtIs;|fkfWFHSnlvtbs1Wz45g2Ak@z$i z)|?Xf`}sH;dS{Ic!w>ewPUdbH7IHsFSqZXyZVp`g&DL(%=l) zykC{aqBe1fvshF4z#k%puh+9{DSkliof&WsV*>ai*T>!&}wV5x~A zjE!KGxvw8e=U-Dh85KePi-m`?u$Qu+Kk|L7!E=3h2qNft8DQNcKh66k?leuW&dbWP z=kjA~!LDYy5q0mdW$P*%M*9WEVv^5~+woG|L5VD$D>6+Wm6T071{$Xqy7$2p3>qWr zE|blMKoW)~4GM%0HTay262E{9u7^hp)6Li1Qa@FOD8ctQso_Q1CMKmqcKJQN(V}87 zm0H`-4*CTsZ4c;0OKYR+P?j4dcNhw6B>68@<93_c-M`csnT1u3E4$lt(}b(NEx=Q$ z_6|H9r+POqLw=kMW_DbE-E-eTx?Jjnjt#!1+{tTz2j7zD!6ky*a|Wm}bGL=k-ht~w zn}Q%I7hQ3__>&AD@%SG1KpWC9sepAN6$Yh7Xx`fCXNHjMWN#vIoeHym=n-~EZ?s@2 z%62`9^n`T|>~lCNF7Cc!2~DnbMtfCMtd#@@uFbte=N~;q4mNUTUlZvqJ?DBUjH`=t z|5?S->-o92@gDQP&E0jAe10d|DK08JF3hsd`KjC}v^c8-c7*p6>*KA2wjG*cMwI4< zu0tnwmHU7!dPcq~T#bl?d_hv&s=~CdI|IE%cHrvGedyokHbM>T?Ki@GyjY|A91m7_ zr*bHeZHr-#rI)E~SaIk6)fN81O`6NeETKxfZhS`~);1^JK~U2f^X^y=@VZ4-``1!X zEIAhF@IA&t0o4!h&(Rkx)DR9b73kb{*zuMuj6jp4XEX#ZaNlB_ci~`>_*6b}_{Fo2 z)cu;FFY$TZ#2*HNeMu<4U_6qU9|U%b?L#s@-8-;r@RuE~iw}nTf_i*+=}G*i`|SKa z(V5XWMJWTpm?mYR9uy^Pn9*miQmVLAN{iZviaufqMW)7Iew4y37%$;)cwx{Ndp14A zzXp|HXqGegymT(S{2=NL^0VvVnxZz|&o&7pc<48NY%0{71vqWjWT7k(?6oE}z;Y*h zpVDBk)MUszBgj6*3stRJutt!|>sE7*S^~CYK|t~#POEf;_O;QL`J9(gpqo-Mkv}~M z9uLpLbKbkN)D&CNM)*kNXE@VU0X+=x`3dS$TwH&+LWb9m7P809N`5SLVS+>xiIfdb z9xo^$!`_cv&%U(e;sNI2d+$x{dU@E&6Ta(AR*mN9y4nWeB1Qq|YpF&y;CzXXKR&W|Wk zoBy0m0q`YPIsOWsxl~t>*Q6r^xt^+6i>EJ$e-Qd#o4y$ARYR=X_C6w?l6}aX&!mpf zexjX0!aaXgz~fU}Q?USe;OV~8#EEyqduOwCZ%lgWV2-u~ng zB&<&|V~+-LXl}-m6L~rTa5uK+jWxcdG!PH?(AOsB;2OX~kvW~>-Ir}AwHNM3ZW|wz zFNm-N1BYzcm(8DowbQm1`YBH<=VU*D973Xv z+3rU)$Q`SgiL;0=|#OtRtc!%<*c9>k#@<-V~QjvZCvG zLqk;?`V4f5LXl$4yw_wZG1x$SX1}RvlzJ)CVUvbF|FKNbP;iT+)8%4;pQjV2k*Ak7zFx=l}P`_H}M*$9$P{+bn|Im#LIdj&Qg znhi_1byWv%YlW6r7?ziKV5hLHFn;+NtFK+A;mSTw!cE@sIFB=4WlGJQa6ZZ*faGHz z3e2(lhmd{20qcnD$jlsfDVBYGzE-ctpsnr|5p;VRfg(!%;Yvpv!h$yR z

uDGJJe)N@QTfe$Fho?q5bSP3qV#yP|1OAdvEYy>khpn)h_M)%4)=Auvdik1>9( zZFh_$&B|^ijHmQ~Cbr?=_NEo+Bj9->zy}X00t7p}gSZ(TDD?qoLuB0yUmZ>vrarDD z4mWVU!`;iMOpew%>jht0q?nhq658#Q_t2lzNd(WZ?dSg2odL+US;ZE&62yy4tj@}x znK>xEl5QFyqF!KsVdibhbVE~*Uv))Zr*MK8rqh(p=zCaf(t+%TBQo@;nb~<7I{RQa z>v(Bx@mm42{`9p^uPR-yM9)-GL+(|Vd$j%H4Cw2Se19tG_^n(+~|4~TZe8``+8Rjnrld1QOO z5JdSh_6qCg&mc0m`k|=k>L{{(#d}yWl=b!Kau7|5)D3!)#!6d%Hw@w94O#8nk?Ke9 zX-&h7pAh|OKPMfo?iGXFo0&!ofB1F^8;X5L_cR2rm{q9n=Sy79aGR7IN z*-D*dylPYTa~oSDD#|I1cq=TlIQHuc)ZZk3>~U?B-FS(eXT7r4k)Xdj`{g{*K8;T^ zQ@)>Vpf6b;A7}f(JoSwT&+|U;6!Sg!wc@A|RD`tkjIRAZV^juE^8S?A^~qY^!s`A% z)|7a|g|qu=_ap0A5olX7>$qlJrlhWFkH^C3X1uYm=-EzV@nLfsVF^5?efmkSe`2PU zazf+Uc-3=PD?o`JLO?6%5EMPu;^7>0?B@B~5RDkM2+O{S`K3(jix}R_q zhv$MpAs>P*+aR{u^cOcG0Qs&Ut%^pu^+#br_yB3V6HveCQ7EnI>Zxloy82nZ0e_|c zEBqCu?TB>#hqE+WBW=*wY+pyh3vCX6q(f#$yr8HG`06~q0otz$D)^uG=>(Dz)!`~> zhpO)xXIX%kTCtkUU|jp(`5{ko{<_q$d>ueK<_ixBmy)$bG|EKX0M?Z%;j9bdK~T7d z=2CFZD(`s7Z!bHd`3HPPA;Hi6W(wkxOLw&W*l>3CD8ROJU3F@n{ma+{GC z{|t`=Ft0;pzge>s-jel2is*)tPV%(@1A6d3a#z57`sBR+Z)_k4%~~h|`1G4w`^yP~ z)>hOO&CrSlA<{`^H(AiDdTqbdJtm$(3wXH6?UY;9AHx0c6r~)+FN-BYgTs4T{qSPzU_fN99T?Yr^tJbwdC zMO;2>|7&e#4WeT*2s`7cfHk9G^JxL*|MWx9x+z^*+QJS1mt@3$#{qC>YRgCKI6zAO zug=emO%vqz7!C$n0FSUs_C{-fmj~oG&=L{$`hw(k;BUf4zaJ1&m~%cW(3_>HN*E|4 zb#%XK@IhX|7=8vhPoRr@9%nbdFnk1BoU%^)r=9$Yia~T}45|Bx55n=6lNI0&zHCyVz)&YKAj652EkGQ}3EwAbtkVg))K-=w~ASkr~{4rs5 zl(njj{Zp5jcuW09T@(M+H4q1Y^IOA5ZLzOR6D7NowlBRI6$@_=8o-C?Q_vQ$HGG+f zxes4dq2xrbEPMA1p!2`)cPpNydH;I{s4YY^(F>SY&)2`arR-1i-+Al*00030|Ag4r z3dG!c9^f5F!ZsU1uE9qMI#6#0Lq=uPyGHFyz4xxG@KyMld^yo?8xtGiEf@xAM#D&2 z>-(N}t+%GHv$FoYr-;fsR9so{?e8j;xKOqzkNsxb^(~A>k04sa75{1lNVyma%uXr5?)v^|!hFctX1!0_0)fr{3F(bK1IwCPlU|T~fcAP<H=sDS4}{st8Rouz2*CE*5)4 zy0*xZ3(RBk@gM(YdHmDksdHfJ72Q1bEpXeC3oIye2Pe2awzMA3(MO{mWO;ARHWo65 zEHr&(8=aBEj{pl_h>OjK9yLo1V#*ED+G~09a2Sz*3N-f76qG(~BK(2u84K31IRPIQ zZ4mwc;jqK;s z;jw)Lz!QjG!D*xt_2l7e`et@vWm?IGT5`pwn>5s0u03 zA3jG_1bW&@X+F8=vddtwMpPHJ=K{>^n>Tn9@nVxnLJs0NA1LgfOl}zP2VA%=YszRy zFNAaFalO=9OnE7P>M!<7*+%`4b7bfgd}zu1Af($^(Dc}lAwv?jDXY8@+GhfcVQ&G> zR}Ece57Cs!Rn{=Ig3>~)HA>`V0ywdDM5BrUYwQ@mK*Yh>Sr7HyeH+Pr@Wvu?lLv^e zC7PGPg;buwS{xQ_+8z zv+>`ZY=!{^hC@!Es#787g@={QYk_3X2h1N+@LQHeIPXRz(+n8K!_)l_D>7zhDP~m!sE#F6i%nml&jJP@G5B7;?PMl# zQvJ?Y!i8!W$c05x08)QHso?O06n{{064M{sOD65v-s4{*Jv3bnP){8%oGG>|dKVXm zv%qZ#+8kC*(tsb#R9)dWpE8-qLrsywzM$MOcAW_=WW!jHDF8IK5RRHH^+KV zA{mVt(nHSPzT!?zR-$XB>$K1Vw3o0h0P?I+@M6S`UXTRtLxkKMZpSb=9JRvJZ9e%K z@)8}s#STxPPzhKL-1#L@9Js;|gz9>?3+@TW`;KuRA81hKrxK(*4;j|82B6wvD=d9} zJqC@snUFKEzaYMXQqeHyKfE~Q_&n^pH^JYt45^D(aK$B}Zt(~}nYSU*_;myEmmZ3P z3yPthov$r%^p!fA z-M8Fn3^#?lx+wMb@aXKVfO3M=sb3M8_RU#{QOKLU`Az?${l?Yvk@X9`1;o^**jsIq z^tZ{izW+$`J6+rAW1tN(1M7Evoq+2xH#?$d8ccwAGbLhwcg=(>TbQNANZG17N6>Oc z-Fie>wD&c0q(M1|b`ISsi2krB`RQgugVS4y4CK4#UzCIBJ*KkJUO&7Pk%L;AAF}`B z92yf{2efp!(pT-wJ2xV^R3s&u@s5nP78jYypAS)HYNOxQI^>**Z$Our;)J2c@t+05yPh6g7gd6M59zRwj++_gr);g-dKRaoF*KMY0Ead`r# z40S>mKX1_=%o#J1Mz(s}#L1^(g+|E+#%vQgNE0NGZilJ>W8<1;i^Av;r^CmMA}_9z zc&kK{XPMa%#P`CDArT4{O41Dr~4)aNs7sS*H`P@)E zY5|vsrLCodCi+{gWr>jYM;-~K-D2vlC<-K<3Q`Jh#l8i2eASQLE^>SAf$kjoEa;BY zwd^IQXc<_6rT3z$;oO&!K*_9wj*~xtkn$i}Nv2>BanJ~+ zvb0~2*D#NIh;bqxqQSS4bU_5s(v8-wA|E(WSqL5*laDK|0C0jW&V$I{o7#MwiCQ?J zeyD3K#?C$A8=Z?3BOVbOc7qeD6aVB~)D{R!>rGodSBS>Q>_zrIwRA{OT9q=>k zfWRS-;XA&Lf~8O5$9AE%>B>OuX5d@P(F1;#jNG}!iQvz&b-x=(^T&Vrr0lo8(eLAH zL7x`D@V2NC9Su9*%6xZY#wGz9LM;>eqFa)xbJjGg1AftZ@yGAWf;Mi^j}MvV5lU2D z_NmoO8RkFWoJW@Y3_m2%rby1c00SYO+=?1>8$hboL#2|WF&;Tjm&DC@Gfk<5p8NYK z6}#9Zf@P@__$(!TQs+BGpiZopohLuAhN6Xw&HocZajHqBctdYy z2qmg)UpI>1dOE*drbkZ<*XX-4`;gX^ijPuGGhrOjy;lN?eticg3D(ESkx zQxi;C*1qC*x^3o=N8zwjI&~={Tu%<`PjnKqNfzZ7rzAjIhJ#f?fM~m{BfMpibpR`% zQV|B_ICk>%RrwYg2)Yd}!*9mr!OQ1ll0f@zbIlT6X~Z}ja4f|{AAll;1%8qqc{>8U zI=W)tlE__h&^I@N*zZATQMFz)H7^s0cD0%78B96|<(>%R?N87jcslC67|1jw07;YK zb|sc?jAEeF`VQ5j&#v$6cE@wT%)3o*k1jmzzL@y;Sd@6|ZYGBkIzZEemP%mFkP@9Y zIdtTg zPKYm_6&5C;6~1;-WANa;Peyi|(U)6n_f1-Wk=ed>GS6^Tiu#3y+~?W#+KEGSE_PqB z^_-}R>8O(CPTXz&84QWvT z(ggydq4m-zSP(mzWc;G$0_qhFw%il5fO9v`o0*dJj-i_*8l&2iOv-2;)BraaT`B>q z-%Q!pK|!16b=((7@DMJz1ajX1dhCmTKb@lHU+q|ea>hgSJZI(L$eV%XUuZ)9e^SRxoz}H zXJXOSaythFxMgn`#l?0`m1&R}YWl_Ry9Cu`7zJ0zoe056Zp%kU_8bT5EK0ITTEyi^(BXoCfYwmXlCmhC7O^CmR{3-HwK*&uXG5 zh>AgSCx@Tpq`TDT%?r8!`8VPe2H_Gr>wnZA{woMWq8wY6jijFL4FmUw;|AQcTrgXs zOTKrFqHET0iveuRuq-|@g3=!|X>((QDA2XJVIXEGM_vJCPGOOlGmo2td^G3{NT$YL z7^+aoQ0F)e8VB=61Lg!F3-E{BI&@$n-`;^~=TA#R_2~Gw9^iPBHNXnP5y}Km12REP z+g!0e;&mn-C>4#%iAR$&MxG`I-Ru&{ZM<Gl;6GgQwxX{i-#ibq zGc+lxbpN`d`GLJmDhX1y{gUMCW{lo{G8jY123M9qfY*akr9%O5PniX#EoKl98v70&le2({=%3JPo1toNB$ibwl{Ngq${S6?Mhn_697b8n z{RIlNyn#m>~Wr;34lkWj!;|$MCbWyj^dCPa@p69v^X=I0!84pN} z2*lJ*K6;i#@56Oml!bCJM4ylAXhxt7?xQ)25gN*gdpgod7M#xWE@!89!B~DES(71h z>Alg-Mi;x7el2Z<5<3XkuqKp@48{ZdLKX4V6}GqgVu+Qkk?K!DQUkPY-jIKOv`(O( z_V`?Zl8z_rqdrtV^6Yfs1BLJxHm%2a`e(@rc+KQ>?}#t7$K|i2u4gk@32ba=>xYL= zgB9@kUBD6F<*I)!IrOI35`o{#7)`luxSeT(gz?*PUu4i9O3t6aYbd}cYZ zv#WMlb47E?c@q)y5T;Xs)f;1?HmH>(Y+O=;2Fma2;_-g==m#17Tg z6`#F*sLfMtEoFvJtF&@aYl4j7q{nOx*uv|En1|L(*iIy-hM;RJ|MS{TexNtd0dkDQ zi@zajk!)8U0kmf%e7gpPfmXy!)ofc~SPhNC`j}|=#z`Lr$eZJ!Y7zjZ;hkb@D4Hv03ccK4ABBY` zmes~G-R!R00#MbDLH9QV#~R$adXIrXn=CnEiRU5NMbRLh%L61=h^H7rdLxfbdWN{Q z3>VOl+gho$oTTC}?KW7x;{VjK%I9}AM%$ntY{0krW%f6tD>Op4x;210Y0dnZxYP%)b*HPT z0vVv&=DDTRpOX~!NGQn179W3!JhUY@oo^FZ+(C;Mo=G5e$Y&2Xe=w_*6{Ilz89 zqmj#PW7dd`R}*N4U3(IO5IybS)!VV^9PXM9DAlCa#Vj=tKY(jYgXRmv*ra&WGS*VT zyA&{cK$Ynl3n)X$&zNLW>p|@v#ktY$9}?0ft()yxzckD7!r57iu)sXi)g){RBPUiYhkf6V|tz?&4PATkb{N z*{``AP^b}2-dYG(RKtDv>NmS>(!ZO8JUNOCt0fwHkW}vxa~Bf%9CPXKE#-*{qV!2l ztMK(S*KaJRqu5)QOY-PP7P`R!!6E0_NXZmQ$rCXnWpNDZi9#@+Xq-(&vAOV+iUUl4 ze=5>Qqk=olR3{M^J>p1H$!lNbuBsY%PEe~jF}qoa9YZrwJA|H#{DUJ|fj@2?RVh(F znv@T4;x+NmQ<24il3`R~lZ!aEu&+;)|Hmg#(Nk5Y^}29v#*HRxD1U03zS#EAPOhXt zdQD|OV$Tj~;>C^}65M%XX}*>K8($S{SUt1u+#zJ4SO62lEa<%s>H`h|m)_RGnOrnH zD}saPg`DEU(iL60#4XB`Fbs$*K@bJ)H7sZB@x!mHn78TfhgBq;ff2rf)estT@9)oH z^kg2mFs2-A7eov4*n}z@4B+VK@;AxQ9dN*Sg7!%x@z8vn&qY5IZDMWlhU2N%iuhQX zVvkmq!A%)_LmGH4sFU%i$NPW?`IjH41!JP55;7p1aj=if0Oh#YRdM2Qa)3h^D8&&w zei6I~Lo*(84e0@#D>hhi>Y!Kdig_<^S)So`PSdTEA09*SGNzy!HBqMYkVY zorDwI#U>d;XzRa5YTYmurd87fp$PUTSds2Xd{XUxO$9SGp0WqUWpCtzDXI8~R;YGQ z*&uB1s|u<>`@J!=QwN2>Q@cMEd0q^YnGez3&xsyyGtB^EQ3zyjnbiQi5(Wdc0>S|^ zmAiRTlY7TjPtxngz_5XGl>8nj(jW!>{5^0WdbR)fY%`EUY4+Y1vrE+RT)%DvLr>sk z2<@>JZXFl!dnnGtseZgiH32tfbzzaaeAFU-82Ld!2H`I}jB+!~+eX-)>$+Y)`}Ty5 z;>{8HC(L7X%bs)8@s~SW>3MJz6`~r8z<^H(L$2lp<^ha_`lOFMFhcQDSXuHM5_Tcm zDlbNe62BCz@K~<^*XhX{3=jLP?(3xD8FMR5(-9QHj{&1h0~fWv)Ss!$$}y`dE4i0! zU(@W^#~_|kp9srzutJ(Kht2mxH-+5^mbxXvHw3`MHtR15uF#x_65mx`Ks-|8Z7Bm6n!EfM!jBolXcczJ;~5ba_J6OSBb9no{o-beeyS$ zK6ed^Ir}?nj+6w<-}O8g^*RF}n$&wQ%Cyru(AxxNv(#)F3Ku z6frkEc!Kij6+1o+!=rI?NxRiB@kW9kYCbB%Tt`$o5fg46IYmvMIg4ryoBs;{0RR7l zm`4u8%6grj0n4x^g4Ups4Gx%-01uiuHgjkW1Tg2EIr0*`Cauat^WGLQi!9HW!wSXQ!H9rz)|~rzk>?ydAz~pE>6#gmX7)v zvl|V1IbZyF7uG{?mpq+vgkRg_q@H5%22jBh%E3lx59|>LlB}`NrWeAU|d+tfhCsOTMku?nxvPR84))ATqu6EX1Ziky3_0ed|y{>4E z^l3^u7WCWyKK;c~zWAIIF|?+_;1v0-&z>BhXY5f4{u!jy!$dS86+zqqD4=4*lTTw& zSm8*bsTG56#>gZhpiC`Avo*ul#6tXc%*$X(R4F&9?A>Sux*4eEMYb#rG*C+JyZchG zQKFq;o}`f{CFo=BD~&hpAr!Ef#K@SWCQ41Mm<`vrIfs_*o6|)b>pV*(AMPM5@gF77 z@Nncso`eO9m)Vb>f!e!)GaHu^wGx1$ubqk@O1tM9xDBeAHK?RJOf0}eWu&R1&|pr< zCyQc@`arwXZ_p2Cf++*580S_IB?g`CPZkBJtU<1u$W##`t;_&wlXr|gmM=>D{*3U* zBsMfYNV`I$7`YA&qF#QKNBVTkznBT)u;#2ESjIZVtCwTP*9W zv_}d+2-y!lx*JM*RNg?X3}N8FDuxL-I_X|bq7JYTMEiXJl zkeaOMD7PnP6S;7>$Zt@_L5Zm(RZzP{w9L2x!zuQ-drrf&GF)Xqhq+iVpsYkY%f)ML z`MgSF4@m{MVBiE!DE zuYYiP%@A*B{k79RK`ItNw2mNXg6Yg?c*}&3%e2RWPE1pUjC}@Jx2$K%jLc7`fBf2tBN^a;!4Z4uhyxQSKGc?=TGe(eNq>5 z1lE5>t{GT)j4+S6&59qNI%V&o<43M8SWb1^({U)Q`$KO#IyQXP5VsZ9E?CtXc#5wN zL$M?o;GM=$FjdDHgTbY<_LR$H6z!=!<}xd3CD5}jlnmtbUYRa^z!7%*;<(Z28RKo( z?a}yUuw1Lf(e-s&$qC!Vl?GHeGyk?PPYAzOmtKtT@^5{)PEV0^%kwVg3KdDPycFgX z_LFtDeg9RvUQF<6@jtcNt8FjleznSvCFdXI zAO7cG{yhHf9sm1p|MHjr_RjzP9sl*OKmX-#{^k6?yyN?~f1Uq0{{COi*SjB+<{$sD zSjOJjmU<}v$3NuC;|D8zh>+(=RAxZv2{B)5-c+aFbhg`Yx%cXIW`LY?B*Fb!k>F^| zN@@Z5GbmyQR!Ex)cD9vtXIy{*2w9{HcCg0kIT%xxg9r(IgkF)5^kJ!v1DmE$seiJg z*y$!}a+7B!jxf3h6hIlLk}Cvxl^tjtl+G((BN}3@sn}Aq$mreBECLD%MbN+`J#rv0 z5GdHtZ@+tdwjs!&00Th$zi~&GRL8HonU2r)6-yQrBj?rO1d$dVty;KdDilW=aMw*g zbl5rm1_f=Pt5-rllj43)O*iM?eH(>Y0&Xtar(|IWYxqzV8OTKnZ;vI2ClR_gK^DxW zi0@l7hDQeVFqO9J+s7`(55ZSV!kB|dnsh$yHN3$up%@ik^ylui$6Vf0HQnboQrdFb zJ>s+B$5}Z0JWysAI3+jN1444=RE?gG3kNw4XhL=@fO2H`=pS!Ub<~9<{wBG_`SL@j z`jff0@iOODpKIc3zW6bjb6N}Y4S5}hzRex?B|t4cQ1Tddb))YlzK?a~Fx{OyN4jId z*`#;4b(bhlDmnfYE_~9&IOpcNH!N``gUy5lqCZ-?dTQCOaU45MaL-jLm4c` zyS62SJZsRh&zH}MeiEo`1*vCHf5DI^;E-jJ;#~boUn2&RDr2?NLG`$JbiHtWLOBkP zdfMkP$KFCa(vSxl+9VqWQk%r{^64-kCX64;D|z_#S83A-VOJnMM$b05!TK6Alp$VD z3L)bWBO|8)-AgtRG0-@tZNsn97RaY+56hMjn1DpUKhS{cmQ1Twvis$4G-Ts`pS@cy z=d*L$?3{)*bU&c+?4iUt!w(2oG{CV5Ok~7;`jc^uQ6Q`Es0eAaMu&xq$fzw(o2wZPFnIQkd$G?WpyzlCg0OQ9+fD zIDjSMy%m?Pfj4B1JA(%)UoW?ZI09}2r?{{5S3k5pf;RlaCaL+XwbW(jzj`YK~^sce#W8@FqbkJGy2nWO-Bv%Wu z4!xlpH9UXl4?GR!{Qd4jth8n|RP|W{@)$*w-6EhXViP!kDGr+u0ztI(XOBrl#1SMJ z&e-E4O?055L8es2^-Rq^dmsqaTCUQ}TQj7+ZaKRE&By$1aAnb)4CO zi1x3WrTO&irC%xldvWaSDoLH=qY8FA_XqfL?n2#00K%&;xjY<`hO9vb8ySV_a#N6! zKh*iuCGdl{Lj~j^u@R$+tTcNJbCCDXdi6m=9xa%0%VcyRS82ehP*D5haz$<^VP8M`1G zCmQndHXmB(F`Vu;9{Eo<$tBmP7YZu?Y=4bQgeGFLwmm22*FZ%v{d^oE$8Ut2o+td{ zKhy+4$0DNEAhNz$k5_(j<9SQHpP+>&BlMyQLP|-8OxBNVe)D%f$ONlky@+FC8oJ

&*jb%4c7d^Y8e$;~T9%l5o18eJ4;_ybgzo zC4o&=6B@d;7%X_;2|Jyz!_uw{H`pT%ZVHI9w7cKZ({t&_;b6s=Jc>vXMR!N|i-r+U zX3fX3KI1S{y-UIuC}Jtv!p$l`k^p6T25^h&oTvadmX5gZR?4UP*Ce*$Ez#~I(JYGh zdPb*l`aw*3Sn3>VW8TVo2v7vxyL8Ce~0YZl%G}#3SX*}*7^zsvD zU5-iyxhRkOs6kifL3qt>s{+#v`bb!kebnmu1-rpt&slhF+Mo0%z#7e+*zsoD0STw2QB5;Q)(%li=|F};Y*0sb}*hE#@=n^+0hiV77p?yV9QyYIgOU;^^jA4y+ zkkMs}mw7A<8y3JJN^3i;D7oL{vAkR3(#FM1$sa8C2Z#Ni|AfQ->EChKKmErXmWdmE zAdrhev#mjB+>Jb(*>@^_U=sPX=DEu46c#h*?1&ohoUeSU9qS$O;_)qd+$MV4I{QY2 zSe)je8Z_lD*ypNPmK9saYpBn|e6BDIF65urRAF1jZ=(U@+8~fn`so|zrFH&L7|M*m12%^vDt)DntZVKmZ$Qq z!oi@=ns7PNrj}yCiPY+lYrd5r3o#VMOE<$K)c3iZAEoF*{R{1Baz za1pa$Zx2WtJA-Tl0nYPK?kvXVh0Wa&N_!<00^OJ-oeH}tUUEW6_khwLYMQkp)2to> ze3oKQHx&AoI?oI>zi(FtVvh>sIPo%@-+3Ij=4wFkuVUa0HSVs9SnaGcn5#Q`im7<7 z(73Jf;uL`{=L5md0{H860o1#n!L_uvxg((IQYy7+1$tX< zMO|8eL|^&4|J-E+H1R4t>q&BXI`^rzaDK(eG2@u|%uDYIDXT0EDa=v{F)y;{c8IR} z;^mC#$XzZ&A*W4BWYEZS7?1bT(C&-4Ki~_Qk8^(EK7X!kaTY-Kws@{r0NK2~TcMG{ z#k;*d-Q!jx?wO6KVEs=PJ>Tv51s^bJk(!)Ehz&0~kM!?wKdhb(|DBnUInCv={ z0Bu&p`!NIcanKz7tF$dzOc|~KM|v2HLaoZ8C*+4psy3Gh`b~^lhe;yO?gvw7iGHe> zjZXKrhRHS>bTv!2dMhfxYnd2THgUbKpKi!?-?dxGYR)#=Eoq^x&Q(3J%{EOF;Ytz# z&uKn?25H^fx2D~{68QSvpeJmDyh~7!Xc+vJG`YJ)D1hMv2X=DE@$lw5aOg>*i_5n$ zdV{1=*GD3$SyLiweo1$LtstCkXiO5oTMrZ1S9x(2@=a_;Yu`IiidMl5<8ZVAteJ*A ze7PZrk7DUHTf>hrfeX!*LUN0z6{zXP#w#I5CQN#|+ConEhpCAaHnnO*wR=f}Z-*apFo0&#uX9wfUDoNjs*jFC_?smKtoBny=`w#c9h53w7a_bU8RcAjpQg;@G7P&YA3*N;z2(Sh<1Hgd`AGGl*vYYo z(|g)JN|8_~0URy1dhol@U$t#tkJWdc5^z!fa5#-6R(YK{*2i89xP$~1@L7bSQ*U)~ zp%O=YRRu`}+FtwYIi|T$mRrJ*HuA4~M^YY&!_HnLtuDx}M> zxzl$;shnJks5s2{4Ugppe4!j#@Hz}GEA%Gp%o4`#=4(+Nz%rC+4I5=i*kC;&CM+@@ zcTX3?sXVB#ORwObB26+DOQqL z=Lu0CNj7UIifV4)3$jWso7eDJK!T-2li|XCHJr=J4Vk9tDI-|NbXj_Rs&0%l`TQ^LQuc@l@(4U=JpxCvh&+UOW<3b*+|kMe>+9)#+$rT$j%It?o|PPq)=*tqhu!A z;pNVn3XGK)zN(@javKuF>1l_*cdtxQi1s*&qcaepe|&$J>nlJDthmKO1R{_pvLCG< z4*-~#8G)R=T7LGC7go6m70bwBqAZ_gro3JYcL|aw#Zx$pxCuypMdPBrl4S_ODaglJ2szEAgpkRwx`Kr;{vwNXroRm>J|Vvas&p*;nzu;Pp9PAt zBgU3;%Di00e-kvPdSriNDoca2Ql|}@#_e@H1X4qwrGeaQdcN70gRsiGIJXOonY2t9 zEdxNP^Vp@g!5ZJiGK!sHMJHzX$!~VZh9;NvIP)$KJA?+*#*9kYxzZt{p0_nUJ%O&L zej~Q6_VXvDJyr7@BXLK86N+edh1y+h?If2-4OieyOsFSS8d-j^Hx&()eAB`Rm}mIb zseFJeN5%TfWd!N5*5}|T(weH;#5Ex&nm4^TcMrVTsLnZ6UM?$BLIl_WeYwmAS^1dS z&7Z&S92bI{9;16Qg<;GUjpq*Xt@fALy-n|{W_h`cnG5CQR<2=XTFJ9d+h6fx`E?d2 zs~^n_j_`~GoxRgxSVXN*Nf`qcZq;M}Y3k)xQWs4|Pv1ST` zymM_5G4pBiC|?1~6-gZo10k(5cnhzfyPznCs3fIv2_Dz-S2Y`!30?d|J3sS87wl3P$-DVO}nY?vgOfIh5ZcI)t< zT>}3E1RP{nOX6c;Nl&#k)kM0~zA`!(GRGbUN!l$gMUjFDJh+4pY|1vZcFu?6yiw>C z-Y%f5P0PXkeV7J*?fZbB`%XZI+O1`#K;0kadttHm+IgVFAV8_F%)B$0Cij$kE_)Dq zPL{5E_fudM&4`R5G!F78OpJ)@ln$nTrs%vmu*zemry+ZOGiMC_D*wUrDD18`3r3ev8s=4LrV%01i~F zF;W z%d!5j0I^m?g|vI~FR^9wj^JUtgOBv;esR|Qo@5B$_TLf3)fW@)`hcoI0+>h71?xSA zwse%ypZwQ}Y&DGbo8zake~rE>aFozdO>TjtP?t2lk9m{G27yvLu?#1M&I{658`L?$j@ZHbN=6Tuca!a&?0um z4v1jw9eXJ=C#x)Sr_kJ?x(+q%DFbIMqW$G%W67S}B&OKIo%SZLWvOv$Q}Np7s1Y&= zP*4_M!Io88X|duNOxe2%%5$0asuw!2W5x+@%oF+GC(;UR2l7-R0RfOHZTRW~`!5@Q zyFjZE=1m(PIU%=)uo!O3sLr%Qd$A~FNjHe-dBwmerIV@vol-VtVCK<&=yr$(dJ0!~ zcY_bLOu!N>r8ku;(~oE%)Hkn3S-ZX{pA0p@6BH>8O4cMWiuzEmTnW9A8xf)2Qh+uf zexYpe!t4#wrI57-;(2`Mvn4Qk`P2w4(qd}pDX0tgmFVbuy`l!@JUsVlNsg8g!g6>q zWfW>jUdK*n)^Wvm*3ICf_M6UYN$1z8?(%i;Jl$Qil!&eT5`5K$xvrKL|7_Y^rZ($2&!9SDh19VgL$xaqILf z7YCc%okKb7sbwjp(Wy5abXRVX7y3l)YhTNO79p*&GJqDdLi7wE zjorYtv$TBXN%v^Pt{*Y?E~CdEF=K$eF>Ho(y%MUR;$ic9^xSGJd32D%9UK(C)^|W? zaNPwNQGP@Wh*&1C=94aKYVd=(LkKbEHeiJ*hUJ*e?R!N487dfbN8SgKPaJK*&uty( zCoQ1mM9cIWV>g<|6g@+0?h+ zWh%S;fBbVQUrwij4O4yRINL?!TNI~;#V*q!Cj$+w=T%28DC{L6F0DWnCjJwug4y%v z3dp7r5Nx$%OryN-7YtTzL^n=wpT*^ZoEFT8fJr}}UW(Ir&I@E`*5FYz#bt1KpirW% z#rcyJ31!D+vGF76h*E)KbG@jkN$JAmcbgmk2D^x4r}--a!4MP*cUAf-O$ZC`fP37>uLnF zf{LG~Sf;P`;UmRYS;oi%nB>C)yb!FoxNx0Z**5K;n$Tp3`aq(>O`A?CY}HX`U{o~f$mFHHc1_#ib&R9= z3b1z*o75wy5Z-uDNc1l1#81U$4TFV6E3BG3%x>qfD7gsUWpRY37Kt!< zj%|WX;z6p`=+R`cgbdcY1j`q%6-pzPz2fE35%N=A@=X(z04lSIC8MQ2u3ghDC;L&+}rj~1UVlrREh#JT+)z@Ev%7us@TzC~qR!+er(Z^O8nfSrB*^Bn9 z#th{+dE1hk;7u+f0-&wQ-sps07=~52kYe7n$yAYPr8B=XD5I$aEmP+?a|jGI^O%XF zwIEt^@*d%2R;0}#MTb2`K8k9k07#V=^gc*eHEb&mJY%#f5XcD>RqUju@v4i&vZc9> zdaHuW>w*CkQV>wnGZ()P(1n&WRMnV064rz9d7(aA)9&yoM+2vCCT{fTp5ihDB<@fU zi1d2+iJggz@*T0@(*@I+{yB_+L3;80K)&%!=6OUd4fR)JXre%`s@1gPB1D(_JX=#Z za^J|cU_~Z3rU!`3woxOUp+n9x#Hg6LGitZ56EoTl9K+n`>+xR{H|+L{1!-oBrqf08 zniG$!#24aKE|3E{#roP;W;<8Y6+D5&UfdrNWi06)yTPHEUH1-_wOKXpuotsqYDD!< z&7}G}7uZ3#j&)YGh9hp(JiRQ`v?y#e4<8m_WACc0nBWF!BFZuI$e&Z7xgW!xOJiE+ zC9}tn5O6;VIyFP@_(jq+O;;Bl0OxGi!}6dhiSMXIOCv^GiBGWs5;7FHYH8(yg-!yA z;j&4@$o5QudE5`vp~sNqc+|}Cz>Su^-D!$qGN14VP-fG#Rwbz(#-@{ z*p=6wVU8N8w&6_@UG?<9y+Z+nBc6#Om#|mKshtusCf@Z&TaOk-$)t;b5+D z5~87XCu@HE(intTRG{v2${Pe7H!oOl@63;ZOcEMPL^o5)c1ompbDoH;e_6vAs1s&LV6)>XuEztF*ED3qKoa@zBT`Mq8Meoq>?sJe=` zr@LT+to_1p#(#0)cRz&!Ph2`83^$Lnd6ZFWur8gK64MF@+u}PCp1^r{ZSbP|3@#;{ zCda^gr7QA*G!g(OmBdQw3LroBW=XhG?8yEePx|!3r*0zh%aFwsGNQ$@#$ECP>m|pb zGyu!6O}GKD3cJcTAeMJb?WqhfkbB2Av7Krjyh|L@7%^>vmS9nJ!aKB9=l1PLM(haJyM>c)(XF|$*XqXO)4mUAr^2tE9X^S`V#u~ zrCn)Di6Nwx)k{J6_0ywZb+rAX^*#~y>c6&PYexoaNA00jcZ!?M{YY(AZ@H5bIg$8C zq;IfK(Rz{}>5L5Nl?{-t;@{Oq^0&%d{EH#VeE$QP`1msVr&s^La35X5c zuP3|LVchHag|&vqId|KidM2sAQDH>!UT1Z2zi>K*SpTxRcaK+Yzh8LUn|uA_o!)*B zB{*HY{kYp++?u=|2fZNxaZCl{rC8w)Mey6gY^q4yIlI#U9XP~+R&1RweG{G+lRJOeBPc{ zm!}n)uJh`y{$ak`vDe1en>~^Sv1fhVqFbv*LClWHJJ$Ix^n`E;C1Ms!!s7X4d0S?` zuadzT`AbVDpwmljk1kS?HEhBQM=c}U#O~U*EJa_GsY*MvQw7lAEAq}=Ekw{(M`un) znTj}N>{lSLCIX>X0&dlN(z?4skL-g-aYW5N#gd57Q%F1$7lTcoML`Ej5xNKo?-ZV7 zNm`;bT+6iKm^Elj+UqN-HV{H@rFnyJ0Ws`SFQWLl$QAl3peJCDFb~ts*APNNfs&TgYLn`W1K36pf3xo%<}%Xml4#oZ zGoM>z8`H8x?5gf0LSM{Nk4zzqvwfWW77z3L+bLLdInz|8A0eBy!uAaMizgkXxNj*PVxKSCJ_>@u?!sGh$KmSEVOdQoM0 zp(*hW1vr!?RW7I8AYMG!1ZiK){BzXpfw{t)Q!jKM?E_Lx0rANUQR8cSh}i`4$jxE% zt_evxu$toRi~CqUp+LdK*^FGLyk#Lt&mJ{O6s(H&W^4lyQ*9!N@X(p-D5?c- zRN-?8(=ysXMIRieB@^{5N%+Z8AF_;mySNpg5z#pki0K(zL&zs7I;PoHZhHu0QzM+N z!DMQ8q+9?n5tSOA_Mp2q6#4dp$^t4-U0PA=(u##ly?8vS*T{zAqFebTABv7+Y1Cr_ zC;*cJg{0Uug8D*{e)}oXhv;=5ix_H6)%qOQ8HX*)8laDO6^GdHwB&j}z8s;e9E@xb@*j!8fLQtEdT!>dGdI&LY)0yW?YlIWrvt zqN83O{#qYfMaV`JdyqGeGz0t(iIn!u8#rVvQ$>k%r7IXLY;n#{?4I3cDys@*-!mCeEe(VcCLE*sq zZg@=u`AC~FtdQePTTEfk@%aYcVa<{MPA6*ASHm!bZ~5&s5)}ePL_OUr^n7~+@e`2M zqwBIeQYR`b08M9K!wf8fgIIYYiAI{4C8evlI~)1mKK0IjTS+qXk|m65i+*J z&}Zpw#Ix{#NN|Xe_NEjG{RA1l5weFk2lEOOqH&kU@8Mysif(U8_jq0{rUxc!FW->Y_}RVDP$RbYYSp1VL3AOCqT>cvt5@`L#1NmAPa?OL4p!);Aiu9P z-Ny7YK(vfZ{%c|pt2>MfUtBk*KfoR6ulUskT#O`>?f`7)@>Tm?aI5EHl#bcEKj}&u z6m7>iCJnceKu8k9_3({*q2_SZ6g?uF#QG)8BIooy9$X{WrC&B+X|Q5I2E|fCnn9;u z<0(P@WA=Brekj=sj69$eD!+OEhD%C8-^Pr}?D92g@#eyDVtECv$@|P9dCOITsKqyi z80W*J23T5z&V$!JCWe^hGC!gsw8Mik<`C99lY>GYsI11+yHio20me9kdtT$td&(z{ z;dj<-ZMR7;S>8U6E*YFXtqgjEM%wj0APDm^^hUq1N0%CXxIrGKQ z&f%gx>g&&RplrlyZbwS9)A6Ri)5C~~Y=Av(D`3Bn)8x)ISGc?|v|YER!Oir3%-VMQ zc-!_i-GDcsmB7+)g)($z zoh%(0dnw;yvLG}N3?in-A>`#RlH3>xy0aelpqbzEs0{%`l_wx^=5eUYUpRWd3d$$I z(0fdn(>P12fAyuH?+zIs@!_P5RS{`yR&tt|7n>Rd^@P}ol7lyQ&vTP`Nw2hO`0(7A z5ioFO9qeLyxfAbRN2I%VyMMfjOjW01;G^6cfkv!G*m|_fYA6w!hFWEq^P*S8{QeK- zM+f}xfB5%x!2kM(zt;gj{eMdb2sLu+0P_J#P1Ewa==osyp8fmzCBf(UirwbbL>wtK z!o%dg<@l7=iv*7_Sy7GpVnJNC@p~3-5%7ZJR~E0&Xf1iEFv@<6KlbxgNPVhCDP(k7 zss?F9D)V4e1Sct;3+5O;4*mQn7+C`xqN_qAG+3 zY9)2XR}0sIW%7`#d1F{gE{Nw@nmG#5wp3mxpYM-Qf)Y!otILvIr^;3Dd0u{=FbsXdyj+(kGbK^p zEUn7wgwqt4w6u~pl!}|MXX>!yiyL;kkhQ?TqLY41-Yy-P6|f3MzVz z%)MYh2+1DGQ$N>LiE8L72}PcCG0JkopuSvgVdDU86Mb!7ML|;*(3F=K9uTu+Q|YKh zKEi4t9)g*o)M)wuTle6=A9&S}#7oeRH1{|~31E}bs96h?0x%I-=OaX!yK}LCgDAWk znVr{&vd))3k}`Jf#>;PODx-P3_~zcX0mg0ajq7E!@~24<3^wMbr)XUE4(T0gk#qCx-2#2$NowN!GX z52&+d-{Bz8 zzBO?Ghs!2stR>Q&+uEn?ul+L6Z9}?yP{Kq9BN7fw<6d=ac&Z4su|RJe82TgQ0{g%< zlkuY0aa6JGAXXIgNueN#pbIY`i>EDnA{Kpwq9_RE@u^cVYv!n1iSUp|N#;&1|3w z8fPdDSC~m@TsYu@;XV1vv#eW?xGnk+zTCGbt~O+!(>bCKtAa=be;!Xe2=aZe3-#^$ zdv#=j{H)zRQ?-XLg9x7I9=v|1o{Kw|d^`{7Q&Lx6;SmGiM8W%pm6p>ZDH@}C5p!@#MwKW%h7bH$m(0hj;|nISc!gk&6oEx=pf zouhg2B6EiWzB)aWzXTI6o~q+UUVHgK@9ic*PL-d7K#8#vJ*_6eVyKcKw@G?1zmJ#8 z>y)n_QCYq=gSKCk17wV`^DtF+ll^tz_Q-H9m~1ya%0U%1Nz0?~Slp-~two^*;C139 zQ?Mh;V|t#?V0*-N=Kx&PEOm~&RZ7`m!w}EkvTy+%9eJCD1x&?V}%ZQ9g^he%hyH4QH zd?lf`B!lA54>etw7x$Gyq$3(a>ahl2Ds?wADI6O)GMug{vW5dngu)sqj3P6LuR@%1 zMF^NheOlYz(a(oCN%?S`iNLa{5~?fXMsnAVZ_;y#!eFFF!@5Ib7IVy>rNZ6y^1OQU zs1{&LwxX7mS$7ls@%hBd2otNIdqKIyImBFm08Se=UM(?R8Y<*mBy>@0>}XqoW zpN=b=>!1>}?~!vG1uOa%RUPk$P1s{;KChq(OTQ@|<-QGDkRN?-$8y;$ZdGPW`JL2{ z?JJSDjlIHQoqR>wv-msaMLPHW($l195x)&*P@9SquZDoms{q`is?NNB_=msyAAkJu zU;p^ykN@*GfBf+u-}Y_(`1gPOdHsjK_W#`*|Lbr5Z2xlox4-t+d-!GT-~WBJ{04Vh zn(_PJ|E^S@KT1-NoB19#RmG6)Fs*e$u`*|SC zVv*Ty-db|bXB_C5N1q@1#>5e06qe;&j=vO~=N@x|?P?o}HCu|*7NE5y@!d876hD%s zU^t`m@*@X;n+o&RUKdnjYn|fC2g~ftz$fF*Y>?SdvdZ}J)tME%@Gn4+2Pw}WycZ;& zOalJhkY}!=x1w5^1lIw+13fa#4_K#7B?KBS^XT|%qXuZ4lu4>8cWgfb*bOe>+Zr;J zu=6;3{DrbE{>O{cbE*W`%Z%@PCvPq-SCAn;u4NLd&lrVS-X|)abRT>f6Bbk{Gno$yxPe<}eGLH}6C z_?Q#{GQA&pzSA?lGENIzzu^#osS$+g4(C#`oU1^?^;MD8vN@_;*p9*I%lW$p;jf7X z1f=JLVjO+SozdoKZW-gKyhzpcBSMcSo1KR$GL1Nj^wb)%0iCIgi*O_UhhV)OJ6OD? zM}tlHpyFa+W6~aj3#d=?7A=JnzuGc`mEW@>hLOMV{_Y+;@zgl%by@{tmtrwli_(C7 zceZC^Sj`Jw7y$RFHZ~`p)r`0Lc^1xASyPO}A9SgkiYdp(pexU?GJ^x}z(HrZXf@4+ z3VFn#I`1FO7+05aVnm2tY?5)&{&|4@);1$8)vna2^^B9lj+I23x#+rCpUUawhp^RS zXJpZmj#B*KcMEOZi%qT4KysW-E#Kn8;%!Uf2& z)bEeFc=|CLgoJNUjY1xU5-H7O*8zy`a>){iit~*8wCFybYX+p{8?RVrp~3bSj+2kx z=*sx`jlEDFh<*mY7K|JNl~eFB$p8ZS(H%5bqW0D@Er||(m$iOTBdtDA3`#YN0klfr zA54CgsT0uR7eWihq{VLG9vdPa%K8gIs`Hno2F(KWK<=f80WO_wp;=hQ&h9G;aYnoFNMd{jt3CVe@IYw+YM1Q}tgV|Tr$C==pP zYO*M5bimON7gT&R>t2l4g!Er}vy0_kl$3*?TLlSBH@Ql@jy*kcUb6M-&*=C-z$^7oo39){ZXNZiXOD`bB$&MmcG6ivc}ha{8lS0$(Yw2>|DA6TyK zvAS$)GJn#DQ=mz6kA)&ITed)H8B^Xi7j_SpZAMi zYz<7fg5HV?qVQ@Yk-i~w=B+hEN}8t#Ua4R$Zk(KLff1(J4Oghigv?OrJ}B(U#mV{V z>ft4Yoh{-fvMP3&30E@UC~gV1J5#moxF#Cvt0`EB+`2(B?YkbEOb4)>dV|->C z}TXA`n7IR0DqKLG=`gt((!xmH3173*Y)~O@gWh1w^RKytQ_VIxMi>))7 zT-o+%ks8;+0K1_<7IQC#`T9lDTO7lL0KFC7B|pv%f}>0FP*xs42+GD2j{ z(mGaL-~=}Z3Jfm4hYX%g$2E48m}{>hoEfRJrhSjUViXSQ2dLQ&L?*BZP`IrN0k4Mu zN;>{p?*GR-fAe4dZQl9c|M{Qu&R_cf8t;tn4ncY6bx4-I4p-)mL+I3UUzYuALx)8% zxe(0~zYsS;gq_++S}RdYcF0ep!-PFzexs__CsScE`TJ%8z}Ofku>sR{Dud+~X9bAS zL$;o@4|amDo%sBIkOz=94O9hPjn7B#o6rpD1b#ppY_R>YP#v64Dz!qeE*}8h72rmU z4I_3CLO=)rkZs`4vP7xsIDRBr5Wz<05?Yeh9>o3xpk^_7UHPGM`~xi;)*Kr;7r~KH zO{wDW#03|2kb9~aQAVt-{|OJ;)Q2+H0dZN1NFeEcZ&3ur!U5>qYE9G7Oh)H0z^e3H z!5@wom@o*Z#*9El-%YS_xrQhec4Xdc-gQ5~1SFhe!z2(N0fn2a@~E|qrf=0%R&d(C zl!9y5_+0Tyu4V9}4S5AWpLwA`-=e}nf3-Z^VUehkT(a=?%4{B~q6_V^MNPU#BIV$! zdv2fP0E*gT6d7FBTU44m-WOhB^4Df?Mbd}$t|<bafZ&1kIrZ{#qj21~UPUF1bgu-E|zO7KhS4RMeL+3y9GDx#aNf z6$lc=1|23y8Jk<0)cWkR6#gM#73ffx*%J*x@l9uQ4jm&mtgY7PP~Su4F0ti(mNV$G z)LNo(Y)Co;qGdED15mB+5d}cv&4(qZFc9QAGs$*03Yxu`bQbMXGAkB-M>?vt=QZRdc zNB~vk38NKCK>lD0gs1+Ub@`=f^cpT}Xl@-q64CWTF%XFlcY0khPIp=66Lku0*tu4G z(NFjxt1&U`MG-gmhxF`9o=C`xOAPHTBvR=o=$^QW04v`!i$#-YPgOtNGlAxDc=uYS zBiW-+uYI4-A7;7bO_Q%_eDW$a3(=H>B5OYm9|g^R%FkpW52$y%VM+2qV;t3zdqP1-k4Z14%*W12W8;{*o(^A80*) z5$YhQV=CwDa{S!i4sA^=tjQ6>onoWIw^_bv=yHDq4ImeX`eeofJyG!HWHJFgW|dS? zw3E5y(o zF+vp}srD^Xhqaab-A*kBOrr1MHrAb!pf>e=dTj^@-Y>vInLuIYaTcl3hKUM58% z+Z=Dz*U;0TWw*s_9E6`#ttnJ(V^TDJ;05SoURK4ph#7V^U0U_csOi)h?@%fR2330U*KsCtCRmjS;05C2!^j4vum4C3W37`V7sb%j-rk z#wi7@t|?BGW)L^J< zzxmSd*so7`$T!!2jJhA+e8)Fm_nxTXW~uv5@Ve*!8qdS)@|#=q3*w>;lHw;O#TR)< zM*B0^X+WHJ!YSn3_;Pur!>quY!`88XjqA}B{?S!WNn#j7F~zb7nKvq0kolZHWf@HP zdS@xFJ@%SgBf68ae{p}UG5uQ^^@BPTd)cu2-Ur>*{ulR8C#NAx`lm6p+Cv?D`v2~K zEfcu^Y3zar%SqesgA@|)_%J3WOT7)dLmK%4bGrE! zC-Y$5RtmFstbdGmJmln8~2rtlDbRYU{~yBLO+#4KVx*wnnEq)EgaWB#PiYifV+48unTZXIB4|nmrjY#_ zAO+g0c$8S$x&`@hwpBL+=3T+z*PjIM0)hS8O>rz7RPiq&QfyL}&e0!gdlv zOFBkamz(q@>DdDW<4KQ3Ys^(c33kbX02e^$zkcvcVhx-p5))8xKYDK(Z-x0Bdgfqw zZ-D&jd6Nt!wTK^Q^cBSh7sR1eE5*oO@}Dm!Pl+cMw{FfeBR#uPt`3-003$k=S-m${Km?Hp+!3vWZ`CyTFvC6xFp~ssy+l3x=#Sy3}5+PW)v*o&1as?aOW6 zmdO|990Y-OtEv6@)?Y%=_if|4J$Z3bN?OHQmbT})wOQcZH76lQhIo6{0LE=&sI(y- z5a@BT0I%ei!-TiKJQfFdO>i_GCB4<$wJZeGlGWq#0QLb{L+ygA=|E1V&=%(1lfIRp z-OFSuA)Hjx#|5qzGa1{S)~%mSZ^?n-0{|y>>rb{jHtL)Re8ni2r}*4K`Sts52+x(p zIZTqRpoV|>a)7i=MaOpPZ2KhreiwZo5*E4Kr3HL5Hs1dQ00960e0N6=gv^%Rvt(PA z#Q4k zZyK-U0}*}=1rgDvez#E6&b})s@D+@`zn7lYUwKuV#pb9MC0B7G0NYljf}N;O8C)i1 zpUEBwnh#8gp0Wm2S&nX!Rd+`761NM?>~8qdxBj-{k|L>l6%pieq!6BF&+<%^EH2}*>+OK;yj8M1Zq~5YUA>r&42Mvym*Tq29huH} z86%IGmc&-18(Q4rcCx<7A~h9G3eJ{`ANGtKC9>LlExz;p>~D$`Y&AeXc?8+&PMl^W zYe=s5_u!P~p(b>%#%)%y=OS!XY;7TSEjww!+K;>d3YRCd%N6NrgwE(cRJnQ&PIb@1 z*WRVGaWJZif3d^E9Q6VUV7m-Ure zRE!scu!yjH?mT2cCm4i&1po88f zp%9Es)!4oqZf=f9IX#64pe*%V4YtXB(WTL*h_;;RcUB?-+})OEwa?Uf#^rBtuKgQt z5&cc^e8lG6KE>0SoY^$&v3tWuty(ixWXne?Q1%lytV7gf=3fqO4Sg&hQLx4Shc?1a z_up&!Z07QV1W7&>Sh$fP%=WWrQPjo7b+6jruLN`v4w{egih@aO6GJT{wRWgO_gmO`SD53$Os7 z15;aFn}Z6Ma=};efkm`1`I*8GE$l6>uiLm=&@&_6;!A1+RN5lCW`0tQuj=pGnZ@h~ zyi1h>(frCKYyfOr4(<@kh2+OpQoh@NdNl&YYgs6=aJ#)oMC?P9+shs^e(*$X*0EOf%pL>e-}2}`tRcKDD1lLAB@G# zzp=ZR;ci!~?uBcbG@ z9j-a^a0vIfN4HR-NR40M%#eMn@4Ud4=tyy0&D4el+bT zVf)*D_9NrYr24@VX2HlzzUv0Q(h6R=aHug6qJ8-!vfCD(x%(Qx0ShmwPR>84pJ zNqT%7*=}R^Q_oIrs$g~-oGzU`OLSOgj$}AkgI8g)-wvfOJy3yP>+4I$V31P-Aut!( z+Wi_+!}f6@ws(rBxI*JhY)IJZjpnDAD?$<*zPc-sn%}5o%L^5g80TOr=cMl}OF%Fd z^skYY(E5((5U&N|UbpS`2ZGq%MKxGOZLSQ8`=f(97fnf)8Ph~pgZ)g`K_UGSR*}2A z#WYacbPYij2NRx$*I7f{l%C*yk&V89+Ktm{ph}`iN@aJkp32<#l7M&+JuXvp1Z7y+ zq}8dLRW{1-H&x8jN6YK#M?hZ$S8!$xI46CB(h0$lsdYw4nHHSN$u>U z4+E>SNf_OgVYT!G;D8Vxv-i6mTq;<4SVE0 ztvtk^CEu+PkKf^;hBTh8COD*n#?+vzV^EZ^IZ@c_^)LVOPyg#L|M&Rs5B~AL{pBzJ z`8ofW2mkS3Z~x^#{oD0_c<{gf>%X4=di>+RU9Zo6C(S?qbFqxQy)E@n{?C8PmD?LM z%#<|OJERpJ#DJ|LGt}%zl&$y0`2I4{@axmfcYLl(Fy615ka8H8T*-smD0{t>|- zzn!)^h+J(8rp(e@`@%1QvH8BW6~0b$?j@!j;uRSY&+{^4agPr=ZDG^q;e|J&nFruv zCTI`4lJ9Dz8%3ftmf_t*cpMhNsPcuGEh^HXhS#;Bz3;+E(8#r6CPfW9G&O8@6jwwv z9Bm*eNg5Q$Lfe&L+cl+@fEt(7Tyzc7sm1|Ks!lF4}Nr9 ze3;9lk|&-g!yF14oX4k4&y4ozf|D-4H4Z#_9t9awn8cT zmPN1;B@zW_&wo=}ad$MpNk$c;kJ3N6p9_5eIx3NpjHUC#N_`hlZ93dkJf-YG7uYAK z)0R?|&qAQkASRrX2hcuLki3BW#{B~u$1^9+Jd7+9`FtqV+VL3C`t5cM+E3z6wHi-@ zE=Nd0EaBZeP_li+aF)@bar;E6rHdO;=<5FPA>-3kuEac;n9`w`0Y+R&!F|kaQv6)y zEFD_&E8awMvUk)}vWiGA|=B}cQm;RH%lEAv*7{&cn`fIQo{*mJ+Vx}OVy*ue?HNO zW0!O2ilweKMPfi_#9(<@dF2GfOs1BSvWu;01u!rJ#gay5^DjNYx%AW%)i*C?V~gfu zXw*gb7+ngEV*4!!mO;b2La*c1#+xC>=RSTT9t)X{8F6=Z7oN+;z5=I9+=#d|14~WG z-$P%Lk8Z)q%oY_$Hui=D9E&arNvB8W74%<~9uNoVSv7t;QKG^Uy6!2y&D8g1TP9Wc zgbioZ$9f&Jz|X%72r?}}4sfzG1ztyLDQFQYb$FAJTJwVs69!O1G7;2g=N<{Qeb{Yt zAhPea5j%QFf}Cp=!eEy!0wY>vdX=1o`2Hdqi8PdGpD$EBQEJL*_V^)d+R=Hi%+TUk zBvP3Gqsmd3NTw7V2l~QkY_gP@dzS}aIr^T#v|&w${;+6Bl)c;*!w#~0?cf`Cq@DaZ zP!*l_usc@`;)$VWs+ULx3Jp_(#Syzp4a41YkZy&Z3;Ax-ue27DyfK;+IrCR*`rtro zOlM<#@arVpRufsu6K3JrLfVCeVQ2XwShRdlFIcSc;_2Htpq#qbm^4sZQeV(SldPK7 z)PnR(haIT7#@ik@F?=iw_BbEZqx;HxFa2a5P?31@r5LYx5DfjvgD#zCfCYnR$b0C} z(SkZwCG8C6zB)9bd&Yj+DfuPi{ko%=<*aw!5p9nZ1hZbjRBUwX-`a|L%cgAxLEt_^ z1rNiNb{_O0yI8>IsOX_eYpF%d_vAY`ZabLfBK3;yf;*ZQ#VHaN3H92@gC1Shi1uoK zvkG1GUu5G}?hWcSA&(b4>DCbNyulvfN|u3wQ~8X3*sUvEuei7Lt`CM<64Ar2p;wy_ zS2~_@jIUiRPPzudMQo_#!pW0H^tZGb-u97-Y>^Pd3O;NGc{N4uvW_6~fO_(BZD;Uk zFZKiNtb_JLU9cVL#%qBSy{+>%?N^U4V+DL(A02K6l4(*GDVSK&2Wlif)du;D1wvTK z*)Da@6W$7C5tgJj!NA@QmGBIy!{>WsjY7|1?!~-b!yS?r#5n~kG9BZQ674;{9OIT> zi(xTlW6t)x50SF*v!m2}MYlEi9$vGj*a7<$bdaQi=E>n*(R#8BWK5`3$aYEAj2mAJ z_3VaV3g~7N@b9?C<}97S(#e6ok7M$P>C|xqAQ3s1ftq%t_{pUkUX}8Nxoo9w_evRF zf`c!GaT!|KhMoHWGG&Tt<`y?>!vh}OFww2`d27;$T&x*F|A^+qy_-Q)6Dx_3ODr)t zG9?dCUi&d{G`vj{i^Bd46kkvLiM{D~ihYVaXvAW@)-9Ou$4Hp(;tT_8Y#bHsdP9$| zg#9`u0VZ?->sa{HchaCfGJDWS_5-fIS*;IMJzdunE%w^ty?1EMq7Iji&4tdrPu7b~ zm&73rv($l+=RHU1fx>W+H0XI>1*VbK?)ej};`_=q1F1u20@*vTChW(qicbmpQQL?h zn)GD*%7?&JfpO8CHW!u15&@P&midK3Ie2@x_eHn)CJI9VqEbyUJWlV7g_{*e06VA= zj1-u@w#g>80!5=wy+4*LJ4Q1XrEw$!^xf+;^lKXW4$iMhjPZbtn;PEZA{(>Q(Cp@a zU|8M247^3ur?sTB2&^YePT6ol4x8a5$wfT_Gw5I+c;12Xf{Co!Rw?LYRr`Da)~#P6OHL6Ruv0RTqNx(jcYJg?!3Rw`-$5Uh5YbGk$>)i0YZ51 zL2yb+c}eo?{sZ4XLHWtd4$0{XY1*H#{tX+{*e6}pCw+YWj_Xg*6Ll3Ab$u=7ACUh9 zbgi#2xEw=!tp@Wqlz+$VU&*_{W#)aFhn%vP`AQw=BbjM)NjzLuH~ui82!~bm40@d~ zBB%N@NGsTnq&|Z<3I0_VF2k<)%&7(*e*JaM-{Z-Dj_3BzpR*C4Ime&l$$yTQR$K$` z`bmT)gkZpcagk+fj+xDxp*80FkkvKq0_k=n7x#?s)p=)RG8y%U$+fvmI+g!UTIg0u zr9M<yy`uTArKlR>y?mNmFmd)7S3ybdeR2p3XU#^sd-(2p!+}4-R*Pd6wCmMv! zLFZ%4V`bsg98)dH6db+Jg2HpM-2EX2+an{nV$CmS@6RgurI|Oenzk&A75o^!cMxlc z@>y#De72DZVIn|_eN6$N_&C3JD8|rQ47$6UhevbN+Glg~DUXPkg{l3G z2i)5kFn;&zD4y?xun9;c0|Gx0^YRGuF^;dxIe2Rk7cqvokz`TgHJxPo#emj47JUP$ zK&K|34@^f-?&PP*Ps`!3=-+!~ZgFO!uT($2QTB;n6WFXV+u!AK-7j2D=8bJM5I$=r z-pj`zoWuL@JMK?&>0Fh1(&dS~(Lmv?@fK$klM`#GB^)BsWA za)5S8$q?Blki#aN0+zbaljz82=;3#f>V>eYNuGUE=VPA=Cj&V8J?Q z?&+G3(RpD`WV(Z#By3|vQhvnQ2Kcp(W_*%Hz{DV<$R8aDCCDwtR1kl zhmTvztg3+#*7BLNW0r6KRSnat1y+uc5 zs3Tl7(2Gzty+F-;wDBgTGQA#MeiPOzb@mojE8KYbOr9Ofdsht=@(TNXY3QWh2gOV5 zWnI4l@&E>1fLiO>6gB#W{B z&{^A1tbD~MZnX<^SK7s3Qk7`$qVElWB>o%N5WwEl!WQXBdbtdWj&@VyT&5i?pf)_l zOGNC`#t?Nc&3&Kk*iInc-o1UY52Iq1AR=Ze|YP~I>V_>#Fo2Z z8bbbrEK)6qgapas9+7{4Q>$KS?J_3z?RkCe^d?apb$yaT_=q%}v|5aunZxcT>(n z0ij46{=_Ei>!O8kpoBaw?B@9hqhf^Pl!7E_?Ep)?mtAUHqd<2m?<7E}o&rJ%+SO5n z3aXP6oR2Xl-+X1|o0*f=`IHvb^xc(xIEXl&pYN8X*{u>Db`6(5*v@W;%JGFRQFUcX z21AjygVHim+S|=pa75akFl1v!_%wTrqRXM|)CAr(V0U4&8gSp{aIGsU-z$$~;|j8R z<6R9;bc5yPE%%5A4gDGI4%VDI7L`k@6LoipjN%X=(eyz}j{14P`GH0AUW)V+~~boQ8_%lS+dO0-0l|-ub&6pro4d zPjo17MfgRP&UfLWCmn@@BL+yeNc@+T+#gKw4-Xe?qtY@eFHs?hS13R{Q#?1udLYK< z&7n(`CF%{zrY7M`zyzuWAGOi?VQGYG|33f#0RR7dc}EV!N|sy;kOU28AP71_uOx6m z?*`DKSoFT=$sN7-o(I&Kv=o1LFZ2b>6c8YjRhi7p_wq#qY7PY4Yra94JXG*WE>Epe z1PS*NEcvt=xQC)UgvXSCBSX11f{C^+7>EKY|##wqjhl?2%n`D_*SzyA6^|MIube|_cXQF72@ z`>j;VvJ{b>q6j`_j!Cj@v!U3R)VZD+fg@TN5~4;;oKH-oAKtJd{R2OU!j>*H7w(2YTqCbZiyE*dPFnFKS^gTWXLex#2ViR4+(hL060_%y{^ z=)&I=SP|h%!6R=2cfqQYfh$nW*J~EsD^9?cX}{(7#n1!o^Rq~#8K{%HWJ!n;4wg8p;T>D zUVT!TD**YqO9qEWmC+5;dY_#pfu(~({HBq1zyithrVuy5WrBOB6|NTrYNK_L1`HJf zo^2|(uhnc-R{WxJo*6LbBs9T)f^}o?Gvge@m{15e+R;`%q@R~O%;z?w5s5(D(JgkA zG6tugvCeK{R~Jv_h@`GLb8-IYV|pmrf-+C9RCqfkEwCm0f`7<#F=E`V1^A+2;e8{ z9hAQwu5MTZIng_0($`D{hzw^X=;)Npwd?Ta`_cbEg^JNHfqcQM4QxO<$IM`Y(UUzO zoPJ#QKvPKBfQyDCneVOWH~}D%MO4+D3&qAZToh4sxgs1e)lFwqpj~~_xnuw;zFPbz zr`+VC+$Yr69B81!R`R4zw$AA63&r^&CgY|LCY%Vz2SY1ln>(mb+MK`Ktt_gH>eGFb zHIEvC9Ea?bCRz+C`W}kaMJ~pC9iAE41H{N$v1x*Da^T1aCn1`U03WOBGto`k6^P(c z*blXbsi?EbF+Zr27w(lGVhHk5a7^uy$m-o-g9s2}FRh^K2WRS5nEmD*B8*=3=)0;P zGaoZluE(s_CKjAf6xkOTTGJ`+$pb7wJU|?^lED*H83!^~5-;O#T2~ex9huk!L6)Tj zVI!mF5e{PFFX&S+^Ni-gzgQ9hasZ*? z{+`%gJJuiK?$&U&ojKl`MTVJJA5J$p^3Gh`xFA=&Um=1xy16q#{;DA`$ z=BZz}O0Fsdgy-wPfMH*ro)R(6V;#`vfN7&Y;HIxb$@JVIg+=X8;|5@@-07CRKDCBuBsK5r$moS4F$deDusWaQc_+y$4A$_n5<-HH(>ZM(xa|@pT`ksA`m@2M2^D9vk5q(ZD)Mk z0H{T74It`*>A>uk*2=w?ogFJ|p1sQf1g(lW3jRLO4|tBXp-8rq{fDp)b92KcpTXaW zstCJ39Y7RfoFzB7+(1X%M_rERl;m!BXSjfCKf?K0*F~mg$Bu*@0u<20UoARHQ5e4Ou*Ty#_L1Jt98QxyzdMs=~mUm-x@~&gu zT)c*T%_C1X|tgLvCbWoWEM`G*Z#utcIKcNc< zubse7X!1vrVg^A5MrE!B>KaJR#`l9jIRWxiLgi&u4-3(0E?BhVvyDBo?uCoPF;H*0 z`=vvCdv0}+FhZsfULXpT3JDFt5PGUMN^33Jcr(is^cKyMMyIc~6~~J<4sHVE^%g$| zlb;RsxW?>~GU8+#TWHo7Z2$_LNHfY)#P4erDTu=WnifYMTQ6?!l~ZsGx4<@mY&vdV zz}vH5KpJ*JH7NBGSUrr45@!0xbXY_Ew9>P&SXipfZ*EAnt`{`w@|@igoodWIjyT+d zg&puq>LxN;fi?FmBfRc;*2`R^QG4LKuZfVl zAh4Z3GLc~bahAbCeLoB{1Eic#0nr@?RK%Y~B;Gj0o(ep3mSjbmWTXy??e&1~;UJS+ zaGJ8A&K=sYkcaP~B9nXYWg+}CytGK=s8#5C2X+Hax3HeIaQQ+(l7JEZjD-)LRF?>-ZDm)!w7y{2?j3BJUjH z_q~6@A9F|%Oehs&CIkgTM#)eJp)~aQ%hr1}vQ0c!LhAUf;AQM7a>k9TnKmHEG@JM~Xmc&_GM`tO%| zkoid+mlOHFzk^mm5q4u@qV%~1R-piOg5pSAN)~5**8RC01wwc+{CTePhJ_8Mq^%(dGov7K+JMZ~fcp&xk2|pVEG3 z(tGzDf#jR-pWuyXsJ{c~p&04*JHToE8N_Mr&|A-bj;79Y__u2*_ph~jYK)DnI0z@- z|ADvuC`%o8y8~7firYwffS(+2dVl@H1O4HD#DD(x{m*~@=RfVxJN-R#IGycVWH)zJZK z%CFkwTac#M7*+7qDDwicN+5KOUBIO$@k75Z!UzKJ%MMhs9vL9o6@*N1hwMH_S1fo zi!t_4RCwNFFbiqUq80(UP9Iw|WywHY2MV$Mm4?~O(8MmlJ1gOj8fj2p!RE>vVfrBk zp=dF!%`B0(x2Dh|lPI?=y64fVo&%9mr_Q8+o!gqt+5216DWVL)dg32F3hF&cFsQY= zH}?*@S}0L?jn42q`Z&s%B-P|@;G9kzzIX0hdDM|`qQ1bh8%}(h$-84}2zN3B14RRd zLPF!1{^co?yQo1w?kYoPq$NQ|j{z>OUhMPx_7Wa=h|l#|pyM9ft}=j6LS|eZeKQuf ziqotiK82mBT{Dmhd{A*{{~Ln_#DLEjFY~?0xvd~IwV>2kle$tE9;MBBs&CO+y;g2E z@oImpeeH}dK^KGf%5>PED5i#xh2N|#2+4>_=Y?1A^3V>257Ka0TQ1&nCvv7!4E2q6 z_|4ui3WrDJ+&;)ETK)k1iib0bMGpw=5Gx6=SmSX%t{xYY9$c*V%gAXnPJ+K2J*Q7l zr+)CW1wznjzD=bL;W!Zvv#yDM&XEUF!2bP4U`6LIJ)qv}Z#bLecrM>pbfWO4XkUjJ zNFWBjrv?b>jEL#Mpb4BActdDdU|v`J)hc;&<2P_=7&46!4)i@pQXfzY+~~e!R?v{z zjM=#$5Lp5^`<3oXhec{^{F=dM`6f(`mZ*7$jun>g|Wy~#h!zj@EoLQoz)oX4`Ijc@yCJQh~ zK6n%gG<&Pw@!%iIr`gLaExCDNEn~fcuj%`c)=u2Tfk+0$6H&a<=3IJ^+ z+wJ00LLV3lHL*MrfeO|x-KG<8ag!{9ZR4=^KJ(keHH7z&7!EVI{!K|&ucqtuyxDSZ zhzA1~)D-kF^z3qV4|dGUVr1#6Dx1#e+w$~L zNJW0q@^pY+RM*IuWF`tsy+#uQIaEdJzm6+O7vTJ|E%T;fD|9Svp<0ns55DB10Q~&@ z%LL<$=ND{kSRC?C1e|b$5}G1dy9zqQ7diJY{U|31whUYbl$Wn5wqiF`f}yY{o4*6N zbfgA~Rka0BWcSt!XKl#CH7sW(@*qg&t=B~KcjDM`Gv2rozTWpcZ=o zWJ&BC&c`m<9)KZT6+7XqZLdhMfNj~xB&gpk1>i~DD5Q>)(udGbke+}KL;r^4Ksvl& z5!b>AV8Vrggu2y-UIRi0L&zgFGDt2en1hSR&Gl*WX85?|o)u0WX)&N;{Vfs_DqUS~ zfZQ>eoA4(4b6hKAS{VdeI~)&naDzVy)b%s?%=KSNw;$kgyT1U(f^g5R4<;gmNYR5mpivyY2uc$k_&3qeSoP|PSO z;?)V#A`S~Mi6GKbI^XUC#R}r7B2UB;DTa3dfTKLign&~uHp&{KgIYvKg zy-Q}~wO~lTwchuy;m01$r#Zr3M1PiR;xfG0^XhT?`gpxLz7AT{sfUwqclJy@sqln> zYQecBB{?`csySyy_A+J!Qm|!t)0kzA&O64c6??O$j9162(HgSEG%_%7SbXX9>)}zY z6fFL;Kzp=bf;>JQl9ynablUnMB}!G#-v8(~`h@`g9#>i(6DW^TrYV?yvmJ{P(m>q@ z%-SB4Kt?-XAhacg6$=IesCIP$^oe^GLcsa~6UO;{3|90y+0=99gLEGAWfBmOF>*+t*|AwB$7mcS+oi9a}=>>Yg#@b)or4UwP+#(D*BrXU&SeT+oG7a5nf7ynFjt`W+|+ zAV!@ac#&e9ug>MlfwAsXM}eP%pdTUEzQIswsS-e;258U$X^@O7OT)w@8Bl?1^hrh( z=(O9x_`HGZ;u|lu;3Q&6Kj4R&k>Me-hHjAyJCgXJi&u=%=)Pp0j*#i=KUyKCXf8%f zvhJTW7%)!jq zrB#Y|#z5jE6XA;2{f-5*NWf)a9`vOeRkyPKF8b-H3MWnn!fdkj!_M}$>MJk6@lT=F zYwI&)&zvqlQye~@v`r6Z;sqa@yO>qq%tB?g_&O+|iSqw8mpeYk}&{P5k67 zb=?U{aA0MD1|g_I){NPB7vB_GUymWvRSFgs5_?_MNs9huD6bX+Bkkfvq3gP9SZdGS zMMH=&!M=ktCZD8-eKgfSrW-_rV!@}{Hk-OHS} zpi#_yBe!@&$1fy9I-3UhxZnIRST6)3^W|NLUZMr@xvUiS0pb^br@$DWb7(69y|4li z{DHn?i#~BHx?8jreHMyv3-Pe&XMd3voeF#CWct(ZD^4LIc-uHicO0c$T+o(>9cLmM zrX?toU*?Rr*6DR|5%W<+%zY-f;xiZ$*AW6~ziaZYYouhNDIKg=)z@vK^J1hp*{nk9 zQodw1UgSOfHPS=#rtco%UA9CR;PtCn|1Nj4hlcZv@JXbnG%hd->jq&0G}@4?3!`UE zd$>U`K5mM9YEStYleR1{0jGrF;@xU)QC?SaJ7SYT#0il{(umbCu^Pn17;-R*2IC5tRJ2z~UIo6}3g@n&<#RX?Upw%<7bOefZAL z11~wLIjGStS9V>75` zk!v0T63rHiJup0fkXcw{#d$j~^F@!ui`#V{FQ?M`%qUJ z7fmQEAM}WahHym#%*%`VXmT|4-sk@U00960#CS&z#B6rl7_bcih76xUD;r!eCjnY? zGv^$4%$d!c7CwiM;k8fYBbZ~!qkjZQTeI;{s0N89-uFUP1B|Jcc~7FVG4NNF9!wr# zQ?86O{G|-c%K7lnVViW3tr8rRDkJ;kEBA;Cwoo8b`W6xlJs}rB>PbIReD?_r!k1l0 zxmj(G4MJGI(41V56v_i_2hAS5BVycdwCXv=&S~SLNgr`pAhN0}>#1dUrn%}aGT>*? zd&YlbjtURsBPEVInJp#)RJe-XuPIqKp8qOG<*E|K!jI4f@zJJ!_9c<{|-Xk;E1PZ?H&5-pe~brbBYq9Lx%#R}~$ER}F7 zYur+vW{1z!n%it6XU#i!Sfg?$356bFOVnPL^bxTIVzedvBX_?` zhbbS@m>f54027c0BgZC|5Zi6$i1K17K6X|W1|N!my!F9hgF|Fx<00hB?C)(_Q%u_o ziCln$e){5oF@tf92DI2HQhKL{ljEY#N%Pw$m2rs*3>BuOrK1DIxs;@#j!nf^-4xP5 zXPm-njtzjTW#DXo!PuB5*O&)MT~Sl${jeknj>X-6kA)AYz&Do)yS=MzNKJKPgNw7j zpPX>X{6IeDM*-*?vAVUnu-XWsWJ$upiaU~P5HlB_hz=?3K{&`IPiXz34lYWRkM(Jj z^F%dzWjB#RYsZGU^B9N;txT!ckJ@?3TvwiY#I3>fR7aQg6WGjB}51UWUDpqt9t%P7{85EXb5rsfZ;0>Xcr! zDRCNTZJkhj!q8WSC2Ug|DJQfK49FwU0d1ffBBPgbA)_&q%OU$CfaAw-yXGvUIU4!+ zkylCT>CbUTrG-KUqYe?Q#Y&#jLRFzfQFXXZ8LM0v3bxX}2w;mV%*x{H;?Yi|rUs*H zH(B;T#z0B;lu8Bg-or=v)>mel#`kwG*&y@L4rqr9Tev3_;uQl{5S#TSltvF5*h1Yy z3?L$KR5X=GDsa6ckMMh@oJ&{4(Qf3zteG}DnYuBfWvDc6asoR6SoO_TRUZAJM1N2v zVtwf-L#1ue2~i+FA(S~P8SNa*ZWtDWF~>r+D}oIo0NFR+F-^zIfIPI_Y?wZc8q<$& z#C#%?Ny&mAhOGZ0jA^q4$Ew~arzIOR6MID4hV5S%cJ%;zz%E@1q!6nj8Yq=Te0K#1 zHWkn7`lYx0Ynd~msc~v)a9;^%mU`-pZ6!b#c7WwWdkiN1f!=dAFl;tDfIp-1#c7Rf zI)7ky;Mk`&)rW{G8rb=WcnIdf2nA67ly#hOuTRKy5xO`^wq&h}4?mOh*G+gb z44yC%v*_~3Vy%x3IFeK#JaiFA!qa0(*=sO)NJ$Ab#EI| zo`oKv;E1LtP>Z3l7VpmzkaU7>eUwV-1*(BN;nBu4r(YHjVC}aA0c=$O3iuV_2T3bs zxX8zx^_j|$afUE>eKUXYy-DWLSO-@Yi$XtD5s9H0(jIIgy0jXhBmZWYeutdyi z!nsSQPna^DI7%;^9qv!<6!?v>^>d=_dK6R)LB}G{A2yU;3Pq8+_938T%R0JYid zj2EH#&e&!bE&RcyHg!a)y!LO6gZTYvv1(+(B%ccwHvX)u$o+jAy?N)8_~rE8BX$Q> z5O>E-nQCS7&DTO?S$-!x;GRfKk$PQHT3&sC$STp*0R!tU?CGM-PRYsyOb}`7)1Qlv zIIsHsW@>^)n{`Cl@tfjgHrJvg{n{vOv`fhk`jN$Q_n@(8D0nG_%th;YI)+)skBDo5 zF!~}Jkv0c`{$jw#E?qyyuDQxLTSum=0S>4jU}1SHD(Q&p?g@GdxQhr=kiER`JBH;_ z_?X;=I-MMX6tR&P%!zO^zy}grLwZi=U6P}Vp>3Pu%g4EqOt9{;o&I? zFL9uQrwF)BbNpGm{8_jBkH7u5bxZlTf2~{o%>NbLl8+u>ikj!2;)cfk80c#(OP{#; zN)E^1#RS&01<6y4z%`~tQol~$N}I?jbdva;)F^#@jy-0wPL@<2SC3BIJpnGI?^#)j zlNk;>b|)i@4u^-iL%X5x=Vq$WBsHcI`Hmy0BhX>&5EeV?T5F!4QK3L62+X)s(qqQ)j=Xgi|wIdbPUiz5&xt1m%< zj+LN+NAyb=b1Meohy&H7B0yTo(rSIv&qwiIIdu|yTZyTUn4}$k@dY3T+|(z)AQ7;9 zWz^I#E$K%beK;<0i2XZQ@GK#~5-9i0vU7bO?JJhzGA6VL`_&E|AOu}bWApdNnrco;CNdbQs%8_Xttmx2|5PQ3XtL`PrdIJDA93IgE`vfK4@HSzS_X6d0`QAbA91J5xV}K!N;>o2y`eN0 zo6a19lw>o+!l5p?p^o4#TZ}oqggavu&^Uj*4HaqJj8Qk-Z3F!Rr4S8=noQiVkrBh| zr`?azpVMQFK@Fo7x*%y1lG(N!nuk1lI(i5@R+Wyx7QEw;Qjn$X8Ky=y!A!b5B>Q~V zpbr5ey|HW?RG@BK0}OMjs1OJ<(FC0Uy!lAb-L*M_(txQxhpz_DistCFktuwEr2THU zCmPdcq{h-fS?U-ppDT~!qI(z2pVED|lw=&l53o>URY;nuj>QoSa!04?aH&O{qM7!y z_uS_k`2yVxlreN@PPPFpwwA+SBJLumc%C0ofa8WN_j=YJ%6&6F8h&$3XoSnkwFMIC7e97!b5}jcq8xPA(te z4OD2sl1`g-3P+jjScO#GdCdDDTLXj{TSIQ>%~e9|odShAT|8~0f?Ysq=iB+i-pdM@ z)+8WV;M4lmZauVKRjKr(f^~P*1pI2M>Xs>QQSjPGQCmFgztqQ&{8rL;V=L>n9)%6QQY+HHrcI)51Y^$+x}$wHG#lE`?Huj zIp!;L-lG;z55$)WiFc@iEuqR$Z)3$bSs&LAw)50U0fraT0n?`BMu>;*q8*l?y^<`) z?WJMiV3Df>8yx4gO4yB)Drf+tdf&a-qrH#kUfG7)F;dx0*WS|^Y*}AjnTtH)tVviB z)&3IJ&3)k2DdGGDaxfI0Y$MkNZ&ag911A!~S@fjH;nup-whv>Vq9mwOEq+1PW(4^p6=6oeA zSZQ<<3Vj=RJ6gO7D4Yb6u&?Ny)N%+j z(_xY58fw!{QJM;8BIUJ{(#l05_2jiUNF+jI=v~GD#BTU#7f7ZU z`4tOqV-+@j#rH;l8L1a>=ifEh)E7!jDPI&=0gANreJxHFInoDe^=xrDSR!C$fL?;ByUn!)sa#9&8vSV_^r z_KU}Yw$KoOb8?IiA({Yj%g7&YgN97RX}5>q^aK*5 zIbxH*0XHki)Z?5H4fl9EC85#~E0|Ik>@d1L#fYw`yugYG2~gl~2(S+xK7QiT67WpY zz|tYWjGOeBmvY{h*Wo`|F?I`3A_BHu3v*bbc0_Od5v@P)ytBxvJL%Jji&NE1PaMYT z`?wf`zm_rMUS;ig21cIW2ZhAps7$T2iWoStlfK}JY>y8r7S>)+P=y}12DkeVUL4u? zWTl@E5VhF~MxQ+cg)fGaat&C;9hF^qXkmrEpK?IEH~xs#ANW8?^!4Z8!tQ^8=U+IC zv%6S)_pq?v_#=M*nP=&_ycShO-uNRzf8eOg8Tb|%giW1*%)S$^eS`f&wAn>lpb9f0 z1Bjhyz_JNOt-nxq3`ibs;f#+X19J4$7){pzvkNa2n{b&BxRSSLb39B*) z`0P$8p7+=w0vGIyL(1z%qeU%S76XGXzT!~Q>i_P_trzwOWd z*MItJfA(koul=taK>UGnmX|M53eEPWD7BvmJ^#y?p2y{tBRb#Dq(QNH*(1HL?KP*N z9>o%fA-c7N-miP@YyB>MCh$}4Gh6Jn4Sa%*fs|TiM7D*z&%pWY)usO3ki6$JA~Z971e8}ekL zA7+&K%eF(O@2$P^wDn?g3M9Pp5ms0ODxWT#0)WsqZGUr{aw zB6cAsZTjtbpXT9eWQZ=m-;McWvCwPRRroPRyt3q^_}z<4 zp25|psvZ_9Ywe2_X3CSZ-(HGc<+0gB5sV6zS|CUHnOCUL%DVmLx_s!xw8rDvpv$Qv zPl&gHsYck-tIWz*mOd`;tJz zeVM?aHRpuMXy##Czv@0U#i@&nPccE@D1aK%V@DTG7g(f;(jz8CeH}Ddxa7%)HXlzY zKn8$ORR?x)lzHVmX@7-ftPqdLzEp!?u$^vVQfA7%V7Y{>GJ);v0o?4vMe6%R|MBam zDy(hQEK(hG6-iR64bHc9VCGsAx=8Y;1rm@A-&N-omK3SPVB)kfno%lI=LEl$tsnI(W{@98dHoHENPVdIj}+t++`^%Q@YI*KiVw5zckaa(vcMd^&6 z_e8-Huy|f9)fre-fw*a1F5F_1Pe`#i1)-FzE{!(E{EfZE)dFDzr@S4kWF$qBw#84YN>o=kIQ=EjxzyF$LMVBvxSUd=x8MDooGb$T{T~1T0RR7dn8^;a zz?z;G44vT9wg0YZGETCG3BTs5Gj$Ah}gVW z*jTl)aXTehRm0`8Kj1x?EwWXYn*K}$>UlntglOJ8?}wY4JSQ(S0fSrq?s=(fMiN)8 zrp-f3J#(RZpfR_nQM$cm4^qgkozCK3-5d}nAzo;AeoXxl&48MkIiCwT=;*8uS?JCC zf?GTxYEz1<>81tE+j{Cto9k3xd{+T~yTJxsHt0Gx#lVPKeDf2o6w{)3_bm zrr0>`%VDQD{gZgB#6X#^3x_jCzb{&(6coydH!bGGXNO4Q9WA;SV$f&{w+DCy4kGGf zNX~7lNAuP5(C6fssHXY@d`KCRzydxu-a1O3;Jru=mxkp2%3{1jiO^#6FyLUF);fto zNGtCR&(Y4lK%FUDWHr{<9WeZ!Y9b^(91X1Wx?W2UES_QC z5o5sVgSa(g&LAt23Vn_0+|w(p_~qCLsHa`qtrVg+IQdP|yR9jKMkIRWxdEj2>@BcI zk0-b3aH20vD39z|KW12ybl5s_PS#0f-(pbl{p9BeL@5UexS7i`|F!!a-65Ru6#h%JO* z*hOTy<0agxDcymFkzdB=&Z^0kDs;(?QD66!+L}}4;RKMJJ%aQJ(*g%N>;(4wi7=P8 zpK5jJl&XMENHADxPCC#tQs_0i1NioDZ65H9nCkTM7lL~5M|Z2Mf5~SJ*+D6bp8Cn3 zt?u(Te}=*-aL=yhIhA`VKM{N9AHmhA#~n8@a>pw7R=V<6esY@I<*VINVOCBVAs-Z< zv*Je~Xzusi0dN)`7dUzg@B0Ua>_WIr6vw|0>)?&y+Km^1Y}L>142u^vkNt`6ZneDk zfL1~CAkHn}Py6o2HzQ&SxJV&)j190+R1qy4CUx-bY1s#S<#Z>I2ji+NLg*HPk{RQx zEPcl(mP>gvvKGfY5Y5baanB#f@&D)K!*R#qNC-_))Ig^^V4MT2VL*&`ta*R>(a1l2 z(%ze4><{xjc3r-loj)=7;<@8;B$VAnBEWQAoGpN|pU3PRsI^=7U;6NKaIkmW|CIKD z^cVHTYX?}Y%I*x#uAR6-Vqi1E{g;3GpMUw=@!#(9zyIMcfBCQX`LB2R&;NS)FaPD= z>i_93|LY(A_5A1f@Bdbx&$dshfBt7U^{u^3W%v6Z|CFm&_N=k(iFzmyWLrHY3408N z?fS)jtH}GMitn>mDAIhbl7#j$uU>kTult5ilgWtPlaeLcBlUN~3JBSAG%0r^AE2*K zLsA^mo8M4CK}vKGsOa@@!cB6pt~$ zv8c%&?zD&2Gk+L?z$q_QZ#WYT3>)@15yg#!40T23nIh!r8`(>ZntQOfv141yJem&n@5`u0E?@^Dt^#HhtO}@k(6{RsYARmZUQxgTv#+ zw4Otiev2yTS1ZhTJ8wA#?cC{K+7E_;nd?y(NNC#7LqC@uoM#DlBe}q&mz|A`43Qrv zLRl0O&X27V(;rle%O5U7ERz^-P`8tiLm-4l(2|8I$b85hsrSGPmg!(ta^-r78ft=AFYjwXi9|xH*+T9}G*2Hq=-c{(T6cnAl6e`~@m6CE?$4NMnt3nA@ z>u_4CsbSOGFXYLhdmn8|uj@@9Xr%K)6@`d#OW!UcNxnT(@m!CAFEx(OB$)e`Bd=>f zRa=pizQL~E6Wg{)=3s~zhnYyfS;>A00ZX(k(U%a~l%wY%{TYT#)oR~$GiYX%- z>)6jIgt}5)`9oIRPMxy!RgU5mVlypWL8)WOm5?e>u&at6(H<}bbjnDX?Br8D7_^yt zgH2mcc}eMP(A!#{&VWWXR4`*?QJH4VxHSP zREeus$39d;n|hwZvZj4O4hi8aK(w-Y&}!)J5Ofkb&+%unlx1$s_cezj;VbY^W~egw zfyfA!Y1?=J4vELb<{NfGrFbh!nhEA_M|f&9(2dN9Psa+kp+kOXUI(rARj4&gJg7O( zfI*ON3d^gqoyQx@4sUguyP&_$+rQ&`3qluO?!;O-oeMe;Zx*WEOTmgYvVSC%aa7AM zsIr#5hxu{yP7YJzP`|WzE10pW&6bt6%`C^_p1_|NJ9cAUANj8O`_==yOXdm729Ek- zg_wx)r?%O2K>|9vlMG8-j7dzQ)NamuT4LE~yc=}@n(D_pJwz`hz91o9 zlblb1)OITfiscuu0(37-+$2|mCx}06+jBYTtv4(kFE-n9&Ygc_gjyMsGL$|wM|2DD zGft{Z5WDhOC~2`vY-%#x3;y_wiu)&Y>Cyp28hCpbDt4zA;6N0oR4r9?6|-3h=68H3 z#Bn8XiRG&5{?Rq_cfb=Jup{#lslrrJti=(f7@l26e_k~UYO};&2ETX|BEq}#W-xxD zWr}fjDSB?q>zg9F5yGq51NL`6hRtP%R-RJT$?Ek zvO$u;u=Ujyf^4O$H-*!~M+J7Tis1ZS5XE`wd_Djw?jQe61#$F?3<}=Q@cYCTh{+-m z4~j3Y|Jchp5jQWk(mOgst;@bItcNuLsfnKz${@u zL^!Q8#yn()K{uM3pwnBy$zT7Tv;Nfo9nSKe_40?aNWhmv zeejRC@M5=-ZwL9(Wc^vlkuS2?qrN%oXo44u z1oiE#1S6XspZp{y|aVF}~GEiWopCf-3~Y zySF4eiB|=Dc;|p!-uC7&zAMDHhda4-dT6^|W62KEa$ZoEITjV{>fqpdUcZTGMptxkgs=OmZWBl(&+5UV z6_a@FM@ajt_a1Vp#kpuGMj15M#3o`R9An;QILaCfpAPlfQ18QCUgK}3w8b)CK>Xs% zvpS^|Y{mXs9y&6q_v)t-osBBGg@Js$Mo;>Y`3KT{oX~$Wk(mtIS zeCpuyINq-OVa`a<=-e_4?m7FZ>C%WlU)w1$_NshQ&OjSgPaD@yw+Hh&ZfYw7`2}vL z7r~uV*P-$bsX&O<7sZ7HLWJL&C$m}odr*vfPz#W<2J0H22>9i3-P)8DZR>~TUByza1!8ZaZb-A~p%YD&161>g^ zW4vVQrR-ZI@5uJJ$5u%6F0UUkQWg95_*krBbcNQU*#HQZmxjbP*vWj)0LqkYshCek zhubAD=I*5!9tAbokEAd3ED(OVN9eguxSJW&3f)cJ7Zti6tf{;mGG)q>uvDxYEk6dC z`i|DS*(cdJ&;{N-<)&l49wSGk_#md0g^RMl;JF>#{GKfr-Y-l&4!0dMtZnrSxU7k?^uej#aJrt+!x+@1ox~TcXnJ4qw8{%_i_d z6`c6$SsX|5CXQF-u$vGVU0*P%N34Vyh9GfJK5_lB(K-nIWYGH?`LxLEOleFeC%2+-Q zhgB%L{rjEJep?Oy3b%y;KcnQ{trNtUVBA!j0Pr-Qp|BYrd>;<&HLur5jk(p_=oeQ< z%jwin4;uDhd~Qqqh4zuJSD7x?bwY{8*25DopDK~y$xFGos}|WKOYZ23j!P_+IqRwz z1HA}Fn7E>^&lAFQ5cQzlSuXKZ4x~ z^fJo9CpMo;)0*|(?>EGhLE7CE;!BR9&T zs3_*^kH>f2keNy)k$d5rmYs9VH54#4yy7NzDPOLf)L2uJG9}my;yX>p@GI4Xgy9#J zS0_J#Ffl0mlaYKYB8u7+R{%fU?sFyViAAQrC{nhTLW`?6*HXt>ME)61XC-a{doh)n z$$KRnhq5a~_}z23GMkF=yW3oztZ#H>h0hv_I;^P0sf?t_fucyXB{i$vlGvrO5G()g}3!tBXeN?0b%p14`heUlz@o zVRR7A&~+T49hx5OEsbaL@BvsA2N+3Rj#=U@KrJJA3A$N!j5s(<`@KKWDs_r3#l zDQ@9x=b9|K#I`1Z7$B09n#GbM)XsbqY!<7pFqATRS?Cht5Hc8n#-!cvl)?|5VI9VzC$4t=Z0dD z#6TE4dPSgUS}CN-Ak7SYjP zB@^w8fP{Q?J8dOB#r6idi2&b$6AcHnsCC3>sF;bUzNe1eR*LNB4P_OV=h9wmA47Hf6IW9i?Gq_zkmP1u!4TVC-@cf2K z7(RS_xDn1fIx*f*g};sD96TZftJgtH7`A#x@H)p_&IvE*C2WRZdXEwFV=eKF6xTX% zLOUx|T6HryIQ-)2#wcyr?o9vT)xRiJD4e%e;8zH%UZJ zv74Wz{aOS2RE;woEXvGKLZYjx>sClD1MG8ztxN4*U$>@3ACl7g8O>!dJ>f$&Bex4= zs2A%+Vye8RDEobZsqXCd`0-qcPPdAF6B)NQ_>*di z0B)|t#qL2EIJbCZF26YK87$Z#qxb;g=V?zA)1K!(oCIT)2jYi&xX#ViU^qybRf7`U z?)mD#V_3S_!EQ1tRmx{e1c4IT6IaoI`a{^lvK6NOYj;RSEc?E=H-?Y_4>12+UIaIx zc*TY~v1`%%)%L*_Cnpq1Fp*x;Rc&32<^`!?8M%)@A_Xf1-uw~yJF#T}0>kbMD9 z58F(8bM9f@P`Re_s@wum&C`tf4rO(C8r9EzHz$LM&?SXw00F&b)3Vio&XXe#R++Xb zYn${8+0r+DIOlTjAb5>P`9xZD6H*Enll;#R2+CDDe6JS$~| zFQdA^6LBGv)h!m38nHk_-DL(01@Yp#!5NU{sm+)X$U%pss!YB0JlH$mjsmM+QeZXG z(g4>u0+^;Q1=zE7z3E9P0GP84VeEIpf&_2PxzK6lP(%=c*t}QvL@dp+5qjD-9Z8g= zzJP_s&daqu0Rl3hQt3D z+k27#<9eckDvgUA)_lzqzVq)fJW&_VJ?V&$jRrw1n_2xmJMLLki+3Wg`)w#TX&5FT zYp6exCmJls`;92{P<9rL?hb*B&um_fta#Iexdx~=ohY&1nZas`U)41)@Fj4UU^$S{ z(sjiybxTKSVYgrrjAkp=IO+^EjEV?_$?@H@F#Q1a_bU5vkF;RzHZk#-){XLbCceeo zzNgJk9SyaV0yfAv)SA0|KKzBu>YPi70BJI2i`cmA$Q--YsZm(6bu;Q#Sn&p8FUQec zc_tej2iqrskd^>FEH32g8ZgpIvy29`o(fE`B>j(rV90WTFdD@hPqEm%{|5j7|Nq5T z*{-BYcCCBWZP~JA`4g;ua*Kz6WNxV+m=Z`xrX(5iEg1rmd7i$&pW?6cH#kpK9l3ne zl3y%2q@}eY*gJMW>;QJ63qBf3%d-1-olTkop*41TwYWo{_M&CRiQ{*HHus{N_Tbs| zs!5L-**tj6SDe`wnPn@BMrDd_)EnD|Q8qWO#>I_~cvd0Y0=!UAEhbICjwHR&P~S=o z95fipdLKwY)yion;07|PVWjnxdd1lT4Fd)$q?Qt{N}!<7!#jxWLE1DGTtNSpC%higdIv9mRM0v6!vKVEaYlAAaHJa~h z$=ilrjSVmLX5kbIa_JMvn2qhWCN4L?#Um&=T(?NU*blZ9$qRjPkXt(0P_-UcYgh38 zed}?Dpjf7iR{e?K3xC)1q9*%0-Vf|w_*09il5%PE9rhRg)K9y>jY9kh{R{ulNof)j zA&}oee&L@wLSKlIDJHgC91^sW=^++p!a+(M#6W1OcxhE z4xk~U+)!+wK#xNDhXZpsGf*141*K;O!LTaDJi;Xw9E-Gpv}mZz<~X1|Zo+Ct2hxjj zxbb%+un{w*B$jlyV;%}T)0^)ozwoD)M~xMj{X1S?_*0)YaaR0KxL^2(Zok z`xpMyM=GW39R9@P3;)p6nBz*y&nY9sEPEygLO&EVsNpzYtt1Ie>YY~`E;H|S$NP7@ zzVN3$IpQq1BF=WgwtLUpIgwH;PVYe91tQrly_! z^{;>W^SAHqpWpb`fBN?A@810PZ~WoM@V7ty?Em)0fBxeS{%HU5vp?>28x?=~3mn?| zHVv;P`?o)(m&5ZJY?0CX9cS#zGf^#}Zau&198D3sJa#TB?Z0k~aFaR-^_<16iHl7|ZCjJJ26`!sdaIX&dGxw= z+9Mp+FvjTxCUruZn=UW**waw72=ie-?ZA=k$o7KsOia)T&fY}2X<4Vwz z1R(i%xpF?Fl0Yw&9qmwo*wf=b7|wv4vNZ(r3GQY?QKQ^=O%A zq*Bx5J`5Q$KbUNzvFAmp4Ie18rU>I1->2oiVdraMWq?cNZ4UqlE9J5XX)YEz z3a(4M+n3mhXD8lP#IFI*UZWndj(pC=F1*C`lfGRPJR@RHPw`RlqR0~vT-}x8CYt-Gh`gYUEVm}w{KuMCXS|{F@ z>@2>Rh7c1)8p?ahzlN&KvQwmlERk5}GI2AJ5;U#QIHZrv>*|9m`6?O^8tCu)Pn=c-P znP)Iov!f={%I7vxi!i@C5pyfqj!d?xI2$ZapCgUJY|H5B3zEEQ+O{io?Ir~FJL}Mu z5g}ALAVg9t9e%E;0JB|(JQEK@pXNR@?FI{xsvMi)AzHqVxT*mUIdaWQI}rFrN^-A{i&OM{eAEdy0UF-U>B?fetp+;R(j1rFd*od?F)MJ>s^Wj)JM!MsZ6K7u&3CE%ozCUiuad_ zk_NFZ8D8%mRj)U?@`{rC;_>)+ez0aeHxat1mJ^^$Ok=CSinJ6|&ve%kE6w(e73Z65 zzD`a?w16wvt*7x4AqKr-_H+{*obIRIEw2y!7OB_xkrNYEF$;Xb$DSl{aqNX$yOKT7 zT0B$DO3+d=jRZQZ(GX~`>WYa8mCPZXDk?{-O(OKro+y0( zI>-zToCKa;wSv&p6W?C}cY0;yu9^AG1uA0QcFah+G(Rsns8e#eShqnpi)<|CX1$@; z3)$=f#X@bAO$`2O)C!9x|RD#d9Ok0fsendf0cM0oOp1+C6}?*42TA zV5cClgIP81ZBLa!GJqk!o@i$lMMN3cT#BEl)>CGvoIPP;!KBpJR`FY3E28#x+AV#| z(9}Ej_ZD9C+@bxHm`m{$HD@yMDFfSJ9aj&t%vW?6MU5i$r{mAX1B}LjD5wgo(^k96 zazkiV#%?C%vXuG=G(kIwRi7-Zc|CePPS4lZi+P6R5ylUh(IFhMRtNpPem;RT!}I1P zBr!}#&_FNk2=%1Q)fJx-0Gu05mvK2v@x;{{%T`6V`HV%#bBM4T;bH;fxtOtV02+GX#JxX&9a*z54A&L zTkcPXN#l3cElWZ~mXRedJ8DpDmL@)$BInrQH`^s)i=7aQRvipOs;6c)m_82AVmG=S zxK&Y6FINR@59V%-swfy+9i_kA!*W=5wY!hO_n!F*1O2QIAQ%jjK^}es<4nPgv}k z`;wdT09+Zq(#JauxJOgUshDEWB5w;>E_oIQ6)~qrmP639ym<#^d{+tUrBl`AmPQhb zq8C~zi;_(#AU|cM%4zVp2gpn^f^vEnI<0#G+UX3an+*>3iOA_0Sl}jAtZOfXb!Q%j zg>!J*0v?9E!QDujbu6|auOWLv`Hw6;9oK1+;)bpb@`_$a-Nlie7?^Ie@X{!TF4h*0Y#MmUag&ld2|=K6tp*C1H!fc2O;jHeBg z5bqZ8WLK$$1Wt388|W9J2^_?s8QcSF^ASS>wTn}^}sJq2d1vpG8ozNK4je;PJU@`2w zn0(LSPWv%;g9wulrQ3iO3X|9nbj$H64$2YIl>**hD>iBw1UEn2q?Du>i#>ACz=h3m zoCgo5!?^`&XV@T_^nZrB3+$=h)}?hduL){qKLHuK4yp=J~__ zx&Oca;{OkS!$0`N{`a5#Z@;P6Pk-fqiT`W7kNAK5+5h0HVBS9G`Dp(I6FH%gwM7t` 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-2cQt)+j_B1omQE)CwO)N=GQOM7; zQV1^1Q}9i!R8TNfFf_3;Hn%b~R4_6yG*ciJXfrZE0HdcbKLZ1UF9QQZ#hk~<2?-et zNoESq7?K>>I1*Yu6@Th(7V-{KmXM5)sG0k>SyFO_WS4faHw_QMQ2j5%09hWZgORFZf@lbIQI%;8`InfibU<` zDHDC;&aPg0{qo(#f#G|N84mo_WO&MS-g_TIe??g4q9-4|6y9GHmg#UJWA%Bjy1H%a z*UPTF|Ix7SQ-NEEWykwjyX`hN+pS++^XPZorwO~S>U$s3`1z(r zzYl!>?#|KeKhJkAzyJ5^@;_0ct3y@1Uy0gJ`x7+x{e$ukB2LN9oct%wb%6ZbNwE@&aSsR`YcH<_2BGc|Hz|!&zdH# zU`bXLNEhfo!uTvmGBPJoBuBeb=b#}AbE?hvL=NTaZNB`Mr*P=5XYciNS$HG2noi$kQ1Iin`eZWVNGeqh$p`Si2vjEk}Q3$J`@5o6iCF854N@R~iUJf8%0{+O*P zzdP@y_^!F%=H^9ZcT8+xn&w`(h~4TsU*WwcdOSj9$7=<Ajd=DQ{@eJf{ZG_mwWLRn*IZqEs3q0) z*0%Te_S@J0u0QYoZplJJhbHN^;`k-$e;MxIirY~VVY}wE)?>fLBHhZ~ACJB43AyM| z60z-V$EJJVD$RDynpLyy!J~Z%ReOD-Ws7+s+D*Z-qynB}f 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;