dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
px4-firmware/matlab/scripts/Inertial Nav EKF/M_code24.txt

682 lines
82 KiB
Plaintext
Raw Blame History

SF = zeros(25,1);
SF(1) = daz_b/2 + dazNoise/2 - (daz*daz_s)/2;
SF(2) = day_b/2 + dayNoise/2 - (day*day_s)/2;
SF(3) = dax_b/2 + daxNoise/2 - (dax*dax_s)/2;
SF(4) = q3/2 - (q0*SF(1))/2 + (q1*SF(2))/2 - (q2*SF(3))/2;
SF(5) = q0/2 - (q1*SF(3))/2 - (q2*SF(2))/2 + (q3*SF(1))/2;
SF(6) = q1/2 + (q0*SF(3))/2 - (q2*SF(1))/2 - (q3*SF(2))/2;
SF(7) = q3/2 + (q0*SF(1))/2 - (q1*SF(2))/2 - (q2*SF(3))/2;
SF(8) = q0/2 - (q1*SF(3))/2 + (q2*SF(2))/2 - (q3*SF(1))/2;
SF(9) = q0/2 + (q1*SF(3))/2 - (q2*SF(2))/2 - (q3*SF(1))/2;
SF(10) = q2/2 + (q0*SF(2))/2 + (q1*SF(1))/2 + (q3*SF(3))/2;
SF(11) = q2/2 - (q0*SF(2))/2 - (q1*SF(1))/2 + (q3*SF(3))/2;
SF(12) = q2/2 + (q0*SF(2))/2 - (q1*SF(1))/2 - (q3*SF(3))/2;
SF(13) = q1/2 + (q0*SF(3))/2 + (q2*SF(1))/2 + (q3*SF(2))/2;
SF(14) = q1/2 - (q0*SF(3))/2 + (q2*SF(1))/2 - (q3*SF(2))/2;
SF(15) = q3/2 + (q0*SF(1))/2 + (q1*SF(2))/2 + (q2*SF(3))/2;
SF(16) = - q0^2 - q1^2 - q2^2 - q3^2;
SF(17) = dvz_b - dvz + dvzNoise;
SF(18) = dvx - dvxNoise;
SF(19) = dvy - dvyNoise;
SF(20) = q2^2;
SF(21) = SF(20) - q0^2 + q1^2 - q3^2;
SF(22) = SF(20) + q0^2 - q1^2 - q3^2;
SF(23) = 2*q0*q1 - 2*q2*q3;
SF(24) = SF(20) - q0^2 - q1^2 + q3^2;
SF(25) = 2*q1*q2;
SG = zeros(5,1);
SG(1) = - q0^2 - q1^2 - q2^2 - q3^2;
SG(2) = q3^2;
SG(3) = q2^2;
SG(4) = q1^2;
SG(5) = q0^2;
SQ = zeros(10,1);
SQ(1) = - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);
SQ(2) = dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvzNoise^2*(2*q0*q2 + 2*q1*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);
SQ(3) = dvyNoise^2*(2*q0*q3 - 2*q1*q2)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxNoise^2*(2*q0*q3 + 2*q1*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
SQ(4) = SG(1)^2;
SQ(5) = dvyNoise^2;
SQ(6) = dvzNoise^2;
SQ(7) = dvxNoise^2;
SQ(8) = 2*q2*q3;
SQ(9) = 2*q1*q3;
SQ(10) = 2*q1*q2;
SPP = zeros(23,1);
SPP(1) = SF(18)*(2*q0*q1 + 2*q2*q3) + SF(19)*(2*q0*q2 - 2*q1*q3);
SPP(2) = SF(19)*(2*q0*q2 + 2*q1*q3) + SF(17)*(SF(25) - 2*q0*q3);
SPP(3) = 2*q3*SF(9) + 2*q1*SF(12) - 2*q0*SF(15) - 2*q2*SF(14);
SPP(4) = 2*q1*SF(8) + 2*q2*SF(7) - 2*q0*SF(13) - 2*q3*SF(11);
SPP(5) = 2*q0*SF(7) - 2*q3*SF(8) - 2*q1*SF(11) + 2*q2*SF(13);
SPP(6) = 2*q0*SF(9) + 2*q2*SF(12) + 2*q1*SF(14) + 2*q3*SF(15);
SPP(7) = 2*q0*SF(8) + 2*q3*SF(7) + 2*q2*SF(11) + 2*q1*SF(13);
SPP(8) = 2*q1*SF(4) - 2*q2*SF(5) - 2*q3*SF(6) + 2*q0*SF(10);
SPP(9) = 2*q0*SF(6) - 2*q1*SF(5) - 2*q2*SF(4) + 2*q3*SF(10);
SPP(10) = SF(19)*SF(21) - SF(17)*(2*q0*q1 + 2*q2*q3);
SPP(11) = SF(18)*SF(21) + SF(17)*(2*q0*q2 - 2*q1*q3);
SPP(12) = SF(18)*SF(22) - SF(19)*(SF(25) + 2*q0*q3);
SPP(13) = SF(18)*SF(23) - SF(17)*(SF(25) + 2*q0*q3);
SPP(14) = 2*q0*SF(5) + 2*q1*SF(6) + 2*q3*SF(4) + 2*q2*SF(10);
SPP(15) = 2*q2*SF(9) - 2*q0*SF(12) - 2*q1*SF(15) + 2*q3*SF(14);
SPP(16) = SF(19)*SF(24) + SF(18)*(SF(25) - 2*q0*q3);
SPP(17) = daz*SF(20) + daz*q0^2 + daz*q1^2 + daz*q3^2;
SPP(18) = day*SF(20) + day*q0^2 + day*q1^2 + day*q3^2;
SPP(19) = dax*SF(20) + dax*q0^2 + dax*q1^2 + dax*q3^2;
SPP(20) = SF(17)*SF(24) - SF(18)*(2*q0*q2 + 2*q1*q3);
SPP(21) = SF(17)*SF(22) - SF(19)*SF(23);
SPP(22) = 2*q0*q2 + 2*q1*q3;
SPP(23) = SF(16);
nextP = zeros(24,24);
nextP(1,1) = SPP(6)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(5)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(8)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,10)*SPP(6) - OP(2,10)*SPP(5) + OP(3,10)*SPP(8) + OP(10,10)*SPP(23) + OP(13,10)*SPP(19)) + SPP(19)*(OP(1,13)*SPP(6) - OP(2,13)*SPP(5) + OP(3,13)*SPP(8) + OP(10,13)*SPP(23) + OP(13,13)*SPP(19)) + daxNoise^2*SQ(4);
nextP(1,2) = SPP(7)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) - SPP(3)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(9)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,11)*SPP(6) - OP(2,11)*SPP(5) + OP(3,11)*SPP(8) + OP(10,11)*SPP(23) + OP(13,11)*SPP(19)) + SPP(18)*(OP(1,14)*SPP(6) - OP(2,14)*SPP(5) + OP(3,14)*SPP(8) + OP(10,14)*SPP(23) + OP(13,14)*SPP(19));
nextP(2,2) = SPP(7)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) - SPP(3)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) - SPP(9)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(23)*(OP(2,11)*SPP(7) - OP(1,11)*SPP(3) - OP(3,11)*SPP(9) + OP(11,11)*SPP(23) + OP(14,11)*SPP(18)) + SPP(18)*(OP(2,14)*SPP(7) - OP(1,14)*SPP(3) - OP(3,14)*SPP(9) + OP(11,14)*SPP(23) + OP(14,14)*SPP(18)) + dayNoise^2*SQ(4);
nextP(1,3) = SPP(15)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(4)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(14)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,12)*SPP(6) - OP(2,12)*SPP(5) + OP(3,12)*SPP(8) + OP(10,12)*SPP(23) + OP(13,12)*SPP(19)) + SPP(17)*(OP(1,15)*SPP(6) - OP(2,15)*SPP(5) + OP(3,15)*SPP(8) + OP(10,15)*SPP(23) + OP(13,15)*SPP(19));
nextP(2,3) = SPP(15)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) - SPP(4)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(14)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(23)*(OP(2,12)*SPP(7) - OP(1,12)*SPP(3) - OP(3,12)*SPP(9) + OP(11,12)*SPP(23) + OP(14,12)*SPP(18)) + SPP(17)*(OP(2,15)*SPP(7) - OP(1,15)*SPP(3) - OP(3,15)*SPP(9) + OP(11,15)*SPP(23) + OP(14,15)*SPP(18));
nextP(3,3) = SPP(15)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) - SPP(4)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(14)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) + SPP(23)*(OP(1,12)*SPP(15) - OP(2,12)*SPP(4) + OP(3,12)*SPP(14) + OP(12,12)*SPP(23) + OP(15,12)*SPP(17)) + SPP(17)*(OP(1,15)*SPP(15) - OP(2,15)*SPP(4) + OP(3,15)*SPP(14) + OP(12,15)*SPP(23) + OP(15,15)*SPP(17)) + dazNoise^2*SQ(4);
nextP(1,4) = OP(1,4)*SPP(6) - OP(2,4)*SPP(5) + OP(3,4)*SPP(8) + OP(10,4)*SPP(23) + OP(13,4)*SPP(19) + SPP(2)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(20)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(16)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) - SPP(22)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19));
nextP(2,4) = OP(2,4)*SPP(7) - OP(1,4)*SPP(3) - OP(3,4)*SPP(9) + OP(11,4)*SPP(23) + OP(14,4)*SPP(18) + SPP(2)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(20)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(16)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) - SPP(22)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18));
nextP(3,4) = OP(1,4)*SPP(15) - OP(2,4)*SPP(4) + OP(3,4)*SPP(14) + OP(12,4)*SPP(23) + OP(15,4)*SPP(17) + SPP(2)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(20)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(16)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) - SPP(22)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17));
nextP(4,4) = OP(4,4) + OP(1,4)*SPP(2) + OP(2,4)*SPP(20) + OP(3,4)*SPP(16) - OP(16,4)*SPP(22) + SQ(6)*(SQ(9) + 2*q0*q2)^2 + SQ(5)*(SQ(10) - 2*q0*q3)^2 + SPP(2)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(20)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22)) + SPP(16)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22)) - SPP(22)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) + SQ(7)*(SG(2) + SG(3) - SG(4) - SG(5))^2;
nextP(1,5) = OP(1,5)*SPP(6) - OP(2,5)*SPP(5) + OP(3,5)*SPP(8) + OP(10,5)*SPP(23) + OP(13,5)*SPP(19) + SF(23)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19)) + SPP(13)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(21)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(12)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19));
nextP(2,5) = OP(2,5)*SPP(7) - OP(1,5)*SPP(3) - OP(3,5)*SPP(9) + OP(11,5)*SPP(23) + OP(14,5)*SPP(18) + SF(23)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18)) + SPP(13)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(21)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(12)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18));
nextP(3,5) = OP(1,5)*SPP(15) - OP(2,5)*SPP(4) + OP(3,5)*SPP(14) + OP(12,5)*SPP(23) + OP(15,5)*SPP(17) + SF(23)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17)) + SPP(13)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(21)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(12)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17));
nextP(4,5) = OP(4,5) + SQ(3) + OP(1,5)*SPP(2) + OP(2,5)*SPP(20) + OP(3,5)*SPP(16) - OP(16,5)*SPP(22) + SF(23)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) + SPP(13)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22)) + SPP(21)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(12)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22));
nextP(5,5) = OP(5,5) + OP(16,5)*SF(23) + OP(1,5)*SPP(21) + OP(2,5)*SPP(13) + OP(3,5)*SPP(12) + SQ(6)*(SQ(8) - 2*q0*q1)^2 + SQ(7)*(SQ(10) + 2*q0*q3)^2 + SF(23)*(OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12)) + SPP(13)*(OP(5,2) + OP(16,2)*SF(23) + OP(1,2)*SPP(21) + OP(2,2)*SPP(13) + OP(3,2)*SPP(12)) + SPP(21)*(OP(5,1) + OP(16,1)*SF(23) + OP(1,1)*SPP(21) + OP(2,1)*SPP(13) + OP(3,1)*SPP(12)) + SPP(12)*(OP(5,3) + OP(16,3)*SF(23) + OP(1,3)*SPP(21) + OP(2,3)*SPP(13) + OP(3,3)*SPP(12)) + SQ(5)*(SG(2) - SG(3) + SG(4) - SG(5))^2;
nextP(1,6) = OP(1,6)*SPP(6) - OP(2,6)*SPP(5) + OP(3,6)*SPP(8) + OP(10,6)*SPP(23) + OP(13,6)*SPP(19) + SF(21)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19)) - SPP(10)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(1)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(11)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19));
nextP(2,6) = OP(2,6)*SPP(7) - OP(1,6)*SPP(3) - OP(3,6)*SPP(9) + OP(11,6)*SPP(23) + OP(14,6)*SPP(18) + SF(21)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18)) - SPP(10)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(1)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(11)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18));
nextP(3,6) = OP(1,6)*SPP(15) - OP(2,6)*SPP(4) + OP(3,6)*SPP(14) + OP(12,6)*SPP(23) + OP(15,6)*SPP(17) + SF(21)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17)) - SPP(10)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(1)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) + SPP(11)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17));
nextP(4,6) = OP(4,6) + SQ(2) + OP(1,6)*SPP(2) + OP(2,6)*SPP(20) + OP(3,6)*SPP(16) - OP(16,6)*SPP(22) + SF(21)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) - SPP(10)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(1)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22)) + SPP(11)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22));
nextP(5,6) = OP(5,6) + SQ(1) + OP(16,6)*SF(23) + OP(1,6)*SPP(21) + OP(2,6)*SPP(13) + OP(3,6)*SPP(12) + SF(21)*(OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12)) - SPP(10)*(OP(5,1) + OP(16,1)*SF(23) + OP(1,1)*SPP(21) + OP(2,1)*SPP(13) + OP(3,1)*SPP(12)) + SPP(1)*(OP(5,3) + OP(16,3)*SF(23) + OP(1,3)*SPP(21) + OP(2,3)*SPP(13) + OP(3,3)*SPP(12)) + SPP(11)*(OP(5,2) + OP(16,2)*SF(23) + OP(1,2)*SPP(21) + OP(2,2)*SPP(13) + OP(3,2)*SPP(12));
nextP(6,6) = OP(6,6) + OP(16,6)*SF(21) - OP(1,6)*SPP(10) + OP(2,6)*SPP(11) + OP(3,6)*SPP(1) + SQ(5)*(SQ(8) + 2*q0*q1)^2 + SQ(7)*(SQ(9) - 2*q0*q2)^2 + SF(21)*(OP(6,16) + OP(16,16)*SF(21) - OP(1,16)*SPP(10) + OP(2,16)*SPP(11) + OP(3,16)*SPP(1)) - SPP(10)*(OP(6,1) + OP(16,1)*SF(21) - OP(1,1)*SPP(10) + OP(2,1)*SPP(11) + OP(3,1)*SPP(1)) + SPP(1)*(OP(6,3) + OP(16,3)*SF(21) - OP(1,3)*SPP(10) + OP(2,3)*SPP(11) + OP(3,3)*SPP(1)) + SPP(11)*(OP(6,2) + OP(16,2)*SF(21) - OP(1,2)*SPP(10) + OP(2,2)*SPP(11) + OP(3,2)*SPP(1)) + SQ(6)*(SG(2) - SG(3) - SG(4) + SG(5))^2;
nextP(1,7) = OP(1,7)*SPP(6) - OP(2,7)*SPP(5) + OP(3,7)*SPP(8) + OP(10,7)*SPP(23) + OP(13,7)*SPP(19) + dt*(OP(1,4)*SPP(6) - OP(2,4)*SPP(5) + OP(3,4)*SPP(8) + OP(10,4)*SPP(23) + OP(13,4)*SPP(19));
nextP(2,7) = OP(2,7)*SPP(7) - OP(1,7)*SPP(3) - OP(3,7)*SPP(9) + OP(11,7)*SPP(23) + OP(14,7)*SPP(18) + dt*(OP(2,4)*SPP(7) - OP(1,4)*SPP(3) - OP(3,4)*SPP(9) + OP(11,4)*SPP(23) + OP(14,4)*SPP(18));
nextP(3,7) = OP(1,7)*SPP(15) - OP(2,7)*SPP(4) + OP(3,7)*SPP(14) + OP(12,7)*SPP(23) + OP(15,7)*SPP(17) + dt*(OP(1,4)*SPP(15) - OP(2,4)*SPP(4) + OP(3,4)*SPP(14) + OP(12,4)*SPP(23) + OP(15,4)*SPP(17));
nextP(4,7) = OP(4,7) + OP(1,7)*SPP(2) + OP(2,7)*SPP(20) + OP(3,7)*SPP(16) - OP(16,7)*SPP(22) + dt*(OP(4,4) + OP(1,4)*SPP(2) + OP(2,4)*SPP(20) + OP(3,4)*SPP(16) - OP(16,4)*SPP(22));
nextP(5,7) = OP(5,7) + OP(16,7)*SF(23) + OP(1,7)*SPP(21) + OP(2,7)*SPP(13) + OP(3,7)*SPP(12) + dt*(OP(5,4) + OP(16,4)*SF(23) + OP(1,4)*SPP(21) + OP(2,4)*SPP(13) + OP(3,4)*SPP(12));
nextP(6,7) = OP(6,7) + OP(16,7)*SF(21) - OP(1,7)*SPP(10) + OP(2,7)*SPP(11) + OP(3,7)*SPP(1) + dt*(OP(6,4) + OP(16,4)*SF(21) - OP(1,4)*SPP(10) + OP(2,4)*SPP(11) + OP(3,4)*SPP(1));
nextP(7,7) = OP(7,7) + OP(4,7)*dt + dt*(OP(7,4) + OP(4,4)*dt);
nextP(1,8) = OP(1,8)*SPP(6) - OP(2,8)*SPP(5) + OP(3,8)*SPP(8) + OP(10,8)*SPP(23) + OP(13,8)*SPP(19) + dt*(OP(1,5)*SPP(6) - OP(2,5)*SPP(5) + OP(3,5)*SPP(8) + OP(10,5)*SPP(23) + OP(13,5)*SPP(19));
nextP(2,8) = OP(2,8)*SPP(7) - OP(1,8)*SPP(3) - OP(3,8)*SPP(9) + OP(11,8)*SPP(23) + OP(14,8)*SPP(18) + dt*(OP(2,5)*SPP(7) - OP(1,5)*SPP(3) - OP(3,5)*SPP(9) + OP(11,5)*SPP(23) + OP(14,5)*SPP(18));
nextP(3,8) = OP(1,8)*SPP(15) - OP(2,8)*SPP(4) + OP(3,8)*SPP(14) + OP(12,8)*SPP(23) + OP(15,8)*SPP(17) + dt*(OP(1,5)*SPP(15) - OP(2,5)*SPP(4) + OP(3,5)*SPP(14) + OP(12,5)*SPP(23) + OP(15,5)*SPP(17));
nextP(4,8) = OP(4,8) + OP(1,8)*SPP(2) + OP(2,8)*SPP(20) + OP(3,8)*SPP(16) - OP(16,8)*SPP(22) + dt*(OP(4,5) + OP(1,5)*SPP(2) + OP(2,5)*SPP(20) + OP(3,5)*SPP(16) - OP(16,5)*SPP(22));
nextP(5,8) = OP(5,8) + OP(16,8)*SF(23) + OP(1,8)*SPP(21) + OP(2,8)*SPP(13) + OP(3,8)*SPP(12) + dt*(OP(5,5) + OP(16,5)*SF(23) + OP(1,5)*SPP(21) + OP(2,5)*SPP(13) + OP(3,5)*SPP(12));
nextP(6,8) = OP(6,8) + OP(16,8)*SF(21) - OP(1,8)*SPP(10) + OP(2,8)*SPP(11) + OP(3,8)*SPP(1) + dt*(OP(6,5) + OP(16,5)*SF(21) - OP(1,5)*SPP(10) + OP(2,5)*SPP(11) + OP(3,5)*SPP(1));
nextP(7,8) = OP(7,8) + OP(4,8)*dt + dt*(OP(7,5) + OP(4,5)*dt);
nextP(8,8) = OP(8,8) + OP(5,8)*dt + dt*(OP(8,5) + OP(5,5)*dt);
nextP(1,9) = OP(1,9)*SPP(6) - OP(2,9)*SPP(5) + OP(3,9)*SPP(8) + OP(10,9)*SPP(23) + OP(13,9)*SPP(19) + dt*(OP(1,6)*SPP(6) - OP(2,6)*SPP(5) + OP(3,6)*SPP(8) + OP(10,6)*SPP(23) + OP(13,6)*SPP(19));
nextP(2,9) = OP(2,9)*SPP(7) - OP(1,9)*SPP(3) - OP(3,9)*SPP(9) + OP(11,9)*SPP(23) + OP(14,9)*SPP(18) + dt*(OP(2,6)*SPP(7) - OP(1,6)*SPP(3) - OP(3,6)*SPP(9) + OP(11,6)*SPP(23) + OP(14,6)*SPP(18));
nextP(3,9) = OP(1,9)*SPP(15) - OP(2,9)*SPP(4) + OP(3,9)*SPP(14) + OP(12,9)*SPP(23) + OP(15,9)*SPP(17) + dt*(OP(1,6)*SPP(15) - OP(2,6)*SPP(4) + OP(3,6)*SPP(14) + OP(12,6)*SPP(23) + OP(15,6)*SPP(17));
nextP(4,9) = OP(4,9) + OP(1,9)*SPP(2) + OP(2,9)*SPP(20) + OP(3,9)*SPP(16) - OP(16,9)*SPP(22) + dt*(OP(4,6) + OP(1,6)*SPP(2) + OP(2,6)*SPP(20) + OP(3,6)*SPP(16) - OP(16,6)*SPP(22));
nextP(5,9) = OP(5,9) + OP(16,9)*SF(23) + OP(1,9)*SPP(21) + OP(2,9)*SPP(13) + OP(3,9)*SPP(12) + dt*(OP(5,6) + OP(16,6)*SF(23) + OP(1,6)*SPP(21) + OP(2,6)*SPP(13) + OP(3,6)*SPP(12));
nextP(6,9) = OP(6,9) + OP(16,9)*SF(21) - OP(1,9)*SPP(10) + OP(2,9)*SPP(11) + OP(3,9)*SPP(1) + dt*(OP(6,6) + OP(16,6)*SF(21) - OP(1,6)*SPP(10) + OP(2,6)*SPP(11) + OP(3,6)*SPP(1));
nextP(7,9) = OP(7,9) + OP(4,9)*dt + dt*(OP(7,6) + OP(4,6)*dt);
nextP(8,9) = OP(8,9) + OP(5,9)*dt + dt*(OP(8,6) + OP(5,6)*dt);
nextP(9,9) = OP(9,9) + OP(6,9)*dt + dt*(OP(9,6) + OP(6,6)*dt);
nextP(1,10) = OP(1,10)*SPP(6) - OP(2,10)*SPP(5) + OP(3,10)*SPP(8) + OP(10,10)*SPP(23) + OP(13,10)*SPP(19);
nextP(2,10) = OP(2,10)*SPP(7) - OP(1,10)*SPP(3) - OP(3,10)*SPP(9) + OP(11,10)*SPP(23) + OP(14,10)*SPP(18);
nextP(3,10) = OP(1,10)*SPP(15) - OP(2,10)*SPP(4) + OP(3,10)*SPP(14) + OP(12,10)*SPP(23) + OP(15,10)*SPP(17);
nextP(4,10) = OP(4,10) + OP(1,10)*SPP(2) + OP(2,10)*SPP(20) + OP(3,10)*SPP(16) - OP(16,10)*SPP(22);
nextP(5,10) = OP(5,10) + OP(16,10)*SF(23) + OP(1,10)*SPP(21) + OP(2,10)*SPP(13) + OP(3,10)*SPP(12);
nextP(6,10) = OP(6,10) + OP(16,10)*SF(21) - OP(1,10)*SPP(10) + OP(2,10)*SPP(11) + OP(3,10)*SPP(1);
nextP(7,10) = OP(7,10) + OP(4,10)*dt;
nextP(8,10) = OP(8,10) + OP(5,10)*dt;
nextP(9,10) = OP(9,10) + OP(6,10)*dt;
nextP(10,10) = OP(10,10);
nextP(1,11) = OP(1,11)*SPP(6) - OP(2,11)*SPP(5) + OP(3,11)*SPP(8) + OP(10,11)*SPP(23) + OP(13,11)*SPP(19);
nextP(2,11) = OP(2,11)*SPP(7) - OP(1,11)*SPP(3) - OP(3,11)*SPP(9) + OP(11,11)*SPP(23) + OP(14,11)*SPP(18);
nextP(3,11) = OP(1,11)*SPP(15) - OP(2,11)*SPP(4) + OP(3,11)*SPP(14) + OP(12,11)*SPP(23) + OP(15,11)*SPP(17);
nextP(4,11) = OP(4,11) + OP(1,11)*SPP(2) + OP(2,11)*SPP(20) + OP(3,11)*SPP(16) - OP(16,11)*SPP(22);
nextP(5,11) = OP(5,11) + OP(16,11)*SF(23) + OP(1,11)*SPP(21) + OP(2,11)*SPP(13) + OP(3,11)*SPP(12);
nextP(6,11) = OP(6,11) + OP(16,11)*SF(21) - OP(1,11)*SPP(10) + OP(2,11)*SPP(11) + OP(3,11)*SPP(1);
nextP(7,11) = OP(7,11) + OP(4,11)*dt;
nextP(8,11) = OP(8,11) + OP(5,11)*dt;
nextP(9,11) = OP(9,11) + OP(6,11)*dt;
nextP(10,11) = OP(10,11);
nextP(11,11) = OP(11,11);
nextP(1,12) = OP(1,12)*SPP(6) - OP(2,12)*SPP(5) + OP(3,12)*SPP(8) + OP(10,12)*SPP(23) + OP(13,12)*SPP(19);
nextP(2,12) = OP(2,12)*SPP(7) - OP(1,12)*SPP(3) - OP(3,12)*SPP(9) + OP(11,12)*SPP(23) + OP(14,12)*SPP(18);
nextP(3,12) = OP(1,12)*SPP(15) - OP(2,12)*SPP(4) + OP(3,12)*SPP(14) + OP(12,12)*SPP(23) + OP(15,12)*SPP(17);
nextP(4,12) = OP(4,12) + OP(1,12)*SPP(2) + OP(2,12)*SPP(20) + OP(3,12)*SPP(16) - OP(16,12)*SPP(22);
nextP(5,12) = OP(5,12) + OP(16,12)*SF(23) + OP(1,12)*SPP(21) + OP(2,12)*SPP(13) + OP(3,12)*SPP(12);
nextP(6,12) = OP(6,12) + OP(16,12)*SF(21) - OP(1,12)*SPP(10) + OP(2,12)*SPP(11) + OP(3,12)*SPP(1);
nextP(7,12) = OP(7,12) + OP(4,12)*dt;
nextP(8,12) = OP(8,12) + OP(5,12)*dt;
nextP(9,12) = OP(9,12) + OP(6,12)*dt;
nextP(10,12) = OP(10,12);
nextP(11,12) = OP(11,12);
nextP(12,12) = OP(12,12);
nextP(1,13) = OP(1,13)*SPP(6) - OP(2,13)*SPP(5) + OP(3,13)*SPP(8) + OP(10,13)*SPP(23) + OP(13,13)*SPP(19);
nextP(2,13) = OP(2,13)*SPP(7) - OP(1,13)*SPP(3) - OP(3,13)*SPP(9) + OP(11,13)*SPP(23) + OP(14,13)*SPP(18);
nextP(3,13) = OP(1,13)*SPP(15) - OP(2,13)*SPP(4) + OP(3,13)*SPP(14) + OP(12,13)*SPP(23) + OP(15,13)*SPP(17);
nextP(4,13) = OP(4,13) + OP(1,13)*SPP(2) + OP(2,13)*SPP(20) + OP(3,13)*SPP(16) - OP(16,13)*SPP(22);
nextP(5,13) = OP(5,13) + OP(16,13)*SF(23) + OP(1,13)*SPP(21) + OP(2,13)*SPP(13) + OP(3,13)*SPP(12);
nextP(6,13) = OP(6,13) + OP(16,13)*SF(21) - OP(1,13)*SPP(10) + OP(2,13)*SPP(11) + OP(3,13)*SPP(1);
nextP(7,13) = OP(7,13) + OP(4,13)*dt;
nextP(8,13) = OP(8,13) + OP(5,13)*dt;
nextP(9,13) = OP(9,13) + OP(6,13)*dt;
nextP(10,13) = OP(10,13);
nextP(11,13) = OP(11,13);
nextP(12,13) = OP(12,13);
nextP(13,13) = OP(13,13);
nextP(1,14) = OP(1,14)*SPP(6) - OP(2,14)*SPP(5) + OP(3,14)*SPP(8) + OP(10,14)*SPP(23) + OP(13,14)*SPP(19);
nextP(2,14) = OP(2,14)*SPP(7) - OP(1,14)*SPP(3) - OP(3,14)*SPP(9) + OP(11,14)*SPP(23) + OP(14,14)*SPP(18);
nextP(3,14) = OP(1,14)*SPP(15) - OP(2,14)*SPP(4) + OP(3,14)*SPP(14) + OP(12,14)*SPP(23) + OP(15,14)*SPP(17);
nextP(4,14) = OP(4,14) + OP(1,14)*SPP(2) + OP(2,14)*SPP(20) + OP(3,14)*SPP(16) - OP(16,14)*SPP(22);
nextP(5,14) = OP(5,14) + OP(16,14)*SF(23) + OP(1,14)*SPP(21) + OP(2,14)*SPP(13) + OP(3,14)*SPP(12);
nextP(6,14) = OP(6,14) + OP(16,14)*SF(21) - OP(1,14)*SPP(10) + OP(2,14)*SPP(11) + OP(3,14)*SPP(1);
nextP(7,14) = OP(7,14) + OP(4,14)*dt;
nextP(8,14) = OP(8,14) + OP(5,14)*dt;
nextP(9,14) = OP(9,14) + OP(6,14)*dt;
nextP(10,14) = OP(10,14);
nextP(11,14) = OP(11,14);
nextP(12,14) = OP(12,14);
nextP(13,14) = OP(13,14);
nextP(14,14) = OP(14,14);
nextP(1,15) = OP(1,15)*SPP(6) - OP(2,15)*SPP(5) + OP(3,15)*SPP(8) + OP(10,15)*SPP(23) + OP(13,15)*SPP(19);
nextP(2,15) = OP(2,15)*SPP(7) - OP(1,15)*SPP(3) - OP(3,15)*SPP(9) + OP(11,15)*SPP(23) + OP(14,15)*SPP(18);
nextP(3,15) = OP(1,15)*SPP(15) - OP(2,15)*SPP(4) + OP(3,15)*SPP(14) + OP(12,15)*SPP(23) + OP(15,15)*SPP(17);
nextP(4,15) = OP(4,15) + OP(1,15)*SPP(2) + OP(2,15)*SPP(20) + OP(3,15)*SPP(16) - OP(16,15)*SPP(22);
nextP(5,15) = OP(5,15) + OP(16,15)*SF(23) + OP(1,15)*SPP(21) + OP(2,15)*SPP(13) + OP(3,15)*SPP(12);
nextP(6,15) = OP(6,15) + OP(16,15)*SF(21) - OP(1,15)*SPP(10) + OP(2,15)*SPP(11) + OP(3,15)*SPP(1);
nextP(7,15) = OP(7,15) + OP(4,15)*dt;
nextP(8,15) = OP(8,15) + OP(5,15)*dt;
nextP(9,15) = OP(9,15) + OP(6,15)*dt;
nextP(10,15) = OP(10,15);
nextP(11,15) = OP(11,15);
nextP(12,15) = OP(12,15);
nextP(13,15) = OP(13,15);
nextP(14,15) = OP(14,15);
nextP(15,15) = OP(15,15);
nextP(1,16) = OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19);
nextP(2,16) = OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18);
nextP(3,16) = OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17);
nextP(4,16) = OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22);
nextP(5,16) = OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12);
nextP(6,16) = OP(6,16) + OP(16,16)*SF(21) - OP(1,16)*SPP(10) + OP(2,16)*SPP(11) + OP(3,16)*SPP(1);
nextP(7,16) = OP(7,16) + OP(4,16)*dt;
nextP(8,16) = OP(8,16) + OP(5,16)*dt;
nextP(9,16) = OP(9,16) + OP(6,16)*dt;
nextP(10,16) = OP(10,16);
nextP(11,16) = OP(11,16);
nextP(12,16) = OP(12,16);
nextP(13,16) = OP(13,16);
nextP(14,16) = OP(14,16);
nextP(15,16) = OP(15,16);
nextP(16,16) = OP(16,16);
nextP(1,17) = OP(1,17)*SPP(6) - OP(2,17)*SPP(5) + OP(3,17)*SPP(8) + OP(10,17)*SPP(23) + OP(13,17)*SPP(19);
nextP(2,17) = OP(2,17)*SPP(7) - OP(1,17)*SPP(3) - OP(3,17)*SPP(9) + OP(11,17)*SPP(23) + OP(14,17)*SPP(18);
nextP(3,17) = OP(1,17)*SPP(15) - OP(2,17)*SPP(4) + OP(3,17)*SPP(14) + OP(12,17)*SPP(23) + OP(15,17)*SPP(17);
nextP(4,17) = OP(4,17) + OP(1,17)*SPP(2) + OP(2,17)*SPP(20) + OP(3,17)*SPP(16) - OP(16,17)*SPP(22);
nextP(5,17) = OP(5,17) + OP(16,17)*SF(23) + OP(1,17)*SPP(21) + OP(2,17)*SPP(13) + OP(3,17)*SPP(12);
nextP(6,17) = OP(6,17) + OP(16,17)*SF(21) - OP(1,17)*SPP(10) + OP(2,17)*SPP(11) + OP(3,17)*SPP(1);
nextP(7,17) = OP(7,17) + OP(4,17)*dt;
nextP(8,17) = OP(8,17) + OP(5,17)*dt;
nextP(9,17) = OP(9,17) + OP(6,17)*dt;
nextP(10,17) = OP(10,17);
nextP(11,17) = OP(11,17);
nextP(12,17) = OP(12,17);
nextP(13,17) = OP(13,17);
nextP(14,17) = OP(14,17);
nextP(15,17) = OP(15,17);
nextP(16,17) = OP(16,17);
nextP(17,17) = OP(17,17);
nextP(1,18) = OP(1,18)*SPP(6) - OP(2,18)*SPP(5) + OP(3,18)*SPP(8) + OP(10,18)*SPP(23) + OP(13,18)*SPP(19);
nextP(2,18) = OP(2,18)*SPP(7) - OP(1,18)*SPP(3) - OP(3,18)*SPP(9) + OP(11,18)*SPP(23) + OP(14,18)*SPP(18);
nextP(3,18) = OP(1,18)*SPP(15) - OP(2,18)*SPP(4) + OP(3,18)*SPP(14) + OP(12,18)*SPP(23) + OP(15,18)*SPP(17);
nextP(4,18) = OP(4,18) + OP(1,18)*SPP(2) + OP(2,18)*SPP(20) + OP(3,18)*SPP(16) - OP(16,18)*SPP(22);
nextP(5,18) = OP(5,18) + OP(16,18)*SF(23) + OP(1,18)*SPP(21) + OP(2,18)*SPP(13) + OP(3,18)*SPP(12);
nextP(6,18) = OP(6,18) + OP(16,18)*SF(21) - OP(1,18)*SPP(10) + OP(2,18)*SPP(11) + OP(3,18)*SPP(1);
nextP(7,18) = OP(7,18) + OP(4,18)*dt;
nextP(8,18) = OP(8,18) + OP(5,18)*dt;
nextP(9,18) = OP(9,18) + OP(6,18)*dt;
nextP(10,18) = OP(10,18);
nextP(11,18) = OP(11,18);
nextP(12,18) = OP(12,18);
nextP(13,18) = OP(13,18);
nextP(14,18) = OP(14,18);
nextP(15,18) = OP(15,18);
nextP(16,18) = OP(16,18);
nextP(17,18) = OP(17,18);
nextP(18,18) = OP(18,18);
nextP(1,19) = OP(1,19)*SPP(6) - OP(2,19)*SPP(5) + OP(3,19)*SPP(8) + OP(10,19)*SPP(23) + OP(13,19)*SPP(19);
nextP(2,19) = OP(2,19)*SPP(7) - OP(1,19)*SPP(3) - OP(3,19)*SPP(9) + OP(11,19)*SPP(23) + OP(14,19)*SPP(18);
nextP(3,19) = OP(1,19)*SPP(15) - OP(2,19)*SPP(4) + OP(3,19)*SPP(14) + OP(12,19)*SPP(23) + OP(15,19)*SPP(17);
nextP(4,19) = OP(4,19) + OP(1,19)*SPP(2) + OP(2,19)*SPP(20) + OP(3,19)*SPP(16) - OP(16,19)*SPP(22);
nextP(5,19) = OP(5,19) + OP(16,19)*SF(23) + OP(1,19)*SPP(21) + OP(2,19)*SPP(13) + OP(3,19)*SPP(12);
nextP(6,19) = OP(6,19) + OP(16,19)*SF(21) - OP(1,19)*SPP(10) + OP(2,19)*SPP(11) + OP(3,19)*SPP(1);
nextP(7,19) = OP(7,19) + OP(4,19)*dt;
nextP(8,19) = OP(8,19) + OP(5,19)*dt;
nextP(9,19) = OP(9,19) + OP(6,19)*dt;
nextP(10,19) = OP(10,19);
nextP(11,19) = OP(11,19);
nextP(12,19) = OP(12,19);
nextP(13,19) = OP(13,19);
nextP(14,19) = OP(14,19);
nextP(15,19) = OP(15,19);
nextP(16,19) = OP(16,19);
nextP(17,19) = OP(17,19);
nextP(18,19) = OP(18,19);
nextP(19,19) = OP(19,19);
nextP(1,20) = OP(1,20)*SPP(6) - OP(2,20)*SPP(5) + OP(3,20)*SPP(8) + OP(10,20)*SPP(23) + OP(13,20)*SPP(19);
nextP(2,20) = OP(2,20)*SPP(7) - OP(1,20)*SPP(3) - OP(3,20)*SPP(9) + OP(11,20)*SPP(23) + OP(14,20)*SPP(18);
nextP(3,20) = OP(1,20)*SPP(15) - OP(2,20)*SPP(4) + OP(3,20)*SPP(14) + OP(12,20)*SPP(23) + OP(15,20)*SPP(17);
nextP(4,20) = OP(4,20) + OP(1,20)*SPP(2) + OP(2,20)*SPP(20) + OP(3,20)*SPP(16) - OP(16,20)*SPP(22);
nextP(5,20) = OP(5,20) + OP(16,20)*SF(23) + OP(1,20)*SPP(21) + OP(2,20)*SPP(13) + OP(3,20)*SPP(12);
nextP(6,20) = OP(6,20) + OP(16,20)*SF(21) - OP(1,20)*SPP(10) + OP(2,20)*SPP(11) + OP(3,20)*SPP(1);
nextP(7,20) = OP(7,20) + OP(4,20)*dt;
nextP(8,20) = OP(8,20) + OP(5,20)*dt;
nextP(9,20) = OP(9,20) + OP(6,20)*dt;
nextP(10,20) = OP(10,20);
nextP(11,20) = OP(11,20);
nextP(12,20) = OP(12,20);
nextP(13,20) = OP(13,20);
nextP(14,20) = OP(14,20);
nextP(15,20) = OP(15,20);
nextP(16,20) = OP(16,20);
nextP(17,20) = OP(17,20);
nextP(18,20) = OP(18,20);
nextP(19,20) = OP(19,20);
nextP(20,20) = OP(20,20);
nextP(1,21) = OP(1,21)*SPP(6) - OP(2,21)*SPP(5) + OP(3,21)*SPP(8) + OP(10,21)*SPP(23) + OP(13,21)*SPP(19);
nextP(2,21) = OP(2,21)*SPP(7) - OP(1,21)*SPP(3) - OP(3,21)*SPP(9) + OP(11,21)*SPP(23) + OP(14,21)*SPP(18);
nextP(3,21) = OP(1,21)*SPP(15) - OP(2,21)*SPP(4) + OP(3,21)*SPP(14) + OP(12,21)*SPP(23) + OP(15,21)*SPP(17);
nextP(4,21) = OP(4,21) + OP(1,21)*SPP(2) + OP(2,21)*SPP(20) + OP(3,21)*SPP(16) - OP(16,21)*SPP(22);
nextP(5,21) = OP(5,21) + OP(16,21)*SF(23) + OP(1,21)*SPP(21) + OP(2,21)*SPP(13) + OP(3,21)*SPP(12);
nextP(6,21) = OP(6,21) + OP(16,21)*SF(21) - OP(1,21)*SPP(10) + OP(2,21)*SPP(11) + OP(3,21)*SPP(1);
nextP(7,21) = OP(7,21) + OP(4,21)*dt;
nextP(8,21) = OP(8,21) + OP(5,21)*dt;
nextP(9,21) = OP(9,21) + OP(6,21)*dt;
nextP(10,21) = OP(10,21);
nextP(11,21) = OP(11,21);
nextP(12,21) = OP(12,21);
nextP(13,21) = OP(13,21);
nextP(14,21) = OP(14,21);
nextP(15,21) = OP(15,21);
nextP(16,21) = OP(16,21);
nextP(17,21) = OP(17,21);
nextP(18,21) = OP(18,21);
nextP(19,21) = OP(19,21);
nextP(20,21) = OP(20,21);
nextP(21,21) = OP(21,21);
nextP(1,22) = OP(1,22)*SPP(6) - OP(2,22)*SPP(5) + OP(3,22)*SPP(8) + OP(10,22)*SPP(23) + OP(13,22)*SPP(19);
nextP(2,22) = OP(2,22)*SPP(7) - OP(1,22)*SPP(3) - OP(3,22)*SPP(9) + OP(11,22)*SPP(23) + OP(14,22)*SPP(18);
nextP(3,22) = OP(1,22)*SPP(15) - OP(2,22)*SPP(4) + OP(3,22)*SPP(14) + OP(12,22)*SPP(23) + OP(15,22)*SPP(17);
nextP(4,22) = OP(4,22) + OP(1,22)*SPP(2) + OP(2,22)*SPP(20) + OP(3,22)*SPP(16) - OP(16,22)*SPP(22);
nextP(5,22) = OP(5,22) + OP(16,22)*SF(23) + OP(1,22)*SPP(21) + OP(2,22)*SPP(13) + OP(3,22)*SPP(12);
nextP(6,22) = OP(6,22) + OP(16,22)*SF(21) - OP(1,22)*SPP(10) + OP(2,22)*SPP(11) + OP(3,22)*SPP(1);
nextP(7,22) = OP(7,22) + OP(4,22)*dt;
nextP(8,22) = OP(8,22) + OP(5,22)*dt;
nextP(9,22) = OP(9,22) + OP(6,22)*dt;
nextP(10,22) = OP(10,22);
nextP(11,22) = OP(11,22);
nextP(12,22) = OP(12,22);
nextP(13,22) = OP(13,22);
nextP(14,22) = OP(14,22);
nextP(15,22) = OP(15,22);
nextP(16,22) = OP(16,22);
nextP(17,22) = OP(17,22);
nextP(18,22) = OP(18,22);
nextP(19,22) = OP(19,22);
nextP(20,22) = OP(20,22);
nextP(21,22) = OP(21,22);
nextP(22,22) = OP(22,22);
nextP(1,23) = OP(1,23)*SPP(6) - OP(2,23)*SPP(5) + OP(3,23)*SPP(8) + OP(10,23)*SPP(23) + OP(13,23)*SPP(19);
nextP(2,23) = OP(2,23)*SPP(7) - OP(1,23)*SPP(3) - OP(3,23)*SPP(9) + OP(11,23)*SPP(23) + OP(14,23)*SPP(18);
nextP(3,23) = OP(1,23)*SPP(15) - OP(2,23)*SPP(4) + OP(3,23)*SPP(14) + OP(12,23)*SPP(23) + OP(15,23)*SPP(17);
nextP(4,23) = OP(4,23) + OP(1,23)*SPP(2) + OP(2,23)*SPP(20) + OP(3,23)*SPP(16) - OP(16,23)*SPP(22);
nextP(5,23) = OP(5,23) + OP(16,23)*SF(23) + OP(1,23)*SPP(21) + OP(2,23)*SPP(13) + OP(3,23)*SPP(12);
nextP(6,23) = OP(6,23) + OP(16,23)*SF(21) - OP(1,23)*SPP(10) + OP(2,23)*SPP(11) + OP(3,23)*SPP(1);
nextP(7,23) = OP(7,23) + OP(4,23)*dt;
nextP(8,23) = OP(8,23) + OP(5,23)*dt;
nextP(9,23) = OP(9,23) + OP(6,23)*dt;
nextP(10,23) = OP(10,23);
nextP(11,23) = OP(11,23);
nextP(12,23) = OP(12,23);
nextP(13,23) = OP(13,23);
nextP(14,23) = OP(14,23);
nextP(15,23) = OP(15,23);
nextP(16,23) = OP(16,23);
nextP(17,23) = OP(17,23);
nextP(18,23) = OP(18,23);
nextP(19,23) = OP(19,23);
nextP(20,23) = OP(20,23);
nextP(21,23) = OP(21,23);
nextP(22,23) = OP(22,23);
nextP(23,23) = OP(23,23);
nextP(1,24) = OP(1,24)*SPP(6) - OP(2,24)*SPP(5) + OP(3,24)*SPP(8) + OP(10,24)*SPP(23) + OP(13,24)*SPP(19);
nextP(2,24) = OP(2,24)*SPP(7) - OP(1,24)*SPP(3) - OP(3,24)*SPP(9) + OP(11,24)*SPP(23) + OP(14,24)*SPP(18);
nextP(3,24) = OP(1,24)*SPP(15) - OP(2,24)*SPP(4) + OP(3,24)*SPP(14) + OP(12,24)*SPP(23) + OP(15,24)*SPP(17);
nextP(4,24) = OP(4,24) + OP(1,24)*SPP(2) + OP(2,24)*SPP(20) + OP(3,24)*SPP(16) - OP(16,24)*SPP(22);
nextP(5,24) = OP(5,24) + OP(16,24)*SF(23) + OP(1,24)*SPP(21) + OP(2,24)*SPP(13) + OP(3,24)*SPP(12);
nextP(6,24) = OP(6,24) + OP(16,24)*SF(21) - OP(1,24)*SPP(10) + OP(2,24)*SPP(11) + OP(3,24)*SPP(1);
nextP(7,24) = OP(7,24) + OP(4,24)*dt;
nextP(8,24) = OP(8,24) + OP(5,24)*dt;
nextP(9,24) = OP(9,24) + OP(6,24)*dt;
nextP(10,24) = OP(10,24);
nextP(11,24) = OP(11,24);
nextP(12,24) = OP(12,24);
nextP(13,24) = OP(13,24);
nextP(14,24) = OP(14,24);
nextP(15,24) = OP(15,24);
nextP(16,24) = OP(16,24);
nextP(17,24) = OP(17,24);
nextP(18,24) = OP(18,24);
nextP(19,24) = OP(19,24);
nextP(20,24) = OP(20,24);
nextP(21,24) = OP(21,24);
nextP(22,24) = OP(22,24);
nextP(23,24) = OP(23,24);
nextP(24,24) = OP(24,24);
SH_TAS = zeros(3,1);
SH_TAS(1) = 1/((ve - vwe)^2 + (vn - vwn)^2 + vd^2)^(1/2);
SH_TAS(2) = (SH_TAS(1)*(2*ve - 2*vwe))/2;
SH_TAS(3) = (SH_TAS(1)*(2*vn - 2*vwn))/2;
H_TAS = zeros(1,24);
H_TAS(1,4) = SH_TAS(3);
H_TAS(1,5) = SH_TAS(2);
H_TAS(1,6) = vd*SH_TAS(1);
H_TAS(1,23) = -SH_TAS(3);
H_TAS(1,24) = -SH_TAS(2);
SK_TAS = zeros(2,1);
SK_TAS(1) = 1/(R_TAS + SH_TAS(3)*(OP(4,4)*SH_TAS(3) + OP(5,4)*SH_TAS(2) - OP(23,4)*SH_TAS(3) - OP(24,4)*SH_TAS(2) + OP(6,4)*vd*SH_TAS(1)) + SH_TAS(2)*(OP(4,5)*SH_TAS(3) + OP(5,5)*SH_TAS(2) - OP(23,5)*SH_TAS(3) - OP(24,5)*SH_TAS(2) + OP(6,5)*vd*SH_TAS(1)) - SH_TAS(3)*(OP(4,23)*SH_TAS(3) + OP(5,23)*SH_TAS(2) - OP(23,23)*SH_TAS(3) - OP(24,23)*SH_TAS(2) + OP(6,23)*vd*SH_TAS(1)) - SH_TAS(2)*(OP(4,24)*SH_TAS(3) + OP(5,24)*SH_TAS(2) - OP(23,24)*SH_TAS(3) - OP(24,24)*SH_TAS(2) + OP(6,24)*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP(4,6)*SH_TAS(3) + OP(5,6)*SH_TAS(2) - OP(23,6)*SH_TAS(3) - OP(24,6)*SH_TAS(2) + OP(6,6)*vd*SH_TAS(1)));
SK_TAS(2) = SH_TAS(2);
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_TAS(1)*(OP(1,4)*SH_TAS(3) - OP(1,23)*SH_TAS(3) + OP(1,5)*SK_TAS(2) - OP(1,24)*SK_TAS(2) + OP(1,6)*vd*SH_TAS(1));
Kfusion(2) = SK_TAS(1)*(OP(2,4)*SH_TAS(3) - OP(2,23)*SH_TAS(3) + OP(2,5)*SK_TAS(2) - OP(2,24)*SK_TAS(2) + OP(2,6)*vd*SH_TAS(1));
Kfusion(3) = SK_TAS(1)*(OP(3,4)*SH_TAS(3) - OP(3,23)*SH_TAS(3) + OP(3,5)*SK_TAS(2) - OP(3,24)*SK_TAS(2) + OP(3,6)*vd*SH_TAS(1));
Kfusion(4) = SK_TAS(1)*(OP(4,4)*SH_TAS(3) - OP(4,23)*SH_TAS(3) + OP(4,5)*SK_TAS(2) - OP(4,24)*SK_TAS(2) + OP(4,6)*vd*SH_TAS(1));
Kfusion(5) = SK_TAS(1)*(OP(5,4)*SH_TAS(3) - OP(5,23)*SH_TAS(3) + OP(5,5)*SK_TAS(2) - OP(5,24)*SK_TAS(2) + OP(5,6)*vd*SH_TAS(1));
Kfusion(6) = SK_TAS(1)*(OP(6,4)*SH_TAS(3) - OP(6,23)*SH_TAS(3) + OP(6,5)*SK_TAS(2) - OP(6,24)*SK_TAS(2) + OP(6,6)*vd*SH_TAS(1));
Kfusion(7) = SK_TAS(1)*(OP(7,4)*SH_TAS(3) - OP(7,23)*SH_TAS(3) + OP(7,5)*SK_TAS(2) - OP(7,24)*SK_TAS(2) + OP(7,6)*vd*SH_TAS(1));
Kfusion(8) = SK_TAS(1)*(OP(8,4)*SH_TAS(3) - OP(8,23)*SH_TAS(3) + OP(8,5)*SK_TAS(2) - OP(8,24)*SK_TAS(2) + OP(8,6)*vd*SH_TAS(1));
Kfusion(9) = SK_TAS(1)*(OP(9,4)*SH_TAS(3) - OP(9,23)*SH_TAS(3) + OP(9,5)*SK_TAS(2) - OP(9,24)*SK_TAS(2) + OP(9,6)*vd*SH_TAS(1));
Kfusion(10) = SK_TAS(1)*(OP(10,4)*SH_TAS(3) - OP(10,23)*SH_TAS(3) + OP(10,5)*SK_TAS(2) - OP(10,24)*SK_TAS(2) + OP(10,6)*vd*SH_TAS(1));
Kfusion(11) = SK_TAS(1)*(OP(11,4)*SH_TAS(3) - OP(11,23)*SH_TAS(3) + OP(11,5)*SK_TAS(2) - OP(11,24)*SK_TAS(2) + OP(11,6)*vd*SH_TAS(1));
Kfusion(12) = SK_TAS(1)*(OP(12,4)*SH_TAS(3) - OP(12,23)*SH_TAS(3) + OP(12,5)*SK_TAS(2) - OP(12,24)*SK_TAS(2) + OP(12,6)*vd*SH_TAS(1));
Kfusion(13) = SK_TAS(1)*(OP(13,4)*SH_TAS(3) - OP(13,23)*SH_TAS(3) + OP(13,5)*SK_TAS(2) - OP(13,24)*SK_TAS(2) + OP(13,6)*vd*SH_TAS(1));
Kfusion(14) = SK_TAS(1)*(OP(14,4)*SH_TAS(3) - OP(14,23)*SH_TAS(3) + OP(14,5)*SK_TAS(2) - OP(14,24)*SK_TAS(2) + OP(14,6)*vd*SH_TAS(1));
Kfusion(15) = SK_TAS(1)*(OP(15,4)*SH_TAS(3) - OP(15,23)*SH_TAS(3) + OP(15,5)*SK_TAS(2) - OP(15,24)*SK_TAS(2) + OP(15,6)*vd*SH_TAS(1));
Kfusion(16) = SK_TAS(1)*(OP(16,4)*SH_TAS(3) - OP(16,23)*SH_TAS(3) + OP(16,5)*SK_TAS(2) - OP(16,24)*SK_TAS(2) + OP(16,6)*vd*SH_TAS(1));
Kfusion(17) = SK_TAS(1)*(OP(17,4)*SH_TAS(3) - OP(17,23)*SH_TAS(3) + OP(17,5)*SK_TAS(2) - OP(17,24)*SK_TAS(2) + OP(17,6)*vd*SH_TAS(1));
Kfusion(18) = SK_TAS(1)*(OP(18,4)*SH_TAS(3) - OP(18,23)*SH_TAS(3) + OP(18,5)*SK_TAS(2) - OP(18,24)*SK_TAS(2) + OP(18,6)*vd*SH_TAS(1));
Kfusion(19) = SK_TAS(1)*(OP(19,4)*SH_TAS(3) - OP(19,23)*SH_TAS(3) + OP(19,5)*SK_TAS(2) - OP(19,24)*SK_TAS(2) + OP(19,6)*vd*SH_TAS(1));
Kfusion(20) = SK_TAS(1)*(OP(20,4)*SH_TAS(3) - OP(20,23)*SH_TAS(3) + OP(20,5)*SK_TAS(2) - OP(20,24)*SK_TAS(2) + OP(20,6)*vd*SH_TAS(1));
Kfusion(21) = SK_TAS(1)*(OP(21,4)*SH_TAS(3) - OP(21,23)*SH_TAS(3) + OP(21,5)*SK_TAS(2) - OP(21,24)*SK_TAS(2) + OP(21,6)*vd*SH_TAS(1));
Kfusion(22) = SK_TAS(1)*(OP(22,4)*SH_TAS(3) - OP(22,23)*SH_TAS(3) + OP(22,5)*SK_TAS(2) - OP(22,24)*SK_TAS(2) + OP(22,6)*vd*SH_TAS(1));
Kfusion(23) = SK_TAS(1)*(OP(23,4)*SH_TAS(3) - OP(23,23)*SH_TAS(3) + OP(23,5)*SK_TAS(2) - OP(23,24)*SK_TAS(2) + OP(23,6)*vd*SH_TAS(1));
Kfusion(24) = SK_TAS(1)*(OP(24,4)*SH_TAS(3) - OP(24,23)*SH_TAS(3) + OP(24,5)*SK_TAS(2) - OP(24,24)*SK_TAS(2) + OP(24,6)*vd*SH_TAS(1));
SH_BETA = zeros(10,1);
SH_BETA(1) = (2*conj(q0)*conj(q3) + 2*conj(q1)*conj(q2))*(ve - vwe) - vd*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)) + (vn - vwn)*(conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2);
SH_BETA(2) = vd*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) - (2*conj(q0)*conj(q3) - 2*conj(q1)*conj(q2))*(vn - vwn) + (ve - vwe)*(conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2);
SH_BETA(3) = (2*conj(q0)*conj(q2) + 2*conj(q1)*conj(q3))*(vn - vwn) - (2*conj(q0)*conj(q1) - 2*conj(q2)*conj(q3))*(ve - vwe) + vd*(conj(q0)^2 - conj(q1)^2 - conj(q2)^2 + conj(q3)^2);
SH_BETA(4) = (conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2)/SH_BETA(1);
SH_BETA(5) = 1/SH_BETA(1)^2;
SH_BETA(6) = conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2;
SH_BETA(7) = 2*conj(q0)*conj(q3);
SH_BETA(8) = 1/SH_BETA(1);
SH_BETA(9) = SH_BETA(7) + 2*conj(q1)*conj(q2);
SH_BETA(10) = SH_BETA(7) - 2*conj(q1)*conj(q2);
H_BETA = zeros(1,24);
H_BETA(1,1) = SH_BETA(3)*SH_BETA(8);
H_BETA(1,2) = SH_BETA(2)*SH_BETA(3)*SH_BETA(5);
H_BETA(1,3) = - SH_BETA(2)^2*SH_BETA(5) - 1;
H_BETA(1,4) = - SH_BETA(8)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(6);
H_BETA(1,5) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9);
H_BETA(1,6) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3));
H_BETA(1,23) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6);
H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*SH_BETA(9) - SH_BETA(4);
SK_BETA = zeros(6,1);
SK_BETA(1) = 1/(R_BETA + (SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)))*(OP(23,6)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,6)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,6)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,6)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,6)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,6)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,6)*SH_BETA(3)*SH_BETA(8) + OP(2,6)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP(23,5)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,5)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,5)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,5)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,5)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,5)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,5)*SH_BETA(3)*SH_BETA(8) + OP(2,5)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP(23,24)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,24)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,24)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,24)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,24)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,24)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,24)*SH_BETA(3)*SH_BETA(8) + OP(2,24)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP(23,4)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,4)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,4)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,4)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,4)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,4)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,4)*SH_BETA(3)*SH_BETA(8) + OP(2,4)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP(23,23)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,23)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,23)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,23)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,23)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,23)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,23)*SH_BETA(3)*SH_BETA(8) + OP(2,23)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(2)^2*SH_BETA(5) + 1)*(OP(23,3)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,3)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,3)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,3)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,3)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,3)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,3)*SH_BETA(3)*SH_BETA(8) + OP(2,3)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(3)*SH_BETA(8)*(OP(23,1)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,1)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,1)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,1)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,1)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,1)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,1)*SH_BETA(3)*SH_BETA(8) + OP(2,1)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(2)*SH_BETA(3)*SH_BETA(5)*(OP(23,2)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,2)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,2)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,2)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,2)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,2)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,2)*SH_BETA(3)*SH_BETA(8) + OP(2,2)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)));
SK_BETA(2) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3));
SK_BETA(3) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6);
SK_BETA(4) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9);
SK_BETA(5) = SH_BETA(2)^2*SH_BETA(5) + 1;
SK_BETA(6) = SH_BETA(3);
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_BETA(1)*(OP(1,6)*SK_BETA(2) - OP(1,3)*SK_BETA(5) - OP(1,4)*SK_BETA(3) + OP(1,5)*SK_BETA(4) + OP(1,23)*SK_BETA(3) - OP(1,24)*SK_BETA(4) + OP(1,1)*SH_BETA(8)*SK_BETA(6) + OP(1,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(2) = SK_BETA(1)*(OP(2,6)*SK_BETA(2) - OP(2,3)*SK_BETA(5) - OP(2,4)*SK_BETA(3) + OP(2,5)*SK_BETA(4) + OP(2,23)*SK_BETA(3) - OP(2,24)*SK_BETA(4) + OP(2,1)*SH_BETA(8)*SK_BETA(6) + OP(2,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(3) = SK_BETA(1)*(OP(3,6)*SK_BETA(2) - OP(3,3)*SK_BETA(5) - OP(3,4)*SK_BETA(3) + OP(3,5)*SK_BETA(4) + OP(3,23)*SK_BETA(3) - OP(3,24)*SK_BETA(4) + OP(3,1)*SH_BETA(8)*SK_BETA(6) + OP(3,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(4) = SK_BETA(1)*(OP(4,6)*SK_BETA(2) - OP(4,3)*SK_BETA(5) - OP(4,4)*SK_BETA(3) + OP(4,5)*SK_BETA(4) + OP(4,23)*SK_BETA(3) - OP(4,24)*SK_BETA(4) + OP(4,1)*SH_BETA(8)*SK_BETA(6) + OP(4,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(5) = SK_BETA(1)*(OP(5,6)*SK_BETA(2) - OP(5,3)*SK_BETA(5) - OP(5,4)*SK_BETA(3) + OP(5,5)*SK_BETA(4) + OP(5,23)*SK_BETA(3) - OP(5,24)*SK_BETA(4) + OP(5,1)*SH_BETA(8)*SK_BETA(6) + OP(5,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(6) = SK_BETA(1)*(OP(6,6)*SK_BETA(2) - OP(6,3)*SK_BETA(5) - OP(6,4)*SK_BETA(3) + OP(6,5)*SK_BETA(4) + OP(6,23)*SK_BETA(3) - OP(6,24)*SK_BETA(4) + OP(6,1)*SH_BETA(8)*SK_BETA(6) + OP(6,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(7) = SK_BETA(1)*(OP(7,6)*SK_BETA(2) - OP(7,3)*SK_BETA(5) - OP(7,4)*SK_BETA(3) + OP(7,5)*SK_BETA(4) + OP(7,23)*SK_BETA(3) - OP(7,24)*SK_BETA(4) + OP(7,1)*SH_BETA(8)*SK_BETA(6) + OP(7,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(8) = SK_BETA(1)*(OP(8,6)*SK_BETA(2) - OP(8,3)*SK_BETA(5) - OP(8,4)*SK_BETA(3) + OP(8,5)*SK_BETA(4) + OP(8,23)*SK_BETA(3) - OP(8,24)*SK_BETA(4) + OP(8,1)*SH_BETA(8)*SK_BETA(6) + OP(8,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(9) = SK_BETA(1)*(OP(9,6)*SK_BETA(2) - OP(9,3)*SK_BETA(5) - OP(9,4)*SK_BETA(3) + OP(9,5)*SK_BETA(4) + OP(9,23)*SK_BETA(3) - OP(9,24)*SK_BETA(4) + OP(9,1)*SH_BETA(8)*SK_BETA(6) + OP(9,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(10) = SK_BETA(1)*(OP(10,6)*SK_BETA(2) - OP(10,3)*SK_BETA(5) - OP(10,4)*SK_BETA(3) + OP(10,5)*SK_BETA(4) + OP(10,23)*SK_BETA(3) - OP(10,24)*SK_BETA(4) + OP(10,1)*SH_BETA(8)*SK_BETA(6) + OP(10,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(11) = SK_BETA(1)*(OP(11,6)*SK_BETA(2) - OP(11,3)*SK_BETA(5) - OP(11,4)*SK_BETA(3) + OP(11,5)*SK_BETA(4) + OP(11,23)*SK_BETA(3) - OP(11,24)*SK_BETA(4) + OP(11,1)*SH_BETA(8)*SK_BETA(6) + OP(11,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(12) = SK_BETA(1)*(OP(12,6)*SK_BETA(2) - OP(12,3)*SK_BETA(5) - OP(12,4)*SK_BETA(3) + OP(12,5)*SK_BETA(4) + OP(12,23)*SK_BETA(3) - OP(12,24)*SK_BETA(4) + OP(12,1)*SH_BETA(8)*SK_BETA(6) + OP(12,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(13) = SK_BETA(1)*(OP(13,6)*SK_BETA(2) - OP(13,3)*SK_BETA(5) - OP(13,4)*SK_BETA(3) + OP(13,5)*SK_BETA(4) + OP(13,23)*SK_BETA(3) - OP(13,24)*SK_BETA(4) + OP(13,1)*SH_BETA(8)*SK_BETA(6) + OP(13,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(14) = SK_BETA(1)*(OP(14,6)*SK_BETA(2) - OP(14,3)*SK_BETA(5) - OP(14,4)*SK_BETA(3) + OP(14,5)*SK_BETA(4) + OP(14,23)*SK_BETA(3) - OP(14,24)*SK_BETA(4) + OP(14,1)*SH_BETA(8)*SK_BETA(6) + OP(14,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(15) = SK_BETA(1)*(OP(15,6)*SK_BETA(2) - OP(15,3)*SK_BETA(5) - OP(15,4)*SK_BETA(3) + OP(15,5)*SK_BETA(4) + OP(15,23)*SK_BETA(3) - OP(15,24)*SK_BETA(4) + OP(15,1)*SH_BETA(8)*SK_BETA(6) + OP(15,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(16) = SK_BETA(1)*(OP(16,6)*SK_BETA(2) - OP(16,3)*SK_BETA(5) - OP(16,4)*SK_BETA(3) + OP(16,5)*SK_BETA(4) + OP(16,23)*SK_BETA(3) - OP(16,24)*SK_BETA(4) + OP(16,1)*SH_BETA(8)*SK_BETA(6) + OP(16,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(17) = SK_BETA(1)*(OP(17,6)*SK_BETA(2) - OP(17,3)*SK_BETA(5) - OP(17,4)*SK_BETA(3) + OP(17,5)*SK_BETA(4) + OP(17,23)*SK_BETA(3) - OP(17,24)*SK_BETA(4) + OP(17,1)*SH_BETA(8)*SK_BETA(6) + OP(17,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(18) = SK_BETA(1)*(OP(18,6)*SK_BETA(2) - OP(18,3)*SK_BETA(5) - OP(18,4)*SK_BETA(3) + OP(18,5)*SK_BETA(4) + OP(18,23)*SK_BETA(3) - OP(18,24)*SK_BETA(4) + OP(18,1)*SH_BETA(8)*SK_BETA(6) + OP(18,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(19) = SK_BETA(1)*(OP(19,6)*SK_BETA(2) - OP(19,3)*SK_BETA(5) - OP(19,4)*SK_BETA(3) + OP(19,5)*SK_BETA(4) + OP(19,23)*SK_BETA(3) - OP(19,24)*SK_BETA(4) + OP(19,1)*SH_BETA(8)*SK_BETA(6) + OP(19,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(20) = SK_BETA(1)*(OP(20,6)*SK_BETA(2) - OP(20,3)*SK_BETA(5) - OP(20,4)*SK_BETA(3) + OP(20,5)*SK_BETA(4) + OP(20,23)*SK_BETA(3) - OP(20,24)*SK_BETA(4) + OP(20,1)*SH_BETA(8)*SK_BETA(6) + OP(20,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(21) = SK_BETA(1)*(OP(21,6)*SK_BETA(2) - OP(21,3)*SK_BETA(5) - OP(21,4)*SK_BETA(3) + OP(21,5)*SK_BETA(4) + OP(21,23)*SK_BETA(3) - OP(21,24)*SK_BETA(4) + OP(21,1)*SH_BETA(8)*SK_BETA(6) + OP(21,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(22) = SK_BETA(1)*(OP(22,6)*SK_BETA(2) - OP(22,3)*SK_BETA(5) - OP(22,4)*SK_BETA(3) + OP(22,5)*SK_BETA(4) + OP(22,23)*SK_BETA(3) - OP(22,24)*SK_BETA(4) + OP(22,1)*SH_BETA(8)*SK_BETA(6) + OP(22,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(23) = SK_BETA(1)*(OP(23,6)*SK_BETA(2) - OP(23,3)*SK_BETA(5) - OP(23,4)*SK_BETA(3) + OP(23,5)*SK_BETA(4) + OP(23,23)*SK_BETA(3) - OP(23,24)*SK_BETA(4) + OP(23,1)*SH_BETA(8)*SK_BETA(6) + OP(23,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
Kfusion(24) = SK_BETA(1)*(OP(24,6)*SK_BETA(2) - OP(24,3)*SK_BETA(5) - OP(24,4)*SK_BETA(3) + OP(24,5)*SK_BETA(4) + OP(24,23)*SK_BETA(3) - OP(24,24)*SK_BETA(4) + OP(24,1)*SH_BETA(8)*SK_BETA(6) + OP(24,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6));
SH_MAG = zeros(9,1);
SH_MAG(1) = q0^2 - q1^2 + q2^2 - q3^2;
SH_MAG(2) = q0^2 + q1^2 - q2^2 - q3^2;
SH_MAG(3) = q0^2 - q1^2 - q2^2 + q3^2;
SH_MAG(4) = 2*q0*q1 + 2*q2*q3;
SH_MAG(5) = 2*q0*q3 + 2*q1*q2;
SH_MAG(6) = 2*q0*q2 + 2*q1*q3;
SH_MAG(7) = magE*(2*q0*q1 - 2*q2*q3);
SH_MAG(8) = 2*q1*q3 - 2*q0*q2;
SH_MAG(9) = 2*q0*q3;
H_MAG = zeros(1,24);
H_MAG(2) = SH_MAG(7) - magD*SH_MAG(3) - magN*SH_MAG(6);
H_MAG(3) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2);
H_MAG(17) = SH_MAG(2);
H_MAG(18) = SH_MAG(5);
H_MAG(19) = SH_MAG(8);
H_MAG(20) = 1;
SK_MX = zeros(4,1);
SK_MX(1) = 1/(OP(20,20) + R_MAG - OP(2,20)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,20)*SH_MAG(2) + OP(18,20)*SH_MAG(5) + OP(19,20)*SH_MAG(8) + OP(3,20)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) - (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP(20,2) - OP(2,2)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,2)*SH_MAG(2) + OP(18,2)*SH_MAG(5) + OP(19,2)*SH_MAG(8) + OP(3,2)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(2)*(OP(20,17) - OP(2,17)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,17)*SH_MAG(2) + OP(18,17)*SH_MAG(5) + OP(19,17)*SH_MAG(8) + OP(3,17)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(5)*(OP(20,18) - OP(2,18)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,18)*SH_MAG(2) + OP(18,18)*SH_MAG(5) + OP(19,18)*SH_MAG(8) + OP(3,18)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(8)*(OP(20,19) - OP(2,19)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,19)*SH_MAG(2) + OP(18,19)*SH_MAG(5) + OP(19,19)*SH_MAG(8) + OP(3,19)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP(20,3) - OP(2,3)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,3)*SH_MAG(2) + OP(18,3)*SH_MAG(5) + OP(19,3)*SH_MAG(8) + OP(3,3)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))));
SK_MX(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2);
SK_MX(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6);
SK_MX(4) = SH_MAG(8);
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_MX(1)*(OP(1,20) + OP(1,17)*SH_MAG(2) + OP(1,18)*SH_MAG(5) - OP(1,2)*SK_MX(3) + OP(1,3)*SK_MX(2) + OP(1,19)*SK_MX(4));
Kfusion(2) = SK_MX(1)*(OP(2,20) + OP(2,17)*SH_MAG(2) + OP(2,18)*SH_MAG(5) - OP(2,2)*SK_MX(3) + OP(2,3)*SK_MX(2) + OP(2,19)*SK_MX(4));
Kfusion(3) = SK_MX(1)*(OP(3,20) + OP(3,17)*SH_MAG(2) + OP(3,18)*SH_MAG(5) - OP(3,2)*SK_MX(3) + OP(3,3)*SK_MX(2) + OP(3,19)*SK_MX(4));
Kfusion(4) = SK_MX(1)*(OP(4,20) + OP(4,17)*SH_MAG(2) + OP(4,18)*SH_MAG(5) - OP(4,2)*SK_MX(3) + OP(4,3)*SK_MX(2) + OP(4,19)*SK_MX(4));
Kfusion(5) = SK_MX(1)*(OP(5,20) + OP(5,17)*SH_MAG(2) + OP(5,18)*SH_MAG(5) - OP(5,2)*SK_MX(3) + OP(5,3)*SK_MX(2) + OP(5,19)*SK_MX(4));
Kfusion(6) = SK_MX(1)*(OP(6,20) + OP(6,17)*SH_MAG(2) + OP(6,18)*SH_MAG(5) - OP(6,2)*SK_MX(3) + OP(6,3)*SK_MX(2) + OP(6,19)*SK_MX(4));
Kfusion(7) = SK_MX(1)*(OP(7,20) + OP(7,17)*SH_MAG(2) + OP(7,18)*SH_MAG(5) - OP(7,2)*SK_MX(3) + OP(7,3)*SK_MX(2) + OP(7,19)*SK_MX(4));
Kfusion(8) = SK_MX(1)*(OP(8,20) + OP(8,17)*SH_MAG(2) + OP(8,18)*SH_MAG(5) - OP(8,2)*SK_MX(3) + OP(8,3)*SK_MX(2) + OP(8,19)*SK_MX(4));
Kfusion(9) = SK_MX(1)*(OP(9,20) + OP(9,17)*SH_MAG(2) + OP(9,18)*SH_MAG(5) - OP(9,2)*SK_MX(3) + OP(9,3)*SK_MX(2) + OP(9,19)*SK_MX(4));
Kfusion(10) = SK_MX(1)*(OP(10,20) + OP(10,17)*SH_MAG(2) + OP(10,18)*SH_MAG(5) - OP(10,2)*SK_MX(3) + OP(10,3)*SK_MX(2) + OP(10,19)*SK_MX(4));
Kfusion(11) = SK_MX(1)*(OP(11,20) + OP(11,17)*SH_MAG(2) + OP(11,18)*SH_MAG(5) - OP(11,2)*SK_MX(3) + OP(11,3)*SK_MX(2) + OP(11,19)*SK_MX(4));
Kfusion(12) = SK_MX(1)*(OP(12,20) + OP(12,17)*SH_MAG(2) + OP(12,18)*SH_MAG(5) - OP(12,2)*SK_MX(3) + OP(12,3)*SK_MX(2) + OP(12,19)*SK_MX(4));
Kfusion(13) = SK_MX(1)*(OP(13,20) + OP(13,17)*SH_MAG(2) + OP(13,18)*SH_MAG(5) - OP(13,2)*SK_MX(3) + OP(13,3)*SK_MX(2) + OP(13,19)*SK_MX(4));
Kfusion(14) = SK_MX(1)*(OP(14,20) + OP(14,17)*SH_MAG(2) + OP(14,18)*SH_MAG(5) - OP(14,2)*SK_MX(3) + OP(14,3)*SK_MX(2) + OP(14,19)*SK_MX(4));
Kfusion(15) = SK_MX(1)*(OP(15,20) + OP(15,17)*SH_MAG(2) + OP(15,18)*SH_MAG(5) - OP(15,2)*SK_MX(3) + OP(15,3)*SK_MX(2) + OP(15,19)*SK_MX(4));
Kfusion(16) = SK_MX(1)*(OP(16,20) + OP(16,17)*SH_MAG(2) + OP(16,18)*SH_MAG(5) - OP(16,2)*SK_MX(3) + OP(16,3)*SK_MX(2) + OP(16,19)*SK_MX(4));
Kfusion(17) = SK_MX(1)*(OP(17,20) + OP(17,17)*SH_MAG(2) + OP(17,18)*SH_MAG(5) - OP(17,2)*SK_MX(3) + OP(17,3)*SK_MX(2) + OP(17,19)*SK_MX(4));
Kfusion(18) = SK_MX(1)*(OP(18,20) + OP(18,17)*SH_MAG(2) + OP(18,18)*SH_MAG(5) - OP(18,2)*SK_MX(3) + OP(18,3)*SK_MX(2) + OP(18,19)*SK_MX(4));
Kfusion(19) = SK_MX(1)*(OP(19,20) + OP(19,17)*SH_MAG(2) + OP(19,18)*SH_MAG(5) - OP(19,2)*SK_MX(3) + OP(19,3)*SK_MX(2) + OP(19,19)*SK_MX(4));
Kfusion(20) = SK_MX(1)*(OP(20,20) + OP(20,17)*SH_MAG(2) + OP(20,18)*SH_MAG(5) - OP(20,2)*SK_MX(3) + OP(20,3)*SK_MX(2) + OP(20,19)*SK_MX(4));
Kfusion(21) = SK_MX(1)*(OP(21,20) + OP(21,17)*SH_MAG(2) + OP(21,18)*SH_MAG(5) - OP(21,2)*SK_MX(3) + OP(21,3)*SK_MX(2) + OP(21,19)*SK_MX(4));
Kfusion(22) = SK_MX(1)*(OP(22,20) + OP(22,17)*SH_MAG(2) + OP(22,18)*SH_MAG(5) - OP(22,2)*SK_MX(3) + OP(22,3)*SK_MX(2) + OP(22,19)*SK_MX(4));
Kfusion(23) = SK_MX(1)*(OP(23,20) + OP(23,17)*SH_MAG(2) + OP(23,18)*SH_MAG(5) - OP(23,2)*SK_MX(3) + OP(23,3)*SK_MX(2) + OP(23,19)*SK_MX(4));
Kfusion(24) = SK_MX(1)*(OP(24,20) + OP(24,17)*SH_MAG(2) + OP(24,18)*SH_MAG(5) - OP(24,2)*SK_MX(3) + OP(24,3)*SK_MX(2) + OP(24,19)*SK_MX(4));
H_MAG = zeros(1,24);
H_MAG(1) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6);
H_MAG(3) = - magE*SH_MAG(5) - magD*SH_MAG(8) - magN*SH_MAG(2);
H_MAG(17) = 2*q1*q2 - SH_MAG(9);
H_MAG(18) = SH_MAG(1);
H_MAG(19) = SH_MAG(4);
H_MAG(21) = 1;
SK_MY = zeros(4,1);
SK_MY(1) = 1/(OP(21,21) + R_MAG + OP(1,21)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,21)*SH_MAG(1) + OP(19,21)*SH_MAG(4) - (SH_MAG(9) - 2*q1*q2)*(OP(21,17) + OP(1,17)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,17)*SH_MAG(1) + OP(19,17)*SH_MAG(4) - OP(3,17)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,17)*(SH_MAG(9) - 2*q1*q2)) - OP(3,21)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP(21,1) + OP(1,1)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,1)*SH_MAG(1) + OP(19,1)*SH_MAG(4) - OP(3,1)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,1)*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(1)*(OP(21,18) + OP(1,18)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,18)*SH_MAG(1) + OP(19,18)*SH_MAG(4) - OP(3,18)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,18)*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(4)*(OP(21,19) + OP(1,19)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,19)*SH_MAG(1) + OP(19,19)*SH_MAG(4) - OP(3,19)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,19)*(SH_MAG(9) - 2*q1*q2)) - OP(17,21)*(SH_MAG(9) - 2*q1*q2) - (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP(21,3) + OP(1,3)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,3)*SH_MAG(1) + OP(19,3)*SH_MAG(4) - OP(3,3)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,3)*(SH_MAG(9) - 2*q1*q2)));
SK_MY(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2);
SK_MY(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6);
SK_MY(4) = SH_MAG(9) - 2*q1*q2;
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_MY(1)*(OP(1,21) + OP(1,18)*SH_MAG(1) + OP(1,19)*SH_MAG(4) + OP(1,1)*SK_MY(3) - OP(1,3)*SK_MY(2) - OP(1,17)*SK_MY(4));
Kfusion(2) = SK_MY(1)*(OP(2,21) + OP(2,18)*SH_MAG(1) + OP(2,19)*SH_MAG(4) + OP(2,1)*SK_MY(3) - OP(2,3)*SK_MY(2) - OP(2,17)*SK_MY(4));
Kfusion(3) = SK_MY(1)*(OP(3,21) + OP(3,18)*SH_MAG(1) + OP(3,19)*SH_MAG(4) + OP(3,1)*SK_MY(3) - OP(3,3)*SK_MY(2) - OP(3,17)*SK_MY(4));
Kfusion(4) = SK_MY(1)*(OP(4,21) + OP(4,18)*SH_MAG(1) + OP(4,19)*SH_MAG(4) + OP(4,1)*SK_MY(3) - OP(4,3)*SK_MY(2) - OP(4,17)*SK_MY(4));
Kfusion(5) = SK_MY(1)*(OP(5,21) + OP(5,18)*SH_MAG(1) + OP(5,19)*SH_MAG(4) + OP(5,1)*SK_MY(3) - OP(5,3)*SK_MY(2) - OP(5,17)*SK_MY(4));
Kfusion(6) = SK_MY(1)*(OP(6,21) + OP(6,18)*SH_MAG(1) + OP(6,19)*SH_MAG(4) + OP(6,1)*SK_MY(3) - OP(6,3)*SK_MY(2) - OP(6,17)*SK_MY(4));
Kfusion(7) = SK_MY(1)*(OP(7,21) + OP(7,18)*SH_MAG(1) + OP(7,19)*SH_MAG(4) + OP(7,1)*SK_MY(3) - OP(7,3)*SK_MY(2) - OP(7,17)*SK_MY(4));
Kfusion(8) = SK_MY(1)*(OP(8,21) + OP(8,18)*SH_MAG(1) + OP(8,19)*SH_MAG(4) + OP(8,1)*SK_MY(3) - OP(8,3)*SK_MY(2) - OP(8,17)*SK_MY(4));
Kfusion(9) = SK_MY(1)*(OP(9,21) + OP(9,18)*SH_MAG(1) + OP(9,19)*SH_MAG(4) + OP(9,1)*SK_MY(3) - OP(9,3)*SK_MY(2) - OP(9,17)*SK_MY(4));
Kfusion(10) = SK_MY(1)*(OP(10,21) + OP(10,18)*SH_MAG(1) + OP(10,19)*SH_MAG(4) + OP(10,1)*SK_MY(3) - OP(10,3)*SK_MY(2) - OP(10,17)*SK_MY(4));
Kfusion(11) = SK_MY(1)*(OP(11,21) + OP(11,18)*SH_MAG(1) + OP(11,19)*SH_MAG(4) + OP(11,1)*SK_MY(3) - OP(11,3)*SK_MY(2) - OP(11,17)*SK_MY(4));
Kfusion(12) = SK_MY(1)*(OP(12,21) + OP(12,18)*SH_MAG(1) + OP(12,19)*SH_MAG(4) + OP(12,1)*SK_MY(3) - OP(12,3)*SK_MY(2) - OP(12,17)*SK_MY(4));
Kfusion(13) = SK_MY(1)*(OP(13,21) + OP(13,18)*SH_MAG(1) + OP(13,19)*SH_MAG(4) + OP(13,1)*SK_MY(3) - OP(13,3)*SK_MY(2) - OP(13,17)*SK_MY(4));
Kfusion(14) = SK_MY(1)*(OP(14,21) + OP(14,18)*SH_MAG(1) + OP(14,19)*SH_MAG(4) + OP(14,1)*SK_MY(3) - OP(14,3)*SK_MY(2) - OP(14,17)*SK_MY(4));
Kfusion(15) = SK_MY(1)*(OP(15,21) + OP(15,18)*SH_MAG(1) + OP(15,19)*SH_MAG(4) + OP(15,1)*SK_MY(3) - OP(15,3)*SK_MY(2) - OP(15,17)*SK_MY(4));
Kfusion(16) = SK_MY(1)*(OP(16,21) + OP(16,18)*SH_MAG(1) + OP(16,19)*SH_MAG(4) + OP(16,1)*SK_MY(3) - OP(16,3)*SK_MY(2) - OP(16,17)*SK_MY(4));
Kfusion(17) = SK_MY(1)*(OP(17,21) + OP(17,18)*SH_MAG(1) + OP(17,19)*SH_MAG(4) + OP(17,1)*SK_MY(3) - OP(17,3)*SK_MY(2) - OP(17,17)*SK_MY(4));
Kfusion(18) = SK_MY(1)*(OP(18,21) + OP(18,18)*SH_MAG(1) + OP(18,19)*SH_MAG(4) + OP(18,1)*SK_MY(3) - OP(18,3)*SK_MY(2) - OP(18,17)*SK_MY(4));
Kfusion(19) = SK_MY(1)*(OP(19,21) + OP(19,18)*SH_MAG(1) + OP(19,19)*SH_MAG(4) + OP(19,1)*SK_MY(3) - OP(19,3)*SK_MY(2) - OP(19,17)*SK_MY(4));
Kfusion(20) = SK_MY(1)*(OP(20,21) + OP(20,18)*SH_MAG(1) + OP(20,19)*SH_MAG(4) + OP(20,1)*SK_MY(3) - OP(20,3)*SK_MY(2) - OP(20,17)*SK_MY(4));
Kfusion(21) = SK_MY(1)*(OP(21,21) + OP(21,18)*SH_MAG(1) + OP(21,19)*SH_MAG(4) + OP(21,1)*SK_MY(3) - OP(21,3)*SK_MY(2) - OP(21,17)*SK_MY(4));
Kfusion(22) = SK_MY(1)*(OP(22,21) + OP(22,18)*SH_MAG(1) + OP(22,19)*SH_MAG(4) + OP(22,1)*SK_MY(3) - OP(22,3)*SK_MY(2) - OP(22,17)*SK_MY(4));
Kfusion(23) = SK_MY(1)*(OP(23,21) + OP(23,18)*SH_MAG(1) + OP(23,19)*SH_MAG(4) + OP(23,1)*SK_MY(3) - OP(23,3)*SK_MY(2) - OP(23,17)*SK_MY(4));
Kfusion(24) = SK_MY(1)*(OP(24,21) + OP(24,18)*SH_MAG(1) + OP(24,19)*SH_MAG(4) + OP(24,1)*SK_MY(3) - OP(24,3)*SK_MY(2) - OP(24,17)*SK_MY(4));
H_MAG = zeros(1,24);
H_MAG(1) = magN*(SH_MAG(9) - 2*q1*q2) - magD*SH_MAG(4) - magE*SH_MAG(1);
H_MAG(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2);
H_MAG(17) = SH_MAG(6);
H_MAG(18) = 2*q2*q3 - 2*q0*q1;
H_MAG(19) = SH_MAG(3);
H_MAG(22) = 1;
SK_MZ = zeros(4,1);
SK_MZ(1) = 1/(OP(22,22) + R_MAG + OP(17,22)*SH_MAG(6) + OP(19,22)*SH_MAG(3) - (2*q0*q1 - 2*q2*q3)*(OP(22,18) + OP(17,18)*SH_MAG(6) + OP(19,18)*SH_MAG(3) - OP(1,18)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,18)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,18)*(2*q0*q1 - 2*q2*q3)) - OP(1,22)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,22)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + SH_MAG(6)*(OP(22,17) + OP(17,17)*SH_MAG(6) + OP(19,17)*SH_MAG(3) - OP(1,17)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,17)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,17)*(2*q0*q1 - 2*q2*q3)) + SH_MAG(3)*(OP(22,19) + OP(17,19)*SH_MAG(6) + OP(19,19)*SH_MAG(3) - OP(1,19)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,19)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,19)*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP(22,1) + OP(17,1)*SH_MAG(6) + OP(19,1)*SH_MAG(3) - OP(1,1)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,1)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,1)*(2*q0*q1 - 2*q2*q3)) - OP(18,22)*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP(22,2) + OP(17,2)*SH_MAG(6) + OP(19,2)*SH_MAG(3) - OP(1,2)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,2)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,2)*(2*q0*q1 - 2*q2*q3)));
SK_MZ(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2);
SK_MZ(3) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2);
SK_MZ(4) = 2*q0*q1 - 2*q2*q3;
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_MZ(1)*(OP(1,22) + OP(1,19)*SH_MAG(3) + OP(1,17)*SH_MAG(6) - OP(1,1)*SK_MZ(2) + OP(1,2)*SK_MZ(3) - OP(1,18)*SK_MZ(4));
Kfusion(2) = SK_MZ(1)*(OP(2,22) + OP(2,19)*SH_MAG(3) + OP(2,17)*SH_MAG(6) - OP(2,1)*SK_MZ(2) + OP(2,2)*SK_MZ(3) - OP(2,18)*SK_MZ(4));
Kfusion(3) = SK_MZ(1)*(OP(3,22) + OP(3,19)*SH_MAG(3) + OP(3,17)*SH_MAG(6) - OP(3,1)*SK_MZ(2) + OP(3,2)*SK_MZ(3) - OP(3,18)*SK_MZ(4));
Kfusion(4) = SK_MZ(1)*(OP(4,22) + OP(4,19)*SH_MAG(3) + OP(4,17)*SH_MAG(6) - OP(4,1)*SK_MZ(2) + OP(4,2)*SK_MZ(3) - OP(4,18)*SK_MZ(4));
Kfusion(5) = SK_MZ(1)*(OP(5,22) + OP(5,19)*SH_MAG(3) + OP(5,17)*SH_MAG(6) - OP(5,1)*SK_MZ(2) + OP(5,2)*SK_MZ(3) - OP(5,18)*SK_MZ(4));
Kfusion(6) = SK_MZ(1)*(OP(6,22) + OP(6,19)*SH_MAG(3) + OP(6,17)*SH_MAG(6) - OP(6,1)*SK_MZ(2) + OP(6,2)*SK_MZ(3) - OP(6,18)*SK_MZ(4));
Kfusion(7) = SK_MZ(1)*(OP(7,22) + OP(7,19)*SH_MAG(3) + OP(7,17)*SH_MAG(6) - OP(7,1)*SK_MZ(2) + OP(7,2)*SK_MZ(3) - OP(7,18)*SK_MZ(4));
Kfusion(8) = SK_MZ(1)*(OP(8,22) + OP(8,19)*SH_MAG(3) + OP(8,17)*SH_MAG(6) - OP(8,1)*SK_MZ(2) + OP(8,2)*SK_MZ(3) - OP(8,18)*SK_MZ(4));
Kfusion(9) = SK_MZ(1)*(OP(9,22) + OP(9,19)*SH_MAG(3) + OP(9,17)*SH_MAG(6) - OP(9,1)*SK_MZ(2) + OP(9,2)*SK_MZ(3) - OP(9,18)*SK_MZ(4));
Kfusion(10) = SK_MZ(1)*(OP(10,22) + OP(10,19)*SH_MAG(3) + OP(10,17)*SH_MAG(6) - OP(10,1)*SK_MZ(2) + OP(10,2)*SK_MZ(3) - OP(10,18)*SK_MZ(4));
Kfusion(11) = SK_MZ(1)*(OP(11,22) + OP(11,19)*SH_MAG(3) + OP(11,17)*SH_MAG(6) - OP(11,1)*SK_MZ(2) + OP(11,2)*SK_MZ(3) - OP(11,18)*SK_MZ(4));
Kfusion(12) = SK_MZ(1)*(OP(12,22) + OP(12,19)*SH_MAG(3) + OP(12,17)*SH_MAG(6) - OP(12,1)*SK_MZ(2) + OP(12,2)*SK_MZ(3) - OP(12,18)*SK_MZ(4));
Kfusion(13) = SK_MZ(1)*(OP(13,22) + OP(13,19)*SH_MAG(3) + OP(13,17)*SH_MAG(6) - OP(13,1)*SK_MZ(2) + OP(13,2)*SK_MZ(3) - OP(13,18)*SK_MZ(4));
Kfusion(14) = SK_MZ(1)*(OP(14,22) + OP(14,19)*SH_MAG(3) + OP(14,17)*SH_MAG(6) - OP(14,1)*SK_MZ(2) + OP(14,2)*SK_MZ(3) - OP(14,18)*SK_MZ(4));
Kfusion(15) = SK_MZ(1)*(OP(15,22) + OP(15,19)*SH_MAG(3) + OP(15,17)*SH_MAG(6) - OP(15,1)*SK_MZ(2) + OP(15,2)*SK_MZ(3) - OP(15,18)*SK_MZ(4));
Kfusion(16) = SK_MZ(1)*(OP(16,22) + OP(16,19)*SH_MAG(3) + OP(16,17)*SH_MAG(6) - OP(16,1)*SK_MZ(2) + OP(16,2)*SK_MZ(3) - OP(16,18)*SK_MZ(4));
Kfusion(17) = SK_MZ(1)*(OP(17,22) + OP(17,19)*SH_MAG(3) + OP(17,17)*SH_MAG(6) - OP(17,1)*SK_MZ(2) + OP(17,2)*SK_MZ(3) - OP(17,18)*SK_MZ(4));
Kfusion(18) = SK_MZ(1)*(OP(18,22) + OP(18,19)*SH_MAG(3) + OP(18,17)*SH_MAG(6) - OP(18,1)*SK_MZ(2) + OP(18,2)*SK_MZ(3) - OP(18,18)*SK_MZ(4));
Kfusion(19) = SK_MZ(1)*(OP(19,22) + OP(19,19)*SH_MAG(3) + OP(19,17)*SH_MAG(6) - OP(19,1)*SK_MZ(2) + OP(19,2)*SK_MZ(3) - OP(19,18)*SK_MZ(4));
Kfusion(20) = SK_MZ(1)*(OP(20,22) + OP(20,19)*SH_MAG(3) + OP(20,17)*SH_MAG(6) - OP(20,1)*SK_MZ(2) + OP(20,2)*SK_MZ(3) - OP(20,18)*SK_MZ(4));
Kfusion(21) = SK_MZ(1)*(OP(21,22) + OP(21,19)*SH_MAG(3) + OP(21,17)*SH_MAG(6) - OP(21,1)*SK_MZ(2) + OP(21,2)*SK_MZ(3) - OP(21,18)*SK_MZ(4));
Kfusion(22) = SK_MZ(1)*(OP(22,22) + OP(22,19)*SH_MAG(3) + OP(22,17)*SH_MAG(6) - OP(22,1)*SK_MZ(2) + OP(22,2)*SK_MZ(3) - OP(22,18)*SK_MZ(4));
Kfusion(23) = SK_MZ(1)*(OP(23,22) + OP(23,19)*SH_MAG(3) + OP(23,17)*SH_MAG(6) - OP(23,1)*SK_MZ(2) + OP(23,2)*SK_MZ(3) - OP(23,18)*SK_MZ(4));
Kfusion(24) = SK_MZ(1)*(OP(24,22) + OP(24,19)*SH_MAG(3) + OP(24,17)*SH_MAG(6) - OP(24,1)*SK_MZ(2) + OP(24,2)*SK_MZ(3) - OP(24,18)*SK_MZ(4));
SH_ACCX = zeros(7,1);
SH_ACCX(1) = q0^2 + q1^2 - q2^2 - q3^2;
SH_ACCX(2) = 2*q0*q3 + 2*q1*q2;
SH_ACCX(3) = vn - vwn;
SH_ACCX(4) = ve - vwe;
SH_ACCX(5) = q3^2;
SH_ACCX(6) = 2*q0*q2;
SH_ACCX(7) = 2*q0*q1;
H_ACCX = zeros(1,24);
H_ACCX(1,2) = Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2));
H_ACCX(1,3) = Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3));
H_ACCX(1,4) = -Kaccx*SH_ACCX(1);
H_ACCX(1,5) = -Kaccx*SH_ACCX(2);
H_ACCX(1,6) = Kaccx*(SH_ACCX(6) - 2*q1*q3);
H_ACCX(1,23) = Kaccx*SH_ACCX(1);
H_ACCX(1,24) = Kaccx*SH_ACCX(2);
SK_ACCX = zeros(5,1);
SK_ACCX(1) = 1/(R_ACC - Kaccx*SH_ACCX(1)*(Kaccx*OP(23,4)*SH_ACCX(1) - Kaccx*OP(5,4)*SH_ACCX(2) - Kaccx*OP(4,4)*SH_ACCX(1) + Kaccx*OP(24,4)*SH_ACCX(2) + Kaccx*OP(3,4)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,4)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,4)*(SH_ACCX(6) - 2*q1*q3)) - Kaccx*SH_ACCX(2)*(Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(5,5)*SH_ACCX(2) - Kaccx*OP(4,5)*SH_ACCX(1) + Kaccx*OP(24,5)*SH_ACCX(2) + Kaccx*OP(3,5)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,5)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,5)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(1)*(Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(5,23)*SH_ACCX(2) - Kaccx*OP(4,23)*SH_ACCX(1) + Kaccx*OP(24,23)*SH_ACCX(2) + Kaccx*OP(3,23)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,23)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,23)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(2)*(Kaccx*OP(23,24)*SH_ACCX(1) - Kaccx*OP(5,24)*SH_ACCX(2) - Kaccx*OP(4,24)*SH_ACCX(1) + Kaccx*OP(24,24)*SH_ACCX(2) + Kaccx*OP(3,24)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,24)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,24)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3))*(Kaccx*OP(23,3)*SH_ACCX(1) - Kaccx*OP(5,3)*SH_ACCX(2) - Kaccx*OP(4,3)*SH_ACCX(1) + Kaccx*OP(24,3)*SH_ACCX(2) + Kaccx*OP(3,3)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,3)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,3)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2))*(Kaccx*OP(23,2)*SH_ACCX(1) - Kaccx*OP(5,2)*SH_ACCX(2) - Kaccx*OP(4,2)*SH_ACCX(1) + Kaccx*OP(24,2)*SH_ACCX(2) + Kaccx*OP(3,2)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,2)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,2)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(6) - 2*q1*q3)*(Kaccx*OP(23,6)*SH_ACCX(1) - Kaccx*OP(5,6)*SH_ACCX(2) - Kaccx*OP(4,6)*SH_ACCX(1) + Kaccx*OP(24,6)*SH_ACCX(2) + Kaccx*OP(3,6)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,6)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,6)*(SH_ACCX(6) - 2*q1*q3)));
SK_ACCX(2) = SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3);
SK_ACCX(3) = SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2);
SK_ACCX(4) = SH_ACCX(6) - 2*q1*q3;
SK_ACCX(5) = SH_ACCX(2);
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_ACCX(1)*(Kaccx*OP(1,23)*SH_ACCX(1) - Kaccx*OP(1,4)*SH_ACCX(1) + Kaccx*OP(1,2)*SK_ACCX(3) + Kaccx*OP(1,3)*SK_ACCX(2) - Kaccx*OP(1,5)*SK_ACCX(5) + Kaccx*OP(1,6)*SK_ACCX(4) + Kaccx*OP(1,24)*SK_ACCX(5));
Kfusion(2) = SK_ACCX(1)*(Kaccx*OP(2,23)*SH_ACCX(1) - Kaccx*OP(2,4)*SH_ACCX(1) + Kaccx*OP(2,2)*SK_ACCX(3) + Kaccx*OP(2,3)*SK_ACCX(2) - Kaccx*OP(2,5)*SK_ACCX(5) + Kaccx*OP(2,6)*SK_ACCX(4) + Kaccx*OP(2,24)*SK_ACCX(5));
Kfusion(3) = SK_ACCX(1)*(Kaccx*OP(3,23)*SH_ACCX(1) - Kaccx*OP(3,4)*SH_ACCX(1) + Kaccx*OP(3,2)*SK_ACCX(3) + Kaccx*OP(3,3)*SK_ACCX(2) - Kaccx*OP(3,5)*SK_ACCX(5) + Kaccx*OP(3,6)*SK_ACCX(4) + Kaccx*OP(3,24)*SK_ACCX(5));
Kfusion(4) = SK_ACCX(1)*(Kaccx*OP(4,23)*SH_ACCX(1) - Kaccx*OP(4,4)*SH_ACCX(1) + Kaccx*OP(4,2)*SK_ACCX(3) + Kaccx*OP(4,3)*SK_ACCX(2) - Kaccx*OP(4,5)*SK_ACCX(5) + Kaccx*OP(4,6)*SK_ACCX(4) + Kaccx*OP(4,24)*SK_ACCX(5));
Kfusion(5) = SK_ACCX(1)*(Kaccx*OP(5,23)*SH_ACCX(1) - Kaccx*OP(5,4)*SH_ACCX(1) + Kaccx*OP(5,2)*SK_ACCX(3) + Kaccx*OP(5,3)*SK_ACCX(2) - Kaccx*OP(5,5)*SK_ACCX(5) + Kaccx*OP(5,6)*SK_ACCX(4) + Kaccx*OP(5,24)*SK_ACCX(5));
Kfusion(6) = SK_ACCX(1)*(Kaccx*OP(6,23)*SH_ACCX(1) - Kaccx*OP(6,4)*SH_ACCX(1) + Kaccx*OP(6,2)*SK_ACCX(3) + Kaccx*OP(6,3)*SK_ACCX(2) - Kaccx*OP(6,5)*SK_ACCX(5) + Kaccx*OP(6,6)*SK_ACCX(4) + Kaccx*OP(6,24)*SK_ACCX(5));
Kfusion(7) = SK_ACCX(1)*(Kaccx*OP(7,23)*SH_ACCX(1) - Kaccx*OP(7,4)*SH_ACCX(1) + Kaccx*OP(7,2)*SK_ACCX(3) + Kaccx*OP(7,3)*SK_ACCX(2) - Kaccx*OP(7,5)*SK_ACCX(5) + Kaccx*OP(7,6)*SK_ACCX(4) + Kaccx*OP(7,24)*SK_ACCX(5));
Kfusion(8) = SK_ACCX(1)*(Kaccx*OP(8,23)*SH_ACCX(1) - Kaccx*OP(8,4)*SH_ACCX(1) + Kaccx*OP(8,2)*SK_ACCX(3) + Kaccx*OP(8,3)*SK_ACCX(2) - Kaccx*OP(8,5)*SK_ACCX(5) + Kaccx*OP(8,6)*SK_ACCX(4) + Kaccx*OP(8,24)*SK_ACCX(5));
Kfusion(9) = SK_ACCX(1)*(Kaccx*OP(9,23)*SH_ACCX(1) - Kaccx*OP(9,4)*SH_ACCX(1) + Kaccx*OP(9,2)*SK_ACCX(3) + Kaccx*OP(9,3)*SK_ACCX(2) - Kaccx*OP(9,5)*SK_ACCX(5) + Kaccx*OP(9,6)*SK_ACCX(4) + Kaccx*OP(9,24)*SK_ACCX(5));
Kfusion(10) = SK_ACCX(1)*(Kaccx*OP(10,23)*SH_ACCX(1) - Kaccx*OP(10,4)*SH_ACCX(1) + Kaccx*OP(10,2)*SK_ACCX(3) + Kaccx*OP(10,3)*SK_ACCX(2) - Kaccx*OP(10,5)*SK_ACCX(5) + Kaccx*OP(10,6)*SK_ACCX(4) + Kaccx*OP(10,24)*SK_ACCX(5));
Kfusion(11) = SK_ACCX(1)*(Kaccx*OP(11,23)*SH_ACCX(1) - Kaccx*OP(11,4)*SH_ACCX(1) + Kaccx*OP(11,2)*SK_ACCX(3) + Kaccx*OP(11,3)*SK_ACCX(2) - Kaccx*OP(11,5)*SK_ACCX(5) + Kaccx*OP(11,6)*SK_ACCX(4) + Kaccx*OP(11,24)*SK_ACCX(5));
Kfusion(12) = SK_ACCX(1)*(Kaccx*OP(12,23)*SH_ACCX(1) - Kaccx*OP(12,4)*SH_ACCX(1) + Kaccx*OP(12,2)*SK_ACCX(3) + Kaccx*OP(12,3)*SK_ACCX(2) - Kaccx*OP(12,5)*SK_ACCX(5) + Kaccx*OP(12,6)*SK_ACCX(4) + Kaccx*OP(12,24)*SK_ACCX(5));
Kfusion(13) = SK_ACCX(1)*(Kaccx*OP(13,23)*SH_ACCX(1) - Kaccx*OP(13,4)*SH_ACCX(1) + Kaccx*OP(13,2)*SK_ACCX(3) + Kaccx*OP(13,3)*SK_ACCX(2) - Kaccx*OP(13,5)*SK_ACCX(5) + Kaccx*OP(13,6)*SK_ACCX(4) + Kaccx*OP(13,24)*SK_ACCX(5));
Kfusion(14) = SK_ACCX(1)*(Kaccx*OP(14,23)*SH_ACCX(1) - Kaccx*OP(14,4)*SH_ACCX(1) + Kaccx*OP(14,2)*SK_ACCX(3) + Kaccx*OP(14,3)*SK_ACCX(2) - Kaccx*OP(14,5)*SK_ACCX(5) + Kaccx*OP(14,6)*SK_ACCX(4) + Kaccx*OP(14,24)*SK_ACCX(5));
Kfusion(15) = SK_ACCX(1)*(Kaccx*OP(15,23)*SH_ACCX(1) - Kaccx*OP(15,4)*SH_ACCX(1) + Kaccx*OP(15,2)*SK_ACCX(3) + Kaccx*OP(15,3)*SK_ACCX(2) - Kaccx*OP(15,5)*SK_ACCX(5) + Kaccx*OP(15,6)*SK_ACCX(4) + Kaccx*OP(15,24)*SK_ACCX(5));
Kfusion(16) = SK_ACCX(1)*(Kaccx*OP(16,23)*SH_ACCX(1) - Kaccx*OP(16,4)*SH_ACCX(1) + Kaccx*OP(16,2)*SK_ACCX(3) + Kaccx*OP(16,3)*SK_ACCX(2) - Kaccx*OP(16,5)*SK_ACCX(5) + Kaccx*OP(16,6)*SK_ACCX(4) + Kaccx*OP(16,24)*SK_ACCX(5));
Kfusion(17) = SK_ACCX(1)*(Kaccx*OP(17,23)*SH_ACCX(1) - Kaccx*OP(17,4)*SH_ACCX(1) + Kaccx*OP(17,2)*SK_ACCX(3) + Kaccx*OP(17,3)*SK_ACCX(2) - Kaccx*OP(17,5)*SK_ACCX(5) + Kaccx*OP(17,6)*SK_ACCX(4) + Kaccx*OP(17,24)*SK_ACCX(5));
Kfusion(18) = SK_ACCX(1)*(Kaccx*OP(18,23)*SH_ACCX(1) - Kaccx*OP(18,4)*SH_ACCX(1) + Kaccx*OP(18,2)*SK_ACCX(3) + Kaccx*OP(18,3)*SK_ACCX(2) - Kaccx*OP(18,5)*SK_ACCX(5) + Kaccx*OP(18,6)*SK_ACCX(4) + Kaccx*OP(18,24)*SK_ACCX(5));
Kfusion(19) = SK_ACCX(1)*(Kaccx*OP(19,23)*SH_ACCX(1) - Kaccx*OP(19,4)*SH_ACCX(1) + Kaccx*OP(19,2)*SK_ACCX(3) + Kaccx*OP(19,3)*SK_ACCX(2) - Kaccx*OP(19,5)*SK_ACCX(5) + Kaccx*OP(19,6)*SK_ACCX(4) + Kaccx*OP(19,24)*SK_ACCX(5));
Kfusion(20) = SK_ACCX(1)*(Kaccx*OP(20,23)*SH_ACCX(1) - Kaccx*OP(20,4)*SH_ACCX(1) + Kaccx*OP(20,2)*SK_ACCX(3) + Kaccx*OP(20,3)*SK_ACCX(2) - Kaccx*OP(20,5)*SK_ACCX(5) + Kaccx*OP(20,6)*SK_ACCX(4) + Kaccx*OP(20,24)*SK_ACCX(5));
Kfusion(21) = SK_ACCX(1)*(Kaccx*OP(21,23)*SH_ACCX(1) - Kaccx*OP(21,4)*SH_ACCX(1) + Kaccx*OP(21,2)*SK_ACCX(3) + Kaccx*OP(21,3)*SK_ACCX(2) - Kaccx*OP(21,5)*SK_ACCX(5) + Kaccx*OP(21,6)*SK_ACCX(4) + Kaccx*OP(21,24)*SK_ACCX(5));
Kfusion(22) = SK_ACCX(1)*(Kaccx*OP(22,23)*SH_ACCX(1) - Kaccx*OP(22,4)*SH_ACCX(1) + Kaccx*OP(22,2)*SK_ACCX(3) + Kaccx*OP(22,3)*SK_ACCX(2) - Kaccx*OP(22,5)*SK_ACCX(5) + Kaccx*OP(22,6)*SK_ACCX(4) + Kaccx*OP(22,24)*SK_ACCX(5));
Kfusion(23) = SK_ACCX(1)*(Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(23,4)*SH_ACCX(1) + Kaccx*OP(23,2)*SK_ACCX(3) + Kaccx*OP(23,3)*SK_ACCX(2) - Kaccx*OP(23,5)*SK_ACCX(5) + Kaccx*OP(23,6)*SK_ACCX(4) + Kaccx*OP(23,24)*SK_ACCX(5));
Kfusion(24) = SK_ACCX(1)*(Kaccx*OP(24,23)*SH_ACCX(1) - Kaccx*OP(24,4)*SH_ACCX(1) + Kaccx*OP(24,2)*SK_ACCX(3) + Kaccx*OP(24,3)*SK_ACCX(2) - Kaccx*OP(24,5)*SK_ACCX(5) + Kaccx*OP(24,6)*SK_ACCX(4) + Kaccx*OP(24,24)*SK_ACCX(5));
SH_ACCY = zeros(6,1);
SH_ACCY(1) = q0^2 - q1^2 + q2^2 - q3^2;
SH_ACCY(2) = ve - vwe;
SH_ACCY(3) = vn - vwn;
SH_ACCY(4) = q1^2;
SH_ACCY(5) = 2*q0*q1;
SH_ACCY(6) = 2*q0*q3;
H_ACCY = zeros(1,24);
H_ACCY(1,1) = Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2));
H_ACCY(1,3) = Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3));
H_ACCY(1,4) = Kaccy*(SH_ACCY(6) - 2*q1*q2);
H_ACCY(1,5) = -Kaccy*SH_ACCY(1);
H_ACCY(1,6) = -Kaccy*(SH_ACCY(5) + 2*q2*q3);
H_ACCY(1,23) = -2*Kaccy*(q0*q3 - q1*q2);
H_ACCY(1,24) = Kaccy*SH_ACCY(1);
SK_ACCY = zeros(7,1);
SK_ACCY(1) = 1/(R_ACC - Kaccy*SH_ACCY(1)*(Kaccy*OP(24,5)*SH_ACCY(1) - Kaccy*OP(5,5)*SH_ACCY(1) + Kaccy*OP(1,5)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,5)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,5)*(q0*q3 - q1*q2) + Kaccy*OP(4,5)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,5)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*SH_ACCY(1)*(Kaccy*OP(24,24)*SH_ACCY(1) - Kaccy*OP(5,24)*SH_ACCY(1) + Kaccy*OP(1,24)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,24)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,24)*(q0*q3 - q1*q2) + Kaccy*OP(4,24)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,24)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2))*(Kaccy*OP(24,1)*SH_ACCY(1) - Kaccy*OP(5,1)*SH_ACCY(1) + Kaccy*OP(1,1)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,1)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,1)*(q0*q3 - q1*q2) + Kaccy*OP(4,1)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,1)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3))*(Kaccy*OP(24,3)*SH_ACCY(1) - Kaccy*OP(5,3)*SH_ACCY(1) + Kaccy*OP(1,3)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,3)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,3)*(q0*q3 - q1*q2) + Kaccy*OP(4,3)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,3)*(SH_ACCY(5) + 2*q2*q3)) - 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP(24,23)*SH_ACCY(1) - Kaccy*OP(5,23)*SH_ACCY(1) + Kaccy*OP(1,23)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,23)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,23)*(q0*q3 - q1*q2) + Kaccy*OP(4,23)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,23)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(6) - 2*q1*q2)*(Kaccy*OP(24,4)*SH_ACCY(1) - Kaccy*OP(5,4)*SH_ACCY(1) + Kaccy*OP(1,4)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,4)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,4)*(q0*q3 - q1*q2) + Kaccy*OP(4,4)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,4)*(SH_ACCY(5) + 2*q2*q3)) - Kaccy*(SH_ACCY(5) + 2*q2*q3)*(Kaccy*OP(24,6)*SH_ACCY(1) - Kaccy*OP(5,6)*SH_ACCY(1) + Kaccy*OP(1,6)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,6)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,6)*(q0*q3 - q1*q2) + Kaccy*OP(4,6)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,6)*(SH_ACCY(5) + 2*q2*q3)));
SK_ACCY(2) = SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3);
SK_ACCY(3) = SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2);
SK_ACCY(4) = q0*q3 - q1*q2;
SK_ACCY(5) = SH_ACCY(6) - 2*q1*q2;
SK_ACCY(6) = SH_ACCY(5) + 2*q2*q3;
SK_ACCY(7) = SH_ACCY(1);
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_ACCY(1)*(Kaccy*OP(1,1)*SK_ACCY(3) + Kaccy*OP(1,3)*SK_ACCY(2) + Kaccy*OP(1,4)*SK_ACCY(5) - Kaccy*OP(1,5)*SK_ACCY(7) - Kaccy*OP(1,6)*SK_ACCY(6) - 2*Kaccy*OP(1,23)*SK_ACCY(4) + Kaccy*OP(1,24)*SK_ACCY(7));
Kfusion(2) = SK_ACCY(1)*(Kaccy*OP(2,1)*SK_ACCY(3) + Kaccy*OP(2,3)*SK_ACCY(2) + Kaccy*OP(2,4)*SK_ACCY(5) - Kaccy*OP(2,5)*SK_ACCY(7) - Kaccy*OP(2,6)*SK_ACCY(6) - 2*Kaccy*OP(2,23)*SK_ACCY(4) + Kaccy*OP(2,24)*SK_ACCY(7));
Kfusion(3) = SK_ACCY(1)*(Kaccy*OP(3,1)*SK_ACCY(3) + Kaccy*OP(3,3)*SK_ACCY(2) + Kaccy*OP(3,4)*SK_ACCY(5) - Kaccy*OP(3,5)*SK_ACCY(7) - Kaccy*OP(3,6)*SK_ACCY(6) - 2*Kaccy*OP(3,23)*SK_ACCY(4) + Kaccy*OP(3,24)*SK_ACCY(7));
Kfusion(4) = SK_ACCY(1)*(Kaccy*OP(4,1)*SK_ACCY(3) + Kaccy*OP(4,3)*SK_ACCY(2) + Kaccy*OP(4,4)*SK_ACCY(5) - Kaccy*OP(4,5)*SK_ACCY(7) - Kaccy*OP(4,6)*SK_ACCY(6) - 2*Kaccy*OP(4,23)*SK_ACCY(4) + Kaccy*OP(4,24)*SK_ACCY(7));
Kfusion(5) = SK_ACCY(1)*(Kaccy*OP(5,1)*SK_ACCY(3) + Kaccy*OP(5,3)*SK_ACCY(2) + Kaccy*OP(5,4)*SK_ACCY(5) - Kaccy*OP(5,5)*SK_ACCY(7) - Kaccy*OP(5,6)*SK_ACCY(6) - 2*Kaccy*OP(5,23)*SK_ACCY(4) + Kaccy*OP(5,24)*SK_ACCY(7));
Kfusion(6) = SK_ACCY(1)*(Kaccy*OP(6,1)*SK_ACCY(3) + Kaccy*OP(6,3)*SK_ACCY(2) + Kaccy*OP(6,4)*SK_ACCY(5) - Kaccy*OP(6,5)*SK_ACCY(7) - Kaccy*OP(6,6)*SK_ACCY(6) - 2*Kaccy*OP(6,23)*SK_ACCY(4) + Kaccy*OP(6,24)*SK_ACCY(7));
Kfusion(7) = SK_ACCY(1)*(Kaccy*OP(7,1)*SK_ACCY(3) + Kaccy*OP(7,3)*SK_ACCY(2) + Kaccy*OP(7,4)*SK_ACCY(5) - Kaccy*OP(7,5)*SK_ACCY(7) - Kaccy*OP(7,6)*SK_ACCY(6) - 2*Kaccy*OP(7,23)*SK_ACCY(4) + Kaccy*OP(7,24)*SK_ACCY(7));
Kfusion(8) = SK_ACCY(1)*(Kaccy*OP(8,1)*SK_ACCY(3) + Kaccy*OP(8,3)*SK_ACCY(2) + Kaccy*OP(8,4)*SK_ACCY(5) - Kaccy*OP(8,5)*SK_ACCY(7) - Kaccy*OP(8,6)*SK_ACCY(6) - 2*Kaccy*OP(8,23)*SK_ACCY(4) + Kaccy*OP(8,24)*SK_ACCY(7));
Kfusion(9) = SK_ACCY(1)*(Kaccy*OP(9,1)*SK_ACCY(3) + Kaccy*OP(9,3)*SK_ACCY(2) + Kaccy*OP(9,4)*SK_ACCY(5) - Kaccy*OP(9,5)*SK_ACCY(7) - Kaccy*OP(9,6)*SK_ACCY(6) - 2*Kaccy*OP(9,23)*SK_ACCY(4) + Kaccy*OP(9,24)*SK_ACCY(7));
Kfusion(10) = SK_ACCY(1)*(Kaccy*OP(10,1)*SK_ACCY(3) + Kaccy*OP(10,3)*SK_ACCY(2) + Kaccy*OP(10,4)*SK_ACCY(5) - Kaccy*OP(10,5)*SK_ACCY(7) - Kaccy*OP(10,6)*SK_ACCY(6) - 2*Kaccy*OP(10,23)*SK_ACCY(4) + Kaccy*OP(10,24)*SK_ACCY(7));
Kfusion(11) = SK_ACCY(1)*(Kaccy*OP(11,1)*SK_ACCY(3) + Kaccy*OP(11,3)*SK_ACCY(2) + Kaccy*OP(11,4)*SK_ACCY(5) - Kaccy*OP(11,5)*SK_ACCY(7) - Kaccy*OP(11,6)*SK_ACCY(6) - 2*Kaccy*OP(11,23)*SK_ACCY(4) + Kaccy*OP(11,24)*SK_ACCY(7));
Kfusion(12) = SK_ACCY(1)*(Kaccy*OP(12,1)*SK_ACCY(3) + Kaccy*OP(12,3)*SK_ACCY(2) + Kaccy*OP(12,4)*SK_ACCY(5) - Kaccy*OP(12,5)*SK_ACCY(7) - Kaccy*OP(12,6)*SK_ACCY(6) - 2*Kaccy*OP(12,23)*SK_ACCY(4) + Kaccy*OP(12,24)*SK_ACCY(7));
Kfusion(13) = SK_ACCY(1)*(Kaccy*OP(13,1)*SK_ACCY(3) + Kaccy*OP(13,3)*SK_ACCY(2) + Kaccy*OP(13,4)*SK_ACCY(5) - Kaccy*OP(13,5)*SK_ACCY(7) - Kaccy*OP(13,6)*SK_ACCY(6) - 2*Kaccy*OP(13,23)*SK_ACCY(4) + Kaccy*OP(13,24)*SK_ACCY(7));
Kfusion(14) = SK_ACCY(1)*(Kaccy*OP(14,1)*SK_ACCY(3) + Kaccy*OP(14,3)*SK_ACCY(2) + Kaccy*OP(14,4)*SK_ACCY(5) - Kaccy*OP(14,5)*SK_ACCY(7) - Kaccy*OP(14,6)*SK_ACCY(6) - 2*Kaccy*OP(14,23)*SK_ACCY(4) + Kaccy*OP(14,24)*SK_ACCY(7));
Kfusion(15) = SK_ACCY(1)*(Kaccy*OP(15,1)*SK_ACCY(3) + Kaccy*OP(15,3)*SK_ACCY(2) + Kaccy*OP(15,4)*SK_ACCY(5) - Kaccy*OP(15,5)*SK_ACCY(7) - Kaccy*OP(15,6)*SK_ACCY(6) - 2*Kaccy*OP(15,23)*SK_ACCY(4) + Kaccy*OP(15,24)*SK_ACCY(7));
Kfusion(16) = SK_ACCY(1)*(Kaccy*OP(16,1)*SK_ACCY(3) + Kaccy*OP(16,3)*SK_ACCY(2) + Kaccy*OP(16,4)*SK_ACCY(5) - Kaccy*OP(16,5)*SK_ACCY(7) - Kaccy*OP(16,6)*SK_ACCY(6) - 2*Kaccy*OP(16,23)*SK_ACCY(4) + Kaccy*OP(16,24)*SK_ACCY(7));
Kfusion(17) = SK_ACCY(1)*(Kaccy*OP(17,1)*SK_ACCY(3) + Kaccy*OP(17,3)*SK_ACCY(2) + Kaccy*OP(17,4)*SK_ACCY(5) - Kaccy*OP(17,5)*SK_ACCY(7) - Kaccy*OP(17,6)*SK_ACCY(6) - 2*Kaccy*OP(17,23)*SK_ACCY(4) + Kaccy*OP(17,24)*SK_ACCY(7));
Kfusion(18) = SK_ACCY(1)*(Kaccy*OP(18,1)*SK_ACCY(3) + Kaccy*OP(18,3)*SK_ACCY(2) + Kaccy*OP(18,4)*SK_ACCY(5) - Kaccy*OP(18,5)*SK_ACCY(7) - Kaccy*OP(18,6)*SK_ACCY(6) - 2*Kaccy*OP(18,23)*SK_ACCY(4) + Kaccy*OP(18,24)*SK_ACCY(7));
Kfusion(19) = SK_ACCY(1)*(Kaccy*OP(19,1)*SK_ACCY(3) + Kaccy*OP(19,3)*SK_ACCY(2) + Kaccy*OP(19,4)*SK_ACCY(5) - Kaccy*OP(19,5)*SK_ACCY(7) - Kaccy*OP(19,6)*SK_ACCY(6) - 2*Kaccy*OP(19,23)*SK_ACCY(4) + Kaccy*OP(19,24)*SK_ACCY(7));
Kfusion(20) = SK_ACCY(1)*(Kaccy*OP(20,1)*SK_ACCY(3) + Kaccy*OP(20,3)*SK_ACCY(2) + Kaccy*OP(20,4)*SK_ACCY(5) - Kaccy*OP(20,5)*SK_ACCY(7) - Kaccy*OP(20,6)*SK_ACCY(6) - 2*Kaccy*OP(20,23)*SK_ACCY(4) + Kaccy*OP(20,24)*SK_ACCY(7));
Kfusion(21) = SK_ACCY(1)*(Kaccy*OP(21,1)*SK_ACCY(3) + Kaccy*OP(21,3)*SK_ACCY(2) + Kaccy*OP(21,4)*SK_ACCY(5) - Kaccy*OP(21,5)*SK_ACCY(7) - Kaccy*OP(21,6)*SK_ACCY(6) - 2*Kaccy*OP(21,23)*SK_ACCY(4) + Kaccy*OP(21,24)*SK_ACCY(7));
Kfusion(22) = SK_ACCY(1)*(Kaccy*OP(22,1)*SK_ACCY(3) + Kaccy*OP(22,3)*SK_ACCY(2) + Kaccy*OP(22,4)*SK_ACCY(5) - Kaccy*OP(22,5)*SK_ACCY(7) - Kaccy*OP(22,6)*SK_ACCY(6) - 2*Kaccy*OP(22,23)*SK_ACCY(4) + Kaccy*OP(22,24)*SK_ACCY(7));
Kfusion(23) = SK_ACCY(1)*(Kaccy*OP(23,1)*SK_ACCY(3) + Kaccy*OP(23,3)*SK_ACCY(2) + Kaccy*OP(23,4)*SK_ACCY(5) - Kaccy*OP(23,5)*SK_ACCY(7) - Kaccy*OP(23,6)*SK_ACCY(6) - 2*Kaccy*OP(23,23)*SK_ACCY(4) + Kaccy*OP(23,24)*SK_ACCY(7));
Kfusion(24) = SK_ACCY(1)*(Kaccy*OP(24,1)*SK_ACCY(3) + Kaccy*OP(24,3)*SK_ACCY(2) + Kaccy*OP(24,4)*SK_ACCY(5) - Kaccy*OP(24,5)*SK_ACCY(7) - Kaccy*OP(24,6)*SK_ACCY(6) - 2*Kaccy*OP(24,23)*SK_ACCY(4) + Kaccy*OP(24,24)*SK_ACCY(7));
View Git Blame Copy Permalink