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 fd834d9025..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/Airspeed.mat and /dev/null differ
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 bf456947ab..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/Drag.mat and /dev/null differ
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 c302dd2ae0..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/Magnetometer.mat and /dev/null differ
diff --git a/EKF/matlab/scripts/Inertial Nav EKF/NormQuat.m b/EKF/matlab/scripts/Inertial Nav EKF/NormQuat.m
deleted file mode 100644
index 7d1913efda..0000000000
--- a/EKF/matlab/scripts/Inertial Nav EKF/NormQuat.m	
+++ /dev/null
@@ -1,5 +0,0 @@
-% normalise the quaternion
-function quaternion = normQuat(quaternion)
-
-quatMag = sqrt(quaternion(1)^2 + quaternion(2)^2 + quaternion(3)^2 + quaternion(4)^2);
-quaternion(1:4) = quaternion / quatMag;
diff --git a/EKF/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m b/EKF/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m
deleted file mode 100644
index 16d4c353ca..0000000000
--- a/EKF/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m	
+++ /dev/null
@@ -1,29 +0,0 @@
-function [SymExpOut,SubExpArray] = OptimiseAlgebra(SymExpIn,SubExpName)
-
-% Loop through symbolic expression, identifying repeated expressions and
-% bringing them out as shared expression or sub expressions
-% do this until no further repeated expressions found
-% This can significantly reduce computations
-
-syms SubExpIn SubExpArray ;
-
-SubExpArray(1,1) = 'invalid';
-index = 0;
-f_complete = 0;
-while f_complete==0
-    index = index + 1;
-    SubExpIn = [SubExpName,'(',num2str(index),')'];
-    SubExpInStore{index} = SubExpIn;
-    [SymExpOut,SubExpOut]=subexpr(SymExpIn,SubExpIn);
-        for k = 1:index
-            if SubExpOut == SubExpInStore{k}
-                f_complete = 1;
-            end
-        end
-    if f_complete || index > 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 f6d8de8d86..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/Sideslip.mat and /dev/null differ
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 c87c9e4772..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat and /dev/null differ
diff --git a/EKF/matlab/scripts/Inertial Nav EKF/StatePrediction.mat b/EKF/matlab/scripts/Inertial Nav EKF/StatePrediction.mat
deleted file mode 100644
index abf15241a0..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/StatePrediction.mat and /dev/null differ
diff --git a/EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat b/EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat
deleted file mode 100644
index 17edcd19d9..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat and /dev/null differ
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 28375e5022..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/temp1.mat and /dev/null differ
diff --git a/EKF/matlab/scripts/Inertial Nav EKF/temp2.mat b/EKF/matlab/scripts/Inertial Nav EKF/temp2.mat
deleted file mode 100644
index 698457b1fd..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/temp2.mat and /dev/null differ
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 24e0cc9cf4..0000000000
Binary files a/EKF/matlab/scripts/Inertial Nav EKF/temp3.mat and /dev/null differ
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;