diff --git a/matlab/scripts/Inertial Nav EKF/Airspeed.mat b/matlab/scripts/Inertial Nav EKF/Airspeed.mat index 9dc46ab0fa..fd834d9025 100644 Binary files a/matlab/scripts/Inertial Nav EKF/Airspeed.mat and b/matlab/scripts/Inertial Nav EKF/Airspeed.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/C_code24.txt b/matlab/scripts/Inertial Nav EKF/C_code24.txt index 6572c81d01..7c6f9d1b5f 100644 --- a/matlab/scripts/Inertial Nav EKF/C_code24.txt +++ b/matlab/scripts/Inertial Nav EKF/C_code24.txt @@ -1,217 +1,205 @@ -float SF[25][1]; -SF[0] = daz_b/2 + dazNoise/2 - (daz*daz_s)/2; -SF[1] = day_b/2 + dayNoise/2 - (day*day_s)/2; -SF[2] = dax_b/2 + daxNoise/2 - (dax*dax_s)/2; -SF[3] = q3/2 - (q0*SF[0])/2 + (q1*SF[1])/2 - (q2*SF[2])/2; -SF[4] = q0/2 - (q1*SF[2])/2 - (q2*SF[1])/2 + (q3*SF[0])/2; -SF[5] = q1/2 + (q0*SF[2])/2 - (q2*SF[0])/2 - (q3*SF[1])/2; -SF[6] = q3/2 + (q0*SF[0])/2 - (q1*SF[1])/2 - (q2*SF[2])/2; -SF[7] = q0/2 - (q1*SF[2])/2 + (q2*SF[1])/2 - (q3*SF[0])/2; -SF[8] = q0/2 + (q1*SF[2])/2 - (q2*SF[1])/2 - (q3*SF[0])/2; -SF[9] = q2/2 + (q0*SF[1])/2 + (q1*SF[0])/2 + (q3*SF[2])/2; -SF[10] = q2/2 - (q0*SF[1])/2 - (q1*SF[0])/2 + (q3*SF[2])/2; -SF[11] = q2/2 + (q0*SF[1])/2 - (q1*SF[0])/2 - (q3*SF[2])/2; -SF[12] = q1/2 + (q0*SF[2])/2 + (q2*SF[0])/2 + (q3*SF[1])/2; -SF[13] = q1/2 - (q0*SF[2])/2 + (q2*SF[0])/2 - (q3*SF[1])/2; -SF[14] = q3/2 + (q0*SF[0])/2 + (q1*SF[1])/2 + (q2*SF[2])/2; -SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3); -SF[16] = dvz_b - dvz + dvzNoise; -SF[17] = dvx - dvxNoise; -SF[18] = dvy - dvyNoise; -SF[19] = sq(q2); -SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3); -SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3); -SF[22] = 2*q0*q1 - 2*q2*q3; -SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3); -SF[24] = 2*q1*q2; -float SG[5][1]; -SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3); +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); -float SQ[10][1]; -SQ[0] = - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2); -SQ[1] = sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + sq(dvzNoise)*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2); -SQ[2] = sq(dvyNoise)*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvxNoise)*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3); -SQ[3] = sq(SG[0]); -SQ[4] = sq(dvyNoise); -SQ[5] = sq(dvzNoise); -SQ[6] = sq(dvxNoise); -SQ[7] = 2*q2*q3; -SQ[8] = 2*q1*q3; -SQ[9] = 2*q1*q2; -float SPP[23][1]; -SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3); -SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3); -SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13]; -SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10]; -SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12]; -SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14]; -SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12]; -SPP[7] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]; -SPP[8] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9]; -SPP[9] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3); -SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3); -SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3); -SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3); -SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9]; -SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13]; -SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3); -SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3); -SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3); -SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3); -SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3); -SPP[20] = SF[16]*SF[21] - SF[18]*SF[22]; -SPP[21] = 2*q0*q2 + 2*q1*q3; -SPP[22] = SF[15]; +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] = SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[7]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18]) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18]) + sq(daxNoise)*SQ[3]; -nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[8]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[7] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[7] + P[9][13]*SPP[22] + P[12][13]*SPP[18]); -nextP[1][1] = SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) - SPP[8]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17]) + sq(dayNoise)*SQ[3]; -nextP[0][2] = SPP[14]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[3]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[13]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[7] + P[9][11]*SPP[22] + P[12][11]*SPP[18]) + SPP[16]*(P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[7] + P[9][14]*SPP[22] + P[12][14]*SPP[18]); -nextP[1][2] = SPP[14]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) - SPP[3]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[13]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[8] + P[10][11]*SPP[22] + P[13][11]*SPP[17]) + SPP[16]*(P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[8] + P[10][14]*SPP[22] + P[13][14]*SPP[17]); -nextP[2][2] = SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]) + sq(dazNoise)*SQ[3]; -nextP[0][3] = P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[7] + P[9][3]*SPP[22] + P[12][3]*SPP[18] + SPP[1]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[19]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[15]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[21]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]); -nextP[1][3] = P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[8] + P[10][3]*SPP[22] + P[13][3]*SPP[17] + SPP[1]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[19]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[15]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[21]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]); -nextP[2][3] = P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16] + SPP[1]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[19]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[15]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) - SPP[21]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]); -nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + SQ[5]*sq(SQ[8] + 2*q0*q2) + SQ[4]*sq(SQ[9] - 2*q0*q3) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SQ[6]*sq(SG[1] + SG[2] - SG[3] - SG[4]); -nextP[0][4] = P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[7] + P[9][4]*SPP[22] + P[12][4]*SPP[18] + SF[22]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + SPP[12]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[20]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[11]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]); -nextP[1][4] = P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[8] + P[10][4]*SPP[22] + P[13][4]*SPP[17] + SF[22]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + SPP[12]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[20]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[11]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]); -nextP[2][4] = P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16] + SF[22]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + SPP[12]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[20]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[11]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]); -nextP[3][4] = P[3][4] + SQ[2] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21] + SF[22]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SPP[12]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[20]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[11]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]); -nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + SQ[5]*sq(SQ[7] - 2*q0*q1) + SQ[6]*sq(SQ[9] + 2*q0*q3) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SQ[4]*sq(SG[1] - SG[2] + SG[3] - SG[4]); -nextP[0][5] = P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[7] + P[9][5]*SPP[22] + P[12][5]*SPP[18] + SF[20]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) - SPP[9]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[0]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[10]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]); -nextP[1][5] = P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[8] + P[10][5]*SPP[22] + P[13][5]*SPP[17] + SF[20]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) - SPP[9]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[0]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[10]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]); -nextP[2][5] = P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16] + SF[20]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) - SPP[9]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[0]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[10]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]); -nextP[3][5] = P[3][5] + SQ[1] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21] + SF[20]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) - SPP[9]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[0]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) + SPP[10]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]); -nextP[4][5] = P[4][5] + SQ[0] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11] + SF[20]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) - SPP[9]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[0]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SPP[10]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]); -nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + SQ[4]*sq(SQ[7] + 2*q0*q1) + SQ[6]*sq(SQ[8] - 2*q0*q2) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[9]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[9] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[9] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[9] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + SQ[5]*sq(SG[1] - SG[2] - SG[3] + SG[4]); -nextP[0][6] = P[0][6]*SPP[5] - P[1][6]*SPP[4] + P[2][6]*SPP[7] + P[9][6]*SPP[22] + P[12][6]*SPP[18] + dt*(P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[7] + P[9][3]*SPP[22] + P[12][3]*SPP[18]); -nextP[1][6] = P[1][6]*SPP[6] - P[0][6]*SPP[2] - P[2][6]*SPP[8] + P[10][6]*SPP[22] + P[13][6]*SPP[17] + dt*(P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[8] + P[10][3]*SPP[22] + P[13][3]*SPP[17]); -nextP[2][6] = P[0][6]*SPP[14] - P[1][6]*SPP[3] + P[2][6]*SPP[13] + P[11][6]*SPP[22] + P[14][6]*SPP[16] + dt*(P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16]); -nextP[3][6] = P[3][6] + P[0][6]*SPP[1] + P[1][6]*SPP[19] + P[2][6]*SPP[15] - P[15][6]*SPP[21] + dt*(P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21]); -nextP[4][6] = P[4][6] + P[15][6]*SF[22] + P[0][6]*SPP[20] + P[1][6]*SPP[12] + P[2][6]*SPP[11] + dt*(P[4][3] + P[15][3]*SF[22] + P[0][3]*SPP[20] + P[1][3]*SPP[12] + P[2][3]*SPP[11]); -nextP[5][6] = P[5][6] + P[15][6]*SF[20] - P[0][6]*SPP[9] + P[1][6]*SPP[10] + P[2][6]*SPP[0] + dt*(P[5][3] + P[15][3]*SF[20] - P[0][3]*SPP[9] + P[1][3]*SPP[10] + P[2][3]*SPP[0]); -nextP[6][6] = P[6][6] + P[3][6]*dt + dt*(P[6][3] + P[3][3]*dt); -nextP[0][7] = P[0][7]*SPP[5] - P[1][7]*SPP[4] + P[2][7]*SPP[7] + P[9][7]*SPP[22] + P[12][7]*SPP[18] + dt*(P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[7] + P[9][4]*SPP[22] + P[12][4]*SPP[18]); -nextP[1][7] = P[1][7]*SPP[6] - P[0][7]*SPP[2] - P[2][7]*SPP[8] + P[10][7]*SPP[22] + P[13][7]*SPP[17] + dt*(P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[8] + P[10][4]*SPP[22] + P[13][4]*SPP[17]); -nextP[2][7] = P[0][7]*SPP[14] - P[1][7]*SPP[3] + P[2][7]*SPP[13] + P[11][7]*SPP[22] + P[14][7]*SPP[16] + dt*(P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16]); -nextP[3][7] = P[3][7] + P[0][7]*SPP[1] + P[1][7]*SPP[19] + P[2][7]*SPP[15] - P[15][7]*SPP[21] + dt*(P[3][4] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21]); -nextP[4][7] = P[4][7] + P[15][7]*SF[22] + P[0][7]*SPP[20] + P[1][7]*SPP[12] + P[2][7]*SPP[11] + dt*(P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11]); -nextP[5][7] = P[5][7] + P[15][7]*SF[20] - P[0][7]*SPP[9] + P[1][7]*SPP[10] + P[2][7]*SPP[0] + dt*(P[5][4] + P[15][4]*SF[20] - P[0][4]*SPP[9] + P[1][4]*SPP[10] + P[2][4]*SPP[0]); -nextP[6][7] = P[6][7] + P[3][7]*dt + dt*(P[6][4] + P[3][4]*dt); +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]*SPP[5] - P[1][8]*SPP[4] + P[2][8]*SPP[7] + P[9][8]*SPP[22] + P[12][8]*SPP[18] + dt*(P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[7] + P[9][5]*SPP[22] + P[12][5]*SPP[18]); -nextP[1][8] = P[1][8]*SPP[6] - P[0][8]*SPP[2] - P[2][8]*SPP[8] + P[10][8]*SPP[22] + P[13][8]*SPP[17] + dt*(P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[8] + P[10][5]*SPP[22] + P[13][5]*SPP[17]); -nextP[2][8] = P[0][8]*SPP[14] - P[1][8]*SPP[3] + P[2][8]*SPP[13] + P[11][8]*SPP[22] + P[14][8]*SPP[16] + dt*(P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16]); -nextP[3][8] = P[3][8] + P[0][8]*SPP[1] + P[1][8]*SPP[19] + P[2][8]*SPP[15] - P[15][8]*SPP[21] + dt*(P[3][5] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21]); -nextP[4][8] = P[4][8] + P[15][8]*SF[22] + P[0][8]*SPP[20] + P[1][8]*SPP[12] + P[2][8]*SPP[11] + dt*(P[4][5] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11]); -nextP[5][8] = P[5][8] + P[15][8]*SF[20] - P[0][8]*SPP[9] + P[1][8]*SPP[10] + P[2][8]*SPP[0] + dt*(P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0]); -nextP[6][8] = P[6][8] + P[3][8]*dt + dt*(P[6][5] + P[3][5]*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]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18]; -nextP[1][9] = P[1][9]*SPP[6] - P[0][9]*SPP[2] - P[2][9]*SPP[8] + P[10][9]*SPP[22] + P[13][9]*SPP[17]; -nextP[2][9] = P[0][9]*SPP[14] - P[1][9]*SPP[3] + P[2][9]*SPP[13] + P[11][9]*SPP[22] + P[14][9]*SPP[16]; -nextP[3][9] = P[3][9] + P[0][9]*SPP[1] + P[1][9]*SPP[19] + P[2][9]*SPP[15] - P[15][9]*SPP[21]; -nextP[4][9] = P[4][9] + P[15][9]*SF[22] + P[0][9]*SPP[20] + P[1][9]*SPP[12] + P[2][9]*SPP[11]; -nextP[5][9] = P[5][9] + P[15][9]*SF[20] - P[0][9]*SPP[9] + P[1][9]*SPP[10] + P[2][9]*SPP[0]; -nextP[6][9] = P[6][9] + P[3][9]*dt; -nextP[7][9] = P[7][9] + P[4][9]*dt; -nextP[8][9] = P[8][9] + P[5][9]*dt; -nextP[9][9] = P[9][9]; -nextP[0][10] = P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[7] + P[9][10]*SPP[22] + P[12][10]*SPP[18]; -nextP[1][10] = P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17]; -nextP[2][10] = P[0][10]*SPP[14] - P[1][10]*SPP[3] + P[2][10]*SPP[13] + P[11][10]*SPP[22] + P[14][10]*SPP[16]; -nextP[3][10] = P[3][10] + P[0][10]*SPP[1] + P[1][10]*SPP[19] + P[2][10]*SPP[15] - P[15][10]*SPP[21]; -nextP[4][10] = P[4][10] + P[15][10]*SF[22] + P[0][10]*SPP[20] + P[1][10]*SPP[12] + P[2][10]*SPP[11]; -nextP[5][10] = P[5][10] + P[15][10]*SF[20] - P[0][10]*SPP[9] + P[1][10]*SPP[10] + P[2][10]*SPP[0]; -nextP[6][10] = P[6][10] + P[3][10]*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]; +nextP[9][10] = P[9][10] + P[6][10]*dt; nextP[10][10] = P[10][10]; -nextP[0][11] = P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[7] + P[9][11]*SPP[22] + P[12][11]*SPP[18]; -nextP[1][11] = P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[8] + P[10][11]*SPP[22] + P[13][11]*SPP[17]; -nextP[2][11] = P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]; -nextP[3][11] = P[3][11] + P[0][11]*SPP[1] + P[1][11]*SPP[19] + P[2][11]*SPP[15] - P[15][11]*SPP[21]; -nextP[4][11] = P[4][11] + P[15][11]*SF[22] + P[0][11]*SPP[20] + P[1][11]*SPP[12] + P[2][11]*SPP[11]; -nextP[5][11] = P[5][11] + P[15][11]*SF[20] - P[0][11]*SPP[9] + P[1][11]*SPP[10] + P[2][11]*SPP[0]; -nextP[6][11] = P[6][11] + P[3][11]*dt; +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]; +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]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18]; -nextP[1][12] = P[1][12]*SPP[6] - P[0][12]*SPP[2] - P[2][12]*SPP[8] + P[10][12]*SPP[22] + P[13][12]*SPP[17]; -nextP[2][12] = P[0][12]*SPP[14] - P[1][12]*SPP[3] + P[2][12]*SPP[13] + P[11][12]*SPP[22] + P[14][12]*SPP[16]; -nextP[3][12] = P[3][12] + P[0][12]*SPP[1] + P[1][12]*SPP[19] + P[2][12]*SPP[15] - P[15][12]*SPP[21]; -nextP[4][12] = P[4][12] + P[15][12]*SF[22] + P[0][12]*SPP[20] + P[1][12]*SPP[12] + P[2][12]*SPP[11]; -nextP[5][12] = P[5][12] + P[15][12]*SF[20] - P[0][12]*SPP[9] + P[1][12]*SPP[10] + P[2][12]*SPP[0]; -nextP[6][12] = P[6][12] + P[3][12]*dt; +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]; +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]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[7] + P[9][13]*SPP[22] + P[12][13]*SPP[18]; -nextP[1][13] = P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17]; -nextP[2][13] = P[0][13]*SPP[14] - P[1][13]*SPP[3] + P[2][13]*SPP[13] + P[11][13]*SPP[22] + P[14][13]*SPP[16]; -nextP[3][13] = P[3][13] + P[0][13]*SPP[1] + P[1][13]*SPP[19] + P[2][13]*SPP[15] - P[15][13]*SPP[21]; -nextP[4][13] = P[4][13] + P[15][13]*SF[22] + P[0][13]*SPP[20] + P[1][13]*SPP[12] + P[2][13]*SPP[11]; -nextP[5][13] = P[5][13] + P[15][13]*SF[20] - P[0][13]*SPP[9] + P[1][13]*SPP[10] + P[2][13]*SPP[0]; -nextP[6][13] = P[6][13] + P[3][13]*dt; +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]; +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]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[7] + P[9][14]*SPP[22] + P[12][14]*SPP[18]; -nextP[1][14] = P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[8] + P[10][14]*SPP[22] + P[13][14]*SPP[17]; -nextP[2][14] = P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]; -nextP[3][14] = P[3][14] + P[0][14]*SPP[1] + P[1][14]*SPP[19] + P[2][14]*SPP[15] - P[15][14]*SPP[21]; -nextP[4][14] = P[4][14] + P[15][14]*SF[22] + P[0][14]*SPP[20] + P[1][14]*SPP[12] + P[2][14]*SPP[11]; -nextP[5][14] = P[5][14] + P[15][14]*SF[20] - P[0][14]*SPP[9] + P[1][14]*SPP[10] + P[2][14]*SPP[0]; -nextP[6][14] = P[6][14] + P[3][14]*dt; +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]; +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]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]; -nextP[1][15] = P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]; -nextP[2][15] = P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]; -nextP[3][15] = P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]; -nextP[4][15] = P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]; -nextP[5][15] = P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0]; -nextP[6][15] = P[6][15] + P[3][15]*dt; +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]; +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]*SPP[5] - P[1][16]*SPP[4] + P[2][16]*SPP[7] + P[9][16]*SPP[22] + P[12][16]*SPP[18]; -nextP[1][16] = P[1][16]*SPP[6] - P[0][16]*SPP[2] - P[2][16]*SPP[8] + P[10][16]*SPP[22] + P[13][16]*SPP[17]; -nextP[2][16] = P[0][16]*SPP[14] - P[1][16]*SPP[3] + P[2][16]*SPP[13] + P[11][16]*SPP[22] + P[14][16]*SPP[16]; -nextP[3][16] = P[3][16] + P[0][16]*SPP[1] + P[1][16]*SPP[19] + P[2][16]*SPP[15] - P[15][16]*SPP[21]; -nextP[4][16] = P[4][16] + P[15][16]*SF[22] + P[0][16]*SPP[20] + P[1][16]*SPP[12] + P[2][16]*SPP[11]; -nextP[5][16] = P[5][16] + P[15][16]*SF[20] - P[0][16]*SPP[9] + P[1][16]*SPP[10] + P[2][16]*SPP[0]; -nextP[6][16] = P[6][16] + P[3][16]*dt; +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]; +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]; @@ -219,16 +207,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]*SPP[5] - P[1][17]*SPP[4] + P[2][17]*SPP[7] + P[9][17]*SPP[22] + P[12][17]*SPP[18]; -nextP[1][17] = P[1][17]*SPP[6] - P[0][17]*SPP[2] - P[2][17]*SPP[8] + P[10][17]*SPP[22] + P[13][17]*SPP[17]; -nextP[2][17] = P[0][17]*SPP[14] - P[1][17]*SPP[3] + P[2][17]*SPP[13] + P[11][17]*SPP[22] + P[14][17]*SPP[16]; -nextP[3][17] = P[3][17] + P[0][17]*SPP[1] + P[1][17]*SPP[19] + P[2][17]*SPP[15] - P[15][17]*SPP[21]; -nextP[4][17] = P[4][17] + P[15][17]*SF[22] + P[0][17]*SPP[20] + P[1][17]*SPP[12] + P[2][17]*SPP[11]; -nextP[5][17] = P[5][17] + P[15][17]*SF[20] - P[0][17]*SPP[9] + P[1][17]*SPP[10] + P[2][17]*SPP[0]; -nextP[6][17] = P[6][17] + P[3][17]*dt; +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]; +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]; @@ -237,16 +225,16 @@ 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]*SPP[5] - P[1][18]*SPP[4] + P[2][18]*SPP[7] + P[9][18]*SPP[22] + P[12][18]*SPP[18]; -nextP[1][18] = P[1][18]*SPP[6] - P[0][18]*SPP[2] - P[2][18]*SPP[8] + P[10][18]*SPP[22] + P[13][18]*SPP[17]; -nextP[2][18] = P[0][18]*SPP[14] - P[1][18]*SPP[3] + P[2][18]*SPP[13] + P[11][18]*SPP[22] + P[14][18]*SPP[16]; -nextP[3][18] = P[3][18] + P[0][18]*SPP[1] + P[1][18]*SPP[19] + P[2][18]*SPP[15] - P[15][18]*SPP[21]; -nextP[4][18] = P[4][18] + P[15][18]*SF[22] + P[0][18]*SPP[20] + P[1][18]*SPP[12] + P[2][18]*SPP[11]; -nextP[5][18] = P[5][18] + P[15][18]*SF[20] - P[0][18]*SPP[9] + P[1][18]*SPP[10] + P[2][18]*SPP[0]; -nextP[6][18] = P[6][18] + P[3][18]*dt; +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]; +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]; @@ -256,16 +244,16 @@ 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]*SPP[5] - P[1][19]*SPP[4] + P[2][19]*SPP[7] + P[9][19]*SPP[22] + P[12][19]*SPP[18]; -nextP[1][19] = P[1][19]*SPP[6] - P[0][19]*SPP[2] - P[2][19]*SPP[8] + P[10][19]*SPP[22] + P[13][19]*SPP[17]; -nextP[2][19] = P[0][19]*SPP[14] - P[1][19]*SPP[3] + P[2][19]*SPP[13] + P[11][19]*SPP[22] + P[14][19]*SPP[16]; -nextP[3][19] = P[3][19] + P[0][19]*SPP[1] + P[1][19]*SPP[19] + P[2][19]*SPP[15] - P[15][19]*SPP[21]; -nextP[4][19] = P[4][19] + P[15][19]*SF[22] + P[0][19]*SPP[20] + P[1][19]*SPP[12] + P[2][19]*SPP[11]; -nextP[5][19] = P[5][19] + P[15][19]*SF[20] - P[0][19]*SPP[9] + P[1][19]*SPP[10] + P[2][19]*SPP[0]; -nextP[6][19] = P[6][19] + P[3][19]*dt; +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]; +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]; @@ -276,16 +264,16 @@ 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]*SPP[5] - P[1][20]*SPP[4] + P[2][20]*SPP[7] + P[9][20]*SPP[22] + P[12][20]*SPP[18]; -nextP[1][20] = P[1][20]*SPP[6] - P[0][20]*SPP[2] - P[2][20]*SPP[8] + P[10][20]*SPP[22] + P[13][20]*SPP[17]; -nextP[2][20] = P[0][20]*SPP[14] - P[1][20]*SPP[3] + P[2][20]*SPP[13] + P[11][20]*SPP[22] + P[14][20]*SPP[16]; -nextP[3][20] = P[3][20] + P[0][20]*SPP[1] + P[1][20]*SPP[19] + P[2][20]*SPP[15] - P[15][20]*SPP[21]; -nextP[4][20] = P[4][20] + P[15][20]*SF[22] + P[0][20]*SPP[20] + P[1][20]*SPP[12] + P[2][20]*SPP[11]; -nextP[5][20] = P[5][20] + P[15][20]*SF[20] - P[0][20]*SPP[9] + P[1][20]*SPP[10] + P[2][20]*SPP[0]; -nextP[6][20] = P[6][20] + P[3][20]*dt; +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]; +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]; @@ -297,16 +285,16 @@ 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]*SPP[5] - P[1][21]*SPP[4] + P[2][21]*SPP[7] + P[9][21]*SPP[22] + P[12][21]*SPP[18]; -nextP[1][21] = P[1][21]*SPP[6] - P[0][21]*SPP[2] - P[2][21]*SPP[8] + P[10][21]*SPP[22] + P[13][21]*SPP[17]; -nextP[2][21] = P[0][21]*SPP[14] - P[1][21]*SPP[3] + P[2][21]*SPP[13] + P[11][21]*SPP[22] + P[14][21]*SPP[16]; -nextP[3][21] = P[3][21] + P[0][21]*SPP[1] + P[1][21]*SPP[19] + P[2][21]*SPP[15] - P[15][21]*SPP[21]; -nextP[4][21] = P[4][21] + P[15][21]*SF[22] + P[0][21]*SPP[20] + P[1][21]*SPP[12] + P[2][21]*SPP[11]; -nextP[5][21] = P[5][21] + P[15][21]*SF[20] - P[0][21]*SPP[9] + P[1][21]*SPP[10] + P[2][21]*SPP[0]; -nextP[6][21] = P[6][21] + P[3][21]*dt; +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]; +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]; @@ -319,16 +307,16 @@ 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]*SPP[5] - P[1][22]*SPP[4] + P[2][22]*SPP[7] + P[9][22]*SPP[22] + P[12][22]*SPP[18]; -nextP[1][22] = P[1][22]*SPP[6] - P[0][22]*SPP[2] - P[2][22]*SPP[8] + P[10][22]*SPP[22] + P[13][22]*SPP[17]; -nextP[2][22] = P[0][22]*SPP[14] - P[1][22]*SPP[3] + P[2][22]*SPP[13] + P[11][22]*SPP[22] + P[14][22]*SPP[16]; -nextP[3][22] = P[3][22] + P[0][22]*SPP[1] + P[1][22]*SPP[19] + P[2][22]*SPP[15] - P[15][22]*SPP[21]; -nextP[4][22] = P[4][22] + P[15][22]*SF[22] + P[0][22]*SPP[20] + P[1][22]*SPP[12] + P[2][22]*SPP[11]; -nextP[5][22] = P[5][22] + P[15][22]*SF[20] - P[0][22]*SPP[9] + P[1][22]*SPP[10] + P[2][22]*SPP[0]; -nextP[6][22] = P[6][22] + P[3][22]*dt; +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]; +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]; @@ -342,16 +330,16 @@ 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]*SPP[5] - P[1][23]*SPP[4] + P[2][23]*SPP[7] + P[9][23]*SPP[22] + P[12][23]*SPP[18]; -nextP[1][23] = P[1][23]*SPP[6] - P[0][23]*SPP[2] - P[2][23]*SPP[8] + P[10][23]*SPP[22] + P[13][23]*SPP[17]; -nextP[2][23] = P[0][23]*SPP[14] - P[1][23]*SPP[3] + P[2][23]*SPP[13] + P[11][23]*SPP[22] + P[14][23]*SPP[16]; -nextP[3][23] = P[3][23] + P[0][23]*SPP[1] + P[1][23]*SPP[19] + P[2][23]*SPP[15] - P[15][23]*SPP[21]; -nextP[4][23] = P[4][23] + P[15][23]*SF[22] + P[0][23]*SPP[20] + P[1][23]*SPP[12] + P[2][23]*SPP[11]; -nextP[5][23] = P[5][23] + P[15][23]*SF[20] - P[0][23]*SPP[9] + P[1][23]*SPP[10] + P[2][23]*SPP[0]; -nextP[6][23] = P[6][23] + P[3][23]*dt; +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]; +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]; @@ -371,311 +359,328 @@ 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][3] = SH_TAS[2]; -H_TAS[0][4] = SH_TAS[1]; -H_TAS[0][5] = vd*SH_TAS[0]; +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[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0])); +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][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][5]*vd*SH_TAS[0]); -Kfusion[1] = SK_TAS[0]*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][5]*vd*SH_TAS[0]); -Kfusion[2] = SK_TAS[0]*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][5]*vd*SH_TAS[0]); -Kfusion[3] = SK_TAS[0]*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][5]*vd*SH_TAS[0]); -Kfusion[4] = SK_TAS[0]*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][5]*vd*SH_TAS[0]); -Kfusion[5] = SK_TAS[0]*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][5]*vd*SH_TAS[0]); -Kfusion[6] = SK_TAS[0]*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][5]*vd*SH_TAS[0]); -Kfusion[7] = SK_TAS[0]*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][5]*vd*SH_TAS[0]); -Kfusion[8] = SK_TAS[0]*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][5]*vd*SH_TAS[0]); -Kfusion[9] = SK_TAS[0]*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][5]*vd*SH_TAS[0]); -Kfusion[10] = SK_TAS[0]*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][5]*vd*SH_TAS[0]); -Kfusion[11] = SK_TAS[0]*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][5]*vd*SH_TAS[0]); -Kfusion[12] = SK_TAS[0]*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][5]*vd*SH_TAS[0]); -Kfusion[13] = SK_TAS[0]*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][5]*vd*SH_TAS[0]); -Kfusion[14] = SK_TAS[0]*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][5]*vd*SH_TAS[0]); -Kfusion[15] = SK_TAS[0]*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][5]*vd*SH_TAS[0]); -Kfusion[16] = SK_TAS[0]*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][5]*vd*SH_TAS[0]); -Kfusion[17] = SK_TAS[0]*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][5]*vd*SH_TAS[0]); -Kfusion[18] = SK_TAS[0]*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][5]*vd*SH_TAS[0]); -Kfusion[19] = SK_TAS[0]*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][5]*vd*SH_TAS[0]); -Kfusion[20] = SK_TAS[0]*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][5]*vd*SH_TAS[0]); -Kfusion[21] = SK_TAS[0]*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][5]*vd*SH_TAS[0]); -Kfusion[22] = SK_TAS[0]*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][5]*vd*SH_TAS[0]); -Kfusion[23] = SK_TAS[0]*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][5]*vd*SH_TAS[0]); -float SH_BETA[10][1]; -SH_BETA[0] = (2*conj(q0)*conj(q3) + 2*conj(q1)*conj(q2))*(ve - vwe) - vd*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)) + (vn - vwn)*(conjsq(q0) + conjsq(q1) - conjsq(q2) - conjsq(q3)); -SH_BETA[1] = vd*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) - (2*conj(q0)*conj(q3) - 2*conj(q1)*conj(q2))*(vn - vwn) + (ve - vwe)*(conjsq(q0) - conjsq(q1) + conjsq(q2) - conjsq(q3)); -SH_BETA[2] = (2*conj(q0)*conj(q2) + 2*conj(q1)*conj(q3))*(vn - vwn) - (2*conj(q0)*conj(q1) - 2*conj(q2)*conj(q3))*(ve - vwe) + vd*(conjsq(q0) - conjsq(q1) - conjsq(q2) + conjsq(q3)); -SH_BETA[3] = (conjsq(q0) - conjsq(q1) + conjsq(q2) - conjsq(q3))/SH_BETA[0]; +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] = conjsq(q0) + conjsq(q1) - conjsq(q2) - conjsq(q3); -SH_BETA[6] = 2*conj(q0)*conj(q3); -SH_BETA[7] = 1/SH_BETA[0]; -SH_BETA[8] = SH_BETA[6] + 2*conj(q1)*conj(q2); -SH_BETA[9] = SH_BETA[6] - 2*conj(q1)*conj(q2); +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[2]*SH_BETA[7]; -H_BETA[0][1] = SH_BETA[1]*SH_BETA[2]*SH_BETA[4]; -H_BETA[0][2] = - sq(SH_BETA[1])*SH_BETA[4] - 1; -H_BETA[0][3] = - SH_BETA[7]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[5]; -H_BETA[0][4] = SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -H_BETA[0][5] = SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -H_BETA[0][22] = SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]; -H_BETA[0][23] = SH_BETA[1]*SH_BETA[4]*SH_BETA[8] - SH_BETA[3]; -float SK_BETA[6][1]; -SK_BETA[0] = 1/(R_BETA + (SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)))*(P[22][5]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][5]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][5]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][5]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][5]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[7] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) + (SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][4]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][4]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][4]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][4]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][4]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[7] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) - (SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][23]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][23]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][23]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][23]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][23]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[7] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) - (SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5])*(P[22][3]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][3]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][3]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][3]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][3]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[7] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) + (SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5])*(P[22][22]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][22]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][22]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][22]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][22]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[7] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) - (sq(SH_BETA[1])*SH_BETA[4] + 1)*(P[22][2]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][2]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][2]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][2]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][2]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[7] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) + SH_BETA[2]*SH_BETA[7]*(P[22][0]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][0]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][0]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][0]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][0]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[7] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[4]*(P[22][1]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][1]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][1]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][1]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][1]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[7] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4])); -SK_BETA[1] = SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -SK_BETA[2] = SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]; -SK_BETA[3] = SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -SK_BETA[4] = sq(SH_BETA[1])*SH_BETA[4] + 1; -SK_BETA[5] = SH_BETA[2]; +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][5]*SK_BETA[1] - P[0][2]*SK_BETA[4] - P[0][3]*SK_BETA[2] + P[0][4]*SK_BETA[3] + P[0][22]*SK_BETA[2] - P[0][23]*SK_BETA[3] + P[0][0]*SH_BETA[7]*SK_BETA[5] + P[0][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[1] = SK_BETA[0]*(P[1][5]*SK_BETA[1] - P[1][2]*SK_BETA[4] - P[1][3]*SK_BETA[2] + P[1][4]*SK_BETA[3] + P[1][22]*SK_BETA[2] - P[1][23]*SK_BETA[3] + P[1][0]*SH_BETA[7]*SK_BETA[5] + P[1][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[2] = SK_BETA[0]*(P[2][5]*SK_BETA[1] - P[2][2]*SK_BETA[4] - P[2][3]*SK_BETA[2] + P[2][4]*SK_BETA[3] + P[2][22]*SK_BETA[2] - P[2][23]*SK_BETA[3] + P[2][0]*SH_BETA[7]*SK_BETA[5] + P[2][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[3] = SK_BETA[0]*(P[3][5]*SK_BETA[1] - P[3][2]*SK_BETA[4] - P[3][3]*SK_BETA[2] + P[3][4]*SK_BETA[3] + P[3][22]*SK_BETA[2] - P[3][23]*SK_BETA[3] + P[3][0]*SH_BETA[7]*SK_BETA[5] + P[3][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[4] = SK_BETA[0]*(P[4][5]*SK_BETA[1] - P[4][2]*SK_BETA[4] - P[4][3]*SK_BETA[2] + P[4][4]*SK_BETA[3] + P[4][22]*SK_BETA[2] - P[4][23]*SK_BETA[3] + P[4][0]*SH_BETA[7]*SK_BETA[5] + P[4][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[5] = SK_BETA[0]*(P[5][5]*SK_BETA[1] - P[5][2]*SK_BETA[4] - P[5][3]*SK_BETA[2] + P[5][4]*SK_BETA[3] + P[5][22]*SK_BETA[2] - P[5][23]*SK_BETA[3] + P[5][0]*SH_BETA[7]*SK_BETA[5] + P[5][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[6] = SK_BETA[0]*(P[6][5]*SK_BETA[1] - P[6][2]*SK_BETA[4] - P[6][3]*SK_BETA[2] + P[6][4]*SK_BETA[3] + P[6][22]*SK_BETA[2] - P[6][23]*SK_BETA[3] + P[6][0]*SH_BETA[7]*SK_BETA[5] + P[6][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[7] = SK_BETA[0]*(P[7][5]*SK_BETA[1] - P[7][2]*SK_BETA[4] - P[7][3]*SK_BETA[2] + P[7][4]*SK_BETA[3] + P[7][22]*SK_BETA[2] - P[7][23]*SK_BETA[3] + P[7][0]*SH_BETA[7]*SK_BETA[5] + P[7][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[8] = SK_BETA[0]*(P[8][5]*SK_BETA[1] - P[8][2]*SK_BETA[4] - P[8][3]*SK_BETA[2] + P[8][4]*SK_BETA[3] + P[8][22]*SK_BETA[2] - P[8][23]*SK_BETA[3] + P[8][0]*SH_BETA[7]*SK_BETA[5] + P[8][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[9] = SK_BETA[0]*(P[9][5]*SK_BETA[1] - P[9][2]*SK_BETA[4] - P[9][3]*SK_BETA[2] + P[9][4]*SK_BETA[3] + P[9][22]*SK_BETA[2] - P[9][23]*SK_BETA[3] + P[9][0]*SH_BETA[7]*SK_BETA[5] + P[9][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[10] = SK_BETA[0]*(P[10][5]*SK_BETA[1] - P[10][2]*SK_BETA[4] - P[10][3]*SK_BETA[2] + P[10][4]*SK_BETA[3] + P[10][22]*SK_BETA[2] - P[10][23]*SK_BETA[3] + P[10][0]*SH_BETA[7]*SK_BETA[5] + P[10][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[11] = SK_BETA[0]*(P[11][5]*SK_BETA[1] - P[11][2]*SK_BETA[4] - P[11][3]*SK_BETA[2] + P[11][4]*SK_BETA[3] + P[11][22]*SK_BETA[2] - P[11][23]*SK_BETA[3] + P[11][0]*SH_BETA[7]*SK_BETA[5] + P[11][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[12] = SK_BETA[0]*(P[12][5]*SK_BETA[1] - P[12][2]*SK_BETA[4] - P[12][3]*SK_BETA[2] + P[12][4]*SK_BETA[3] + P[12][22]*SK_BETA[2] - P[12][23]*SK_BETA[3] + P[12][0]*SH_BETA[7]*SK_BETA[5] + P[12][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[13] = SK_BETA[0]*(P[13][5]*SK_BETA[1] - P[13][2]*SK_BETA[4] - P[13][3]*SK_BETA[2] + P[13][4]*SK_BETA[3] + P[13][22]*SK_BETA[2] - P[13][23]*SK_BETA[3] + P[13][0]*SH_BETA[7]*SK_BETA[5] + P[13][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[14] = SK_BETA[0]*(P[14][5]*SK_BETA[1] - P[14][2]*SK_BETA[4] - P[14][3]*SK_BETA[2] + P[14][4]*SK_BETA[3] + P[14][22]*SK_BETA[2] - P[14][23]*SK_BETA[3] + P[14][0]*SH_BETA[7]*SK_BETA[5] + P[14][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[15] = SK_BETA[0]*(P[15][5]*SK_BETA[1] - P[15][2]*SK_BETA[4] - P[15][3]*SK_BETA[2] + P[15][4]*SK_BETA[3] + P[15][22]*SK_BETA[2] - P[15][23]*SK_BETA[3] + P[15][0]*SH_BETA[7]*SK_BETA[5] + P[15][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[16] = SK_BETA[0]*(P[16][5]*SK_BETA[1] - P[16][2]*SK_BETA[4] - P[16][3]*SK_BETA[2] + P[16][4]*SK_BETA[3] + P[16][22]*SK_BETA[2] - P[16][23]*SK_BETA[3] + P[16][0]*SH_BETA[7]*SK_BETA[5] + P[16][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[17] = SK_BETA[0]*(P[17][5]*SK_BETA[1] - P[17][2]*SK_BETA[4] - P[17][3]*SK_BETA[2] + P[17][4]*SK_BETA[3] + P[17][22]*SK_BETA[2] - P[17][23]*SK_BETA[3] + P[17][0]*SH_BETA[7]*SK_BETA[5] + P[17][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[18] = SK_BETA[0]*(P[18][5]*SK_BETA[1] - P[18][2]*SK_BETA[4] - P[18][3]*SK_BETA[2] + P[18][4]*SK_BETA[3] + P[18][22]*SK_BETA[2] - P[18][23]*SK_BETA[3] + P[18][0]*SH_BETA[7]*SK_BETA[5] + P[18][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[19] = SK_BETA[0]*(P[19][5]*SK_BETA[1] - P[19][2]*SK_BETA[4] - P[19][3]*SK_BETA[2] + P[19][4]*SK_BETA[3] + P[19][22]*SK_BETA[2] - P[19][23]*SK_BETA[3] + P[19][0]*SH_BETA[7]*SK_BETA[5] + P[19][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[20] = SK_BETA[0]*(P[20][5]*SK_BETA[1] - P[20][2]*SK_BETA[4] - P[20][3]*SK_BETA[2] + P[20][4]*SK_BETA[3] + P[20][22]*SK_BETA[2] - P[20][23]*SK_BETA[3] + P[20][0]*SH_BETA[7]*SK_BETA[5] + P[20][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[21] = SK_BETA[0]*(P[21][5]*SK_BETA[1] - P[21][2]*SK_BETA[4] - P[21][3]*SK_BETA[2] + P[21][4]*SK_BETA[3] + P[21][22]*SK_BETA[2] - P[21][23]*SK_BETA[3] + P[21][0]*SH_BETA[7]*SK_BETA[5] + P[21][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[22] = SK_BETA[0]*(P[22][5]*SK_BETA[1] - P[22][2]*SK_BETA[4] - P[22][3]*SK_BETA[2] + P[22][4]*SK_BETA[3] + P[22][22]*SK_BETA[2] - P[22][23]*SK_BETA[3] + P[22][0]*SH_BETA[7]*SK_BETA[5] + P[22][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[23] = SK_BETA[0]*(P[23][5]*SK_BETA[1] - P[23][2]*SK_BETA[4] - P[23][3]*SK_BETA[2] + P[23][4]*SK_BETA[3] + P[23][22]*SK_BETA[2] - P[23][23]*SK_BETA[3] + P[23][0]*SH_BETA[7]*SK_BETA[5] + P[23][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); +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] = sq(q0) - sq(q1) + sq(q2) - sq(q3); -SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); -SH_MAG[3] = 2*q0*q1 + 2*q2*q3; -SH_MAG[4] = 2*q0*q3 + 2*q1*q2; -SH_MAG[5] = 2*q0*q2 + 2*q1*q3; -SH_MAG[6] = magE*(2*q0*q1 - 2*q2*q3); -SH_MAG[7] = 2*q1*q3 - 2*q0*q2; -SH_MAG[8] = 2*q0*q3; +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[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5]; -H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -H_MAG[16] = SH_MAG[1]; -H_MAG[17] = SH_MAG[4]; -H_MAG[18] = SH_MAG[7]; +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[4][1]; -SK_MX[0] = 1/(P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)))); -SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -SK_MX[3] = SH_MAG[7]; +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][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]); -Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]); -Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]); -Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]); -Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]); -Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]); -Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]); -Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]); -Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]); -Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]); -Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]); -Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]); -Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]); -Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]); -Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]); -Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]); -Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]); -Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]); -Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]); -Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]); -Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]); -Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]); -Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]); -Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]); +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] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1]; -H_MAG[16] = 2*q1*q2 - SH_MAG[8]; -H_MAG[17] = SH_MAG[0]; -H_MAG[18] = SH_MAG[3]; +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[4][1]; -SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2*q1*q2)) - P[16][20]*(SH_MAG[8] - 2*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2*q1*q2))); -SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; -SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -SK_MY[3] = SH_MAG[8] - 2*q1*q2; +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][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]); -Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]); -Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]); -Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]); -Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]); -Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]); -Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]); -Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]); -Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]); -Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]); -Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]); -Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]); -Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]); -Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]); -Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]); -Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]); -Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]); -Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]); -Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]); -Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]); -Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]); -Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]); -Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]); -Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]); +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] = magN*(SH_MAG[8] - 2*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0]; -H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; -H_MAG[16] = SH_MAG[5]; +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[2]; +H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; H_MAG[21] = 1; -float SK_MZ[4][1]; -SK_MZ[0] = 1/(P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2*q0*q1 - 2*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2*q0*q1 - 2*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2*q0*q1 - 2*q2*q3)) - P[17][21]*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2*q0*q1 - 2*q2*q3))); -SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[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][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]); -Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]); -Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]); -Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]); -Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]); -Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]); -Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]); -Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]); -Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]); -Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]); -Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]); -Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]); -Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]); -Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]); -Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]); -Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]); -Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]); -Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]); -Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]); -Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]); -Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]); -Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]); -Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]); -Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]); -float SH_ACCX[7][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] = 2*q0*q3 + 2*q1*q2; -SH_ACCX[2] = vn - vwn; -SH_ACCX[3] = ve - vwe; -SH_ACCX[4] = sq(q3); -SH_ACCX[5] = 2*q0*q2; -SH_ACCX[6] = 2*q0*q1; +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][1] = Kaccx*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))); -H_ACCX[0][2] = Kaccx*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)); -H_ACCX[0][3] = -Kaccx*SH_ACCX[0]; -H_ACCX[0][4] = -Kaccx*SH_ACCX[1]; -H_ACCX[0][5] = Kaccx*(SH_ACCX[5] - 2*q1*q3); +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[1]; -float SK_ACCX[5][1]; -SK_ACCX[0] = 1/(R_ACC - Kaccx*SH_ACCX[0]*(Kaccx*P[22][3]*SH_ACCX[0] - Kaccx*P[4][3]*SH_ACCX[1] - Kaccx*P[3][3]*SH_ACCX[0] + Kaccx*P[23][3]*SH_ACCX[1] + Kaccx*P[2][3]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][3]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][3]*(SH_ACCX[5] - 2*q1*q3)) - Kaccx*SH_ACCX[1]*(Kaccx*P[22][4]*SH_ACCX[0] - Kaccx*P[4][4]*SH_ACCX[1] - Kaccx*P[3][4]*SH_ACCX[0] + Kaccx*P[23][4]*SH_ACCX[1] + Kaccx*P[2][4]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][4]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][4]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*SH_ACCX[0]*(Kaccx*P[22][22]*SH_ACCX[0] - Kaccx*P[4][22]*SH_ACCX[1] - Kaccx*P[3][22]*SH_ACCX[0] + Kaccx*P[23][22]*SH_ACCX[1] + Kaccx*P[2][22]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][22]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][22]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*SH_ACCX[1]*(Kaccx*P[22][23]*SH_ACCX[0] - Kaccx*P[4][23]*SH_ACCX[1] - Kaccx*P[3][23]*SH_ACCX[0] + Kaccx*P[23][23]*SH_ACCX[1] + Kaccx*P[2][23]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][23]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][23]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3))*(Kaccx*P[22][2]*SH_ACCX[0] - Kaccx*P[4][2]*SH_ACCX[1] - Kaccx*P[3][2]*SH_ACCX[0] + Kaccx*P[23][2]*SH_ACCX[1] + Kaccx*P[2][2]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][2]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][2]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2)))*(Kaccx*P[22][1]*SH_ACCX[0] - Kaccx*P[4][1]*SH_ACCX[1] - Kaccx*P[3][1]*SH_ACCX[0] + Kaccx*P[23][1]*SH_ACCX[1] + Kaccx*P[2][1]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][1]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][1]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*(SH_ACCX[5] - 2*q1*q3)*(Kaccx*P[22][5]*SH_ACCX[0] - Kaccx*P[4][5]*SH_ACCX[1] - Kaccx*P[3][5]*SH_ACCX[0] + Kaccx*P[23][5]*SH_ACCX[1] + Kaccx*P[2][5]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][5]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][5]*(SH_ACCX[5] - 2*q1*q3))); -SK_ACCX[1] = SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3); -SK_ACCX[2] = SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2)); -SK_ACCX[3] = SH_ACCX[5] - 2*q1*q3; -SK_ACCX[4] = SH_ACCX[1]; +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][22]*SH_ACCX[0] - Kaccx*P[0][3]*SH_ACCX[0] + Kaccx*P[0][1]*SK_ACCX[2] + Kaccx*P[0][2]*SK_ACCX[1] - Kaccx*P[0][4]*SK_ACCX[4] + Kaccx*P[0][5]*SK_ACCX[3] + Kaccx*P[0][23]*SK_ACCX[4]); -Kfusion[1] = SK_ACCX[0]*(Kaccx*P[1][22]*SH_ACCX[0] - Kaccx*P[1][3]*SH_ACCX[0] + Kaccx*P[1][1]*SK_ACCX[2] + Kaccx*P[1][2]*SK_ACCX[1] - Kaccx*P[1][4]*SK_ACCX[4] + Kaccx*P[1][5]*SK_ACCX[3] + Kaccx*P[1][23]*SK_ACCX[4]); -Kfusion[2] = SK_ACCX[0]*(Kaccx*P[2][22]*SH_ACCX[0] - Kaccx*P[2][3]*SH_ACCX[0] + Kaccx*P[2][1]*SK_ACCX[2] + Kaccx*P[2][2]*SK_ACCX[1] - Kaccx*P[2][4]*SK_ACCX[4] + Kaccx*P[2][5]*SK_ACCX[3] + Kaccx*P[2][23]*SK_ACCX[4]); -Kfusion[3] = SK_ACCX[0]*(Kaccx*P[3][22]*SH_ACCX[0] - Kaccx*P[3][3]*SH_ACCX[0] + Kaccx*P[3][1]*SK_ACCX[2] + Kaccx*P[3][2]*SK_ACCX[1] - Kaccx*P[3][4]*SK_ACCX[4] + Kaccx*P[3][5]*SK_ACCX[3] + Kaccx*P[3][23]*SK_ACCX[4]); -Kfusion[4] = SK_ACCX[0]*(Kaccx*P[4][22]*SH_ACCX[0] - Kaccx*P[4][3]*SH_ACCX[0] + Kaccx*P[4][1]*SK_ACCX[2] + Kaccx*P[4][2]*SK_ACCX[1] - Kaccx*P[4][4]*SK_ACCX[4] + Kaccx*P[4][5]*SK_ACCX[3] + Kaccx*P[4][23]*SK_ACCX[4]); -Kfusion[5] = SK_ACCX[0]*(Kaccx*P[5][22]*SH_ACCX[0] - Kaccx*P[5][3]*SH_ACCX[0] + Kaccx*P[5][1]*SK_ACCX[2] + Kaccx*P[5][2]*SK_ACCX[1] - Kaccx*P[5][4]*SK_ACCX[4] + Kaccx*P[5][5]*SK_ACCX[3] + Kaccx*P[5][23]*SK_ACCX[4]); -Kfusion[6] = SK_ACCX[0]*(Kaccx*P[6][22]*SH_ACCX[0] - Kaccx*P[6][3]*SH_ACCX[0] + Kaccx*P[6][1]*SK_ACCX[2] + Kaccx*P[6][2]*SK_ACCX[1] - Kaccx*P[6][4]*SK_ACCX[4] + Kaccx*P[6][5]*SK_ACCX[3] + Kaccx*P[6][23]*SK_ACCX[4]); -Kfusion[7] = SK_ACCX[0]*(Kaccx*P[7][22]*SH_ACCX[0] - Kaccx*P[7][3]*SH_ACCX[0] + Kaccx*P[7][1]*SK_ACCX[2] + Kaccx*P[7][2]*SK_ACCX[1] - Kaccx*P[7][4]*SK_ACCX[4] + Kaccx*P[7][5]*SK_ACCX[3] + Kaccx*P[7][23]*SK_ACCX[4]); -Kfusion[8] = SK_ACCX[0]*(Kaccx*P[8][22]*SH_ACCX[0] - Kaccx*P[8][3]*SH_ACCX[0] + Kaccx*P[8][1]*SK_ACCX[2] + Kaccx*P[8][2]*SK_ACCX[1] - Kaccx*P[8][4]*SK_ACCX[4] + Kaccx*P[8][5]*SK_ACCX[3] + Kaccx*P[8][23]*SK_ACCX[4]); -Kfusion[9] = SK_ACCX[0]*(Kaccx*P[9][22]*SH_ACCX[0] - Kaccx*P[9][3]*SH_ACCX[0] + Kaccx*P[9][1]*SK_ACCX[2] + Kaccx*P[9][2]*SK_ACCX[1] - Kaccx*P[9][4]*SK_ACCX[4] + Kaccx*P[9][5]*SK_ACCX[3] + Kaccx*P[9][23]*SK_ACCX[4]); -Kfusion[10] = SK_ACCX[0]*(Kaccx*P[10][22]*SH_ACCX[0] - Kaccx*P[10][3]*SH_ACCX[0] + Kaccx*P[10][1]*SK_ACCX[2] + Kaccx*P[10][2]*SK_ACCX[1] - Kaccx*P[10][4]*SK_ACCX[4] + Kaccx*P[10][5]*SK_ACCX[3] + Kaccx*P[10][23]*SK_ACCX[4]); -Kfusion[11] = SK_ACCX[0]*(Kaccx*P[11][22]*SH_ACCX[0] - Kaccx*P[11][3]*SH_ACCX[0] + Kaccx*P[11][1]*SK_ACCX[2] + Kaccx*P[11][2]*SK_ACCX[1] - Kaccx*P[11][4]*SK_ACCX[4] + Kaccx*P[11][5]*SK_ACCX[3] + Kaccx*P[11][23]*SK_ACCX[4]); -Kfusion[12] = SK_ACCX[0]*(Kaccx*P[12][22]*SH_ACCX[0] - Kaccx*P[12][3]*SH_ACCX[0] + Kaccx*P[12][1]*SK_ACCX[2] + Kaccx*P[12][2]*SK_ACCX[1] - Kaccx*P[12][4]*SK_ACCX[4] + Kaccx*P[12][5]*SK_ACCX[3] + Kaccx*P[12][23]*SK_ACCX[4]); -Kfusion[13] = SK_ACCX[0]*(Kaccx*P[13][22]*SH_ACCX[0] - Kaccx*P[13][3]*SH_ACCX[0] + Kaccx*P[13][1]*SK_ACCX[2] + Kaccx*P[13][2]*SK_ACCX[1] - Kaccx*P[13][4]*SK_ACCX[4] + Kaccx*P[13][5]*SK_ACCX[3] + Kaccx*P[13][23]*SK_ACCX[4]); -Kfusion[14] = SK_ACCX[0]*(Kaccx*P[14][22]*SH_ACCX[0] - Kaccx*P[14][3]*SH_ACCX[0] + Kaccx*P[14][1]*SK_ACCX[2] + Kaccx*P[14][2]*SK_ACCX[1] - Kaccx*P[14][4]*SK_ACCX[4] + Kaccx*P[14][5]*SK_ACCX[3] + Kaccx*P[14][23]*SK_ACCX[4]); -Kfusion[15] = SK_ACCX[0]*(Kaccx*P[15][22]*SH_ACCX[0] - Kaccx*P[15][3]*SH_ACCX[0] + Kaccx*P[15][1]*SK_ACCX[2] + Kaccx*P[15][2]*SK_ACCX[1] - Kaccx*P[15][4]*SK_ACCX[4] + Kaccx*P[15][5]*SK_ACCX[3] + Kaccx*P[15][23]*SK_ACCX[4]); -Kfusion[16] = SK_ACCX[0]*(Kaccx*P[16][22]*SH_ACCX[0] - Kaccx*P[16][3]*SH_ACCX[0] + Kaccx*P[16][1]*SK_ACCX[2] + Kaccx*P[16][2]*SK_ACCX[1] - Kaccx*P[16][4]*SK_ACCX[4] + Kaccx*P[16][5]*SK_ACCX[3] + Kaccx*P[16][23]*SK_ACCX[4]); -Kfusion[17] = SK_ACCX[0]*(Kaccx*P[17][22]*SH_ACCX[0] - Kaccx*P[17][3]*SH_ACCX[0] + Kaccx*P[17][1]*SK_ACCX[2] + Kaccx*P[17][2]*SK_ACCX[1] - Kaccx*P[17][4]*SK_ACCX[4] + Kaccx*P[17][5]*SK_ACCX[3] + Kaccx*P[17][23]*SK_ACCX[4]); -Kfusion[18] = SK_ACCX[0]*(Kaccx*P[18][22]*SH_ACCX[0] - Kaccx*P[18][3]*SH_ACCX[0] + Kaccx*P[18][1]*SK_ACCX[2] + Kaccx*P[18][2]*SK_ACCX[1] - Kaccx*P[18][4]*SK_ACCX[4] + Kaccx*P[18][5]*SK_ACCX[3] + Kaccx*P[18][23]*SK_ACCX[4]); -Kfusion[19] = SK_ACCX[0]*(Kaccx*P[19][22]*SH_ACCX[0] - Kaccx*P[19][3]*SH_ACCX[0] + Kaccx*P[19][1]*SK_ACCX[2] + Kaccx*P[19][2]*SK_ACCX[1] - Kaccx*P[19][4]*SK_ACCX[4] + Kaccx*P[19][5]*SK_ACCX[3] + Kaccx*P[19][23]*SK_ACCX[4]); -Kfusion[20] = SK_ACCX[0]*(Kaccx*P[20][22]*SH_ACCX[0] - Kaccx*P[20][3]*SH_ACCX[0] + Kaccx*P[20][1]*SK_ACCX[2] + Kaccx*P[20][2]*SK_ACCX[1] - Kaccx*P[20][4]*SK_ACCX[4] + Kaccx*P[20][5]*SK_ACCX[3] + Kaccx*P[20][23]*SK_ACCX[4]); -Kfusion[21] = SK_ACCX[0]*(Kaccx*P[21][22]*SH_ACCX[0] - Kaccx*P[21][3]*SH_ACCX[0] + Kaccx*P[21][1]*SK_ACCX[2] + Kaccx*P[21][2]*SK_ACCX[1] - Kaccx*P[21][4]*SK_ACCX[4] + Kaccx*P[21][5]*SK_ACCX[3] + Kaccx*P[21][23]*SK_ACCX[4]); -Kfusion[22] = SK_ACCX[0]*(Kaccx*P[22][22]*SH_ACCX[0] - Kaccx*P[22][3]*SH_ACCX[0] + Kaccx*P[22][1]*SK_ACCX[2] + Kaccx*P[22][2]*SK_ACCX[1] - Kaccx*P[22][4]*SK_ACCX[4] + Kaccx*P[22][5]*SK_ACCX[3] + Kaccx*P[22][23]*SK_ACCX[4]); -Kfusion[23] = SK_ACCX[0]*(Kaccx*P[23][22]*SH_ACCX[0] - Kaccx*P[23][3]*SH_ACCX[0] + Kaccx*P[23][1]*SK_ACCX[2] + Kaccx*P[23][2]*SK_ACCX[1] - Kaccx*P[23][4]*SK_ACCX[4] + Kaccx*P[23][5]*SK_ACCX[3] + Kaccx*P[23][23]*SK_ACCX[4]); -float SH_ACCY[6][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] = ve - vwe; -SH_ACCY[2] = vn - vwn; -SH_ACCY[3] = sq(q1); -SH_ACCY[4] = 2*q0*q1; -SH_ACCY[5] = 2*q0*q3; +SH_ACCY[1] = vn - vwn; +SH_ACCY[2] = ve - vwe; float H_ACCY[1][24]; -H_ACCY[0][0] = Kaccy*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))); -H_ACCY[0][2] = Kaccy*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)); -H_ACCY[0][3] = Kaccy*(SH_ACCY[5] - 2*q1*q2); -H_ACCY[0][4] = -Kaccy*SH_ACCY[0]; -H_ACCY[0][5] = -Kaccy*(SH_ACCY[4] + 2*q2*q3); +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[7][1]; -SK_ACCY[0] = 1/(R_ACC - Kaccy*SH_ACCY[0]*(Kaccy*P[23][4]*SH_ACCY[0] - Kaccy*P[4][4]*SH_ACCY[0] + Kaccy*P[0][4]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][4]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][4]*(q0*q3 - q1*q2) + Kaccy*P[3][4]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][4]*(SH_ACCY[4] + 2*q2*q3)) + Kaccy*SH_ACCY[0]*(Kaccy*P[23][23]*SH_ACCY[0] - Kaccy*P[4][23]*SH_ACCY[0] + Kaccy*P[0][23]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][23]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][23]*(q0*q3 - q1*q2) + Kaccy*P[3][23]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][23]*(SH_ACCY[4] + 2*q2*q3)) + Kaccy*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3)))*(Kaccy*P[23][0]*SH_ACCY[0] - Kaccy*P[4][0]*SH_ACCY[0] + Kaccy*P[0][0]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][0]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][0]*(q0*q3 - q1*q2) + Kaccy*P[3][0]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][0]*(SH_ACCY[4] + 2*q2*q3)) + Kaccy*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3))*(Kaccy*P[23][2]*SH_ACCY[0] - Kaccy*P[4][2]*SH_ACCY[0] + Kaccy*P[0][2]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][2]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][2]*(q0*q3 - q1*q2) + Kaccy*P[3][2]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][2]*(SH_ACCY[4] + 2*q2*q3)) - 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*P[23][22]*SH_ACCY[0] - Kaccy*P[4][22]*SH_ACCY[0] + Kaccy*P[0][22]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][22]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][22]*(q0*q3 - q1*q2) + Kaccy*P[3][22]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][22]*(SH_ACCY[4] + 2*q2*q3)) + Kaccy*(SH_ACCY[5] - 2*q1*q2)*(Kaccy*P[23][3]*SH_ACCY[0] - Kaccy*P[4][3]*SH_ACCY[0] + Kaccy*P[0][3]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][3]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][3]*(q0*q3 - q1*q2) + Kaccy*P[3][3]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][3]*(SH_ACCY[4] + 2*q2*q3)) - Kaccy*(SH_ACCY[4] + 2*q2*q3)*(Kaccy*P[23][5]*SH_ACCY[0] - Kaccy*P[4][5]*SH_ACCY[0] + Kaccy*P[0][5]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][5]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][5]*(q0*q3 - q1*q2) + Kaccy*P[3][5]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][5]*(SH_ACCY[4] + 2*q2*q3))); -SK_ACCY[1] = SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3); -SK_ACCY[2] = SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3)); -SK_ACCY[3] = q0*q3 - q1*q2; -SK_ACCY[4] = SH_ACCY[5] - 2*q1*q2; -SK_ACCY[5] = SH_ACCY[4] + 2*q2*q3; -SK_ACCY[6] = 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[2] + Kaccy*P[0][2]*SK_ACCY[1] + Kaccy*P[0][3]*SK_ACCY[4] - Kaccy*P[0][4]*SK_ACCY[6] - Kaccy*P[0][5]*SK_ACCY[5] - 2*Kaccy*P[0][22]*SK_ACCY[3] + Kaccy*P[0][23]*SK_ACCY[6]); -Kfusion[1] = SK_ACCY[0]*(Kaccy*P[1][0]*SK_ACCY[2] + Kaccy*P[1][2]*SK_ACCY[1] + Kaccy*P[1][3]*SK_ACCY[4] - Kaccy*P[1][4]*SK_ACCY[6] - Kaccy*P[1][5]*SK_ACCY[5] - 2*Kaccy*P[1][22]*SK_ACCY[3] + Kaccy*P[1][23]*SK_ACCY[6]); -Kfusion[2] = SK_ACCY[0]*(Kaccy*P[2][0]*SK_ACCY[2] + Kaccy*P[2][2]*SK_ACCY[1] + Kaccy*P[2][3]*SK_ACCY[4] - Kaccy*P[2][4]*SK_ACCY[6] - Kaccy*P[2][5]*SK_ACCY[5] - 2*Kaccy*P[2][22]*SK_ACCY[3] + Kaccy*P[2][23]*SK_ACCY[6]); -Kfusion[3] = SK_ACCY[0]*(Kaccy*P[3][0]*SK_ACCY[2] + Kaccy*P[3][2]*SK_ACCY[1] + Kaccy*P[3][3]*SK_ACCY[4] - Kaccy*P[3][4]*SK_ACCY[6] - Kaccy*P[3][5]*SK_ACCY[5] - 2*Kaccy*P[3][22]*SK_ACCY[3] + Kaccy*P[3][23]*SK_ACCY[6]); -Kfusion[4] = SK_ACCY[0]*(Kaccy*P[4][0]*SK_ACCY[2] + Kaccy*P[4][2]*SK_ACCY[1] + Kaccy*P[4][3]*SK_ACCY[4] - Kaccy*P[4][4]*SK_ACCY[6] - Kaccy*P[4][5]*SK_ACCY[5] - 2*Kaccy*P[4][22]*SK_ACCY[3] + Kaccy*P[4][23]*SK_ACCY[6]); -Kfusion[5] = SK_ACCY[0]*(Kaccy*P[5][0]*SK_ACCY[2] + Kaccy*P[5][2]*SK_ACCY[1] + Kaccy*P[5][3]*SK_ACCY[4] - Kaccy*P[5][4]*SK_ACCY[6] - Kaccy*P[5][5]*SK_ACCY[5] - 2*Kaccy*P[5][22]*SK_ACCY[3] + Kaccy*P[5][23]*SK_ACCY[6]); -Kfusion[6] = SK_ACCY[0]*(Kaccy*P[6][0]*SK_ACCY[2] + Kaccy*P[6][2]*SK_ACCY[1] + Kaccy*P[6][3]*SK_ACCY[4] - Kaccy*P[6][4]*SK_ACCY[6] - Kaccy*P[6][5]*SK_ACCY[5] - 2*Kaccy*P[6][22]*SK_ACCY[3] + Kaccy*P[6][23]*SK_ACCY[6]); -Kfusion[7] = SK_ACCY[0]*(Kaccy*P[7][0]*SK_ACCY[2] + Kaccy*P[7][2]*SK_ACCY[1] + Kaccy*P[7][3]*SK_ACCY[4] - Kaccy*P[7][4]*SK_ACCY[6] - Kaccy*P[7][5]*SK_ACCY[5] - 2*Kaccy*P[7][22]*SK_ACCY[3] + Kaccy*P[7][23]*SK_ACCY[6]); -Kfusion[8] = SK_ACCY[0]*(Kaccy*P[8][0]*SK_ACCY[2] + Kaccy*P[8][2]*SK_ACCY[1] + Kaccy*P[8][3]*SK_ACCY[4] - Kaccy*P[8][4]*SK_ACCY[6] - Kaccy*P[8][5]*SK_ACCY[5] - 2*Kaccy*P[8][22]*SK_ACCY[3] + Kaccy*P[8][23]*SK_ACCY[6]); -Kfusion[9] = SK_ACCY[0]*(Kaccy*P[9][0]*SK_ACCY[2] + Kaccy*P[9][2]*SK_ACCY[1] + Kaccy*P[9][3]*SK_ACCY[4] - Kaccy*P[9][4]*SK_ACCY[6] - Kaccy*P[9][5]*SK_ACCY[5] - 2*Kaccy*P[9][22]*SK_ACCY[3] + Kaccy*P[9][23]*SK_ACCY[6]); -Kfusion[10] = SK_ACCY[0]*(Kaccy*P[10][0]*SK_ACCY[2] + Kaccy*P[10][2]*SK_ACCY[1] + Kaccy*P[10][3]*SK_ACCY[4] - Kaccy*P[10][4]*SK_ACCY[6] - Kaccy*P[10][5]*SK_ACCY[5] - 2*Kaccy*P[10][22]*SK_ACCY[3] + Kaccy*P[10][23]*SK_ACCY[6]); -Kfusion[11] = SK_ACCY[0]*(Kaccy*P[11][0]*SK_ACCY[2] + Kaccy*P[11][2]*SK_ACCY[1] + Kaccy*P[11][3]*SK_ACCY[4] - Kaccy*P[11][4]*SK_ACCY[6] - Kaccy*P[11][5]*SK_ACCY[5] - 2*Kaccy*P[11][22]*SK_ACCY[3] + Kaccy*P[11][23]*SK_ACCY[6]); -Kfusion[12] = SK_ACCY[0]*(Kaccy*P[12][0]*SK_ACCY[2] + Kaccy*P[12][2]*SK_ACCY[1] + Kaccy*P[12][3]*SK_ACCY[4] - Kaccy*P[12][4]*SK_ACCY[6] - Kaccy*P[12][5]*SK_ACCY[5] - 2*Kaccy*P[12][22]*SK_ACCY[3] + Kaccy*P[12][23]*SK_ACCY[6]); -Kfusion[13] = SK_ACCY[0]*(Kaccy*P[13][0]*SK_ACCY[2] + Kaccy*P[13][2]*SK_ACCY[1] + Kaccy*P[13][3]*SK_ACCY[4] - Kaccy*P[13][4]*SK_ACCY[6] - Kaccy*P[13][5]*SK_ACCY[5] - 2*Kaccy*P[13][22]*SK_ACCY[3] + Kaccy*P[13][23]*SK_ACCY[6]); -Kfusion[14] = SK_ACCY[0]*(Kaccy*P[14][0]*SK_ACCY[2] + Kaccy*P[14][2]*SK_ACCY[1] + Kaccy*P[14][3]*SK_ACCY[4] - Kaccy*P[14][4]*SK_ACCY[6] - Kaccy*P[14][5]*SK_ACCY[5] - 2*Kaccy*P[14][22]*SK_ACCY[3] + Kaccy*P[14][23]*SK_ACCY[6]); -Kfusion[15] = SK_ACCY[0]*(Kaccy*P[15][0]*SK_ACCY[2] + Kaccy*P[15][2]*SK_ACCY[1] + Kaccy*P[15][3]*SK_ACCY[4] - Kaccy*P[15][4]*SK_ACCY[6] - Kaccy*P[15][5]*SK_ACCY[5] - 2*Kaccy*P[15][22]*SK_ACCY[3] + Kaccy*P[15][23]*SK_ACCY[6]); -Kfusion[16] = SK_ACCY[0]*(Kaccy*P[16][0]*SK_ACCY[2] + Kaccy*P[16][2]*SK_ACCY[1] + Kaccy*P[16][3]*SK_ACCY[4] - Kaccy*P[16][4]*SK_ACCY[6] - Kaccy*P[16][5]*SK_ACCY[5] - 2*Kaccy*P[16][22]*SK_ACCY[3] + Kaccy*P[16][23]*SK_ACCY[6]); -Kfusion[17] = SK_ACCY[0]*(Kaccy*P[17][0]*SK_ACCY[2] + Kaccy*P[17][2]*SK_ACCY[1] + Kaccy*P[17][3]*SK_ACCY[4] - Kaccy*P[17][4]*SK_ACCY[6] - Kaccy*P[17][5]*SK_ACCY[5] - 2*Kaccy*P[17][22]*SK_ACCY[3] + Kaccy*P[17][23]*SK_ACCY[6]); -Kfusion[18] = SK_ACCY[0]*(Kaccy*P[18][0]*SK_ACCY[2] + Kaccy*P[18][2]*SK_ACCY[1] + Kaccy*P[18][3]*SK_ACCY[4] - Kaccy*P[18][4]*SK_ACCY[6] - Kaccy*P[18][5]*SK_ACCY[5] - 2*Kaccy*P[18][22]*SK_ACCY[3] + Kaccy*P[18][23]*SK_ACCY[6]); -Kfusion[19] = SK_ACCY[0]*(Kaccy*P[19][0]*SK_ACCY[2] + Kaccy*P[19][2]*SK_ACCY[1] + Kaccy*P[19][3]*SK_ACCY[4] - Kaccy*P[19][4]*SK_ACCY[6] - Kaccy*P[19][5]*SK_ACCY[5] - 2*Kaccy*P[19][22]*SK_ACCY[3] + Kaccy*P[19][23]*SK_ACCY[6]); -Kfusion[20] = SK_ACCY[0]*(Kaccy*P[20][0]*SK_ACCY[2] + Kaccy*P[20][2]*SK_ACCY[1] + Kaccy*P[20][3]*SK_ACCY[4] - Kaccy*P[20][4]*SK_ACCY[6] - Kaccy*P[20][5]*SK_ACCY[5] - 2*Kaccy*P[20][22]*SK_ACCY[3] + Kaccy*P[20][23]*SK_ACCY[6]); -Kfusion[21] = SK_ACCY[0]*(Kaccy*P[21][0]*SK_ACCY[2] + Kaccy*P[21][2]*SK_ACCY[1] + Kaccy*P[21][3]*SK_ACCY[4] - Kaccy*P[21][4]*SK_ACCY[6] - Kaccy*P[21][5]*SK_ACCY[5] - 2*Kaccy*P[21][22]*SK_ACCY[3] + Kaccy*P[21][23]*SK_ACCY[6]); -Kfusion[22] = SK_ACCY[0]*(Kaccy*P[22][0]*SK_ACCY[2] + Kaccy*P[22][2]*SK_ACCY[1] + Kaccy*P[22][3]*SK_ACCY[4] - Kaccy*P[22][4]*SK_ACCY[6] - Kaccy*P[22][5]*SK_ACCY[5] - 2*Kaccy*P[22][22]*SK_ACCY[3] + Kaccy*P[22][23]*SK_ACCY[6]); -Kfusion[23] = SK_ACCY[0]*(Kaccy*P[23][0]*SK_ACCY[2] + Kaccy*P[23][2]*SK_ACCY[1] + Kaccy*P[23][3]*SK_ACCY[4] - Kaccy*P[23][4]*SK_ACCY[6] - Kaccy*P[23][5]*SK_ACCY[5] - 2*Kaccy*P[23][22]*SK_ACCY[3] + Kaccy*P[23][23]*SK_ACCY[6]); +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/matlab/scripts/Inertial Nav EKF/Drag.mat b/matlab/scripts/Inertial Nav EKF/Drag.mat index 7b9b77124a..bf456947ab 100644 Binary files a/matlab/scripts/Inertial Nav EKF/Drag.mat and b/matlab/scripts/Inertial Nav EKF/Drag.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/H_LOSX.c b/matlab/scripts/Inertial Nav EKF/H_LOSX.c index c7ba63108e..e2223cc9e4 100644 --- a/matlab/scripts/Inertial Nav EKF/H_LOSX.c +++ b/matlab/scripts/Inertial Nav EKF/H_LOSX.c @@ -1,15 +1,8 @@ t2 = 1.0/range; -t3 = q0*q0; -t4 = q1*q1; -t5 = q2*q2; -t6 = q3*q3; -t7 = q0*q2*2.0; -t8 = q1*q3*2.0; -t9 = q0*q3*2.0; -t10 = q1*q2*2.0; -t11 = q0*q1*2.0; -A0[0][0] = t2*(vn*(t7+t8)+vd*(t3-t4-t5+t6)-ve*(t11-q2*q3*2.0)); -A0[0][2] = -t2*(ve*(t9+t10)-vd*(t7-t8)+vn*(t3+t4-t5-t6)); -A0[0][3] = -t2*(t9-t10); -A0[0][4] = t2*(t3-t4+t5-t6); -A0[0][5] = t2*(t11+q2*q3*2.0); +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/matlab/scripts/Inertial Nav EKF/H_LOSY.c b/matlab/scripts/Inertial Nav EKF/H_LOSY.c index 1c7ee5be18..306eed204b 100644 --- a/matlab/scripts/Inertial Nav EKF/H_LOSY.c +++ b/matlab/scripts/Inertial Nav EKF/H_LOSY.c @@ -1,14 +1,8 @@ t2 = 1.0/range; -t3 = q0*q0; -t4 = q1*q1; -t5 = q2*q2; -t6 = q3*q3; -t7 = q0*q1*2.0; -t8 = q0*q3*2.0; -t9 = q0*q2*2.0; -t10 = q1*q3*2.0; -A0[0][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0)); -A0[0][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0)-vn*(t8-q1*q2*2.0)); -A0[0][3] = -t2*(t3+t4-t5-t6); -A0[0][4] = -t2*(t8+q1*q2*2.0); -A0[0][5] = t2*(t9-t10); +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/matlab/scripts/Inertial Nav EKF/K_LOSX.c b/matlab/scripts/Inertial Nav EKF/K_LOSX.c index fe44362016..5044d0cc6e 100644 --- a/matlab/scripts/Inertial Nav EKF/K_LOSX.c +++ b/matlab/scripts/Inertial Nav EKF/K_LOSX.c @@ -1,89 +1,117 @@ t2 = 1.0/range; -t3 = q0*q1*2.0; -t4 = q2*q3*2.0; -t5 = q0*q0; -t6 = q1*q1; -t7 = q2*q2; -t8 = q3*q3; -t9 = q0*q2*2.0; -t10 = q1*q3*2.0; -t11 = q0*q3*2.0; -t12 = q1*q2*2.0; -t13 = t11-t12; -t14 = t3+t4; -t15 = t5-t6-t7+t8; -t16 = t15*vd; -t17 = t3-t4; -t18 = t9+t10; -t19 = t18*vn; -t28 = t17*ve; -t20 = t16+t19-t28; -t21 = t5+t6-t7-t8; -t22 = t21*vn; -t23 = t9-t10; -t24 = t11+t12; -t25 = t24*ve; -t29 = t23*vd; -t26 = t22+t25-t29; -t27 = t5-t6+t7-t8; -t30 = P[0][0]*t2*t20; -t31 = P[5][3]*t2*t14; -t32 = P[0][3]*t2*t20; -t33 = P[4][3]*t2*t27; -t56 = P[3][3]*t2*t13; -t57 = P[2][3]*t2*t26; -t34 = t31+t32+t33-t56-t57; -t35 = P[5][5]*t2*t14; -t36 = P[0][5]*t2*t20; -t37 = P[4][5]*t2*t27; -t59 = P[3][5]*t2*t13; -t60 = P[2][5]*t2*t26; -t38 = t35+t36+t37-t59-t60; -t39 = t2*t14*t38; -t40 = P[5][0]*t2*t14; -t41 = P[4][0]*t2*t27; -t61 = P[3][0]*t2*t13; -t62 = P[2][0]*t2*t26; -t42 = t30+t40+t41-t61-t62; -t43 = t2*t20*t42; -t44 = P[5][2]*t2*t14; -t45 = P[0][2]*t2*t20; -t46 = P[4][2]*t2*t27; -t55 = P[2][2]*t2*t26; -t63 = P[3][2]*t2*t13; -t47 = t44+t45+t46-t55-t63; -t48 = P[5][4]*t2*t14; -t49 = P[0][4]*t2*t20; -t50 = P[4][4]*t2*t27; -t65 = P[3][4]*t2*t13; -t66 = P[2][4]*t2*t26; -t51 = t48+t49+t50-t65-t66; -t52 = t2*t27*t51; -t58 = t2*t13*t34; -t64 = t2*t26*t47; -t53 = R_LOS+t39+t43+t52-t58-t64; -t54 = 1.0/t53; -A0[0][0] = t54*(t30-P[0][3]*t2*(t11-q1*q2*2.0)+P[0][5]*t2*t14-P[0][2]*t2*t26+P[0][4]*t2*t27); -A0[1][0] = t54*(-P[1][3]*t2*t13+P[1][5]*t2*t14+P[1][0]*t2*t20-P[1][2]*t2*t26+P[1][4]*t2*t27); -A0[2][0] = t54*(-t55-P[2][3]*t2*t13+P[2][5]*t2*t14+P[2][0]*t2*t20+P[2][4]*t2*t27); -A0[3][0] = t54*(-t56+P[3][5]*t2*t14+P[3][0]*t2*t20-P[3][2]*t2*t26+P[3][4]*t2*t27); -A0[4][0] = t54*(t50-P[4][3]*t2*t13+P[4][5]*t2*t14+P[4][0]*t2*t20-P[4][2]*t2*t26); -A0[5][0] = t54*(t35-P[5][3]*t2*t13+P[5][0]*t2*t20-P[5][2]*t2*t26+P[5][4]*t2*t27); -A0[6][0] = t54*(-P[6][3]*t2*t13+P[6][5]*t2*t14+P[6][0]*t2*t20-P[6][2]*t2*t26+P[6][4]*t2*t27); -A0[7][0] = t54*(-P[7][3]*t2*t13+P[7][5]*t2*t14+P[7][0]*t2*t20-P[7][2]*t2*t26+P[7][4]*t2*t27); -A0[8][0] = t54*(-P[8][3]*t2*t13+P[8][5]*t2*t14+P[8][0]*t2*t20-P[8][2]*t2*t26+P[8][4]*t2*t27); -A0[9][0] = t54*(-P[9][3]*t2*t13+P[9][5]*t2*t14+P[9][0]*t2*t20-P[9][2]*t2*t26+P[9][4]*t2*t27); -A0[10][0] = t54*(-P[10][3]*t2*t13+P[10][5]*t2*t14+P[10][0]*t2*t20-P[10][2]*t2*t26+P[10][4]*t2*t27); -A0[11][0] = t54*(-P[11][3]*t2*t13+P[11][5]*t2*t14+P[11][0]*t2*t20-P[11][2]*t2*t26+P[11][4]*t2*t27); -A0[12][0] = t54*(-P[12][3]*t2*t13+P[12][5]*t2*t14+P[12][0]*t2*t20-P[12][2]*t2*t26+P[12][4]*t2*t27); -A0[13][0] = t54*(-P[13][3]*t2*t13+P[13][5]*t2*t14+P[13][0]*t2*t20-P[13][2]*t2*t26+P[13][4]*t2*t27); -A0[14][0] = t54*(-P[14][3]*t2*t13+P[14][5]*t2*t14+P[14][0]*t2*t20-P[14][2]*t2*t26+P[14][4]*t2*t27); -A0[15][0] = t54*(-P[15][3]*t2*t13+P[15][5]*t2*t14+P[15][0]*t2*t20-P[15][2]*t2*t26+P[15][4]*t2*t27); -A0[16][0] = t54*(-P[16][3]*t2*t13+P[16][5]*t2*t14+P[16][0]*t2*t20-P[16][2]*t2*t26+P[16][4]*t2*t27); -A0[17][0] = t54*(-P[17][3]*t2*t13+P[17][5]*t2*t14+P[17][0]*t2*t20-P[17][2]*t2*t26+P[17][4]*t2*t27); -A0[18][0] = t54*(-P[18][3]*t2*t13+P[18][5]*t2*t14+P[18][0]*t2*t20-P[18][2]*t2*t26+P[18][4]*t2*t27); -A0[19][0] = t54*(-P[19][3]*t2*t13+P[19][5]*t2*t14+P[19][0]*t2*t20-P[19][2]*t2*t26+P[19][4]*t2*t27); -A0[20][0] = t54*(-P[20][3]*t2*t13+P[20][5]*t2*t14+P[20][0]*t2*t20-P[20][2]*t2*t26+P[20][4]*t2*t27); -A0[21][0] = t54*(-P[21][3]*t2*t13+P[21][5]*t2*t14+P[21][0]*t2*t20-P[21][2]*t2*t26+P[21][4]*t2*t27); -A0[22][0] = t54*(-P[22][3]*t2*t13+P[22][5]*t2*t14+P[22][0]*t2*t20-P[22][2]*t2*t26+P[22][4]*t2*t27); -A0[23][0] = t54*(-P[23][3]*t2*t13+P[23][5]*t2*t14+P[23][0]*t2*t20-P[23][2]*t2*t26+P[23][4]*t2*t27); +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/matlab/scripts/Inertial Nav EKF/K_LOSY.c b/matlab/scripts/Inertial Nav EKF/K_LOSY.c index abd0c8f983..8abde33643 100644 --- a/matlab/scripts/Inertial Nav EKF/K_LOSY.c +++ b/matlab/scripts/Inertial Nav EKF/K_LOSY.c @@ -1,89 +1,117 @@ t2 = 1.0/range; -t3 = q0*q2*2.0; -t4 = q0*q0; -t5 = q1*q1; -t6 = q2*q2; -t7 = q3*q3; -t8 = q0*q1*2.0; -t9 = q0*q3*2.0; -t10 = q1*q2*2.0; -t11 = t9+t10; -t12 = q1*q3*2.0; -t13 = t4-t5-t6+t7; -t14 = t13*vd; -t15 = q2*q3*2.0; -t16 = t3+t12; -t17 = t16*vn; -t18 = t4-t5+t6-t7; -t19 = t18*ve; -t20 = t8+t15; -t21 = t20*vd; -t22 = t9-t10; -t28 = t22*vn; -t23 = t19+t21-t28; -t24 = t4+t5-t6-t7; -t25 = t3-t12; -t26 = t8-t15; -t29 = t26*ve; -t27 = t14+t17-t29; -t30 = P[4][4]*t2*t11; -t31 = P[2][4]*t2*t23; -t32 = P[3][4]*t2*t24; -t56 = P[5][4]*t2*t25; -t57 = P[1][4]*t2*t27; -t33 = t30+t31+t32-t56-t57; -t34 = t2*t11*t33; -t35 = P[4][5]*t2*t11; -t36 = P[2][5]*t2*t23; -t37 = P[3][5]*t2*t24; -t58 = P[5][5]*t2*t25; -t59 = P[1][5]*t2*t27; -t38 = t35+t36+t37-t58-t59; -t39 = P[4][1]*t2*t11; -t40 = P[2][1]*t2*t23; -t41 = P[3][1]*t2*t24; -t55 = P[1][1]*t2*t27; -t61 = P[5][1]*t2*t25; -t42 = t39+t40+t41-t55-t61; -t43 = P[4][2]*t2*t11; -t44 = P[2][2]*t2*t23; -t45 = P[3][2]*t2*t24; -t63 = P[5][2]*t2*t25; -t64 = P[1][2]*t2*t27; -t46 = t43+t44+t45-t63-t64; -t47 = t2*t23*t46; -t48 = P[4][3]*t2*t11; -t49 = P[2][3]*t2*t23; -t50 = P[3][3]*t2*t24; -t65 = P[5][3]*t2*t25; -t66 = P[1][3]*t2*t27; -t51 = t48+t49+t50-t65-t66; -t52 = t2*t24*t51; -t60 = t2*t25*t38; -t62 = t2*t27*t42; -t53 = R_LOS+t34+t47+t52-t60-t62; -t54 = 1.0/t53; -A0[0][0] = -t54*(P[0][4]*t2*t11+P[0][2]*t2*t23+P[0][3]*t2*t24-P[0][1]*t2*t27-P[0][5]*t2*t25); -A0[1][0] = -t54*(-t55+P[1][4]*t2*t11+P[1][2]*t2*t23+P[1][3]*t2*t24-P[1][5]*t2*t25); -A0[2][0] = -t54*(t44+P[2][4]*t2*t11+P[2][3]*t2*t24-P[2][1]*t2*t27-P[2][5]*t2*t25); -A0[3][0] = -t54*(t50+P[3][4]*t2*t11+P[3][2]*t2*t23-P[3][1]*t2*t27-P[3][5]*t2*t25); -A0[4][0] = -t54*(t30+P[4][2]*t2*t23+P[4][3]*t2*t24-P[4][1]*t2*t27-P[4][5]*t2*t25); -A0[5][0] = -t54*(-t58+P[5][4]*t2*t11+P[5][2]*t2*t23+P[5][3]*t2*t24-P[5][1]*t2*t27); -A0[6][0] = -t54*(P[6][4]*t2*t11+P[6][2]*t2*t23+P[6][3]*t2*t24-P[6][1]*t2*t27-P[6][5]*t2*t25); -A0[7][0] = -t54*(P[7][4]*t2*t11+P[7][2]*t2*t23+P[7][3]*t2*t24-P[7][1]*t2*t27-P[7][5]*t2*t25); -A0[8][0] = -t54*(P[8][4]*t2*t11+P[8][2]*t2*t23+P[8][3]*t2*t24-P[8][1]*t2*t27-P[8][5]*t2*t25); -A0[9][0] = -t54*(P[9][4]*t2*t11+P[9][2]*t2*t23+P[9][3]*t2*t24-P[9][1]*t2*t27-P[9][5]*t2*t25); -A0[10][0] = -t54*(P[10][4]*t2*t11+P[10][2]*t2*t23+P[10][3]*t2*t24-P[10][1]*t2*t27-P[10][5]*t2*t25); -A0[11][0] = -t54*(P[11][4]*t2*t11+P[11][2]*t2*t23+P[11][3]*t2*t24-P[11][1]*t2*t27-P[11][5]*t2*t25); -A0[12][0] = -t54*(P[12][4]*t2*t11+P[12][2]*t2*t23+P[12][3]*t2*t24-P[12][1]*t2*t27-P[12][5]*t2*t25); -A0[13][0] = -t54*(P[13][4]*t2*t11+P[13][2]*t2*t23+P[13][3]*t2*t24-P[13][1]*t2*t27-P[13][5]*t2*t25); -A0[14][0] = -t54*(P[14][4]*t2*t11+P[14][2]*t2*t23+P[14][3]*t2*t24-P[14][1]*t2*t27-P[14][5]*t2*t25); -A0[15][0] = -t54*(P[15][4]*t2*t11+P[15][2]*t2*t23+P[15][3]*t2*t24-P[15][1]*t2*t27-P[15][5]*t2*t25); -A0[16][0] = -t54*(P[16][4]*t2*t11+P[16][2]*t2*t23+P[16][3]*t2*t24-P[16][1]*t2*t27-P[16][5]*t2*t25); -A0[17][0] = -t54*(P[17][4]*t2*t11+P[17][2]*t2*t23+P[17][3]*t2*t24-P[17][1]*t2*t27-P[17][5]*t2*t25); -A0[18][0] = -t54*(P[18][4]*t2*t11+P[18][2]*t2*t23+P[18][3]*t2*t24-P[18][1]*t2*t27-P[18][5]*t2*t25); -A0[19][0] = -t54*(P[19][4]*t2*t11+P[19][2]*t2*t23+P[19][3]*t2*t24-P[19][1]*t2*t27-P[19][5]*t2*t25); -A0[20][0] = -t54*(P[20][4]*t2*t11+P[20][2]*t2*t23+P[20][3]*t2*t24-P[20][1]*t2*t27-P[20][5]*t2*t25); -A0[21][0] = -t54*(P[21][4]*t2*t11+P[21][2]*t2*t23+P[21][3]*t2*t24-P[21][1]*t2*t27-P[21][5]*t2*t25); -A0[22][0] = -t54*(P[22][4]*t2*t11+P[22][2]*t2*t23+P[22][3]*t2*t24-P[22][1]*t2*t27-P[22][5]*t2*t25); -A0[23][0] = -t54*(P[23][4]*t2*t11+P[23][2]*t2*t23+P[23][3]*t2*t24-P[23][1]*t2*t27-P[23][5]*t2*t25); +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/matlab/scripts/Inertial Nav EKF/M_code24.txt b/matlab/scripts/Inertial Nav EKF/M_code24.txt index 00527b0767..aa51c6ff7e 100644 --- a/matlab/scripts/Inertial Nav EKF/M_code24.txt +++ b/matlab/scripts/Inertial Nav EKF/M_code24.txt @@ -1,217 +1,205 @@ -SF = zeros(25,1); -SF(1) = daz_b/2 + dazNoise/2 - (daz*daz_s)/2; -SF(2) = day_b/2 + dayNoise/2 - (day*day_s)/2; -SF(3) = dax_b/2 + daxNoise/2 - (dax*dax_s)/2; -SF(4) = q3/2 - (q0*SF(1))/2 + (q1*SF(2))/2 - (q2*SF(3))/2; -SF(5) = q0/2 - (q1*SF(3))/2 - (q2*SF(2))/2 + (q3*SF(1))/2; -SF(6) = q1/2 + (q0*SF(3))/2 - (q2*SF(1))/2 - (q3*SF(2))/2; -SF(7) = q3/2 + (q0*SF(1))/2 - (q1*SF(2))/2 - (q2*SF(3))/2; -SF(8) = q0/2 - (q1*SF(3))/2 + (q2*SF(2))/2 - (q3*SF(1))/2; -SF(9) = q0/2 + (q1*SF(3))/2 - (q2*SF(2))/2 - (q3*SF(1))/2; -SF(10) = q2/2 + (q0*SF(2))/2 + (q1*SF(1))/2 + (q3*SF(3))/2; -SF(11) = q2/2 - (q0*SF(2))/2 - (q1*SF(1))/2 + (q3*SF(3))/2; -SF(12) = q2/2 + (q0*SF(2))/2 - (q1*SF(1))/2 - (q3*SF(3))/2; -SF(13) = q1/2 + (q0*SF(3))/2 + (q2*SF(1))/2 + (q3*SF(2))/2; -SF(14) = q1/2 - (q0*SF(3))/2 + (q2*SF(1))/2 - (q3*SF(2))/2; -SF(15) = q3/2 + (q0*SF(1))/2 + (q1*SF(2))/2 + (q2*SF(3))/2; -SF(16) = - q0^2 - q1^2 - q2^2 - q3^2; -SF(17) = dvz_b - dvz + dvzNoise; -SF(18) = dvx - dvxNoise; -SF(19) = dvy - dvyNoise; -SF(20) = q2^2; -SF(21) = SF(20) - q0^2 + q1^2 - q3^2; -SF(22) = SF(20) + q0^2 - q1^2 - q3^2; -SF(23) = 2*q0*q1 - 2*q2*q3; -SF(24) = SF(20) - q0^2 - q1^2 + q3^2; -SF(25) = 2*q1*q2; -SG = zeros(5,1); -SG(1) = - q0^2 - q1^2 - q2^2 - q3^2; +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; -SQ = zeros(10,1); -SQ(1) = - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2); -SQ(2) = dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvzNoise^2*(2*q0*q2 + 2*q1*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2); -SQ(3) = dvyNoise^2*(2*q0*q3 - 2*q1*q2)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxNoise^2*(2*q0*q3 + 2*q1*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3); -SQ(4) = SG(1)^2; -SQ(5) = dvyNoise^2; -SQ(6) = dvzNoise^2; -SQ(7) = dvxNoise^2; -SQ(8) = 2*q2*q3; -SQ(9) = 2*q1*q3; -SQ(10) = 2*q1*q2; -SPP = zeros(23,1); -SPP(1) = SF(18)*(2*q0*q1 + 2*q2*q3) + SF(19)*(2*q0*q2 - 2*q1*q3); -SPP(2) = SF(19)*(2*q0*q2 + 2*q1*q3) + SF(17)*(SF(25) - 2*q0*q3); -SPP(3) = 2*q3*SF(9) + 2*q1*SF(12) - 2*q0*SF(15) - 2*q2*SF(14); -SPP(4) = 2*q1*SF(8) + 2*q2*SF(7) - 2*q0*SF(13) - 2*q3*SF(11); -SPP(5) = 2*q0*SF(7) - 2*q3*SF(8) - 2*q1*SF(11) + 2*q2*SF(13); -SPP(6) = 2*q0*SF(9) + 2*q2*SF(12) + 2*q1*SF(14) + 2*q3*SF(15); -SPP(7) = 2*q0*SF(8) + 2*q3*SF(7) + 2*q2*SF(11) + 2*q1*SF(13); -SPP(8) = 2*q1*SF(4) - 2*q2*SF(5) - 2*q3*SF(6) + 2*q0*SF(10); -SPP(9) = 2*q0*SF(6) - 2*q1*SF(5) - 2*q2*SF(4) + 2*q3*SF(10); -SPP(10) = SF(19)*SF(21) - SF(17)*(2*q0*q1 + 2*q2*q3); -SPP(11) = SF(18)*SF(21) + SF(17)*(2*q0*q2 - 2*q1*q3); -SPP(12) = SF(18)*SF(22) - SF(19)*(SF(25) + 2*q0*q3); -SPP(13) = SF(18)*SF(23) - SF(17)*(SF(25) + 2*q0*q3); -SPP(14) = 2*q0*SF(5) + 2*q1*SF(6) + 2*q3*SF(4) + 2*q2*SF(10); -SPP(15) = 2*q2*SF(9) - 2*q0*SF(12) - 2*q1*SF(15) + 2*q3*SF(14); -SPP(16) = SF(19)*SF(24) + SF(18)*(SF(25) - 2*q0*q3); -SPP(17) = daz*SF(20) + daz*q0^2 + daz*q1^2 + daz*q3^2; -SPP(18) = day*SF(20) + day*q0^2 + day*q1^2 + day*q3^2; -SPP(19) = dax*SF(20) + dax*q0^2 + dax*q1^2 + dax*q3^2; -SPP(20) = SF(17)*SF(24) - SF(18)*(2*q0*q2 + 2*q1*q3); -SPP(21) = SF(17)*SF(22) - SF(19)*SF(23); -SPP(22) = 2*q0*q2 + 2*q1*q3; -SPP(23) = SF(16); +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) = SPP(6)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(5)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(8)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,10)*SPP(6) - OP(2,10)*SPP(5) + OP(3,10)*SPP(8) + OP(10,10)*SPP(23) + OP(13,10)*SPP(19)) + SPP(19)*(OP(1,13)*SPP(6) - OP(2,13)*SPP(5) + OP(3,13)*SPP(8) + OP(10,13)*SPP(23) + OP(13,13)*SPP(19)) + daxNoise^2*SQ(4); -nextP(1,2) = SPP(7)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) - SPP(3)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(9)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,11)*SPP(6) - OP(2,11)*SPP(5) + OP(3,11)*SPP(8) + OP(10,11)*SPP(23) + OP(13,11)*SPP(19)) + SPP(18)*(OP(1,14)*SPP(6) - OP(2,14)*SPP(5) + OP(3,14)*SPP(8) + OP(10,14)*SPP(23) + OP(13,14)*SPP(19)); -nextP(2,2) = SPP(7)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) - SPP(3)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) - SPP(9)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(23)*(OP(2,11)*SPP(7) - OP(1,11)*SPP(3) - OP(3,11)*SPP(9) + OP(11,11)*SPP(23) + OP(14,11)*SPP(18)) + SPP(18)*(OP(2,14)*SPP(7) - OP(1,14)*SPP(3) - OP(3,14)*SPP(9) + OP(11,14)*SPP(23) + OP(14,14)*SPP(18)) + dayNoise^2*SQ(4); -nextP(1,3) = SPP(15)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(4)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(14)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,12)*SPP(6) - OP(2,12)*SPP(5) + OP(3,12)*SPP(8) + OP(10,12)*SPP(23) + OP(13,12)*SPP(19)) + SPP(17)*(OP(1,15)*SPP(6) - OP(2,15)*SPP(5) + OP(3,15)*SPP(8) + OP(10,15)*SPP(23) + OP(13,15)*SPP(19)); -nextP(2,3) = SPP(15)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) - SPP(4)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(14)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(23)*(OP(2,12)*SPP(7) - OP(1,12)*SPP(3) - OP(3,12)*SPP(9) + OP(11,12)*SPP(23) + OP(14,12)*SPP(18)) + SPP(17)*(OP(2,15)*SPP(7) - OP(1,15)*SPP(3) - OP(3,15)*SPP(9) + OP(11,15)*SPP(23) + OP(14,15)*SPP(18)); -nextP(3,3) = SPP(15)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) - SPP(4)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(14)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) + SPP(23)*(OP(1,12)*SPP(15) - OP(2,12)*SPP(4) + OP(3,12)*SPP(14) + OP(12,12)*SPP(23) + OP(15,12)*SPP(17)) + SPP(17)*(OP(1,15)*SPP(15) - OP(2,15)*SPP(4) + OP(3,15)*SPP(14) + OP(12,15)*SPP(23) + OP(15,15)*SPP(17)) + dazNoise^2*SQ(4); -nextP(1,4) = OP(1,4)*SPP(6) - OP(2,4)*SPP(5) + OP(3,4)*SPP(8) + OP(10,4)*SPP(23) + OP(13,4)*SPP(19) + SPP(2)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(20)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(16)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) - SPP(22)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19)); -nextP(2,4) = OP(2,4)*SPP(7) - OP(1,4)*SPP(3) - OP(3,4)*SPP(9) + OP(11,4)*SPP(23) + OP(14,4)*SPP(18) + SPP(2)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(20)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(16)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) - SPP(22)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18)); -nextP(3,4) = OP(1,4)*SPP(15) - OP(2,4)*SPP(4) + OP(3,4)*SPP(14) + OP(12,4)*SPP(23) + OP(15,4)*SPP(17) + SPP(2)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(20)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(16)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) - SPP(22)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17)); -nextP(4,4) = OP(4,4) + OP(1,4)*SPP(2) + OP(2,4)*SPP(20) + OP(3,4)*SPP(16) - OP(16,4)*SPP(22) + SQ(6)*(SQ(9) + 2*q0*q2)^2 + SQ(5)*(SQ(10) - 2*q0*q3)^2 + SPP(2)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(20)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22)) + SPP(16)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22)) - SPP(22)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) + SQ(7)*(SG(2) + SG(3) - SG(4) - SG(5))^2; -nextP(1,5) = OP(1,5)*SPP(6) - OP(2,5)*SPP(5) + OP(3,5)*SPP(8) + OP(10,5)*SPP(23) + OP(13,5)*SPP(19) + SF(23)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19)) + SPP(13)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(21)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(12)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)); -nextP(2,5) = OP(2,5)*SPP(7) - OP(1,5)*SPP(3) - OP(3,5)*SPP(9) + OP(11,5)*SPP(23) + OP(14,5)*SPP(18) + SF(23)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18)) + SPP(13)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(21)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(12)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)); -nextP(3,5) = OP(1,5)*SPP(15) - OP(2,5)*SPP(4) + OP(3,5)*SPP(14) + OP(12,5)*SPP(23) + OP(15,5)*SPP(17) + SF(23)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17)) + SPP(13)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(21)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(12)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)); -nextP(4,5) = OP(4,5) + SQ(3) + OP(1,5)*SPP(2) + OP(2,5)*SPP(20) + OP(3,5)*SPP(16) - OP(16,5)*SPP(22) + SF(23)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) + SPP(13)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22)) + SPP(21)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(12)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22)); -nextP(5,5) = OP(5,5) + OP(16,5)*SF(23) + OP(1,5)*SPP(21) + OP(2,5)*SPP(13) + OP(3,5)*SPP(12) + SQ(6)*(SQ(8) - 2*q0*q1)^2 + SQ(7)*(SQ(10) + 2*q0*q3)^2 + SF(23)*(OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12)) + SPP(13)*(OP(5,2) + OP(16,2)*SF(23) + OP(1,2)*SPP(21) + OP(2,2)*SPP(13) + OP(3,2)*SPP(12)) + SPP(21)*(OP(5,1) + OP(16,1)*SF(23) + OP(1,1)*SPP(21) + OP(2,1)*SPP(13) + OP(3,1)*SPP(12)) + SPP(12)*(OP(5,3) + OP(16,3)*SF(23) + OP(1,3)*SPP(21) + OP(2,3)*SPP(13) + OP(3,3)*SPP(12)) + SQ(5)*(SG(2) - SG(3) + SG(4) - SG(5))^2; -nextP(1,6) = OP(1,6)*SPP(6) - OP(2,6)*SPP(5) + OP(3,6)*SPP(8) + OP(10,6)*SPP(23) + OP(13,6)*SPP(19) + SF(21)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19)) - SPP(10)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(1)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(11)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)); -nextP(2,6) = OP(2,6)*SPP(7) - OP(1,6)*SPP(3) - OP(3,6)*SPP(9) + OP(11,6)*SPP(23) + OP(14,6)*SPP(18) + SF(21)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18)) - SPP(10)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(1)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(11)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)); -nextP(3,6) = OP(1,6)*SPP(15) - OP(2,6)*SPP(4) + OP(3,6)*SPP(14) + OP(12,6)*SPP(23) + OP(15,6)*SPP(17) + SF(21)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17)) - SPP(10)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(1)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) + SPP(11)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)); -nextP(4,6) = OP(4,6) + SQ(2) + OP(1,6)*SPP(2) + OP(2,6)*SPP(20) + OP(3,6)*SPP(16) - OP(16,6)*SPP(22) + SF(21)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) - SPP(10)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(1)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22)) + SPP(11)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22)); -nextP(5,6) = OP(5,6) + SQ(1) + OP(16,6)*SF(23) + OP(1,6)*SPP(21) + OP(2,6)*SPP(13) + OP(3,6)*SPP(12) + SF(21)*(OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12)) - SPP(10)*(OP(5,1) + OP(16,1)*SF(23) + OP(1,1)*SPP(21) + OP(2,1)*SPP(13) + OP(3,1)*SPP(12)) + SPP(1)*(OP(5,3) + OP(16,3)*SF(23) + OP(1,3)*SPP(21) + OP(2,3)*SPP(13) + OP(3,3)*SPP(12)) + SPP(11)*(OP(5,2) + OP(16,2)*SF(23) + OP(1,2)*SPP(21) + OP(2,2)*SPP(13) + OP(3,2)*SPP(12)); -nextP(6,6) = OP(6,6) + OP(16,6)*SF(21) - OP(1,6)*SPP(10) + OP(2,6)*SPP(11) + OP(3,6)*SPP(1) + SQ(5)*(SQ(8) + 2*q0*q1)^2 + SQ(7)*(SQ(9) - 2*q0*q2)^2 + SF(21)*(OP(6,16) + OP(16,16)*SF(21) - OP(1,16)*SPP(10) + OP(2,16)*SPP(11) + OP(3,16)*SPP(1)) - SPP(10)*(OP(6,1) + OP(16,1)*SF(21) - OP(1,1)*SPP(10) + OP(2,1)*SPP(11) + OP(3,1)*SPP(1)) + SPP(1)*(OP(6,3) + OP(16,3)*SF(21) - OP(1,3)*SPP(10) + OP(2,3)*SPP(11) + OP(3,3)*SPP(1)) + SPP(11)*(OP(6,2) + OP(16,2)*SF(21) - OP(1,2)*SPP(10) + OP(2,2)*SPP(11) + OP(3,2)*SPP(1)) + SQ(6)*(SG(2) - SG(3) - SG(4) + SG(5))^2; -nextP(1,7) = OP(1,7)*SPP(6) - OP(2,7)*SPP(5) + OP(3,7)*SPP(8) + OP(10,7)*SPP(23) + OP(13,7)*SPP(19) + dt*(OP(1,4)*SPP(6) - OP(2,4)*SPP(5) + OP(3,4)*SPP(8) + OP(10,4)*SPP(23) + OP(13,4)*SPP(19)); -nextP(2,7) = OP(2,7)*SPP(7) - OP(1,7)*SPP(3) - OP(3,7)*SPP(9) + OP(11,7)*SPP(23) + OP(14,7)*SPP(18) + dt*(OP(2,4)*SPP(7) - OP(1,4)*SPP(3) - OP(3,4)*SPP(9) + OP(11,4)*SPP(23) + OP(14,4)*SPP(18)); -nextP(3,7) = OP(1,7)*SPP(15) - OP(2,7)*SPP(4) + OP(3,7)*SPP(14) + OP(12,7)*SPP(23) + OP(15,7)*SPP(17) + dt*(OP(1,4)*SPP(15) - OP(2,4)*SPP(4) + OP(3,4)*SPP(14) + OP(12,4)*SPP(23) + OP(15,4)*SPP(17)); -nextP(4,7) = OP(4,7) + OP(1,7)*SPP(2) + OP(2,7)*SPP(20) + OP(3,7)*SPP(16) - OP(16,7)*SPP(22) + dt*(OP(4,4) + OP(1,4)*SPP(2) + OP(2,4)*SPP(20) + OP(3,4)*SPP(16) - OP(16,4)*SPP(22)); -nextP(5,7) = OP(5,7) + OP(16,7)*SF(23) + OP(1,7)*SPP(21) + OP(2,7)*SPP(13) + OP(3,7)*SPP(12) + dt*(OP(5,4) + OP(16,4)*SF(23) + OP(1,4)*SPP(21) + OP(2,4)*SPP(13) + OP(3,4)*SPP(12)); -nextP(6,7) = OP(6,7) + OP(16,7)*SF(21) - OP(1,7)*SPP(10) + OP(2,7)*SPP(11) + OP(3,7)*SPP(1) + dt*(OP(6,4) + OP(16,4)*SF(21) - OP(1,4)*SPP(10) + OP(2,4)*SPP(11) + OP(3,4)*SPP(1)); -nextP(7,7) = OP(7,7) + OP(4,7)*dt + dt*(OP(7,4) + OP(4,4)*dt); -nextP(1,8) = OP(1,8)*SPP(6) - OP(2,8)*SPP(5) + OP(3,8)*SPP(8) + OP(10,8)*SPP(23) + OP(13,8)*SPP(19) + dt*(OP(1,5)*SPP(6) - OP(2,5)*SPP(5) + OP(3,5)*SPP(8) + OP(10,5)*SPP(23) + OP(13,5)*SPP(19)); -nextP(2,8) = OP(2,8)*SPP(7) - OP(1,8)*SPP(3) - OP(3,8)*SPP(9) + OP(11,8)*SPP(23) + OP(14,8)*SPP(18) + dt*(OP(2,5)*SPP(7) - OP(1,5)*SPP(3) - OP(3,5)*SPP(9) + OP(11,5)*SPP(23) + OP(14,5)*SPP(18)); -nextP(3,8) = OP(1,8)*SPP(15) - OP(2,8)*SPP(4) + OP(3,8)*SPP(14) + OP(12,8)*SPP(23) + OP(15,8)*SPP(17) + dt*(OP(1,5)*SPP(15) - OP(2,5)*SPP(4) + OP(3,5)*SPP(14) + OP(12,5)*SPP(23) + OP(15,5)*SPP(17)); -nextP(4,8) = OP(4,8) + OP(1,8)*SPP(2) + OP(2,8)*SPP(20) + OP(3,8)*SPP(16) - OP(16,8)*SPP(22) + dt*(OP(4,5) + OP(1,5)*SPP(2) + OP(2,5)*SPP(20) + OP(3,5)*SPP(16) - OP(16,5)*SPP(22)); -nextP(5,8) = OP(5,8) + OP(16,8)*SF(23) + OP(1,8)*SPP(21) + OP(2,8)*SPP(13) + OP(3,8)*SPP(12) + dt*(OP(5,5) + OP(16,5)*SF(23) + OP(1,5)*SPP(21) + OP(2,5)*SPP(13) + OP(3,5)*SPP(12)); -nextP(6,8) = OP(6,8) + OP(16,8)*SF(21) - OP(1,8)*SPP(10) + OP(2,8)*SPP(11) + OP(3,8)*SPP(1) + dt*(OP(6,5) + OP(16,5)*SF(21) - OP(1,5)*SPP(10) + OP(2,5)*SPP(11) + OP(3,5)*SPP(1)); -nextP(7,8) = OP(7,8) + OP(4,8)*dt + dt*(OP(7,5) + OP(4,5)*dt); +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)*SPP(6) - OP(2,9)*SPP(5) + OP(3,9)*SPP(8) + OP(10,9)*SPP(23) + OP(13,9)*SPP(19) + dt*(OP(1,6)*SPP(6) - OP(2,6)*SPP(5) + OP(3,6)*SPP(8) + OP(10,6)*SPP(23) + OP(13,6)*SPP(19)); -nextP(2,9) = OP(2,9)*SPP(7) - OP(1,9)*SPP(3) - OP(3,9)*SPP(9) + OP(11,9)*SPP(23) + OP(14,9)*SPP(18) + dt*(OP(2,6)*SPP(7) - OP(1,6)*SPP(3) - OP(3,6)*SPP(9) + OP(11,6)*SPP(23) + OP(14,6)*SPP(18)); -nextP(3,9) = OP(1,9)*SPP(15) - OP(2,9)*SPP(4) + OP(3,9)*SPP(14) + OP(12,9)*SPP(23) + OP(15,9)*SPP(17) + dt*(OP(1,6)*SPP(15) - OP(2,6)*SPP(4) + OP(3,6)*SPP(14) + OP(12,6)*SPP(23) + OP(15,6)*SPP(17)); -nextP(4,9) = OP(4,9) + OP(1,9)*SPP(2) + OP(2,9)*SPP(20) + OP(3,9)*SPP(16) - OP(16,9)*SPP(22) + dt*(OP(4,6) + OP(1,6)*SPP(2) + OP(2,6)*SPP(20) + OP(3,6)*SPP(16) - OP(16,6)*SPP(22)); -nextP(5,9) = OP(5,9) + OP(16,9)*SF(23) + OP(1,9)*SPP(21) + OP(2,9)*SPP(13) + OP(3,9)*SPP(12) + dt*(OP(5,6) + OP(16,6)*SF(23) + OP(1,6)*SPP(21) + OP(2,6)*SPP(13) + OP(3,6)*SPP(12)); -nextP(6,9) = OP(6,9) + OP(16,9)*SF(21) - OP(1,9)*SPP(10) + OP(2,9)*SPP(11) + OP(3,9)*SPP(1) + dt*(OP(6,6) + OP(16,6)*SF(21) - OP(1,6)*SPP(10) + OP(2,6)*SPP(11) + OP(3,6)*SPP(1)); -nextP(7,9) = OP(7,9) + OP(4,9)*dt + dt*(OP(7,6) + OP(4,6)*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)*SPP(6) - OP(2,10)*SPP(5) + OP(3,10)*SPP(8) + OP(10,10)*SPP(23) + OP(13,10)*SPP(19); -nextP(2,10) = OP(2,10)*SPP(7) - OP(1,10)*SPP(3) - OP(3,10)*SPP(9) + OP(11,10)*SPP(23) + OP(14,10)*SPP(18); -nextP(3,10) = OP(1,10)*SPP(15) - OP(2,10)*SPP(4) + OP(3,10)*SPP(14) + OP(12,10)*SPP(23) + OP(15,10)*SPP(17); -nextP(4,10) = OP(4,10) + OP(1,10)*SPP(2) + OP(2,10)*SPP(20) + OP(3,10)*SPP(16) - OP(16,10)*SPP(22); -nextP(5,10) = OP(5,10) + OP(16,10)*SF(23) + OP(1,10)*SPP(21) + OP(2,10)*SPP(13) + OP(3,10)*SPP(12); -nextP(6,10) = OP(6,10) + OP(16,10)*SF(21) - OP(1,10)*SPP(10) + OP(2,10)*SPP(11) + OP(3,10)*SPP(1); -nextP(7,10) = OP(7,10) + OP(4,10)*dt; -nextP(8,10) = OP(8,10) + OP(5,10)*dt; -nextP(9,10) = OP(9,10) + OP(6,10)*dt; -nextP(10,10) = OP(10,10); -nextP(1,11) = OP(1,11)*SPP(6) - OP(2,11)*SPP(5) + OP(3,11)*SPP(8) + OP(10,11)*SPP(23) + OP(13,11)*SPP(19); -nextP(2,11) = OP(2,11)*SPP(7) - OP(1,11)*SPP(3) - OP(3,11)*SPP(9) + OP(11,11)*SPP(23) + OP(14,11)*SPP(18); -nextP(3,11) = OP(1,11)*SPP(15) - OP(2,11)*SPP(4) + OP(3,11)*SPP(14) + OP(12,11)*SPP(23) + OP(15,11)*SPP(17); -nextP(4,11) = OP(4,11) + OP(1,11)*SPP(2) + OP(2,11)*SPP(20) + OP(3,11)*SPP(16) - OP(16,11)*SPP(22); -nextP(5,11) = OP(5,11) + OP(16,11)*SF(23) + OP(1,11)*SPP(21) + OP(2,11)*SPP(13) + OP(3,11)*SPP(12); -nextP(6,11) = OP(6,11) + OP(16,11)*SF(21) - OP(1,11)*SPP(10) + OP(2,11)*SPP(11) + OP(3,11)*SPP(1); -nextP(7,11) = OP(7,11) + OP(4,11)*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); +nextP(10,11) = OP(10,11) + OP(7,11)*dt; nextP(11,11) = OP(11,11); -nextP(1,12) = OP(1,12)*SPP(6) - OP(2,12)*SPP(5) + OP(3,12)*SPP(8) + OP(10,12)*SPP(23) + OP(13,12)*SPP(19); -nextP(2,12) = OP(2,12)*SPP(7) - OP(1,12)*SPP(3) - OP(3,12)*SPP(9) + OP(11,12)*SPP(23) + OP(14,12)*SPP(18); -nextP(3,12) = OP(1,12)*SPP(15) - OP(2,12)*SPP(4) + OP(3,12)*SPP(14) + OP(12,12)*SPP(23) + OP(15,12)*SPP(17); -nextP(4,12) = OP(4,12) + OP(1,12)*SPP(2) + OP(2,12)*SPP(20) + OP(3,12)*SPP(16) - OP(16,12)*SPP(22); -nextP(5,12) = OP(5,12) + OP(16,12)*SF(23) + OP(1,12)*SPP(21) + OP(2,12)*SPP(13) + OP(3,12)*SPP(12); -nextP(6,12) = OP(6,12) + OP(16,12)*SF(21) - OP(1,12)*SPP(10) + OP(2,12)*SPP(11) + OP(3,12)*SPP(1); -nextP(7,12) = OP(7,12) + OP(4,12)*dt; +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); +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)*SPP(6) - OP(2,13)*SPP(5) + OP(3,13)*SPP(8) + OP(10,13)*SPP(23) + OP(13,13)*SPP(19); -nextP(2,13) = OP(2,13)*SPP(7) - OP(1,13)*SPP(3) - OP(3,13)*SPP(9) + OP(11,13)*SPP(23) + OP(14,13)*SPP(18); -nextP(3,13) = OP(1,13)*SPP(15) - OP(2,13)*SPP(4) + OP(3,13)*SPP(14) + OP(12,13)*SPP(23) + OP(15,13)*SPP(17); -nextP(4,13) = OP(4,13) + OP(1,13)*SPP(2) + OP(2,13)*SPP(20) + OP(3,13)*SPP(16) - OP(16,13)*SPP(22); -nextP(5,13) = OP(5,13) + OP(16,13)*SF(23) + OP(1,13)*SPP(21) + OP(2,13)*SPP(13) + OP(3,13)*SPP(12); -nextP(6,13) = OP(6,13) + OP(16,13)*SF(21) - OP(1,13)*SPP(10) + OP(2,13)*SPP(11) + OP(3,13)*SPP(1); -nextP(7,13) = OP(7,13) + OP(4,13)*dt; +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); +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)*SPP(6) - OP(2,14)*SPP(5) + OP(3,14)*SPP(8) + OP(10,14)*SPP(23) + OP(13,14)*SPP(19); -nextP(2,14) = OP(2,14)*SPP(7) - OP(1,14)*SPP(3) - OP(3,14)*SPP(9) + OP(11,14)*SPP(23) + OP(14,14)*SPP(18); -nextP(3,14) = OP(1,14)*SPP(15) - OP(2,14)*SPP(4) + OP(3,14)*SPP(14) + OP(12,14)*SPP(23) + OP(15,14)*SPP(17); -nextP(4,14) = OP(4,14) + OP(1,14)*SPP(2) + OP(2,14)*SPP(20) + OP(3,14)*SPP(16) - OP(16,14)*SPP(22); -nextP(5,14) = OP(5,14) + OP(16,14)*SF(23) + OP(1,14)*SPP(21) + OP(2,14)*SPP(13) + OP(3,14)*SPP(12); -nextP(6,14) = OP(6,14) + OP(16,14)*SF(21) - OP(1,14)*SPP(10) + OP(2,14)*SPP(11) + OP(3,14)*SPP(1); -nextP(7,14) = OP(7,14) + OP(4,14)*dt; +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); +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)*SPP(6) - OP(2,15)*SPP(5) + OP(3,15)*SPP(8) + OP(10,15)*SPP(23) + OP(13,15)*SPP(19); -nextP(2,15) = OP(2,15)*SPP(7) - OP(1,15)*SPP(3) - OP(3,15)*SPP(9) + OP(11,15)*SPP(23) + OP(14,15)*SPP(18); -nextP(3,15) = OP(1,15)*SPP(15) - OP(2,15)*SPP(4) + OP(3,15)*SPP(14) + OP(12,15)*SPP(23) + OP(15,15)*SPP(17); -nextP(4,15) = OP(4,15) + OP(1,15)*SPP(2) + OP(2,15)*SPP(20) + OP(3,15)*SPP(16) - OP(16,15)*SPP(22); -nextP(5,15) = OP(5,15) + OP(16,15)*SF(23) + OP(1,15)*SPP(21) + OP(2,15)*SPP(13) + OP(3,15)*SPP(12); -nextP(6,15) = OP(6,15) + OP(16,15)*SF(21) - OP(1,15)*SPP(10) + OP(2,15)*SPP(11) + OP(3,15)*SPP(1); -nextP(7,15) = OP(7,15) + OP(4,15)*dt; +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); +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)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19); -nextP(2,16) = OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18); -nextP(3,16) = OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17); -nextP(4,16) = OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22); -nextP(5,16) = OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12); -nextP(6,16) = OP(6,16) + OP(16,16)*SF(21) - OP(1,16)*SPP(10) + OP(2,16)*SPP(11) + OP(3,16)*SPP(1); -nextP(7,16) = OP(7,16) + OP(4,16)*dt; +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); +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)*SPP(6) - OP(2,17)*SPP(5) + OP(3,17)*SPP(8) + OP(10,17)*SPP(23) + OP(13,17)*SPP(19); -nextP(2,17) = OP(2,17)*SPP(7) - OP(1,17)*SPP(3) - OP(3,17)*SPP(9) + OP(11,17)*SPP(23) + OP(14,17)*SPP(18); -nextP(3,17) = OP(1,17)*SPP(15) - OP(2,17)*SPP(4) + OP(3,17)*SPP(14) + OP(12,17)*SPP(23) + OP(15,17)*SPP(17); -nextP(4,17) = OP(4,17) + OP(1,17)*SPP(2) + OP(2,17)*SPP(20) + OP(3,17)*SPP(16) - OP(16,17)*SPP(22); -nextP(5,17) = OP(5,17) + OP(16,17)*SF(23) + OP(1,17)*SPP(21) + OP(2,17)*SPP(13) + OP(3,17)*SPP(12); -nextP(6,17) = OP(6,17) + OP(16,17)*SF(21) - OP(1,17)*SPP(10) + OP(2,17)*SPP(11) + OP(3,17)*SPP(1); -nextP(7,17) = OP(7,17) + OP(4,17)*dt; +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); +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); @@ -219,16 +207,16 @@ 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)*SPP(6) - OP(2,18)*SPP(5) + OP(3,18)*SPP(8) + OP(10,18)*SPP(23) + OP(13,18)*SPP(19); -nextP(2,18) = OP(2,18)*SPP(7) - OP(1,18)*SPP(3) - OP(3,18)*SPP(9) + OP(11,18)*SPP(23) + OP(14,18)*SPP(18); -nextP(3,18) = OP(1,18)*SPP(15) - OP(2,18)*SPP(4) + OP(3,18)*SPP(14) + OP(12,18)*SPP(23) + OP(15,18)*SPP(17); -nextP(4,18) = OP(4,18) + OP(1,18)*SPP(2) + OP(2,18)*SPP(20) + OP(3,18)*SPP(16) - OP(16,18)*SPP(22); -nextP(5,18) = OP(5,18) + OP(16,18)*SF(23) + OP(1,18)*SPP(21) + OP(2,18)*SPP(13) + OP(3,18)*SPP(12); -nextP(6,18) = OP(6,18) + OP(16,18)*SF(21) - OP(1,18)*SPP(10) + OP(2,18)*SPP(11) + OP(3,18)*SPP(1); -nextP(7,18) = OP(7,18) + OP(4,18)*dt; +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); +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); @@ -237,16 +225,16 @@ 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)*SPP(6) - OP(2,19)*SPP(5) + OP(3,19)*SPP(8) + OP(10,19)*SPP(23) + OP(13,19)*SPP(19); -nextP(2,19) = OP(2,19)*SPP(7) - OP(1,19)*SPP(3) - OP(3,19)*SPP(9) + OP(11,19)*SPP(23) + OP(14,19)*SPP(18); -nextP(3,19) = OP(1,19)*SPP(15) - OP(2,19)*SPP(4) + OP(3,19)*SPP(14) + OP(12,19)*SPP(23) + OP(15,19)*SPP(17); -nextP(4,19) = OP(4,19) + OP(1,19)*SPP(2) + OP(2,19)*SPP(20) + OP(3,19)*SPP(16) - OP(16,19)*SPP(22); -nextP(5,19) = OP(5,19) + OP(16,19)*SF(23) + OP(1,19)*SPP(21) + OP(2,19)*SPP(13) + OP(3,19)*SPP(12); -nextP(6,19) = OP(6,19) + OP(16,19)*SF(21) - OP(1,19)*SPP(10) + OP(2,19)*SPP(11) + OP(3,19)*SPP(1); -nextP(7,19) = OP(7,19) + OP(4,19)*dt; +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); +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); @@ -256,16 +244,16 @@ 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)*SPP(6) - OP(2,20)*SPP(5) + OP(3,20)*SPP(8) + OP(10,20)*SPP(23) + OP(13,20)*SPP(19); -nextP(2,20) = OP(2,20)*SPP(7) - OP(1,20)*SPP(3) - OP(3,20)*SPP(9) + OP(11,20)*SPP(23) + OP(14,20)*SPP(18); -nextP(3,20) = OP(1,20)*SPP(15) - OP(2,20)*SPP(4) + OP(3,20)*SPP(14) + OP(12,20)*SPP(23) + OP(15,20)*SPP(17); -nextP(4,20) = OP(4,20) + OP(1,20)*SPP(2) + OP(2,20)*SPP(20) + OP(3,20)*SPP(16) - OP(16,20)*SPP(22); -nextP(5,20) = OP(5,20) + OP(16,20)*SF(23) + OP(1,20)*SPP(21) + OP(2,20)*SPP(13) + OP(3,20)*SPP(12); -nextP(6,20) = OP(6,20) + OP(16,20)*SF(21) - OP(1,20)*SPP(10) + OP(2,20)*SPP(11) + OP(3,20)*SPP(1); -nextP(7,20) = OP(7,20) + OP(4,20)*dt; +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); +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); @@ -276,16 +264,16 @@ 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)*SPP(6) - OP(2,21)*SPP(5) + OP(3,21)*SPP(8) + OP(10,21)*SPP(23) + OP(13,21)*SPP(19); -nextP(2,21) = OP(2,21)*SPP(7) - OP(1,21)*SPP(3) - OP(3,21)*SPP(9) + OP(11,21)*SPP(23) + OP(14,21)*SPP(18); -nextP(3,21) = OP(1,21)*SPP(15) - OP(2,21)*SPP(4) + OP(3,21)*SPP(14) + OP(12,21)*SPP(23) + OP(15,21)*SPP(17); -nextP(4,21) = OP(4,21) + OP(1,21)*SPP(2) + OP(2,21)*SPP(20) + OP(3,21)*SPP(16) - OP(16,21)*SPP(22); -nextP(5,21) = OP(5,21) + OP(16,21)*SF(23) + OP(1,21)*SPP(21) + OP(2,21)*SPP(13) + OP(3,21)*SPP(12); -nextP(6,21) = OP(6,21) + OP(16,21)*SF(21) - OP(1,21)*SPP(10) + OP(2,21)*SPP(11) + OP(3,21)*SPP(1); -nextP(7,21) = OP(7,21) + OP(4,21)*dt; +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); +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); @@ -297,16 +285,16 @@ 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)*SPP(6) - OP(2,22)*SPP(5) + OP(3,22)*SPP(8) + OP(10,22)*SPP(23) + OP(13,22)*SPP(19); -nextP(2,22) = OP(2,22)*SPP(7) - OP(1,22)*SPP(3) - OP(3,22)*SPP(9) + OP(11,22)*SPP(23) + OP(14,22)*SPP(18); -nextP(3,22) = OP(1,22)*SPP(15) - OP(2,22)*SPP(4) + OP(3,22)*SPP(14) + OP(12,22)*SPP(23) + OP(15,22)*SPP(17); -nextP(4,22) = OP(4,22) + OP(1,22)*SPP(2) + OP(2,22)*SPP(20) + OP(3,22)*SPP(16) - OP(16,22)*SPP(22); -nextP(5,22) = OP(5,22) + OP(16,22)*SF(23) + OP(1,22)*SPP(21) + OP(2,22)*SPP(13) + OP(3,22)*SPP(12); -nextP(6,22) = OP(6,22) + OP(16,22)*SF(21) - OP(1,22)*SPP(10) + OP(2,22)*SPP(11) + OP(3,22)*SPP(1); -nextP(7,22) = OP(7,22) + OP(4,22)*dt; +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); +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); @@ -319,16 +307,16 @@ 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)*SPP(6) - OP(2,23)*SPP(5) + OP(3,23)*SPP(8) + OP(10,23)*SPP(23) + OP(13,23)*SPP(19); -nextP(2,23) = OP(2,23)*SPP(7) - OP(1,23)*SPP(3) - OP(3,23)*SPP(9) + OP(11,23)*SPP(23) + OP(14,23)*SPP(18); -nextP(3,23) = OP(1,23)*SPP(15) - OP(2,23)*SPP(4) + OP(3,23)*SPP(14) + OP(12,23)*SPP(23) + OP(15,23)*SPP(17); -nextP(4,23) = OP(4,23) + OP(1,23)*SPP(2) + OP(2,23)*SPP(20) + OP(3,23)*SPP(16) - OP(16,23)*SPP(22); -nextP(5,23) = OP(5,23) + OP(16,23)*SF(23) + OP(1,23)*SPP(21) + OP(2,23)*SPP(13) + OP(3,23)*SPP(12); -nextP(6,23) = OP(6,23) + OP(16,23)*SF(21) - OP(1,23)*SPP(10) + OP(2,23)*SPP(11) + OP(3,23)*SPP(1); -nextP(7,23) = OP(7,23) + OP(4,23)*dt; +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); +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); @@ -342,16 +330,16 @@ 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)*SPP(6) - OP(2,24)*SPP(5) + OP(3,24)*SPP(8) + OP(10,24)*SPP(23) + OP(13,24)*SPP(19); -nextP(2,24) = OP(2,24)*SPP(7) - OP(1,24)*SPP(3) - OP(3,24)*SPP(9) + OP(11,24)*SPP(23) + OP(14,24)*SPP(18); -nextP(3,24) = OP(1,24)*SPP(15) - OP(2,24)*SPP(4) + OP(3,24)*SPP(14) + OP(12,24)*SPP(23) + OP(15,24)*SPP(17); -nextP(4,24) = OP(4,24) + OP(1,24)*SPP(2) + OP(2,24)*SPP(20) + OP(3,24)*SPP(16) - OP(16,24)*SPP(22); -nextP(5,24) = OP(5,24) + OP(16,24)*SF(23) + OP(1,24)*SPP(21) + OP(2,24)*SPP(13) + OP(3,24)*SPP(12); -nextP(6,24) = OP(6,24) + OP(16,24)*SF(21) - OP(1,24)*SPP(10) + OP(2,24)*SPP(11) + OP(3,24)*SPP(1); -nextP(7,24) = OP(7,24) + OP(4,24)*dt; +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); +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); @@ -371,311 +359,328 @@ 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,4) = SH_TAS(3); -H_TAS(1,5) = SH_TAS(2); -H_TAS(1,6) = vd*SH_TAS(1); +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(4,4)*SH_TAS(3) + OP(5,4)*SH_TAS(2) - OP(23,4)*SH_TAS(3) - OP(24,4)*SH_TAS(2) + OP(6,4)*vd*SH_TAS(1)) + SH_TAS(2)*(OP(4,5)*SH_TAS(3) + OP(5,5)*SH_TAS(2) - OP(23,5)*SH_TAS(3) - OP(24,5)*SH_TAS(2) + OP(6,5)*vd*SH_TAS(1)) - SH_TAS(3)*(OP(4,23)*SH_TAS(3) + OP(5,23)*SH_TAS(2) - OP(23,23)*SH_TAS(3) - OP(24,23)*SH_TAS(2) + OP(6,23)*vd*SH_TAS(1)) - SH_TAS(2)*(OP(4,24)*SH_TAS(3) + OP(5,24)*SH_TAS(2) - OP(23,24)*SH_TAS(3) - OP(24,24)*SH_TAS(2) + OP(6,24)*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP(4,6)*SH_TAS(3) + OP(5,6)*SH_TAS(2) - OP(23,6)*SH_TAS(3) - OP(24,6)*SH_TAS(2) + OP(6,6)*vd*SH_TAS(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,4)*SH_TAS(3) - OP(1,23)*SH_TAS(3) + OP(1,5)*SK_TAS(2) - OP(1,24)*SK_TAS(2) + OP(1,6)*vd*SH_TAS(1)); -Kfusion(2) = SK_TAS(1)*(OP(2,4)*SH_TAS(3) - OP(2,23)*SH_TAS(3) + OP(2,5)*SK_TAS(2) - OP(2,24)*SK_TAS(2) + OP(2,6)*vd*SH_TAS(1)); -Kfusion(3) = SK_TAS(1)*(OP(3,4)*SH_TAS(3) - OP(3,23)*SH_TAS(3) + OP(3,5)*SK_TAS(2) - OP(3,24)*SK_TAS(2) + OP(3,6)*vd*SH_TAS(1)); -Kfusion(4) = SK_TAS(1)*(OP(4,4)*SH_TAS(3) - OP(4,23)*SH_TAS(3) + OP(4,5)*SK_TAS(2) - OP(4,24)*SK_TAS(2) + OP(4,6)*vd*SH_TAS(1)); -Kfusion(5) = SK_TAS(1)*(OP(5,4)*SH_TAS(3) - OP(5,23)*SH_TAS(3) + OP(5,5)*SK_TAS(2) - OP(5,24)*SK_TAS(2) + OP(5,6)*vd*SH_TAS(1)); -Kfusion(6) = SK_TAS(1)*(OP(6,4)*SH_TAS(3) - OP(6,23)*SH_TAS(3) + OP(6,5)*SK_TAS(2) - OP(6,24)*SK_TAS(2) + OP(6,6)*vd*SH_TAS(1)); -Kfusion(7) = SK_TAS(1)*(OP(7,4)*SH_TAS(3) - OP(7,23)*SH_TAS(3) + OP(7,5)*SK_TAS(2) - OP(7,24)*SK_TAS(2) + OP(7,6)*vd*SH_TAS(1)); -Kfusion(8) = SK_TAS(1)*(OP(8,4)*SH_TAS(3) - OP(8,23)*SH_TAS(3) + OP(8,5)*SK_TAS(2) - OP(8,24)*SK_TAS(2) + OP(8,6)*vd*SH_TAS(1)); -Kfusion(9) = SK_TAS(1)*(OP(9,4)*SH_TAS(3) - OP(9,23)*SH_TAS(3) + OP(9,5)*SK_TAS(2) - OP(9,24)*SK_TAS(2) + OP(9,6)*vd*SH_TAS(1)); -Kfusion(10) = SK_TAS(1)*(OP(10,4)*SH_TAS(3) - OP(10,23)*SH_TAS(3) + OP(10,5)*SK_TAS(2) - OP(10,24)*SK_TAS(2) + OP(10,6)*vd*SH_TAS(1)); -Kfusion(11) = SK_TAS(1)*(OP(11,4)*SH_TAS(3) - OP(11,23)*SH_TAS(3) + OP(11,5)*SK_TAS(2) - OP(11,24)*SK_TAS(2) + OP(11,6)*vd*SH_TAS(1)); -Kfusion(12) = SK_TAS(1)*(OP(12,4)*SH_TAS(3) - OP(12,23)*SH_TAS(3) + OP(12,5)*SK_TAS(2) - OP(12,24)*SK_TAS(2) + OP(12,6)*vd*SH_TAS(1)); -Kfusion(13) = SK_TAS(1)*(OP(13,4)*SH_TAS(3) - OP(13,23)*SH_TAS(3) + OP(13,5)*SK_TAS(2) - OP(13,24)*SK_TAS(2) + OP(13,6)*vd*SH_TAS(1)); -Kfusion(14) = SK_TAS(1)*(OP(14,4)*SH_TAS(3) - OP(14,23)*SH_TAS(3) + OP(14,5)*SK_TAS(2) - OP(14,24)*SK_TAS(2) + OP(14,6)*vd*SH_TAS(1)); -Kfusion(15) = SK_TAS(1)*(OP(15,4)*SH_TAS(3) - OP(15,23)*SH_TAS(3) + OP(15,5)*SK_TAS(2) - OP(15,24)*SK_TAS(2) + OP(15,6)*vd*SH_TAS(1)); -Kfusion(16) = SK_TAS(1)*(OP(16,4)*SH_TAS(3) - OP(16,23)*SH_TAS(3) + OP(16,5)*SK_TAS(2) - OP(16,24)*SK_TAS(2) + OP(16,6)*vd*SH_TAS(1)); -Kfusion(17) = SK_TAS(1)*(OP(17,4)*SH_TAS(3) - OP(17,23)*SH_TAS(3) + OP(17,5)*SK_TAS(2) - OP(17,24)*SK_TAS(2) + OP(17,6)*vd*SH_TAS(1)); -Kfusion(18) = SK_TAS(1)*(OP(18,4)*SH_TAS(3) - OP(18,23)*SH_TAS(3) + OP(18,5)*SK_TAS(2) - OP(18,24)*SK_TAS(2) + OP(18,6)*vd*SH_TAS(1)); -Kfusion(19) = SK_TAS(1)*(OP(19,4)*SH_TAS(3) - OP(19,23)*SH_TAS(3) + OP(19,5)*SK_TAS(2) - OP(19,24)*SK_TAS(2) + OP(19,6)*vd*SH_TAS(1)); -Kfusion(20) = SK_TAS(1)*(OP(20,4)*SH_TAS(3) - OP(20,23)*SH_TAS(3) + OP(20,5)*SK_TAS(2) - OP(20,24)*SK_TAS(2) + OP(20,6)*vd*SH_TAS(1)); -Kfusion(21) = SK_TAS(1)*(OP(21,4)*SH_TAS(3) - OP(21,23)*SH_TAS(3) + OP(21,5)*SK_TAS(2) - OP(21,24)*SK_TAS(2) + OP(21,6)*vd*SH_TAS(1)); -Kfusion(22) = SK_TAS(1)*(OP(22,4)*SH_TAS(3) - OP(22,23)*SH_TAS(3) + OP(22,5)*SK_TAS(2) - OP(22,24)*SK_TAS(2) + OP(22,6)*vd*SH_TAS(1)); -Kfusion(23) = SK_TAS(1)*(OP(23,4)*SH_TAS(3) - OP(23,23)*SH_TAS(3) + OP(23,5)*SK_TAS(2) - OP(23,24)*SK_TAS(2) + OP(23,6)*vd*SH_TAS(1)); -Kfusion(24) = SK_TAS(1)*(OP(24,4)*SH_TAS(3) - OP(24,23)*SH_TAS(3) + OP(24,5)*SK_TAS(2) - OP(24,24)*SK_TAS(2) + OP(24,6)*vd*SH_TAS(1)); -SH_BETA = zeros(10,1); -SH_BETA(1) = (2*conj(q0)*conj(q3) + 2*conj(q1)*conj(q2))*(ve - vwe) - vd*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)) + (vn - vwn)*(conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2); -SH_BETA(2) = vd*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) - (2*conj(q0)*conj(q3) - 2*conj(q1)*conj(q2))*(vn - vwn) + (ve - vwe)*(conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2); -SH_BETA(3) = (2*conj(q0)*conj(q2) + 2*conj(q1)*conj(q3))*(vn - vwn) - (2*conj(q0)*conj(q1) - 2*conj(q2)*conj(q3))*(ve - vwe) + vd*(conj(q0)^2 - conj(q1)^2 - conj(q2)^2 + conj(q3)^2); -SH_BETA(4) = (conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2)/SH_BETA(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) = conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2; -SH_BETA(7) = 2*conj(q0)*conj(q3); -SH_BETA(8) = 1/SH_BETA(1); -SH_BETA(9) = SH_BETA(7) + 2*conj(q1)*conj(q2); -SH_BETA(10) = SH_BETA(7) - 2*conj(q1)*conj(q2); +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(3)*SH_BETA(8); -H_BETA(1,2) = SH_BETA(2)*SH_BETA(3)*SH_BETA(5); -H_BETA(1,3) = - SH_BETA(2)^2*SH_BETA(5) - 1; -H_BETA(1,4) = - SH_BETA(8)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -H_BETA(1,5) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -H_BETA(1,6) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -H_BETA(1,23) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*SH_BETA(9) - SH_BETA(4); -SK_BETA = zeros(6,1); -SK_BETA(1) = 1/(R_BETA + (SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)))*(OP(23,6)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,6)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,6)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,6)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,6)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,6)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,6)*SH_BETA(3)*SH_BETA(8) + OP(2,6)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP(23,5)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,5)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,5)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,5)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,5)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,5)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,5)*SH_BETA(3)*SH_BETA(8) + OP(2,5)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP(23,24)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,24)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,24)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,24)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,24)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,24)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,24)*SH_BETA(3)*SH_BETA(8) + OP(2,24)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP(23,4)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,4)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,4)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,4)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,4)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,4)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,4)*SH_BETA(3)*SH_BETA(8) + OP(2,4)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP(23,23)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,23)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,23)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,23)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,23)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,23)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,23)*SH_BETA(3)*SH_BETA(8) + OP(2,23)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(2)^2*SH_BETA(5) + 1)*(OP(23,3)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,3)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,3)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,3)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,3)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,3)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,3)*SH_BETA(3)*SH_BETA(8) + OP(2,3)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(3)*SH_BETA(8)*(OP(23,1)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,1)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,1)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,1)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,1)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,1)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,1)*SH_BETA(3)*SH_BETA(8) + OP(2,1)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(2)*SH_BETA(3)*SH_BETA(5)*(OP(23,2)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,2)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,2)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,2)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,2)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,2)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,2)*SH_BETA(3)*SH_BETA(8) + OP(2,2)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5))); -SK_BETA(2) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -SK_BETA(3) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -SK_BETA(4) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -SK_BETA(5) = SH_BETA(2)^2*SH_BETA(5) + 1; -SK_BETA(6) = SH_BETA(3); +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,6)*SK_BETA(2) - OP(1,3)*SK_BETA(5) - OP(1,4)*SK_BETA(3) + OP(1,5)*SK_BETA(4) + OP(1,23)*SK_BETA(3) - OP(1,24)*SK_BETA(4) + OP(1,1)*SH_BETA(8)*SK_BETA(6) + OP(1,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(2) = SK_BETA(1)*(OP(2,6)*SK_BETA(2) - OP(2,3)*SK_BETA(5) - OP(2,4)*SK_BETA(3) + OP(2,5)*SK_BETA(4) + OP(2,23)*SK_BETA(3) - OP(2,24)*SK_BETA(4) + OP(2,1)*SH_BETA(8)*SK_BETA(6) + OP(2,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(3) = SK_BETA(1)*(OP(3,6)*SK_BETA(2) - OP(3,3)*SK_BETA(5) - OP(3,4)*SK_BETA(3) + OP(3,5)*SK_BETA(4) + OP(3,23)*SK_BETA(3) - OP(3,24)*SK_BETA(4) + OP(3,1)*SH_BETA(8)*SK_BETA(6) + OP(3,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(4) = SK_BETA(1)*(OP(4,6)*SK_BETA(2) - OP(4,3)*SK_BETA(5) - OP(4,4)*SK_BETA(3) + OP(4,5)*SK_BETA(4) + OP(4,23)*SK_BETA(3) - OP(4,24)*SK_BETA(4) + OP(4,1)*SH_BETA(8)*SK_BETA(6) + OP(4,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(5) = SK_BETA(1)*(OP(5,6)*SK_BETA(2) - OP(5,3)*SK_BETA(5) - OP(5,4)*SK_BETA(3) + OP(5,5)*SK_BETA(4) + OP(5,23)*SK_BETA(3) - OP(5,24)*SK_BETA(4) + OP(5,1)*SH_BETA(8)*SK_BETA(6) + OP(5,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(6) = SK_BETA(1)*(OP(6,6)*SK_BETA(2) - OP(6,3)*SK_BETA(5) - OP(6,4)*SK_BETA(3) + OP(6,5)*SK_BETA(4) + OP(6,23)*SK_BETA(3) - OP(6,24)*SK_BETA(4) + OP(6,1)*SH_BETA(8)*SK_BETA(6) + OP(6,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(7) = SK_BETA(1)*(OP(7,6)*SK_BETA(2) - OP(7,3)*SK_BETA(5) - OP(7,4)*SK_BETA(3) + OP(7,5)*SK_BETA(4) + OP(7,23)*SK_BETA(3) - OP(7,24)*SK_BETA(4) + OP(7,1)*SH_BETA(8)*SK_BETA(6) + OP(7,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(8) = SK_BETA(1)*(OP(8,6)*SK_BETA(2) - OP(8,3)*SK_BETA(5) - OP(8,4)*SK_BETA(3) + OP(8,5)*SK_BETA(4) + OP(8,23)*SK_BETA(3) - OP(8,24)*SK_BETA(4) + OP(8,1)*SH_BETA(8)*SK_BETA(6) + OP(8,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(9) = SK_BETA(1)*(OP(9,6)*SK_BETA(2) - OP(9,3)*SK_BETA(5) - OP(9,4)*SK_BETA(3) + OP(9,5)*SK_BETA(4) + OP(9,23)*SK_BETA(3) - OP(9,24)*SK_BETA(4) + OP(9,1)*SH_BETA(8)*SK_BETA(6) + OP(9,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(10) = SK_BETA(1)*(OP(10,6)*SK_BETA(2) - OP(10,3)*SK_BETA(5) - OP(10,4)*SK_BETA(3) + OP(10,5)*SK_BETA(4) + OP(10,23)*SK_BETA(3) - OP(10,24)*SK_BETA(4) + OP(10,1)*SH_BETA(8)*SK_BETA(6) + OP(10,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(11) = SK_BETA(1)*(OP(11,6)*SK_BETA(2) - OP(11,3)*SK_BETA(5) - OP(11,4)*SK_BETA(3) + OP(11,5)*SK_BETA(4) + OP(11,23)*SK_BETA(3) - OP(11,24)*SK_BETA(4) + OP(11,1)*SH_BETA(8)*SK_BETA(6) + OP(11,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(12) = SK_BETA(1)*(OP(12,6)*SK_BETA(2) - OP(12,3)*SK_BETA(5) - OP(12,4)*SK_BETA(3) + OP(12,5)*SK_BETA(4) + OP(12,23)*SK_BETA(3) - OP(12,24)*SK_BETA(4) + OP(12,1)*SH_BETA(8)*SK_BETA(6) + OP(12,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(13) = SK_BETA(1)*(OP(13,6)*SK_BETA(2) - OP(13,3)*SK_BETA(5) - OP(13,4)*SK_BETA(3) + OP(13,5)*SK_BETA(4) + OP(13,23)*SK_BETA(3) - OP(13,24)*SK_BETA(4) + OP(13,1)*SH_BETA(8)*SK_BETA(6) + OP(13,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(14) = SK_BETA(1)*(OP(14,6)*SK_BETA(2) - OP(14,3)*SK_BETA(5) - OP(14,4)*SK_BETA(3) + OP(14,5)*SK_BETA(4) + OP(14,23)*SK_BETA(3) - OP(14,24)*SK_BETA(4) + OP(14,1)*SH_BETA(8)*SK_BETA(6) + OP(14,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(15) = SK_BETA(1)*(OP(15,6)*SK_BETA(2) - OP(15,3)*SK_BETA(5) - OP(15,4)*SK_BETA(3) + OP(15,5)*SK_BETA(4) + OP(15,23)*SK_BETA(3) - OP(15,24)*SK_BETA(4) + OP(15,1)*SH_BETA(8)*SK_BETA(6) + OP(15,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(16) = SK_BETA(1)*(OP(16,6)*SK_BETA(2) - OP(16,3)*SK_BETA(5) - OP(16,4)*SK_BETA(3) + OP(16,5)*SK_BETA(4) + OP(16,23)*SK_BETA(3) - OP(16,24)*SK_BETA(4) + OP(16,1)*SH_BETA(8)*SK_BETA(6) + OP(16,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(17) = SK_BETA(1)*(OP(17,6)*SK_BETA(2) - OP(17,3)*SK_BETA(5) - OP(17,4)*SK_BETA(3) + OP(17,5)*SK_BETA(4) + OP(17,23)*SK_BETA(3) - OP(17,24)*SK_BETA(4) + OP(17,1)*SH_BETA(8)*SK_BETA(6) + OP(17,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(18) = SK_BETA(1)*(OP(18,6)*SK_BETA(2) - OP(18,3)*SK_BETA(5) - OP(18,4)*SK_BETA(3) + OP(18,5)*SK_BETA(4) + OP(18,23)*SK_BETA(3) - OP(18,24)*SK_BETA(4) + OP(18,1)*SH_BETA(8)*SK_BETA(6) + OP(18,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(19) = SK_BETA(1)*(OP(19,6)*SK_BETA(2) - OP(19,3)*SK_BETA(5) - OP(19,4)*SK_BETA(3) + OP(19,5)*SK_BETA(4) + OP(19,23)*SK_BETA(3) - OP(19,24)*SK_BETA(4) + OP(19,1)*SH_BETA(8)*SK_BETA(6) + OP(19,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(20) = SK_BETA(1)*(OP(20,6)*SK_BETA(2) - OP(20,3)*SK_BETA(5) - OP(20,4)*SK_BETA(3) + OP(20,5)*SK_BETA(4) + OP(20,23)*SK_BETA(3) - OP(20,24)*SK_BETA(4) + OP(20,1)*SH_BETA(8)*SK_BETA(6) + OP(20,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(21) = SK_BETA(1)*(OP(21,6)*SK_BETA(2) - OP(21,3)*SK_BETA(5) - OP(21,4)*SK_BETA(3) + OP(21,5)*SK_BETA(4) + OP(21,23)*SK_BETA(3) - OP(21,24)*SK_BETA(4) + OP(21,1)*SH_BETA(8)*SK_BETA(6) + OP(21,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(22) = SK_BETA(1)*(OP(22,6)*SK_BETA(2) - OP(22,3)*SK_BETA(5) - OP(22,4)*SK_BETA(3) + OP(22,5)*SK_BETA(4) + OP(22,23)*SK_BETA(3) - OP(22,24)*SK_BETA(4) + OP(22,1)*SH_BETA(8)*SK_BETA(6) + OP(22,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(23) = SK_BETA(1)*(OP(23,6)*SK_BETA(2) - OP(23,3)*SK_BETA(5) - OP(23,4)*SK_BETA(3) + OP(23,5)*SK_BETA(4) + OP(23,23)*SK_BETA(3) - OP(23,24)*SK_BETA(4) + OP(23,1)*SH_BETA(8)*SK_BETA(6) + OP(23,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(24) = SK_BETA(1)*(OP(24,6)*SK_BETA(2) - OP(24,3)*SK_BETA(5) - OP(24,4)*SK_BETA(3) + OP(24,5)*SK_BETA(4) + OP(24,23)*SK_BETA(3) - OP(24,24)*SK_BETA(4) + OP(24,1)*SH_BETA(8)*SK_BETA(6) + OP(24,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); +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) = q0^2 - q1^2 + q2^2 - q3^2; -SH_MAG(2) = q0^2 + q1^2 - q2^2 - q3^2; -SH_MAG(3) = q0^2 - q1^2 - q2^2 + q3^2; -SH_MAG(4) = 2*q0*q1 + 2*q2*q3; -SH_MAG(5) = 2*q0*q3 + 2*q1*q2; -SH_MAG(6) = 2*q0*q2 + 2*q1*q3; -SH_MAG(7) = magE*(2*q0*q1 - 2*q2*q3); -SH_MAG(8) = 2*q1*q3 - 2*q0*q2; -SH_MAG(9) = 2*q0*q3; +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(2) = SH_MAG(7) - magD*SH_MAG(3) - magN*SH_MAG(6); -H_MAG(3) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -H_MAG(17) = SH_MAG(2); -H_MAG(18) = SH_MAG(5); -H_MAG(19) = SH_MAG(8); +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(4,1); -SK_MX(1) = 1/(OP(20,20) + R_MAG - OP(2,20)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,20)*SH_MAG(2) + OP(18,20)*SH_MAG(5) + OP(19,20)*SH_MAG(8) + OP(3,20)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) - (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP(20,2) - OP(2,2)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,2)*SH_MAG(2) + OP(18,2)*SH_MAG(5) + OP(19,2)*SH_MAG(8) + OP(3,2)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(2)*(OP(20,17) - OP(2,17)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,17)*SH_MAG(2) + OP(18,17)*SH_MAG(5) + OP(19,17)*SH_MAG(8) + OP(3,17)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(5)*(OP(20,18) - OP(2,18)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,18)*SH_MAG(2) + OP(18,18)*SH_MAG(5) + OP(19,18)*SH_MAG(8) + OP(3,18)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(8)*(OP(20,19) - OP(2,19)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,19)*SH_MAG(2) + OP(18,19)*SH_MAG(5) + OP(19,19)*SH_MAG(8) + OP(3,19)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP(20,3) - OP(2,3)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,3)*SH_MAG(2) + OP(18,3)*SH_MAG(5) + OP(19,3)*SH_MAG(8) + OP(3,3)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)))); -SK_MX(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -SK_MX(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -SK_MX(4) = SH_MAG(8); +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,17)*SH_MAG(2) + OP(1,18)*SH_MAG(5) - OP(1,2)*SK_MX(3) + OP(1,3)*SK_MX(2) + OP(1,19)*SK_MX(4)); -Kfusion(2) = SK_MX(1)*(OP(2,20) + OP(2,17)*SH_MAG(2) + OP(2,18)*SH_MAG(5) - OP(2,2)*SK_MX(3) + OP(2,3)*SK_MX(2) + OP(2,19)*SK_MX(4)); -Kfusion(3) = SK_MX(1)*(OP(3,20) + OP(3,17)*SH_MAG(2) + OP(3,18)*SH_MAG(5) - OP(3,2)*SK_MX(3) + OP(3,3)*SK_MX(2) + OP(3,19)*SK_MX(4)); -Kfusion(4) = SK_MX(1)*(OP(4,20) + OP(4,17)*SH_MAG(2) + OP(4,18)*SH_MAG(5) - OP(4,2)*SK_MX(3) + OP(4,3)*SK_MX(2) + OP(4,19)*SK_MX(4)); -Kfusion(5) = SK_MX(1)*(OP(5,20) + OP(5,17)*SH_MAG(2) + OP(5,18)*SH_MAG(5) - OP(5,2)*SK_MX(3) + OP(5,3)*SK_MX(2) + OP(5,19)*SK_MX(4)); -Kfusion(6) = SK_MX(1)*(OP(6,20) + OP(6,17)*SH_MAG(2) + OP(6,18)*SH_MAG(5) - OP(6,2)*SK_MX(3) + OP(6,3)*SK_MX(2) + OP(6,19)*SK_MX(4)); -Kfusion(7) = SK_MX(1)*(OP(7,20) + OP(7,17)*SH_MAG(2) + OP(7,18)*SH_MAG(5) - OP(7,2)*SK_MX(3) + OP(7,3)*SK_MX(2) + OP(7,19)*SK_MX(4)); -Kfusion(8) = SK_MX(1)*(OP(8,20) + OP(8,17)*SH_MAG(2) + OP(8,18)*SH_MAG(5) - OP(8,2)*SK_MX(3) + OP(8,3)*SK_MX(2) + OP(8,19)*SK_MX(4)); -Kfusion(9) = SK_MX(1)*(OP(9,20) + OP(9,17)*SH_MAG(2) + OP(9,18)*SH_MAG(5) - OP(9,2)*SK_MX(3) + OP(9,3)*SK_MX(2) + OP(9,19)*SK_MX(4)); -Kfusion(10) = SK_MX(1)*(OP(10,20) + OP(10,17)*SH_MAG(2) + OP(10,18)*SH_MAG(5) - OP(10,2)*SK_MX(3) + OP(10,3)*SK_MX(2) + OP(10,19)*SK_MX(4)); -Kfusion(11) = SK_MX(1)*(OP(11,20) + OP(11,17)*SH_MAG(2) + OP(11,18)*SH_MAG(5) - OP(11,2)*SK_MX(3) + OP(11,3)*SK_MX(2) + OP(11,19)*SK_MX(4)); -Kfusion(12) = SK_MX(1)*(OP(12,20) + OP(12,17)*SH_MAG(2) + OP(12,18)*SH_MAG(5) - OP(12,2)*SK_MX(3) + OP(12,3)*SK_MX(2) + OP(12,19)*SK_MX(4)); -Kfusion(13) = SK_MX(1)*(OP(13,20) + OP(13,17)*SH_MAG(2) + OP(13,18)*SH_MAG(5) - OP(13,2)*SK_MX(3) + OP(13,3)*SK_MX(2) + OP(13,19)*SK_MX(4)); -Kfusion(14) = SK_MX(1)*(OP(14,20) + OP(14,17)*SH_MAG(2) + OP(14,18)*SH_MAG(5) - OP(14,2)*SK_MX(3) + OP(14,3)*SK_MX(2) + OP(14,19)*SK_MX(4)); -Kfusion(15) = SK_MX(1)*(OP(15,20) + OP(15,17)*SH_MAG(2) + OP(15,18)*SH_MAG(5) - OP(15,2)*SK_MX(3) + OP(15,3)*SK_MX(2) + OP(15,19)*SK_MX(4)); -Kfusion(16) = SK_MX(1)*(OP(16,20) + OP(16,17)*SH_MAG(2) + OP(16,18)*SH_MAG(5) - OP(16,2)*SK_MX(3) + OP(16,3)*SK_MX(2) + OP(16,19)*SK_MX(4)); -Kfusion(17) = SK_MX(1)*(OP(17,20) + OP(17,17)*SH_MAG(2) + OP(17,18)*SH_MAG(5) - OP(17,2)*SK_MX(3) + OP(17,3)*SK_MX(2) + OP(17,19)*SK_MX(4)); -Kfusion(18) = SK_MX(1)*(OP(18,20) + OP(18,17)*SH_MAG(2) + OP(18,18)*SH_MAG(5) - OP(18,2)*SK_MX(3) + OP(18,3)*SK_MX(2) + OP(18,19)*SK_MX(4)); -Kfusion(19) = SK_MX(1)*(OP(19,20) + OP(19,17)*SH_MAG(2) + OP(19,18)*SH_MAG(5) - OP(19,2)*SK_MX(3) + OP(19,3)*SK_MX(2) + OP(19,19)*SK_MX(4)); -Kfusion(20) = SK_MX(1)*(OP(20,20) + OP(20,17)*SH_MAG(2) + OP(20,18)*SH_MAG(5) - OP(20,2)*SK_MX(3) + OP(20,3)*SK_MX(2) + OP(20,19)*SK_MX(4)); -Kfusion(21) = SK_MX(1)*(OP(21,20) + OP(21,17)*SH_MAG(2) + OP(21,18)*SH_MAG(5) - OP(21,2)*SK_MX(3) + OP(21,3)*SK_MX(2) + OP(21,19)*SK_MX(4)); -Kfusion(22) = SK_MX(1)*(OP(22,20) + OP(22,17)*SH_MAG(2) + OP(22,18)*SH_MAG(5) - OP(22,2)*SK_MX(3) + OP(22,3)*SK_MX(2) + OP(22,19)*SK_MX(4)); -Kfusion(23) = SK_MX(1)*(OP(23,20) + OP(23,17)*SH_MAG(2) + OP(23,18)*SH_MAG(5) - OP(23,2)*SK_MX(3) + OP(23,3)*SK_MX(2) + OP(23,19)*SK_MX(4)); -Kfusion(24) = SK_MX(1)*(OP(24,20) + OP(24,17)*SH_MAG(2) + OP(24,18)*SH_MAG(5) - OP(24,2)*SK_MX(3) + OP(24,3)*SK_MX(2) + OP(24,19)*SK_MX(4)); +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) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -H_MAG(3) = - magE*SH_MAG(5) - magD*SH_MAG(8) - magN*SH_MAG(2); -H_MAG(17) = 2*q1*q2 - SH_MAG(9); -H_MAG(18) = SH_MAG(1); -H_MAG(19) = SH_MAG(4); +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(4,1); -SK_MY(1) = 1/(OP(21,21) + R_MAG + OP(1,21)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,21)*SH_MAG(1) + OP(19,21)*SH_MAG(4) - (SH_MAG(9) - 2*q1*q2)*(OP(21,17) + OP(1,17)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,17)*SH_MAG(1) + OP(19,17)*SH_MAG(4) - OP(3,17)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,17)*(SH_MAG(9) - 2*q1*q2)) - OP(3,21)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP(21,1) + OP(1,1)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,1)*SH_MAG(1) + OP(19,1)*SH_MAG(4) - OP(3,1)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,1)*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(1)*(OP(21,18) + OP(1,18)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,18)*SH_MAG(1) + OP(19,18)*SH_MAG(4) - OP(3,18)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,18)*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(4)*(OP(21,19) + OP(1,19)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,19)*SH_MAG(1) + OP(19,19)*SH_MAG(4) - OP(3,19)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,19)*(SH_MAG(9) - 2*q1*q2)) - OP(17,21)*(SH_MAG(9) - 2*q1*q2) - (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP(21,3) + OP(1,3)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,3)*SH_MAG(1) + OP(19,3)*SH_MAG(4) - OP(3,3)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,3)*(SH_MAG(9) - 2*q1*q2))); -SK_MY(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); -SK_MY(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -SK_MY(4) = SH_MAG(9) - 2*q1*q2; +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,18)*SH_MAG(1) + OP(1,19)*SH_MAG(4) + OP(1,1)*SK_MY(3) - OP(1,3)*SK_MY(2) - OP(1,17)*SK_MY(4)); -Kfusion(2) = SK_MY(1)*(OP(2,21) + OP(2,18)*SH_MAG(1) + OP(2,19)*SH_MAG(4) + OP(2,1)*SK_MY(3) - OP(2,3)*SK_MY(2) - OP(2,17)*SK_MY(4)); -Kfusion(3) = SK_MY(1)*(OP(3,21) + OP(3,18)*SH_MAG(1) + OP(3,19)*SH_MAG(4) + OP(3,1)*SK_MY(3) - OP(3,3)*SK_MY(2) - OP(3,17)*SK_MY(4)); -Kfusion(4) = SK_MY(1)*(OP(4,21) + OP(4,18)*SH_MAG(1) + OP(4,19)*SH_MAG(4) + OP(4,1)*SK_MY(3) - OP(4,3)*SK_MY(2) - OP(4,17)*SK_MY(4)); -Kfusion(5) = SK_MY(1)*(OP(5,21) + OP(5,18)*SH_MAG(1) + OP(5,19)*SH_MAG(4) + OP(5,1)*SK_MY(3) - OP(5,3)*SK_MY(2) - OP(5,17)*SK_MY(4)); -Kfusion(6) = SK_MY(1)*(OP(6,21) + OP(6,18)*SH_MAG(1) + OP(6,19)*SH_MAG(4) + OP(6,1)*SK_MY(3) - OP(6,3)*SK_MY(2) - OP(6,17)*SK_MY(4)); -Kfusion(7) = SK_MY(1)*(OP(7,21) + OP(7,18)*SH_MAG(1) + OP(7,19)*SH_MAG(4) + OP(7,1)*SK_MY(3) - OP(7,3)*SK_MY(2) - OP(7,17)*SK_MY(4)); -Kfusion(8) = SK_MY(1)*(OP(8,21) + OP(8,18)*SH_MAG(1) + OP(8,19)*SH_MAG(4) + OP(8,1)*SK_MY(3) - OP(8,3)*SK_MY(2) - OP(8,17)*SK_MY(4)); -Kfusion(9) = SK_MY(1)*(OP(9,21) + OP(9,18)*SH_MAG(1) + OP(9,19)*SH_MAG(4) + OP(9,1)*SK_MY(3) - OP(9,3)*SK_MY(2) - OP(9,17)*SK_MY(4)); -Kfusion(10) = SK_MY(1)*(OP(10,21) + OP(10,18)*SH_MAG(1) + OP(10,19)*SH_MAG(4) + OP(10,1)*SK_MY(3) - OP(10,3)*SK_MY(2) - OP(10,17)*SK_MY(4)); -Kfusion(11) = SK_MY(1)*(OP(11,21) + OP(11,18)*SH_MAG(1) + OP(11,19)*SH_MAG(4) + OP(11,1)*SK_MY(3) - OP(11,3)*SK_MY(2) - OP(11,17)*SK_MY(4)); -Kfusion(12) = SK_MY(1)*(OP(12,21) + OP(12,18)*SH_MAG(1) + OP(12,19)*SH_MAG(4) + OP(12,1)*SK_MY(3) - OP(12,3)*SK_MY(2) - OP(12,17)*SK_MY(4)); -Kfusion(13) = SK_MY(1)*(OP(13,21) + OP(13,18)*SH_MAG(1) + OP(13,19)*SH_MAG(4) + OP(13,1)*SK_MY(3) - OP(13,3)*SK_MY(2) - OP(13,17)*SK_MY(4)); -Kfusion(14) = SK_MY(1)*(OP(14,21) + OP(14,18)*SH_MAG(1) + OP(14,19)*SH_MAG(4) + OP(14,1)*SK_MY(3) - OP(14,3)*SK_MY(2) - OP(14,17)*SK_MY(4)); -Kfusion(15) = SK_MY(1)*(OP(15,21) + OP(15,18)*SH_MAG(1) + OP(15,19)*SH_MAG(4) + OP(15,1)*SK_MY(3) - OP(15,3)*SK_MY(2) - OP(15,17)*SK_MY(4)); -Kfusion(16) = SK_MY(1)*(OP(16,21) + OP(16,18)*SH_MAG(1) + OP(16,19)*SH_MAG(4) + OP(16,1)*SK_MY(3) - OP(16,3)*SK_MY(2) - OP(16,17)*SK_MY(4)); -Kfusion(17) = SK_MY(1)*(OP(17,21) + OP(17,18)*SH_MAG(1) + OP(17,19)*SH_MAG(4) + OP(17,1)*SK_MY(3) - OP(17,3)*SK_MY(2) - OP(17,17)*SK_MY(4)); -Kfusion(18) = SK_MY(1)*(OP(18,21) + OP(18,18)*SH_MAG(1) + OP(18,19)*SH_MAG(4) + OP(18,1)*SK_MY(3) - OP(18,3)*SK_MY(2) - OP(18,17)*SK_MY(4)); -Kfusion(19) = SK_MY(1)*(OP(19,21) + OP(19,18)*SH_MAG(1) + OP(19,19)*SH_MAG(4) + OP(19,1)*SK_MY(3) - OP(19,3)*SK_MY(2) - OP(19,17)*SK_MY(4)); -Kfusion(20) = SK_MY(1)*(OP(20,21) + OP(20,18)*SH_MAG(1) + OP(20,19)*SH_MAG(4) + OP(20,1)*SK_MY(3) - OP(20,3)*SK_MY(2) - OP(20,17)*SK_MY(4)); -Kfusion(21) = SK_MY(1)*(OP(21,21) + OP(21,18)*SH_MAG(1) + OP(21,19)*SH_MAG(4) + OP(21,1)*SK_MY(3) - OP(21,3)*SK_MY(2) - OP(21,17)*SK_MY(4)); -Kfusion(22) = SK_MY(1)*(OP(22,21) + OP(22,18)*SH_MAG(1) + OP(22,19)*SH_MAG(4) + OP(22,1)*SK_MY(3) - OP(22,3)*SK_MY(2) - OP(22,17)*SK_MY(4)); -Kfusion(23) = SK_MY(1)*(OP(23,21) + OP(23,18)*SH_MAG(1) + OP(23,19)*SH_MAG(4) + OP(23,1)*SK_MY(3) - OP(23,3)*SK_MY(2) - OP(23,17)*SK_MY(4)); -Kfusion(24) = SK_MY(1)*(OP(24,21) + OP(24,18)*SH_MAG(1) + OP(24,19)*SH_MAG(4) + OP(24,1)*SK_MY(3) - OP(24,3)*SK_MY(2) - OP(24,17)*SK_MY(4)); +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) = magN*(SH_MAG(9) - 2*q1*q2) - magD*SH_MAG(4) - magE*SH_MAG(1); -H_MAG(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); -H_MAG(17) = SH_MAG(6); +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(3); +H_MAG(19) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7); H_MAG(22) = 1; -SK_MZ = zeros(4,1); -SK_MZ(1) = 1/(OP(22,22) + R_MAG + OP(17,22)*SH_MAG(6) + OP(19,22)*SH_MAG(3) - (2*q0*q1 - 2*q2*q3)*(OP(22,18) + OP(17,18)*SH_MAG(6) + OP(19,18)*SH_MAG(3) - OP(1,18)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,18)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,18)*(2*q0*q1 - 2*q2*q3)) - OP(1,22)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,22)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + SH_MAG(6)*(OP(22,17) + OP(17,17)*SH_MAG(6) + OP(19,17)*SH_MAG(3) - OP(1,17)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,17)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,17)*(2*q0*q1 - 2*q2*q3)) + SH_MAG(3)*(OP(22,19) + OP(17,19)*SH_MAG(6) + OP(19,19)*SH_MAG(3) - OP(1,19)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,19)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,19)*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP(22,1) + OP(17,1)*SH_MAG(6) + OP(19,1)*SH_MAG(3) - OP(1,1)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,1)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,1)*(2*q0*q1 - 2*q2*q3)) - OP(18,22)*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP(22,2) + OP(17,2)*SH_MAG(6) + OP(19,2)*SH_MAG(3) - OP(1,2)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,2)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,2)*(2*q0*q1 - 2*q2*q3))); -SK_MZ(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -SK_MZ(3) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); +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,19)*SH_MAG(3) + OP(1,17)*SH_MAG(6) - OP(1,1)*SK_MZ(2) + OP(1,2)*SK_MZ(3) - OP(1,18)*SK_MZ(4)); -Kfusion(2) = SK_MZ(1)*(OP(2,22) + OP(2,19)*SH_MAG(3) + OP(2,17)*SH_MAG(6) - OP(2,1)*SK_MZ(2) + OP(2,2)*SK_MZ(3) - OP(2,18)*SK_MZ(4)); -Kfusion(3) = SK_MZ(1)*(OP(3,22) + OP(3,19)*SH_MAG(3) + OP(3,17)*SH_MAG(6) - OP(3,1)*SK_MZ(2) + OP(3,2)*SK_MZ(3) - OP(3,18)*SK_MZ(4)); -Kfusion(4) = SK_MZ(1)*(OP(4,22) + OP(4,19)*SH_MAG(3) + OP(4,17)*SH_MAG(6) - OP(4,1)*SK_MZ(2) + OP(4,2)*SK_MZ(3) - OP(4,18)*SK_MZ(4)); -Kfusion(5) = SK_MZ(1)*(OP(5,22) + OP(5,19)*SH_MAG(3) + OP(5,17)*SH_MAG(6) - OP(5,1)*SK_MZ(2) + OP(5,2)*SK_MZ(3) - OP(5,18)*SK_MZ(4)); -Kfusion(6) = SK_MZ(1)*(OP(6,22) + OP(6,19)*SH_MAG(3) + OP(6,17)*SH_MAG(6) - OP(6,1)*SK_MZ(2) + OP(6,2)*SK_MZ(3) - OP(6,18)*SK_MZ(4)); -Kfusion(7) = SK_MZ(1)*(OP(7,22) + OP(7,19)*SH_MAG(3) + OP(7,17)*SH_MAG(6) - OP(7,1)*SK_MZ(2) + OP(7,2)*SK_MZ(3) - OP(7,18)*SK_MZ(4)); -Kfusion(8) = SK_MZ(1)*(OP(8,22) + OP(8,19)*SH_MAG(3) + OP(8,17)*SH_MAG(6) - OP(8,1)*SK_MZ(2) + OP(8,2)*SK_MZ(3) - OP(8,18)*SK_MZ(4)); -Kfusion(9) = SK_MZ(1)*(OP(9,22) + OP(9,19)*SH_MAG(3) + OP(9,17)*SH_MAG(6) - OP(9,1)*SK_MZ(2) + OP(9,2)*SK_MZ(3) - OP(9,18)*SK_MZ(4)); -Kfusion(10) = SK_MZ(1)*(OP(10,22) + OP(10,19)*SH_MAG(3) + OP(10,17)*SH_MAG(6) - OP(10,1)*SK_MZ(2) + OP(10,2)*SK_MZ(3) - OP(10,18)*SK_MZ(4)); -Kfusion(11) = SK_MZ(1)*(OP(11,22) + OP(11,19)*SH_MAG(3) + OP(11,17)*SH_MAG(6) - OP(11,1)*SK_MZ(2) + OP(11,2)*SK_MZ(3) - OP(11,18)*SK_MZ(4)); -Kfusion(12) = SK_MZ(1)*(OP(12,22) + OP(12,19)*SH_MAG(3) + OP(12,17)*SH_MAG(6) - OP(12,1)*SK_MZ(2) + OP(12,2)*SK_MZ(3) - OP(12,18)*SK_MZ(4)); -Kfusion(13) = SK_MZ(1)*(OP(13,22) + OP(13,19)*SH_MAG(3) + OP(13,17)*SH_MAG(6) - OP(13,1)*SK_MZ(2) + OP(13,2)*SK_MZ(3) - OP(13,18)*SK_MZ(4)); -Kfusion(14) = SK_MZ(1)*(OP(14,22) + OP(14,19)*SH_MAG(3) + OP(14,17)*SH_MAG(6) - OP(14,1)*SK_MZ(2) + OP(14,2)*SK_MZ(3) - OP(14,18)*SK_MZ(4)); -Kfusion(15) = SK_MZ(1)*(OP(15,22) + OP(15,19)*SH_MAG(3) + OP(15,17)*SH_MAG(6) - OP(15,1)*SK_MZ(2) + OP(15,2)*SK_MZ(3) - OP(15,18)*SK_MZ(4)); -Kfusion(16) = SK_MZ(1)*(OP(16,22) + OP(16,19)*SH_MAG(3) + OP(16,17)*SH_MAG(6) - OP(16,1)*SK_MZ(2) + OP(16,2)*SK_MZ(3) - OP(16,18)*SK_MZ(4)); -Kfusion(17) = SK_MZ(1)*(OP(17,22) + OP(17,19)*SH_MAG(3) + OP(17,17)*SH_MAG(6) - OP(17,1)*SK_MZ(2) + OP(17,2)*SK_MZ(3) - OP(17,18)*SK_MZ(4)); -Kfusion(18) = SK_MZ(1)*(OP(18,22) + OP(18,19)*SH_MAG(3) + OP(18,17)*SH_MAG(6) - OP(18,1)*SK_MZ(2) + OP(18,2)*SK_MZ(3) - OP(18,18)*SK_MZ(4)); -Kfusion(19) = SK_MZ(1)*(OP(19,22) + OP(19,19)*SH_MAG(3) + OP(19,17)*SH_MAG(6) - OP(19,1)*SK_MZ(2) + OP(19,2)*SK_MZ(3) - OP(19,18)*SK_MZ(4)); -Kfusion(20) = SK_MZ(1)*(OP(20,22) + OP(20,19)*SH_MAG(3) + OP(20,17)*SH_MAG(6) - OP(20,1)*SK_MZ(2) + OP(20,2)*SK_MZ(3) - OP(20,18)*SK_MZ(4)); -Kfusion(21) = SK_MZ(1)*(OP(21,22) + OP(21,19)*SH_MAG(3) + OP(21,17)*SH_MAG(6) - OP(21,1)*SK_MZ(2) + OP(21,2)*SK_MZ(3) - OP(21,18)*SK_MZ(4)); -Kfusion(22) = SK_MZ(1)*(OP(22,22) + OP(22,19)*SH_MAG(3) + OP(22,17)*SH_MAG(6) - OP(22,1)*SK_MZ(2) + OP(22,2)*SK_MZ(3) - OP(22,18)*SK_MZ(4)); -Kfusion(23) = SK_MZ(1)*(OP(23,22) + OP(23,19)*SH_MAG(3) + OP(23,17)*SH_MAG(6) - OP(23,1)*SK_MZ(2) + OP(23,2)*SK_MZ(3) - OP(23,18)*SK_MZ(4)); -Kfusion(24) = SK_MZ(1)*(OP(24,22) + OP(24,19)*SH_MAG(3) + OP(24,17)*SH_MAG(6) - OP(24,1)*SK_MZ(2) + OP(24,2)*SK_MZ(3) - OP(24,18)*SK_MZ(4)); -SH_ACCX = zeros(7,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) = 2*q0*q3 + 2*q1*q2; -SH_ACCX(3) = vn - vwn; -SH_ACCX(4) = ve - vwe; -SH_ACCX(5) = q3^2; -SH_ACCX(6) = 2*q0*q2; -SH_ACCX(7) = 2*q0*q1; +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,2) = Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)); -H_ACCX(1,3) = Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)); -H_ACCX(1,4) = -Kaccx*SH_ACCX(1); -H_ACCX(1,5) = -Kaccx*SH_ACCX(2); -H_ACCX(1,6) = Kaccx*(SH_ACCX(6) - 2*q1*q3); +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(2); -SK_ACCX = zeros(5,1); -SK_ACCX(1) = 1/(R_ACC - Kaccx*SH_ACCX(1)*(Kaccx*OP(23,4)*SH_ACCX(1) - Kaccx*OP(5,4)*SH_ACCX(2) - Kaccx*OP(4,4)*SH_ACCX(1) + Kaccx*OP(24,4)*SH_ACCX(2) + Kaccx*OP(3,4)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,4)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,4)*(SH_ACCX(6) - 2*q1*q3)) - Kaccx*SH_ACCX(2)*(Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(5,5)*SH_ACCX(2) - Kaccx*OP(4,5)*SH_ACCX(1) + Kaccx*OP(24,5)*SH_ACCX(2) + Kaccx*OP(3,5)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,5)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,5)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(1)*(Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(5,23)*SH_ACCX(2) - Kaccx*OP(4,23)*SH_ACCX(1) + Kaccx*OP(24,23)*SH_ACCX(2) + Kaccx*OP(3,23)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,23)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,23)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(2)*(Kaccx*OP(23,24)*SH_ACCX(1) - Kaccx*OP(5,24)*SH_ACCX(2) - Kaccx*OP(4,24)*SH_ACCX(1) + Kaccx*OP(24,24)*SH_ACCX(2) + Kaccx*OP(3,24)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,24)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,24)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3))*(Kaccx*OP(23,3)*SH_ACCX(1) - Kaccx*OP(5,3)*SH_ACCX(2) - Kaccx*OP(4,3)*SH_ACCX(1) + Kaccx*OP(24,3)*SH_ACCX(2) + Kaccx*OP(3,3)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,3)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,3)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2))*(Kaccx*OP(23,2)*SH_ACCX(1) - Kaccx*OP(5,2)*SH_ACCX(2) - Kaccx*OP(4,2)*SH_ACCX(1) + Kaccx*OP(24,2)*SH_ACCX(2) + Kaccx*OP(3,2)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,2)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,2)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(6) - 2*q1*q3)*(Kaccx*OP(23,6)*SH_ACCX(1) - Kaccx*OP(5,6)*SH_ACCX(2) - Kaccx*OP(4,6)*SH_ACCX(1) + Kaccx*OP(24,6)*SH_ACCX(2) + Kaccx*OP(3,6)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,6)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,6)*(SH_ACCX(6) - 2*q1*q3))); -SK_ACCX(2) = SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3); -SK_ACCX(3) = SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2); -SK_ACCX(4) = SH_ACCX(6) - 2*q1*q3; -SK_ACCX(5) = SH_ACCX(2); +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,23)*SH_ACCX(1) - Kaccx*OP(1,4)*SH_ACCX(1) + Kaccx*OP(1,2)*SK_ACCX(3) + Kaccx*OP(1,3)*SK_ACCX(2) - Kaccx*OP(1,5)*SK_ACCX(5) + Kaccx*OP(1,6)*SK_ACCX(4) + Kaccx*OP(1,24)*SK_ACCX(5)); -Kfusion(2) = SK_ACCX(1)*(Kaccx*OP(2,23)*SH_ACCX(1) - Kaccx*OP(2,4)*SH_ACCX(1) + Kaccx*OP(2,2)*SK_ACCX(3) + Kaccx*OP(2,3)*SK_ACCX(2) - Kaccx*OP(2,5)*SK_ACCX(5) + Kaccx*OP(2,6)*SK_ACCX(4) + Kaccx*OP(2,24)*SK_ACCX(5)); -Kfusion(3) = SK_ACCX(1)*(Kaccx*OP(3,23)*SH_ACCX(1) - Kaccx*OP(3,4)*SH_ACCX(1) + Kaccx*OP(3,2)*SK_ACCX(3) + Kaccx*OP(3,3)*SK_ACCX(2) - Kaccx*OP(3,5)*SK_ACCX(5) + Kaccx*OP(3,6)*SK_ACCX(4) + Kaccx*OP(3,24)*SK_ACCX(5)); -Kfusion(4) = SK_ACCX(1)*(Kaccx*OP(4,23)*SH_ACCX(1) - Kaccx*OP(4,4)*SH_ACCX(1) + Kaccx*OP(4,2)*SK_ACCX(3) + Kaccx*OP(4,3)*SK_ACCX(2) - Kaccx*OP(4,5)*SK_ACCX(5) + Kaccx*OP(4,6)*SK_ACCX(4) + Kaccx*OP(4,24)*SK_ACCX(5)); -Kfusion(5) = SK_ACCX(1)*(Kaccx*OP(5,23)*SH_ACCX(1) - Kaccx*OP(5,4)*SH_ACCX(1) + Kaccx*OP(5,2)*SK_ACCX(3) + Kaccx*OP(5,3)*SK_ACCX(2) - Kaccx*OP(5,5)*SK_ACCX(5) + Kaccx*OP(5,6)*SK_ACCX(4) + Kaccx*OP(5,24)*SK_ACCX(5)); -Kfusion(6) = SK_ACCX(1)*(Kaccx*OP(6,23)*SH_ACCX(1) - Kaccx*OP(6,4)*SH_ACCX(1) + Kaccx*OP(6,2)*SK_ACCX(3) + Kaccx*OP(6,3)*SK_ACCX(2) - Kaccx*OP(6,5)*SK_ACCX(5) + Kaccx*OP(6,6)*SK_ACCX(4) + Kaccx*OP(6,24)*SK_ACCX(5)); -Kfusion(7) = SK_ACCX(1)*(Kaccx*OP(7,23)*SH_ACCX(1) - Kaccx*OP(7,4)*SH_ACCX(1) + Kaccx*OP(7,2)*SK_ACCX(3) + Kaccx*OP(7,3)*SK_ACCX(2) - Kaccx*OP(7,5)*SK_ACCX(5) + Kaccx*OP(7,6)*SK_ACCX(4) + Kaccx*OP(7,24)*SK_ACCX(5)); -Kfusion(8) = SK_ACCX(1)*(Kaccx*OP(8,23)*SH_ACCX(1) - Kaccx*OP(8,4)*SH_ACCX(1) + Kaccx*OP(8,2)*SK_ACCX(3) + Kaccx*OP(8,3)*SK_ACCX(2) - Kaccx*OP(8,5)*SK_ACCX(5) + Kaccx*OP(8,6)*SK_ACCX(4) + Kaccx*OP(8,24)*SK_ACCX(5)); -Kfusion(9) = SK_ACCX(1)*(Kaccx*OP(9,23)*SH_ACCX(1) - Kaccx*OP(9,4)*SH_ACCX(1) + Kaccx*OP(9,2)*SK_ACCX(3) + Kaccx*OP(9,3)*SK_ACCX(2) - Kaccx*OP(9,5)*SK_ACCX(5) + Kaccx*OP(9,6)*SK_ACCX(4) + Kaccx*OP(9,24)*SK_ACCX(5)); -Kfusion(10) = SK_ACCX(1)*(Kaccx*OP(10,23)*SH_ACCX(1) - Kaccx*OP(10,4)*SH_ACCX(1) + Kaccx*OP(10,2)*SK_ACCX(3) + Kaccx*OP(10,3)*SK_ACCX(2) - Kaccx*OP(10,5)*SK_ACCX(5) + Kaccx*OP(10,6)*SK_ACCX(4) + Kaccx*OP(10,24)*SK_ACCX(5)); -Kfusion(11) = SK_ACCX(1)*(Kaccx*OP(11,23)*SH_ACCX(1) - Kaccx*OP(11,4)*SH_ACCX(1) + Kaccx*OP(11,2)*SK_ACCX(3) + Kaccx*OP(11,3)*SK_ACCX(2) - Kaccx*OP(11,5)*SK_ACCX(5) + Kaccx*OP(11,6)*SK_ACCX(4) + Kaccx*OP(11,24)*SK_ACCX(5)); -Kfusion(12) = SK_ACCX(1)*(Kaccx*OP(12,23)*SH_ACCX(1) - Kaccx*OP(12,4)*SH_ACCX(1) + Kaccx*OP(12,2)*SK_ACCX(3) + Kaccx*OP(12,3)*SK_ACCX(2) - Kaccx*OP(12,5)*SK_ACCX(5) + Kaccx*OP(12,6)*SK_ACCX(4) + Kaccx*OP(12,24)*SK_ACCX(5)); -Kfusion(13) = SK_ACCX(1)*(Kaccx*OP(13,23)*SH_ACCX(1) - Kaccx*OP(13,4)*SH_ACCX(1) + Kaccx*OP(13,2)*SK_ACCX(3) + Kaccx*OP(13,3)*SK_ACCX(2) - Kaccx*OP(13,5)*SK_ACCX(5) + Kaccx*OP(13,6)*SK_ACCX(4) + Kaccx*OP(13,24)*SK_ACCX(5)); -Kfusion(14) = SK_ACCX(1)*(Kaccx*OP(14,23)*SH_ACCX(1) - Kaccx*OP(14,4)*SH_ACCX(1) + Kaccx*OP(14,2)*SK_ACCX(3) + Kaccx*OP(14,3)*SK_ACCX(2) - Kaccx*OP(14,5)*SK_ACCX(5) + Kaccx*OP(14,6)*SK_ACCX(4) + Kaccx*OP(14,24)*SK_ACCX(5)); -Kfusion(15) = SK_ACCX(1)*(Kaccx*OP(15,23)*SH_ACCX(1) - Kaccx*OP(15,4)*SH_ACCX(1) + Kaccx*OP(15,2)*SK_ACCX(3) + Kaccx*OP(15,3)*SK_ACCX(2) - Kaccx*OP(15,5)*SK_ACCX(5) + Kaccx*OP(15,6)*SK_ACCX(4) + Kaccx*OP(15,24)*SK_ACCX(5)); -Kfusion(16) = SK_ACCX(1)*(Kaccx*OP(16,23)*SH_ACCX(1) - Kaccx*OP(16,4)*SH_ACCX(1) + Kaccx*OP(16,2)*SK_ACCX(3) + Kaccx*OP(16,3)*SK_ACCX(2) - Kaccx*OP(16,5)*SK_ACCX(5) + Kaccx*OP(16,6)*SK_ACCX(4) + Kaccx*OP(16,24)*SK_ACCX(5)); -Kfusion(17) = SK_ACCX(1)*(Kaccx*OP(17,23)*SH_ACCX(1) - Kaccx*OP(17,4)*SH_ACCX(1) + Kaccx*OP(17,2)*SK_ACCX(3) + Kaccx*OP(17,3)*SK_ACCX(2) - Kaccx*OP(17,5)*SK_ACCX(5) + Kaccx*OP(17,6)*SK_ACCX(4) + Kaccx*OP(17,24)*SK_ACCX(5)); -Kfusion(18) = SK_ACCX(1)*(Kaccx*OP(18,23)*SH_ACCX(1) - Kaccx*OP(18,4)*SH_ACCX(1) + Kaccx*OP(18,2)*SK_ACCX(3) + Kaccx*OP(18,3)*SK_ACCX(2) - Kaccx*OP(18,5)*SK_ACCX(5) + Kaccx*OP(18,6)*SK_ACCX(4) + Kaccx*OP(18,24)*SK_ACCX(5)); -Kfusion(19) = SK_ACCX(1)*(Kaccx*OP(19,23)*SH_ACCX(1) - Kaccx*OP(19,4)*SH_ACCX(1) + Kaccx*OP(19,2)*SK_ACCX(3) + Kaccx*OP(19,3)*SK_ACCX(2) - Kaccx*OP(19,5)*SK_ACCX(5) + Kaccx*OP(19,6)*SK_ACCX(4) + Kaccx*OP(19,24)*SK_ACCX(5)); -Kfusion(20) = SK_ACCX(1)*(Kaccx*OP(20,23)*SH_ACCX(1) - Kaccx*OP(20,4)*SH_ACCX(1) + Kaccx*OP(20,2)*SK_ACCX(3) + Kaccx*OP(20,3)*SK_ACCX(2) - Kaccx*OP(20,5)*SK_ACCX(5) + Kaccx*OP(20,6)*SK_ACCX(4) + Kaccx*OP(20,24)*SK_ACCX(5)); -Kfusion(21) = SK_ACCX(1)*(Kaccx*OP(21,23)*SH_ACCX(1) - Kaccx*OP(21,4)*SH_ACCX(1) + Kaccx*OP(21,2)*SK_ACCX(3) + Kaccx*OP(21,3)*SK_ACCX(2) - Kaccx*OP(21,5)*SK_ACCX(5) + Kaccx*OP(21,6)*SK_ACCX(4) + Kaccx*OP(21,24)*SK_ACCX(5)); -Kfusion(22) = SK_ACCX(1)*(Kaccx*OP(22,23)*SH_ACCX(1) - Kaccx*OP(22,4)*SH_ACCX(1) + Kaccx*OP(22,2)*SK_ACCX(3) + Kaccx*OP(22,3)*SK_ACCX(2) - Kaccx*OP(22,5)*SK_ACCX(5) + Kaccx*OP(22,6)*SK_ACCX(4) + Kaccx*OP(22,24)*SK_ACCX(5)); -Kfusion(23) = SK_ACCX(1)*(Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(23,4)*SH_ACCX(1) + Kaccx*OP(23,2)*SK_ACCX(3) + Kaccx*OP(23,3)*SK_ACCX(2) - Kaccx*OP(23,5)*SK_ACCX(5) + Kaccx*OP(23,6)*SK_ACCX(4) + Kaccx*OP(23,24)*SK_ACCX(5)); -Kfusion(24) = SK_ACCX(1)*(Kaccx*OP(24,23)*SH_ACCX(1) - Kaccx*OP(24,4)*SH_ACCX(1) + Kaccx*OP(24,2)*SK_ACCX(3) + Kaccx*OP(24,3)*SK_ACCX(2) - Kaccx*OP(24,5)*SK_ACCX(5) + Kaccx*OP(24,6)*SK_ACCX(4) + Kaccx*OP(24,24)*SK_ACCX(5)); -SH_ACCY = zeros(6,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) = ve - vwe; -SH_ACCY(3) = vn - vwn; -SH_ACCY(4) = q1^2; -SH_ACCY(5) = 2*q0*q1; -SH_ACCY(6) = 2*q0*q3; +SH_ACCY(2) = vn - vwn; +SH_ACCY(3) = ve - vwe; H_ACCY = zeros(1,24); -H_ACCY(1,1) = Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)); -H_ACCY(1,3) = Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)); -H_ACCY(1,4) = Kaccy*(SH_ACCY(6) - 2*q1*q2); -H_ACCY(1,5) = -Kaccy*SH_ACCY(1); -H_ACCY(1,6) = -Kaccy*(SH_ACCY(5) + 2*q2*q3); +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(7,1); -SK_ACCY(1) = 1/(R_ACC - Kaccy*SH_ACCY(1)*(Kaccy*OP(24,5)*SH_ACCY(1) - Kaccy*OP(5,5)*SH_ACCY(1) + Kaccy*OP(1,5)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,5)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,5)*(q0*q3 - q1*q2) + Kaccy*OP(4,5)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,5)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*SH_ACCY(1)*(Kaccy*OP(24,24)*SH_ACCY(1) - Kaccy*OP(5,24)*SH_ACCY(1) + Kaccy*OP(1,24)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,24)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,24)*(q0*q3 - q1*q2) + Kaccy*OP(4,24)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,24)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2))*(Kaccy*OP(24,1)*SH_ACCY(1) - Kaccy*OP(5,1)*SH_ACCY(1) + Kaccy*OP(1,1)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,1)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,1)*(q0*q3 - q1*q2) + Kaccy*OP(4,1)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,1)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3))*(Kaccy*OP(24,3)*SH_ACCY(1) - Kaccy*OP(5,3)*SH_ACCY(1) + Kaccy*OP(1,3)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,3)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,3)*(q0*q3 - q1*q2) + Kaccy*OP(4,3)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,3)*(SH_ACCY(5) + 2*q2*q3)) - 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP(24,23)*SH_ACCY(1) - Kaccy*OP(5,23)*SH_ACCY(1) + Kaccy*OP(1,23)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,23)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,23)*(q0*q3 - q1*q2) + Kaccy*OP(4,23)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,23)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(6) - 2*q1*q2)*(Kaccy*OP(24,4)*SH_ACCY(1) - Kaccy*OP(5,4)*SH_ACCY(1) + Kaccy*OP(1,4)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,4)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,4)*(q0*q3 - q1*q2) + Kaccy*OP(4,4)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,4)*(SH_ACCY(5) + 2*q2*q3)) - Kaccy*(SH_ACCY(5) + 2*q2*q3)*(Kaccy*OP(24,6)*SH_ACCY(1) - Kaccy*OP(5,6)*SH_ACCY(1) + Kaccy*OP(1,6)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,6)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,6)*(q0*q3 - q1*q2) + Kaccy*OP(4,6)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,6)*(SH_ACCY(5) + 2*q2*q3))); -SK_ACCY(2) = SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3); -SK_ACCY(3) = SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2); -SK_ACCY(4) = q0*q3 - q1*q2; -SK_ACCY(5) = SH_ACCY(6) - 2*q1*q2; -SK_ACCY(6) = SH_ACCY(5) + 2*q2*q3; -SK_ACCY(7) = 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(3) + Kaccy*OP(1,3)*SK_ACCY(2) + Kaccy*OP(1,4)*SK_ACCY(5) - Kaccy*OP(1,5)*SK_ACCY(7) - Kaccy*OP(1,6)*SK_ACCY(6) - 2*Kaccy*OP(1,23)*SK_ACCY(4) + Kaccy*OP(1,24)*SK_ACCY(7)); -Kfusion(2) = SK_ACCY(1)*(Kaccy*OP(2,1)*SK_ACCY(3) + Kaccy*OP(2,3)*SK_ACCY(2) + Kaccy*OP(2,4)*SK_ACCY(5) - Kaccy*OP(2,5)*SK_ACCY(7) - Kaccy*OP(2,6)*SK_ACCY(6) - 2*Kaccy*OP(2,23)*SK_ACCY(4) + Kaccy*OP(2,24)*SK_ACCY(7)); -Kfusion(3) = SK_ACCY(1)*(Kaccy*OP(3,1)*SK_ACCY(3) + Kaccy*OP(3,3)*SK_ACCY(2) + Kaccy*OP(3,4)*SK_ACCY(5) - Kaccy*OP(3,5)*SK_ACCY(7) - Kaccy*OP(3,6)*SK_ACCY(6) - 2*Kaccy*OP(3,23)*SK_ACCY(4) + Kaccy*OP(3,24)*SK_ACCY(7)); -Kfusion(4) = SK_ACCY(1)*(Kaccy*OP(4,1)*SK_ACCY(3) + Kaccy*OP(4,3)*SK_ACCY(2) + Kaccy*OP(4,4)*SK_ACCY(5) - Kaccy*OP(4,5)*SK_ACCY(7) - Kaccy*OP(4,6)*SK_ACCY(6) - 2*Kaccy*OP(4,23)*SK_ACCY(4) + Kaccy*OP(4,24)*SK_ACCY(7)); -Kfusion(5) = SK_ACCY(1)*(Kaccy*OP(5,1)*SK_ACCY(3) + Kaccy*OP(5,3)*SK_ACCY(2) + Kaccy*OP(5,4)*SK_ACCY(5) - Kaccy*OP(5,5)*SK_ACCY(7) - Kaccy*OP(5,6)*SK_ACCY(6) - 2*Kaccy*OP(5,23)*SK_ACCY(4) + Kaccy*OP(5,24)*SK_ACCY(7)); -Kfusion(6) = SK_ACCY(1)*(Kaccy*OP(6,1)*SK_ACCY(3) + Kaccy*OP(6,3)*SK_ACCY(2) + Kaccy*OP(6,4)*SK_ACCY(5) - Kaccy*OP(6,5)*SK_ACCY(7) - Kaccy*OP(6,6)*SK_ACCY(6) - 2*Kaccy*OP(6,23)*SK_ACCY(4) + Kaccy*OP(6,24)*SK_ACCY(7)); -Kfusion(7) = SK_ACCY(1)*(Kaccy*OP(7,1)*SK_ACCY(3) + Kaccy*OP(7,3)*SK_ACCY(2) + Kaccy*OP(7,4)*SK_ACCY(5) - Kaccy*OP(7,5)*SK_ACCY(7) - Kaccy*OP(7,6)*SK_ACCY(6) - 2*Kaccy*OP(7,23)*SK_ACCY(4) + Kaccy*OP(7,24)*SK_ACCY(7)); -Kfusion(8) = SK_ACCY(1)*(Kaccy*OP(8,1)*SK_ACCY(3) + Kaccy*OP(8,3)*SK_ACCY(2) + Kaccy*OP(8,4)*SK_ACCY(5) - Kaccy*OP(8,5)*SK_ACCY(7) - Kaccy*OP(8,6)*SK_ACCY(6) - 2*Kaccy*OP(8,23)*SK_ACCY(4) + Kaccy*OP(8,24)*SK_ACCY(7)); -Kfusion(9) = SK_ACCY(1)*(Kaccy*OP(9,1)*SK_ACCY(3) + Kaccy*OP(9,3)*SK_ACCY(2) + Kaccy*OP(9,4)*SK_ACCY(5) - Kaccy*OP(9,5)*SK_ACCY(7) - Kaccy*OP(9,6)*SK_ACCY(6) - 2*Kaccy*OP(9,23)*SK_ACCY(4) + Kaccy*OP(9,24)*SK_ACCY(7)); -Kfusion(10) = SK_ACCY(1)*(Kaccy*OP(10,1)*SK_ACCY(3) + Kaccy*OP(10,3)*SK_ACCY(2) + Kaccy*OP(10,4)*SK_ACCY(5) - Kaccy*OP(10,5)*SK_ACCY(7) - Kaccy*OP(10,6)*SK_ACCY(6) - 2*Kaccy*OP(10,23)*SK_ACCY(4) + Kaccy*OP(10,24)*SK_ACCY(7)); -Kfusion(11) = SK_ACCY(1)*(Kaccy*OP(11,1)*SK_ACCY(3) + Kaccy*OP(11,3)*SK_ACCY(2) + Kaccy*OP(11,4)*SK_ACCY(5) - Kaccy*OP(11,5)*SK_ACCY(7) - Kaccy*OP(11,6)*SK_ACCY(6) - 2*Kaccy*OP(11,23)*SK_ACCY(4) + Kaccy*OP(11,24)*SK_ACCY(7)); -Kfusion(12) = SK_ACCY(1)*(Kaccy*OP(12,1)*SK_ACCY(3) + Kaccy*OP(12,3)*SK_ACCY(2) + Kaccy*OP(12,4)*SK_ACCY(5) - Kaccy*OP(12,5)*SK_ACCY(7) - Kaccy*OP(12,6)*SK_ACCY(6) - 2*Kaccy*OP(12,23)*SK_ACCY(4) + Kaccy*OP(12,24)*SK_ACCY(7)); -Kfusion(13) = SK_ACCY(1)*(Kaccy*OP(13,1)*SK_ACCY(3) + Kaccy*OP(13,3)*SK_ACCY(2) + Kaccy*OP(13,4)*SK_ACCY(5) - Kaccy*OP(13,5)*SK_ACCY(7) - Kaccy*OP(13,6)*SK_ACCY(6) - 2*Kaccy*OP(13,23)*SK_ACCY(4) + Kaccy*OP(13,24)*SK_ACCY(7)); -Kfusion(14) = SK_ACCY(1)*(Kaccy*OP(14,1)*SK_ACCY(3) + Kaccy*OP(14,3)*SK_ACCY(2) + Kaccy*OP(14,4)*SK_ACCY(5) - Kaccy*OP(14,5)*SK_ACCY(7) - Kaccy*OP(14,6)*SK_ACCY(6) - 2*Kaccy*OP(14,23)*SK_ACCY(4) + Kaccy*OP(14,24)*SK_ACCY(7)); -Kfusion(15) = SK_ACCY(1)*(Kaccy*OP(15,1)*SK_ACCY(3) + Kaccy*OP(15,3)*SK_ACCY(2) + Kaccy*OP(15,4)*SK_ACCY(5) - Kaccy*OP(15,5)*SK_ACCY(7) - Kaccy*OP(15,6)*SK_ACCY(6) - 2*Kaccy*OP(15,23)*SK_ACCY(4) + Kaccy*OP(15,24)*SK_ACCY(7)); -Kfusion(16) = SK_ACCY(1)*(Kaccy*OP(16,1)*SK_ACCY(3) + Kaccy*OP(16,3)*SK_ACCY(2) + Kaccy*OP(16,4)*SK_ACCY(5) - Kaccy*OP(16,5)*SK_ACCY(7) - Kaccy*OP(16,6)*SK_ACCY(6) - 2*Kaccy*OP(16,23)*SK_ACCY(4) + Kaccy*OP(16,24)*SK_ACCY(7)); -Kfusion(17) = SK_ACCY(1)*(Kaccy*OP(17,1)*SK_ACCY(3) + Kaccy*OP(17,3)*SK_ACCY(2) + Kaccy*OP(17,4)*SK_ACCY(5) - Kaccy*OP(17,5)*SK_ACCY(7) - Kaccy*OP(17,6)*SK_ACCY(6) - 2*Kaccy*OP(17,23)*SK_ACCY(4) + Kaccy*OP(17,24)*SK_ACCY(7)); -Kfusion(18) = SK_ACCY(1)*(Kaccy*OP(18,1)*SK_ACCY(3) + Kaccy*OP(18,3)*SK_ACCY(2) + Kaccy*OP(18,4)*SK_ACCY(5) - Kaccy*OP(18,5)*SK_ACCY(7) - Kaccy*OP(18,6)*SK_ACCY(6) - 2*Kaccy*OP(18,23)*SK_ACCY(4) + Kaccy*OP(18,24)*SK_ACCY(7)); -Kfusion(19) = SK_ACCY(1)*(Kaccy*OP(19,1)*SK_ACCY(3) + Kaccy*OP(19,3)*SK_ACCY(2) + Kaccy*OP(19,4)*SK_ACCY(5) - Kaccy*OP(19,5)*SK_ACCY(7) - Kaccy*OP(19,6)*SK_ACCY(6) - 2*Kaccy*OP(19,23)*SK_ACCY(4) + Kaccy*OP(19,24)*SK_ACCY(7)); -Kfusion(20) = SK_ACCY(1)*(Kaccy*OP(20,1)*SK_ACCY(3) + Kaccy*OP(20,3)*SK_ACCY(2) + Kaccy*OP(20,4)*SK_ACCY(5) - Kaccy*OP(20,5)*SK_ACCY(7) - Kaccy*OP(20,6)*SK_ACCY(6) - 2*Kaccy*OP(20,23)*SK_ACCY(4) + Kaccy*OP(20,24)*SK_ACCY(7)); -Kfusion(21) = SK_ACCY(1)*(Kaccy*OP(21,1)*SK_ACCY(3) + Kaccy*OP(21,3)*SK_ACCY(2) + Kaccy*OP(21,4)*SK_ACCY(5) - Kaccy*OP(21,5)*SK_ACCY(7) - Kaccy*OP(21,6)*SK_ACCY(6) - 2*Kaccy*OP(21,23)*SK_ACCY(4) + Kaccy*OP(21,24)*SK_ACCY(7)); -Kfusion(22) = SK_ACCY(1)*(Kaccy*OP(22,1)*SK_ACCY(3) + Kaccy*OP(22,3)*SK_ACCY(2) + Kaccy*OP(22,4)*SK_ACCY(5) - Kaccy*OP(22,5)*SK_ACCY(7) - Kaccy*OP(22,6)*SK_ACCY(6) - 2*Kaccy*OP(22,23)*SK_ACCY(4) + Kaccy*OP(22,24)*SK_ACCY(7)); -Kfusion(23) = SK_ACCY(1)*(Kaccy*OP(23,1)*SK_ACCY(3) + Kaccy*OP(23,3)*SK_ACCY(2) + Kaccy*OP(23,4)*SK_ACCY(5) - Kaccy*OP(23,5)*SK_ACCY(7) - Kaccy*OP(23,6)*SK_ACCY(6) - 2*Kaccy*OP(23,23)*SK_ACCY(4) + Kaccy*OP(23,24)*SK_ACCY(7)); -Kfusion(24) = SK_ACCY(1)*(Kaccy*OP(24,1)*SK_ACCY(3) + Kaccy*OP(24,3)*SK_ACCY(2) + Kaccy*OP(24,4)*SK_ACCY(5) - Kaccy*OP(24,5)*SK_ACCY(7) - Kaccy*OP(24,6)*SK_ACCY(6) - 2*Kaccy*OP(24,23)*SK_ACCY(4) + Kaccy*OP(24,24)*SK_ACCY(7)); +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/matlab/scripts/Inertial Nav EKF/Magnetometer.mat b/matlab/scripts/Inertial Nav EKF/Magnetometer.mat index a4525f2295..c302dd2ae0 100644 Binary files a/matlab/scripts/Inertial Nav EKF/Magnetometer.mat and b/matlab/scripts/Inertial Nav EKF/Magnetometer.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/Sideslip.mat b/matlab/scripts/Inertial Nav EKF/Sideslip.mat index c0abe0b417..f6d8de8d86 100644 Binary files a/matlab/scripts/Inertial Nav EKF/Sideslip.mat and b/matlab/scripts/Inertial Nav EKF/Sideslip.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat b/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat index 76145de186..c87c9e4772 100644 Binary files a/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat and b/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/StatePrediction.mat b/matlab/scripts/Inertial Nav EKF/StatePrediction.mat index 3f6faf511e..abf15241a0 100644 Binary files a/matlab/scripts/Inertial Nav EKF/StatePrediction.mat and b/matlab/scripts/Inertial Nav EKF/StatePrediction.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat b/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat index 3cd17e14f3..17edcd19d9 100644 Binary files a/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat and b/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt b/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt index 5c61fc39e1..5e225c8f2d 100644 --- a/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt +++ b/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt @@ -1,224 +1,212 @@ -SF = zeros(25,1); -SF(1) = daz_b/2 + dazNoise/2 - (daz*daz_s)/2; -SF(2) = day_b/2 + dayNoise/2 - (day*day_s)/2; -SF(3) = dax_b/2 + daxNoise/2 - (dax*dax_s)/2; -SF(4) = q3/2 - (q0*SF(1))/2 + (q1*SF(2))/2 - (q2*SF(3))/2; -SF(5) = q0/2 - (q1*SF(3))/2 - (q2*SF(2))/2 + (q3*SF(1))/2; -SF(6) = q1/2 + (q0*SF(3))/2 - (q2*SF(1))/2 - (q3*SF(2))/2; -SF(7) = q3/2 + (q0*SF(1))/2 - (q1*SF(2))/2 - (q2*SF(3))/2; -SF(8) = q0/2 - (q1*SF(3))/2 + (q2*SF(2))/2 - (q3*SF(1))/2; -SF(9) = q0/2 + (q1*SF(3))/2 - (q2*SF(2))/2 - (q3*SF(1))/2; -SF(10) = q2/2 + (q0*SF(2))/2 + (q1*SF(1))/2 + (q3*SF(3))/2; -SF(11) = q2/2 - (q0*SF(2))/2 - (q1*SF(1))/2 + (q3*SF(3))/2; -SF(12) = q2/2 + (q0*SF(2))/2 - (q1*SF(1))/2 - (q3*SF(3))/2; -SF(13) = q1/2 + (q0*SF(3))/2 + (q2*SF(1))/2 + (q3*SF(2))/2; -SF(14) = q1/2 - (q0*SF(3))/2 + (q2*SF(1))/2 - (q3*SF(2))/2; -SF(15) = q3/2 + (q0*SF(1))/2 + (q1*SF(2))/2 + (q2*SF(3))/2; -SF(16) = - q0^2 - q1^2 - q2^2 - q3^2; -SF(17) = dvz_b - dvz + dvzNoise; -SF(18) = dvx - dvxNoise; -SF(19) = dvy - dvyNoise; -SF(20) = q2^2; -SF(21) = SF(20) - q0^2 + q1^2 - q3^2; -SF(22) = SF(20) + q0^2 - q1^2 - q3^2; -SF(23) = 2*q0*q1 - 2*q2*q3; -SF(24) = SF(20) - q0^2 - q1^2 + q3^2; -SF(25) = 2*q1*q2; +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(5,1); -SG(1) = - q0^2 - q1^2 - q2^2 - q3^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(10,1); -SQ(1) = - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2); -SQ(2) = dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvzNoise^2*(2*q0*q2 + 2*q1*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2); -SQ(3) = dvyNoise^2*(2*q0*q3 - 2*q1*q2)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxNoise^2*(2*q0*q3 + 2*q1*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3); -SQ(4) = SG(1)^2; -SQ(5) = dvyNoise^2; -SQ(6) = dvzNoise^2; -SQ(7) = dvxNoise^2; -SQ(8) = 2*q2*q3; -SQ(9) = 2*q1*q3; -SQ(10) = 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(23,1); -SPP(1) = SF(18)*(2*q0*q1 + 2*q2*q3) + SF(19)*(2*q0*q2 - 2*q1*q3); -SPP(2) = SF(19)*(2*q0*q2 + 2*q1*q3) + SF(17)*(SF(25) - 2*q0*q3); -SPP(3) = 2*q3*SF(9) + 2*q1*SF(12) - 2*q0*SF(15) - 2*q2*SF(14); -SPP(4) = 2*q1*SF(8) + 2*q2*SF(7) - 2*q0*SF(13) - 2*q3*SF(11); -SPP(5) = 2*q0*SF(7) - 2*q3*SF(8) - 2*q1*SF(11) + 2*q2*SF(13); -SPP(6) = 2*q0*SF(9) + 2*q2*SF(12) + 2*q1*SF(14) + 2*q3*SF(15); -SPP(7) = 2*q0*SF(8) + 2*q3*SF(7) + 2*q2*SF(11) + 2*q1*SF(13); -SPP(8) = 2*q1*SF(4) - 2*q2*SF(5) - 2*q3*SF(6) + 2*q0*SF(10); -SPP(9) = 2*q0*SF(6) - 2*q1*SF(5) - 2*q2*SF(4) + 2*q3*SF(10); -SPP(10) = SF(19)*SF(21) - SF(17)*(2*q0*q1 + 2*q2*q3); -SPP(11) = SF(18)*SF(21) + SF(17)*(2*q0*q2 - 2*q1*q3); -SPP(12) = SF(18)*SF(22) - SF(19)*(SF(25) + 2*q0*q3); -SPP(13) = SF(18)*SF(23) - SF(17)*(SF(25) + 2*q0*q3); -SPP(14) = 2*q0*SF(5) + 2*q1*SF(6) + 2*q3*SF(4) + 2*q2*SF(10); -SPP(15) = 2*q2*SF(9) - 2*q0*SF(12) - 2*q1*SF(15) + 2*q3*SF(14); -SPP(16) = SF(19)*SF(24) + SF(18)*(SF(25) - 2*q0*q3); -SPP(17) = daz*SF(20) + daz*q0^2 + daz*q1^2 + daz*q3^2; -SPP(18) = day*SF(20) + day*q0^2 + day*q1^2 + day*q3^2; -SPP(19) = dax*SF(20) + dax*q0^2 + dax*q1^2 + dax*q3^2; -SPP(20) = SF(17)*SF(24) - SF(18)*(2*q0*q2 + 2*q1*q3); -SPP(21) = SF(17)*SF(22) - SF(19)*SF(23); -SPP(22) = 2*q0*q2 + 2*q1*q3; -SPP(23) = SF(16); +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) = SPP(6)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) - SPP(5)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) + SPP(8)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) + SPP(23)*(OP_l_1_c_10_r_*SPP(6) - OP_l_2_c_10_r_*SPP(5) + OP_l_3_c_10_r_*SPP(8) + OP_l_10_c_10_r_*SPP(23) + OP_l_13_c_10_r_*SPP(19)) + SPP(19)*(OP_l_1_c_13_r_*SPP(6) - OP_l_2_c_13_r_*SPP(5) + OP_l_3_c_13_r_*SPP(8) + OP_l_10_c_13_r_*SPP(23) + OP_l_13_c_13_r_*SPP(19)) + daxNoise^2*SQ(4); -nextP(1,2) = SPP(7)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) - SPP(3)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) - SPP(9)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) + SPP(23)*(OP_l_1_c_11_r_*SPP(6) - OP_l_2_c_11_r_*SPP(5) + OP_l_3_c_11_r_*SPP(8) + OP_l_10_c_11_r_*SPP(23) + OP_l_13_c_11_r_*SPP(19)) + SPP(18)*(OP_l_1_c_14_r_*SPP(6) - OP_l_2_c_14_r_*SPP(5) + OP_l_3_c_14_r_*SPP(8) + OP_l_10_c_14_r_*SPP(23) + OP_l_13_c_14_r_*SPP(19)); -nextP(2,2) = SPP(7)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)) - SPP(3)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) - SPP(9)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)) + SPP(23)*(OP_l_2_c_11_r_*SPP(7) - OP_l_1_c_11_r_*SPP(3) - OP_l_3_c_11_r_*SPP(9) + OP_l_11_c_11_r_*SPP(23) + OP_l_14_c_11_r_*SPP(18)) + SPP(18)*(OP_l_2_c_14_r_*SPP(7) - OP_l_1_c_14_r_*SPP(3) - OP_l_3_c_14_r_*SPP(9) + OP_l_11_c_14_r_*SPP(23) + OP_l_14_c_14_r_*SPP(18)) + dayNoise^2*SQ(4); -nextP(1,3) = SPP(15)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) - SPP(4)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) + SPP(14)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) + SPP(23)*(OP_l_1_c_12_r_*SPP(6) - OP_l_2_c_12_r_*SPP(5) + OP_l_3_c_12_r_*SPP(8) + OP_l_10_c_12_r_*SPP(23) + OP_l_13_c_12_r_*SPP(19)) + SPP(17)*(OP_l_1_c_15_r_*SPP(6) - OP_l_2_c_15_r_*SPP(5) + OP_l_3_c_15_r_*SPP(8) + OP_l_10_c_15_r_*SPP(23) + OP_l_13_c_15_r_*SPP(19)); -nextP(2,3) = SPP(15)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) - SPP(4)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)) + SPP(14)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)) + SPP(23)*(OP_l_2_c_12_r_*SPP(7) - OP_l_1_c_12_r_*SPP(3) - OP_l_3_c_12_r_*SPP(9) + OP_l_11_c_12_r_*SPP(23) + OP_l_14_c_12_r_*SPP(18)) + SPP(17)*(OP_l_2_c_15_r_*SPP(7) - OP_l_1_c_15_r_*SPP(3) - OP_l_3_c_15_r_*SPP(9) + OP_l_11_c_15_r_*SPP(23) + OP_l_14_c_15_r_*SPP(18)); -nextP(3,3) = SPP(15)*(OP_l_1_c_1_r_*SPP(15) - OP_l_2_c_1_r_*SPP(4) + OP_l_3_c_1_r_*SPP(14) + OP_l_12_c_1_r_*SPP(23) + OP_l_15_c_1_r_*SPP(17)) - SPP(4)*(OP_l_1_c_2_r_*SPP(15) - OP_l_2_c_2_r_*SPP(4) + OP_l_3_c_2_r_*SPP(14) + OP_l_12_c_2_r_*SPP(23) + OP_l_15_c_2_r_*SPP(17)) + SPP(14)*(OP_l_1_c_3_r_*SPP(15) - OP_l_2_c_3_r_*SPP(4) + OP_l_3_c_3_r_*SPP(14) + OP_l_12_c_3_r_*SPP(23) + OP_l_15_c_3_r_*SPP(17)) + SPP(23)*(OP_l_1_c_12_r_*SPP(15) - OP_l_2_c_12_r_*SPP(4) + OP_l_3_c_12_r_*SPP(14) + OP_l_12_c_12_r_*SPP(23) + OP_l_15_c_12_r_*SPP(17)) + SPP(17)*(OP_l_1_c_15_r_*SPP(15) - OP_l_2_c_15_r_*SPP(4) + OP_l_3_c_15_r_*SPP(14) + OP_l_12_c_15_r_*SPP(23) + OP_l_15_c_15_r_*SPP(17)) + dazNoise^2*SQ(4); -nextP(1,4) = OP_l_1_c_4_r_*SPP(6) - OP_l_2_c_4_r_*SPP(5) + OP_l_3_c_4_r_*SPP(8) + OP_l_10_c_4_r_*SPP(23) + OP_l_13_c_4_r_*SPP(19) + SPP(2)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) + SPP(20)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) + SPP(16)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) - SPP(22)*(OP_l_1_c_16_r_*SPP(6) - OP_l_2_c_16_r_*SPP(5) + OP_l_3_c_16_r_*SPP(8) + OP_l_10_c_16_r_*SPP(23) + OP_l_13_c_16_r_*SPP(19)); -nextP(2,4) = OP_l_2_c_4_r_*SPP(7) - OP_l_1_c_4_r_*SPP(3) - OP_l_3_c_4_r_*SPP(9) + OP_l_11_c_4_r_*SPP(23) + OP_l_14_c_4_r_*SPP(18) + SPP(2)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) + SPP(20)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)) + SPP(16)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)) - SPP(22)*(OP_l_2_c_16_r_*SPP(7) - OP_l_1_c_16_r_*SPP(3) - OP_l_3_c_16_r_*SPP(9) + OP_l_11_c_16_r_*SPP(23) + OP_l_14_c_16_r_*SPP(18)); -nextP(3,4) = OP_l_1_c_4_r_*SPP(15) - OP_l_2_c_4_r_*SPP(4) + OP_l_3_c_4_r_*SPP(14) + OP_l_12_c_4_r_*SPP(23) + OP_l_15_c_4_r_*SPP(17) + SPP(2)*(OP_l_1_c_1_r_*SPP(15) - OP_l_2_c_1_r_*SPP(4) + OP_l_3_c_1_r_*SPP(14) + OP_l_12_c_1_r_*SPP(23) + OP_l_15_c_1_r_*SPP(17)) + SPP(20)*(OP_l_1_c_2_r_*SPP(15) - OP_l_2_c_2_r_*SPP(4) + OP_l_3_c_2_r_*SPP(14) + OP_l_12_c_2_r_*SPP(23) + OP_l_15_c_2_r_*SPP(17)) + SPP(16)*(OP_l_1_c_3_r_*SPP(15) - OP_l_2_c_3_r_*SPP(4) + OP_l_3_c_3_r_*SPP(14) + OP_l_12_c_3_r_*SPP(23) + OP_l_15_c_3_r_*SPP(17)) - SPP(22)*(OP_l_1_c_16_r_*SPP(15) - OP_l_2_c_16_r_*SPP(4) + OP_l_3_c_16_r_*SPP(14) + OP_l_12_c_16_r_*SPP(23) + OP_l_15_c_16_r_*SPP(17)); -nextP(4,4) = OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SPP(2) + OP_l_2_c_4_r_*SPP(20) + OP_l_3_c_4_r_*SPP(16) - OP_l_16_c_4_r_*SPP(22) + SQ(6)*(SQ(9) + 2*q0*q2)^2 + SQ(5)*(SQ(10) - 2*q0*q3)^2 + SPP(2)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(2) + OP_l_2_c_1_r_*SPP(20) + OP_l_3_c_1_r_*SPP(16) - OP_l_16_c_1_r_*SPP(22)) + SPP(20)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(2) + OP_l_2_c_2_r_*SPP(20) + OP_l_3_c_2_r_*SPP(16) - OP_l_16_c_2_r_*SPP(22)) + SPP(16)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(2) + OP_l_2_c_3_r_*SPP(20) + OP_l_3_c_3_r_*SPP(16) - OP_l_16_c_3_r_*SPP(22)) - SPP(22)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SPP(2) + OP_l_2_c_16_r_*SPP(20) + OP_l_3_c_16_r_*SPP(16) - OP_l_16_c_16_r_*SPP(22)) + SQ(7)*(SG(2) + SG(3) - SG(4) - SG(5))^2; -nextP(1,5) = OP_l_1_c_5_r_*SPP(6) - OP_l_2_c_5_r_*SPP(5) + OP_l_3_c_5_r_*SPP(8) + OP_l_10_c_5_r_*SPP(23) + OP_l_13_c_5_r_*SPP(19) + SF(23)*(OP_l_1_c_16_r_*SPP(6) - OP_l_2_c_16_r_*SPP(5) + OP_l_3_c_16_r_*SPP(8) + OP_l_10_c_16_r_*SPP(23) + OP_l_13_c_16_r_*SPP(19)) + SPP(13)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) + SPP(21)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) + SPP(12)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)); -nextP(2,5) = OP_l_2_c_5_r_*SPP(7) - OP_l_1_c_5_r_*SPP(3) - OP_l_3_c_5_r_*SPP(9) + OP_l_11_c_5_r_*SPP(23) + OP_l_14_c_5_r_*SPP(18) + SF(23)*(OP_l_2_c_16_r_*SPP(7) - OP_l_1_c_16_r_*SPP(3) - OP_l_3_c_16_r_*SPP(9) + OP_l_11_c_16_r_*SPP(23) + OP_l_14_c_16_r_*SPP(18)) + SPP(13)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)) + SPP(21)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) + SPP(12)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)); -nextP(3,5) = OP_l_1_c_5_r_*SPP(15) - OP_l_2_c_5_r_*SPP(4) + OP_l_3_c_5_r_*SPP(14) + OP_l_12_c_5_r_*SPP(23) + OP_l_15_c_5_r_*SPP(17) + SF(23)*(OP_l_1_c_16_r_*SPP(15) - OP_l_2_c_16_r_*SPP(4) + OP_l_3_c_16_r_*SPP(14) + OP_l_12_c_16_r_*SPP(23) + OP_l_15_c_16_r_*SPP(17)) + SPP(13)*(OP_l_1_c_2_r_*SPP(15) - OP_l_2_c_2_r_*SPP(4) + OP_l_3_c_2_r_*SPP(14) + OP_l_12_c_2_r_*SPP(23) + OP_l_15_c_2_r_*SPP(17)) + SPP(21)*(OP_l_1_c_1_r_*SPP(15) - OP_l_2_c_1_r_*SPP(4) + OP_l_3_c_1_r_*SPP(14) + OP_l_12_c_1_r_*SPP(23) + OP_l_15_c_1_r_*SPP(17)) + SPP(12)*(OP_l_1_c_3_r_*SPP(15) - OP_l_2_c_3_r_*SPP(4) + OP_l_3_c_3_r_*SPP(14) + OP_l_12_c_3_r_*SPP(23) + OP_l_15_c_3_r_*SPP(17)); -nextP(4,5) = OP_l_4_c_5_r_ + SQ(3) + OP_l_1_c_5_r_*SPP(2) + OP_l_2_c_5_r_*SPP(20) + OP_l_3_c_5_r_*SPP(16) - OP_l_16_c_5_r_*SPP(22) + SF(23)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SPP(2) + OP_l_2_c_16_r_*SPP(20) + OP_l_3_c_16_r_*SPP(16) - OP_l_16_c_16_r_*SPP(22)) + SPP(13)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(2) + OP_l_2_c_2_r_*SPP(20) + OP_l_3_c_2_r_*SPP(16) - OP_l_16_c_2_r_*SPP(22)) + SPP(21)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(2) + OP_l_2_c_1_r_*SPP(20) + OP_l_3_c_1_r_*SPP(16) - OP_l_16_c_1_r_*SPP(22)) + SPP(12)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(2) + OP_l_2_c_3_r_*SPP(20) + OP_l_3_c_3_r_*SPP(16) - OP_l_16_c_3_r_*SPP(22)); -nextP(5,5) = OP_l_5_c_5_r_ + OP_l_16_c_5_r_*SF(23) + OP_l_1_c_5_r_*SPP(21) + OP_l_2_c_5_r_*SPP(13) + OP_l_3_c_5_r_*SPP(12) + SQ(6)*(SQ(8) - 2*q0*q1)^2 + SQ(7)*(SQ(10) + 2*q0*q3)^2 + SF(23)*(OP_l_5_c_16_r_ + OP_l_16_c_16_r_*SF(23) + OP_l_1_c_16_r_*SPP(21) + OP_l_2_c_16_r_*SPP(13) + OP_l_3_c_16_r_*SPP(12)) + SPP(13)*(OP_l_5_c_2_r_ + OP_l_16_c_2_r_*SF(23) + OP_l_1_c_2_r_*SPP(21) + OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(12)) + SPP(21)*(OP_l_5_c_1_r_ + OP_l_16_c_1_r_*SF(23) + OP_l_1_c_1_r_*SPP(21) + OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(12)) + SPP(12)*(OP_l_5_c_3_r_ + OP_l_16_c_3_r_*SF(23) + OP_l_1_c_3_r_*SPP(21) + OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(12)) + SQ(5)*(SG(2) - SG(3) + SG(4) - SG(5))^2; -nextP(1,6) = OP_l_1_c_6_r_*SPP(6) - OP_l_2_c_6_r_*SPP(5) + OP_l_3_c_6_r_*SPP(8) + OP_l_10_c_6_r_*SPP(23) + OP_l_13_c_6_r_*SPP(19) + SF(21)*(OP_l_1_c_16_r_*SPP(6) - OP_l_2_c_16_r_*SPP(5) + OP_l_3_c_16_r_*SPP(8) + OP_l_10_c_16_r_*SPP(23) + OP_l_13_c_16_r_*SPP(19)) - SPP(10)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) + SPP(1)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) + SPP(11)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)); -nextP(2,6) = OP_l_2_c_6_r_*SPP(7) - OP_l_1_c_6_r_*SPP(3) - OP_l_3_c_6_r_*SPP(9) + OP_l_11_c_6_r_*SPP(23) + OP_l_14_c_6_r_*SPP(18) + SF(21)*(OP_l_2_c_16_r_*SPP(7) - OP_l_1_c_16_r_*SPP(3) - OP_l_3_c_16_r_*SPP(9) + OP_l_11_c_16_r_*SPP(23) + OP_l_14_c_16_r_*SPP(18)) - SPP(10)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) + SPP(1)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)) + SPP(11)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)); -nextP(3,6) = OP_l_1_c_6_r_*SPP(15) - OP_l_2_c_6_r_*SPP(4) + OP_l_3_c_6_r_*SPP(14) + OP_l_12_c_6_r_*SPP(23) + OP_l_15_c_6_r_*SPP(17) + SF(21)*(OP_l_1_c_16_r_*SPP(15) - OP_l_2_c_16_r_*SPP(4) + OP_l_3_c_16_r_*SPP(14) + OP_l_12_c_16_r_*SPP(23) + OP_l_15_c_16_r_*SPP(17)) - SPP(10)*(OP_l_1_c_1_r_*SPP(15) - OP_l_2_c_1_r_*SPP(4) + OP_l_3_c_1_r_*SPP(14) + OP_l_12_c_1_r_*SPP(23) + OP_l_15_c_1_r_*SPP(17)) + SPP(1)*(OP_l_1_c_3_r_*SPP(15) - OP_l_2_c_3_r_*SPP(4) + OP_l_3_c_3_r_*SPP(14) + OP_l_12_c_3_r_*SPP(23) + OP_l_15_c_3_r_*SPP(17)) + SPP(11)*(OP_l_1_c_2_r_*SPP(15) - OP_l_2_c_2_r_*SPP(4) + OP_l_3_c_2_r_*SPP(14) + OP_l_12_c_2_r_*SPP(23) + OP_l_15_c_2_r_*SPP(17)); -nextP(4,6) = OP_l_4_c_6_r_ + SQ(2) + OP_l_1_c_6_r_*SPP(2) + OP_l_2_c_6_r_*SPP(20) + OP_l_3_c_6_r_*SPP(16) - OP_l_16_c_6_r_*SPP(22) + SF(21)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SPP(2) + OP_l_2_c_16_r_*SPP(20) + OP_l_3_c_16_r_*SPP(16) - OP_l_16_c_16_r_*SPP(22)) - SPP(10)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(2) + OP_l_2_c_1_r_*SPP(20) + OP_l_3_c_1_r_*SPP(16) - OP_l_16_c_1_r_*SPP(22)) + SPP(1)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(2) + OP_l_2_c_3_r_*SPP(20) + OP_l_3_c_3_r_*SPP(16) - OP_l_16_c_3_r_*SPP(22)) + SPP(11)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(2) + OP_l_2_c_2_r_*SPP(20) + OP_l_3_c_2_r_*SPP(16) - OP_l_16_c_2_r_*SPP(22)); -nextP(5,6) = OP_l_5_c_6_r_ + SQ(1) + OP_l_16_c_6_r_*SF(23) + OP_l_1_c_6_r_*SPP(21) + OP_l_2_c_6_r_*SPP(13) + OP_l_3_c_6_r_*SPP(12) + SF(21)*(OP_l_5_c_16_r_ + OP_l_16_c_16_r_*SF(23) + OP_l_1_c_16_r_*SPP(21) + OP_l_2_c_16_r_*SPP(13) + OP_l_3_c_16_r_*SPP(12)) - SPP(10)*(OP_l_5_c_1_r_ + OP_l_16_c_1_r_*SF(23) + OP_l_1_c_1_r_*SPP(21) + OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(12)) + SPP(1)*(OP_l_5_c_3_r_ + OP_l_16_c_3_r_*SF(23) + OP_l_1_c_3_r_*SPP(21) + OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(12)) + SPP(11)*(OP_l_5_c_2_r_ + OP_l_16_c_2_r_*SF(23) + OP_l_1_c_2_r_*SPP(21) + OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(12)); -nextP(6,6) = OP_l_6_c_6_r_ + OP_l_16_c_6_r_*SF(21) - OP_l_1_c_6_r_*SPP(10) + OP_l_2_c_6_r_*SPP(11) + OP_l_3_c_6_r_*SPP(1) + SQ(5)*(SQ(8) + 2*q0*q1)^2 + SQ(7)*(SQ(9) - 2*q0*q2)^2 + SF(21)*(OP_l_6_c_16_r_ + OP_l_16_c_16_r_*SF(21) - OP_l_1_c_16_r_*SPP(10) + OP_l_2_c_16_r_*SPP(11) + OP_l_3_c_16_r_*SPP(1)) - SPP(10)*(OP_l_6_c_1_r_ + OP_l_16_c_1_r_*SF(21) - OP_l_1_c_1_r_*SPP(10) + OP_l_2_c_1_r_*SPP(11) + OP_l_3_c_1_r_*SPP(1)) + SPP(1)*(OP_l_6_c_3_r_ + OP_l_16_c_3_r_*SF(21) - OP_l_1_c_3_r_*SPP(10) + OP_l_2_c_3_r_*SPP(11) + OP_l_3_c_3_r_*SPP(1)) + SPP(11)*(OP_l_6_c_2_r_ + OP_l_16_c_2_r_*SF(21) - OP_l_1_c_2_r_*SPP(10) + OP_l_2_c_2_r_*SPP(11) + OP_l_3_c_2_r_*SPP(1)) + SQ(6)*(SG(2) - SG(3) - SG(4) + SG(5))^2; -nextP(1,7) = OP_l_1_c_7_r_*SPP(6) - OP_l_2_c_7_r_*SPP(5) + OP_l_3_c_7_r_*SPP(8) + OP_l_10_c_7_r_*SPP(23) + OP_l_13_c_7_r_*SPP(19) + dt*(OP_l_1_c_4_r_*SPP(6) - OP_l_2_c_4_r_*SPP(5) + OP_l_3_c_4_r_*SPP(8) + OP_l_10_c_4_r_*SPP(23) + OP_l_13_c_4_r_*SPP(19)); -nextP(2,7) = OP_l_2_c_7_r_*SPP(7) - OP_l_1_c_7_r_*SPP(3) - OP_l_3_c_7_r_*SPP(9) + OP_l_11_c_7_r_*SPP(23) + OP_l_14_c_7_r_*SPP(18) + dt*(OP_l_2_c_4_r_*SPP(7) - OP_l_1_c_4_r_*SPP(3) - OP_l_3_c_4_r_*SPP(9) + OP_l_11_c_4_r_*SPP(23) + OP_l_14_c_4_r_*SPP(18)); -nextP(3,7) = OP_l_1_c_7_r_*SPP(15) - OP_l_2_c_7_r_*SPP(4) + OP_l_3_c_7_r_*SPP(14) + OP_l_12_c_7_r_*SPP(23) + OP_l_15_c_7_r_*SPP(17) + dt*(OP_l_1_c_4_r_*SPP(15) - OP_l_2_c_4_r_*SPP(4) + OP_l_3_c_4_r_*SPP(14) + OP_l_12_c_4_r_*SPP(23) + OP_l_15_c_4_r_*SPP(17)); -nextP(4,7) = OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SPP(2) + OP_l_2_c_7_r_*SPP(20) + OP_l_3_c_7_r_*SPP(16) - OP_l_16_c_7_r_*SPP(22) + dt*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SPP(2) + OP_l_2_c_4_r_*SPP(20) + OP_l_3_c_4_r_*SPP(16) - OP_l_16_c_4_r_*SPP(22)); -nextP(5,7) = OP_l_5_c_7_r_ + OP_l_16_c_7_r_*SF(23) + OP_l_1_c_7_r_*SPP(21) + OP_l_2_c_7_r_*SPP(13) + OP_l_3_c_7_r_*SPP(12) + dt*(OP_l_5_c_4_r_ + OP_l_16_c_4_r_*SF(23) + OP_l_1_c_4_r_*SPP(21) + OP_l_2_c_4_r_*SPP(13) + OP_l_3_c_4_r_*SPP(12)); -nextP(6,7) = OP_l_6_c_7_r_ + OP_l_16_c_7_r_*SF(21) - OP_l_1_c_7_r_*SPP(10) + OP_l_2_c_7_r_*SPP(11) + OP_l_3_c_7_r_*SPP(1) + dt*(OP_l_6_c_4_r_ + OP_l_16_c_4_r_*SF(21) - OP_l_1_c_4_r_*SPP(10) + OP_l_2_c_4_r_*SPP(11) + OP_l_3_c_4_r_*SPP(1)); -nextP(7,7) = OP_l_7_c_7_r_ + OP_l_4_c_7_r_*dt + dt*(OP_l_7_c_4_r_ + OP_l_4_c_4_r_*dt); -nextP(1,8) = OP_l_1_c_8_r_*SPP(6) - OP_l_2_c_8_r_*SPP(5) + OP_l_3_c_8_r_*SPP(8) + OP_l_10_c_8_r_*SPP(23) + OP_l_13_c_8_r_*SPP(19) + dt*(OP_l_1_c_5_r_*SPP(6) - OP_l_2_c_5_r_*SPP(5) + OP_l_3_c_5_r_*SPP(8) + OP_l_10_c_5_r_*SPP(23) + OP_l_13_c_5_r_*SPP(19)); -nextP(2,8) = OP_l_2_c_8_r_*SPP(7) - OP_l_1_c_8_r_*SPP(3) - OP_l_3_c_8_r_*SPP(9) + OP_l_11_c_8_r_*SPP(23) + OP_l_14_c_8_r_*SPP(18) + dt*(OP_l_2_c_5_r_*SPP(7) - OP_l_1_c_5_r_*SPP(3) - OP_l_3_c_5_r_*SPP(9) + OP_l_11_c_5_r_*SPP(23) + OP_l_14_c_5_r_*SPP(18)); -nextP(3,8) = OP_l_1_c_8_r_*SPP(15) - OP_l_2_c_8_r_*SPP(4) + OP_l_3_c_8_r_*SPP(14) + OP_l_12_c_8_r_*SPP(23) + OP_l_15_c_8_r_*SPP(17) + dt*(OP_l_1_c_5_r_*SPP(15) - OP_l_2_c_5_r_*SPP(4) + OP_l_3_c_5_r_*SPP(14) + OP_l_12_c_5_r_*SPP(23) + OP_l_15_c_5_r_*SPP(17)); -nextP(4,8) = OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SPP(2) + OP_l_2_c_8_r_*SPP(20) + OP_l_3_c_8_r_*SPP(16) - OP_l_16_c_8_r_*SPP(22) + dt*(OP_l_4_c_5_r_ + OP_l_1_c_5_r_*SPP(2) + OP_l_2_c_5_r_*SPP(20) + OP_l_3_c_5_r_*SPP(16) - OP_l_16_c_5_r_*SPP(22)); -nextP(5,8) = OP_l_5_c_8_r_ + OP_l_16_c_8_r_*SF(23) + OP_l_1_c_8_r_*SPP(21) + OP_l_2_c_8_r_*SPP(13) + OP_l_3_c_8_r_*SPP(12) + dt*(OP_l_5_c_5_r_ + OP_l_16_c_5_r_*SF(23) + OP_l_1_c_5_r_*SPP(21) + OP_l_2_c_5_r_*SPP(13) + OP_l_3_c_5_r_*SPP(12)); -nextP(6,8) = OP_l_6_c_8_r_ + OP_l_16_c_8_r_*SF(21) - OP_l_1_c_8_r_*SPP(10) + OP_l_2_c_8_r_*SPP(11) + OP_l_3_c_8_r_*SPP(1) + dt*(OP_l_6_c_5_r_ + OP_l_16_c_5_r_*SF(21) - OP_l_1_c_5_r_*SPP(10) + OP_l_2_c_5_r_*SPP(11) + OP_l_3_c_5_r_*SPP(1)); -nextP(7,8) = OP_l_7_c_8_r_ + OP_l_4_c_8_r_*dt + dt*(OP_l_7_c_5_r_ + OP_l_4_c_5_r_*dt); +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_*SPP(6) - OP_l_2_c_9_r_*SPP(5) + OP_l_3_c_9_r_*SPP(8) + OP_l_10_c_9_r_*SPP(23) + OP_l_13_c_9_r_*SPP(19) + dt*(OP_l_1_c_6_r_*SPP(6) - OP_l_2_c_6_r_*SPP(5) + OP_l_3_c_6_r_*SPP(8) + OP_l_10_c_6_r_*SPP(23) + OP_l_13_c_6_r_*SPP(19)); -nextP(2,9) = OP_l_2_c_9_r_*SPP(7) - OP_l_1_c_9_r_*SPP(3) - OP_l_3_c_9_r_*SPP(9) + OP_l_11_c_9_r_*SPP(23) + OP_l_14_c_9_r_*SPP(18) + dt*(OP_l_2_c_6_r_*SPP(7) - OP_l_1_c_6_r_*SPP(3) - OP_l_3_c_6_r_*SPP(9) + OP_l_11_c_6_r_*SPP(23) + OP_l_14_c_6_r_*SPP(18)); -nextP(3,9) = OP_l_1_c_9_r_*SPP(15) - OP_l_2_c_9_r_*SPP(4) + OP_l_3_c_9_r_*SPP(14) + OP_l_12_c_9_r_*SPP(23) + OP_l_15_c_9_r_*SPP(17) + dt*(OP_l_1_c_6_r_*SPP(15) - OP_l_2_c_6_r_*SPP(4) + OP_l_3_c_6_r_*SPP(14) + OP_l_12_c_6_r_*SPP(23) + OP_l_15_c_6_r_*SPP(17)); -nextP(4,9) = OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SPP(2) + OP_l_2_c_9_r_*SPP(20) + OP_l_3_c_9_r_*SPP(16) - OP_l_16_c_9_r_*SPP(22) + dt*(OP_l_4_c_6_r_ + OP_l_1_c_6_r_*SPP(2) + OP_l_2_c_6_r_*SPP(20) + OP_l_3_c_6_r_*SPP(16) - OP_l_16_c_6_r_*SPP(22)); -nextP(5,9) = OP_l_5_c_9_r_ + OP_l_16_c_9_r_*SF(23) + OP_l_1_c_9_r_*SPP(21) + OP_l_2_c_9_r_*SPP(13) + OP_l_3_c_9_r_*SPP(12) + dt*(OP_l_5_c_6_r_ + OP_l_16_c_6_r_*SF(23) + OP_l_1_c_6_r_*SPP(21) + OP_l_2_c_6_r_*SPP(13) + OP_l_3_c_6_r_*SPP(12)); -nextP(6,9) = OP_l_6_c_9_r_ + OP_l_16_c_9_r_*SF(21) - OP_l_1_c_9_r_*SPP(10) + OP_l_2_c_9_r_*SPP(11) + OP_l_3_c_9_r_*SPP(1) + dt*(OP_l_6_c_6_r_ + OP_l_16_c_6_r_*SF(21) - OP_l_1_c_6_r_*SPP(10) + OP_l_2_c_6_r_*SPP(11) + OP_l_3_c_6_r_*SPP(1)); -nextP(7,9) = OP_l_7_c_9_r_ + OP_l_4_c_9_r_*dt + dt*(OP_l_7_c_6_r_ + OP_l_4_c_6_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_*SPP(6) - OP_l_2_c_10_r_*SPP(5) + OP_l_3_c_10_r_*SPP(8) + OP_l_10_c_10_r_*SPP(23) + OP_l_13_c_10_r_*SPP(19); -nextP(2,10) = OP_l_2_c_10_r_*SPP(7) - OP_l_1_c_10_r_*SPP(3) - OP_l_3_c_10_r_*SPP(9) + OP_l_11_c_10_r_*SPP(23) + OP_l_14_c_10_r_*SPP(18); -nextP(3,10) = OP_l_1_c_10_r_*SPP(15) - OP_l_2_c_10_r_*SPP(4) + OP_l_3_c_10_r_*SPP(14) + OP_l_12_c_10_r_*SPP(23) + OP_l_15_c_10_r_*SPP(17); -nextP(4,10) = OP_l_4_c_10_r_ + OP_l_1_c_10_r_*SPP(2) + OP_l_2_c_10_r_*SPP(20) + OP_l_3_c_10_r_*SPP(16) - OP_l_16_c_10_r_*SPP(22); -nextP(5,10) = OP_l_5_c_10_r_ + OP_l_16_c_10_r_*SF(23) + OP_l_1_c_10_r_*SPP(21) + OP_l_2_c_10_r_*SPP(13) + OP_l_3_c_10_r_*SPP(12); -nextP(6,10) = OP_l_6_c_10_r_ + OP_l_16_c_10_r_*SF(21) - OP_l_1_c_10_r_*SPP(10) + OP_l_2_c_10_r_*SPP(11) + OP_l_3_c_10_r_*SPP(1); -nextP(7,10) = OP_l_7_c_10_r_ + OP_l_4_c_10_r_*dt; -nextP(8,10) = OP_l_8_c_10_r_ + OP_l_5_c_10_r_*dt; -nextP(9,10) = OP_l_9_c_10_r_ + OP_l_6_c_10_r_*dt; -nextP(10,10) = OP_l_10_c_10_r_; -nextP(1,11) = OP_l_1_c_11_r_*SPP(6) - OP_l_2_c_11_r_*SPP(5) + OP_l_3_c_11_r_*SPP(8) + OP_l_10_c_11_r_*SPP(23) + OP_l_13_c_11_r_*SPP(19); -nextP(2,11) = OP_l_2_c_11_r_*SPP(7) - OP_l_1_c_11_r_*SPP(3) - OP_l_3_c_11_r_*SPP(9) + OP_l_11_c_11_r_*SPP(23) + OP_l_14_c_11_r_*SPP(18); -nextP(3,11) = OP_l_1_c_11_r_*SPP(15) - OP_l_2_c_11_r_*SPP(4) + OP_l_3_c_11_r_*SPP(14) + OP_l_12_c_11_r_*SPP(23) + OP_l_15_c_11_r_*SPP(17); -nextP(4,11) = OP_l_4_c_11_r_ + OP_l_1_c_11_r_*SPP(2) + OP_l_2_c_11_r_*SPP(20) + OP_l_3_c_11_r_*SPP(16) - OP_l_16_c_11_r_*SPP(22); -nextP(5,11) = OP_l_5_c_11_r_ + OP_l_16_c_11_r_*SF(23) + OP_l_1_c_11_r_*SPP(21) + OP_l_2_c_11_r_*SPP(13) + OP_l_3_c_11_r_*SPP(12); -nextP(6,11) = OP_l_6_c_11_r_ + OP_l_16_c_11_r_*SF(21) - OP_l_1_c_11_r_*SPP(10) + OP_l_2_c_11_r_*SPP(11) + OP_l_3_c_11_r_*SPP(1); -nextP(7,11) = OP_l_7_c_11_r_ + OP_l_4_c_11_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_; +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_*SPP(6) - OP_l_2_c_12_r_*SPP(5) + OP_l_3_c_12_r_*SPP(8) + OP_l_10_c_12_r_*SPP(23) + OP_l_13_c_12_r_*SPP(19); -nextP(2,12) = OP_l_2_c_12_r_*SPP(7) - OP_l_1_c_12_r_*SPP(3) - OP_l_3_c_12_r_*SPP(9) + OP_l_11_c_12_r_*SPP(23) + OP_l_14_c_12_r_*SPP(18); -nextP(3,12) = OP_l_1_c_12_r_*SPP(15) - OP_l_2_c_12_r_*SPP(4) + OP_l_3_c_12_r_*SPP(14) + OP_l_12_c_12_r_*SPP(23) + OP_l_15_c_12_r_*SPP(17); -nextP(4,12) = OP_l_4_c_12_r_ + OP_l_1_c_12_r_*SPP(2) + OP_l_2_c_12_r_*SPP(20) + OP_l_3_c_12_r_*SPP(16) - OP_l_16_c_12_r_*SPP(22); -nextP(5,12) = OP_l_5_c_12_r_ + OP_l_16_c_12_r_*SF(23) + OP_l_1_c_12_r_*SPP(21) + OP_l_2_c_12_r_*SPP(13) + OP_l_3_c_12_r_*SPP(12); -nextP(6,12) = OP_l_6_c_12_r_ + OP_l_16_c_12_r_*SF(21) - OP_l_1_c_12_r_*SPP(10) + OP_l_2_c_12_r_*SPP(11) + OP_l_3_c_12_r_*SPP(1); -nextP(7,12) = OP_l_7_c_12_r_ + OP_l_4_c_12_r_*dt; +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_; +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_*SPP(6) - OP_l_2_c_13_r_*SPP(5) + OP_l_3_c_13_r_*SPP(8) + OP_l_10_c_13_r_*SPP(23) + OP_l_13_c_13_r_*SPP(19); -nextP(2,13) = OP_l_2_c_13_r_*SPP(7) - OP_l_1_c_13_r_*SPP(3) - OP_l_3_c_13_r_*SPP(9) + OP_l_11_c_13_r_*SPP(23) + OP_l_14_c_13_r_*SPP(18); -nextP(3,13) = OP_l_1_c_13_r_*SPP(15) - OP_l_2_c_13_r_*SPP(4) + OP_l_3_c_13_r_*SPP(14) + OP_l_12_c_13_r_*SPP(23) + OP_l_15_c_13_r_*SPP(17); -nextP(4,13) = OP_l_4_c_13_r_ + OP_l_1_c_13_r_*SPP(2) + OP_l_2_c_13_r_*SPP(20) + OP_l_3_c_13_r_*SPP(16) - OP_l_16_c_13_r_*SPP(22); -nextP(5,13) = OP_l_5_c_13_r_ + OP_l_16_c_13_r_*SF(23) + OP_l_1_c_13_r_*SPP(21) + OP_l_2_c_13_r_*SPP(13) + OP_l_3_c_13_r_*SPP(12); -nextP(6,13) = OP_l_6_c_13_r_ + OP_l_16_c_13_r_*SF(21) - OP_l_1_c_13_r_*SPP(10) + OP_l_2_c_13_r_*SPP(11) + OP_l_3_c_13_r_*SPP(1); -nextP(7,13) = OP_l_7_c_13_r_ + OP_l_4_c_13_r_*dt; +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_; +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_*SPP(6) - OP_l_2_c_14_r_*SPP(5) + OP_l_3_c_14_r_*SPP(8) + OP_l_10_c_14_r_*SPP(23) + OP_l_13_c_14_r_*SPP(19); -nextP(2,14) = OP_l_2_c_14_r_*SPP(7) - OP_l_1_c_14_r_*SPP(3) - OP_l_3_c_14_r_*SPP(9) + OP_l_11_c_14_r_*SPP(23) + OP_l_14_c_14_r_*SPP(18); -nextP(3,14) = OP_l_1_c_14_r_*SPP(15) - OP_l_2_c_14_r_*SPP(4) + OP_l_3_c_14_r_*SPP(14) + OP_l_12_c_14_r_*SPP(23) + OP_l_15_c_14_r_*SPP(17); -nextP(4,14) = OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SPP(2) + OP_l_2_c_14_r_*SPP(20) + OP_l_3_c_14_r_*SPP(16) - OP_l_16_c_14_r_*SPP(22); -nextP(5,14) = OP_l_5_c_14_r_ + OP_l_16_c_14_r_*SF(23) + OP_l_1_c_14_r_*SPP(21) + OP_l_2_c_14_r_*SPP(13) + OP_l_3_c_14_r_*SPP(12); -nextP(6,14) = OP_l_6_c_14_r_ + OP_l_16_c_14_r_*SF(21) - OP_l_1_c_14_r_*SPP(10) + OP_l_2_c_14_r_*SPP(11) + OP_l_3_c_14_r_*SPP(1); -nextP(7,14) = OP_l_7_c_14_r_ + OP_l_4_c_14_r_*dt; +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_; +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_*SPP(6) - OP_l_2_c_15_r_*SPP(5) + OP_l_3_c_15_r_*SPP(8) + OP_l_10_c_15_r_*SPP(23) + OP_l_13_c_15_r_*SPP(19); -nextP(2,15) = OP_l_2_c_15_r_*SPP(7) - OP_l_1_c_15_r_*SPP(3) - OP_l_3_c_15_r_*SPP(9) + OP_l_11_c_15_r_*SPP(23) + OP_l_14_c_15_r_*SPP(18); -nextP(3,15) = OP_l_1_c_15_r_*SPP(15) - OP_l_2_c_15_r_*SPP(4) + OP_l_3_c_15_r_*SPP(14) + OP_l_12_c_15_r_*SPP(23) + OP_l_15_c_15_r_*SPP(17); -nextP(4,15) = OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SPP(2) + OP_l_2_c_15_r_*SPP(20) + OP_l_3_c_15_r_*SPP(16) - OP_l_16_c_15_r_*SPP(22); -nextP(5,15) = OP_l_5_c_15_r_ + OP_l_16_c_15_r_*SF(23) + OP_l_1_c_15_r_*SPP(21) + OP_l_2_c_15_r_*SPP(13) + OP_l_3_c_15_r_*SPP(12); -nextP(6,15) = OP_l_6_c_15_r_ + OP_l_16_c_15_r_*SF(21) - OP_l_1_c_15_r_*SPP(10) + OP_l_2_c_15_r_*SPP(11) + OP_l_3_c_15_r_*SPP(1); -nextP(7,15) = OP_l_7_c_15_r_ + OP_l_4_c_15_r_*dt; +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_; +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_*SPP(6) - OP_l_2_c_16_r_*SPP(5) + OP_l_3_c_16_r_*SPP(8) + OP_l_10_c_16_r_*SPP(23) + OP_l_13_c_16_r_*SPP(19); -nextP(2,16) = OP_l_2_c_16_r_*SPP(7) - OP_l_1_c_16_r_*SPP(3) - OP_l_3_c_16_r_*SPP(9) + OP_l_11_c_16_r_*SPP(23) + OP_l_14_c_16_r_*SPP(18); -nextP(3,16) = OP_l_1_c_16_r_*SPP(15) - OP_l_2_c_16_r_*SPP(4) + OP_l_3_c_16_r_*SPP(14) + OP_l_12_c_16_r_*SPP(23) + OP_l_15_c_16_r_*SPP(17); -nextP(4,16) = OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SPP(2) + OP_l_2_c_16_r_*SPP(20) + OP_l_3_c_16_r_*SPP(16) - OP_l_16_c_16_r_*SPP(22); -nextP(5,16) = OP_l_5_c_16_r_ + OP_l_16_c_16_r_*SF(23) + OP_l_1_c_16_r_*SPP(21) + OP_l_2_c_16_r_*SPP(13) + OP_l_3_c_16_r_*SPP(12); -nextP(6,16) = OP_l_6_c_16_r_ + OP_l_16_c_16_r_*SF(21) - OP_l_1_c_16_r_*SPP(10) + OP_l_2_c_16_r_*SPP(11) + OP_l_3_c_16_r_*SPP(1); -nextP(7,16) = OP_l_7_c_16_r_ + OP_l_4_c_16_r_*dt; +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_; +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_*SPP(6) - OP_l_2_c_17_r_*SPP(5) + OP_l_3_c_17_r_*SPP(8) + OP_l_10_c_17_r_*SPP(23) + OP_l_13_c_17_r_*SPP(19); -nextP(2,17) = OP_l_2_c_17_r_*SPP(7) - OP_l_1_c_17_r_*SPP(3) - OP_l_3_c_17_r_*SPP(9) + OP_l_11_c_17_r_*SPP(23) + OP_l_14_c_17_r_*SPP(18); -nextP(3,17) = OP_l_1_c_17_r_*SPP(15) - OP_l_2_c_17_r_*SPP(4) + OP_l_3_c_17_r_*SPP(14) + OP_l_12_c_17_r_*SPP(23) + OP_l_15_c_17_r_*SPP(17); -nextP(4,17) = OP_l_4_c_17_r_ + OP_l_1_c_17_r_*SPP(2) + OP_l_2_c_17_r_*SPP(20) + OP_l_3_c_17_r_*SPP(16) - OP_l_16_c_17_r_*SPP(22); -nextP(5,17) = OP_l_5_c_17_r_ + OP_l_16_c_17_r_*SF(23) + OP_l_1_c_17_r_*SPP(21) + OP_l_2_c_17_r_*SPP(13) + OP_l_3_c_17_r_*SPP(12); -nextP(6,17) = OP_l_6_c_17_r_ + OP_l_16_c_17_r_*SF(21) - OP_l_1_c_17_r_*SPP(10) + OP_l_2_c_17_r_*SPP(11) + OP_l_3_c_17_r_*SPP(1); -nextP(7,17) = OP_l_7_c_17_r_ + OP_l_4_c_17_r_*dt; +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_; +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_; @@ -226,16 +214,16 @@ 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_*SPP(6) - OP_l_2_c_18_r_*SPP(5) + OP_l_3_c_18_r_*SPP(8) + OP_l_10_c_18_r_*SPP(23) + OP_l_13_c_18_r_*SPP(19); -nextP(2,18) = OP_l_2_c_18_r_*SPP(7) - OP_l_1_c_18_r_*SPP(3) - OP_l_3_c_18_r_*SPP(9) + OP_l_11_c_18_r_*SPP(23) + OP_l_14_c_18_r_*SPP(18); -nextP(3,18) = OP_l_1_c_18_r_*SPP(15) - OP_l_2_c_18_r_*SPP(4) + OP_l_3_c_18_r_*SPP(14) + OP_l_12_c_18_r_*SPP(23) + OP_l_15_c_18_r_*SPP(17); -nextP(4,18) = OP_l_4_c_18_r_ + OP_l_1_c_18_r_*SPP(2) + OP_l_2_c_18_r_*SPP(20) + OP_l_3_c_18_r_*SPP(16) - OP_l_16_c_18_r_*SPP(22); -nextP(5,18) = OP_l_5_c_18_r_ + OP_l_16_c_18_r_*SF(23) + OP_l_1_c_18_r_*SPP(21) + OP_l_2_c_18_r_*SPP(13) + OP_l_3_c_18_r_*SPP(12); -nextP(6,18) = OP_l_6_c_18_r_ + OP_l_16_c_18_r_*SF(21) - OP_l_1_c_18_r_*SPP(10) + OP_l_2_c_18_r_*SPP(11) + OP_l_3_c_18_r_*SPP(1); -nextP(7,18) = OP_l_7_c_18_r_ + OP_l_4_c_18_r_*dt; +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_; +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_; @@ -244,16 +232,16 @@ 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_*SPP(6) - OP_l_2_c_19_r_*SPP(5) + OP_l_3_c_19_r_*SPP(8) + OP_l_10_c_19_r_*SPP(23) + OP_l_13_c_19_r_*SPP(19); -nextP(2,19) = OP_l_2_c_19_r_*SPP(7) - OP_l_1_c_19_r_*SPP(3) - OP_l_3_c_19_r_*SPP(9) + OP_l_11_c_19_r_*SPP(23) + OP_l_14_c_19_r_*SPP(18); -nextP(3,19) = OP_l_1_c_19_r_*SPP(15) - OP_l_2_c_19_r_*SPP(4) + OP_l_3_c_19_r_*SPP(14) + OP_l_12_c_19_r_*SPP(23) + OP_l_15_c_19_r_*SPP(17); -nextP(4,19) = OP_l_4_c_19_r_ + OP_l_1_c_19_r_*SPP(2) + OP_l_2_c_19_r_*SPP(20) + OP_l_3_c_19_r_*SPP(16) - OP_l_16_c_19_r_*SPP(22); -nextP(5,19) = OP_l_5_c_19_r_ + OP_l_16_c_19_r_*SF(23) + OP_l_1_c_19_r_*SPP(21) + OP_l_2_c_19_r_*SPP(13) + OP_l_3_c_19_r_*SPP(12); -nextP(6,19) = OP_l_6_c_19_r_ + OP_l_16_c_19_r_*SF(21) - OP_l_1_c_19_r_*SPP(10) + OP_l_2_c_19_r_*SPP(11) + OP_l_3_c_19_r_*SPP(1); -nextP(7,19) = OP_l_7_c_19_r_ + OP_l_4_c_19_r_*dt; +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_; +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_; @@ -263,16 +251,16 @@ 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_*SPP(6) - OP_l_2_c_20_r_*SPP(5) + OP_l_3_c_20_r_*SPP(8) + OP_l_10_c_20_r_*SPP(23) + OP_l_13_c_20_r_*SPP(19); -nextP(2,20) = OP_l_2_c_20_r_*SPP(7) - OP_l_1_c_20_r_*SPP(3) - OP_l_3_c_20_r_*SPP(9) + OP_l_11_c_20_r_*SPP(23) + OP_l_14_c_20_r_*SPP(18); -nextP(3,20) = OP_l_1_c_20_r_*SPP(15) - OP_l_2_c_20_r_*SPP(4) + OP_l_3_c_20_r_*SPP(14) + OP_l_12_c_20_r_*SPP(23) + OP_l_15_c_20_r_*SPP(17); -nextP(4,20) = OP_l_4_c_20_r_ + OP_l_1_c_20_r_*SPP(2) + OP_l_2_c_20_r_*SPP(20) + OP_l_3_c_20_r_*SPP(16) - OP_l_16_c_20_r_*SPP(22); -nextP(5,20) = OP_l_5_c_20_r_ + OP_l_16_c_20_r_*SF(23) + OP_l_1_c_20_r_*SPP(21) + OP_l_2_c_20_r_*SPP(13) + OP_l_3_c_20_r_*SPP(12); -nextP(6,20) = OP_l_6_c_20_r_ + OP_l_16_c_20_r_*SF(21) - OP_l_1_c_20_r_*SPP(10) + OP_l_2_c_20_r_*SPP(11) + OP_l_3_c_20_r_*SPP(1); -nextP(7,20) = OP_l_7_c_20_r_ + OP_l_4_c_20_r_*dt; +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_; +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_; @@ -283,16 +271,16 @@ 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_*SPP(6) - OP_l_2_c_21_r_*SPP(5) + OP_l_3_c_21_r_*SPP(8) + OP_l_10_c_21_r_*SPP(23) + OP_l_13_c_21_r_*SPP(19); -nextP(2,21) = OP_l_2_c_21_r_*SPP(7) - OP_l_1_c_21_r_*SPP(3) - OP_l_3_c_21_r_*SPP(9) + OP_l_11_c_21_r_*SPP(23) + OP_l_14_c_21_r_*SPP(18); -nextP(3,21) = OP_l_1_c_21_r_*SPP(15) - OP_l_2_c_21_r_*SPP(4) + OP_l_3_c_21_r_*SPP(14) + OP_l_12_c_21_r_*SPP(23) + OP_l_15_c_21_r_*SPP(17); -nextP(4,21) = OP_l_4_c_21_r_ + OP_l_1_c_21_r_*SPP(2) + OP_l_2_c_21_r_*SPP(20) + OP_l_3_c_21_r_*SPP(16) - OP_l_16_c_21_r_*SPP(22); -nextP(5,21) = OP_l_5_c_21_r_ + OP_l_16_c_21_r_*SF(23) + OP_l_1_c_21_r_*SPP(21) + OP_l_2_c_21_r_*SPP(13) + OP_l_3_c_21_r_*SPP(12); -nextP(6,21) = OP_l_6_c_21_r_ + OP_l_16_c_21_r_*SF(21) - OP_l_1_c_21_r_*SPP(10) + OP_l_2_c_21_r_*SPP(11) + OP_l_3_c_21_r_*SPP(1); -nextP(7,21) = OP_l_7_c_21_r_ + OP_l_4_c_21_r_*dt; +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_; +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_; @@ -304,16 +292,16 @@ 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_*SPP(6) - OP_l_2_c_22_r_*SPP(5) + OP_l_3_c_22_r_*SPP(8) + OP_l_10_c_22_r_*SPP(23) + OP_l_13_c_22_r_*SPP(19); -nextP(2,22) = OP_l_2_c_22_r_*SPP(7) - OP_l_1_c_22_r_*SPP(3) - OP_l_3_c_22_r_*SPP(9) + OP_l_11_c_22_r_*SPP(23) + OP_l_14_c_22_r_*SPP(18); -nextP(3,22) = OP_l_1_c_22_r_*SPP(15) - OP_l_2_c_22_r_*SPP(4) + OP_l_3_c_22_r_*SPP(14) + OP_l_12_c_22_r_*SPP(23) + OP_l_15_c_22_r_*SPP(17); -nextP(4,22) = OP_l_4_c_22_r_ + OP_l_1_c_22_r_*SPP(2) + OP_l_2_c_22_r_*SPP(20) + OP_l_3_c_22_r_*SPP(16) - OP_l_16_c_22_r_*SPP(22); -nextP(5,22) = OP_l_5_c_22_r_ + OP_l_16_c_22_r_*SF(23) + OP_l_1_c_22_r_*SPP(21) + OP_l_2_c_22_r_*SPP(13) + OP_l_3_c_22_r_*SPP(12); -nextP(6,22) = OP_l_6_c_22_r_ + OP_l_16_c_22_r_*SF(21) - OP_l_1_c_22_r_*SPP(10) + OP_l_2_c_22_r_*SPP(11) + OP_l_3_c_22_r_*SPP(1); -nextP(7,22) = OP_l_7_c_22_r_ + OP_l_4_c_22_r_*dt; +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_; +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_; @@ -326,16 +314,16 @@ 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_*SPP(6) - OP_l_2_c_23_r_*SPP(5) + OP_l_3_c_23_r_*SPP(8) + OP_l_10_c_23_r_*SPP(23) + OP_l_13_c_23_r_*SPP(19); -nextP(2,23) = OP_l_2_c_23_r_*SPP(7) - OP_l_1_c_23_r_*SPP(3) - OP_l_3_c_23_r_*SPP(9) + OP_l_11_c_23_r_*SPP(23) + OP_l_14_c_23_r_*SPP(18); -nextP(3,23) = OP_l_1_c_23_r_*SPP(15) - OP_l_2_c_23_r_*SPP(4) + OP_l_3_c_23_r_*SPP(14) + OP_l_12_c_23_r_*SPP(23) + OP_l_15_c_23_r_*SPP(17); -nextP(4,23) = OP_l_4_c_23_r_ + OP_l_1_c_23_r_*SPP(2) + OP_l_2_c_23_r_*SPP(20) + OP_l_3_c_23_r_*SPP(16) - OP_l_16_c_23_r_*SPP(22); -nextP(5,23) = OP_l_5_c_23_r_ + OP_l_16_c_23_r_*SF(23) + OP_l_1_c_23_r_*SPP(21) + OP_l_2_c_23_r_*SPP(13) + OP_l_3_c_23_r_*SPP(12); -nextP(6,23) = OP_l_6_c_23_r_ + OP_l_16_c_23_r_*SF(21) - OP_l_1_c_23_r_*SPP(10) + OP_l_2_c_23_r_*SPP(11) + OP_l_3_c_23_r_*SPP(1); -nextP(7,23) = OP_l_7_c_23_r_ + OP_l_4_c_23_r_*dt; +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_; +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_; @@ -349,16 +337,16 @@ 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_*SPP(6) - OP_l_2_c_24_r_*SPP(5) + OP_l_3_c_24_r_*SPP(8) + OP_l_10_c_24_r_*SPP(23) + OP_l_13_c_24_r_*SPP(19); -nextP(2,24) = OP_l_2_c_24_r_*SPP(7) - OP_l_1_c_24_r_*SPP(3) - OP_l_3_c_24_r_*SPP(9) + OP_l_11_c_24_r_*SPP(23) + OP_l_14_c_24_r_*SPP(18); -nextP(3,24) = OP_l_1_c_24_r_*SPP(15) - OP_l_2_c_24_r_*SPP(4) + OP_l_3_c_24_r_*SPP(14) + OP_l_12_c_24_r_*SPP(23) + OP_l_15_c_24_r_*SPP(17); -nextP(4,24) = OP_l_4_c_24_r_ + OP_l_1_c_24_r_*SPP(2) + OP_l_2_c_24_r_*SPP(20) + OP_l_3_c_24_r_*SPP(16) - OP_l_16_c_24_r_*SPP(22); -nextP(5,24) = OP_l_5_c_24_r_ + OP_l_16_c_24_r_*SF(23) + OP_l_1_c_24_r_*SPP(21) + OP_l_2_c_24_r_*SPP(13) + OP_l_3_c_24_r_*SPP(12); -nextP(6,24) = OP_l_6_c_24_r_ + OP_l_16_c_24_r_*SF(21) - OP_l_1_c_24_r_*SPP(10) + OP_l_2_c_24_r_*SPP(11) + OP_l_3_c_24_r_*SPP(1); -nextP(7,24) = OP_l_7_c_24_r_ + OP_l_4_c_24_r_*dt; +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_; +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_; @@ -381,357 +369,374 @@ 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,4) = SH_TAS(3); -H_TAS(1,5) = SH_TAS(2); -H_TAS(1,6) = vd*SH_TAS(1); +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_4_c_4_r_*SH_TAS(3) + OP_l_5_c_4_r_*SH_TAS(2) - OP_l_23_c_4_r_*SH_TAS(3) - OP_l_24_c_4_r_*SH_TAS(2) + OP_l_6_c_4_r_*vd*SH_TAS(1)) + SH_TAS(2)*(OP_l_4_c_5_r_*SH_TAS(3) + OP_l_5_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_6_c_5_r_*vd*SH_TAS(1)) - SH_TAS(3)*(OP_l_4_c_23_r_*SH_TAS(3) + OP_l_5_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_6_c_23_r_*vd*SH_TAS(1)) - SH_TAS(2)*(OP_l_4_c_24_r_*SH_TAS(3) + OP_l_5_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_6_c_24_r_*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP_l_4_c_6_r_*SH_TAS(3) + OP_l_5_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_6_c_6_r_*vd*SH_TAS(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_4_r_*SH_TAS(3) - OP_l_1_c_23_r_*SH_TAS(3) + OP_l_1_c_5_r_*SK_TAS(2) - OP_l_1_c_24_r_*SK_TAS(2) + OP_l_1_c_6_r_*vd*SH_TAS(1)); -Kfusion(2) = SK_TAS(1)*(OP_l_2_c_4_r_*SH_TAS(3) - OP_l_2_c_23_r_*SH_TAS(3) + OP_l_2_c_5_r_*SK_TAS(2) - OP_l_2_c_24_r_*SK_TAS(2) + OP_l_2_c_6_r_*vd*SH_TAS(1)); -Kfusion(3) = SK_TAS(1)*(OP_l_3_c_4_r_*SH_TAS(3) - OP_l_3_c_23_r_*SH_TAS(3) + OP_l_3_c_5_r_*SK_TAS(2) - OP_l_3_c_24_r_*SK_TAS(2) + OP_l_3_c_6_r_*vd*SH_TAS(1)); -Kfusion(4) = SK_TAS(1)*(OP_l_4_c_4_r_*SH_TAS(3) - OP_l_4_c_23_r_*SH_TAS(3) + OP_l_4_c_5_r_*SK_TAS(2) - OP_l_4_c_24_r_*SK_TAS(2) + OP_l_4_c_6_r_*vd*SH_TAS(1)); -Kfusion(5) = SK_TAS(1)*(OP_l_5_c_4_r_*SH_TAS(3) - OP_l_5_c_23_r_*SH_TAS(3) + OP_l_5_c_5_r_*SK_TAS(2) - OP_l_5_c_24_r_*SK_TAS(2) + OP_l_5_c_6_r_*vd*SH_TAS(1)); -Kfusion(6) = SK_TAS(1)*(OP_l_6_c_4_r_*SH_TAS(3) - OP_l_6_c_23_r_*SH_TAS(3) + OP_l_6_c_5_r_*SK_TAS(2) - OP_l_6_c_24_r_*SK_TAS(2) + OP_l_6_c_6_r_*vd*SH_TAS(1)); -Kfusion(7) = SK_TAS(1)*(OP_l_7_c_4_r_*SH_TAS(3) - OP_l_7_c_23_r_*SH_TAS(3) + OP_l_7_c_5_r_*SK_TAS(2) - OP_l_7_c_24_r_*SK_TAS(2) + OP_l_7_c_6_r_*vd*SH_TAS(1)); -Kfusion(8) = SK_TAS(1)*(OP_l_8_c_4_r_*SH_TAS(3) - OP_l_8_c_23_r_*SH_TAS(3) + OP_l_8_c_5_r_*SK_TAS(2) - OP_l_8_c_24_r_*SK_TAS(2) + OP_l_8_c_6_r_*vd*SH_TAS(1)); -Kfusion(9) = SK_TAS(1)*(OP_l_9_c_4_r_*SH_TAS(3) - OP_l_9_c_23_r_*SH_TAS(3) + OP_l_9_c_5_r_*SK_TAS(2) - OP_l_9_c_24_r_*SK_TAS(2) + OP_l_9_c_6_r_*vd*SH_TAS(1)); -Kfusion(10) = SK_TAS(1)*(OP_l_10_c_4_r_*SH_TAS(3) - OP_l_10_c_23_r_*SH_TAS(3) + OP_l_10_c_5_r_*SK_TAS(2) - OP_l_10_c_24_r_*SK_TAS(2) + OP_l_10_c_6_r_*vd*SH_TAS(1)); -Kfusion(11) = SK_TAS(1)*(OP_l_11_c_4_r_*SH_TAS(3) - OP_l_11_c_23_r_*SH_TAS(3) + OP_l_11_c_5_r_*SK_TAS(2) - OP_l_11_c_24_r_*SK_TAS(2) + OP_l_11_c_6_r_*vd*SH_TAS(1)); -Kfusion(12) = SK_TAS(1)*(OP_l_12_c_4_r_*SH_TAS(3) - OP_l_12_c_23_r_*SH_TAS(3) + OP_l_12_c_5_r_*SK_TAS(2) - OP_l_12_c_24_r_*SK_TAS(2) + OP_l_12_c_6_r_*vd*SH_TAS(1)); -Kfusion(13) = SK_TAS(1)*(OP_l_13_c_4_r_*SH_TAS(3) - OP_l_13_c_23_r_*SH_TAS(3) + OP_l_13_c_5_r_*SK_TAS(2) - OP_l_13_c_24_r_*SK_TAS(2) + OP_l_13_c_6_r_*vd*SH_TAS(1)); -Kfusion(14) = SK_TAS(1)*(OP_l_14_c_4_r_*SH_TAS(3) - OP_l_14_c_23_r_*SH_TAS(3) + OP_l_14_c_5_r_*SK_TAS(2) - OP_l_14_c_24_r_*SK_TAS(2) + OP_l_14_c_6_r_*vd*SH_TAS(1)); -Kfusion(15) = SK_TAS(1)*(OP_l_15_c_4_r_*SH_TAS(3) - OP_l_15_c_23_r_*SH_TAS(3) + OP_l_15_c_5_r_*SK_TAS(2) - OP_l_15_c_24_r_*SK_TAS(2) + OP_l_15_c_6_r_*vd*SH_TAS(1)); -Kfusion(16) = SK_TAS(1)*(OP_l_16_c_4_r_*SH_TAS(3) - OP_l_16_c_23_r_*SH_TAS(3) + OP_l_16_c_5_r_*SK_TAS(2) - OP_l_16_c_24_r_*SK_TAS(2) + OP_l_16_c_6_r_*vd*SH_TAS(1)); -Kfusion(17) = SK_TAS(1)*(OP_l_17_c_4_r_*SH_TAS(3) - OP_l_17_c_23_r_*SH_TAS(3) + OP_l_17_c_5_r_*SK_TAS(2) - OP_l_17_c_24_r_*SK_TAS(2) + OP_l_17_c_6_r_*vd*SH_TAS(1)); -Kfusion(18) = SK_TAS(1)*(OP_l_18_c_4_r_*SH_TAS(3) - OP_l_18_c_23_r_*SH_TAS(3) + OP_l_18_c_5_r_*SK_TAS(2) - OP_l_18_c_24_r_*SK_TAS(2) + OP_l_18_c_6_r_*vd*SH_TAS(1)); -Kfusion(19) = SK_TAS(1)*(OP_l_19_c_4_r_*SH_TAS(3) - OP_l_19_c_23_r_*SH_TAS(3) + OP_l_19_c_5_r_*SK_TAS(2) - OP_l_19_c_24_r_*SK_TAS(2) + OP_l_19_c_6_r_*vd*SH_TAS(1)); -Kfusion(20) = SK_TAS(1)*(OP_l_20_c_4_r_*SH_TAS(3) - OP_l_20_c_23_r_*SH_TAS(3) + OP_l_20_c_5_r_*SK_TAS(2) - OP_l_20_c_24_r_*SK_TAS(2) + OP_l_20_c_6_r_*vd*SH_TAS(1)); -Kfusion(21) = SK_TAS(1)*(OP_l_21_c_4_r_*SH_TAS(3) - OP_l_21_c_23_r_*SH_TAS(3) + OP_l_21_c_5_r_*SK_TAS(2) - OP_l_21_c_24_r_*SK_TAS(2) + OP_l_21_c_6_r_*vd*SH_TAS(1)); -Kfusion(22) = SK_TAS(1)*(OP_l_22_c_4_r_*SH_TAS(3) - OP_l_22_c_23_r_*SH_TAS(3) + OP_l_22_c_5_r_*SK_TAS(2) - OP_l_22_c_24_r_*SK_TAS(2) + OP_l_22_c_6_r_*vd*SH_TAS(1)); -Kfusion(23) = SK_TAS(1)*(OP_l_23_c_4_r_*SH_TAS(3) - OP_l_23_c_23_r_*SH_TAS(3) + OP_l_23_c_5_r_*SK_TAS(2) - OP_l_23_c_24_r_*SK_TAS(2) + OP_l_23_c_6_r_*vd*SH_TAS(1)); -Kfusion(24) = SK_TAS(1)*(OP_l_24_c_4_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(3) + OP_l_24_c_5_r_*SK_TAS(2) - OP_l_24_c_24_r_*SK_TAS(2) + OP_l_24_c_6_r_*vd*SH_TAS(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(10,1); -SH_BETA(1) = (2*conj(q0)*conj(q3) + 2*conj(q1)*conj(q2))*(ve - vwe) - vd*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)) + (vn - vwn)*(conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2); -SH_BETA(2) = vd*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) - (2*conj(q0)*conj(q3) - 2*conj(q1)*conj(q2))*(vn - vwn) + (ve - vwe)*(conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2); -SH_BETA(3) = (2*conj(q0)*conj(q2) + 2*conj(q1)*conj(q3))*(vn - vwn) - (2*conj(q0)*conj(q1) - 2*conj(q2)*conj(q3))*(ve - vwe) + vd*(conj(q0)^2 - conj(q1)^2 - conj(q2)^2 + conj(q3)^2); -SH_BETA(4) = (conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2)/SH_BETA(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) = conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2; -SH_BETA(7) = 2*conj(q0)*conj(q3); -SH_BETA(8) = 1/SH_BETA(1); -SH_BETA(9) = SH_BETA(7) + 2*conj(q1)*conj(q2); -SH_BETA(10) = SH_BETA(7) - 2*conj(q1)*conj(q2); +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(3)*SH_BETA(8); -H_BETA(1,2) = SH_BETA(2)*SH_BETA(3)*SH_BETA(5); -H_BETA(1,3) = - SH_BETA(2)^2*SH_BETA(5) - 1; -H_BETA(1,4) = - SH_BETA(8)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -H_BETA(1,5) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -H_BETA(1,6) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -H_BETA(1,23) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*SH_BETA(9) - SH_BETA(4); +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(6,1); -SK_BETA(1) = 1/(R_BETA + (SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)))*(OP_l_23_c_6_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_6_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_6_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_6_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_6_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_6_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_6_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_6_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP_l_23_c_5_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_5_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_5_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_5_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_5_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_5_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_5_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_5_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP_l_23_c_24_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_24_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_24_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_24_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_24_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_24_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_24_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_24_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP_l_23_c_4_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_4_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_4_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_4_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_4_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_4_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_4_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_4_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP_l_23_c_23_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_23_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_23_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_23_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_23_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_23_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_23_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_23_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(2)^2*SH_BETA(5) + 1)*(OP_l_23_c_3_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_3_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_3_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_3_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_3_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_3_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_3_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_3_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(3)*SH_BETA(8)*(OP_l_23_c_1_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_1_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_1_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_1_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_1_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_1_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_1_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_1_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(2)*SH_BETA(3)*SH_BETA(5)*(OP_l_23_c_2_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_2_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_2_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_2_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_2_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_2_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_2_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_2_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5))); -SK_BETA(2) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -SK_BETA(3) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -SK_BETA(4) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -SK_BETA(5) = SH_BETA(2)^2*SH_BETA(5) + 1; -SK_BETA(6) = SH_BETA(3); +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_6_r_*SK_BETA(2) - OP_l_1_c_3_r_*SK_BETA(5) - OP_l_1_c_4_r_*SK_BETA(3) + OP_l_1_c_5_r_*SK_BETA(4) + OP_l_1_c_23_r_*SK_BETA(3) - OP_l_1_c_24_r_*SK_BETA(4) + OP_l_1_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_1_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(2) = SK_BETA(1)*(OP_l_2_c_6_r_*SK_BETA(2) - OP_l_2_c_3_r_*SK_BETA(5) - OP_l_2_c_4_r_*SK_BETA(3) + OP_l_2_c_5_r_*SK_BETA(4) + OP_l_2_c_23_r_*SK_BETA(3) - OP_l_2_c_24_r_*SK_BETA(4) + OP_l_2_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_2_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(3) = SK_BETA(1)*(OP_l_3_c_6_r_*SK_BETA(2) - OP_l_3_c_3_r_*SK_BETA(5) - OP_l_3_c_4_r_*SK_BETA(3) + OP_l_3_c_5_r_*SK_BETA(4) + OP_l_3_c_23_r_*SK_BETA(3) - OP_l_3_c_24_r_*SK_BETA(4) + OP_l_3_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_3_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(4) = SK_BETA(1)*(OP_l_4_c_6_r_*SK_BETA(2) - OP_l_4_c_3_r_*SK_BETA(5) - OP_l_4_c_4_r_*SK_BETA(3) + OP_l_4_c_5_r_*SK_BETA(4) + OP_l_4_c_23_r_*SK_BETA(3) - OP_l_4_c_24_r_*SK_BETA(4) + OP_l_4_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_4_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(5) = SK_BETA(1)*(OP_l_5_c_6_r_*SK_BETA(2) - OP_l_5_c_3_r_*SK_BETA(5) - OP_l_5_c_4_r_*SK_BETA(3) + OP_l_5_c_5_r_*SK_BETA(4) + OP_l_5_c_23_r_*SK_BETA(3) - OP_l_5_c_24_r_*SK_BETA(4) + OP_l_5_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_5_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(6) = SK_BETA(1)*(OP_l_6_c_6_r_*SK_BETA(2) - OP_l_6_c_3_r_*SK_BETA(5) - OP_l_6_c_4_r_*SK_BETA(3) + OP_l_6_c_5_r_*SK_BETA(4) + OP_l_6_c_23_r_*SK_BETA(3) - OP_l_6_c_24_r_*SK_BETA(4) + OP_l_6_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_6_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(7) = SK_BETA(1)*(OP_l_7_c_6_r_*SK_BETA(2) - OP_l_7_c_3_r_*SK_BETA(5) - OP_l_7_c_4_r_*SK_BETA(3) + OP_l_7_c_5_r_*SK_BETA(4) + OP_l_7_c_23_r_*SK_BETA(3) - OP_l_7_c_24_r_*SK_BETA(4) + OP_l_7_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_7_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(8) = SK_BETA(1)*(OP_l_8_c_6_r_*SK_BETA(2) - OP_l_8_c_3_r_*SK_BETA(5) - OP_l_8_c_4_r_*SK_BETA(3) + OP_l_8_c_5_r_*SK_BETA(4) + OP_l_8_c_23_r_*SK_BETA(3) - OP_l_8_c_24_r_*SK_BETA(4) + OP_l_8_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_8_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(9) = SK_BETA(1)*(OP_l_9_c_6_r_*SK_BETA(2) - OP_l_9_c_3_r_*SK_BETA(5) - OP_l_9_c_4_r_*SK_BETA(3) + OP_l_9_c_5_r_*SK_BETA(4) + OP_l_9_c_23_r_*SK_BETA(3) - OP_l_9_c_24_r_*SK_BETA(4) + OP_l_9_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_9_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(10) = SK_BETA(1)*(OP_l_10_c_6_r_*SK_BETA(2) - OP_l_10_c_3_r_*SK_BETA(5) - OP_l_10_c_4_r_*SK_BETA(3) + OP_l_10_c_5_r_*SK_BETA(4) + OP_l_10_c_23_r_*SK_BETA(3) - OP_l_10_c_24_r_*SK_BETA(4) + OP_l_10_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_10_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(11) = SK_BETA(1)*(OP_l_11_c_6_r_*SK_BETA(2) - OP_l_11_c_3_r_*SK_BETA(5) - OP_l_11_c_4_r_*SK_BETA(3) + OP_l_11_c_5_r_*SK_BETA(4) + OP_l_11_c_23_r_*SK_BETA(3) - OP_l_11_c_24_r_*SK_BETA(4) + OP_l_11_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_11_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(12) = SK_BETA(1)*(OP_l_12_c_6_r_*SK_BETA(2) - OP_l_12_c_3_r_*SK_BETA(5) - OP_l_12_c_4_r_*SK_BETA(3) + OP_l_12_c_5_r_*SK_BETA(4) + OP_l_12_c_23_r_*SK_BETA(3) - OP_l_12_c_24_r_*SK_BETA(4) + OP_l_12_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_12_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(13) = SK_BETA(1)*(OP_l_13_c_6_r_*SK_BETA(2) - OP_l_13_c_3_r_*SK_BETA(5) - OP_l_13_c_4_r_*SK_BETA(3) + OP_l_13_c_5_r_*SK_BETA(4) + OP_l_13_c_23_r_*SK_BETA(3) - OP_l_13_c_24_r_*SK_BETA(4) + OP_l_13_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_13_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(14) = SK_BETA(1)*(OP_l_14_c_6_r_*SK_BETA(2) - OP_l_14_c_3_r_*SK_BETA(5) - OP_l_14_c_4_r_*SK_BETA(3) + OP_l_14_c_5_r_*SK_BETA(4) + OP_l_14_c_23_r_*SK_BETA(3) - OP_l_14_c_24_r_*SK_BETA(4) + OP_l_14_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_14_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(15) = SK_BETA(1)*(OP_l_15_c_6_r_*SK_BETA(2) - OP_l_15_c_3_r_*SK_BETA(5) - OP_l_15_c_4_r_*SK_BETA(3) + OP_l_15_c_5_r_*SK_BETA(4) + OP_l_15_c_23_r_*SK_BETA(3) - OP_l_15_c_24_r_*SK_BETA(4) + OP_l_15_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_15_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(16) = SK_BETA(1)*(OP_l_16_c_6_r_*SK_BETA(2) - OP_l_16_c_3_r_*SK_BETA(5) - OP_l_16_c_4_r_*SK_BETA(3) + OP_l_16_c_5_r_*SK_BETA(4) + OP_l_16_c_23_r_*SK_BETA(3) - OP_l_16_c_24_r_*SK_BETA(4) + OP_l_16_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_16_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(17) = SK_BETA(1)*(OP_l_17_c_6_r_*SK_BETA(2) - OP_l_17_c_3_r_*SK_BETA(5) - OP_l_17_c_4_r_*SK_BETA(3) + OP_l_17_c_5_r_*SK_BETA(4) + OP_l_17_c_23_r_*SK_BETA(3) - OP_l_17_c_24_r_*SK_BETA(4) + OP_l_17_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_17_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(18) = SK_BETA(1)*(OP_l_18_c_6_r_*SK_BETA(2) - OP_l_18_c_3_r_*SK_BETA(5) - OP_l_18_c_4_r_*SK_BETA(3) + OP_l_18_c_5_r_*SK_BETA(4) + OP_l_18_c_23_r_*SK_BETA(3) - OP_l_18_c_24_r_*SK_BETA(4) + OP_l_18_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_18_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(19) = SK_BETA(1)*(OP_l_19_c_6_r_*SK_BETA(2) - OP_l_19_c_3_r_*SK_BETA(5) - OP_l_19_c_4_r_*SK_BETA(3) + OP_l_19_c_5_r_*SK_BETA(4) + OP_l_19_c_23_r_*SK_BETA(3) - OP_l_19_c_24_r_*SK_BETA(4) + OP_l_19_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_19_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(20) = SK_BETA(1)*(OP_l_20_c_6_r_*SK_BETA(2) - OP_l_20_c_3_r_*SK_BETA(5) - OP_l_20_c_4_r_*SK_BETA(3) + OP_l_20_c_5_r_*SK_BETA(4) + OP_l_20_c_23_r_*SK_BETA(3) - OP_l_20_c_24_r_*SK_BETA(4) + OP_l_20_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_20_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(21) = SK_BETA(1)*(OP_l_21_c_6_r_*SK_BETA(2) - OP_l_21_c_3_r_*SK_BETA(5) - OP_l_21_c_4_r_*SK_BETA(3) + OP_l_21_c_5_r_*SK_BETA(4) + OP_l_21_c_23_r_*SK_BETA(3) - OP_l_21_c_24_r_*SK_BETA(4) + OP_l_21_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_21_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(22) = SK_BETA(1)*(OP_l_22_c_6_r_*SK_BETA(2) - OP_l_22_c_3_r_*SK_BETA(5) - OP_l_22_c_4_r_*SK_BETA(3) + OP_l_22_c_5_r_*SK_BETA(4) + OP_l_22_c_23_r_*SK_BETA(3) - OP_l_22_c_24_r_*SK_BETA(4) + OP_l_22_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_22_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(23) = SK_BETA(1)*(OP_l_23_c_6_r_*SK_BETA(2) - OP_l_23_c_3_r_*SK_BETA(5) - OP_l_23_c_4_r_*SK_BETA(3) + OP_l_23_c_5_r_*SK_BETA(4) + OP_l_23_c_23_r_*SK_BETA(3) - OP_l_23_c_24_r_*SK_BETA(4) + OP_l_23_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_23_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(24) = SK_BETA(1)*(OP_l_24_c_6_r_*SK_BETA(2) - OP_l_24_c_3_r_*SK_BETA(5) - OP_l_24_c_4_r_*SK_BETA(3) + OP_l_24_c_5_r_*SK_BETA(4) + OP_l_24_c_23_r_*SK_BETA(3) - OP_l_24_c_24_r_*SK_BETA(4) + OP_l_24_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_24_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); +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) = q0^2 - q1^2 + q2^2 - q3^2; -SH_MAG(2) = q0^2 + q1^2 - q2^2 - q3^2; -SH_MAG(3) = q0^2 - q1^2 - q2^2 + q3^2; -SH_MAG(4) = 2*q0*q1 + 2*q2*q3; -SH_MAG(5) = 2*q0*q3 + 2*q1*q2; -SH_MAG(6) = 2*q0*q2 + 2*q1*q3; -SH_MAG(7) = magE*(2*q0*q1 - 2*q2*q3); -SH_MAG(8) = 2*q1*q3 - 2*q0*q2; -SH_MAG(9) = 2*q0*q3; +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(2) = SH_MAG(7) - magD*SH_MAG(3) - magN*SH_MAG(6); -H_MAG(3) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -H_MAG(17) = SH_MAG(2); -H_MAG(18) = SH_MAG(5); -H_MAG(19) = SH_MAG(8); +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(4,1); -SK_MX(1) = 1/(OP_l_20_c_20_r_ + R_MAG - OP_l_2_c_20_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_20_r_*SH_MAG(2) + OP_l_18_c_20_r_*SH_MAG(5) + OP_l_19_c_20_r_*SH_MAG(8) + OP_l_3_c_20_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) - (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP_l_20_c_2_r_ - OP_l_2_c_2_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_2_r_*SH_MAG(2) + OP_l_18_c_2_r_*SH_MAG(5) + OP_l_19_c_2_r_*SH_MAG(8) + OP_l_3_c_2_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(2)*(OP_l_20_c_17_r_ - OP_l_2_c_17_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_17_r_*SH_MAG(2) + OP_l_18_c_17_r_*SH_MAG(5) + OP_l_19_c_17_r_*SH_MAG(8) + OP_l_3_c_17_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(5)*(OP_l_20_c_18_r_ - OP_l_2_c_18_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_18_r_*SH_MAG(2) + OP_l_18_c_18_r_*SH_MAG(5) + OP_l_19_c_18_r_*SH_MAG(8) + OP_l_3_c_18_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(8)*(OP_l_20_c_19_r_ - OP_l_2_c_19_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_19_r_*SH_MAG(2) + OP_l_18_c_19_r_*SH_MAG(5) + OP_l_19_c_19_r_*SH_MAG(8) + OP_l_3_c_19_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP_l_20_c_3_r_ - OP_l_2_c_3_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_3_r_*SH_MAG(2) + OP_l_18_c_3_r_*SH_MAG(5) + OP_l_19_c_3_r_*SH_MAG(8) + OP_l_3_c_3_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)))); -SK_MX(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -SK_MX(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -SK_MX(4) = SH_MAG(8); +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_17_r_*SH_MAG(2) + OP_l_1_c_18_r_*SH_MAG(5) - OP_l_1_c_2_r_*SK_MX(3) + OP_l_1_c_3_r_*SK_MX(2) + OP_l_1_c_19_r_*SK_MX(4)); -Kfusion(2) = SK_MX(1)*(OP_l_2_c_20_r_ + OP_l_2_c_17_r_*SH_MAG(2) + OP_l_2_c_18_r_*SH_MAG(5) - OP_l_2_c_2_r_*SK_MX(3) + OP_l_2_c_3_r_*SK_MX(2) + OP_l_2_c_19_r_*SK_MX(4)); -Kfusion(3) = SK_MX(1)*(OP_l_3_c_20_r_ + OP_l_3_c_17_r_*SH_MAG(2) + OP_l_3_c_18_r_*SH_MAG(5) - OP_l_3_c_2_r_*SK_MX(3) + OP_l_3_c_3_r_*SK_MX(2) + OP_l_3_c_19_r_*SK_MX(4)); -Kfusion(4) = SK_MX(1)*(OP_l_4_c_20_r_ + OP_l_4_c_17_r_*SH_MAG(2) + OP_l_4_c_18_r_*SH_MAG(5) - OP_l_4_c_2_r_*SK_MX(3) + OP_l_4_c_3_r_*SK_MX(2) + OP_l_4_c_19_r_*SK_MX(4)); -Kfusion(5) = SK_MX(1)*(OP_l_5_c_20_r_ + OP_l_5_c_17_r_*SH_MAG(2) + OP_l_5_c_18_r_*SH_MAG(5) - OP_l_5_c_2_r_*SK_MX(3) + OP_l_5_c_3_r_*SK_MX(2) + OP_l_5_c_19_r_*SK_MX(4)); -Kfusion(6) = SK_MX(1)*(OP_l_6_c_20_r_ + OP_l_6_c_17_r_*SH_MAG(2) + OP_l_6_c_18_r_*SH_MAG(5) - OP_l_6_c_2_r_*SK_MX(3) + OP_l_6_c_3_r_*SK_MX(2) + OP_l_6_c_19_r_*SK_MX(4)); -Kfusion(7) = SK_MX(1)*(OP_l_7_c_20_r_ + OP_l_7_c_17_r_*SH_MAG(2) + OP_l_7_c_18_r_*SH_MAG(5) - OP_l_7_c_2_r_*SK_MX(3) + OP_l_7_c_3_r_*SK_MX(2) + OP_l_7_c_19_r_*SK_MX(4)); -Kfusion(8) = SK_MX(1)*(OP_l_8_c_20_r_ + OP_l_8_c_17_r_*SH_MAG(2) + OP_l_8_c_18_r_*SH_MAG(5) - OP_l_8_c_2_r_*SK_MX(3) + OP_l_8_c_3_r_*SK_MX(2) + OP_l_8_c_19_r_*SK_MX(4)); -Kfusion(9) = SK_MX(1)*(OP_l_9_c_20_r_ + OP_l_9_c_17_r_*SH_MAG(2) + OP_l_9_c_18_r_*SH_MAG(5) - OP_l_9_c_2_r_*SK_MX(3) + OP_l_9_c_3_r_*SK_MX(2) + OP_l_9_c_19_r_*SK_MX(4)); -Kfusion(10) = SK_MX(1)*(OP_l_10_c_20_r_ + OP_l_10_c_17_r_*SH_MAG(2) + OP_l_10_c_18_r_*SH_MAG(5) - OP_l_10_c_2_r_*SK_MX(3) + OP_l_10_c_3_r_*SK_MX(2) + OP_l_10_c_19_r_*SK_MX(4)); -Kfusion(11) = SK_MX(1)*(OP_l_11_c_20_r_ + OP_l_11_c_17_r_*SH_MAG(2) + OP_l_11_c_18_r_*SH_MAG(5) - OP_l_11_c_2_r_*SK_MX(3) + OP_l_11_c_3_r_*SK_MX(2) + OP_l_11_c_19_r_*SK_MX(4)); -Kfusion(12) = SK_MX(1)*(OP_l_12_c_20_r_ + OP_l_12_c_17_r_*SH_MAG(2) + OP_l_12_c_18_r_*SH_MAG(5) - OP_l_12_c_2_r_*SK_MX(3) + OP_l_12_c_3_r_*SK_MX(2) + OP_l_12_c_19_r_*SK_MX(4)); -Kfusion(13) = SK_MX(1)*(OP_l_13_c_20_r_ + OP_l_13_c_17_r_*SH_MAG(2) + OP_l_13_c_18_r_*SH_MAG(5) - OP_l_13_c_2_r_*SK_MX(3) + OP_l_13_c_3_r_*SK_MX(2) + OP_l_13_c_19_r_*SK_MX(4)); -Kfusion(14) = SK_MX(1)*(OP_l_14_c_20_r_ + OP_l_14_c_17_r_*SH_MAG(2) + OP_l_14_c_18_r_*SH_MAG(5) - OP_l_14_c_2_r_*SK_MX(3) + OP_l_14_c_3_r_*SK_MX(2) + OP_l_14_c_19_r_*SK_MX(4)); -Kfusion(15) = SK_MX(1)*(OP_l_15_c_20_r_ + OP_l_15_c_17_r_*SH_MAG(2) + OP_l_15_c_18_r_*SH_MAG(5) - OP_l_15_c_2_r_*SK_MX(3) + OP_l_15_c_3_r_*SK_MX(2) + OP_l_15_c_19_r_*SK_MX(4)); -Kfusion(16) = SK_MX(1)*(OP_l_16_c_20_r_ + OP_l_16_c_17_r_*SH_MAG(2) + OP_l_16_c_18_r_*SH_MAG(5) - OP_l_16_c_2_r_*SK_MX(3) + OP_l_16_c_3_r_*SK_MX(2) + OP_l_16_c_19_r_*SK_MX(4)); -Kfusion(17) = SK_MX(1)*(OP_l_17_c_20_r_ + OP_l_17_c_17_r_*SH_MAG(2) + OP_l_17_c_18_r_*SH_MAG(5) - OP_l_17_c_2_r_*SK_MX(3) + OP_l_17_c_3_r_*SK_MX(2) + OP_l_17_c_19_r_*SK_MX(4)); -Kfusion(18) = SK_MX(1)*(OP_l_18_c_20_r_ + OP_l_18_c_17_r_*SH_MAG(2) + OP_l_18_c_18_r_*SH_MAG(5) - OP_l_18_c_2_r_*SK_MX(3) + OP_l_18_c_3_r_*SK_MX(2) + OP_l_18_c_19_r_*SK_MX(4)); -Kfusion(19) = SK_MX(1)*(OP_l_19_c_20_r_ + OP_l_19_c_17_r_*SH_MAG(2) + OP_l_19_c_18_r_*SH_MAG(5) - OP_l_19_c_2_r_*SK_MX(3) + OP_l_19_c_3_r_*SK_MX(2) + OP_l_19_c_19_r_*SK_MX(4)); -Kfusion(20) = SK_MX(1)*(OP_l_20_c_20_r_ + OP_l_20_c_17_r_*SH_MAG(2) + OP_l_20_c_18_r_*SH_MAG(5) - OP_l_20_c_2_r_*SK_MX(3) + OP_l_20_c_3_r_*SK_MX(2) + OP_l_20_c_19_r_*SK_MX(4)); -Kfusion(21) = SK_MX(1)*(OP_l_21_c_20_r_ + OP_l_21_c_17_r_*SH_MAG(2) + OP_l_21_c_18_r_*SH_MAG(5) - OP_l_21_c_2_r_*SK_MX(3) + OP_l_21_c_3_r_*SK_MX(2) + OP_l_21_c_19_r_*SK_MX(4)); -Kfusion(22) = SK_MX(1)*(OP_l_22_c_20_r_ + OP_l_22_c_17_r_*SH_MAG(2) + OP_l_22_c_18_r_*SH_MAG(5) - OP_l_22_c_2_r_*SK_MX(3) + OP_l_22_c_3_r_*SK_MX(2) + OP_l_22_c_19_r_*SK_MX(4)); -Kfusion(23) = SK_MX(1)*(OP_l_23_c_20_r_ + OP_l_23_c_17_r_*SH_MAG(2) + OP_l_23_c_18_r_*SH_MAG(5) - OP_l_23_c_2_r_*SK_MX(3) + OP_l_23_c_3_r_*SK_MX(2) + OP_l_23_c_19_r_*SK_MX(4)); -Kfusion(24) = SK_MX(1)*(OP_l_24_c_20_r_ + OP_l_24_c_17_r_*SH_MAG(2) + OP_l_24_c_18_r_*SH_MAG(5) - OP_l_24_c_2_r_*SK_MX(3) + OP_l_24_c_3_r_*SK_MX(2) + OP_l_24_c_19_r_*SK_MX(4)); +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) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -H_MAG(3) = - magE*SH_MAG(5) - magD*SH_MAG(8) - magN*SH_MAG(2); -H_MAG(17) = 2*q1*q2 - SH_MAG(9); -H_MAG(18) = SH_MAG(1); -H_MAG(19) = SH_MAG(4); +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(4,1); -SK_MY(1) = 1/(OP_l_21_c_21_r_ + R_MAG + OP_l_1_c_21_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_21_r_*SH_MAG(1) + OP_l_19_c_21_r_*SH_MAG(4) - (SH_MAG(9) - 2*q1*q2)*(OP_l_21_c_17_r_ + OP_l_1_c_17_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_17_r_*SH_MAG(1) + OP_l_19_c_17_r_*SH_MAG(4) - OP_l_3_c_17_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_17_r_*(SH_MAG(9) - 2*q1*q2)) - OP_l_3_c_21_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP_l_21_c_1_r_ + OP_l_1_c_1_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_1_r_*SH_MAG(1) + OP_l_19_c_1_r_*SH_MAG(4) - OP_l_3_c_1_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_1_r_*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(1)*(OP_l_21_c_18_r_ + OP_l_1_c_18_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_18_r_*SH_MAG(1) + OP_l_19_c_18_r_*SH_MAG(4) - OP_l_3_c_18_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_18_r_*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(4)*(OP_l_21_c_19_r_ + OP_l_1_c_19_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_19_r_*SH_MAG(1) + OP_l_19_c_19_r_*SH_MAG(4) - OP_l_3_c_19_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_19_r_*(SH_MAG(9) - 2*q1*q2)) - OP_l_17_c_21_r_*(SH_MAG(9) - 2*q1*q2) - (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP_l_21_c_3_r_ + OP_l_1_c_3_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_3_r_*SH_MAG(1) + OP_l_19_c_3_r_*SH_MAG(4) - OP_l_3_c_3_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_3_r_*(SH_MAG(9) - 2*q1*q2))); -SK_MY(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); -SK_MY(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -SK_MY(4) = SH_MAG(9) - 2*q1*q2; +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_18_r_*SH_MAG(1) + OP_l_1_c_19_r_*SH_MAG(4) + OP_l_1_c_1_r_*SK_MY(3) - OP_l_1_c_3_r_*SK_MY(2) - OP_l_1_c_17_r_*SK_MY(4)); -Kfusion(2) = SK_MY(1)*(OP_l_2_c_21_r_ + OP_l_2_c_18_r_*SH_MAG(1) + OP_l_2_c_19_r_*SH_MAG(4) + OP_l_2_c_1_r_*SK_MY(3) - OP_l_2_c_3_r_*SK_MY(2) - OP_l_2_c_17_r_*SK_MY(4)); -Kfusion(3) = SK_MY(1)*(OP_l_3_c_21_r_ + OP_l_3_c_18_r_*SH_MAG(1) + OP_l_3_c_19_r_*SH_MAG(4) + OP_l_3_c_1_r_*SK_MY(3) - OP_l_3_c_3_r_*SK_MY(2) - OP_l_3_c_17_r_*SK_MY(4)); -Kfusion(4) = SK_MY(1)*(OP_l_4_c_21_r_ + OP_l_4_c_18_r_*SH_MAG(1) + OP_l_4_c_19_r_*SH_MAG(4) + OP_l_4_c_1_r_*SK_MY(3) - OP_l_4_c_3_r_*SK_MY(2) - OP_l_4_c_17_r_*SK_MY(4)); -Kfusion(5) = SK_MY(1)*(OP_l_5_c_21_r_ + OP_l_5_c_18_r_*SH_MAG(1) + OP_l_5_c_19_r_*SH_MAG(4) + OP_l_5_c_1_r_*SK_MY(3) - OP_l_5_c_3_r_*SK_MY(2) - OP_l_5_c_17_r_*SK_MY(4)); -Kfusion(6) = SK_MY(1)*(OP_l_6_c_21_r_ + OP_l_6_c_18_r_*SH_MAG(1) + OP_l_6_c_19_r_*SH_MAG(4) + OP_l_6_c_1_r_*SK_MY(3) - OP_l_6_c_3_r_*SK_MY(2) - OP_l_6_c_17_r_*SK_MY(4)); -Kfusion(7) = SK_MY(1)*(OP_l_7_c_21_r_ + OP_l_7_c_18_r_*SH_MAG(1) + OP_l_7_c_19_r_*SH_MAG(4) + OP_l_7_c_1_r_*SK_MY(3) - OP_l_7_c_3_r_*SK_MY(2) - OP_l_7_c_17_r_*SK_MY(4)); -Kfusion(8) = SK_MY(1)*(OP_l_8_c_21_r_ + OP_l_8_c_18_r_*SH_MAG(1) + OP_l_8_c_19_r_*SH_MAG(4) + OP_l_8_c_1_r_*SK_MY(3) - OP_l_8_c_3_r_*SK_MY(2) - OP_l_8_c_17_r_*SK_MY(4)); -Kfusion(9) = SK_MY(1)*(OP_l_9_c_21_r_ + OP_l_9_c_18_r_*SH_MAG(1) + OP_l_9_c_19_r_*SH_MAG(4) + OP_l_9_c_1_r_*SK_MY(3) - OP_l_9_c_3_r_*SK_MY(2) - OP_l_9_c_17_r_*SK_MY(4)); -Kfusion(10) = SK_MY(1)*(OP_l_10_c_21_r_ + OP_l_10_c_18_r_*SH_MAG(1) + OP_l_10_c_19_r_*SH_MAG(4) + OP_l_10_c_1_r_*SK_MY(3) - OP_l_10_c_3_r_*SK_MY(2) - OP_l_10_c_17_r_*SK_MY(4)); -Kfusion(11) = SK_MY(1)*(OP_l_11_c_21_r_ + OP_l_11_c_18_r_*SH_MAG(1) + OP_l_11_c_19_r_*SH_MAG(4) + OP_l_11_c_1_r_*SK_MY(3) - OP_l_11_c_3_r_*SK_MY(2) - OP_l_11_c_17_r_*SK_MY(4)); -Kfusion(12) = SK_MY(1)*(OP_l_12_c_21_r_ + OP_l_12_c_18_r_*SH_MAG(1) + OP_l_12_c_19_r_*SH_MAG(4) + OP_l_12_c_1_r_*SK_MY(3) - OP_l_12_c_3_r_*SK_MY(2) - OP_l_12_c_17_r_*SK_MY(4)); -Kfusion(13) = SK_MY(1)*(OP_l_13_c_21_r_ + OP_l_13_c_18_r_*SH_MAG(1) + OP_l_13_c_19_r_*SH_MAG(4) + OP_l_13_c_1_r_*SK_MY(3) - OP_l_13_c_3_r_*SK_MY(2) - OP_l_13_c_17_r_*SK_MY(4)); -Kfusion(14) = SK_MY(1)*(OP_l_14_c_21_r_ + OP_l_14_c_18_r_*SH_MAG(1) + OP_l_14_c_19_r_*SH_MAG(4) + OP_l_14_c_1_r_*SK_MY(3) - OP_l_14_c_3_r_*SK_MY(2) - OP_l_14_c_17_r_*SK_MY(4)); -Kfusion(15) = SK_MY(1)*(OP_l_15_c_21_r_ + OP_l_15_c_18_r_*SH_MAG(1) + OP_l_15_c_19_r_*SH_MAG(4) + OP_l_15_c_1_r_*SK_MY(3) - OP_l_15_c_3_r_*SK_MY(2) - OP_l_15_c_17_r_*SK_MY(4)); -Kfusion(16) = SK_MY(1)*(OP_l_16_c_21_r_ + OP_l_16_c_18_r_*SH_MAG(1) + OP_l_16_c_19_r_*SH_MAG(4) + OP_l_16_c_1_r_*SK_MY(3) - OP_l_16_c_3_r_*SK_MY(2) - OP_l_16_c_17_r_*SK_MY(4)); -Kfusion(17) = SK_MY(1)*(OP_l_17_c_21_r_ + OP_l_17_c_18_r_*SH_MAG(1) + OP_l_17_c_19_r_*SH_MAG(4) + OP_l_17_c_1_r_*SK_MY(3) - OP_l_17_c_3_r_*SK_MY(2) - OP_l_17_c_17_r_*SK_MY(4)); -Kfusion(18) = SK_MY(1)*(OP_l_18_c_21_r_ + OP_l_18_c_18_r_*SH_MAG(1) + OP_l_18_c_19_r_*SH_MAG(4) + OP_l_18_c_1_r_*SK_MY(3) - OP_l_18_c_3_r_*SK_MY(2) - OP_l_18_c_17_r_*SK_MY(4)); -Kfusion(19) = SK_MY(1)*(OP_l_19_c_21_r_ + OP_l_19_c_18_r_*SH_MAG(1) + OP_l_19_c_19_r_*SH_MAG(4) + OP_l_19_c_1_r_*SK_MY(3) - OP_l_19_c_3_r_*SK_MY(2) - OP_l_19_c_17_r_*SK_MY(4)); -Kfusion(20) = SK_MY(1)*(OP_l_20_c_21_r_ + OP_l_20_c_18_r_*SH_MAG(1) + OP_l_20_c_19_r_*SH_MAG(4) + OP_l_20_c_1_r_*SK_MY(3) - OP_l_20_c_3_r_*SK_MY(2) - OP_l_20_c_17_r_*SK_MY(4)); -Kfusion(21) = SK_MY(1)*(OP_l_21_c_21_r_ + OP_l_21_c_18_r_*SH_MAG(1) + OP_l_21_c_19_r_*SH_MAG(4) + OP_l_21_c_1_r_*SK_MY(3) - OP_l_21_c_3_r_*SK_MY(2) - OP_l_21_c_17_r_*SK_MY(4)); -Kfusion(22) = SK_MY(1)*(OP_l_22_c_21_r_ + OP_l_22_c_18_r_*SH_MAG(1) + OP_l_22_c_19_r_*SH_MAG(4) + OP_l_22_c_1_r_*SK_MY(3) - OP_l_22_c_3_r_*SK_MY(2) - OP_l_22_c_17_r_*SK_MY(4)); -Kfusion(23) = SK_MY(1)*(OP_l_23_c_21_r_ + OP_l_23_c_18_r_*SH_MAG(1) + OP_l_23_c_19_r_*SH_MAG(4) + OP_l_23_c_1_r_*SK_MY(3) - OP_l_23_c_3_r_*SK_MY(2) - OP_l_23_c_17_r_*SK_MY(4)); -Kfusion(24) = SK_MY(1)*(OP_l_24_c_21_r_ + OP_l_24_c_18_r_*SH_MAG(1) + OP_l_24_c_19_r_*SH_MAG(4) + OP_l_24_c_1_r_*SK_MY(3) - OP_l_24_c_3_r_*SK_MY(2) - OP_l_24_c_17_r_*SK_MY(4)); +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) = magN*(SH_MAG(9) - 2*q1*q2) - magD*SH_MAG(4) - magE*SH_MAG(1); -H_MAG(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); -H_MAG(17) = SH_MAG(6); +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(3); +H_MAG(19) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7); H_MAG(22) = 1; -SK_MZ = zeros(4,1); -SK_MZ(1) = 1/(OP_l_22_c_22_r_ + R_MAG + OP_l_17_c_22_r_*SH_MAG(6) + OP_l_19_c_22_r_*SH_MAG(3) - (2*q0*q1 - 2*q2*q3)*(OP_l_22_c_18_r_ + OP_l_17_c_18_r_*SH_MAG(6) + OP_l_19_c_18_r_*SH_MAG(3) - OP_l_1_c_18_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_18_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_18_r_*(2*q0*q1 - 2*q2*q3)) - OP_l_1_c_22_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_22_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + SH_MAG(6)*(OP_l_22_c_17_r_ + OP_l_17_c_17_r_*SH_MAG(6) + OP_l_19_c_17_r_*SH_MAG(3) - OP_l_1_c_17_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_17_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_17_r_*(2*q0*q1 - 2*q2*q3)) + SH_MAG(3)*(OP_l_22_c_19_r_ + OP_l_17_c_19_r_*SH_MAG(6) + OP_l_19_c_19_r_*SH_MAG(3) - OP_l_1_c_19_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_19_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_19_r_*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP_l_22_c_1_r_ + OP_l_17_c_1_r_*SH_MAG(6) + OP_l_19_c_1_r_*SH_MAG(3) - OP_l_1_c_1_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_1_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_1_r_*(2*q0*q1 - 2*q2*q3)) - OP_l_18_c_22_r_*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP_l_22_c_2_r_ + OP_l_17_c_2_r_*SH_MAG(6) + OP_l_19_c_2_r_*SH_MAG(3) - OP_l_1_c_2_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_2_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_2_r_*(2*q0*q1 - 2*q2*q3))); -SK_MZ(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -SK_MZ(3) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); +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_19_r_*SH_MAG(3) + OP_l_1_c_17_r_*SH_MAG(6) - OP_l_1_c_1_r_*SK_MZ(2) + OP_l_1_c_2_r_*SK_MZ(3) - OP_l_1_c_18_r_*SK_MZ(4)); -Kfusion(2) = SK_MZ(1)*(OP_l_2_c_22_r_ + OP_l_2_c_19_r_*SH_MAG(3) + OP_l_2_c_17_r_*SH_MAG(6) - OP_l_2_c_1_r_*SK_MZ(2) + OP_l_2_c_2_r_*SK_MZ(3) - OP_l_2_c_18_r_*SK_MZ(4)); -Kfusion(3) = SK_MZ(1)*(OP_l_3_c_22_r_ + OP_l_3_c_19_r_*SH_MAG(3) + OP_l_3_c_17_r_*SH_MAG(6) - OP_l_3_c_1_r_*SK_MZ(2) + OP_l_3_c_2_r_*SK_MZ(3) - OP_l_3_c_18_r_*SK_MZ(4)); -Kfusion(4) = SK_MZ(1)*(OP_l_4_c_22_r_ + OP_l_4_c_19_r_*SH_MAG(3) + OP_l_4_c_17_r_*SH_MAG(6) - OP_l_4_c_1_r_*SK_MZ(2) + OP_l_4_c_2_r_*SK_MZ(3) - OP_l_4_c_18_r_*SK_MZ(4)); -Kfusion(5) = SK_MZ(1)*(OP_l_5_c_22_r_ + OP_l_5_c_19_r_*SH_MAG(3) + OP_l_5_c_17_r_*SH_MAG(6) - OP_l_5_c_1_r_*SK_MZ(2) + OP_l_5_c_2_r_*SK_MZ(3) - OP_l_5_c_18_r_*SK_MZ(4)); -Kfusion(6) = SK_MZ(1)*(OP_l_6_c_22_r_ + OP_l_6_c_19_r_*SH_MAG(3) + OP_l_6_c_17_r_*SH_MAG(6) - OP_l_6_c_1_r_*SK_MZ(2) + OP_l_6_c_2_r_*SK_MZ(3) - OP_l_6_c_18_r_*SK_MZ(4)); -Kfusion(7) = SK_MZ(1)*(OP_l_7_c_22_r_ + OP_l_7_c_19_r_*SH_MAG(3) + OP_l_7_c_17_r_*SH_MAG(6) - OP_l_7_c_1_r_*SK_MZ(2) + OP_l_7_c_2_r_*SK_MZ(3) - OP_l_7_c_18_r_*SK_MZ(4)); -Kfusion(8) = SK_MZ(1)*(OP_l_8_c_22_r_ + OP_l_8_c_19_r_*SH_MAG(3) + OP_l_8_c_17_r_*SH_MAG(6) - OP_l_8_c_1_r_*SK_MZ(2) + OP_l_8_c_2_r_*SK_MZ(3) - OP_l_8_c_18_r_*SK_MZ(4)); -Kfusion(9) = SK_MZ(1)*(OP_l_9_c_22_r_ + OP_l_9_c_19_r_*SH_MAG(3) + OP_l_9_c_17_r_*SH_MAG(6) - OP_l_9_c_1_r_*SK_MZ(2) + OP_l_9_c_2_r_*SK_MZ(3) - OP_l_9_c_18_r_*SK_MZ(4)); -Kfusion(10) = SK_MZ(1)*(OP_l_10_c_22_r_ + OP_l_10_c_19_r_*SH_MAG(3) + OP_l_10_c_17_r_*SH_MAG(6) - OP_l_10_c_1_r_*SK_MZ(2) + OP_l_10_c_2_r_*SK_MZ(3) - OP_l_10_c_18_r_*SK_MZ(4)); -Kfusion(11) = SK_MZ(1)*(OP_l_11_c_22_r_ + OP_l_11_c_19_r_*SH_MAG(3) + OP_l_11_c_17_r_*SH_MAG(6) - OP_l_11_c_1_r_*SK_MZ(2) + OP_l_11_c_2_r_*SK_MZ(3) - OP_l_11_c_18_r_*SK_MZ(4)); -Kfusion(12) = SK_MZ(1)*(OP_l_12_c_22_r_ + OP_l_12_c_19_r_*SH_MAG(3) + OP_l_12_c_17_r_*SH_MAG(6) - OP_l_12_c_1_r_*SK_MZ(2) + OP_l_12_c_2_r_*SK_MZ(3) - OP_l_12_c_18_r_*SK_MZ(4)); -Kfusion(13) = SK_MZ(1)*(OP_l_13_c_22_r_ + OP_l_13_c_19_r_*SH_MAG(3) + OP_l_13_c_17_r_*SH_MAG(6) - OP_l_13_c_1_r_*SK_MZ(2) + OP_l_13_c_2_r_*SK_MZ(3) - OP_l_13_c_18_r_*SK_MZ(4)); -Kfusion(14) = SK_MZ(1)*(OP_l_14_c_22_r_ + OP_l_14_c_19_r_*SH_MAG(3) + OP_l_14_c_17_r_*SH_MAG(6) - OP_l_14_c_1_r_*SK_MZ(2) + OP_l_14_c_2_r_*SK_MZ(3) - OP_l_14_c_18_r_*SK_MZ(4)); -Kfusion(15) = SK_MZ(1)*(OP_l_15_c_22_r_ + OP_l_15_c_19_r_*SH_MAG(3) + OP_l_15_c_17_r_*SH_MAG(6) - OP_l_15_c_1_r_*SK_MZ(2) + OP_l_15_c_2_r_*SK_MZ(3) - OP_l_15_c_18_r_*SK_MZ(4)); -Kfusion(16) = SK_MZ(1)*(OP_l_16_c_22_r_ + OP_l_16_c_19_r_*SH_MAG(3) + OP_l_16_c_17_r_*SH_MAG(6) - OP_l_16_c_1_r_*SK_MZ(2) + OP_l_16_c_2_r_*SK_MZ(3) - OP_l_16_c_18_r_*SK_MZ(4)); -Kfusion(17) = SK_MZ(1)*(OP_l_17_c_22_r_ + OP_l_17_c_19_r_*SH_MAG(3) + OP_l_17_c_17_r_*SH_MAG(6) - OP_l_17_c_1_r_*SK_MZ(2) + OP_l_17_c_2_r_*SK_MZ(3) - OP_l_17_c_18_r_*SK_MZ(4)); -Kfusion(18) = SK_MZ(1)*(OP_l_18_c_22_r_ + OP_l_18_c_19_r_*SH_MAG(3) + OP_l_18_c_17_r_*SH_MAG(6) - OP_l_18_c_1_r_*SK_MZ(2) + OP_l_18_c_2_r_*SK_MZ(3) - OP_l_18_c_18_r_*SK_MZ(4)); -Kfusion(19) = SK_MZ(1)*(OP_l_19_c_22_r_ + OP_l_19_c_19_r_*SH_MAG(3) + OP_l_19_c_17_r_*SH_MAG(6) - OP_l_19_c_1_r_*SK_MZ(2) + OP_l_19_c_2_r_*SK_MZ(3) - OP_l_19_c_18_r_*SK_MZ(4)); -Kfusion(20) = SK_MZ(1)*(OP_l_20_c_22_r_ + OP_l_20_c_19_r_*SH_MAG(3) + OP_l_20_c_17_r_*SH_MAG(6) - OP_l_20_c_1_r_*SK_MZ(2) + OP_l_20_c_2_r_*SK_MZ(3) - OP_l_20_c_18_r_*SK_MZ(4)); -Kfusion(21) = SK_MZ(1)*(OP_l_21_c_22_r_ + OP_l_21_c_19_r_*SH_MAG(3) + OP_l_21_c_17_r_*SH_MAG(6) - OP_l_21_c_1_r_*SK_MZ(2) + OP_l_21_c_2_r_*SK_MZ(3) - OP_l_21_c_18_r_*SK_MZ(4)); -Kfusion(22) = SK_MZ(1)*(OP_l_22_c_22_r_ + OP_l_22_c_19_r_*SH_MAG(3) + OP_l_22_c_17_r_*SH_MAG(6) - OP_l_22_c_1_r_*SK_MZ(2) + OP_l_22_c_2_r_*SK_MZ(3) - OP_l_22_c_18_r_*SK_MZ(4)); -Kfusion(23) = SK_MZ(1)*(OP_l_23_c_22_r_ + OP_l_23_c_19_r_*SH_MAG(3) + OP_l_23_c_17_r_*SH_MAG(6) - OP_l_23_c_1_r_*SK_MZ(2) + OP_l_23_c_2_r_*SK_MZ(3) - OP_l_23_c_18_r_*SK_MZ(4)); -Kfusion(24) = SK_MZ(1)*(OP_l_24_c_22_r_ + OP_l_24_c_19_r_*SH_MAG(3) + OP_l_24_c_17_r_*SH_MAG(6) - OP_l_24_c_1_r_*SK_MZ(2) + OP_l_24_c_2_r_*SK_MZ(3) - OP_l_24_c_18_r_*SK_MZ(4)); +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(7,1); +SH_ACCX = zeros(4,1); SH_ACCX(1) = q0^2 + q1^2 - q2^2 - q3^2; -SH_ACCX(2) = 2*q0*q3 + 2*q1*q2; -SH_ACCX(3) = vn - vwn; -SH_ACCX(4) = ve - vwe; -SH_ACCX(5) = q3^2; -SH_ACCX(6) = 2*q0*q2; -SH_ACCX(7) = 2*q0*q1; +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,2) = Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)); -H_ACCX(1,3) = Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)); -H_ACCX(1,4) = -Kaccx*SH_ACCX(1); -H_ACCX(1,5) = -Kaccx*SH_ACCX(2); -H_ACCX(1,6) = Kaccx*(SH_ACCX(6) - 2*q1*q3); +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(2); +H_ACCX(1,24) = Kaccx*SH_ACCX(4); -SK_ACCX = zeros(5,1); -SK_ACCX(1) = 1/(R_ACC - Kaccx*SH_ACCX(1)*(Kaccx*OP_l_23_c_4_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_4_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_4_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_4_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_4_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_4_r_*(SH_ACCX(6) - 2*q1*q3)) - Kaccx*SH_ACCX(2)*(Kaccx*OP_l_23_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_5_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_5_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_5_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_5_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_5_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_5_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(1)*(Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_23_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_23_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_23_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_23_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_23_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(2)*(Kaccx*OP_l_23_c_24_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_24_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_24_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_24_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_24_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_24_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_24_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3))*(Kaccx*OP_l_23_c_3_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_3_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_3_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_3_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_3_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_3_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_3_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2))*(Kaccx*OP_l_23_c_2_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_2_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_2_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_2_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_2_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_2_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_2_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(6) - 2*q1*q3)*(Kaccx*OP_l_23_c_6_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_6_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_6_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_6_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_6_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_6_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_6_r_*(SH_ACCX(6) - 2*q1*q3))); -SK_ACCX(2) = SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3); -SK_ACCX(3) = SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2); -SK_ACCX(4) = SH_ACCX(6) - 2*q1*q3; -SK_ACCX(5) = SH_ACCX(2); +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_23_r_*SH_ACCX(1) - Kaccx*OP_l_1_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_1_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_1_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_1_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_1_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_1_c_24_r_*SK_ACCX(5)); -Kfusion(2) = SK_ACCX(1)*(Kaccx*OP_l_2_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_2_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_2_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_2_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_2_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_2_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_2_c_24_r_*SK_ACCX(5)); -Kfusion(3) = SK_ACCX(1)*(Kaccx*OP_l_3_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_3_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_3_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_3_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_3_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_3_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_3_c_24_r_*SK_ACCX(5)); -Kfusion(4) = SK_ACCX(1)*(Kaccx*OP_l_4_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_4_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_4_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_4_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_4_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_4_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_4_c_24_r_*SK_ACCX(5)); -Kfusion(5) = SK_ACCX(1)*(Kaccx*OP_l_5_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_5_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_5_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_5_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_5_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_5_c_24_r_*SK_ACCX(5)); -Kfusion(6) = SK_ACCX(1)*(Kaccx*OP_l_6_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_6_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_6_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_6_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_6_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_6_c_24_r_*SK_ACCX(5)); -Kfusion(7) = SK_ACCX(1)*(Kaccx*OP_l_7_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_7_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_7_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_7_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_7_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_7_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_7_c_24_r_*SK_ACCX(5)); -Kfusion(8) = SK_ACCX(1)*(Kaccx*OP_l_8_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_8_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_8_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_8_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_8_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_8_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_8_c_24_r_*SK_ACCX(5)); -Kfusion(9) = SK_ACCX(1)*(Kaccx*OP_l_9_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_9_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_9_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_9_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_9_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_9_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_9_c_24_r_*SK_ACCX(5)); -Kfusion(10) = SK_ACCX(1)*(Kaccx*OP_l_10_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_10_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_10_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_10_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_10_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_10_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_10_c_24_r_*SK_ACCX(5)); -Kfusion(11) = SK_ACCX(1)*(Kaccx*OP_l_11_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_11_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_11_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_11_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_11_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_11_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_11_c_24_r_*SK_ACCX(5)); -Kfusion(12) = SK_ACCX(1)*(Kaccx*OP_l_12_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_12_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_12_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_12_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_12_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_12_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_12_c_24_r_*SK_ACCX(5)); -Kfusion(13) = SK_ACCX(1)*(Kaccx*OP_l_13_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_13_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_13_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_13_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_13_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_13_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_13_c_24_r_*SK_ACCX(5)); -Kfusion(14) = SK_ACCX(1)*(Kaccx*OP_l_14_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_14_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_14_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_14_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_14_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_14_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_14_c_24_r_*SK_ACCX(5)); -Kfusion(15) = SK_ACCX(1)*(Kaccx*OP_l_15_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_15_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_15_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_15_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_15_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_15_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_15_c_24_r_*SK_ACCX(5)); -Kfusion(16) = SK_ACCX(1)*(Kaccx*OP_l_16_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_16_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_16_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_16_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_16_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_16_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_16_c_24_r_*SK_ACCX(5)); -Kfusion(17) = SK_ACCX(1)*(Kaccx*OP_l_17_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_17_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_17_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_17_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_17_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_17_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_17_c_24_r_*SK_ACCX(5)); -Kfusion(18) = SK_ACCX(1)*(Kaccx*OP_l_18_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_18_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_18_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_18_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_18_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_18_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_18_c_24_r_*SK_ACCX(5)); -Kfusion(19) = SK_ACCX(1)*(Kaccx*OP_l_19_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_19_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_19_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_19_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_19_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_19_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_19_c_24_r_*SK_ACCX(5)); -Kfusion(20) = SK_ACCX(1)*(Kaccx*OP_l_20_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_20_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_20_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_20_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_20_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_20_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_20_c_24_r_*SK_ACCX(5)); -Kfusion(21) = SK_ACCX(1)*(Kaccx*OP_l_21_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_21_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_21_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_21_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_21_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_21_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_21_c_24_r_*SK_ACCX(5)); -Kfusion(22) = SK_ACCX(1)*(Kaccx*OP_l_22_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_22_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_22_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_22_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_22_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_22_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_22_c_24_r_*SK_ACCX(5)); -Kfusion(23) = SK_ACCX(1)*(Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_23_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_23_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_23_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_23_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_23_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_23_c_24_r_*SK_ACCX(5)); -Kfusion(24) = SK_ACCX(1)*(Kaccx*OP_l_24_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_24_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_24_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_24_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_24_c_24_r_*SK_ACCX(5)); +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(6,1); +SH_ACCY = zeros(3,1); SH_ACCY(1) = q0^2 - q1^2 + q2^2 - q3^2; -SH_ACCY(2) = ve - vwe; -SH_ACCY(3) = vn - vwn; -SH_ACCY(4) = q1^2; -SH_ACCY(5) = 2*q0*q1; -SH_ACCY(6) = 2*q0*q3; +SH_ACCY(2) = vn - vwn; +SH_ACCY(3) = ve - vwe; H_ACCY = zeros(1,24); -H_ACCY(1,1) = Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)); -H_ACCY(1,3) = Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)); -H_ACCY(1,4) = Kaccy*(SH_ACCY(6) - 2*q1*q2); -H_ACCY(1,5) = -Kaccy*SH_ACCY(1); -H_ACCY(1,6) = -Kaccy*(SH_ACCY(5) + 2*q2*q3); +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(7,1); -SK_ACCY(1) = 1/(R_ACC - Kaccy*SH_ACCY(1)*(Kaccy*OP_l_24_c_5_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_5_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_5_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_5_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_5_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_5_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_5_r_*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*SH_ACCY(1)*(Kaccy*OP_l_24_c_24_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_24_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_24_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_24_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_24_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_24_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_24_r_*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2))*(Kaccy*OP_l_24_c_1_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_1_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_1_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_1_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_1_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_1_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_1_r_*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3))*(Kaccy*OP_l_24_c_3_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_3_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_3_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_3_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_3_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_3_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_3_r_*(SH_ACCY(5) + 2*q2*q3)) - 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP_l_24_c_23_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_23_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_23_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_23_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_23_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_23_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_23_r_*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(6) - 2*q1*q2)*(Kaccy*OP_l_24_c_4_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_4_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_4_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_4_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_4_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_4_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_4_r_*(SH_ACCY(5) + 2*q2*q3)) - Kaccy*(SH_ACCY(5) + 2*q2*q3)*(Kaccy*OP_l_24_c_6_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_6_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_6_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_6_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_6_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_6_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_6_r_*(SH_ACCY(5) + 2*q2*q3))); -SK_ACCY(2) = SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3); -SK_ACCY(3) = SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2); -SK_ACCY(4) = q0*q3 - q1*q2; -SK_ACCY(5) = SH_ACCY(6) - 2*q1*q2; -SK_ACCY(6) = SH_ACCY(5) + 2*q2*q3; -SK_ACCY(7) = 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(3) + Kaccy*OP_l_1_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_1_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_1_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_1_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_1_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_1_c_24_r_*SK_ACCY(7)); -Kfusion(2) = SK_ACCY(1)*(Kaccy*OP_l_2_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_2_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_2_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_2_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_2_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_2_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_2_c_24_r_*SK_ACCY(7)); -Kfusion(3) = SK_ACCY(1)*(Kaccy*OP_l_3_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_3_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_3_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_3_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_3_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_3_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_3_c_24_r_*SK_ACCY(7)); -Kfusion(4) = SK_ACCY(1)*(Kaccy*OP_l_4_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_4_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_4_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_4_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_4_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_4_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_4_c_24_r_*SK_ACCY(7)); -Kfusion(5) = SK_ACCY(1)*(Kaccy*OP_l_5_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_5_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_5_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_5_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_5_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_5_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_5_c_24_r_*SK_ACCY(7)); -Kfusion(6) = SK_ACCY(1)*(Kaccy*OP_l_6_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_6_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_6_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_6_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_6_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_6_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_6_c_24_r_*SK_ACCY(7)); -Kfusion(7) = SK_ACCY(1)*(Kaccy*OP_l_7_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_7_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_7_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_7_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_7_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_7_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_7_c_24_r_*SK_ACCY(7)); -Kfusion(8) = SK_ACCY(1)*(Kaccy*OP_l_8_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_8_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_8_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_8_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_8_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_8_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_8_c_24_r_*SK_ACCY(7)); -Kfusion(9) = SK_ACCY(1)*(Kaccy*OP_l_9_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_9_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_9_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_9_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_9_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_9_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_9_c_24_r_*SK_ACCY(7)); -Kfusion(10) = SK_ACCY(1)*(Kaccy*OP_l_10_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_10_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_10_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_10_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_10_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_10_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_10_c_24_r_*SK_ACCY(7)); -Kfusion(11) = SK_ACCY(1)*(Kaccy*OP_l_11_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_11_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_11_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_11_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_11_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_11_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_11_c_24_r_*SK_ACCY(7)); -Kfusion(12) = SK_ACCY(1)*(Kaccy*OP_l_12_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_12_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_12_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_12_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_12_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_12_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_12_c_24_r_*SK_ACCY(7)); -Kfusion(13) = SK_ACCY(1)*(Kaccy*OP_l_13_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_13_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_13_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_13_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_13_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_13_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_13_c_24_r_*SK_ACCY(7)); -Kfusion(14) = SK_ACCY(1)*(Kaccy*OP_l_14_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_14_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_14_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_14_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_14_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_14_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_14_c_24_r_*SK_ACCY(7)); -Kfusion(15) = SK_ACCY(1)*(Kaccy*OP_l_15_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_15_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_15_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_15_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_15_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_15_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_15_c_24_r_*SK_ACCY(7)); -Kfusion(16) = SK_ACCY(1)*(Kaccy*OP_l_16_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_16_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_16_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_16_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_16_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_16_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_16_c_24_r_*SK_ACCY(7)); -Kfusion(17) = SK_ACCY(1)*(Kaccy*OP_l_17_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_17_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_17_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_17_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_17_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_17_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_17_c_24_r_*SK_ACCY(7)); -Kfusion(18) = SK_ACCY(1)*(Kaccy*OP_l_18_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_18_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_18_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_18_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_18_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_18_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_18_c_24_r_*SK_ACCY(7)); -Kfusion(19) = SK_ACCY(1)*(Kaccy*OP_l_19_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_19_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_19_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_19_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_19_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_19_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_19_c_24_r_*SK_ACCY(7)); -Kfusion(20) = SK_ACCY(1)*(Kaccy*OP_l_20_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_20_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_20_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_20_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_20_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_20_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_20_c_24_r_*SK_ACCY(7)); -Kfusion(21) = SK_ACCY(1)*(Kaccy*OP_l_21_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_21_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_21_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_21_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_21_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_21_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_21_c_24_r_*SK_ACCY(7)); -Kfusion(22) = SK_ACCY(1)*(Kaccy*OP_l_22_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_22_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_22_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_22_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_22_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_22_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_22_c_24_r_*SK_ACCY(7)); -Kfusion(23) = SK_ACCY(1)*(Kaccy*OP_l_23_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_23_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_23_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_23_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_23_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_23_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_23_c_24_r_*SK_ACCY(7)); -Kfusion(24) = SK_ACCY(1)*(Kaccy*OP_l_24_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_24_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_24_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_24_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_24_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_24_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_24_c_24_r_*SK_ACCY(7)); +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/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c b/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c index 19868e1a17..399e544299 100644 --- a/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c +++ b/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c @@ -1,16 +1,17 @@ -t2 = q0*q0; -t3 = q1*q1; -t4 = q2*q2; -t5 = q3*q3; -t6 = t2-t3+t4-t5; -t7 = q0*q3*2.0; -t10 = q1*q2*2.0; -t8 = t7-t10; -t9 = 1.0/(t6*t6); -t11 = t8*t8; -t12 = t9*t11; +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/t6; -A0[0][0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0)); -A0[0][2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10)); +A0[0][0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0)*-2.0; +A0[0][1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0)*-2.0; +A0[0][2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0)*2.0; +A0[0][3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0)*2.0; diff --git a/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c b/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c index 1e303866cb..90ad9098c2 100644 --- a/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c +++ b/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c @@ -1,16 +1,17 @@ -t2 = q0*q0; -t3 = q1*q1; -t4 = q2*q2; -t5 = q3*q3; -t6 = t2+t3-t4-t5; -t7 = q0*q3*2.0; -t8 = q1*q2*2.0; -t9 = t7+t8; -t10 = 1.0/(t6*t6); -t11 = t9*t9; -t12 = t10*t11; +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/t6; -A0[0][1] = t14*(t15*(q0*q1*2.0-q2*q3*2.0)+t9*t10*(q0*q2*2.0+q1*q3*2.0)); -A0[0][2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8)); +A0[0][0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0)*-2.0; +A0[0][1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0)*-2.0; +A0[0][2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0)*2.0; +A0[0][3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0)*2.0; diff --git a/matlab/scripts/Inertial Nav EKF/temp1.mat b/matlab/scripts/Inertial Nav EKF/temp1.mat index 804be63a42..28375e5022 100644 Binary files a/matlab/scripts/Inertial Nav EKF/temp1.mat and b/matlab/scripts/Inertial Nav EKF/temp1.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/temp2.mat b/matlab/scripts/Inertial Nav EKF/temp2.mat index bc29af7ab5..698457b1fd 100644 Binary files a/matlab/scripts/Inertial Nav EKF/temp2.mat and b/matlab/scripts/Inertial Nav EKF/temp2.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/temp3.mat b/matlab/scripts/Inertial Nav EKF/temp3.mat index d50def9d24..24e0cc9cf4 100644 Binary files a/matlab/scripts/Inertial Nav EKF/temp3.mat and b/matlab/scripts/Inertial Nav EKF/temp3.mat differ