forked from Archive/PX4-Autopilot
EKF: Include all output files generated by derivation script
This commit is contained in:
Paul Riseborough
Roman
parent
470098e182
commit
c6bd93ed40
Binary file not shown.
|
|
@ -0,0 +1,681 @@
|
|||
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);
|
||||
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];
|
||||
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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[10][16] = P[10][16];
|
||||
nextP[11][16] = P[11][16];
|
||||
nextP[12][16] = P[12][16];
|
||||
nextP[13][16] = P[13][16];
|
||||
nextP[14][16] = P[14][16];
|
||||
nextP[15][16] = P[15][16];
|
||||
nextP[16][16] = P[16][16];
|
||||
nextP[0][17] = P[0][17]*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[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[10][17] = P[10][17];
|
||||
nextP[11][17] = P[11][17];
|
||||
nextP[12][17] = P[12][17];
|
||||
nextP[13][17] = P[13][17];
|
||||
nextP[14][17] = P[14][17];
|
||||
nextP[15][17] = P[15][17];
|
||||
nextP[16][17] = P[16][17];
|
||||
nextP[17][17] = P[17][17];
|
||||
nextP[0][18] = P[0][18]*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[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[10][18] = P[10][18];
|
||||
nextP[11][18] = P[11][18];
|
||||
nextP[12][18] = P[12][18];
|
||||
nextP[13][18] = P[13][18];
|
||||
nextP[14][18] = P[14][18];
|
||||
nextP[15][18] = P[15][18];
|
||||
nextP[16][18] = P[16][18];
|
||||
nextP[17][18] = P[17][18];
|
||||
nextP[18][18] = P[18][18];
|
||||
nextP[0][19] = P[0][19]*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[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[10][19] = P[10][19];
|
||||
nextP[11][19] = P[11][19];
|
||||
nextP[12][19] = P[12][19];
|
||||
nextP[13][19] = P[13][19];
|
||||
nextP[14][19] = P[14][19];
|
||||
nextP[15][19] = P[15][19];
|
||||
nextP[16][19] = P[16][19];
|
||||
nextP[17][19] = P[17][19];
|
||||
nextP[18][19] = P[18][19];
|
||||
nextP[19][19] = P[19][19];
|
||||
nextP[0][20] = P[0][20]*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[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[10][20] = P[10][20];
|
||||
nextP[11][20] = P[11][20];
|
||||
nextP[12][20] = P[12][20];
|
||||
nextP[13][20] = P[13][20];
|
||||
nextP[14][20] = P[14][20];
|
||||
nextP[15][20] = P[15][20];
|
||||
nextP[16][20] = P[16][20];
|
||||
nextP[17][20] = P[17][20];
|
||||
nextP[18][20] = P[18][20];
|
||||
nextP[19][20] = P[19][20];
|
||||
nextP[20][20] = P[20][20];
|
||||
nextP[0][21] = P[0][21]*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[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[10][21] = P[10][21];
|
||||
nextP[11][21] = P[11][21];
|
||||
nextP[12][21] = P[12][21];
|
||||
nextP[13][21] = P[13][21];
|
||||
nextP[14][21] = P[14][21];
|
||||
nextP[15][21] = P[15][21];
|
||||
nextP[16][21] = P[16][21];
|
||||
nextP[17][21] = P[17][21];
|
||||
nextP[18][21] = P[18][21];
|
||||
nextP[19][21] = P[19][21];
|
||||
nextP[20][21] = P[20][21];
|
||||
nextP[21][21] = P[21][21];
|
||||
nextP[0][22] = P[0][22]*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[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[10][22] = P[10][22];
|
||||
nextP[11][22] = P[11][22];
|
||||
nextP[12][22] = P[12][22];
|
||||
nextP[13][22] = P[13][22];
|
||||
nextP[14][22] = P[14][22];
|
||||
nextP[15][22] = P[15][22];
|
||||
nextP[16][22] = P[16][22];
|
||||
nextP[17][22] = P[17][22];
|
||||
nextP[18][22] = P[18][22];
|
||||
nextP[19][22] = P[19][22];
|
||||
nextP[20][22] = P[20][22];
|
||||
nextP[21][22] = P[21][22];
|
||||
nextP[22][22] = P[22][22];
|
||||
nextP[0][23] = P[0][23]*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[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[10][23] = P[10][23];
|
||||
nextP[11][23] = P[11][23];
|
||||
nextP[12][23] = P[12][23];
|
||||
nextP[13][23] = P[13][23];
|
||||
nextP[14][23] = P[14][23];
|
||||
nextP[15][23] = P[15][23];
|
||||
nextP[16][23] = P[16][23];
|
||||
nextP[17][23] = P[17][23];
|
||||
nextP[18][23] = P[18][23];
|
||||
nextP[19][23] = P[19][23];
|
||||
nextP[20][23] = P[20][23];
|
||||
nextP[21][23] = P[21][23];
|
||||
nextP[22][23] = P[22][23];
|
||||
nextP[23][23] = P[23][23];
|
||||
float SH_TAS[3][1];
|
||||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||||
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
|
||||
float H_TAS[1][24];
|
||||
H_TAS[0][3] = SH_TAS[2];
|
||||
H_TAS[0][4] = SH_TAS[1];
|
||||
H_TAS[0][5] = 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[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];
|
||||
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);
|
||||
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];
|
||||
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]);
|
||||
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;
|
||||
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[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 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]);
|
||||
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[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 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]);
|
||||
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[17] = 2*q2*q3 - 2*q0*q1;
|
||||
H_MAG[18] = SH_MAG[2];
|
||||
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];
|
||||
SK_MZ[3] = 2*q0*q1 - 2*q2*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];
|
||||
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;
|
||||
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][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];
|
||||
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];
|
||||
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;
|
||||
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][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 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]);
|
||||
Binary file not shown.
|
|
@ -0,0 +1,15 @@
|
|||
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);
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
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);
|
||||
|
|
@ -0,0 +1,89 @@
|
|||
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);
|
||||
|
|
@ -0,0 +1,89 @@
|
|||
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);
|
||||
|
|
@ -0,0 +1,681 @@
|
|||
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;
|
||||
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);
|
||||
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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(11,17) = OP(11,17);
|
||||
nextP(12,17) = OP(12,17);
|
||||
nextP(13,17) = OP(13,17);
|
||||
nextP(14,17) = OP(14,17);
|
||||
nextP(15,17) = OP(15,17);
|
||||
nextP(16,17) = OP(16,17);
|
||||
nextP(17,17) = OP(17,17);
|
||||
nextP(1,18) = OP(1,18)*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(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(11,18) = OP(11,18);
|
||||
nextP(12,18) = OP(12,18);
|
||||
nextP(13,18) = OP(13,18);
|
||||
nextP(14,18) = OP(14,18);
|
||||
nextP(15,18) = OP(15,18);
|
||||
nextP(16,18) = OP(16,18);
|
||||
nextP(17,18) = OP(17,18);
|
||||
nextP(18,18) = OP(18,18);
|
||||
nextP(1,19) = OP(1,19)*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(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(11,19) = OP(11,19);
|
||||
nextP(12,19) = OP(12,19);
|
||||
nextP(13,19) = OP(13,19);
|
||||
nextP(14,19) = OP(14,19);
|
||||
nextP(15,19) = OP(15,19);
|
||||
nextP(16,19) = OP(16,19);
|
||||
nextP(17,19) = OP(17,19);
|
||||
nextP(18,19) = OP(18,19);
|
||||
nextP(19,19) = OP(19,19);
|
||||
nextP(1,20) = OP(1,20)*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(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(11,20) = OP(11,20);
|
||||
nextP(12,20) = OP(12,20);
|
||||
nextP(13,20) = OP(13,20);
|
||||
nextP(14,20) = OP(14,20);
|
||||
nextP(15,20) = OP(15,20);
|
||||
nextP(16,20) = OP(16,20);
|
||||
nextP(17,20) = OP(17,20);
|
||||
nextP(18,20) = OP(18,20);
|
||||
nextP(19,20) = OP(19,20);
|
||||
nextP(20,20) = OP(20,20);
|
||||
nextP(1,21) = OP(1,21)*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(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(11,21) = OP(11,21);
|
||||
nextP(12,21) = OP(12,21);
|
||||
nextP(13,21) = OP(13,21);
|
||||
nextP(14,21) = OP(14,21);
|
||||
nextP(15,21) = OP(15,21);
|
||||
nextP(16,21) = OP(16,21);
|
||||
nextP(17,21) = OP(17,21);
|
||||
nextP(18,21) = OP(18,21);
|
||||
nextP(19,21) = OP(19,21);
|
||||
nextP(20,21) = OP(20,21);
|
||||
nextP(21,21) = OP(21,21);
|
||||
nextP(1,22) = OP(1,22)*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(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(11,22) = OP(11,22);
|
||||
nextP(12,22) = OP(12,22);
|
||||
nextP(13,22) = OP(13,22);
|
||||
nextP(14,22) = OP(14,22);
|
||||
nextP(15,22) = OP(15,22);
|
||||
nextP(16,22) = OP(16,22);
|
||||
nextP(17,22) = OP(17,22);
|
||||
nextP(18,22) = OP(18,22);
|
||||
nextP(19,22) = OP(19,22);
|
||||
nextP(20,22) = OP(20,22);
|
||||
nextP(21,22) = OP(21,22);
|
||||
nextP(22,22) = OP(22,22);
|
||||
nextP(1,23) = OP(1,23)*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(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(11,23) = OP(11,23);
|
||||
nextP(12,23) = OP(12,23);
|
||||
nextP(13,23) = OP(13,23);
|
||||
nextP(14,23) = OP(14,23);
|
||||
nextP(15,23) = OP(15,23);
|
||||
nextP(16,23) = OP(16,23);
|
||||
nextP(17,23) = OP(17,23);
|
||||
nextP(18,23) = OP(18,23);
|
||||
nextP(19,23) = OP(19,23);
|
||||
nextP(20,23) = OP(20,23);
|
||||
nextP(21,23) = OP(21,23);
|
||||
nextP(22,23) = OP(22,23);
|
||||
nextP(23,23) = OP(23,23);
|
||||
nextP(1,24) = OP(1,24)*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(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(11,24) = OP(11,24);
|
||||
nextP(12,24) = OP(12,24);
|
||||
nextP(13,24) = OP(13,24);
|
||||
nextP(14,24) = OP(14,24);
|
||||
nextP(15,24) = OP(15,24);
|
||||
nextP(16,24) = OP(16,24);
|
||||
nextP(17,24) = OP(17,24);
|
||||
nextP(18,24) = OP(18,24);
|
||||
nextP(19,24) = OP(19,24);
|
||||
nextP(20,24) = OP(20,24);
|
||||
nextP(21,24) = OP(21,24);
|
||||
nextP(22,24) = OP(22,24);
|
||||
nextP(23,24) = OP(23,24);
|
||||
nextP(24,24) = OP(24,24);
|
||||
SH_TAS = zeros(3,1);
|
||||
SH_TAS(1) = 1/((ve - vwe)^2 + (vn - vwn)^2 + vd^2)^(1/2);
|
||||
SH_TAS(2) = (SH_TAS(1)*(2*ve - 2*vwe))/2;
|
||||
SH_TAS(3) = (SH_TAS(1)*(2*vn - 2*vwn))/2;
|
||||
H_TAS = zeros(1,24);
|
||||
H_TAS(1,4) = SH_TAS(3);
|
||||
H_TAS(1,5) = SH_TAS(2);
|
||||
H_TAS(1,6) = 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(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);
|
||||
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);
|
||||
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);
|
||||
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));
|
||||
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;
|
||||
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(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);
|
||||
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));
|
||||
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(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;
|
||||
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));
|
||||
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(18) = 2*q2*q3 - 2*q0*q1;
|
||||
H_MAG(19) = SH_MAG(3);
|
||||
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(4) = 2*q0*q1 - 2*q2*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);
|
||||
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;
|
||||
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,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);
|
||||
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);
|
||||
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;
|
||||
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,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);
|
||||
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));
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,737 @@
|
|||
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;
|
||||
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);
|
||||
|
||||
|
||||
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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(11,17) = OP_l_11_c_17_r_;
|
||||
nextP(12,17) = OP_l_12_c_17_r_;
|
||||
nextP(13,17) = OP_l_13_c_17_r_;
|
||||
nextP(14,17) = OP_l_14_c_17_r_;
|
||||
nextP(15,17) = OP_l_15_c_17_r_;
|
||||
nextP(16,17) = OP_l_16_c_17_r_;
|
||||
nextP(17,17) = OP_l_17_c_17_r_;
|
||||
nextP(1,18) = OP_l_1_c_18_r_*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(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(11,18) = OP_l_11_c_18_r_;
|
||||
nextP(12,18) = OP_l_12_c_18_r_;
|
||||
nextP(13,18) = OP_l_13_c_18_r_;
|
||||
nextP(14,18) = OP_l_14_c_18_r_;
|
||||
nextP(15,18) = OP_l_15_c_18_r_;
|
||||
nextP(16,18) = OP_l_16_c_18_r_;
|
||||
nextP(17,18) = OP_l_17_c_18_r_;
|
||||
nextP(18,18) = OP_l_18_c_18_r_;
|
||||
nextP(1,19) = OP_l_1_c_19_r_*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(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(11,19) = OP_l_11_c_19_r_;
|
||||
nextP(12,19) = OP_l_12_c_19_r_;
|
||||
nextP(13,19) = OP_l_13_c_19_r_;
|
||||
nextP(14,19) = OP_l_14_c_19_r_;
|
||||
nextP(15,19) = OP_l_15_c_19_r_;
|
||||
nextP(16,19) = OP_l_16_c_19_r_;
|
||||
nextP(17,19) = OP_l_17_c_19_r_;
|
||||
nextP(18,19) = OP_l_18_c_19_r_;
|
||||
nextP(19,19) = OP_l_19_c_19_r_;
|
||||
nextP(1,20) = OP_l_1_c_20_r_*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(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(11,20) = OP_l_11_c_20_r_;
|
||||
nextP(12,20) = OP_l_12_c_20_r_;
|
||||
nextP(13,20) = OP_l_13_c_20_r_;
|
||||
nextP(14,20) = OP_l_14_c_20_r_;
|
||||
nextP(15,20) = OP_l_15_c_20_r_;
|
||||
nextP(16,20) = OP_l_16_c_20_r_;
|
||||
nextP(17,20) = OP_l_17_c_20_r_;
|
||||
nextP(18,20) = OP_l_18_c_20_r_;
|
||||
nextP(19,20) = OP_l_19_c_20_r_;
|
||||
nextP(20,20) = OP_l_20_c_20_r_;
|
||||
nextP(1,21) = OP_l_1_c_21_r_*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(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(11,21) = OP_l_11_c_21_r_;
|
||||
nextP(12,21) = OP_l_12_c_21_r_;
|
||||
nextP(13,21) = OP_l_13_c_21_r_;
|
||||
nextP(14,21) = OP_l_14_c_21_r_;
|
||||
nextP(15,21) = OP_l_15_c_21_r_;
|
||||
nextP(16,21) = OP_l_16_c_21_r_;
|
||||
nextP(17,21) = OP_l_17_c_21_r_;
|
||||
nextP(18,21) = OP_l_18_c_21_r_;
|
||||
nextP(19,21) = OP_l_19_c_21_r_;
|
||||
nextP(20,21) = OP_l_20_c_21_r_;
|
||||
nextP(21,21) = OP_l_21_c_21_r_;
|
||||
nextP(1,22) = OP_l_1_c_22_r_*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(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(11,22) = OP_l_11_c_22_r_;
|
||||
nextP(12,22) = OP_l_12_c_22_r_;
|
||||
nextP(13,22) = OP_l_13_c_22_r_;
|
||||
nextP(14,22) = OP_l_14_c_22_r_;
|
||||
nextP(15,22) = OP_l_15_c_22_r_;
|
||||
nextP(16,22) = OP_l_16_c_22_r_;
|
||||
nextP(17,22) = OP_l_17_c_22_r_;
|
||||
nextP(18,22) = OP_l_18_c_22_r_;
|
||||
nextP(19,22) = OP_l_19_c_22_r_;
|
||||
nextP(20,22) = OP_l_20_c_22_r_;
|
||||
nextP(21,22) = OP_l_21_c_22_r_;
|
||||
nextP(22,22) = OP_l_22_c_22_r_;
|
||||
nextP(1,23) = OP_l_1_c_23_r_*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(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(11,23) = OP_l_11_c_23_r_;
|
||||
nextP(12,23) = OP_l_12_c_23_r_;
|
||||
nextP(13,23) = OP_l_13_c_23_r_;
|
||||
nextP(14,23) = OP_l_14_c_23_r_;
|
||||
nextP(15,23) = OP_l_15_c_23_r_;
|
||||
nextP(16,23) = OP_l_16_c_23_r_;
|
||||
nextP(17,23) = OP_l_17_c_23_r_;
|
||||
nextP(18,23) = OP_l_18_c_23_r_;
|
||||
nextP(19,23) = OP_l_19_c_23_r_;
|
||||
nextP(20,23) = OP_l_20_c_23_r_;
|
||||
nextP(21,23) = OP_l_21_c_23_r_;
|
||||
nextP(22,23) = OP_l_22_c_23_r_;
|
||||
nextP(23,23) = OP_l_23_c_23_r_;
|
||||
nextP(1,24) = OP_l_1_c_24_r_*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(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(11,24) = OP_l_11_c_24_r_;
|
||||
nextP(12,24) = OP_l_12_c_24_r_;
|
||||
nextP(13,24) = OP_l_13_c_24_r_;
|
||||
nextP(14,24) = OP_l_14_c_24_r_;
|
||||
nextP(15,24) = OP_l_15_c_24_r_;
|
||||
nextP(16,24) = OP_l_16_c_24_r_;
|
||||
nextP(17,24) = OP_l_17_c_24_r_;
|
||||
nextP(18,24) = OP_l_18_c_24_r_;
|
||||
nextP(19,24) = OP_l_19_c_24_r_;
|
||||
nextP(20,24) = OP_l_20_c_24_r_;
|
||||
nextP(21,24) = OP_l_21_c_24_r_;
|
||||
nextP(22,24) = OP_l_22_c_24_r_;
|
||||
nextP(23,24) = OP_l_23_c_24_r_;
|
||||
nextP(24,24) = OP_l_24_c_24_r_;
|
||||
|
||||
|
||||
SH_TAS = zeros(3,1);
|
||||
SH_TAS(1) = 1/((ve - vwe)^2 + (vn - vwn)^2 + vd^2)^(1/2);
|
||||
SH_TAS(2) = (SH_TAS(1)*(2*ve - 2*vwe))/2;
|
||||
SH_TAS(3) = (SH_TAS(1)*(2*vn - 2*vwn))/2;
|
||||
|
||||
H_TAS = zeros(1,24);
|
||||
H_TAS(1,4) = SH_TAS(3);
|
||||
H_TAS(1,5) = SH_TAS(2);
|
||||
H_TAS(1,6) = 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(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));
|
||||
|
||||
|
||||
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(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);
|
||||
|
||||
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_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);
|
||||
|
||||
|
||||
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));
|
||||
|
||||
|
||||
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;
|
||||
|
||||
|
||||
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(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);
|
||||
|
||||
|
||||
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));
|
||||
|
||||
|
||||
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(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;
|
||||
|
||||
|
||||
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));
|
||||
|
||||
|
||||
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(18) = 2*q2*q3 - 2*q0*q1;
|
||||
H_MAG(19) = SH_MAG(3);
|
||||
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(4) = 2*q0*q1 - 2*q2*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));
|
||||
|
||||
|
||||
SH_ACCX = zeros(7,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;
|
||||
|
||||
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,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_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);
|
||||
|
||||
|
||||
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));
|
||||
|
||||
|
||||
SH_ACCY = zeros(6,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;
|
||||
|
||||
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,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);
|
||||
|
||||
|
||||
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));
|
||||
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
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;
|
||||
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));
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
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;
|
||||
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));
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
t2 = magE*magE;
|
||||
t3 = magN*magN;
|
||||
t4 = t2+t3;
|
||||
t5 = P[16][16]*t2;
|
||||
t6 = P[17][17]*t3;
|
||||
t7 = t2*t2;
|
||||
t8 = R_DECL*t7;
|
||||
t9 = t3*t3;
|
||||
t10 = R_DECL*t9;
|
||||
t11 = R_DECL*t2*t3*2.0;
|
||||
t14 = P[16][17]*magE*magN;
|
||||
t15 = P[17][16]*magE*magN;
|
||||
t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
t13 = 1.0/t12;
|
||||
t16 = conjugate(magE);
|
||||
t17 = conjugate(magN);
|
||||
t18 = t16*t16;
|
||||
t19 = t17*t17;
|
||||
t20 = t18+t19;
|
||||
t21 = 1.0/t20;
|
||||
A0[0][0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN);
|
||||
A0[1][0] = -t4*t13*(P[1][16]*magE-P[1][17]*magN);
|
||||
A0[2][0] = -t4*t13*(P[2][16]*magE-P[2][17]*magN);
|
||||
A0[3][0] = -t4*t13*(P[3][16]*magE-P[3][17]*magN);
|
||||
A0[4][0] = -t4*t13*(P[4][16]*magE-P[4][17]*magN);
|
||||
A0[5][0] = -t4*t13*(P[5][16]*magE-P[5][17]*magN);
|
||||
A0[6][0] = -t4*t13*(P[6][16]*magE-P[6][17]*magN);
|
||||
A0[7][0] = -t4*t13*(P[7][16]*magE-P[7][17]*magN);
|
||||
A0[8][0] = -t4*t13*(P[8][16]*magE-P[8][17]*magN);
|
||||
A0[9][0] = -t4*t13*(P[9][16]*magE-P[9][17]*magN);
|
||||
A0[10][0] = -t4*t13*(P[10][16]*magE-P[10][17]*magN);
|
||||
A0[11][0] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
|
||||
A0[12][0] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
|
||||
A0[13][0] = -t4*t13*(P[13][16]*magE-P[13][17]*magN);
|
||||
A0[14][0] = -t4*t13*(P[14][16]*magE-P[14][17]*magN);
|
||||
A0[15][0] = -t4*t13*(P[15][16]*magE-P[15][17]*magN);
|
||||
A0[16][0] = -t4*t13*(P[16][16]*magE-P[16][17]*magN);
|
||||
A0[16][1] = -t16*t21;
|
||||
A0[17][0] = -t4*t13*(P[17][16]*magE-P[17][17]*magN);
|
||||
A0[17][1] = t17*t21;
|
||||
A0[18][0] = -t4*t13*(P[18][16]*magE-P[18][17]*magN);
|
||||
A0[19][0] = -t4*t13*(P[19][16]*magE-P[19][17]*magN);
|
||||
A0[20][0] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
|
||||
A0[21][0] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
|
||||
A0[22][0] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
|
||||
A0[23][0] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in New Issue