forked from Archive/PX4-Autopilot
687 lines
110 KiB
Plaintext
687 lines
110 KiB
Plaintext
|
float SF[21][1];
|
||
|
|
SF[0] = dvz - dvz_b;
|
||
|
|
SF[1] = dvy - dvy_b;
|
||
|
|
SF[2] = dvx - dvx_b;
|
||
|
|
SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0];
|
||
|
|
SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2];
|
||
|
|
SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1];
|
||
|
|
SF[6] = day/2 - day_b/2;
|
||
|
|
SF[7] = daz/2 - daz_b/2;
|
||
|
|
SF[8] = dax/2 - dax_b/2;
|
||
|
|
SF[9] = dax_b/2 - dax/2;
|
||
|
|
SF[10] = daz_b/2 - daz/2;
|
||
|
|
SF[11] = day_b/2 - day/2;
|
||
|
|
SF[12] = 2*q1*SF[1];
|
||
|
|
SF[13] = 2*q0*SF[0];
|
||
|
|
SF[14] = q1/2;
|
||
|
|
SF[15] = q2/2;
|
||
|
|
SF[16] = q3/2;
|
||
|
|
SF[17] = sq(q3);
|
||
|
|
SF[18] = sq(q2);
|
||
|
|
SF[19] = sq(q1);
|
||
|
|
SF[20] = sq(q0);
|
||
|
|
float SG[8][1];
|
||
|
|
SG[0] = q0/2;
|
||
|
|
SG[1] = sq(q3);
|
||
|
|
SG[2] = sq(q2);
|
||
|
|
SG[3] = sq(q1);
|
||
|
|
SG[4] = sq(q0);
|
||
|
|
SG[5] = 2*q2*q3;
|
||
|
|
SG[6] = 2*q1*q3;
|
||
|
|
SG[7] = 2*q1*q2;
|
||
|
|
float SQ[11][1];
|
||
|
|
SQ[0] = dvzVar*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
|
||
|
|
SQ[1] = dvzVar*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
|
||
|
|
SQ[2] = dvzVar*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyVar*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
|
||
|
|
SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4;
|
||
|
|
SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4;
|
||
|
|
SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4;
|
||
|
|
SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4;
|
||
|
|
SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2;
|
||
|
|
SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4;
|
||
|
|
SQ[9] = sq(SG[0]);
|
||
|
|
SQ[10] = sq(q1);
|
||
|
|
float SPP(11,1);
|
||
|
|
SPP[0] = SF[12] + SF[13] - 2*q2*SF[2];
|
||
|
|
SPP[1] = SF[17] - SF[18] - SF[19] + SF[20];
|
||
|
|
SPP[2] = SF[17] - SF[18] + SF[19] - SF[20];
|
||
|
|
SPP[3] = SF[17] + SF[18] - SF[19] - SF[20];
|
||
|
|
SPP[4] = 2*q0*q2 - 2*q1*q3;
|
||
|
|
SPP[5] = 2*q0*q1 - 2*q2*q3;
|
||
|
|
SPP[6] = 2*q0*q3 - 2*q1*q2;
|
||
|
|
SPP[7] = 2*q0*q1 + 2*q2*q3;
|
||
|
|
SPP[8] = 2*q0*q3 + 2*q1*q2;
|
||
|
|
SPP[9] = 2*q0*q2 + 2*q1*q3;
|
||
|
|
SPP[10] = SF[16];
|
||
|
|
float nextP[24][24];
|
||
|
|
nextP(0,0) = P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[11]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[10]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SF[14]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) + SF[15]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) + SPP[10]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4;
|
||
|
|
nextP(0,1) = P(0,1) + SQ[8] + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10] + SF[8]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[7]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[11]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) - SF[15]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) + SPP[10]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) - (q0*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]))/2;
|
||
|
|
nextP(1,1) = P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] + daxVar*SQ[9] - (P(10,1)*q0)/2 + SF[8]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[7]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SF[11]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) - SF[15]*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)/2) + SPP[10]*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)/2) + (dayVar*sq(q3))/4 + (dazVar*sq(q2))/4 - (q0*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)/2))/2;
|
||
|
|
nextP(0,2) = P(0,2) + SQ[7] + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10] + SF[6]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[10]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[8]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SF[14]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) - SPP[10]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) - (q0*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]))/2;
|
||
|
|
nextP(1,2) = P(1,2) + SQ[5] + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2 + SF[6]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[10]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) + SF[8]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) + SF[14]*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)/2) - SPP[10]*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)/2) - (q0*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)/2))/2;
|
||
|
|
nextP(2,2) = P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])/4 - (P(11,2)*q0)/2 + SF[6]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SF[10]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) + SF[8]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2) + SF[14]*(P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)/2) - SPP[10]*(P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)/2) + (daxVar*sq(q3))/4 - (q0*(P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)/2))/2;
|
||
|
|
nextP(0,3) = P(0,3) + SQ[6] + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10] + SF[7]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[6]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[9]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[15]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) - SF[14]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) - (q0*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]))/2;
|
||
|
|
nextP(1,3) = P(1,3) + SQ[4] + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2 + SF[7]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[6]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) + SF[9]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SF[15]*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)/2) - SF[14]*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)/2) - (q0*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)/2))/2;
|
||
|
|
nextP(2,3) = P(2,3) + SQ[3] + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2 + SF[7]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SF[6]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) + SF[9]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)/2) + SF[15]*(P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)/2) - SF[14]*(P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)/2) - (q0*(P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)/2))/2;
|
||
|
|
nextP(3,3) = P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] + (dayVar*SQ[10])/4 + dazVar*SQ[9] - (P(12,3)*q0)/2 + SF[7]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)/2) + SF[6]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)/2) + SF[9]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)/2) + SF[15]*(P(3,10) + P(0,10)*SF[7] + P(1,10)*SF[6] + P(2,10)*SF[9] + P(10,10)*SF[15] - P(11,10)*SF[14] - (P(12,10)*q0)/2) - SF[14]*(P(3,11) + P(0,11)*SF[7] + P(1,11)*SF[6] + P(2,11)*SF[9] + P(10,11)*SF[15] - P(11,11)*SF[14] - (P(12,11)*q0)/2) + (daxVar*sq(q2))/4 - (q0*(P(3,12) + P(0,12)*SF[7] + P(1,12)*SF[6] + P(2,12)*SF[9] + P(10,12)*SF[15] - P(11,12)*SF[14] - (P(12,12)*q0)/2))/2;
|
||
|
|
nextP(0,4) = P(0,4) + P(1,4)*SF[9] + P(2,4)*SF[11] + P(3,4)*SF[10] + P(10,4)*SF[14] + P(11,4)*SF[15] + P(12,4)*SPP[10] + SF[5]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[3]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SF[4]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SPP[0]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SPP[3]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) + SPP[6]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) - SPP[9]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]);
|
||
|
|
nextP(1,4) = P(1,4) + P(0,4)*SF[8] + P(2,4)*SF[7] + P(3,4)*SF[11] - P(12,4)*SF[15] + P(11,4)*SPP[10] - (P(10,4)*q0)/2 + SF[5]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[3]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) - SF[4]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) + SPP[0]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SPP[3]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)/2) + SPP[6]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)/2) - SPP[9]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)/2);
|
||
|
|
nextP(2,4) = P(2,4) + P(0,4)*SF[6] + P(1,4)*SF[10] + P(3,4)*SF[8] + P(12,4)*SF[14] - P(10,4)*SPP[10] - (P(11,4)*q0)/2 + SF[5]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SF[3]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) - SF[4]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2) + SPP[0]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)/2) + SPP[3]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)/2) + SPP[6]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)/2) - SPP[9]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)/2);
|
||
|
|
nextP(3,4) = P(3,4) + P(0,4)*SF[7] + P(1,4)*SF[6] + P(2,4)*SF[9] + P(10,4)*SF[15] - P(11,4)*SF[14] - (P(12,4)*q0)/2 + SF[5]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)/2) + SF[3]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)/2) - SF[4]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)/2) + SPP[0]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)/2) + SPP[3]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)/2) + SPP[6]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)/2) - SPP[9]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)/2);
|
||
|
|
nextP(4,4) = P(4,4) + P(0,4)*SF[5] + P(1,4)*SF[3] - P(3,4)*SF[4] + P(2,4)*SPP[0] + P(13,4)*SPP[3] + P(14,4)*SPP[6] - P(15,4)*SPP[9] + dvyVar*sq(SG[7] - 2*q0*q3) + dvzVar*sq(SG[6] + 2*q0*q2) + SF[5]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SF[3]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SF[4]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) + SPP[0]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SPP[3]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) + SPP[6]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) - SPP[9]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]);
|
||
|
|
nextP(0,5) = P(0,5) + P(1,5)*SF[9] + P(2,5)*SF[11] + P(3,5)*SF[10] + P(10,5)*SF[14] + P(11,5)*SF[15] + P(12,5)*SPP[10] + SF[4]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[3]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[5]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) - SPP[0]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SPP[8]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) + SPP[2]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) + SPP[5]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]);
|
||
|
|
nextP(1,5) = P(1,5) + P(0,5)*SF[8] + P(2,5)*SF[7] + P(3,5)*SF[11] - P(12,5)*SF[15] + P(11,5)*SPP[10] - (P(10,5)*q0)/2 + SF[4]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SF[3]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SF[5]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) - SPP[0]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) - SPP[8]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)/2) + SPP[2]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)/2) + SPP[5]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)/2);
|
||
|
|
nextP(2,5) = P(2,5) + P(0,5)*SF[6] + P(1,5)*SF[10] + P(3,5)*SF[8] + P(12,5)*SF[14] - P(10,5)*SPP[10] - (P(11,5)*q0)/2 + SF[4]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SF[3]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)/2) + SF[5]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2) - SPP[0]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) - SPP[8]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)/2) + SPP[2]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)/2) + SPP[5]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)/2);
|
||
|
|
nextP(3,5) = P(3,5) + P(0,5)*SF[7] + P(1,5)*SF[6] + P(2,5)*SF[9] + P(10,5)*SF[15] - P(11,5)*SF[14] - (P(12,5)*q0)/2 + SF[4]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)/2) + SF[3]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)/2) + SF[5]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)/2) - SPP[0]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)/2) - SPP[8]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)/2) + SPP[2]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)/2) + SPP[5]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)/2);
|
||
|
|
nextP(4,5) = P(4,5) + SQ[2] + P(0,5)*SF[5] + P(1,5)*SF[3] - P(3,5)*SF[4] + P(2,5)*SPP[0] + P(13,5)*SPP[3] + P(14,5)*SPP[6] - P(15,5)*SPP[9] + SF[4]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SF[3]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SF[5]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) - SPP[0]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SPP[8]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) + SPP[2]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) + SPP[5]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]);
|
||
|
|
nextP(5,5) = P(5,5) + P(0,5)*SF[4] + P(2,5)*SF[3] + P(3,5)*SF[5] - P(1,5)*SPP[0] - P(13,5)*SPP[8] + P(14,5)*SPP[2] + P(15,5)*SPP[5] + dvxVar*sq(SG[7] + 2*q0*q3) + dvzVar*sq(SG[5] - 2*q0*q1) + SF[4]*(P(5,0) + P(0,0)*SF[4] + P(2,0)*SF[3] + P(3,0)*SF[5] - P(1,0)*SPP[0] - P(13,0)*SPP[8] + P(14,0)*SPP[2] + P(15,0)*SPP[5]) + SF[3]*(P(5,2) + P(0,2)*SF[4] + P(2,2)*SF[3] + P(3,2)*SF[5] - P(1,2)*SPP[0] - P(13,2)*SPP[8] + P(14,2)*SPP[2] + P(15,2)*SPP[5]) + SF[5]*(P(5,3) + P(0,3)*SF[4] + P(2,3)*SF[3] + P(3,3)*SF[5] - P(1,3)*SPP[0] - P(13,3)*SPP[8] + P(14,3)*SPP[2] + P(15,3)*SPP[5]) - SPP[0]*(P(5,1) + P(0,1)*SF[4] + P(2,1)*SF[3] + P(3,1)*SF[5] - P(1,1)*SPP[0] - P(13,1)*SPP[8] + P(14,1)*SPP[2] + P(15,1)*SPP[5]) - SPP[8]*(P(5,13) + P(0,13)*SF[4] + P(2,13)*SF[3] + P(3,13)*SF[5] - P(1,13)*SPP[0] - P(13,13)*SPP[8] + P(14,13)*SPP[2] + P(15,13)*SPP[5]) + SPP[2]*(P(5,14) + P(0,14)*SF[4] + P(2,14)*SF[3] + P(3,14)*SF[5] - P(1,14)*SPP[0] - P(13,14)*SPP[8] + P(14,14)*SPP[2] + P(15,14)*SPP[5]) + SPP[5]*(P(5,15) + P(0,15)*SF[4] + P(2,15)*SF[3] + P(3,15)*SF[5] - P(1,15)*SPP[0] - P(13,15)*SPP[8] + P(14,15)*SPP[2] + P(15,15)*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]);
|
||
|
|
nextP(0,6) = P(0,6) + P(1,6)*SF[9] + P(2,6)*SF[11] + P(3,6)*SF[10] + P(10,6)*SF[14] + P(11,6)*SF[15] + P(12,6)*SPP[10] + SF[4]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SF[5]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[3]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SPP[0]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SPP[4]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) - SPP[7]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) - SPP[1]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]);
|
||
|
|
nextP(1,6) = P(1,6) + P(0,6)*SF[8] + P(2,6)*SF[7] + P(3,6)*SF[11] - P(12,6)*SF[15] + P(11,6)*SPP[10] - (P(10,6)*q0)/2 + SF[4]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)/2) - SF[5]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)/2) + SF[3]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)/2) + SPP[0]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)/2) + SPP[4]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)/2) - SPP[7]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)/2) - SPP[1]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)/2);
|
||
|
|
nextP(2,6) = P(2,6) + P(0,6)*SF[6] + P(1,6)*SF[10] + P(3,6)*SF[8] + P(12,6)*SF[14] - P(10,6)*SPP[10] - (P(11,6)*q0)/2 + SF[4]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)/2) - SF[5]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)/2) + SF[3]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)/2) + SPP[0]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)/2) + SPP[4]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)/2) - SPP[7]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)/2) - SPP[1]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)/2);
|
||
|
|
nextP(3,6) = P(3,6) + P(0,6)*SF[7] + P(1,6)*SF[6] + P(2,6)*SF[9] + P(10,6)*SF[15] - P(11,6)*SF[14] - (P(12,6)*q0)/2 + SF[4]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)/2) - SF[5]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)/2) + SF[3]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)/2) + SPP[0]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)/2) + SPP[4]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)/2) - SPP[7]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)/2) - SPP[1]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)/2);
|
||
|
|
nextP(4,6) = P(4,6) + SQ[1] + P(0,6)*SF[5] + P(1,6)*SF[3] - P(3,6)*SF[4] + P(2,6)*SPP[0] + P(13,6)*SPP[3] + P(14,6)*SPP[6] - P(15,6)*SPP[9] + SF[4]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SF[5]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SF[3]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) + SPP[0]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SPP[4]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) - SPP[7]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) - SPP[1]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]);
|
||
|
|
nextP(5,6) = P(5,6) + SQ[0] + P(0,6)*SF[4] + P(2,6)*SF[3] + P(3,6)*SF[5] - P(1,6)*SPP[0] - P(13,6)*SPP[8] + P(14,6)*SPP[2] + P(15,6)*SPP[5] + SF[4]*(P(5,1) + P(0,1)*SF[4] + P(2,1)*SF[3] + P(3,1)*SF[5] - P(1,1)*SPP[0] - P(13,1)*SPP[8] + P(14,1)*SPP[2] + P(15,1)*SPP[5]) - SF[5]*(P(5,2) + P(0,2)*SF[4] + P(2,2)*SF[3] + P(3,2)*SF[5] - P(1,2)*SPP[0] - P(13,2)*SPP[8] + P(14,2)*SPP[2] + P(15,2)*SPP[5]) + SF[3]*(P(5,3) + P(0,3)*SF[4] + P(2,3)*SF[3] + P(3,3)*SF[5] - P(1,3)*SPP[0] - P(13,3)*SPP[8] + P(14,3)*SPP[2] + P(15,3)*SPP[5]) + SPP[0]*(P(5,0) + P(0,0)*SF[4] + P(2,0)*SF[3] + P(3,0)*SF[5] - P(1,0)*SPP[0] - P(13,0)*SPP[8] + P(14,0)*SPP[2] + P(15,0)*SPP[5]) + SPP[4]*(P(5,13) + P(0,13)*SF[4] + P(2,13)*SF[3] + P(3,13)*SF[5] - P(1,13)*SPP[0] - P(13,13)*SPP[8] + P(14,13)*SPP[2] + P(15,13)*SPP[5]) - SPP[7]*(P(5,14) + P(0,14)*SF[4] + P(2,14)*SF[3] + P(3,14)*SF[5] - P(1,14)*SPP[0] - P(13,14)*SPP[8] + P(14,14)*SPP[2] + P(15,14)*SPP[5]) - SPP[1]*(P(5,15) + P(0,15)*SF[4] + P(2,15)*SF[3] + P(3,15)*SF[5] - P(1,15)*SPP[0] - P(13,15)*SPP[8] + P(14,15)*SPP[2] + P(15,15)*SPP[5]);
|
||
|
|
nextP(6,6) = P(6,6) + P(1,6)*SF[4] - P(2,6)*SF[5] + P(3,6)*SF[3] + P(0,6)*SPP[0] + P(13,6)*SPP[4] - P(14,6)*SPP[7] - P(15,6)*SPP[1] + dvxVar*sq(SG[6] - 2*q0*q2) + dvyVar*sq(SG[5] + 2*q0*q1) + SF[4]*(P(6,1) + P(1,1)*SF[4] - P(2,1)*SF[5] + P(3,1)*SF[3] + P(0,1)*SPP[0] + P(13,1)*SPP[4] - P(14,1)*SPP[7] - P(15,1)*SPP[1]) - SF[5]*(P(6,2) + P(1,2)*SF[4] - P(2,2)*SF[5] + P(3,2)*SF[3] + P(0,2)*SPP[0] + P(13,2)*SPP[4] - P(14,2)*SPP[7] - P(15,2)*SPP[1]) + SF[3]*(P(6,3) + P(1,3)*SF[4] - P(2,3)*SF[5] + P(3,3)*SF[3] + P(0,3)*SPP[0] + P(13,3)*SPP[4] - P(14,3)*SPP[7] - P(15,3)*SPP[1]) + SPP[0]*(P(6,0) + P(1,0)*SF[4] - P(2,0)*SF[5] + P(3,0)*SF[3] + P(0,0)*SPP[0] + P(13,0)*SPP[4] - P(14,0)*SPP[7] - P(15,0)*SPP[1]) + SPP[4]*(P(6,13) + P(1,13)*SF[4] - P(2,13)*SF[5] + P(3,13)*SF[3] + P(0,13)*SPP[0] + P(13,13)*SPP[4] - P(14,13)*SPP[7] - P(15,13)*SPP[1]) - SPP[7]*(P(6,14) + P(1,14)*SF[4] - P(2,14)*SF[5] + P(3,14)*SF[3] + P(0,14)*SPP[0] + P(13,14)*SPP[4] - P(14,14)*SPP[7] - P(15,14)*SPP[1]) - SPP[1]*(P(6,15) + P(1,15)*SF[4] - P(2,15)*SF[5] + P(3,15)*SF[3] + P(0,15)*SPP[0] + P(13,15)*SPP[4] - P(14,15)*SPP[7] - P(15,15)*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]);
|
||
|
|
nextP(0,7) = P(0,7) + P(1,7)*SF[9] + P(2,7)*SF[11] + P(3,7)*SF[10] + P(10,7)*SF[14] + P(11,7)*SF[15] + P(12,7)*SPP[10] + dt*(P(0,4) + P(1,4)*SF[9] + P(2,4)*SF[11] + P(3,4)*SF[10] + P(10,4)*SF[14] + P(11,4)*SF[15] + P(12,4)*SPP[10]);
|
||
|
|
nextP(1,7) = P(1,7) + P(0,7)*SF[8] + P(2,7)*SF[7] + P(3,7)*SF[11] - P(12,7)*SF[15] + P(11,7)*SPP[10] - (P(10,7)*q0)/2 + dt*(P(1,4) + P(0,4)*SF[8] + P(2,4)*SF[7] + P(3,4)*SF[11] - P(12,4)*SF[15] + P(11,4)*SPP[10] - (P(10,4)*q0)/2);
|
||
|
|
nextP(2,7) = P(2,7) + P(0,7)*SF[6] + P(1,7)*SF[10] + P(3,7)*SF[8] + P(12,7)*SF[14] - P(10,7)*SPP[10] - (P(11,7)*q0)/2 + dt*(P(2,4) + P(0,4)*SF[6] + P(1,4)*SF[10] + P(3,4)*SF[8] + P(12,4)*SF[14] - P(10,4)*SPP[10] - (P(11,4)*q0)/2);
|
||
|
|
nextP(3,7) = P(3,7) + P(0,7)*SF[7] + P(1,7)*SF[6] + P(2,7)*SF[9] + P(10,7)*SF[15] - P(11,7)*SF[14] - (P(12,7)*q0)/2 + dt*(P(3,4) + P(0,4)*SF[7] + P(1,4)*SF[6] + P(2,4)*SF[9] + P(10,4)*SF[15] - P(11,4)*SF[14] - (P(12,4)*q0)/2);
|
||
|
|
nextP(4,7) = P(4,7) + P(0,7)*SF[5] + P(1,7)*SF[3] - P(3,7)*SF[4] + P(2,7)*SPP[0] + P(13,7)*SPP[3] + P(14,7)*SPP[6] - P(15,7)*SPP[9] + dt*(P(4,4) + P(0,4)*SF[5] + P(1,4)*SF[3] - P(3,4)*SF[4] + P(2,4)*SPP[0] + P(13,4)*SPP[3] + P(14,4)*SPP[6] - P(15,4)*SPP[9]);
|
||
|
|
nextP(5,7) = P(5,7) + P(0,7)*SF[4] + P(2,7)*SF[3] + P(3,7)*SF[5] - P(1,7)*SPP[0] - P(13,7)*SPP[8] + P(14,7)*SPP[2] + P(15,7)*SPP[5] + dt*(P(5,4) + P(0,4)*SF[4] + P(2,4)*SF[3] + P(3,4)*SF[5] - P(1,4)*SPP[0] - P(13,4)*SPP[8] + P(14,4)*SPP[2] + P(15,4)*SPP[5]);
|
||
|
|
nextP(6,7) = P(6,7) + P(1,7)*SF[4] - P(2,7)*SF[5] + P(3,7)*SF[3] + P(0,7)*SPP[0] + P(13,7)*SPP[4] - P(14,7)*SPP[7] - P(15,7)*SPP[1] + dt*(P(6,4) + P(1,4)*SF[4] - P(2,4)*SF[5] + P(3,4)*SF[3] + P(0,4)*SPP[0] + P(13,4)*SPP[4] - P(14,4)*SPP[7] - P(15,4)*SPP[1]);
|
||
|
|
nextP(7,7) = P(7,7) + P(4,7)*dt + dt*(P(7,4) + P(4,4)*dt);
|
||
|
|
nextP(0,8) = P(0,8) + P(1,8)*SF[9] + P(2,8)*SF[11] + P(3,8)*SF[10] + P(10,8)*SF[14] + P(11,8)*SF[15] + P(12,8)*SPP[10] + dt*(P(0,5) + P(1,5)*SF[9] + P(2,5)*SF[11] + P(3,5)*SF[10] + P(10,5)*SF[14] + P(11,5)*SF[15] + P(12,5)*SPP[10]);
|
||
|
|
nextP(1,8) = P(1,8) + P(0,8)*SF[8] + P(2,8)*SF[7] + P(3,8)*SF[11] - P(12,8)*SF[15] + P(11,8)*SPP[10] - (P(10,8)*q0)/2 + dt*(P(1,5) + P(0,5)*SF[8] + P(2,5)*SF[7] + P(3,5)*SF[11] - P(12,5)*SF[15] + P(11,5)*SPP[10] - (P(10,5)*q0)/2);
|
||
|
|
nextP(2,8) = P(2,8) + P(0,8)*SF[6] + P(1,8)*SF[10] + P(3,8)*SF[8] + P(12,8)*SF[14] - P(10,8)*SPP[10] - (P(11,8)*q0)/2 + dt*(P(2,5) + P(0,5)*SF[6] + P(1,5)*SF[10] + P(3,5)*SF[8] + P(12,5)*SF[14] - P(10,5)*SPP[10] - (P(11,5)*q0)/2);
|
||
|
|
nextP(3,8) = P(3,8) + P(0,8)*SF[7] + P(1,8)*SF[6] + P(2,8)*SF[9] + P(10,8)*SF[15] - P(11,8)*SF[14] - (P(12,8)*q0)/2 + dt*(P(3,5) + P(0,5)*SF[7] + P(1,5)*SF[6] + P(2,5)*SF[9] + P(10,5)*SF[15] - P(11,5)*SF[14] - (P(12,5)*q0)/2);
|
||
|
|
nextP(4,8) = P(4,8) + P(0,8)*SF[5] + P(1,8)*SF[3] - P(3,8)*SF[4] + P(2,8)*SPP[0] + P(13,8)*SPP[3] + P(14,8)*SPP[6] - P(15,8)*SPP[9] + dt*(P(4,5) + P(0,5)*SF[5] + P(1,5)*SF[3] - P(3,5)*SF[4] + P(2,5)*SPP[0] + P(13,5)*SPP[3] + P(14,5)*SPP[6] - P(15,5)*SPP[9]);
|
||
|
|
nextP(5,8) = P(5,8) + P(0,8)*SF[4] + P(2,8)*SF[3] + P(3,8)*SF[5] - P(1,8)*SPP[0] - P(13,8)*SPP[8] + P(14,8)*SPP[2] + P(15,8)*SPP[5] + dt*(P(5,5) + P(0,5)*SF[4] + P(2,5)*SF[3] + P(3,5)*SF[5] - P(1,5)*SPP[0] - P(13,5)*SPP[8] + P(14,5)*SPP[2] + P(15,5)*SPP[5]);
|
||
|
|
nextP(6,8) = P(6,8) + P(1,8)*SF[4] - P(2,8)*SF[5] + P(3,8)*SF[3] + P(0,8)*SPP[0] + P(13,8)*SPP[4] - P(14,8)*SPP[7] - P(15,8)*SPP[1] + dt*(P(6,5) + P(1,5)*SF[4] - P(2,5)*SF[5] + P(3,5)*SF[3] + P(0,5)*SPP[0] + P(13,5)*SPP[4] - P(14,5)*SPP[7] - P(15,5)*SPP[1]);
|
||
|
|
nextP(7,8) = P(7,8) + P(4,8)*dt + dt*(P(7,5) + P(4,5)*dt);
|
||
|
|
nextP(8,8) = P(8,8) + P(5,8)*dt + dt*(P(8,5) + P(5,5)*dt);
|
||
|
|
nextP(0,9) = P(0,9) + P(1,9)*SF[9] + P(2,9)*SF[11] + P(3,9)*SF[10] + P(10,9)*SF[14] + P(11,9)*SF[15] + P(12,9)*SPP[10] + dt*(P(0,6) + P(1,6)*SF[9] + P(2,6)*SF[11] + P(3,6)*SF[10] + P(10,6)*SF[14] + P(11,6)*SF[15] + P(12,6)*SPP[10]);
|
||
|
|
nextP(1,9) = P(1,9) + P(0,9)*SF[8] + P(2,9)*SF[7] + P(3,9)*SF[11] - P(12,9)*SF[15] + P(11,9)*SPP[10] - (P(10,9)*q0)/2 + dt*(P(1,6) + P(0,6)*SF[8] + P(2,6)*SF[7] + P(3,6)*SF[11] - P(12,6)*SF[15] + P(11,6)*SPP[10] - (P(10,6)*q0)/2);
|
||
|
|
nextP(2,9) = P(2,9) + P(0,9)*SF[6] + P(1,9)*SF[10] + P(3,9)*SF[8] + P(12,9)*SF[14] - P(10,9)*SPP[10] - (P(11,9)*q0)/2 + dt*(P(2,6) + P(0,6)*SF[6] + P(1,6)*SF[10] + P(3,6)*SF[8] + P(12,6)*SF[14] - P(10,6)*SPP[10] - (P(11,6)*q0)/2);
|
||
|
|
nextP(3,9) = P(3,9) + P(0,9)*SF[7] + P(1,9)*SF[6] + P(2,9)*SF[9] + P(10,9)*SF[15] - P(11,9)*SF[14] - (P(12,9)*q0)/2 + dt*(P(3,6) + P(0,6)*SF[7] + P(1,6)*SF[6] + P(2,6)*SF[9] + P(10,6)*SF[15] - P(11,6)*SF[14] - (P(12,6)*q0)/2);
|
||
|
|
nextP(4,9) = P(4,9) + P(0,9)*SF[5] + P(1,9)*SF[3] - P(3,9)*SF[4] + P(2,9)*SPP[0] + P(13,9)*SPP[3] + P(14,9)*SPP[6] - P(15,9)*SPP[9] + dt*(P(4,6) + P(0,6)*SF[5] + P(1,6)*SF[3] - P(3,6)*SF[4] + P(2,6)*SPP[0] + P(13,6)*SPP[3] + P(14,6)*SPP[6] - P(15,6)*SPP[9]);
|
||
|
|
nextP(5,9) = P(5,9) + P(0,9)*SF[4] + P(2,9)*SF[3] + P(3,9)*SF[5] - P(1,9)*SPP[0] - P(13,9)*SPP[8] + P(14,9)*SPP[2] + P(15,9)*SPP[5] + dt*(P(5,6) + P(0,6)*SF[4] + P(2,6)*SF[3] + P(3,6)*SF[5] - P(1,6)*SPP[0] - P(13,6)*SPP[8] + P(14,6)*SPP[2] + P(15,6)*SPP[5]);
|
||
|
|
nextP(6,9) = P(6,9) + P(1,9)*SF[4] - P(2,9)*SF[5] + P(3,9)*SF[3] + P(0,9)*SPP[0] + P(13,9)*SPP[4] - P(14,9)*SPP[7] - P(15,9)*SPP[1] + dt*(P(6,6) + P(1,6)*SF[4] - P(2,6)*SF[5] + P(3,6)*SF[3] + P(0,6)*SPP[0] + P(13,6)*SPP[4] - P(14,6)*SPP[7] - P(15,6)*SPP[1]);
|
||
|
|
nextP(7,9) = P(7,9) + P(4,9)*dt + dt*(P(7,6) + P(4,6)*dt);
|
||
|
|
nextP(8,9) = P(8,9) + P(5,9)*dt + dt*(P(8,6) + P(5,6)*dt);
|
||
|
|
nextP(9,9) = P(9,9) + P(6,9)*dt + dt*(P(9,6) + P(6,6)*dt);
|
||
|
|
nextP(0,10) = P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10];
|
||
|
|
nextP(1,10) = P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)/2;
|
||
|
|
nextP(2,10) = P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)/2;
|
||
|
|
nextP(3,10) = P(3,10) + P(0,10)*SF[7] + P(1,10)*SF[6] + P(2,10)*SF[9] + P(10,10)*SF[15] - P(11,10)*SF[14] - (P(12,10)*q0)/2;
|
||
|
|
nextP(4,10) = P(4,10) + P(0,10)*SF[5] + P(1,10)*SF[3] - P(3,10)*SF[4] + P(2,10)*SPP[0] + P(13,10)*SPP[3] + P(14,10)*SPP[6] - P(15,10)*SPP[9];
|
||
|
|
nextP(5,10) = P(5,10) + P(0,10)*SF[4] + P(2,10)*SF[3] + P(3,10)*SF[5] - P(1,10)*SPP[0] - P(13,10)*SPP[8] + P(14,10)*SPP[2] + P(15,10)*SPP[5];
|
||
|
|
nextP(6,10) = P(6,10) + P(1,10)*SF[4] - P(2,10)*SF[5] + P(3,10)*SF[3] + P(0,10)*SPP[0] + P(13,10)*SPP[4] - P(14,10)*SPP[7] - P(15,10)*SPP[1];
|
||
|
|
nextP(7,10) = P(7,10) + P(4,10)*dt;
|
||
|
|
nextP(8,10) = P(8,10) + P(5,10)*dt;
|
||
|
|
nextP(9,10) = P(9,10) + P(6,10)*dt;
|
||
|
|
nextP(10,10) = P(10,10);
|
||
|
|
nextP(0,11) = P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10];
|
||
|
|
nextP(1,11) = P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)/2;
|
||
|
|
nextP(2,11) = P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)/2;
|
||
|
|
nextP(3,11) = P(3,11) + P(0,11)*SF[7] + P(1,11)*SF[6] + P(2,11)*SF[9] + P(10,11)*SF[15] - P(11,11)*SF[14] - (P(12,11)*q0)/2;
|
||
|
|
nextP(4,11) = P(4,11) + P(0,11)*SF[5] + P(1,11)*SF[3] - P(3,11)*SF[4] + P(2,11)*SPP[0] + P(13,11)*SPP[3] + P(14,11)*SPP[6] - P(15,11)*SPP[9];
|
||
|
|
nextP(5,11) = P(5,11) + P(0,11)*SF[4] + P(2,11)*SF[3] + P(3,11)*SF[5] - P(1,11)*SPP[0] - P(13,11)*SPP[8] + P(14,11)*SPP[2] + P(15,11)*SPP[5];
|
||
|
|
nextP(6,11) = P(6,11) + P(1,11)*SF[4] - P(2,11)*SF[5] + P(3,11)*SF[3] + P(0,11)*SPP[0] + P(13,11)*SPP[4] - P(14,11)*SPP[7] - P(15,11)*SPP[1];
|
||
|
|
nextP(7,11) = P(7,11) + P(4,11)*dt;
|
||
|
|
nextP(8,11) = P(8,11) + P(5,11)*dt;
|
||
|
|
nextP(9,11) = P(9,11) + P(6,11)*dt;
|
||
|
|
nextP(10,11) = P(10,11);
|
||
|
|
nextP(11,11) = P(11,11);
|
||
|
|
nextP(0,12) = P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10];
|
||
|
|
nextP(1,12) = P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)/2;
|
||
|
|
nextP(2,12) = P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)/2;
|
||
|
|
nextP(3,12) = P(3,12) + P(0,12)*SF[7] + P(1,12)*SF[6] + P(2,12)*SF[9] + P(10,12)*SF[15] - P(11,12)*SF[14] - (P(12,12)*q0)/2;
|
||
|
|
nextP(4,12) = P(4,12) + P(0,12)*SF[5] + P(1,12)*SF[3] - P(3,12)*SF[4] + P(2,12)*SPP[0] + P(13,12)*SPP[3] + P(14,12)*SPP[6] - P(15,12)*SPP[9];
|
||
|
|
nextP(5,12) = P(5,12) + P(0,12)*SF[4] + P(2,12)*SF[3] + P(3,12)*SF[5] - P(1,12)*SPP[0] - P(13,12)*SPP[8] + P(14,12)*SPP[2] + P(15,12)*SPP[5];
|
||
|
|
nextP(6,12) = P(6,12) + P(1,12)*SF[4] - P(2,12)*SF[5] + P(3,12)*SF[3] + P(0,12)*SPP[0] + P(13,12)*SPP[4] - P(14,12)*SPP[7] - P(15,12)*SPP[1];
|
||
|
|
nextP(7,12) = P(7,12) + P(4,12)*dt;
|
||
|
|
nextP(8,12) = P(8,12) + P(5,12)*dt;
|
||
|
|
nextP(9,12) = P(9,12) + P(6,12)*dt;
|
||
|
|
nextP(10,12) = P(10,12);
|
||
|
|
nextP(11,12) = P(11,12);
|
||
|
|
nextP(12,12) = P(12,12);
|
||
|
|
nextP(0,13) = P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10];
|
||
|
|
nextP(1,13) = P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)/2;
|
||
|
|
nextP(2,13) = P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)/2;
|
||
|
|
nextP(3,13) = P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)/2;
|
||
|
|
nextP(4,13) = P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9];
|
||
|
|
nextP(5,13) = P(5,13) + P(0,13)*SF[4] + P(2,13)*SF[3] + P(3,13)*SF[5] - P(1,13)*SPP[0] - P(13,13)*SPP[8] + P(14,13)*SPP[2] + P(15,13)*SPP[5];
|
||
|
|
nextP(6,13) = P(6,13) + P(1,13)*SF[4] - P(2,13)*SF[5] + P(3,13)*SF[3] + P(0,13)*SPP[0] + P(13,13)*SPP[4] - P(14,13)*SPP[7] - P(15,13)*SPP[1];
|
||
|
|
nextP(7,13) = P(7,13) + P(4,13)*dt;
|
||
|
|
nextP(8,13) = P(8,13) + P(5,13)*dt;
|
||
|
|
nextP(9,13) = P(9,13) + P(6,13)*dt;
|
||
|
|
nextP(10,13) = P(10,13);
|
||
|
|
nextP(11,13) = P(11,13);
|
||
|
|
nextP(12,13) = P(12,13);
|
||
|
|
nextP(13,13) = P(13,13);
|
||
|
|
nextP(0,14) = P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10];
|
||
|
|
nextP(1,14) = P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)/2;
|
||
|
|
nextP(2,14) = P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)/2;
|
||
|
|
nextP(3,14) = P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)/2;
|
||
|
|
nextP(4,14) = P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9];
|
||
|
|
nextP(5,14) = P(5,14) + P(0,14)*SF[4] + P(2,14)*SF[3] + P(3,14)*SF[5] - P(1,14)*SPP[0] - P(13,14)*SPP[8] + P(14,14)*SPP[2] + P(15,14)*SPP[5];
|
||
|
|
nextP(6,14) = P(6,14) + P(1,14)*SF[4] - P(2,14)*SF[5] + P(3,14)*SF[3] + P(0,14)*SPP[0] + P(13,14)*SPP[4] - P(14,14)*SPP[7] - P(15,14)*SPP[1];
|
||
|
|
nextP(7,14) = P(7,14) + P(4,14)*dt;
|
||
|
|
nextP(8,14) = P(8,14) + P(5,14)*dt;
|
||
|
|
nextP(9,14) = P(9,14) + P(6,14)*dt;
|
||
|
|
nextP(10,14) = P(10,14);
|
||
|
|
nextP(11,14) = P(11,14);
|
||
|
|
nextP(12,14) = P(12,14);
|
||
|
|
nextP(13,14) = P(13,14);
|
||
|
|
nextP(14,14) = P(14,14);
|
||
|
|
nextP(0,15) = P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10];
|
||
|
|
nextP(1,15) = P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)/2;
|
||
|
|
nextP(2,15) = P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)/2;
|
||
|
|
nextP(3,15) = P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)/2;
|
||
|
|
nextP(4,15) = P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9];
|
||
|
|
nextP(5,15) = P(5,15) + P(0,15)*SF[4] + P(2,15)*SF[3] + P(3,15)*SF[5] - P(1,15)*SPP[0] - P(13,15)*SPP[8] + P(14,15)*SPP[2] + P(15,15)*SPP[5];
|
||
|
|
nextP(6,15) = P(6,15) + P(1,15)*SF[4] - P(2,15)*SF[5] + P(3,15)*SF[3] + P(0,15)*SPP[0] + P(13,15)*SPP[4] - P(14,15)*SPP[7] - P(15,15)*SPP[1];
|
||
|
|
nextP(7,15) = P(7,15) + P(4,15)*dt;
|
||
|
|
nextP(8,15) = P(8,15) + P(5,15)*dt;
|
||
|
|
nextP(9,15) = P(9,15) + P(6,15)*dt;
|
||
|
|
nextP(10,15) = P(10,15);
|
||
|
|
nextP(11,15) = P(11,15);
|
||
|
|
nextP(12,15) = P(12,15);
|
||
|
|
nextP(13,15) = P(13,15);
|
||
|
|
nextP(14,15) = P(14,15);
|
||
|
|
nextP(15,15) = P(15,15);
|
||
|
|
nextP(0,16) = P(0,16) + P(1,16)*SF[9] + P(2,16)*SF[11] + P(3,16)*SF[10] + P(10,16)*SF[14] + P(11,16)*SF[15] + P(12,16)*SPP[10];
|
||
|
|
nextP(1,16) = P(1,16) + P(0,16)*SF[8] + P(2,16)*SF[7] + P(3,16)*SF[11] - P(12,16)*SF[15] + P(11,16)*SPP[10] - (P(10,16)*q0)/2;
|
||
|
|
nextP(2,16) = P(2,16) + P(0,16)*SF[6] + P(1,16)*SF[10] + P(3,16)*SF[8] + P(12,16)*SF[14] - P(10,16)*SPP[10] - (P(11,16)*q0)/2;
|
||
|
|
nextP(3,16) = P(3,16) + P(0,16)*SF[7] + P(1,16)*SF[6] + P(2,16)*SF[9] + P(10,16)*SF[15] - P(11,16)*SF[14] - (P(12,16)*q0)/2;
|
||
|
|
nextP(4,16) = P(4,16) + P(0,16)*SF[5] + P(1,16)*SF[3] - P(3,16)*SF[4] + P(2,16)*SPP[0] + P(13,16)*SPP[3] + P(14,16)*SPP[6] - P(15,16)*SPP[9];
|
||
|
|
nextP(5,16) = P(5,16) + P(0,16)*SF[4] + P(2,16)*SF[3] + P(3,16)*SF[5] - P(1,16)*SPP[0] - P(13,16)*SPP[8] + P(14,16)*SPP[2] + P(15,16)*SPP[5];
|
||
|
|
nextP(6,16) = P(6,16) + P(1,16)*SF[4] - P(2,16)*SF[5] + P(3,16)*SF[3] + P(0,16)*SPP[0] + P(13,16)*SPP[4] - P(14,16)*SPP[7] - P(15,16)*SPP[1];
|
||
|
|
nextP(7,16) = P(7,16) + P(4,16)*dt;
|
||
|
|
nextP(8,16) = P(8,16) + P(5,16)*dt;
|
||
|
|
nextP(9,16) = P(9,16) + P(6,16)*dt;
|
||
|
|
nextP(10,16) = P(10,16);
|
||
|
|
nextP(11,16) = P(11,16);
|
||
|
|
nextP(12,16) = P(12,16);
|
||
|
|
nextP(13,16) = P(13,16);
|
||
|
|
nextP(14,16) = P(14,16);
|
||
|
|
nextP(15,16) = P(15,16);
|
||
|
|
nextP(16,16) = P(16,16);
|
||
|
|
nextP(0,17) = P(0,17) + P(1,17)*SF[9] + P(2,17)*SF[11] + P(3,17)*SF[10] + P(10,17)*SF[14] + P(11,17)*SF[15] + P(12,17)*SPP[10];
|
||
|
|
nextP(1,17) = P(1,17) + P(0,17)*SF[8] + P(2,17)*SF[7] + P(3,17)*SF[11] - P(12,17)*SF[15] + P(11,17)*SPP[10] - (P(10,17)*q0)/2;
|
||
|
|
nextP(2,17) = P(2,17) + P(0,17)*SF[6] + P(1,17)*SF[10] + P(3,17)*SF[8] + P(12,17)*SF[14] - P(10,17)*SPP[10] - (P(11,17)*q0)/2;
|
||
|
|
nextP(3,17) = P(3,17) + P(0,17)*SF[7] + P(1,17)*SF[6] + P(2,17)*SF[9] + P(10,17)*SF[15] - P(11,17)*SF[14] - (P(12,17)*q0)/2;
|
||
|
|
nextP(4,17) = P(4,17) + P(0,17)*SF[5] + P(1,17)*SF[3] - P(3,17)*SF[4] + P(2,17)*SPP[0] + P(13,17)*SPP[3] + P(14,17)*SPP[6] - P(15,17)*SPP[9];
|
||
|
|
nextP(5,17) = P(5,17) + P(0,17)*SF[4] + P(2,17)*SF[3] + P(3,17)*SF[5] - P(1,17)*SPP[0] - P(13,17)*SPP[8] + P(14,17)*SPP[2] + P(15,17)*SPP[5];
|
||
|
|
nextP(6,17) = P(6,17) + P(1,17)*SF[4] - P(2,17)*SF[5] + P(3,17)*SF[3] + P(0,17)*SPP[0] + P(13,17)*SPP[4] - P(14,17)*SPP[7] - P(15,17)*SPP[1];
|
||
|
|
nextP(7,17) = P(7,17) + P(4,17)*dt;
|
||
|
|
nextP(8,17) = P(8,17) + P(5,17)*dt;
|
||
|
|
nextP(9,17) = P(9,17) + P(6,17)*dt;
|
||
|
|
nextP(10,17) = P(10,17);
|
||
|
|
nextP(11,17) = P(11,17);
|
||
|
|
nextP(12,17) = P(12,17);
|
||
|
|
nextP(13,17) = P(13,17);
|
||
|
|
nextP(14,17) = P(14,17);
|
||
|
|
nextP(15,17) = P(15,17);
|
||
|
|
nextP(16,17) = P(16,17);
|
||
|
|
nextP(17,17) = P(17,17);
|
||
|
|
nextP(0,18) = P(0,18) + P(1,18)*SF[9] + P(2,18)*SF[11] + P(3,18)*SF[10] + P(10,18)*SF[14] + P(11,18)*SF[15] + P(12,18)*SPP[10];
|
||
|
|
nextP(1,18) = P(1,18) + P(0,18)*SF[8] + P(2,18)*SF[7] + P(3,18)*SF[11] - P(12,18)*SF[15] + P(11,18)*SPP[10] - (P(10,18)*q0)/2;
|
||
|
|
nextP(2,18) = P(2,18) + P(0,18)*SF[6] + P(1,18)*SF[10] + P(3,18)*SF[8] + P(12,18)*SF[14] - P(10,18)*SPP[10] - (P(11,18)*q0)/2;
|
||
|
|
nextP(3,18) = P(3,18) + P(0,18)*SF[7] + P(1,18)*SF[6] + P(2,18)*SF[9] + P(10,18)*SF[15] - P(11,18)*SF[14] - (P(12,18)*q0)/2;
|
||
|
|
nextP(4,18) = P(4,18) + P(0,18)*SF[5] + P(1,18)*SF[3] - P(3,18)*SF[4] + P(2,18)*SPP[0] + P(13,18)*SPP[3] + P(14,18)*SPP[6] - P(15,18)*SPP[9];
|
||
|
|
nextP(5,18) = P(5,18) + P(0,18)*SF[4] + P(2,18)*SF[3] + P(3,18)*SF[5] - P(1,18)*SPP[0] - P(13,18)*SPP[8] + P(14,18)*SPP[2] + P(15,18)*SPP[5];
|
||
|
|
nextP(6,18) = P(6,18) + P(1,18)*SF[4] - P(2,18)*SF[5] + P(3,18)*SF[3] + P(0,18)*SPP[0] + P(13,18)*SPP[4] - P(14,18)*SPP[7] - P(15,18)*SPP[1];
|
||
|
|
nextP(7,18) = P(7,18) + P(4,18)*dt;
|
||
|
|
nextP(8,18) = P(8,18) + P(5,18)*dt;
|
||
|
|
nextP(9,18) = P(9,18) + P(6,18)*dt;
|
||
|
|
nextP(10,18) = P(10,18);
|
||
|
|
nextP(11,18) = P(11,18);
|
||
|
|
nextP(12,18) = P(12,18);
|
||
|
|
nextP(13,18) = P(13,18);
|
||
|
|
nextP(14,18) = P(14,18);
|
||
|
|
nextP(15,18) = P(15,18);
|
||
|
|
nextP(16,18) = P(16,18);
|
||
|
|
nextP(17,18) = P(17,18);
|
||
|
|
nextP(18,18) = P(18,18);
|
||
|
|
nextP(0,19) = P(0,19) + P(1,19)*SF[9] + P(2,19)*SF[11] + P(3,19)*SF[10] + P(10,19)*SF[14] + P(11,19)*SF[15] + P(12,19)*SPP[10];
|
||
|
|
nextP(1,19) = P(1,19) + P(0,19)*SF[8] + P(2,19)*SF[7] + P(3,19)*SF[11] - P(12,19)*SF[15] + P(11,19)*SPP[10] - (P(10,19)*q0)/2;
|
||
|
|
nextP(2,19) = P(2,19) + P(0,19)*SF[6] + P(1,19)*SF[10] + P(3,19)*SF[8] + P(12,19)*SF[14] - P(10,19)*SPP[10] - (P(11,19)*q0)/2;
|
||
|
|
nextP(3,19) = P(3,19) + P(0,19)*SF[7] + P(1,19)*SF[6] + P(2,19)*SF[9] + P(10,19)*SF[15] - P(11,19)*SF[14] - (P(12,19)*q0)/2;
|
||
|
|
nextP(4,19) = P(4,19) + P(0,19)*SF[5] + P(1,19)*SF[3] - P(3,19)*SF[4] + P(2,19)*SPP[0] + P(13,19)*SPP[3] + P(14,19)*SPP[6] - P(15,19)*SPP[9];
|
||
|
|
nextP(5,19) = P(5,19) + P(0,19)*SF[4] + P(2,19)*SF[3] + P(3,19)*SF[5] - P(1,19)*SPP[0] - P(13,19)*SPP[8] + P(14,19)*SPP[2] + P(15,19)*SPP[5];
|
||
|
|
nextP(6,19) = P(6,19) + P(1,19)*SF[4] - P(2,19)*SF[5] + P(3,19)*SF[3] + P(0,19)*SPP[0] + P(13,19)*SPP[4] - P(14,19)*SPP[7] - P(15,19)*SPP[1];
|
||
|
|
nextP(7,19) = P(7,19) + P(4,19)*dt;
|
||
|
|
nextP(8,19) = P(8,19) + P(5,19)*dt;
|
||
|
|
nextP(9,19) = P(9,19) + P(6,19)*dt;
|
||
|
|
nextP(10,19) = P(10,19);
|
||
|
|
nextP(11,19) = P(11,19);
|
||
|
|
nextP(12,19) = P(12,19);
|
||
|
|
nextP(13,19) = P(13,19);
|
||
|
|
nextP(14,19) = P(14,19);
|
||
|
|
nextP(15,19) = P(15,19);
|
||
|
|
nextP(16,19) = P(16,19);
|
||
|
|
nextP(17,19) = P(17,19);
|
||
|
|
nextP(18,19) = P(18,19);
|
||
|
|
nextP(19,19) = P(19,19);
|
||
|
|
nextP(0,20) = P(0,20) + P(1,20)*SF[9] + P(2,20)*SF[11] + P(3,20)*SF[10] + P(10,20)*SF[14] + P(11,20)*SF[15] + P(12,20)*SPP[10];
|
||
|
|
nextP(1,20) = P(1,20) + P(0,20)*SF[8] + P(2,20)*SF[7] + P(3,20)*SF[11] - P(12,20)*SF[15] + P(11,20)*SPP[10] - (P(10,20)*q0)/2;
|
||
|
|
nextP(2,20) = P(2,20) + P(0,20)*SF[6] + P(1,20)*SF[10] + P(3,20)*SF[8] + P(12,20)*SF[14] - P(10,20)*SPP[10] - (P(11,20)*q0)/2;
|
||
|
|
nextP(3,20) = P(3,20) + P(0,20)*SF[7] + P(1,20)*SF[6] + P(2,20)*SF[9] + P(10,20)*SF[15] - P(11,20)*SF[14] - (P(12,20)*q0)/2;
|
||
|
|
nextP(4,20) = P(4,20) + P(0,20)*SF[5] + P(1,20)*SF[3] - P(3,20)*SF[4] + P(2,20)*SPP[0] + P(13,20)*SPP[3] + P(14,20)*SPP[6] - P(15,20)*SPP[9];
|
||
|
|
nextP(5,20) = P(5,20) + P(0,20)*SF[4] + P(2,20)*SF[3] + P(3,20)*SF[5] - P(1,20)*SPP[0] - P(13,20)*SPP[8] + P(14,20)*SPP[2] + P(15,20)*SPP[5];
|
||
|
|
nextP(6,20) = P(6,20) + P(1,20)*SF[4] - P(2,20)*SF[5] + P(3,20)*SF[3] + P(0,20)*SPP[0] + P(13,20)*SPP[4] - P(14,20)*SPP[7] - P(15,20)*SPP[1];
|
||
|
|
nextP(7,20) = P(7,20) + P(4,20)*dt;
|
||
|
|
nextP(8,20) = P(8,20) + P(5,20)*dt;
|
||
|
|
nextP(9,20) = P(9,20) + P(6,20)*dt;
|
||
|
|
nextP(10,20) = P(10,20);
|
||
|
|
nextP(11,20) = P(11,20);
|
||
|
|
nextP(12,20) = P(12,20);
|
||
|
|
nextP(13,20) = P(13,20);
|
||
|
|
nextP(14,20) = P(14,20);
|
||
|
|
nextP(15,20) = P(15,20);
|
||
|
|
nextP(16,20) = P(16,20);
|
||
|
|
nextP(17,20) = P(17,20);
|
||
|
|
nextP(18,20) = P(18,20);
|
||
|
|
nextP(19,20) = P(19,20);
|
||
|
|
nextP(20,20) = P(20,20);
|
||
|
|
nextP(0,21) = P(0,21) + P(1,21)*SF[9] + P(2,21)*SF[11] + P(3,21)*SF[10] + P(10,21)*SF[14] + P(11,21)*SF[15] + P(12,21)*SPP[10];
|
||
|
|
nextP(1,21) = P(1,21) + P(0,21)*SF[8] + P(2,21)*SF[7] + P(3,21)*SF[11] - P(12,21)*SF[15] + P(11,21)*SPP[10] - (P(10,21)*q0)/2;
|
||
|
|
nextP(2,21) = P(2,21) + P(0,21)*SF[6] + P(1,21)*SF[10] + P(3,21)*SF[8] + P(12,21)*SF[14] - P(10,21)*SPP[10] - (P(11,21)*q0)/2;
|
||
|
|
nextP(3,21) = P(3,21) + P(0,21)*SF[7] + P(1,21)*SF[6] + P(2,21)*SF[9] + P(10,21)*SF[15] - P(11,21)*SF[14] - (P(12,21)*q0)/2;
|
||
|
|
nextP(4,21) = P(4,21) + P(0,21)*SF[5] + P(1,21)*SF[3] - P(3,21)*SF[4] + P(2,21)*SPP[0] + P(13,21)*SPP[3] + P(14,21)*SPP[6] - P(15,21)*SPP[9];
|
||
|
|
nextP(5,21) = P(5,21) + P(0,21)*SF[4] + P(2,21)*SF[3] + P(3,21)*SF[5] - P(1,21)*SPP[0] - P(13,21)*SPP[8] + P(14,21)*SPP[2] + P(15,21)*SPP[5];
|
||
|
|
nextP(6,21) = P(6,21) + P(1,21)*SF[4] - P(2,21)*SF[5] + P(3,21)*SF[3] + P(0,21)*SPP[0] + P(13,21)*SPP[4] - P(14,21)*SPP[7] - P(15,21)*SPP[1];
|
||
|
|
nextP(7,21) = P(7,21) + P(4,21)*dt;
|
||
|
|
nextP(8,21) = P(8,21) + P(5,21)*dt;
|
||
|
|
nextP(9,21) = P(9,21) + P(6,21)*dt;
|
||
|
|
nextP(10,21) = P(10,21);
|
||
|
|
nextP(11,21) = P(11,21);
|
||
|
|
nextP(12,21) = P(12,21);
|
||
|
|
nextP(13,21) = P(13,21);
|
||
|
|
nextP(14,21) = P(14,21);
|
||
|
|
nextP(15,21) = P(15,21);
|
||
|
|
nextP(16,21) = P(16,21);
|
||
|
|
nextP(17,21) = P(17,21);
|
||
|
|
nextP(18,21) = P(18,21);
|
||
|
|
nextP(19,21) = P(19,21);
|
||
|
|
nextP(20,21) = P(20,21);
|
||
|
|
nextP(21,21) = P(21,21);
|
||
|
|
nextP(0,22) = P(0,22) + P(1,22)*SF[9] + P(2,22)*SF[11] + P(3,22)*SF[10] + P(10,22)*SF[14] + P(11,22)*SF[15] + P(12,22)*SPP[10];
|
||
|
|
nextP(1,22) = P(1,22) + P(0,22)*SF[8] + P(2,22)*SF[7] + P(3,22)*SF[11] - P(12,22)*SF[15] + P(11,22)*SPP[10] - (P(10,22)*q0)/2;
|
||
|
|
nextP(2,22) = P(2,22) + P(0,22)*SF[6] + P(1,22)*SF[10] + P(3,22)*SF[8] + P(12,22)*SF[14] - P(10,22)*SPP[10] - (P(11,22)*q0)/2;
|
||
|
|
nextP(3,22) = P(3,22) + P(0,22)*SF[7] + P(1,22)*SF[6] + P(2,22)*SF[9] + P(10,22)*SF[15] - P(11,22)*SF[14] - (P(12,22)*q0)/2;
|
||
|
|
nextP(4,22) = P(4,22) + P(0,22)*SF[5] + P(1,22)*SF[3] - P(3,22)*SF[4] + P(2,22)*SPP[0] + P(13,22)*SPP[3] + P(14,22)*SPP[6] - P(15,22)*SPP[9];
|
||
|
|
nextP(5,22) = P(5,22) + P(0,22)*SF[4] + P(2,22)*SF[3] + P(3,22)*SF[5] - P(1,22)*SPP[0] - P(13,22)*SPP[8] + P(14,22)*SPP[2] + P(15,22)*SPP[5];
|
||
|
|
nextP(6,22) = P(6,22) + P(1,22)*SF[4] - P(2,22)*SF[5] + P(3,22)*SF[3] + P(0,22)*SPP[0] + P(13,22)*SPP[4] - P(14,22)*SPP[7] - P(15,22)*SPP[1];
|
||
|
|
nextP(7,22) = P(7,22) + P(4,22)*dt;
|
||
|
|
nextP(8,22) = P(8,22) + P(5,22)*dt;
|
||
|
|
nextP(9,22) = P(9,22) + P(6,22)*dt;
|
||
|
|
nextP(10,22) = P(10,22);
|
||
|
|
nextP(11,22) = P(11,22);
|
||
|
|
nextP(12,22) = P(12,22);
|
||
|
|
nextP(13,22) = P(13,22);
|
||
|
|
nextP(14,22) = P(14,22);
|
||
|
|
nextP(15,22) = P(15,22);
|
||
|
|
nextP(16,22) = P(16,22);
|
||
|
|
nextP(17,22) = P(17,22);
|
||
|
|
nextP(18,22) = P(18,22);
|
||
|
|
nextP(19,22) = P(19,22);
|
||
|
|
nextP(20,22) = P(20,22);
|
||
|
|
nextP(21,22) = P(21,22);
|
||
|
|
nextP(22,22) = P(22,22);
|
||
|
|
nextP(0,23) = P(0,23) + P(1,23)*SF[9] + P(2,23)*SF[11] + P(3,23)*SF[10] + P(10,23)*SF[14] + P(11,23)*SF[15] + P(12,23)*SPP[10];
|
||
|
|
nextP(1,23) = P(1,23) + P(0,23)*SF[8] + P(2,23)*SF[7] + P(3,23)*SF[11] - P(12,23)*SF[15] + P(11,23)*SPP[10] - (P(10,23)*q0)/2;
|
||
|
|
nextP(2,23) = P(2,23) + P(0,23)*SF[6] + P(1,23)*SF[10] + P(3,23)*SF[8] + P(12,23)*SF[14] - P(10,23)*SPP[10] - (P(11,23)*q0)/2;
|
||
|
|
nextP(3,23) = P(3,23) + P(0,23)*SF[7] + P(1,23)*SF[6] + P(2,23)*SF[9] + P(10,23)*SF[15] - P(11,23)*SF[14] - (P(12,23)*q0)/2;
|
||
|
|
nextP(4,23) = P(4,23) + P(0,23)*SF[5] + P(1,23)*SF[3] - P(3,23)*SF[4] + P(2,23)*SPP[0] + P(13,23)*SPP[3] + P(14,23)*SPP[6] - P(15,23)*SPP[9];
|
||
|
|
nextP(5,23) = P(5,23) + P(0,23)*SF[4] + P(2,23)*SF[3] + P(3,23)*SF[5] - P(1,23)*SPP[0] - P(13,23)*SPP[8] + P(14,23)*SPP[2] + P(15,23)*SPP[5];
|
||
|
|
nextP(6,23) = P(6,23) + P(1,23)*SF[4] - P(2,23)*SF[5] + P(3,23)*SF[3] + P(0,23)*SPP[0] + P(13,23)*SPP[4] - P(14,23)*SPP[7] - P(15,23)*SPP[1];
|
||
|
|
nextP(7,23) = P(7,23) + P(4,23)*dt;
|
||
|
|
nextP(8,23) = P(8,23) + P(5,23)*dt;
|
||
|
|
nextP(9,23) = P(9,23) + P(6,23)*dt;
|
||
|
|
nextP(10,23) = P(10,23);
|
||
|
|
nextP(11,23) = P(11,23);
|
||
|
|
nextP(12,23) = P(12,23);
|
||
|
|
nextP(13,23) = P(13,23);
|
||
|
|
nextP(14,23) = P(14,23);
|
||
|
|
nextP(15,23) = P(15,23);
|
||
|
|
nextP(16,23) = P(16,23);
|
||
|
|
nextP(17,23) = P(17,23);
|
||
|
|
nextP(18,23) = P(18,23);
|
||
|
|
nextP(19,23) = P(19,23);
|
||
|
|
nextP(20,23) = P(20,23);
|
||
|
|
nextP(21,23) = P(21,23);
|
||
|
|
nextP(22,23) = P(22,23);
|
||
|
|
nextP(23,23) = P(23,23);
|
||
|
|
float SH_TAS[3][1];
|
||
|
|
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||
|
|
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
|
||
|
|
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
|
||
|
|
float H_TAS[1][24];
|
||
|
|
H_TAS[0][4] = SH_TAS[2];
|
||
|
|
H_TAS[0][5] = SH_TAS[1];
|
||
|
|
H_TAS[0][6] = vd*SH_TAS[0];
|
||
|
|
H_TAS[0][22] = -SH_TAS[2];
|
||
|
|
H_TAS[0][23] = -SH_TAS[1];
|
||
|
|
float SK_TAS[2][1];
|
||
|
|
SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
|
||
|
|
SK_TAS[1] = SH_TAS[1];
|
||
|
|
float Kfusion[24][1];
|
||
|
|
float Kfusion[1][1];
|
||
|
|
Kfusion[0] = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[1] = SK_TAS[0]*(P(1,4)*SH_TAS[2] - P(1,22)*SH_TAS[2] + P(1,5)*SK_TAS[1] - P(1,23)*SK_TAS[1] + P(1,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[2] = SK_TAS[0]*(P(2,4)*SH_TAS[2] - P(2,22)*SH_TAS[2] + P(2,5)*SK_TAS[1] - P(2,23)*SK_TAS[1] + P(2,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[3] = SK_TAS[0]*(P(3,4)*SH_TAS[2] - P(3,22)*SH_TAS[2] + P(3,5)*SK_TAS[1] - P(3,23)*SK_TAS[1] + P(3,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[4] = SK_TAS[0]*(P(4,4)*SH_TAS[2] - P(4,22)*SH_TAS[2] + P(4,5)*SK_TAS[1] - P(4,23)*SK_TAS[1] + P(4,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[5] = SK_TAS[0]*(P(5,4)*SH_TAS[2] - P(5,22)*SH_TAS[2] + P(5,5)*SK_TAS[1] - P(5,23)*SK_TAS[1] + P(5,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[6] = SK_TAS[0]*(P(6,4)*SH_TAS[2] - P(6,22)*SH_TAS[2] + P(6,5)*SK_TAS[1] - P(6,23)*SK_TAS[1] + P(6,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[7] = SK_TAS[0]*(P(7,4)*SH_TAS[2] - P(7,22)*SH_TAS[2] + P(7,5)*SK_TAS[1] - P(7,23)*SK_TAS[1] + P(7,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[8] = SK_TAS[0]*(P(8,4)*SH_TAS[2] - P(8,22)*SH_TAS[2] + P(8,5)*SK_TAS[1] - P(8,23)*SK_TAS[1] + P(8,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[9] = SK_TAS[0]*(P(9,4)*SH_TAS[2] - P(9,22)*SH_TAS[2] + P(9,5)*SK_TAS[1] - P(9,23)*SK_TAS[1] + P(9,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[10] = SK_TAS[0]*(P(10,4)*SH_TAS[2] - P(10,22)*SH_TAS[2] + P(10,5)*SK_TAS[1] - P(10,23)*SK_TAS[1] + P(10,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[11] = SK_TAS[0]*(P(11,4)*SH_TAS[2] - P(11,22)*SH_TAS[2] + P(11,5)*SK_TAS[1] - P(11,23)*SK_TAS[1] + P(11,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[12] = SK_TAS[0]*(P(12,4)*SH_TAS[2] - P(12,22)*SH_TAS[2] + P(12,5)*SK_TAS[1] - P(12,23)*SK_TAS[1] + P(12,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[13] = SK_TAS[0]*(P(13,4)*SH_TAS[2] - P(13,22)*SH_TAS[2] + P(13,5)*SK_TAS[1] - P(13,23)*SK_TAS[1] + P(13,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[14] = SK_TAS[0]*(P(14,4)*SH_TAS[2] - P(14,22)*SH_TAS[2] + P(14,5)*SK_TAS[1] - P(14,23)*SK_TAS[1] + P(14,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[15] = SK_TAS[0]*(P(15,4)*SH_TAS[2] - P(15,22)*SH_TAS[2] + P(15,5)*SK_TAS[1] - P(15,23)*SK_TAS[1] + P(15,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[16] = SK_TAS[0]*(P(16,4)*SH_TAS[2] - P(16,22)*SH_TAS[2] + P(16,5)*SK_TAS[1] - P(16,23)*SK_TAS[1] + P(16,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[17] = SK_TAS[0]*(P(17,4)*SH_TAS[2] - P(17,22)*SH_TAS[2] + P(17,5)*SK_TAS[1] - P(17,23)*SK_TAS[1] + P(17,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[18] = SK_TAS[0]*(P(18,4)*SH_TAS[2] - P(18,22)*SH_TAS[2] + P(18,5)*SK_TAS[1] - P(18,23)*SK_TAS[1] + P(18,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[19] = SK_TAS[0]*(P(19,4)*SH_TAS[2] - P(19,22)*SH_TAS[2] + P(19,5)*SK_TAS[1] - P(19,23)*SK_TAS[1] + P(19,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[20] = SK_TAS[0]*(P(20,4)*SH_TAS[2] - P(20,22)*SH_TAS[2] + P(20,5)*SK_TAS[1] - P(20,23)*SK_TAS[1] + P(20,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[21] = SK_TAS[0]*(P(21,4)*SH_TAS[2] - P(21,22)*SH_TAS[2] + P(21,5)*SK_TAS[1] - P(21,23)*SK_TAS[1] + P(21,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[22] = SK_TAS[0]*(P(22,4)*SH_TAS[2] - P(22,22)*SH_TAS[2] + P(22,5)*SK_TAS[1] - P(22,23)*SK_TAS[1] + P(22,6)*vd*SH_TAS[0]);
|
||
|
|
Kfusion[23] = SK_TAS[0]*(P(23,4)*SH_TAS[2] - P(23,22)*SH_TAS[2] + P(23,5)*SK_TAS[1] - P(23,23)*SK_TAS[1] + P(23,6)*vd*SH_TAS[0]);
|
||
|
|
float SH_BETA[13][1];
|
||
|
|
SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
|
||
|
|
SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2);
|
||
|
|
SH_BETA[2] = vn - vwn;
|
||
|
|
SH_BETA[3] = ve - vwe;
|
||
|
|
SH_BETA[4] = 1/sq(SH_BETA[0]);
|
||
|
|
SH_BETA[5] = 1/SH_BETA[0];
|
||
|
|
SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
|
||
|
|
SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||
|
|
SH_BETA[8] = 2*q0*SH_BETA[3] - 2*q3*SH_BETA[2] + 2*q1*vd;
|
||
|
|
SH_BETA[9] = 2*q0*SH_BETA[2] + 2*q3*SH_BETA[3] - 2*q2*vd;
|
||
|
|
SH_BETA[10] = 2*q2*SH_BETA[2] - 2*q1*SH_BETA[3] + 2*q0*vd;
|
||
|
|
SH_BETA[11] = 2*q1*SH_BETA[2] + 2*q2*SH_BETA[3] + 2*q3*vd;
|
||
|
|
SH_BETA[12] = 2*q0*q3;
|
||
|
|
float H_BETA[1][24];
|
||
|
|
H_BETA[0][0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9];
|
||
|
|
H_BETA[0][1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11];
|
||
|
|
H_BETA[0][2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
|
||
|
|
H_BETA[0][3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
|
||
|
|
H_BETA[0][4] = - SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
|
||
|
|
H_BETA[0][5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2);
|
||
|
|
H_BETA[0][6] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3);
|
||
|
|
H_BETA[0][22] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
|
||
|
|
H_BETA[0][23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6];
|
||
|
|
float SK_BETA[8][1];
|
||
|
|
SK_BETA[0] = 1/(R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P(22,4)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,4)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,4)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,4)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,4)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,4)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,4)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,4)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,4)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P(22,22)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,22)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,22)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,22)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,22)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,22)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,22)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,22)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,22)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P(22,5)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,5)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,5)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,5)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,5)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,5)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,5)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,5)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,5)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P(22,23)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,23)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,23)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,23)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,23)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,23)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,23)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,23)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,23)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P(22,0)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,0)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,0)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,0)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,0)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,0)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,0)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,0)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,0)*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P(22,1)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,1)*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,1)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P(23,1)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P(0,1)*(SH_BETA[5]*SH
|
||
|
|
SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
|
||
|
|
SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2);
|
||
|
|
SK_BETA[3] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3);
|
||
|
|
SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11];
|
||
|
|
SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9];
|
||
|
|
SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
|
||
|
|
SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
|
||
|
|
float Kfusion[24][1];
|
||
|
|
float Kfusion[1][1];
|
||
|
|
Kfusion[0] = SK_BETA[0]*(P(0,0)*SK_BETA[5] + P(0,1)*SK_BETA[4] - P(0,4)*SK_BETA[1] + P(0,5)*SK_BETA[2] + P(0,2)*SK_BETA[6] + P(0,6)*SK_BETA[3] - P(0,3)*SK_BETA[7] + P(0,22)*SK_BETA[1] - P(0,23)*SK_BETA[2]);
|
||
|
|
Kfusion[1] = SK_BETA[0]*(P(1,0)*SK_BETA[5] + P(1,1)*SK_BETA[4] - P(1,4)*SK_BETA[1] + P(1,5)*SK_BETA[2] + P(1,2)*SK_BETA[6] + P(1,6)*SK_BETA[3] - P(1,3)*SK_BETA[7] + P(1,22)*SK_BETA[1] - P(1,23)*SK_BETA[2]);
|
||
|
|
Kfusion[2] = SK_BETA[0]*(P(2,0)*SK_BETA[5] + P(2,1)*SK_BETA[4] - P(2,4)*SK_BETA[1] + P(2,5)*SK_BETA[2] + P(2,2)*SK_BETA[6] + P(2,6)*SK_BETA[3] - P(2,3)*SK_BETA[7] + P(2,22)*SK_BETA[1] - P(2,23)*SK_BETA[2]);
|
||
|
|
Kfusion[3] = SK_BETA[0]*(P(3,0)*SK_BETA[5] + P(3,1)*SK_BETA[4] - P(3,4)*SK_BETA[1] + P(3,5)*SK_BETA[2] + P(3,2)*SK_BETA[6] + P(3,6)*SK_BETA[3] - P(3,3)*SK_BETA[7] + P(3,22)*SK_BETA[1] - P(3,23)*SK_BETA[2]);
|
||
|
|
Kfusion[4] = SK_BETA[0]*(P(4,0)*SK_BETA[5] + P(4,1)*SK_BETA[4] - P(4,4)*SK_BETA[1] + P(4,5)*SK_BETA[2] + P(4,2)*SK_BETA[6] + P(4,6)*SK_BETA[3] - P(4,3)*SK_BETA[7] + P(4,22)*SK_BETA[1] - P(4,23)*SK_BETA[2]);
|
||
|
|
Kfusion[5] = SK_BETA[0]*(P(5,0)*SK_BETA[5] + P(5,1)*SK_BETA[4] - P(5,4)*SK_BETA[1] + P(5,5)*SK_BETA[2] + P(5,2)*SK_BETA[6] + P(5,6)*SK_BETA[3] - P(5,3)*SK_BETA[7] + P(5,22)*SK_BETA[1] - P(5,23)*SK_BETA[2]);
|
||
|
|
Kfusion[6] = SK_BETA[0]*(P(6,0)*SK_BETA[5] + P(6,1)*SK_BETA[4] - P(6,4)*SK_BETA[1] + P(6,5)*SK_BETA[2] + P(6,2)*SK_BETA[6] + P(6,6)*SK_BETA[3] - P(6,3)*SK_BETA[7] + P(6,22)*SK_BETA[1] - P(6,23)*SK_BETA[2]);
|
||
|
|
Kfusion[7] = SK_BETA[0]*(P(7,0)*SK_BETA[5] + P(7,1)*SK_BETA[4] - P(7,4)*SK_BETA[1] + P(7,5)*SK_BETA[2] + P(7,2)*SK_BETA[6] + P(7,6)*SK_BETA[3] - P(7,3)*SK_BETA[7] + P(7,22)*SK_BETA[1] - P(7,23)*SK_BETA[2]);
|
||
|
|
Kfusion[8] = SK_BETA[0]*(P(8,0)*SK_BETA[5] + P(8,1)*SK_BETA[4] - P(8,4)*SK_BETA[1] + P(8,5)*SK_BETA[2] + P(8,2)*SK_BETA[6] + P(8,6)*SK_BETA[3] - P(8,3)*SK_BETA[7] + P(8,22)*SK_BETA[1] - P(8,23)*SK_BETA[2]);
|
||
|
|
Kfusion[9] = SK_BETA[0]*(P(9,0)*SK_BETA[5] + P(9,1)*SK_BETA[4] - P(9,4)*SK_BETA[1] + P(9,5)*SK_BETA[2] + P(9,2)*SK_BETA[6] + P(9,6)*SK_BETA[3] - P(9,3)*SK_BETA[7] + P(9,22)*SK_BETA[1] - P(9,23)*SK_BETA[2]);
|
||
|
|
Kfusion[10] = SK_BETA[0]*(P(10,0)*SK_BETA[5] + P(10,1)*SK_BETA[4] - P(10,4)*SK_BETA[1] + P(10,5)*SK_BETA[2] + P(10,2)*SK_BETA[6] + P(10,6)*SK_BETA[3] - P(10,3)*SK_BETA[7] + P(10,22)*SK_BETA[1] - P(10,23)*SK_BETA[2]);
|
||
|
|
Kfusion[11] = SK_BETA[0]*(P(11,0)*SK_BETA[5] + P(11,1)*SK_BETA[4] - P(11,4)*SK_BETA[1] + P(11,5)*SK_BETA[2] + P(11,2)*SK_BETA[6] + P(11,6)*SK_BETA[3] - P(11,3)*SK_BETA[7] + P(11,22)*SK_BETA[1] - P(11,23)*SK_BETA[2]);
|
||
|
|
Kfusion[12] = SK_BETA[0]*(P(12,0)*SK_BETA[5] + P(12,1)*SK_BETA[4] - P(12,4)*SK_BETA[1] + P(12,5)*SK_BETA[2] + P(12,2)*SK_BETA[6] + P(12,6)*SK_BETA[3] - P(12,3)*SK_BETA[7] + P(12,22)*SK_BETA[1] - P(12,23)*SK_BETA[2]);
|
||
|
|
Kfusion[13] = SK_BETA[0]*(P(13,0)*SK_BETA[5] + P(13,1)*SK_BETA[4] - P(13,4)*SK_BETA[1] + P(13,5)*SK_BETA[2] + P(13,2)*SK_BETA[6] + P(13,6)*SK_BETA[3] - P(13,3)*SK_BETA[7] + P(13,22)*SK_BETA[1] - P(13,23)*SK_BETA[2]);
|
||
|
|
Kfusion[14] = SK_BETA[0]*(P(14,0)*SK_BETA[5] + P(14,1)*SK_BETA[4] - P(14,4)*SK_BETA[1] + P(14,5)*SK_BETA[2] + P(14,2)*SK_BETA[6] + P(14,6)*SK_BETA[3] - P(14,3)*SK_BETA[7] + P(14,22)*SK_BETA[1] - P(14,23)*SK_BETA[2]);
|
||
|
|
Kfusion[15] = SK_BETA[0]*(P(15,0)*SK_BETA[5] + P(15,1)*SK_BETA[4] - P(15,4)*SK_BETA[1] + P(15,5)*SK_BETA[2] + P(15,2)*SK_BETA[6] + P(15,6)*SK_BETA[3] - P(15,3)*SK_BETA[7] + P(15,22)*SK_BETA[1] - P(15,23)*SK_BETA[2]);
|
||
|
|
Kfusion[16] = SK_BETA[0]*(P(16,0)*SK_BETA[5] + P(16,1)*SK_BETA[4] - P(16,4)*SK_BETA[1] + P(16,5)*SK_BETA[2] + P(16,2)*SK_BETA[6] + P(16,6)*SK_BETA[3] - P(16,3)*SK_BETA[7] + P(16,22)*SK_BETA[1] - P(16,23)*SK_BETA[2]);
|
||
|
|
Kfusion[17] = SK_BETA[0]*(P(17,0)*SK_BETA[5] + P(17,1)*SK_BETA[4] - P(17,4)*SK_BETA[1] + P(17,5)*SK_BETA[2] + P(17,2)*SK_BETA[6] + P(17,6)*SK_BETA[3] - P(17,3)*SK_BETA[7] + P(17,22)*SK_BETA[1] - P(17,23)*SK_BETA[2]);
|
||
|
|
Kfusion[18] = SK_BETA[0]*(P(18,0)*SK_BETA[5] + P(18,1)*SK_BETA[4] - P(18,4)*SK_BETA[1] + P(18,5)*SK_BETA[2] + P(18,2)*SK_BETA[6] + P(18,6)*SK_BETA[3] - P(18,3)*SK_BETA[7] + P(18,22)*SK_BETA[1] - P(18,23)*SK_BETA[2]);
|
||
|
|
Kfusion[19] = SK_BETA[0]*(P(19,0)*SK_BETA[5] + P(19,1)*SK_BETA[4] - P(19,4)*SK_BETA[1] + P(19,5)*SK_BETA[2] + P(19,2)*SK_BETA[6] + P(19,6)*SK_BETA[3] - P(19,3)*SK_BETA[7] + P(19,22)*SK_BETA[1] - P(19,23)*SK_BETA[2]);
|
||
|
|
Kfusion[20] = SK_BETA[0]*(P(20,0)*SK_BETA[5] + P(20,1)*SK_BETA[4] - P(20,4)*SK_BETA[1] + P(20,5)*SK_BETA[2] + P(20,2)*SK_BETA[6] + P(20,6)*SK_BETA[3] - P(20,3)*SK_BETA[7] + P(20,22)*SK_BETA[1] - P(20,23)*SK_BETA[2]);
|
||
|
|
Kfusion[21] = SK_BETA[0]*(P(21,0)*SK_BETA[5] + P(21,1)*SK_BETA[4] - P(21,4)*SK_BETA[1] + P(21,5)*SK_BETA[2] + P(21,2)*SK_BETA[6] + P(21,6)*SK_BETA[3] - P(21,3)*SK_BETA[7] + P(21,22)*SK_BETA[1] - P(21,23)*SK_BETA[2]);
|
||
|
|
Kfusion[22] = SK_BETA[0]*(P(22,0)*SK_BETA[5] + P(22,1)*SK_BETA[4] - P(22,4)*SK_BETA[1] + P(22,5)*SK_BETA[2] + P(22,2)*SK_BETA[6] + P(22,6)*SK_BETA[3] - P(22,3)*SK_BETA[7] + P(22,22)*SK_BETA[1] - P(22,23)*SK_BETA[2]);
|
||
|
|
Kfusion[23] = SK_BETA[0]*(P(23,0)*SK_BETA[5] + P(23,1)*SK_BETA[4] - P(23,4)*SK_BETA[1] + P(23,5)*SK_BETA[2] + P(23,2)*SK_BETA[6] + P(23,6)*SK_BETA[3] - P(23,3)*SK_BETA[7] + P(23,22)*SK_BETA[1] - P(23,23)*SK_BETA[2]);
|
||
|
|
float SH_MAG[9][1];
|
||
|
|
SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
|
||
|
|
SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
|
||
|
|
SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
|
||
|
|
SH_MAG[3] = sq(q3);
|
||
|
|
SH_MAG[4] = sq(q2);
|
||
|
|
SH_MAG[5] = sq(q1);
|
||
|
|
SH_MAG[6] = sq(q0);
|
||
|
|
SH_MAG[7] = 2*magN*q0;
|
||
|
|
SH_MAG[8] = 2*magE*q3;
|
||
|
|
float H_MAG[1][24];
|
||
|
|
H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||
|
|
H_MAG[1] = SH_MAG[0];
|
||
|
|
H_MAG[2] = -SH_MAG[1];
|
||
|
|
H_MAG[3] = SH_MAG[2];
|
||
|
|
H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
|
||
|
|
H_MAG[17] = 2*q0*q3 + 2*q1*q2;
|
||
|
|
H_MAG[18] = 2*q1*q3 - 2*q0*q2;
|
||
|
|
H_MAG[19] = 1;
|
||
|
|
float SK_MX[5][1];
|
||
|
|
SK_MX[0] = 1/(P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2*q0*q3 + 2*q1*q2) - P(18,17)*(2*q0*q2 - 2*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2*q0*q3 + 2*q1*q2) - P(18,18)*(2*q0*q2 - 2*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2*q0*q3 + 2*q1*q2) - P(18,0)*(2*q0*q2 - 2*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P(17,19)*(2*q0*q3 + 2*q1*q2) - P(18,19)*(2*q0*q2 - 2*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2*q0*q3 + 2*q1*q2) - P(18,1)*(2*q0*q2 - 2*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2*q0*q3 + 2*q1*q2) - P(18,2)*(2*q0*q2 - 2*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2*q0*q3 + 2*q1*q2) - P(18,3)*(2*q0*q2 - 2*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2*q0*q3 + 2*q1*q2) - P(18,16)*(2*q0*q2 - 2*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||
|
|
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
|
||
|
|
SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||
|
|
SK_MX[3] = 2*q0*q2 - 2*q1*q3;
|
||
|
|
SK_MX[4] = 2*q0*q3 + 2*q1*q2;
|
||
|
|
float Kfusion[24][1];
|
||
|
|
float Kfusion[1][1];
|
||
|
|
Kfusion[0] = SK_MX[0]*(P(0,19) + P(0,1)*SH_MAG[0] - P(0,2)*SH_MAG[1] + P(0,3)*SH_MAG[2] + P(0,0)*SK_MX[2] - P(0,16)*SK_MX[1] + P(0,17)*SK_MX[4] - P(0,18)*SK_MX[3]);
|
||
|
|
Kfusion[1] = SK_MX[0]*(P(1,19) + P(1,1)*SH_MAG[0] - P(1,2)*SH_MAG[1] + P(1,3)*SH_MAG[2] + P(1,0)*SK_MX[2] - P(1,16)*SK_MX[1] + P(1,17)*SK_MX[4] - P(1,18)*SK_MX[3]);
|
||
|
|
Kfusion[2] = SK_MX[0]*(P(2,19) + P(2,1)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(2,3)*SH_MAG[2] + P(2,0)*SK_MX[2] - P(2,16)*SK_MX[1] + P(2,17)*SK_MX[4] - P(2,18)*SK_MX[3]);
|
||
|
|
Kfusion[3] = SK_MX[0]*(P(3,19) + P(3,1)*SH_MAG[0] - P(3,2)*SH_MAG[1] + P(3,3)*SH_MAG[2] + P(3,0)*SK_MX[2] - P(3,16)*SK_MX[1] + P(3,17)*SK_MX[4] - P(3,18)*SK_MX[3]);
|
||
|
|
Kfusion[4] = SK_MX[0]*(P(4,19) + P(4,1)*SH_MAG[0] - P(4,2)*SH_MAG[1] + P(4,3)*SH_MAG[2] + P(4,0)*SK_MX[2] - P(4,16)*SK_MX[1] + P(4,17)*SK_MX[4] - P(4,18)*SK_MX[3]);
|
||
|
|
Kfusion[5] = SK_MX[0]*(P(5,19) + P(5,1)*SH_MAG[0] - P(5,2)*SH_MAG[1] + P(5,3)*SH_MAG[2] + P(5,0)*SK_MX[2] - P(5,16)*SK_MX[1] + P(5,17)*SK_MX[4] - P(5,18)*SK_MX[3]);
|
||
|
|
Kfusion[6] = SK_MX[0]*(P(6,19) + P(6,1)*SH_MAG[0] - P(6,2)*SH_MAG[1] + P(6,3)*SH_MAG[2] + P(6,0)*SK_MX[2] - P(6,16)*SK_MX[1] + P(6,17)*SK_MX[4] - P(6,18)*SK_MX[3]);
|
||
|
|
Kfusion[7] = SK_MX[0]*(P(7,19) + P(7,1)*SH_MAG[0] - P(7,2)*SH_MAG[1] + P(7,3)*SH_MAG[2] + P(7,0)*SK_MX[2] - P(7,16)*SK_MX[1] + P(7,17)*SK_MX[4] - P(7,18)*SK_MX[3]);
|
||
|
|
Kfusion[8] = SK_MX[0]*(P(8,19) + P(8,1)*SH_MAG[0] - P(8,2)*SH_MAG[1] + P(8,3)*SH_MAG[2] + P(8,0)*SK_MX[2] - P(8,16)*SK_MX[1] + P(8,17)*SK_MX[4] - P(8,18)*SK_MX[3]);
|
||
|
|
Kfusion[9] = SK_MX[0]*(P(9,19) + P(9,1)*SH_MAG[0] - P(9,2)*SH_MAG[1] + P(9,3)*SH_MAG[2] + P(9,0)*SK_MX[2] - P(9,16)*SK_MX[1] + P(9,17)*SK_MX[4] - P(9,18)*SK_MX[3]);
|
||
|
|
Kfusion[10] = SK_MX[0]*(P(10,19) + P(10,1)*SH_MAG[0] - P(10,2)*SH_MAG[1] + P(10,3)*SH_MAG[2] + P(10,0)*SK_MX[2] - P(10,16)*SK_MX[1] + P(10,17)*SK_MX[4] - P(10,18)*SK_MX[3]);
|
||
|
|
Kfusion[11] = SK_MX[0]*(P(11,19) + P(11,1)*SH_MAG[0] - P(11,2)*SH_MAG[1] + P(11,3)*SH_MAG[2] + P(11,0)*SK_MX[2] - P(11,16)*SK_MX[1] + P(11,17)*SK_MX[4] - P(11,18)*SK_MX[3]);
|
||
|
|
Kfusion[12] = SK_MX[0]*(P(12,19) + P(12,1)*SH_MAG[0] - P(12,2)*SH_MAG[1] + P(12,3)*SH_MAG[2] + P(12,0)*SK_MX[2] - P(12,16)*SK_MX[1] + P(12,17)*SK_MX[4] - P(12,18)*SK_MX[3]);
|
||
|
|
Kfusion[13] = SK_MX[0]*(P(13,19) + P(13,1)*SH_MAG[0] - P(13,2)*SH_MAG[1] + P(13,3)*SH_MAG[2] + P(13,0)*SK_MX[2] - P(13,16)*SK_MX[1] + P(13,17)*SK_MX[4] - P(13,18)*SK_MX[3]);
|
||
|
|
Kfusion[14] = SK_MX[0]*(P(14,19) + P(14,1)*SH_MAG[0] - P(14,2)*SH_MAG[1] + P(14,3)*SH_MAG[2] + P(14,0)*SK_MX[2] - P(14,16)*SK_MX[1] + P(14,17)*SK_MX[4] - P(14,18)*SK_MX[3]);
|
||
|
|
Kfusion[15] = SK_MX[0]*(P(15,19) + P(15,1)*SH_MAG[0] - P(15,2)*SH_MAG[1] + P(15,3)*SH_MAG[2] + P(15,0)*SK_MX[2] - P(15,16)*SK_MX[1] + P(15,17)*SK_MX[4] - P(15,18)*SK_MX[3]);
|
||
|
|
Kfusion[16] = SK_MX[0]*(P(16,19) + P(16,1)*SH_MAG[0] - P(16,2)*SH_MAG[1] + P(16,3)*SH_MAG[2] + P(16,0)*SK_MX[2] - P(16,16)*SK_MX[1] + P(16,17)*SK_MX[4] - P(16,18)*SK_MX[3]);
|
||
|
|
Kfusion[17] = SK_MX[0]*(P(17,19) + P(17,1)*SH_MAG[0] - P(17,2)*SH_MAG[1] + P(17,3)*SH_MAG[2] + P(17,0)*SK_MX[2] - P(17,16)*SK_MX[1] + P(17,17)*SK_MX[4] - P(17,18)*SK_MX[3]);
|
||
|
|
Kfusion[18] = SK_MX[0]*(P(18,19) + P(18,1)*SH_MAG[0] - P(18,2)*SH_MAG[1] + P(18,3)*SH_MAG[2] + P(18,0)*SK_MX[2] - P(18,16)*SK_MX[1] + P(18,17)*SK_MX[4] - P(18,18)*SK_MX[3]);
|
||
|
|
Kfusion[19] = SK_MX[0]*(P(19,19) + P(19,1)*SH_MAG[0] - P(19,2)*SH_MAG[1] + P(19,3)*SH_MAG[2] + P(19,0)*SK_MX[2] - P(19,16)*SK_MX[1] + P(19,17)*SK_MX[4] - P(19,18)*SK_MX[3]);
|
||
|
|
Kfusion[20] = SK_MX[0]*(P(20,19) + P(20,1)*SH_MAG[0] - P(20,2)*SH_MAG[1] + P(20,3)*SH_MAG[2] + P(20,0)*SK_MX[2] - P(20,16)*SK_MX[1] + P(20,17)*SK_MX[4] - P(20,18)*SK_MX[3]);
|
||
|
|
Kfusion[21] = SK_MX[0]*(P(21,19) + P(21,1)*SH_MAG[0] - P(21,2)*SH_MAG[1] + P(21,3)*SH_MAG[2] + P(21,0)*SK_MX[2] - P(21,16)*SK_MX[1] + P(21,17)*SK_MX[4] - P(21,18)*SK_MX[3]);
|
||
|
|
Kfusion[22] = SK_MX[0]*(P(22,19) + P(22,1)*SH_MAG[0] - P(22,2)*SH_MAG[1] + P(22,3)*SH_MAG[2] + P(22,0)*SK_MX[2] - P(22,16)*SK_MX[1] + P(22,17)*SK_MX[4] - P(22,18)*SK_MX[3]);
|
||
|
|
Kfusion[23] = SK_MX[0]*(P(23,19) + P(23,1)*SH_MAG[0] - P(23,2)*SH_MAG[1] + P(23,3)*SH_MAG[2] + P(23,0)*SK_MX[2] - P(23,16)*SK_MX[1] + P(23,17)*SK_MX[4] - P(23,18)*SK_MX[3]);
|
||
|
|
float H_MAG[1][24];
|
||
|
|
H_MAG[0] = SH_MAG[2];
|
||
|
|
H_MAG[1] = SH_MAG[1];
|
||
|
|
H_MAG[2] = SH_MAG[0];
|
||
|
|
H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
|
||
|
|
H_MAG[16] = 2*q1*q2 - 2*q0*q3;
|
||
|
|
H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
|
||
|
|
H_MAG[18] = 2*q0*q1 + 2*q2*q3;
|
||
|
|
H_MAG[20] = 1;
|
||
|
|
float SK_MY[5][1];
|
||
|
|
SK_MY[0] = 1/(P(20,20) + R_MAG + P(0,20)*SH_MAG[2] + P(1,20)*SH_MAG[1] + P(2,20)*SH_MAG[0] - P(17,20)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P(20,16) + P(0,16)*SH_MAG[2] + P(1,16)*SH_MAG[1] + P(2,16)*SH_MAG[0] - P(17,16)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,16)*(2*q0*q3 - 2*q1*q2) + P(18,16)*(2*q0*q1 + 2*q2*q3) - P(3,16)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P(20,18) + P(0,18)*SH_MAG[2] + P(1,18)*SH_MAG[1] + P(2,18)*SH_MAG[0] - P(17,18)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,18)*(2*q0*q3 - 2*q1*q2) + P(18,18)*(2*q0*q1 + 2*q2*q3) - P(3,18)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P(20,3) + P(0,3)*SH_MAG[2] + P(1,3)*SH_MAG[1] + P(2,3)*SH_MAG[0] - P(17,3)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,3)*(2*q0*q3 - 2*q1*q2) + P(18,3)*(2*q0*q1 + 2*q2*q3) - P(3,3)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P(16,20)*(2*q0*q3 - 2*q1*q2) + P(18,20)*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P(20,0) + P(0,0)*SH_MAG[2] + P(1,0)*SH_MAG[1] + P(2,0)*SH_MAG[0] - P(17,0)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,0)*(2*q0*q3 - 2*q1*q2) + P(18,0)*(2*q0*q1 + 2*q2*q3) - P(3,0)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P(20,1) + P(0,1)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(2,1)*SH_MAG[0] - P(17,1)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,1)*(2*q0*q3 - 2*q1*q2) + P(18,1)*(2*q0*q1 + 2*q2*q3) - P(3,1)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P(20,2) + P(0,2)*SH_MAG[2] + P(1,2)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(17,2)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,2)*(2*q0*q3 - 2*q1*q2) + P(18,2)*(2*q0*q1 + 2*q2*q3) - P(3,2)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P(20,17) + P(0,17)*SH_MAG[2] + P(1,17)*SH_MAG[1] + P(2,17)*SH_MAG[0] - P(17,17)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,17)*(2*q0*q3 - 2*q1*q2) + P(18,17)*(2*q0*q1 + 2*q2*q3) - P(3,17)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P(3,20)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||
|
|
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
|
||
|
|
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||
|
|
SK_MY[3] = 2*q0*q3 - 2*q1*q2;
|
||
|
|
SK_MY[4] = 2*q0*q1 + 2*q2*q3;
|
||
|
|
float Kfusion[24][1];
|
||
|
|
float Kfusion[1][1];
|
||
|
|
Kfusion[0] = SK_MY[0]*(P(0,20) + P(0,0)*SH_MAG[2] + P(0,1)*SH_MAG[1] + P(0,2)*SH_MAG[0] - P(0,3)*SK_MY[2] - P(0,17)*SK_MY[1] - P(0,16)*SK_MY[3] + P(0,18)*SK_MY[4]);
|
||
|
|
Kfusion[1] = SK_MY[0]*(P(1,20) + P(1,0)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(1,2)*SH_MAG[0] - P(1,3)*SK_MY[2] - P(1,17)*SK_MY[1] - P(1,16)*SK_MY[3] + P(1,18)*SK_MY[4]);
|
||
|
|
Kfusion[2] = SK_MY[0]*(P(2,20) + P(2,0)*SH_MAG[2] + P(2,1)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(2,3)*SK_MY[2] - P(2,17)*SK_MY[1] - P(2,16)*SK_MY[3] + P(2,18)*SK_MY[4]);
|
||
|
|
Kfusion[3] = SK_MY[0]*(P(3,20) + P(3,0)*SH_MAG[2] + P(3,1)*SH_MAG[1] + P(3,2)*SH_MAG[0] - P(3,3)*SK_MY[2] - P(3,17)*SK_MY[1] - P(3,16)*SK_MY[3] + P(3,18)*SK_MY[4]);
|
||
|
|
Kfusion[4] = SK_MY[0]*(P(4,20) + P(4,0)*SH_MAG[2] + P(4,1)*SH_MAG[1] + P(4,2)*SH_MAG[0] - P(4,3)*SK_MY[2] - P(4,17)*SK_MY[1] - P(4,16)*SK_MY[3] + P(4,18)*SK_MY[4]);
|
||
|
|
Kfusion[5] = SK_MY[0]*(P(5,20) + P(5,0)*SH_MAG[2] + P(5,1)*SH_MAG[1] + P(5,2)*SH_MAG[0] - P(5,3)*SK_MY[2] - P(5,17)*SK_MY[1] - P(5,16)*SK_MY[3] + P(5,18)*SK_MY[4]);
|
||
|
|
Kfusion[6] = SK_MY[0]*(P(6,20) + P(6,0)*SH_MAG[2] + P(6,1)*SH_MAG[1] + P(6,2)*SH_MAG[0] - P(6,3)*SK_MY[2] - P(6,17)*SK_MY[1] - P(6,16)*SK_MY[3] + P(6,18)*SK_MY[4]);
|
||
|
|
Kfusion[7] = SK_MY[0]*(P(7,20) + P(7,0)*SH_MAG[2] + P(7,1)*SH_MAG[1] + P(7,2)*SH_MAG[0] - P(7,3)*SK_MY[2] - P(7,17)*SK_MY[1] - P(7,16)*SK_MY[3] + P(7,18)*SK_MY[4]);
|
||
|
|
Kfusion[8] = SK_MY[0]*(P(8,20) + P(8,0)*SH_MAG[2] + P(8,1)*SH_MAG[1] + P(8,2)*SH_MAG[0] - P(8,3)*SK_MY[2] - P(8,17)*SK_MY[1] - P(8,16)*SK_MY[3] + P(8,18)*SK_MY[4]);
|
||
|
|
Kfusion[9] = SK_MY[0]*(P(9,20) + P(9,0)*SH_MAG[2] + P(9,1)*SH_MAG[1] + P(9,2)*SH_MAG[0] - P(9,3)*SK_MY[2] - P(9,17)*SK_MY[1] - P(9,16)*SK_MY[3] + P(9,18)*SK_MY[4]);
|
||
|
|
Kfusion[10] = SK_MY[0]*(P(10,20) + P(10,0)*SH_MAG[2] + P(10,1)*SH_MAG[1] + P(10,2)*SH_MAG[0] - P(10,3)*SK_MY[2] - P(10,17)*SK_MY[1] - P(10,16)*SK_MY[3] + P(10,18)*SK_MY[4]);
|
||
|
|
Kfusion[11] = SK_MY[0]*(P(11,20) + P(11,0)*SH_MAG[2] + P(11,1)*SH_MAG[1] + P(11,2)*SH_MAG[0] - P(11,3)*SK_MY[2] - P(11,17)*SK_MY[1] - P(11,16)*SK_MY[3] + P(11,18)*SK_MY[4]);
|
||
|
|
Kfusion[12] = SK_MY[0]*(P(12,20) + P(12,0)*SH_MAG[2] + P(12,1)*SH_MAG[1] + P(12,2)*SH_MAG[0] - P(12,3)*SK_MY[2] - P(12,17)*SK_MY[1] - P(12,16)*SK_MY[3] + P(12,18)*SK_MY[4]);
|
||
|
|
Kfusion[13] = SK_MY[0]*(P(13,20) + P(13,0)*SH_MAG[2] + P(13,1)*SH_MAG[1] + P(13,2)*SH_MAG[0] - P(13,3)*SK_MY[2] - P(13,17)*SK_MY[1] - P(13,16)*SK_MY[3] + P(13,18)*SK_MY[4]);
|
||
|
|
Kfusion[14] = SK_MY[0]*(P(14,20) + P(14,0)*SH_MAG[2] + P(14,1)*SH_MAG[1] + P(14,2)*SH_MAG[0] - P(14,3)*SK_MY[2] - P(14,17)*SK_MY[1] - P(14,16)*SK_MY[3] + P(14,18)*SK_MY[4]);
|
||
|
|
Kfusion[15] = SK_MY[0]*(P(15,20) + P(15,0)*SH_MAG[2] + P(15,1)*SH_MAG[1] + P(15,2)*SH_MAG[0] - P(15,3)*SK_MY[2] - P(15,17)*SK_MY[1] - P(15,16)*SK_MY[3] + P(15,18)*SK_MY[4]);
|
||
|
|
Kfusion[16] = SK_MY[0]*(P(16,20) + P(16,0)*SH_MAG[2] + P(16,1)*SH_MAG[1] + P(16,2)*SH_MAG[0] - P(16,3)*SK_MY[2] - P(16,17)*SK_MY[1] - P(16,16)*SK_MY[3] + P(16,18)*SK_MY[4]);
|
||
|
|
Kfusion[17] = SK_MY[0]*(P(17,20) + P(17,0)*SH_MAG[2] + P(17,1)*SH_MAG[1] + P(17,2)*SH_MAG[0] - P(17,3)*SK_MY[2] - P(17,17)*SK_MY[1] - P(17,16)*SK_MY[3] + P(17,18)*SK_MY[4]);
|
||
|
|
Kfusion[18] = SK_MY[0]*(P(18,20) + P(18,0)*SH_MAG[2] + P(18,1)*SH_MAG[1] + P(18,2)*SH_MAG[0] - P(18,3)*SK_MY[2] - P(18,17)*SK_MY[1] - P(18,16)*SK_MY[3] + P(18,18)*SK_MY[4]);
|
||
|
|
Kfusion[19] = SK_MY[0]*(P(19,20) + P(19,0)*SH_MAG[2] + P(19,1)*SH_MAG[1] + P(19,2)*SH_MAG[0] - P(19,3)*SK_MY[2] - P(19,17)*SK_MY[1] - P(19,16)*SK_MY[3] + P(19,18)*SK_MY[4]);
|
||
|
|
Kfusion[20] = SK_MY[0]*(P(20,20) + P(20,0)*SH_MAG[2] + P(20,1)*SH_MAG[1] + P(20,2)*SH_MAG[0] - P(20,3)*SK_MY[2] - P(20,17)*SK_MY[1] - P(20,16)*SK_MY[3] + P(20,18)*SK_MY[4]);
|
||
|
|
Kfusion[21] = SK_MY[0]*(P(21,20) + P(21,0)*SH_MAG[2] + P(21,1)*SH_MAG[1] + P(21,2)*SH_MAG[0] - P(21,3)*SK_MY[2] - P(21,17)*SK_MY[1] - P(21,16)*SK_MY[3] + P(21,18)*SK_MY[4]);
|
||
|
|
Kfusion[22] = SK_MY[0]*(P(22,20) + P(22,0)*SH_MAG[2] + P(22,1)*SH_MAG[1] + P(22,2)*SH_MAG[0] - P(22,3)*SK_MY[2] - P(22,17)*SK_MY[1] - P(22,16)*SK_MY[3] + P(22,18)*SK_MY[4]);
|
||
|
|
Kfusion[23] = SK_MY[0]*(P(23,20) + P(23,0)*SH_MAG[2] + P(23,1)*SH_MAG[1] + P(23,2)*SH_MAG[0] - P(23,3)*SK_MY[2] - P(23,17)*SK_MY[1] - P(23,16)*SK_MY[3] + P(23,18)*SK_MY[4]);
|
||
|
|
float H_MAG[1][24];
|
||
|
|
H_MAG[0] = SH_MAG[1];
|
||
|
|
H_MAG[1] = -SH_MAG[2];
|
||
|
|
H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||
|
|
H_MAG[3] = SH_MAG[0];
|
||
|
|
H_MAG[16] = 2*q0*q2 + 2*q1*q3;
|
||
|
|
H_MAG[17] = 2*q2*q3 - 2*q0*q1;
|
||
|
|
H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||
|
|
H_MAG[21] = 1;
|
||
|
|
float SK_MZ[5][1];
|
||
|
|
SK_MZ[0] = 1/(P(21,21) + R_MAG + P(0,21)*SH_MAG[1] - P(1,21)*SH_MAG[2] + P(3,21)*SH_MAG[0] + P(18,21)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2*q1*q3)*(P(21,16) + P(0,16)*SH_MAG[1] - P(1,16)*SH_MAG[2] + P(3,16)*SH_MAG[0] + P(18,16)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,16)*(2*q0*q2 + 2*q1*q3) - P(17,16)*(2*q0*q1 - 2*q2*q3) + P(2,16)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P(21,17) + P(0,17)*SH_MAG[1] - P(1,17)*SH_MAG[2] + P(3,17)*SH_MAG[0] + P(18,17)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,17)*(2*q0*q2 + 2*q1*q3) - P(17,17)*(2*q0*q1 - 2*q2*q3) + P(2,17)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P(21,2) + P(0,2)*SH_MAG[1] - P(1,2)*SH_MAG[2] + P(3,2)*SH_MAG[0] + P(18,2)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,2)*(2*q0*q2 + 2*q1*q3) - P(17,2)*(2*q0*q1 - 2*q2*q3) + P(2,2)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P(16,21)*(2*q0*q2 + 2*q1*q3) - P(17,21)*(2*q0*q1 - 2*q2*q3) + SH_MAG[1]*(P(21,0) + P(0,0)*SH_MAG[1] - P(1,0)*SH_MAG[2] + P(3,0)*SH_MAG[0] + P(18,0)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,0)*(2*q0*q2 + 2*q1*q3) - P(17,0)*(2*q0*q1 - 2*q2*q3) + P(2,0)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[2]*(P(21,1) + P(0,1)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(3,1)*SH_MAG[0] + P(18,1)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,1)*(2*q0*q2 + 2*q1*q3) - P(17,1)*(2*q0*q1 - 2*q2*q3) + P(2,1)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P(21,3) + P(0,3)*SH_MAG[1] - P(1,3)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(18,3)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,3)*(2*q0*q2 + 2*q1*q3) - P(17,3)*(2*q0*q1 - 2*q2*q3) + P(2,3)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P(21,18) + P(0,18)*SH_MAG[1] - P(1,18)*SH_MAG[2] + P(3,18)*SH_MAG[0] + P(18,18)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,18)*(2*q0*q2 + 2*q1*q3) - P(17,18)*(2*q0*q1 - 2*q2*q3) + P(2,18)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P(2,21)*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||
|
|
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||
|
|
SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||
|
|
SK_MZ[3] = 2*q0*q1 - 2*q2*q3;
|
||
|
|
SK_MZ[4] = 2*q0*q2 + 2*q1*q3;
|
||
|
|
float Kfusion[24][1];
|
||
|
|
float Kfusion[1][1];
|
||
|
|
Kfusion[0] = SK_MZ[0]*(P(0,21) + P(0,0)*SH_MAG[1] - P(0,1)*SH_MAG[2] + P(0,3)*SH_MAG[0] + P(0,2)*SK_MZ[2] + P(0,18)*SK_MZ[1] + P(0,16)*SK_MZ[4] - P(0,17)*SK_MZ[3]);
|
||
|
|
Kfusion[1] = SK_MZ[0]*(P(1,21) + P(1,0)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(1,3)*SH_MAG[0] + P(1,2)*SK_MZ[2] + P(1,18)*SK_MZ[1] + P(1,16)*SK_MZ[4] - P(1,17)*SK_MZ[3]);
|
||
|
|
Kfusion[2] = SK_MZ[0]*(P(2,21) + P(2,0)*SH_MAG[1] - P(2,1)*SH_MAG[2] + P(2,3)*SH_MAG[0] + P(2,2)*SK_MZ[2] + P(2,18)*SK_MZ[1] + P(2,16)*SK_MZ[4] - P(2,17)*SK_MZ[3]);
|
||
|
|
Kfusion[3] = SK_MZ[0]*(P(3,21) + P(3,0)*SH_MAG[1] - P(3,1)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(3,2)*SK_MZ[2] + P(3,18)*SK_MZ[1] + P(3,16)*SK_MZ[4] - P(3,17)*SK_MZ[3]);
|
||
|
|
Kfusion[4] = SK_MZ[0]*(P(4,21) + P(4,0)*SH_MAG[1] - P(4,1)*SH_MAG[2] + P(4,3)*SH_MAG[0] + P(4,2)*SK_MZ[2] + P(4,18)*SK_MZ[1] + P(4,16)*SK_MZ[4] - P(4,17)*SK_MZ[3]);
|
||
|
|
Kfusion[5] = SK_MZ[0]*(P(5,21) + P(5,0)*SH_MAG[1] - P(5,1)*SH_MAG[2] + P(5,3)*SH_MAG[0] + P(5,2)*SK_MZ[2] + P(5,18)*SK_MZ[1] + P(5,16)*SK_MZ[4] - P(5,17)*SK_MZ[3]);
|
||
|
|
Kfusion[6] = SK_MZ[0]*(P(6,21) + P(6,0)*SH_MAG[1] - P(6,1)*SH_MAG[2] + P(6,3)*SH_MAG[0] + P(6,2)*SK_MZ[2] + P(6,18)*SK_MZ[1] + P(6,16)*SK_MZ[4] - P(6,17)*SK_MZ[3]);
|
||
|
|
Kfusion[7] = SK_MZ[0]*(P(7,21) + P(7,0)*SH_MAG[1] - P(7,1)*SH_MAG[2] + P(7,3)*SH_MAG[0] + P(7,2)*SK_MZ[2] + P(7,18)*SK_MZ[1] + P(7,16)*SK_MZ[4] - P(7,17)*SK_MZ[3]);
|
||
|
|
Kfusion[8] = SK_MZ[0]*(P(8,21) + P(8,0)*SH_MAG[1] - P(8,1)*SH_MAG[2] + P(8,3)*SH_MAG[0] + P(8,2)*SK_MZ[2] + P(8,18)*SK_MZ[1] + P(8,16)*SK_MZ[4] - P(8,17)*SK_MZ[3]);
|
||
|
|
Kfusion[9] = SK_MZ[0]*(P(9,21) + P(9,0)*SH_MAG[1] - P(9,1)*SH_MAG[2] + P(9,3)*SH_MAG[0] + P(9,2)*SK_MZ[2] + P(9,18)*SK_MZ[1] + P(9,16)*SK_MZ[4] - P(9,17)*SK_MZ[3]);
|
||
|
|
Kfusion[10] = SK_MZ[0]*(P(10,21) + P(10,0)*SH_MAG[1] - P(10,1)*SH_MAG[2] + P(10,3)*SH_MAG[0] + P(10,2)*SK_MZ[2] + P(10,18)*SK_MZ[1] + P(10,16)*SK_MZ[4] - P(10,17)*SK_MZ[3]);
|
||
|
|
Kfusion[11] = SK_MZ[0]*(P(11,21) + P(11,0)*SH_MAG[1] - P(11,1)*SH_MAG[2] + P(11,3)*SH_MAG[0] + P(11,2)*SK_MZ[2] + P(11,18)*SK_MZ[1] + P(11,16)*SK_MZ[4] - P(11,17)*SK_MZ[3]);
|
||
|
|
Kfusion[12] = SK_MZ[0]*(P(12,21) + P(12,0)*SH_MAG[1] - P(12,1)*SH_MAG[2] + P(12,3)*SH_MAG[0] + P(12,2)*SK_MZ[2] + P(12,18)*SK_MZ[1] + P(12,16)*SK_MZ[4] - P(12,17)*SK_MZ[3]);
|
||
|
|
Kfusion[13] = SK_MZ[0]*(P(13,21) + P(13,0)*SH_MAG[1] - P(13,1)*SH_MAG[2] + P(13,3)*SH_MAG[0] + P(13,2)*SK_MZ[2] + P(13,18)*SK_MZ[1] + P(13,16)*SK_MZ[4] - P(13,17)*SK_MZ[3]);
|
||
|
|
Kfusion[14] = SK_MZ[0]*(P(14,21) + P(14,0)*SH_MAG[1] - P(14,1)*SH_MAG[2] + P(14,3)*SH_MAG[0] + P(14,2)*SK_MZ[2] + P(14,18)*SK_MZ[1] + P(14,16)*SK_MZ[4] - P(14,17)*SK_MZ[3]);
|
||
|
|
Kfusion[15] = SK_MZ[0]*(P(15,21) + P(15,0)*SH_MAG[1] - P(15,1)*SH_MAG[2] + P(15,3)*SH_MAG[0] + P(15,2)*SK_MZ[2] + P(15,18)*SK_MZ[1] + P(15,16)*SK_MZ[4] - P(15,17)*SK_MZ[3]);
|
||
|
|
Kfusion[16] = SK_MZ[0]*(P(16,21) + P(16,0)*SH_MAG[1] - P(16,1)*SH_MAG[2] + P(16,3)*SH_MAG[0] + P(16,2)*SK_MZ[2] + P(16,18)*SK_MZ[1] + P(16,16)*SK_MZ[4] - P(16,17)*SK_MZ[3]);
|
||
|
|
Kfusion[17] = SK_MZ[0]*(P(17,21) + P(17,0)*SH_MAG[1] - P(17,1)*SH_MAG[2] + P(17,3)*SH_MAG[0] + P(17,2)*SK_MZ[2] + P(17,18)*SK_MZ[1] + P(17,16)*SK_MZ[4] - P(17,17)*SK_MZ[3]);
|
||
|
|
Kfusion[18] = SK_MZ[0]*(P(18,21) + P(18,0)*SH_MAG[1] - P(18,1)*SH_MAG[2] + P(18,3)*SH_MAG[0] + P(18,2)*SK_MZ[2] + P(18,18)*SK_MZ[1] + P(18,16)*SK_MZ[4] - P(18,17)*SK_MZ[3]);
|
||
|
|
Kfusion[19] = SK_MZ[0]*(P(19,21) + P(19,0)*SH_MAG[1] - P(19,1)*SH_MAG[2] + P(19,3)*SH_MAG[0] + P(19,2)*SK_MZ[2] + P(19,18)*SK_MZ[1] + P(19,16)*SK_MZ[4] - P(19,17)*SK_MZ[3]);
|
||
|
|
Kfusion[20] = SK_MZ[0]*(P(20,21) + P(20,0)*SH_MAG[1] - P(20,1)*SH_MAG[2] + P(20,3)*SH_MAG[0] + P(20,2)*SK_MZ[2] + P(20,18)*SK_MZ[1] + P(20,16)*SK_MZ[4] - P(20,17)*SK_MZ[3]);
|
||
|
|
Kfusion[21] = SK_MZ[0]*(P(21,21) + P(21,0)*SH_MAG[1] - P(21,1)*SH_MAG[2] + P(21,3)*SH_MAG[0] + P(21,2)*SK_MZ[2] + P(21,18)*SK_MZ[1] + P(21,16)*SK_MZ[4] - P(21,17)*SK_MZ[3]);
|
||
|
|
Kfusion[22] = SK_MZ[0]*(P(22,21) + P(22,0)*SH_MAG[1] - P(22,1)*SH_MAG[2] + P(22,3)*SH_MAG[0] + P(22,2)*SK_MZ[2] + P(22,18)*SK_MZ[1] + P(22,16)*SK_MZ[4] - P(22,17)*SK_MZ[3]);
|
||
|
|
Kfusion[23] = SK_MZ[0]*(P(23,21) + P(23,0)*SH_MAG[1] - P(23,1)*SH_MAG[2] + P(23,3)*SH_MAG[0] + P(23,2)*SK_MZ[2] + P(23,18)*SK_MZ[1] + P(23,16)*SK_MZ[4] - P(23,17)*SK_MZ[3]);
|
||
|
|
float SH_ACCX[4][1];
|
||
|
|
SH_ACCX[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||
|
|
SH_ACCX[1] = vn - vwn;
|
||
|
|
SH_ACCX[2] = ve - vwe;
|
||
|
|
SH_ACCX[3] = 2*q0*q3 + 2*q1*q2;
|
||
|
|
float H_ACCX[1][24];
|
||
|
|
H_ACCX[0][0] = -Kaccx*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd);
|
||
|
|
H_ACCX[0][1] = -Kaccx*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd);
|
||
|
|
H_ACCX[0][2] = Kaccx*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd);
|
||
|
|
H_ACCX[0][3] = -Kaccx*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd);
|
||
|
|
H_ACCX[0][4] = -Kaccx*SH_ACCX[0];
|
||
|
|
H_ACCX[0][5] = -Kaccx*SH_ACCX[3];
|
||
|
|
H_ACCX[0][6] = Kaccx*(2*q0*q2 - 2*q1*q3);
|
||
|
|
H_ACCX[0][22] = Kaccx*SH_ACCX[0];
|
||
|
|
H_ACCX[0][23] = Kaccx*SH_ACCX[3];
|
||
|
|
float SK_ACCX[7][1];
|
||
|
|
SK_ACCX[0] = 1/(R_ACC + Kaccx*SH_ACCX[0]*(Kaccx*P(4,4)*SH_ACCX[0] + Kaccx*P(5,4)*SH_ACCX[3] - Kaccx*P(22,4)*SH_ACCX[0] - Kaccx*P(23,4)*SH_ACCX[3] - Kaccx*P(6,4)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,4)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,4)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,4)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,4)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*SH_ACCX[3]*(Kaccx*P(4,5)*SH_ACCX[0] + Kaccx*P(5,5)*SH_ACCX[3] - Kaccx*P(22,5)*SH_ACCX[0] - Kaccx*P(23,5)*SH_ACCX[3] - Kaccx*P(6,5)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,5)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,5)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,5)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,5)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*SH_ACCX[0]*(Kaccx*P(4,22)*SH_ACCX[0] + Kaccx*P(5,22)*SH_ACCX[3] - Kaccx*P(22,22)*SH_ACCX[0] - Kaccx*P(23,22)*SH_ACCX[3] - Kaccx*P(6,22)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,22)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,22)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,22)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,22)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*SH_ACCX[3]*(Kaccx*P(4,23)*SH_ACCX[0] + Kaccx*P(5,23)*SH_ACCX[3] - Kaccx*P(22,23)*SH_ACCX[0] - Kaccx*P(23,23)*SH_ACCX[3] - Kaccx*P(6,23)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,23)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,23)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,23)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,23)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*P(4,6)*SH_ACCX[0] + Kaccx*P(5,6)*SH_ACCX[3] - Kaccx*P(22,6)*SH_ACCX[0] - Kaccx*P(23,6)*SH_ACCX[3] - Kaccx*P(6,6)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,6)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,6)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,6)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,6)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd)*(Kaccx*P(4,0)*SH_ACCX[0] + Kaccx*P(5,0)*SH_ACCX[3] - Kaccx*P(22,0)*SH_ACCX[0] - Kaccx*P(23,0)*SH_ACCX[3] - Kaccx*P(6,0)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,0)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,0)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,0)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,0)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd)*(Kaccx*P(4,1)*SH_ACCX[0] + Kaccx*P(5,1)*SH_ACCX[3] - Kaccx*P(22,1)*SH_ACCX[0] - Kaccx*P(23,1)*SH_ACCX[3] - Kaccx*P(6,1)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,1)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,1)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,1)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,1)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd)*(Kaccx*P(4,2)*SH_ACCX[0] + Kaccx*P(5,2)*SH_ACCX[3] - Kaccx*P(22,2)*SH_ACCX[0] - Kaccx*P(23,2)*SH_ACCX[3] - Kaccx*P(6,2)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,2)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,2)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,2)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,2)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)*(Kaccx*P(4,3)*SH_ACCX[0] + Kaccx*P(5,3)*SH_ACCX[3] - Kaccx*P(22,3)*SH_ACCX[0] - Kaccx*P(23,3)*SH_ACCX[3] - Kaccx*P(6,3)*(2*q0*q2 - 2*q1*q3) + Kaccx*P(0,3)*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P(1,3)*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P(2,3)*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P(3,3)*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)));
|
||
|
|
SK_ACCX[1] = 2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd;
|
||
|
|
SK_ACCX[2] = 2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd;
|
||
|
|
SK_ACCX[3] = 2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd;
|
||
|
|
SK_ACCX[4] = 2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd;
|
||
|
|
SK_ACCX[5] = 2*q0*q2 - 2*q1*q3;
|
||
|
|
SK_ACCX[6] = SH_ACCX[3];
|
||
|
|
float Kfusion[24][1];
|
||
|
|
float Kfusion[1][1];
|
||
|
|
Kfusion[0] = -SK_ACCX[0]*(Kaccx*P(0,4)*SH_ACCX[0] - Kaccx*P(0,22)*SH_ACCX[0] + Kaccx*P(0,0)*SK_ACCX[3] - Kaccx*P(0,2)*SK_ACCX[2] + Kaccx*P(0,3)*SK_ACCX[1] + Kaccx*P(0,1)*SK_ACCX[4] + Kaccx*P(0,5)*SK_ACCX[6] - Kaccx*P(0,6)*SK_ACCX[5] - Kaccx*P(0,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[1] = -SK_ACCX[0]*(Kaccx*P(1,4)*SH_ACCX[0] - Kaccx*P(1,22)*SH_ACCX[0] + Kaccx*P(1,0)*SK_ACCX[3] - Kaccx*P(1,2)*SK_ACCX[2] + Kaccx*P(1,3)*SK_ACCX[1] + Kaccx*P(1,1)*SK_ACCX[4] + Kaccx*P(1,5)*SK_ACCX[6] - Kaccx*P(1,6)*SK_ACCX[5] - Kaccx*P(1,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[2] = -SK_ACCX[0]*(Kaccx*P(2,4)*SH_ACCX[0] - Kaccx*P(2,22)*SH_ACCX[0] + Kaccx*P(2,0)*SK_ACCX[3] - Kaccx*P(2,2)*SK_ACCX[2] + Kaccx*P(2,3)*SK_ACCX[1] + Kaccx*P(2,1)*SK_ACCX[4] + Kaccx*P(2,5)*SK_ACCX[6] - Kaccx*P(2,6)*SK_ACCX[5] - Kaccx*P(2,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[3] = -SK_ACCX[0]*(Kaccx*P(3,4)*SH_ACCX[0] - Kaccx*P(3,22)*SH_ACCX[0] + Kaccx*P(3,0)*SK_ACCX[3] - Kaccx*P(3,2)*SK_ACCX[2] + Kaccx*P(3,3)*SK_ACCX[1] + Kaccx*P(3,1)*SK_ACCX[4] + Kaccx*P(3,5)*SK_ACCX[6] - Kaccx*P(3,6)*SK_ACCX[5] - Kaccx*P(3,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[4] = -SK_ACCX[0]*(Kaccx*P(4,4)*SH_ACCX[0] - Kaccx*P(4,22)*SH_ACCX[0] + Kaccx*P(4,0)*SK_ACCX[3] - Kaccx*P(4,2)*SK_ACCX[2] + Kaccx*P(4,3)*SK_ACCX[1] + Kaccx*P(4,1)*SK_ACCX[4] + Kaccx*P(4,5)*SK_ACCX[6] - Kaccx*P(4,6)*SK_ACCX[5] - Kaccx*P(4,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[5] = -SK_ACCX[0]*(Kaccx*P(5,4)*SH_ACCX[0] - Kaccx*P(5,22)*SH_ACCX[0] + Kaccx*P(5,0)*SK_ACCX[3] - Kaccx*P(5,2)*SK_ACCX[2] + Kaccx*P(5,3)*SK_ACCX[1] + Kaccx*P(5,1)*SK_ACCX[4] + Kaccx*P(5,5)*SK_ACCX[6] - Kaccx*P(5,6)*SK_ACCX[5] - Kaccx*P(5,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[6] = -SK_ACCX[0]*(Kaccx*P(6,4)*SH_ACCX[0] - Kaccx*P(6,22)*SH_ACCX[0] + Kaccx*P(6,0)*SK_ACCX[3] - Kaccx*P(6,2)*SK_ACCX[2] + Kaccx*P(6,3)*SK_ACCX[1] + Kaccx*P(6,1)*SK_ACCX[4] + Kaccx*P(6,5)*SK_ACCX[6] - Kaccx*P(6,6)*SK_ACCX[5] - Kaccx*P(6,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[7] = -SK_ACCX[0]*(Kaccx*P(7,4)*SH_ACCX[0] - Kaccx*P(7,22)*SH_ACCX[0] + Kaccx*P(7,0)*SK_ACCX[3] - Kaccx*P(7,2)*SK_ACCX[2] + Kaccx*P(7,3)*SK_ACCX[1] + Kaccx*P(7,1)*SK_ACCX[4] + Kaccx*P(7,5)*SK_ACCX[6] - Kaccx*P(7,6)*SK_ACCX[5] - Kaccx*P(7,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[8] = -SK_ACCX[0]*(Kaccx*P(8,4)*SH_ACCX[0] - Kaccx*P(8,22)*SH_ACCX[0] + Kaccx*P(8,0)*SK_ACCX[3] - Kaccx*P(8,2)*SK_ACCX[2] + Kaccx*P(8,3)*SK_ACCX[1] + Kaccx*P(8,1)*SK_ACCX[4] + Kaccx*P(8,5)*SK_ACCX[6] - Kaccx*P(8,6)*SK_ACCX[5] - Kaccx*P(8,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[9] = -SK_ACCX[0]*(Kaccx*P(9,4)*SH_ACCX[0] - Kaccx*P(9,22)*SH_ACCX[0] + Kaccx*P(9,0)*SK_ACCX[3] - Kaccx*P(9,2)*SK_ACCX[2] + Kaccx*P(9,3)*SK_ACCX[1] + Kaccx*P(9,1)*SK_ACCX[4] + Kaccx*P(9,5)*SK_ACCX[6] - Kaccx*P(9,6)*SK_ACCX[5] - Kaccx*P(9,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[10] = -SK_ACCX[0]*(Kaccx*P(10,4)*SH_ACCX[0] - Kaccx*P(10,22)*SH_ACCX[0] + Kaccx*P(10,0)*SK_ACCX[3] - Kaccx*P(10,2)*SK_ACCX[2] + Kaccx*P(10,3)*SK_ACCX[1] + Kaccx*P(10,1)*SK_ACCX[4] + Kaccx*P(10,5)*SK_ACCX[6] - Kaccx*P(10,6)*SK_ACCX[5] - Kaccx*P(10,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[11] = -SK_ACCX[0]*(Kaccx*P(11,4)*SH_ACCX[0] - Kaccx*P(11,22)*SH_ACCX[0] + Kaccx*P(11,0)*SK_ACCX[3] - Kaccx*P(11,2)*SK_ACCX[2] + Kaccx*P(11,3)*SK_ACCX[1] + Kaccx*P(11,1)*SK_ACCX[4] + Kaccx*P(11,5)*SK_ACCX[6] - Kaccx*P(11,6)*SK_ACCX[5] - Kaccx*P(11,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[12] = -SK_ACCX[0]*(Kaccx*P(12,4)*SH_ACCX[0] - Kaccx*P(12,22)*SH_ACCX[0] + Kaccx*P(12,0)*SK_ACCX[3] - Kaccx*P(12,2)*SK_ACCX[2] + Kaccx*P(12,3)*SK_ACCX[1] + Kaccx*P(12,1)*SK_ACCX[4] + Kaccx*P(12,5)*SK_ACCX[6] - Kaccx*P(12,6)*SK_ACCX[5] - Kaccx*P(12,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[13] = -SK_ACCX[0]*(Kaccx*P(13,4)*SH_ACCX[0] - Kaccx*P(13,22)*SH_ACCX[0] + Kaccx*P(13,0)*SK_ACCX[3] - Kaccx*P(13,2)*SK_ACCX[2] + Kaccx*P(13,3)*SK_ACCX[1] + Kaccx*P(13,1)*SK_ACCX[4] + Kaccx*P(13,5)*SK_ACCX[6] - Kaccx*P(13,6)*SK_ACCX[5] - Kaccx*P(13,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[14] = -SK_ACCX[0]*(Kaccx*P(14,4)*SH_ACCX[0] - Kaccx*P(14,22)*SH_ACCX[0] + Kaccx*P(14,0)*SK_ACCX[3] - Kaccx*P(14,2)*SK_ACCX[2] + Kaccx*P(14,3)*SK_ACCX[1] + Kaccx*P(14,1)*SK_ACCX[4] + Kaccx*P(14,5)*SK_ACCX[6] - Kaccx*P(14,6)*SK_ACCX[5] - Kaccx*P(14,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[15] = -SK_ACCX[0]*(Kaccx*P(15,4)*SH_ACCX[0] - Kaccx*P(15,22)*SH_ACCX[0] + Kaccx*P(15,0)*SK_ACCX[3] - Kaccx*P(15,2)*SK_ACCX[2] + Kaccx*P(15,3)*SK_ACCX[1] + Kaccx*P(15,1)*SK_ACCX[4] + Kaccx*P(15,5)*SK_ACCX[6] - Kaccx*P(15,6)*SK_ACCX[5] - Kaccx*P(15,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[16] = -SK_ACCX[0]*(Kaccx*P(16,4)*SH_ACCX[0] - Kaccx*P(16,22)*SH_ACCX[0] + Kaccx*P(16,0)*SK_ACCX[3] - Kaccx*P(16,2)*SK_ACCX[2] + Kaccx*P(16,3)*SK_ACCX[1] + Kaccx*P(16,1)*SK_ACCX[4] + Kaccx*P(16,5)*SK_ACCX[6] - Kaccx*P(16,6)*SK_ACCX[5] - Kaccx*P(16,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[17] = -SK_ACCX[0]*(Kaccx*P(17,4)*SH_ACCX[0] - Kaccx*P(17,22)*SH_ACCX[0] + Kaccx*P(17,0)*SK_ACCX[3] - Kaccx*P(17,2)*SK_ACCX[2] + Kaccx*P(17,3)*SK_ACCX[1] + Kaccx*P(17,1)*SK_ACCX[4] + Kaccx*P(17,5)*SK_ACCX[6] - Kaccx*P(17,6)*SK_ACCX[5] - Kaccx*P(17,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[18] = -SK_ACCX[0]*(Kaccx*P(18,4)*SH_ACCX[0] - Kaccx*P(18,22)*SH_ACCX[0] + Kaccx*P(18,0)*SK_ACCX[3] - Kaccx*P(18,2)*SK_ACCX[2] + Kaccx*P(18,3)*SK_ACCX[1] + Kaccx*P(18,1)*SK_ACCX[4] + Kaccx*P(18,5)*SK_ACCX[6] - Kaccx*P(18,6)*SK_ACCX[5] - Kaccx*P(18,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[19] = -SK_ACCX[0]*(Kaccx*P(19,4)*SH_ACCX[0] - Kaccx*P(19,22)*SH_ACCX[0] + Kaccx*P(19,0)*SK_ACCX[3] - Kaccx*P(19,2)*SK_ACCX[2] + Kaccx*P(19,3)*SK_ACCX[1] + Kaccx*P(19,1)*SK_ACCX[4] + Kaccx*P(19,5)*SK_ACCX[6] - Kaccx*P(19,6)*SK_ACCX[5] - Kaccx*P(19,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[20] = -SK_ACCX[0]*(Kaccx*P(20,4)*SH_ACCX[0] - Kaccx*P(20,22)*SH_ACCX[0] + Kaccx*P(20,0)*SK_ACCX[3] - Kaccx*P(20,2)*SK_ACCX[2] + Kaccx*P(20,3)*SK_ACCX[1] + Kaccx*P(20,1)*SK_ACCX[4] + Kaccx*P(20,5)*SK_ACCX[6] - Kaccx*P(20,6)*SK_ACCX[5] - Kaccx*P(20,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[21] = -SK_ACCX[0]*(Kaccx*P(21,4)*SH_ACCX[0] - Kaccx*P(21,22)*SH_ACCX[0] + Kaccx*P(21,0)*SK_ACCX[3] - Kaccx*P(21,2)*SK_ACCX[2] + Kaccx*P(21,3)*SK_ACCX[1] + Kaccx*P(21,1)*SK_ACCX[4] + Kaccx*P(21,5)*SK_ACCX[6] - Kaccx*P(21,6)*SK_ACCX[5] - Kaccx*P(21,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[22] = -SK_ACCX[0]*(Kaccx*P(22,4)*SH_ACCX[0] - Kaccx*P(22,22)*SH_ACCX[0] + Kaccx*P(22,0)*SK_ACCX[3] - Kaccx*P(22,2)*SK_ACCX[2] + Kaccx*P(22,3)*SK_ACCX[1] + Kaccx*P(22,1)*SK_ACCX[4] + Kaccx*P(22,5)*SK_ACCX[6] - Kaccx*P(22,6)*SK_ACCX[5] - Kaccx*P(22,23)*SK_ACCX[6]);
|
||
|
|
Kfusion[23] = -SK_ACCX[0]*(Kaccx*P(23,4)*SH_ACCX[0] - Kaccx*P(23,22)*SH_ACCX[0] + Kaccx*P(23,0)*SK_ACCX[3] - Kaccx*P(23,2)*SK_ACCX[2] + Kaccx*P(23,3)*SK_ACCX[1] + Kaccx*P(23,1)*SK_ACCX[4] + Kaccx*P(23,5)*SK_ACCX[6] - Kaccx*P(23,6)*SK_ACCX[5] - Kaccx*P(23,23)*SK_ACCX[6]);
|
||
|
|
float SH_ACCY[3][1];
|
||
|
|
SH_ACCY[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||
|
|
SH_ACCY[1] = vn - vwn;
|
||
|
|
SH_ACCY[2] = ve - vwe;
|
||
|
|
float H_ACCY[1][24];
|
||
|
|
H_ACCY[0][0] = -Kaccy*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd);
|
||
|
|
H_ACCY[0][1] = -Kaccy*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd);
|
||
|
|
H_ACCY[0][2] = -Kaccy*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd);
|
||
|
|
H_ACCY[0][3] = Kaccy*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd);
|
||
|
|
H_ACCY[0][4] = Kaccy*(2*q0*q3 - 2*q1*q2);
|
||
|
|
H_ACCY[0][5] = -Kaccy*SH_ACCY[0];
|
||
|
|
H_ACCY[0][6] = -Kaccy*(2*q0*q1 + 2*q2*q3);
|
||
|
|
H_ACCY[0][22] = -2*Kaccy*(q0*q3 - q1*q2);
|
||
|
|
H_ACCY[0][23] = Kaccy*SH_ACCY[0];
|
||
|
|
float SK_ACCY[9][1];
|
||
|
|
SK_ACCY[0] = 1/(R_ACC + Kaccy*SH_ACCY[0]*(Kaccy*P(5,5)*SH_ACCY[0] - Kaccy*P(23,5)*SH_ACCY[0] - Kaccy*P(4,5)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,5)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,5)*(q0*q3 - q1*q2) + Kaccy*P(0,5)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,5)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,5)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,5)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*SH_ACCY[0]*(Kaccy*P(5,23)*SH_ACCY[0] - Kaccy*P(23,23)*SH_ACCY[0] - Kaccy*P(4,23)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,23)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,23)*(q0*q3 - q1*q2) + Kaccy*P(0,23)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,23)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,23)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,23)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*P(5,4)*SH_ACCY[0] - Kaccy*P(23,4)*SH_ACCY[0] - Kaccy*P(4,4)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,4)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,4)*(q0*q3 - q1*q2) + Kaccy*P(0,4)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,4)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,4)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,4)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*P(5,6)*SH_ACCY[0] - Kaccy*P(23,6)*SH_ACCY[0] - Kaccy*P(4,6)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,6)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,6)*(q0*q3 - q1*q2) + Kaccy*P(0,6)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,6)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,6)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,6)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*P(5,22)*SH_ACCY[0] - Kaccy*P(23,22)*SH_ACCY[0] - Kaccy*P(4,22)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,22)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,22)*(q0*q3 - q1*q2) + Kaccy*P(0,22)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,22)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,22)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,22)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd)*(Kaccy*P(5,0)*SH_ACCY[0] - Kaccy*P(23,0)*SH_ACCY[0] - Kaccy*P(4,0)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,0)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,0)*(q0*q3 - q1*q2) + Kaccy*P(0,0)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,0)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,0)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,0)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd)*(Kaccy*P(5,1)*SH_ACCY[0] - Kaccy*P(23,1)*SH_ACCY[0] - Kaccy*P(4,1)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,1)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,1)*(q0*q3 - q1*q2) + Kaccy*P(0,1)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,1)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,1)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,1)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd)*(Kaccy*P(5,2)*SH_ACCY[0] - Kaccy*P(23,2)*SH_ACCY[0] - Kaccy*P(4,2)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,2)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,2)*(q0*q3 - q1*q2) + Kaccy*P(0,2)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,2)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,2)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,2)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)*(Kaccy*P(5,3)*SH_ACCY[0] - Kaccy*P(23,3)*SH_ACCY[0] - Kaccy*P(4,3)*(2*q0*q3 - 2*q1*q2) + Kaccy*P(6,3)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P(22,3)*(q0*q3 - q1*q2) + Kaccy*P(0,3)*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P(1,3)*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P(2,3)*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P(3,3)*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)));
|
||
|
|
SK_ACCY[1] = 2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd;
|
||
|
|
SK_ACCY[2] = 2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd;
|
||
|
|
SK_ACCY[3] = 2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd;
|
||
|
|
SK_ACCY[4] = 2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd;
|
||
|
|
SK_ACCY[5] = 2*q0*q3 - 2*q1*q2;
|
||
|
|
SK_ACCY[6] = q0*q3 - q1*q2;
|
||
|
|
SK_ACCY[7] = 2*q0*q1 + 2*q2*q3;
|
||
|
|
SK_ACCY[8] = SH_ACCY[0];
|
||
|
|
float Kfusion[24][1];
|
||
|
|
float Kfusion[1][1];
|
||
|
|
Kfusion[0] = -SK_ACCY[0]*(Kaccy*P(0,0)*SK_ACCY[3] + Kaccy*P(0,1)*SK_ACCY[2] - Kaccy*P(0,3)*SK_ACCY[1] + Kaccy*P(0,2)*SK_ACCY[4] - Kaccy*P(0,4)*SK_ACCY[5] + Kaccy*P(0,5)*SK_ACCY[8] + Kaccy*P(0,6)*SK_ACCY[7] + 2*Kaccy*P(0,22)*SK_ACCY[6] - Kaccy*P(0,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[1] = -SK_ACCY[0]*(Kaccy*P(1,0)*SK_ACCY[3] + Kaccy*P(1,1)*SK_ACCY[2] - Kaccy*P(1,3)*SK_ACCY[1] + Kaccy*P(1,2)*SK_ACCY[4] - Kaccy*P(1,4)*SK_ACCY[5] + Kaccy*P(1,5)*SK_ACCY[8] + Kaccy*P(1,6)*SK_ACCY[7] + 2*Kaccy*P(1,22)*SK_ACCY[6] - Kaccy*P(1,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[2] = -SK_ACCY[0]*(Kaccy*P(2,0)*SK_ACCY[3] + Kaccy*P(2,1)*SK_ACCY[2] - Kaccy*P(2,3)*SK_ACCY[1] + Kaccy*P(2,2)*SK_ACCY[4] - Kaccy*P(2,4)*SK_ACCY[5] + Kaccy*P(2,5)*SK_ACCY[8] + Kaccy*P(2,6)*SK_ACCY[7] + 2*Kaccy*P(2,22)*SK_ACCY[6] - Kaccy*P(2,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[3] = -SK_ACCY[0]*(Kaccy*P(3,0)*SK_ACCY[3] + Kaccy*P(3,1)*SK_ACCY[2] - Kaccy*P(3,3)*SK_ACCY[1] + Kaccy*P(3,2)*SK_ACCY[4] - Kaccy*P(3,4)*SK_ACCY[5] + Kaccy*P(3,5)*SK_ACCY[8] + Kaccy*P(3,6)*SK_ACCY[7] + 2*Kaccy*P(3,22)*SK_ACCY[6] - Kaccy*P(3,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[4] = -SK_ACCY[0]*(Kaccy*P(4,0)*SK_ACCY[3] + Kaccy*P(4,1)*SK_ACCY[2] - Kaccy*P(4,3)*SK_ACCY[1] + Kaccy*P(4,2)*SK_ACCY[4] - Kaccy*P(4,4)*SK_ACCY[5] + Kaccy*P(4,5)*SK_ACCY[8] + Kaccy*P(4,6)*SK_ACCY[7] + 2*Kaccy*P(4,22)*SK_ACCY[6] - Kaccy*P(4,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[5] = -SK_ACCY[0]*(Kaccy*P(5,0)*SK_ACCY[3] + Kaccy*P(5,1)*SK_ACCY[2] - Kaccy*P(5,3)*SK_ACCY[1] + Kaccy*P(5,2)*SK_ACCY[4] - Kaccy*P(5,4)*SK_ACCY[5] + Kaccy*P(5,5)*SK_ACCY[8] + Kaccy*P(5,6)*SK_ACCY[7] + 2*Kaccy*P(5,22)*SK_ACCY[6] - Kaccy*P(5,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[6] = -SK_ACCY[0]*(Kaccy*P(6,0)*SK_ACCY[3] + Kaccy*P(6,1)*SK_ACCY[2] - Kaccy*P(6,3)*SK_ACCY[1] + Kaccy*P(6,2)*SK_ACCY[4] - Kaccy*P(6,4)*SK_ACCY[5] + Kaccy*P(6,5)*SK_ACCY[8] + Kaccy*P(6,6)*SK_ACCY[7] + 2*Kaccy*P(6,22)*SK_ACCY[6] - Kaccy*P(6,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[7] = -SK_ACCY[0]*(Kaccy*P(7,0)*SK_ACCY[3] + Kaccy*P(7,1)*SK_ACCY[2] - Kaccy*P(7,3)*SK_ACCY[1] + Kaccy*P(7,2)*SK_ACCY[4] - Kaccy*P(7,4)*SK_ACCY[5] + Kaccy*P(7,5)*SK_ACCY[8] + Kaccy*P(7,6)*SK_ACCY[7] + 2*Kaccy*P(7,22)*SK_ACCY[6] - Kaccy*P(7,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[8] = -SK_ACCY[0]*(Kaccy*P(8,0)*SK_ACCY[3] + Kaccy*P(8,1)*SK_ACCY[2] - Kaccy*P(8,3)*SK_ACCY[1] + Kaccy*P(8,2)*SK_ACCY[4] - Kaccy*P(8,4)*SK_ACCY[5] + Kaccy*P(8,5)*SK_ACCY[8] + Kaccy*P(8,6)*SK_ACCY[7] + 2*Kaccy*P(8,22)*SK_ACCY[6] - Kaccy*P(8,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[9] = -SK_ACCY[0]*(Kaccy*P(9,0)*SK_ACCY[3] + Kaccy*P(9,1)*SK_ACCY[2] - Kaccy*P(9,3)*SK_ACCY[1] + Kaccy*P(9,2)*SK_ACCY[4] - Kaccy*P(9,4)*SK_ACCY[5] + Kaccy*P(9,5)*SK_ACCY[8] + Kaccy*P(9,6)*SK_ACCY[7] + 2*Kaccy*P(9,22)*SK_ACCY[6] - Kaccy*P(9,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[10] = -SK_ACCY[0]*(Kaccy*P(10,0)*SK_ACCY[3] + Kaccy*P(10,1)*SK_ACCY[2] - Kaccy*P(10,3)*SK_ACCY[1] + Kaccy*P(10,2)*SK_ACCY[4] - Kaccy*P(10,4)*SK_ACCY[5] + Kaccy*P(10,5)*SK_ACCY[8] + Kaccy*P(10,6)*SK_ACCY[7] + 2*Kaccy*P(10,22)*SK_ACCY[6] - Kaccy*P(10,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[11] = -SK_ACCY[0]*(Kaccy*P(11,0)*SK_ACCY[3] + Kaccy*P(11,1)*SK_ACCY[2] - Kaccy*P(11,3)*SK_ACCY[1] + Kaccy*P(11,2)*SK_ACCY[4] - Kaccy*P(11,4)*SK_ACCY[5] + Kaccy*P(11,5)*SK_ACCY[8] + Kaccy*P(11,6)*SK_ACCY[7] + 2*Kaccy*P(11,22)*SK_ACCY[6] - Kaccy*P(11,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[12] = -SK_ACCY[0]*(Kaccy*P(12,0)*SK_ACCY[3] + Kaccy*P(12,1)*SK_ACCY[2] - Kaccy*P(12,3)*SK_ACCY[1] + Kaccy*P(12,2)*SK_ACCY[4] - Kaccy*P(12,4)*SK_ACCY[5] + Kaccy*P(12,5)*SK_ACCY[8] + Kaccy*P(12,6)*SK_ACCY[7] + 2*Kaccy*P(12,22)*SK_ACCY[6] - Kaccy*P(12,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[13] = -SK_ACCY[0]*(Kaccy*P(13,0)*SK_ACCY[3] + Kaccy*P(13,1)*SK_ACCY[2] - Kaccy*P(13,3)*SK_ACCY[1] + Kaccy*P(13,2)*SK_ACCY[4] - Kaccy*P(13,4)*SK_ACCY[5] + Kaccy*P(13,5)*SK_ACCY[8] + Kaccy*P(13,6)*SK_ACCY[7] + 2*Kaccy*P(13,22)*SK_ACCY[6] - Kaccy*P(13,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[14] = -SK_ACCY[0]*(Kaccy*P(14,0)*SK_ACCY[3] + Kaccy*P(14,1)*SK_ACCY[2] - Kaccy*P(14,3)*SK_ACCY[1] + Kaccy*P(14,2)*SK_ACCY[4] - Kaccy*P(14,4)*SK_ACCY[5] + Kaccy*P(14,5)*SK_ACCY[8] + Kaccy*P(14,6)*SK_ACCY[7] + 2*Kaccy*P(14,22)*SK_ACCY[6] - Kaccy*P(14,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[15] = -SK_ACCY[0]*(Kaccy*P(15,0)*SK_ACCY[3] + Kaccy*P(15,1)*SK_ACCY[2] - Kaccy*P(15,3)*SK_ACCY[1] + Kaccy*P(15,2)*SK_ACCY[4] - Kaccy*P(15,4)*SK_ACCY[5] + Kaccy*P(15,5)*SK_ACCY[8] + Kaccy*P(15,6)*SK_ACCY[7] + 2*Kaccy*P(15,22)*SK_ACCY[6] - Kaccy*P(15,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[16] = -SK_ACCY[0]*(Kaccy*P(16,0)*SK_ACCY[3] + Kaccy*P(16,1)*SK_ACCY[2] - Kaccy*P(16,3)*SK_ACCY[1] + Kaccy*P(16,2)*SK_ACCY[4] - Kaccy*P(16,4)*SK_ACCY[5] + Kaccy*P(16,5)*SK_ACCY[8] + Kaccy*P(16,6)*SK_ACCY[7] + 2*Kaccy*P(16,22)*SK_ACCY[6] - Kaccy*P(16,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[17] = -SK_ACCY[0]*(Kaccy*P(17,0)*SK_ACCY[3] + Kaccy*P(17,1)*SK_ACCY[2] - Kaccy*P(17,3)*SK_ACCY[1] + Kaccy*P(17,2)*SK_ACCY[4] - Kaccy*P(17,4)*SK_ACCY[5] + Kaccy*P(17,5)*SK_ACCY[8] + Kaccy*P(17,6)*SK_ACCY[7] + 2*Kaccy*P(17,22)*SK_ACCY[6] - Kaccy*P(17,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[18] = -SK_ACCY[0]*(Kaccy*P(18,0)*SK_ACCY[3] + Kaccy*P(18,1)*SK_ACCY[2] - Kaccy*P(18,3)*SK_ACCY[1] + Kaccy*P(18,2)*SK_ACCY[4] - Kaccy*P(18,4)*SK_ACCY[5] + Kaccy*P(18,5)*SK_ACCY[8] + Kaccy*P(18,6)*SK_ACCY[7] + 2*Kaccy*P(18,22)*SK_ACCY[6] - Kaccy*P(18,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[19] = -SK_ACCY[0]*(Kaccy*P(19,0)*SK_ACCY[3] + Kaccy*P(19,1)*SK_ACCY[2] - Kaccy*P(19,3)*SK_ACCY[1] + Kaccy*P(19,2)*SK_ACCY[4] - Kaccy*P(19,4)*SK_ACCY[5] + Kaccy*P(19,5)*SK_ACCY[8] + Kaccy*P(19,6)*SK_ACCY[7] + 2*Kaccy*P(19,22)*SK_ACCY[6] - Kaccy*P(19,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[20] = -SK_ACCY[0]*(Kaccy*P(20,0)*SK_ACCY[3] + Kaccy*P(20,1)*SK_ACCY[2] - Kaccy*P(20,3)*SK_ACCY[1] + Kaccy*P(20,2)*SK_ACCY[4] - Kaccy*P(20,4)*SK_ACCY[5] + Kaccy*P(20,5)*SK_ACCY[8] + Kaccy*P(20,6)*SK_ACCY[7] + 2*Kaccy*P(20,22)*SK_ACCY[6] - Kaccy*P(20,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[21] = -SK_ACCY[0]*(Kaccy*P(21,0)*SK_ACCY[3] + Kaccy*P(21,1)*SK_ACCY[2] - Kaccy*P(21,3)*SK_ACCY[1] + Kaccy*P(21,2)*SK_ACCY[4] - Kaccy*P(21,4)*SK_ACCY[5] + Kaccy*P(21,5)*SK_ACCY[8] + Kaccy*P(21,6)*SK_ACCY[7] + 2*Kaccy*P(21,22)*SK_ACCY[6] - Kaccy*P(21,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[22] = -SK_ACCY[0]*(Kaccy*P(22,0)*SK_ACCY[3] + Kaccy*P(22,1)*SK_ACCY[2] - Kaccy*P(22,3)*SK_ACCY[1] + Kaccy*P(22,2)*SK_ACCY[4] - Kaccy*P(22,4)*SK_ACCY[5] + Kaccy*P(22,5)*SK_ACCY[8] + Kaccy*P(22,6)*SK_ACCY[7] + 2*Kaccy*P(22,22)*SK_ACCY[6] - Kaccy*P(22,23)*SK_ACCY[8]);
|
||
|
|
Kfusion[23] = -SK_ACCY[0]*(Kaccy*P(23,0)*SK_ACCY[3] + Kaccy*P(23,1)*SK_ACCY[2] - Kaccy*P(23,3)*SK_ACCY[1] + Kaccy*P(23,2)*SK_ACCY[4] - Kaccy*P(23,4)*SK_ACCY[5] + Kaccy*P(23,5)*SK_ACCY[8] + Kaccy*P(23,6)*SK_ACCY[7] + 2*Kaccy*P(23,22)*SK_ACCY[6] - Kaccy*P(23,23)*SK_ACCY[8]);
|