SF = zeros(21,1);
SF(1) = dvz - dvz_b;
SF(2) = dvy - dvy_b;
SF(3) = dvx - dvx_b;
SF(4) = 2*q1*SF(3) + 2*q2*SF(2) + 2*q3*SF(1);
SF(5) = 2*q0*SF(2) - 2*q1*SF(1) + 2*q3*SF(3);
SF(6) = 2*q0*SF(3) + 2*q2*SF(1) - 2*q3*SF(2);
SF(7) = day/2 - day_b/2;
SF(8) = daz/2 - daz_b/2;
SF(9) = dax/2 - dax_b/2;
SF(10) = dax_b/2 - dax/2;
SF(11) = daz_b/2 - daz/2;
SF(12) = day_b/2 - day/2;
SF(13) = 2*q1*SF(2);
SF(14) = 2*q0*SF(1);
SF(15) = q1/2;
SF(16) = q2/2;
SF(17) = q3/2;
SF(18) = q3^2;
SF(19) = q2^2;
SF(20) = q1^2;
SF(21) = q0^2;

SG = zeros(8,1);
SG(1) = q0/2;
SG(2) = q3^2;
SG(3) = q2^2;
SG(4) = q1^2;
SG(5) = q0^2;
SG(6) = 2*q2*q3;
SG(7) = 2*q1*q3;
SG(8) = 2*q1*q2;


SQ = zeros(11,1);
SQ(1) = dvzVar*(SG(6) - 2*q0*q1)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyVar*(SG(6) + 2*q0*q1)*(SG(2) - SG(3) + SG(4) - SG(5)) + dvxVar*(SG(7) - 2*q0*q2)*(SG(8) + 2*q0*q3);
SQ(2) = dvzVar*(SG(7) + 2*q0*q2)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxVar*(SG(7) - 2*q0*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvyVar*(SG(6) + 2*q0*q1)*(SG(8) - 2*q0*q3);
SQ(3) = dvzVar*(SG(6) - 2*q0*q1)*(SG(7) + 2*q0*q2) - dvyVar*(SG(8) - 2*q0*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxVar*(SG(8) + 2*q0*q3)*(SG(2) + SG(3) - SG(4) - SG(5));
SQ(4) = (dayVar*q1*SG(1))/2 - (dazVar*q1*SG(1))/2 - (daxVar*q2*q3)/4;
SQ(5) = (dazVar*q2*SG(1))/2 - (daxVar*q2*SG(1))/2 - (dayVar*q1*q3)/4;
SQ(6) = (daxVar*q3*SG(1))/2 - (dayVar*q3*SG(1))/2 - (dazVar*q1*q2)/4;
SQ(7) = (daxVar*q1*q2)/4 - (dazVar*q3*SG(1))/2 - (dayVar*q1*q2)/4;
SQ(8) = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG(1))/2;
SQ(9) = (dayVar*q2*q3)/4 - (daxVar*q1*SG(1))/2 - (dazVar*q2*q3)/4;
SQ(10) = SG(1)^2;
SQ(11) = q1^2;


SPP = zeros(11,1);
SPP(1) = SF(13) + SF(14) - 2*q2*SF(3);
SPP(2) = SF(18) - SF(19) - SF(20) + SF(21);
SPP(3) = SF(18) - SF(19) + SF(20) - SF(21);
SPP(4) = SF(18) + SF(19) - SF(20) - SF(21);
SPP(5) = 2*q0*q2 - 2*q1*q3;
SPP(6) = 2*q0*q1 - 2*q2*q3;
SPP(7) = 2*q0*q3 - 2*q1*q2;
SPP(8) = 2*q0*q1 + 2*q2*q3;
SPP(9) = 2*q0*q3 + 2*q1*q2;
SPP(10) = 2*q0*q2 + 2*q1*q3;
SPP(11) = SF(17);


nextP = zeros(24,24);
nextP(1,1) = OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11) + (daxVar*SQ(11))/4 + SF(10)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(12)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(11)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SF(15)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) + SF(16)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) + SPP(11)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) + (dayVar*q2^2)/4 + (dazVar*q3^2)/4;
nextP(1,2) = OP_l_1_c_2_r_ + SQ(9) + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11) + SF(9)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(8)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(12)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) - SF(16)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) + SPP(11)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) - (q0*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)))/2;
nextP(2,2) = OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) + daxVar*SQ(10) - (OP_l_11_c_2_r_*q0)/2 + SF(9)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(8)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(12)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) - SF(16)*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2) + SPP(11)*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2) + (dayVar*q3^2)/4 + (dazVar*q2^2)/4 - (q0*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2))/2;
nextP(1,3) = OP_l_1_c_3_r_ + SQ(8) + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11) + SF(7)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(11)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(9)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SF(15)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) - SPP(11)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) - (q0*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)))/2;
nextP(2,3) = OP_l_2_c_3_r_ + SQ(6) + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2 + SF(7)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(11)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) + SF(9)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SF(15)*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2) - SPP(11)*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2) - (q0*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2))/2;
nextP(3,3) = OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) + dayVar*SQ(10) + (dazVar*SQ(11))/4 - (OP_l_12_c_3_r_*q0)/2 + SF(7)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(11)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) + SF(9)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SF(15)*(OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2) - SPP(11)*(OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2) + (daxVar*q3^2)/4 - (q0*(OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2))/2;
nextP(1,4) = OP_l_1_c_4_r_ + SQ(7) + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11) + SF(8)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(7)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(10)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(16)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) - SF(15)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) - (q0*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)))/2;
nextP(2,4) = OP_l_2_c_4_r_ + SQ(5) + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2 + SF(8)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(7)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) + SF(10)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(16)*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2) - SF(15)*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2) - (q0*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2))/2;
nextP(3,4) = OP_l_3_c_4_r_ + SQ(4) + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2 + SF(8)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(7)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) + SF(10)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(16)*(OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2) - SF(15)*(OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2) - (q0*(OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2))/2;
nextP(4,4) = OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) + (dayVar*SQ(11))/4 + dazVar*SQ(10) - (OP_l_13_c_4_r_*q0)/2 + SF(8)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(7)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) + SF(10)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(16)*(OP_l_4_c_11_r_ + OP_l_1_c_11_r_*SF(8) + OP_l_2_c_11_r_*SF(7) + OP_l_3_c_11_r_*SF(10) + OP_l_11_c_11_r_*SF(16) - OP_l_12_c_11_r_*SF(15) - (OP_l_13_c_11_r_*q0)/2) - SF(15)*(OP_l_4_c_12_r_ + OP_l_1_c_12_r_*SF(8) + OP_l_2_c_12_r_*SF(7) + OP_l_3_c_12_r_*SF(10) + OP_l_11_c_12_r_*SF(16) - OP_l_12_c_12_r_*SF(15) - (OP_l_13_c_12_r_*q0)/2) + (daxVar*q2^2)/4 - (q0*(OP_l_4_c_13_r_ + OP_l_1_c_13_r_*SF(8) + OP_l_2_c_13_r_*SF(7) + OP_l_3_c_13_r_*SF(10) + OP_l_11_c_13_r_*SF(16) - OP_l_12_c_13_r_*SF(15) - (OP_l_13_c_13_r_*q0)/2))/2;
nextP(1,5) = OP_l_1_c_5_r_ + OP_l_2_c_5_r_*SF(10) + OP_l_3_c_5_r_*SF(12) + OP_l_4_c_5_r_*SF(11) + OP_l_11_c_5_r_*SF(15) + OP_l_12_c_5_r_*SF(16) + OP_l_13_c_5_r_*SPP(11) + SF(6)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(4)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SF(5)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SPP(1)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SPP(4)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) + SPP(7)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) - SPP(10)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11));
nextP(2,5) = OP_l_2_c_5_r_ + OP_l_1_c_5_r_*SF(9) + OP_l_3_c_5_r_*SF(8) + OP_l_4_c_5_r_*SF(12) - OP_l_13_c_5_r_*SF(16) + OP_l_12_c_5_r_*SPP(11) - (OP_l_11_c_5_r_*q0)/2 + SF(6)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(4)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SF(5)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SPP(1)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SPP(4)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) + SPP(7)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) - SPP(10)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2);
nextP(3,5) = OP_l_3_c_5_r_ + OP_l_1_c_5_r_*SF(7) + OP_l_2_c_5_r_*SF(11) + OP_l_4_c_5_r_*SF(9) + OP_l_13_c_5_r_*SF(15) - OP_l_11_c_5_r_*SPP(11) - (OP_l_12_c_5_r_*q0)/2 + SF(6)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(4)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SF(5)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SPP(1)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SPP(4)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) + SPP(7)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) - SPP(10)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2);
nextP(4,5) = OP_l_4_c_5_r_ + OP_l_1_c_5_r_*SF(8) + OP_l_2_c_5_r_*SF(7) + OP_l_3_c_5_r_*SF(10) + OP_l_11_c_5_r_*SF(16) - OP_l_12_c_5_r_*SF(15) - (OP_l_13_c_5_r_*q0)/2 + SF(6)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(4)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SF(5)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) + SPP(1)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SPP(4)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) + SPP(7)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) - SPP(10)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2);
nextP(5,5) = OP_l_5_c_5_r_ + OP_l_1_c_5_r_*SF(6) + OP_l_2_c_5_r_*SF(4) - OP_l_4_c_5_r_*SF(5) + OP_l_3_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(4) + OP_l_15_c_5_r_*SPP(7) - OP_l_16_c_5_r_*SPP(10) + dvyVar*(SG(8) - 2*q0*q3)^2 + dvzVar*(SG(7) + 2*q0*q2)^2 + SF(6)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SF(4)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SF(5)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) + SPP(1)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SPP(4)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) + SPP(7)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) - SPP(10)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10)) + dvxVar*(SG(2) + SG(3) - SG(4) - SG(5))^2;
nextP(1,6) = OP_l_1_c_6_r_ + OP_l_2_c_6_r_*SF(10) + OP_l_3_c_6_r_*SF(12) + OP_l_4_c_6_r_*SF(11) + OP_l_11_c_6_r_*SF(15) + OP_l_12_c_6_r_*SF(16) + OP_l_13_c_6_r_*SPP(11) + SF(5)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(4)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(6)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) - SPP(1)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SPP(9)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) + SPP(3)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) + SPP(6)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11));
nextP(2,6) = OP_l_2_c_6_r_ + OP_l_1_c_6_r_*SF(9) + OP_l_3_c_6_r_*SF(8) + OP_l_4_c_6_r_*SF(12) - OP_l_13_c_6_r_*SF(16) + OP_l_12_c_6_r_*SPP(11) - (OP_l_11_c_6_r_*q0)/2 + SF(5)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(4)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(6)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) - SPP(1)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SPP(9)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) + SPP(3)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) + SPP(6)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2);
nextP(3,6) = OP_l_3_c_6_r_ + OP_l_1_c_6_r_*SF(7) + OP_l_2_c_6_r_*SF(11) + OP_l_4_c_6_r_*SF(9) + OP_l_13_c_6_r_*SF(15) - OP_l_11_c_6_r_*SPP(11) - (OP_l_12_c_6_r_*q0)/2 + SF(5)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(4)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(6)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) - SPP(1)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SPP(9)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) + SPP(3)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) + SPP(6)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2);
nextP(4,6) = OP_l_4_c_6_r_ + OP_l_1_c_6_r_*SF(8) + OP_l_2_c_6_r_*SF(7) + OP_l_3_c_6_r_*SF(10) + OP_l_11_c_6_r_*SF(16) - OP_l_12_c_6_r_*SF(15) - (OP_l_13_c_6_r_*q0)/2 + SF(5)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(4)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(6)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) - SPP(1)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SPP(9)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) + SPP(3)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) + SPP(6)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2);
nextP(5,6) = OP_l_5_c_6_r_ + SQ(3) + OP_l_1_c_6_r_*SF(6) + OP_l_2_c_6_r_*SF(4) - OP_l_4_c_6_r_*SF(5) + OP_l_3_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(4) + OP_l_15_c_6_r_*SPP(7) - OP_l_16_c_6_r_*SPP(10) + SF(5)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SF(4)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SF(6)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) - SPP(1)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SPP(9)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) + SPP(3)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) + SPP(6)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10));
nextP(6,6) = OP_l_6_c_6_r_ + OP_l_1_c_6_r_*SF(5) + OP_l_3_c_6_r_*SF(4) + OP_l_4_c_6_r_*SF(6) - OP_l_2_c_6_r_*SPP(1) - OP_l_14_c_6_r_*SPP(9) + OP_l_15_c_6_r_*SPP(3) + OP_l_16_c_6_r_*SPP(6) + dvxVar*(SG(8) + 2*q0*q3)^2 + dvzVar*(SG(6) - 2*q0*q1)^2 + SF(5)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SF(5) + OP_l_3_c_1_r_*SF(4) + OP_l_4_c_1_r_*SF(6) - OP_l_2_c_1_r_*SPP(1) - OP_l_14_c_1_r_*SPP(9) + OP_l_15_c_1_r_*SPP(3) + OP_l_16_c_1_r_*SPP(6)) + SF(4)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SF(5) + OP_l_3_c_3_r_*SF(4) + OP_l_4_c_3_r_*SF(6) - OP_l_2_c_3_r_*SPP(1) - OP_l_14_c_3_r_*SPP(9) + OP_l_15_c_3_r_*SPP(3) + OP_l_16_c_3_r_*SPP(6)) + SF(6)*(OP_l_6_c_4_r_ + OP_l_1_c_4_r_*SF(5) + OP_l_3_c_4_r_*SF(4) + OP_l_4_c_4_r_*SF(6) - OP_l_2_c_4_r_*SPP(1) - OP_l_14_c_4_r_*SPP(9) + OP_l_15_c_4_r_*SPP(3) + OP_l_16_c_4_r_*SPP(6)) - SPP(1)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SF(5) + OP_l_3_c_2_r_*SF(4) + OP_l_4_c_2_r_*SF(6) - OP_l_2_c_2_r_*SPP(1) - OP_l_14_c_2_r_*SPP(9) + OP_l_15_c_2_r_*SPP(3) + OP_l_16_c_2_r_*SPP(6)) - SPP(9)*(OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6)) + SPP(3)*(OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6)) + SPP(6)*(OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6)) + dvyVar*(SG(2) - SG(3) + SG(4) - SG(5))^2;
nextP(1,7) = OP_l_1_c_7_r_ + OP_l_2_c_7_r_*SF(10) + OP_l_3_c_7_r_*SF(12) + OP_l_4_c_7_r_*SF(11) + OP_l_11_c_7_r_*SF(15) + OP_l_12_c_7_r_*SF(16) + OP_l_13_c_7_r_*SPP(11) + SF(5)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SF(6)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(4)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SPP(1)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SPP(5)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) - SPP(8)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) - SPP(2)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11));
nextP(2,7) = OP_l_2_c_7_r_ + OP_l_1_c_7_r_*SF(9) + OP_l_3_c_7_r_*SF(8) + OP_l_4_c_7_r_*SF(12) - OP_l_13_c_7_r_*SF(16) + OP_l_12_c_7_r_*SPP(11) - (OP_l_11_c_7_r_*q0)/2 + SF(5)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SF(6)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(4)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SPP(1)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SPP(5)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) - SPP(8)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) - SPP(2)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2);
nextP(3,7) = OP_l_3_c_7_r_ + OP_l_1_c_7_r_*SF(7) + OP_l_2_c_7_r_*SF(11) + OP_l_4_c_7_r_*SF(9) + OP_l_13_c_7_r_*SF(15) - OP_l_11_c_7_r_*SPP(11) - (OP_l_12_c_7_r_*q0)/2 + SF(5)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SF(6)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(4)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SPP(1)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SPP(5)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) - SPP(8)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) - SPP(2)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2);
nextP(4,7) = OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SF(8) + OP_l_2_c_7_r_*SF(7) + OP_l_3_c_7_r_*SF(10) + OP_l_11_c_7_r_*SF(16) - OP_l_12_c_7_r_*SF(15) - (OP_l_13_c_7_r_*q0)/2 + SF(5)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SF(6)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(4)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) + SPP(1)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SPP(5)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) - SPP(8)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) - SPP(2)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2);
nextP(5,7) = OP_l_5_c_7_r_ + SQ(2) + OP_l_1_c_7_r_*SF(6) + OP_l_2_c_7_r_*SF(4) - OP_l_4_c_7_r_*SF(5) + OP_l_3_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(4) + OP_l_15_c_7_r_*SPP(7) - OP_l_16_c_7_r_*SPP(10) + SF(5)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SF(6)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SF(4)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) + SPP(1)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SPP(5)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) - SPP(8)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) - SPP(2)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10));
nextP(6,7) = OP_l_6_c_7_r_ + SQ(1) + OP_l_1_c_7_r_*SF(5) + OP_l_3_c_7_r_*SF(4) + OP_l_4_c_7_r_*SF(6) - OP_l_2_c_7_r_*SPP(1) - OP_l_14_c_7_r_*SPP(9) + OP_l_15_c_7_r_*SPP(3) + OP_l_16_c_7_r_*SPP(6) + SF(5)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SF(5) + OP_l_3_c_2_r_*SF(4) + OP_l_4_c_2_r_*SF(6) - OP_l_2_c_2_r_*SPP(1) - OP_l_14_c_2_r_*SPP(9) + OP_l_15_c_2_r_*SPP(3) + OP_l_16_c_2_r_*SPP(6)) - SF(6)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SF(5) + OP_l_3_c_3_r_*SF(4) + OP_l_4_c_3_r_*SF(6) - OP_l_2_c_3_r_*SPP(1) - OP_l_14_c_3_r_*SPP(9) + OP_l_15_c_3_r_*SPP(3) + OP_l_16_c_3_r_*SPP(6)) + SF(4)*(OP_l_6_c_4_r_ + OP_l_1_c_4_r_*SF(5) + OP_l_3_c_4_r_*SF(4) + OP_l_4_c_4_r_*SF(6) - OP_l_2_c_4_r_*SPP(1) - OP_l_14_c_4_r_*SPP(9) + OP_l_15_c_4_r_*SPP(3) + OP_l_16_c_4_r_*SPP(6)) + SPP(1)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SF(5) + OP_l_3_c_1_r_*SF(4) + OP_l_4_c_1_r_*SF(6) - OP_l_2_c_1_r_*SPP(1) - OP_l_14_c_1_r_*SPP(9) + OP_l_15_c_1_r_*SPP(3) + OP_l_16_c_1_r_*SPP(6)) + SPP(5)*(OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6)) - SPP(8)*(OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6)) - SPP(2)*(OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6));
nextP(7,7) = OP_l_7_c_7_r_ + OP_l_2_c_7_r_*SF(5) - OP_l_3_c_7_r_*SF(6) + OP_l_4_c_7_r_*SF(4) + OP_l_1_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(5) - OP_l_15_c_7_r_*SPP(8) - OP_l_16_c_7_r_*SPP(2) + dvxVar*(SG(7) - 2*q0*q2)^2 + dvyVar*(SG(6) + 2*q0*q1)^2 + SF(5)*(OP_l_7_c_2_r_ + OP_l_2_c_2_r_*SF(5) - OP_l_3_c_2_r_*SF(6) + OP_l_4_c_2_r_*SF(4) + OP_l_1_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(5) - OP_l_15_c_2_r_*SPP(8) - OP_l_16_c_2_r_*SPP(2)) - SF(6)*(OP_l_7_c_3_r_ + OP_l_2_c_3_r_*SF(5) - OP_l_3_c_3_r_*SF(6) + OP_l_4_c_3_r_*SF(4) + OP_l_1_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(5) - OP_l_15_c_3_r_*SPP(8) - OP_l_16_c_3_r_*SPP(2)) + SF(4)*(OP_l_7_c_4_r_ + OP_l_2_c_4_r_*SF(5) - OP_l_3_c_4_r_*SF(6) + OP_l_4_c_4_r_*SF(4) + OP_l_1_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(5) - OP_l_15_c_4_r_*SPP(8) - OP_l_16_c_4_r_*SPP(2)) + SPP(1)*(OP_l_7_c_1_r_ + OP_l_2_c_1_r_*SF(5) - OP_l_3_c_1_r_*SF(6) + OP_l_4_c_1_r_*SF(4) + OP_l_1_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(5) - OP_l_15_c_1_r_*SPP(8) - OP_l_16_c_1_r_*SPP(2)) + SPP(5)*(OP_l_7_c_14_r_ + OP_l_2_c_14_r_*SF(5) - OP_l_3_c_14_r_*SF(6) + OP_l_4_c_14_r_*SF(4) + OP_l_1_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(5) - OP_l_15_c_14_r_*SPP(8) - OP_l_16_c_14_r_*SPP(2)) - SPP(8)*(OP_l_7_c_15_r_ + OP_l_2_c_15_r_*SF(5) - OP_l_3_c_15_r_*SF(6) + OP_l_4_c_15_r_*SF(4) + OP_l_1_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(5) - OP_l_15_c_15_r_*SPP(8) - OP_l_16_c_15_r_*SPP(2)) - SPP(2)*(OP_l_7_c_16_r_ + OP_l_2_c_16_r_*SF(5) - OP_l_3_c_16_r_*SF(6) + OP_l_4_c_16_r_*SF(4) + OP_l_1_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(5) - OP_l_15_c_16_r_*SPP(8) - OP_l_16_c_16_r_*SPP(2)) + dvzVar*(SG(2) - SG(3) - SG(4) + SG(5))^2;
nextP(1,8) = OP_l_1_c_8_r_ + OP_l_2_c_8_r_*SF(10) + OP_l_3_c_8_r_*SF(12) + OP_l_4_c_8_r_*SF(11) + OP_l_11_c_8_r_*SF(15) + OP_l_12_c_8_r_*SF(16) + OP_l_13_c_8_r_*SPP(11) + dt*(OP_l_1_c_5_r_ + OP_l_2_c_5_r_*SF(10) + OP_l_3_c_5_r_*SF(12) + OP_l_4_c_5_r_*SF(11) + OP_l_11_c_5_r_*SF(15) + OP_l_12_c_5_r_*SF(16) + OP_l_13_c_5_r_*SPP(11));
nextP(2,8) = OP_l_2_c_8_r_ + OP_l_1_c_8_r_*SF(9) + OP_l_3_c_8_r_*SF(8) + OP_l_4_c_8_r_*SF(12) - OP_l_13_c_8_r_*SF(16) + OP_l_12_c_8_r_*SPP(11) - (OP_l_11_c_8_r_*q0)/2 + dt*(OP_l_2_c_5_r_ + OP_l_1_c_5_r_*SF(9) + OP_l_3_c_5_r_*SF(8) + OP_l_4_c_5_r_*SF(12) - OP_l_13_c_5_r_*SF(16) + OP_l_12_c_5_r_*SPP(11) - (OP_l_11_c_5_r_*q0)/2);
nextP(3,8) = OP_l_3_c_8_r_ + OP_l_1_c_8_r_*SF(7) + OP_l_2_c_8_r_*SF(11) + OP_l_4_c_8_r_*SF(9) + OP_l_13_c_8_r_*SF(15) - OP_l_11_c_8_r_*SPP(11) - (OP_l_12_c_8_r_*q0)/2 + dt*(OP_l_3_c_5_r_ + OP_l_1_c_5_r_*SF(7) + OP_l_2_c_5_r_*SF(11) + OP_l_4_c_5_r_*SF(9) + OP_l_13_c_5_r_*SF(15) - OP_l_11_c_5_r_*SPP(11) - (OP_l_12_c_5_r_*q0)/2);
nextP(4,8) = OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SF(8) + OP_l_2_c_8_r_*SF(7) + OP_l_3_c_8_r_*SF(10) + OP_l_11_c_8_r_*SF(16) - OP_l_12_c_8_r_*SF(15) - (OP_l_13_c_8_r_*q0)/2 + dt*(OP_l_4_c_5_r_ + OP_l_1_c_5_r_*SF(8) + OP_l_2_c_5_r_*SF(7) + OP_l_3_c_5_r_*SF(10) + OP_l_11_c_5_r_*SF(16) - OP_l_12_c_5_r_*SF(15) - (OP_l_13_c_5_r_*q0)/2);
nextP(5,8) = OP_l_5_c_8_r_ + OP_l_1_c_8_r_*SF(6) + OP_l_2_c_8_r_*SF(4) - OP_l_4_c_8_r_*SF(5) + OP_l_3_c_8_r_*SPP(1) + OP_l_14_c_8_r_*SPP(4) + OP_l_15_c_8_r_*SPP(7) - OP_l_16_c_8_r_*SPP(10) + dt*(OP_l_5_c_5_r_ + OP_l_1_c_5_r_*SF(6) + OP_l_2_c_5_r_*SF(4) - OP_l_4_c_5_r_*SF(5) + OP_l_3_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(4) + OP_l_15_c_5_r_*SPP(7) - OP_l_16_c_5_r_*SPP(10));
nextP(6,8) = OP_l_6_c_8_r_ + OP_l_1_c_8_r_*SF(5) + OP_l_3_c_8_r_*SF(4) + OP_l_4_c_8_r_*SF(6) - OP_l_2_c_8_r_*SPP(1) - OP_l_14_c_8_r_*SPP(9) + OP_l_15_c_8_r_*SPP(3) + OP_l_16_c_8_r_*SPP(6) + dt*(OP_l_6_c_5_r_ + OP_l_1_c_5_r_*SF(5) + OP_l_3_c_5_r_*SF(4) + OP_l_4_c_5_r_*SF(6) - OP_l_2_c_5_r_*SPP(1) - OP_l_14_c_5_r_*SPP(9) + OP_l_15_c_5_r_*SPP(3) + OP_l_16_c_5_r_*SPP(6));
nextP(7,8) = OP_l_7_c_8_r_ + OP_l_2_c_8_r_*SF(5) - OP_l_3_c_8_r_*SF(6) + OP_l_4_c_8_r_*SF(4) + OP_l_1_c_8_r_*SPP(1) + OP_l_14_c_8_r_*SPP(5) - OP_l_15_c_8_r_*SPP(8) - OP_l_16_c_8_r_*SPP(2) + dt*(OP_l_7_c_5_r_ + OP_l_2_c_5_r_*SF(5) - OP_l_3_c_5_r_*SF(6) + OP_l_4_c_5_r_*SF(4) + OP_l_1_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(5) - OP_l_15_c_5_r_*SPP(8) - OP_l_16_c_5_r_*SPP(2));
nextP(8,8) = OP_l_8_c_8_r_ + OP_l_5_c_8_r_*dt + dt*(OP_l_8_c_5_r_ + OP_l_5_c_5_r_*dt);
nextP(1,9) = OP_l_1_c_9_r_ + OP_l_2_c_9_r_*SF(10) + OP_l_3_c_9_r_*SF(12) + OP_l_4_c_9_r_*SF(11) + OP_l_11_c_9_r_*SF(15) + OP_l_12_c_9_r_*SF(16) + OP_l_13_c_9_r_*SPP(11) + dt*(OP_l_1_c_6_r_ + OP_l_2_c_6_r_*SF(10) + OP_l_3_c_6_r_*SF(12) + OP_l_4_c_6_r_*SF(11) + OP_l_11_c_6_r_*SF(15) + OP_l_12_c_6_r_*SF(16) + OP_l_13_c_6_r_*SPP(11));
nextP(2,9) = OP_l_2_c_9_r_ + OP_l_1_c_9_r_*SF(9) + OP_l_3_c_9_r_*SF(8) + OP_l_4_c_9_r_*SF(12) - OP_l_13_c_9_r_*SF(16) + OP_l_12_c_9_r_*SPP(11) - (OP_l_11_c_9_r_*q0)/2 + dt*(OP_l_2_c_6_r_ + OP_l_1_c_6_r_*SF(9) + OP_l_3_c_6_r_*SF(8) + OP_l_4_c_6_r_*SF(12) - OP_l_13_c_6_r_*SF(16) + OP_l_12_c_6_r_*SPP(11) - (OP_l_11_c_6_r_*q0)/2);
nextP(3,9) = OP_l_3_c_9_r_ + OP_l_1_c_9_r_*SF(7) + OP_l_2_c_9_r_*SF(11) + OP_l_4_c_9_r_*SF(9) + OP_l_13_c_9_r_*SF(15) - OP_l_11_c_9_r_*SPP(11) - (OP_l_12_c_9_r_*q0)/2 + dt*(OP_l_3_c_6_r_ + OP_l_1_c_6_r_*SF(7) + OP_l_2_c_6_r_*SF(11) + OP_l_4_c_6_r_*SF(9) + OP_l_13_c_6_r_*SF(15) - OP_l_11_c_6_r_*SPP(11) - (OP_l_12_c_6_r_*q0)/2);
nextP(4,9) = OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SF(8) + OP_l_2_c_9_r_*SF(7) + OP_l_3_c_9_r_*SF(10) + OP_l_11_c_9_r_*SF(16) - OP_l_12_c_9_r_*SF(15) - (OP_l_13_c_9_r_*q0)/2 + dt*(OP_l_4_c_6_r_ + OP_l_1_c_6_r_*SF(8) + OP_l_2_c_6_r_*SF(7) + OP_l_3_c_6_r_*SF(10) + OP_l_11_c_6_r_*SF(16) - OP_l_12_c_6_r_*SF(15) - (OP_l_13_c_6_r_*q0)/2);
nextP(5,9) = OP_l_5_c_9_r_ + OP_l_1_c_9_r_*SF(6) + OP_l_2_c_9_r_*SF(4) - OP_l_4_c_9_r_*SF(5) + OP_l_3_c_9_r_*SPP(1) + OP_l_14_c_9_r_*SPP(4) + OP_l_15_c_9_r_*SPP(7) - OP_l_16_c_9_r_*SPP(10) + dt*(OP_l_5_c_6_r_ + OP_l_1_c_6_r_*SF(6) + OP_l_2_c_6_r_*SF(4) - OP_l_4_c_6_r_*SF(5) + OP_l_3_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(4) + OP_l_15_c_6_r_*SPP(7) - OP_l_16_c_6_r_*SPP(10));
nextP(6,9) = OP_l_6_c_9_r_ + OP_l_1_c_9_r_*SF(5) + OP_l_3_c_9_r_*SF(4) + OP_l_4_c_9_r_*SF(6) - OP_l_2_c_9_r_*SPP(1) - OP_l_14_c_9_r_*SPP(9) + OP_l_15_c_9_r_*SPP(3) + OP_l_16_c_9_r_*SPP(6) + dt*(OP_l_6_c_6_r_ + OP_l_1_c_6_r_*SF(5) + OP_l_3_c_6_r_*SF(4) + OP_l_4_c_6_r_*SF(6) - OP_l_2_c_6_r_*SPP(1) - OP_l_14_c_6_r_*SPP(9) + OP_l_15_c_6_r_*SPP(3) + OP_l_16_c_6_r_*SPP(6));
nextP(7,9) = OP_l_7_c_9_r_ + OP_l_2_c_9_r_*SF(5) - OP_l_3_c_9_r_*SF(6) + OP_l_4_c_9_r_*SF(4) + OP_l_1_c_9_r_*SPP(1) + OP_l_14_c_9_r_*SPP(5) - OP_l_15_c_9_r_*SPP(8) - OP_l_16_c_9_r_*SPP(2) + dt*(OP_l_7_c_6_r_ + OP_l_2_c_6_r_*SF(5) - OP_l_3_c_6_r_*SF(6) + OP_l_4_c_6_r_*SF(4) + OP_l_1_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(5) - OP_l_15_c_6_r_*SPP(8) - OP_l_16_c_6_r_*SPP(2));
nextP(8,9) = OP_l_8_c_9_r_ + OP_l_5_c_9_r_*dt + dt*(OP_l_8_c_6_r_ + OP_l_5_c_6_r_*dt);
nextP(9,9) = OP_l_9_c_9_r_ + OP_l_6_c_9_r_*dt + dt*(OP_l_9_c_6_r_ + OP_l_6_c_6_r_*dt);
nextP(1,10) = OP_l_1_c_10_r_ + OP_l_2_c_10_r_*SF(10) + OP_l_3_c_10_r_*SF(12) + OP_l_4_c_10_r_*SF(11) + OP_l_11_c_10_r_*SF(15) + OP_l_12_c_10_r_*SF(16) + OP_l_13_c_10_r_*SPP(11) + dt*(OP_l_1_c_7_r_ + OP_l_2_c_7_r_*SF(10) + OP_l_3_c_7_r_*SF(12) + OP_l_4_c_7_r_*SF(11) + OP_l_11_c_7_r_*SF(15) + OP_l_12_c_7_r_*SF(16) + OP_l_13_c_7_r_*SPP(11));
nextP(2,10) = OP_l_2_c_10_r_ + OP_l_1_c_10_r_*SF(9) + OP_l_3_c_10_r_*SF(8) + OP_l_4_c_10_r_*SF(12) - OP_l_13_c_10_r_*SF(16) + OP_l_12_c_10_r_*SPP(11) - (OP_l_11_c_10_r_*q0)/2 + dt*(OP_l_2_c_7_r_ + OP_l_1_c_7_r_*SF(9) + OP_l_3_c_7_r_*SF(8) + OP_l_4_c_7_r_*SF(12) - OP_l_13_c_7_r_*SF(16) + OP_l_12_c_7_r_*SPP(11) - (OP_l_11_c_7_r_*q0)/2);
nextP(3,10) = OP_l_3_c_10_r_ + OP_l_1_c_10_r_*SF(7) + OP_l_2_c_10_r_*SF(11) + OP_l_4_c_10_r_*SF(9) + OP_l_13_c_10_r_*SF(15) - OP_l_11_c_10_r_*SPP(11) - (OP_l_12_c_10_r_*q0)/2 + dt*(OP_l_3_c_7_r_ + OP_l_1_c_7_r_*SF(7) + OP_l_2_c_7_r_*SF(11) + OP_l_4_c_7_r_*SF(9) + OP_l_13_c_7_r_*SF(15) - OP_l_11_c_7_r_*SPP(11) - (OP_l_12_c_7_r_*q0)/2);
nextP(4,10) = OP_l_4_c_10_r_ + OP_l_1_c_10_r_*SF(8) + OP_l_2_c_10_r_*SF(7) + OP_l_3_c_10_r_*SF(10) + OP_l_11_c_10_r_*SF(16) - OP_l_12_c_10_r_*SF(15) - (OP_l_13_c_10_r_*q0)/2 + dt*(OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SF(8) + OP_l_2_c_7_r_*SF(7) + OP_l_3_c_7_r_*SF(10) + OP_l_11_c_7_r_*SF(16) - OP_l_12_c_7_r_*SF(15) - (OP_l_13_c_7_r_*q0)/2);
nextP(5,10) = OP_l_5_c_10_r_ + OP_l_1_c_10_r_*SF(6) + OP_l_2_c_10_r_*SF(4) - OP_l_4_c_10_r_*SF(5) + OP_l_3_c_10_r_*SPP(1) + OP_l_14_c_10_r_*SPP(4) + OP_l_15_c_10_r_*SPP(7) - OP_l_16_c_10_r_*SPP(10) + dt*(OP_l_5_c_7_r_ + OP_l_1_c_7_r_*SF(6) + OP_l_2_c_7_r_*SF(4) - OP_l_4_c_7_r_*SF(5) + OP_l_3_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(4) + OP_l_15_c_7_r_*SPP(7) - OP_l_16_c_7_r_*SPP(10));
nextP(6,10) = OP_l_6_c_10_r_ + OP_l_1_c_10_r_*SF(5) + OP_l_3_c_10_r_*SF(4) + OP_l_4_c_10_r_*SF(6) - OP_l_2_c_10_r_*SPP(1) - OP_l_14_c_10_r_*SPP(9) + OP_l_15_c_10_r_*SPP(3) + OP_l_16_c_10_r_*SPP(6) + dt*(OP_l_6_c_7_r_ + OP_l_1_c_7_r_*SF(5) + OP_l_3_c_7_r_*SF(4) + OP_l_4_c_7_r_*SF(6) - OP_l_2_c_7_r_*SPP(1) - OP_l_14_c_7_r_*SPP(9) + OP_l_15_c_7_r_*SPP(3) + OP_l_16_c_7_r_*SPP(6));
nextP(7,10) = OP_l_7_c_10_r_ + OP_l_2_c_10_r_*SF(5) - OP_l_3_c_10_r_*SF(6) + OP_l_4_c_10_r_*SF(4) + OP_l_1_c_10_r_*SPP(1) + OP_l_14_c_10_r_*SPP(5) - OP_l_15_c_10_r_*SPP(8) - OP_l_16_c_10_r_*SPP(2) + dt*(OP_l_7_c_7_r_ + OP_l_2_c_7_r_*SF(5) - OP_l_3_c_7_r_*SF(6) + OP_l_4_c_7_r_*SF(4) + OP_l_1_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(5) - OP_l_15_c_7_r_*SPP(8) - OP_l_16_c_7_r_*SPP(2));
nextP(8,10) = OP_l_8_c_10_r_ + OP_l_5_c_10_r_*dt + dt*(OP_l_8_c_7_r_ + OP_l_5_c_7_r_*dt);
nextP(9,10) = OP_l_9_c_10_r_ + OP_l_6_c_10_r_*dt + dt*(OP_l_9_c_7_r_ + OP_l_6_c_7_r_*dt);
nextP(10,10) = OP_l_10_c_10_r_ + OP_l_7_c_10_r_*dt + dt*(OP_l_10_c_7_r_ + OP_l_7_c_7_r_*dt);
nextP(1,11) = OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11);
nextP(2,11) = OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2;
nextP(3,11) = OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2;
nextP(4,11) = OP_l_4_c_11_r_ + OP_l_1_c_11_r_*SF(8) + OP_l_2_c_11_r_*SF(7) + OP_l_3_c_11_r_*SF(10) + OP_l_11_c_11_r_*SF(16) - OP_l_12_c_11_r_*SF(15) - (OP_l_13_c_11_r_*q0)/2;
nextP(5,11) = OP_l_5_c_11_r_ + OP_l_1_c_11_r_*SF(6) + OP_l_2_c_11_r_*SF(4) - OP_l_4_c_11_r_*SF(5) + OP_l_3_c_11_r_*SPP(1) + OP_l_14_c_11_r_*SPP(4) + OP_l_15_c_11_r_*SPP(7) - OP_l_16_c_11_r_*SPP(10);
nextP(6,11) = OP_l_6_c_11_r_ + OP_l_1_c_11_r_*SF(5) + OP_l_3_c_11_r_*SF(4) + OP_l_4_c_11_r_*SF(6) - OP_l_2_c_11_r_*SPP(1) - OP_l_14_c_11_r_*SPP(9) + OP_l_15_c_11_r_*SPP(3) + OP_l_16_c_11_r_*SPP(6);
nextP(7,11) = OP_l_7_c_11_r_ + OP_l_2_c_11_r_*SF(5) - OP_l_3_c_11_r_*SF(6) + OP_l_4_c_11_r_*SF(4) + OP_l_1_c_11_r_*SPP(1) + OP_l_14_c_11_r_*SPP(5) - OP_l_15_c_11_r_*SPP(8) - OP_l_16_c_11_r_*SPP(2);
nextP(8,11) = OP_l_8_c_11_r_ + OP_l_5_c_11_r_*dt;
nextP(9,11) = OP_l_9_c_11_r_ + OP_l_6_c_11_r_*dt;
nextP(10,11) = OP_l_10_c_11_r_ + OP_l_7_c_11_r_*dt;
nextP(11,11) = OP_l_11_c_11_r_;
nextP(1,12) = OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11);
nextP(2,12) = OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2;
nextP(3,12) = OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2;
nextP(4,12) = OP_l_4_c_12_r_ + OP_l_1_c_12_r_*SF(8) + OP_l_2_c_12_r_*SF(7) + OP_l_3_c_12_r_*SF(10) + OP_l_11_c_12_r_*SF(16) - OP_l_12_c_12_r_*SF(15) - (OP_l_13_c_12_r_*q0)/2;
nextP(5,12) = OP_l_5_c_12_r_ + OP_l_1_c_12_r_*SF(6) + OP_l_2_c_12_r_*SF(4) - OP_l_4_c_12_r_*SF(5) + OP_l_3_c_12_r_*SPP(1) + OP_l_14_c_12_r_*SPP(4) + OP_l_15_c_12_r_*SPP(7) - OP_l_16_c_12_r_*SPP(10);
nextP(6,12) = OP_l_6_c_12_r_ + OP_l_1_c_12_r_*SF(5) + OP_l_3_c_12_r_*SF(4) + OP_l_4_c_12_r_*SF(6) - OP_l_2_c_12_r_*SPP(1) - OP_l_14_c_12_r_*SPP(9) + OP_l_15_c_12_r_*SPP(3) + OP_l_16_c_12_r_*SPP(6);
nextP(7,12) = OP_l_7_c_12_r_ + OP_l_2_c_12_r_*SF(5) - OP_l_3_c_12_r_*SF(6) + OP_l_4_c_12_r_*SF(4) + OP_l_1_c_12_r_*SPP(1) + OP_l_14_c_12_r_*SPP(5) - OP_l_15_c_12_r_*SPP(8) - OP_l_16_c_12_r_*SPP(2);
nextP(8,12) = OP_l_8_c_12_r_ + OP_l_5_c_12_r_*dt;
nextP(9,12) = OP_l_9_c_12_r_ + OP_l_6_c_12_r_*dt;
nextP(10,12) = OP_l_10_c_12_r_ + OP_l_7_c_12_r_*dt;
nextP(11,12) = OP_l_11_c_12_r_;
nextP(12,12) = OP_l_12_c_12_r_;
nextP(1,13) = OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11);
nextP(2,13) = OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2;
nextP(3,13) = OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2;
nextP(4,13) = OP_l_4_c_13_r_ + OP_l_1_c_13_r_*SF(8) + OP_l_2_c_13_r_*SF(7) + OP_l_3_c_13_r_*SF(10) + OP_l_11_c_13_r_*SF(16) - OP_l_12_c_13_r_*SF(15) - (OP_l_13_c_13_r_*q0)/2;
nextP(5,13) = OP_l_5_c_13_r_ + OP_l_1_c_13_r_*SF(6) + OP_l_2_c_13_r_*SF(4) - OP_l_4_c_13_r_*SF(5) + OP_l_3_c_13_r_*SPP(1) + OP_l_14_c_13_r_*SPP(4) + OP_l_15_c_13_r_*SPP(7) - OP_l_16_c_13_r_*SPP(10);
nextP(6,13) = OP_l_6_c_13_r_ + OP_l_1_c_13_r_*SF(5) + OP_l_3_c_13_r_*SF(4) + OP_l_4_c_13_r_*SF(6) - OP_l_2_c_13_r_*SPP(1) - OP_l_14_c_13_r_*SPP(9) + OP_l_15_c_13_r_*SPP(3) + OP_l_16_c_13_r_*SPP(6);
nextP(7,13) = OP_l_7_c_13_r_ + OP_l_2_c_13_r_*SF(5) - OP_l_3_c_13_r_*SF(6) + OP_l_4_c_13_r_*SF(4) + OP_l_1_c_13_r_*SPP(1) + OP_l_14_c_13_r_*SPP(5) - OP_l_15_c_13_r_*SPP(8) - OP_l_16_c_13_r_*SPP(2);
nextP(8,13) = OP_l_8_c_13_r_ + OP_l_5_c_13_r_*dt;
nextP(9,13) = OP_l_9_c_13_r_ + OP_l_6_c_13_r_*dt;
nextP(10,13) = OP_l_10_c_13_r_ + OP_l_7_c_13_r_*dt;
nextP(11,13) = OP_l_11_c_13_r_;
nextP(12,13) = OP_l_12_c_13_r_;
nextP(13,13) = OP_l_13_c_13_r_;
nextP(1,14) = OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11);
nextP(2,14) = OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2;
nextP(3,14) = OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2;
nextP(4,14) = OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2;
nextP(5,14) = OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10);
nextP(6,14) = OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6);
nextP(7,14) = OP_l_7_c_14_r_ + OP_l_2_c_14_r_*SF(5) - OP_l_3_c_14_r_*SF(6) + OP_l_4_c_14_r_*SF(4) + OP_l_1_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(5) - OP_l_15_c_14_r_*SPP(8) - OP_l_16_c_14_r_*SPP(2);
nextP(8,14) = OP_l_8_c_14_r_ + OP_l_5_c_14_r_*dt;
nextP(9,14) = OP_l_9_c_14_r_ + OP_l_6_c_14_r_*dt;
nextP(10,14) = OP_l_10_c_14_r_ + OP_l_7_c_14_r_*dt;
nextP(11,14) = OP_l_11_c_14_r_;
nextP(12,14) = OP_l_12_c_14_r_;
nextP(13,14) = OP_l_13_c_14_r_;
nextP(14,14) = OP_l_14_c_14_r_;
nextP(1,15) = OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11);
nextP(2,15) = OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2;
nextP(3,15) = OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2;
nextP(4,15) = OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2;
nextP(5,15) = OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10);
nextP(6,15) = OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6);
nextP(7,15) = OP_l_7_c_15_r_ + OP_l_2_c_15_r_*SF(5) - OP_l_3_c_15_r_*SF(6) + OP_l_4_c_15_r_*SF(4) + OP_l_1_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(5) - OP_l_15_c_15_r_*SPP(8) - OP_l_16_c_15_r_*SPP(2);
nextP(8,15) = OP_l_8_c_15_r_ + OP_l_5_c_15_r_*dt;
nextP(9,15) = OP_l_9_c_15_r_ + OP_l_6_c_15_r_*dt;
nextP(10,15) = OP_l_10_c_15_r_ + OP_l_7_c_15_r_*dt;
nextP(11,15) = OP_l_11_c_15_r_;
nextP(12,15) = OP_l_12_c_15_r_;
nextP(13,15) = OP_l_13_c_15_r_;
nextP(14,15) = OP_l_14_c_15_r_;
nextP(15,15) = OP_l_15_c_15_r_;
nextP(1,16) = OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11);
nextP(2,16) = OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2;
nextP(3,16) = OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2;
nextP(4,16) = OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2;
nextP(5,16) = OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10);
nextP(6,16) = OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6);
nextP(7,16) = OP_l_7_c_16_r_ + OP_l_2_c_16_r_*SF(5) - OP_l_3_c_16_r_*SF(6) + OP_l_4_c_16_r_*SF(4) + OP_l_1_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(5) - OP_l_15_c_16_r_*SPP(8) - OP_l_16_c_16_r_*SPP(2);
nextP(8,16) = OP_l_8_c_16_r_ + OP_l_5_c_16_r_*dt;
nextP(9,16) = OP_l_9_c_16_r_ + OP_l_6_c_16_r_*dt;
nextP(10,16) = OP_l_10_c_16_r_ + OP_l_7_c_16_r_*dt;
nextP(11,16) = OP_l_11_c_16_r_;
nextP(12,16) = OP_l_12_c_16_r_;
nextP(13,16) = OP_l_13_c_16_r_;
nextP(14,16) = OP_l_14_c_16_r_;
nextP(15,16) = OP_l_15_c_16_r_;
nextP(16,16) = OP_l_16_c_16_r_;
nextP(1,17) = OP_l_1_c_17_r_ + OP_l_2_c_17_r_*SF(10) + OP_l_3_c_17_r_*SF(12) + OP_l_4_c_17_r_*SF(11) + OP_l_11_c_17_r_*SF(15) + OP_l_12_c_17_r_*SF(16) + OP_l_13_c_17_r_*SPP(11);
nextP(2,17) = OP_l_2_c_17_r_ + OP_l_1_c_17_r_*SF(9) + OP_l_3_c_17_r_*SF(8) + OP_l_4_c_17_r_*SF(12) - OP_l_13_c_17_r_*SF(16) + OP_l_12_c_17_r_*SPP(11) - (OP_l_11_c_17_r_*q0)/2;
nextP(3,17) = OP_l_3_c_17_r_ + OP_l_1_c_17_r_*SF(7) + OP_l_2_c_17_r_*SF(11) + OP_l_4_c_17_r_*SF(9) + OP_l_13_c_17_r_*SF(15) - OP_l_11_c_17_r_*SPP(11) - (OP_l_12_c_17_r_*q0)/2;
nextP(4,17) = OP_l_4_c_17_r_ + OP_l_1_c_17_r_*SF(8) + OP_l_2_c_17_r_*SF(7) + OP_l_3_c_17_r_*SF(10) + OP_l_11_c_17_r_*SF(16) - OP_l_12_c_17_r_*SF(15) - (OP_l_13_c_17_r_*q0)/2;
nextP(5,17) = OP_l_5_c_17_r_ + OP_l_1_c_17_r_*SF(6) + OP_l_2_c_17_r_*SF(4) - OP_l_4_c_17_r_*SF(5) + OP_l_3_c_17_r_*SPP(1) + OP_l_14_c_17_r_*SPP(4) + OP_l_15_c_17_r_*SPP(7) - OP_l_16_c_17_r_*SPP(10);
nextP(6,17) = OP_l_6_c_17_r_ + OP_l_1_c_17_r_*SF(5) + OP_l_3_c_17_r_*SF(4) + OP_l_4_c_17_r_*SF(6) - OP_l_2_c_17_r_*SPP(1) - OP_l_14_c_17_r_*SPP(9) + OP_l_15_c_17_r_*SPP(3) + OP_l_16_c_17_r_*SPP(6);
nextP(7,17) = OP_l_7_c_17_r_ + OP_l_2_c_17_r_*SF(5) - OP_l_3_c_17_r_*SF(6) + OP_l_4_c_17_r_*SF(4) + OP_l_1_c_17_r_*SPP(1) + OP_l_14_c_17_r_*SPP(5) - OP_l_15_c_17_r_*SPP(8) - OP_l_16_c_17_r_*SPP(2);
nextP(8,17) = OP_l_8_c_17_r_ + OP_l_5_c_17_r_*dt;
nextP(9,17) = OP_l_9_c_17_r_ + OP_l_6_c_17_r_*dt;
nextP(10,17) = OP_l_10_c_17_r_ + OP_l_7_c_17_r_*dt;
nextP(11,17) = OP_l_11_c_17_r_;
nextP(12,17) = OP_l_12_c_17_r_;
nextP(13,17) = OP_l_13_c_17_r_;
nextP(14,17) = OP_l_14_c_17_r_;
nextP(15,17) = OP_l_15_c_17_r_;
nextP(16,17) = OP_l_16_c_17_r_;
nextP(17,17) = OP_l_17_c_17_r_;
nextP(1,18) = OP_l_1_c_18_r_ + OP_l_2_c_18_r_*SF(10) + OP_l_3_c_18_r_*SF(12) + OP_l_4_c_18_r_*SF(11) + OP_l_11_c_18_r_*SF(15) + OP_l_12_c_18_r_*SF(16) + OP_l_13_c_18_r_*SPP(11);
nextP(2,18) = OP_l_2_c_18_r_ + OP_l_1_c_18_r_*SF(9) + OP_l_3_c_18_r_*SF(8) + OP_l_4_c_18_r_*SF(12) - OP_l_13_c_18_r_*SF(16) + OP_l_12_c_18_r_*SPP(11) - (OP_l_11_c_18_r_*q0)/2;
nextP(3,18) = OP_l_3_c_18_r_ + OP_l_1_c_18_r_*SF(7) + OP_l_2_c_18_r_*SF(11) + OP_l_4_c_18_r_*SF(9) + OP_l_13_c_18_r_*SF(15) - OP_l_11_c_18_r_*SPP(11) - (OP_l_12_c_18_r_*q0)/2;
nextP(4,18) = OP_l_4_c_18_r_ + OP_l_1_c_18_r_*SF(8) + OP_l_2_c_18_r_*SF(7) + OP_l_3_c_18_r_*SF(10) + OP_l_11_c_18_r_*SF(16) - OP_l_12_c_18_r_*SF(15) - (OP_l_13_c_18_r_*q0)/2;
nextP(5,18) = OP_l_5_c_18_r_ + OP_l_1_c_18_r_*SF(6) + OP_l_2_c_18_r_*SF(4) - OP_l_4_c_18_r_*SF(5) + OP_l_3_c_18_r_*SPP(1) + OP_l_14_c_18_r_*SPP(4) + OP_l_15_c_18_r_*SPP(7) - OP_l_16_c_18_r_*SPP(10);
nextP(6,18) = OP_l_6_c_18_r_ + OP_l_1_c_18_r_*SF(5) + OP_l_3_c_18_r_*SF(4) + OP_l_4_c_18_r_*SF(6) - OP_l_2_c_18_r_*SPP(1) - OP_l_14_c_18_r_*SPP(9) + OP_l_15_c_18_r_*SPP(3) + OP_l_16_c_18_r_*SPP(6);
nextP(7,18) = OP_l_7_c_18_r_ + OP_l_2_c_18_r_*SF(5) - OP_l_3_c_18_r_*SF(6) + OP_l_4_c_18_r_*SF(4) + OP_l_1_c_18_r_*SPP(1) + OP_l_14_c_18_r_*SPP(5) - OP_l_15_c_18_r_*SPP(8) - OP_l_16_c_18_r_*SPP(2);
nextP(8,18) = OP_l_8_c_18_r_ + OP_l_5_c_18_r_*dt;
nextP(9,18) = OP_l_9_c_18_r_ + OP_l_6_c_18_r_*dt;
nextP(10,18) = OP_l_10_c_18_r_ + OP_l_7_c_18_r_*dt;
nextP(11,18) = OP_l_11_c_18_r_;
nextP(12,18) = OP_l_12_c_18_r_;
nextP(13,18) = OP_l_13_c_18_r_;
nextP(14,18) = OP_l_14_c_18_r_;
nextP(15,18) = OP_l_15_c_18_r_;
nextP(16,18) = OP_l_16_c_18_r_;
nextP(17,18) = OP_l_17_c_18_r_;
nextP(18,18) = OP_l_18_c_18_r_;
nextP(1,19) = OP_l_1_c_19_r_ + OP_l_2_c_19_r_*SF(10) + OP_l_3_c_19_r_*SF(12) + OP_l_4_c_19_r_*SF(11) + OP_l_11_c_19_r_*SF(15) + OP_l_12_c_19_r_*SF(16) + OP_l_13_c_19_r_*SPP(11);
nextP(2,19) = OP_l_2_c_19_r_ + OP_l_1_c_19_r_*SF(9) + OP_l_3_c_19_r_*SF(8) + OP_l_4_c_19_r_*SF(12) - OP_l_13_c_19_r_*SF(16) + OP_l_12_c_19_r_*SPP(11) - (OP_l_11_c_19_r_*q0)/2;
nextP(3,19) = OP_l_3_c_19_r_ + OP_l_1_c_19_r_*SF(7) + OP_l_2_c_19_r_*SF(11) + OP_l_4_c_19_r_*SF(9) + OP_l_13_c_19_r_*SF(15) - OP_l_11_c_19_r_*SPP(11) - (OP_l_12_c_19_r_*q0)/2;
nextP(4,19) = OP_l_4_c_19_r_ + OP_l_1_c_19_r_*SF(8) + OP_l_2_c_19_r_*SF(7) + OP_l_3_c_19_r_*SF(10) + OP_l_11_c_19_r_*SF(16) - OP_l_12_c_19_r_*SF(15) - (OP_l_13_c_19_r_*q0)/2;
nextP(5,19) = OP_l_5_c_19_r_ + OP_l_1_c_19_r_*SF(6) + OP_l_2_c_19_r_*SF(4) - OP_l_4_c_19_r_*SF(5) + OP_l_3_c_19_r_*SPP(1) + OP_l_14_c_19_r_*SPP(4) + OP_l_15_c_19_r_*SPP(7) - OP_l_16_c_19_r_*SPP(10);
nextP(6,19) = OP_l_6_c_19_r_ + OP_l_1_c_19_r_*SF(5) + OP_l_3_c_19_r_*SF(4) + OP_l_4_c_19_r_*SF(6) - OP_l_2_c_19_r_*SPP(1) - OP_l_14_c_19_r_*SPP(9) + OP_l_15_c_19_r_*SPP(3) + OP_l_16_c_19_r_*SPP(6);
nextP(7,19) = OP_l_7_c_19_r_ + OP_l_2_c_19_r_*SF(5) - OP_l_3_c_19_r_*SF(6) + OP_l_4_c_19_r_*SF(4) + OP_l_1_c_19_r_*SPP(1) + OP_l_14_c_19_r_*SPP(5) - OP_l_15_c_19_r_*SPP(8) - OP_l_16_c_19_r_*SPP(2);
nextP(8,19) = OP_l_8_c_19_r_ + OP_l_5_c_19_r_*dt;
nextP(9,19) = OP_l_9_c_19_r_ + OP_l_6_c_19_r_*dt;
nextP(10,19) = OP_l_10_c_19_r_ + OP_l_7_c_19_r_*dt;
nextP(11,19) = OP_l_11_c_19_r_;
nextP(12,19) = OP_l_12_c_19_r_;
nextP(13,19) = OP_l_13_c_19_r_;
nextP(14,19) = OP_l_14_c_19_r_;
nextP(15,19) = OP_l_15_c_19_r_;
nextP(16,19) = OP_l_16_c_19_r_;
nextP(17,19) = OP_l_17_c_19_r_;
nextP(18,19) = OP_l_18_c_19_r_;
nextP(19,19) = OP_l_19_c_19_r_;
nextP(1,20) = OP_l_1_c_20_r_ + OP_l_2_c_20_r_*SF(10) + OP_l_3_c_20_r_*SF(12) + OP_l_4_c_20_r_*SF(11) + OP_l_11_c_20_r_*SF(15) + OP_l_12_c_20_r_*SF(16) + OP_l_13_c_20_r_*SPP(11);
nextP(2,20) = OP_l_2_c_20_r_ + OP_l_1_c_20_r_*SF(9) + OP_l_3_c_20_r_*SF(8) + OP_l_4_c_20_r_*SF(12) - OP_l_13_c_20_r_*SF(16) + OP_l_12_c_20_r_*SPP(11) - (OP_l_11_c_20_r_*q0)/2;
nextP(3,20) = OP_l_3_c_20_r_ + OP_l_1_c_20_r_*SF(7) + OP_l_2_c_20_r_*SF(11) + OP_l_4_c_20_r_*SF(9) + OP_l_13_c_20_r_*SF(15) - OP_l_11_c_20_r_*SPP(11) - (OP_l_12_c_20_r_*q0)/2;
nextP(4,20) = OP_l_4_c_20_r_ + OP_l_1_c_20_r_*SF(8) + OP_l_2_c_20_r_*SF(7) + OP_l_3_c_20_r_*SF(10) + OP_l_11_c_20_r_*SF(16) - OP_l_12_c_20_r_*SF(15) - (OP_l_13_c_20_r_*q0)/2;
nextP(5,20) = OP_l_5_c_20_r_ + OP_l_1_c_20_r_*SF(6) + OP_l_2_c_20_r_*SF(4) - OP_l_4_c_20_r_*SF(5) + OP_l_3_c_20_r_*SPP(1) + OP_l_14_c_20_r_*SPP(4) + OP_l_15_c_20_r_*SPP(7) - OP_l_16_c_20_r_*SPP(10);
nextP(6,20) = OP_l_6_c_20_r_ + OP_l_1_c_20_r_*SF(5) + OP_l_3_c_20_r_*SF(4) + OP_l_4_c_20_r_*SF(6) - OP_l_2_c_20_r_*SPP(1) - OP_l_14_c_20_r_*SPP(9) + OP_l_15_c_20_r_*SPP(3) + OP_l_16_c_20_r_*SPP(6);
nextP(7,20) = OP_l_7_c_20_r_ + OP_l_2_c_20_r_*SF(5) - OP_l_3_c_20_r_*SF(6) + OP_l_4_c_20_r_*SF(4) + OP_l_1_c_20_r_*SPP(1) + OP_l_14_c_20_r_*SPP(5) - OP_l_15_c_20_r_*SPP(8) - OP_l_16_c_20_r_*SPP(2);
nextP(8,20) = OP_l_8_c_20_r_ + OP_l_5_c_20_r_*dt;
nextP(9,20) = OP_l_9_c_20_r_ + OP_l_6_c_20_r_*dt;
nextP(10,20) = OP_l_10_c_20_r_ + OP_l_7_c_20_r_*dt;
nextP(11,20) = OP_l_11_c_20_r_;
nextP(12,20) = OP_l_12_c_20_r_;
nextP(13,20) = OP_l_13_c_20_r_;
nextP(14,20) = OP_l_14_c_20_r_;
nextP(15,20) = OP_l_15_c_20_r_;
nextP(16,20) = OP_l_16_c_20_r_;
nextP(17,20) = OP_l_17_c_20_r_;
nextP(18,20) = OP_l_18_c_20_r_;
nextP(19,20) = OP_l_19_c_20_r_;
nextP(20,20) = OP_l_20_c_20_r_;
nextP(1,21) = OP_l_1_c_21_r_ + OP_l_2_c_21_r_*SF(10) + OP_l_3_c_21_r_*SF(12) + OP_l_4_c_21_r_*SF(11) + OP_l_11_c_21_r_*SF(15) + OP_l_12_c_21_r_*SF(16) + OP_l_13_c_21_r_*SPP(11);
nextP(2,21) = OP_l_2_c_21_r_ + OP_l_1_c_21_r_*SF(9) + OP_l_3_c_21_r_*SF(8) + OP_l_4_c_21_r_*SF(12) - OP_l_13_c_21_r_*SF(16) + OP_l_12_c_21_r_*SPP(11) - (OP_l_11_c_21_r_*q0)/2;
nextP(3,21) = OP_l_3_c_21_r_ + OP_l_1_c_21_r_*SF(7) + OP_l_2_c_21_r_*SF(11) + OP_l_4_c_21_r_*SF(9) + OP_l_13_c_21_r_*SF(15) - OP_l_11_c_21_r_*SPP(11) - (OP_l_12_c_21_r_*q0)/2;
nextP(4,21) = OP_l_4_c_21_r_ + OP_l_1_c_21_r_*SF(8) + OP_l_2_c_21_r_*SF(7) + OP_l_3_c_21_r_*SF(10) + OP_l_11_c_21_r_*SF(16) - OP_l_12_c_21_r_*SF(15) - (OP_l_13_c_21_r_*q0)/2;
nextP(5,21) = OP_l_5_c_21_r_ + OP_l_1_c_21_r_*SF(6) + OP_l_2_c_21_r_*SF(4) - OP_l_4_c_21_r_*SF(5) + OP_l_3_c_21_r_*SPP(1) + OP_l_14_c_21_r_*SPP(4) + OP_l_15_c_21_r_*SPP(7) - OP_l_16_c_21_r_*SPP(10);
nextP(6,21) = OP_l_6_c_21_r_ + OP_l_1_c_21_r_*SF(5) + OP_l_3_c_21_r_*SF(4) + OP_l_4_c_21_r_*SF(6) - OP_l_2_c_21_r_*SPP(1) - OP_l_14_c_21_r_*SPP(9) + OP_l_15_c_21_r_*SPP(3) + OP_l_16_c_21_r_*SPP(6);
nextP(7,21) = OP_l_7_c_21_r_ + OP_l_2_c_21_r_*SF(5) - OP_l_3_c_21_r_*SF(6) + OP_l_4_c_21_r_*SF(4) + OP_l_1_c_21_r_*SPP(1) + OP_l_14_c_21_r_*SPP(5) - OP_l_15_c_21_r_*SPP(8) - OP_l_16_c_21_r_*SPP(2);
nextP(8,21) = OP_l_8_c_21_r_ + OP_l_5_c_21_r_*dt;
nextP(9,21) = OP_l_9_c_21_r_ + OP_l_6_c_21_r_*dt;
nextP(10,21) = OP_l_10_c_21_r_ + OP_l_7_c_21_r_*dt;
nextP(11,21) = OP_l_11_c_21_r_;
nextP(12,21) = OP_l_12_c_21_r_;
nextP(13,21) = OP_l_13_c_21_r_;
nextP(14,21) = OP_l_14_c_21_r_;
nextP(15,21) = OP_l_15_c_21_r_;
nextP(16,21) = OP_l_16_c_21_r_;
nextP(17,21) = OP_l_17_c_21_r_;
nextP(18,21) = OP_l_18_c_21_r_;
nextP(19,21) = OP_l_19_c_21_r_;
nextP(20,21) = OP_l_20_c_21_r_;
nextP(21,21) = OP_l_21_c_21_r_;
nextP(1,22) = OP_l_1_c_22_r_ + OP_l_2_c_22_r_*SF(10) + OP_l_3_c_22_r_*SF(12) + OP_l_4_c_22_r_*SF(11) + OP_l_11_c_22_r_*SF(15) + OP_l_12_c_22_r_*SF(16) + OP_l_13_c_22_r_*SPP(11);
nextP(2,22) = OP_l_2_c_22_r_ + OP_l_1_c_22_r_*SF(9) + OP_l_3_c_22_r_*SF(8) + OP_l_4_c_22_r_*SF(12) - OP_l_13_c_22_r_*SF(16) + OP_l_12_c_22_r_*SPP(11) - (OP_l_11_c_22_r_*q0)/2;
nextP(3,22) = OP_l_3_c_22_r_ + OP_l_1_c_22_r_*SF(7) + OP_l_2_c_22_r_*SF(11) + OP_l_4_c_22_r_*SF(9) + OP_l_13_c_22_r_*SF(15) - OP_l_11_c_22_r_*SPP(11) - (OP_l_12_c_22_r_*q0)/2;
nextP(4,22) = OP_l_4_c_22_r_ + OP_l_1_c_22_r_*SF(8) + OP_l_2_c_22_r_*SF(7) + OP_l_3_c_22_r_*SF(10) + OP_l_11_c_22_r_*SF(16) - OP_l_12_c_22_r_*SF(15) - (OP_l_13_c_22_r_*q0)/2;
nextP(5,22) = OP_l_5_c_22_r_ + OP_l_1_c_22_r_*SF(6) + OP_l_2_c_22_r_*SF(4) - OP_l_4_c_22_r_*SF(5) + OP_l_3_c_22_r_*SPP(1) + OP_l_14_c_22_r_*SPP(4) + OP_l_15_c_22_r_*SPP(7) - OP_l_16_c_22_r_*SPP(10);
nextP(6,22) = OP_l_6_c_22_r_ + OP_l_1_c_22_r_*SF(5) + OP_l_3_c_22_r_*SF(4) + OP_l_4_c_22_r_*SF(6) - OP_l_2_c_22_r_*SPP(1) - OP_l_14_c_22_r_*SPP(9) + OP_l_15_c_22_r_*SPP(3) + OP_l_16_c_22_r_*SPP(6);
nextP(7,22) = OP_l_7_c_22_r_ + OP_l_2_c_22_r_*SF(5) - OP_l_3_c_22_r_*SF(6) + OP_l_4_c_22_r_*SF(4) + OP_l_1_c_22_r_*SPP(1) + OP_l_14_c_22_r_*SPP(5) - OP_l_15_c_22_r_*SPP(8) - OP_l_16_c_22_r_*SPP(2);
nextP(8,22) = OP_l_8_c_22_r_ + OP_l_5_c_22_r_*dt;
nextP(9,22) = OP_l_9_c_22_r_ + OP_l_6_c_22_r_*dt;
nextP(10,22) = OP_l_10_c_22_r_ + OP_l_7_c_22_r_*dt;
nextP(11,22) = OP_l_11_c_22_r_;
nextP(12,22) = OP_l_12_c_22_r_;
nextP(13,22) = OP_l_13_c_22_r_;
nextP(14,22) = OP_l_14_c_22_r_;
nextP(15,22) = OP_l_15_c_22_r_;
nextP(16,22) = OP_l_16_c_22_r_;
nextP(17,22) = OP_l_17_c_22_r_;
nextP(18,22) = OP_l_18_c_22_r_;
nextP(19,22) = OP_l_19_c_22_r_;
nextP(20,22) = OP_l_20_c_22_r_;
nextP(21,22) = OP_l_21_c_22_r_;
nextP(22,22) = OP_l_22_c_22_r_;
nextP(1,23) = OP_l_1_c_23_r_ + OP_l_2_c_23_r_*SF(10) + OP_l_3_c_23_r_*SF(12) + OP_l_4_c_23_r_*SF(11) + OP_l_11_c_23_r_*SF(15) + OP_l_12_c_23_r_*SF(16) + OP_l_13_c_23_r_*SPP(11);
nextP(2,23) = OP_l_2_c_23_r_ + OP_l_1_c_23_r_*SF(9) + OP_l_3_c_23_r_*SF(8) + OP_l_4_c_23_r_*SF(12) - OP_l_13_c_23_r_*SF(16) + OP_l_12_c_23_r_*SPP(11) - (OP_l_11_c_23_r_*q0)/2;
nextP(3,23) = OP_l_3_c_23_r_ + OP_l_1_c_23_r_*SF(7) + OP_l_2_c_23_r_*SF(11) + OP_l_4_c_23_r_*SF(9) + OP_l_13_c_23_r_*SF(15) - OP_l_11_c_23_r_*SPP(11) - (OP_l_12_c_23_r_*q0)/2;
nextP(4,23) = OP_l_4_c_23_r_ + OP_l_1_c_23_r_*SF(8) + OP_l_2_c_23_r_*SF(7) + OP_l_3_c_23_r_*SF(10) + OP_l_11_c_23_r_*SF(16) - OP_l_12_c_23_r_*SF(15) - (OP_l_13_c_23_r_*q0)/2;
nextP(5,23) = OP_l_5_c_23_r_ + OP_l_1_c_23_r_*SF(6) + OP_l_2_c_23_r_*SF(4) - OP_l_4_c_23_r_*SF(5) + OP_l_3_c_23_r_*SPP(1) + OP_l_14_c_23_r_*SPP(4) + OP_l_15_c_23_r_*SPP(7) - OP_l_16_c_23_r_*SPP(10);
nextP(6,23) = OP_l_6_c_23_r_ + OP_l_1_c_23_r_*SF(5) + OP_l_3_c_23_r_*SF(4) + OP_l_4_c_23_r_*SF(6) - OP_l_2_c_23_r_*SPP(1) - OP_l_14_c_23_r_*SPP(9) + OP_l_15_c_23_r_*SPP(3) + OP_l_16_c_23_r_*SPP(6);
nextP(7,23) = OP_l_7_c_23_r_ + OP_l_2_c_23_r_*SF(5) - OP_l_3_c_23_r_*SF(6) + OP_l_4_c_23_r_*SF(4) + OP_l_1_c_23_r_*SPP(1) + OP_l_14_c_23_r_*SPP(5) - OP_l_15_c_23_r_*SPP(8) - OP_l_16_c_23_r_*SPP(2);
nextP(8,23) = OP_l_8_c_23_r_ + OP_l_5_c_23_r_*dt;
nextP(9,23) = OP_l_9_c_23_r_ + OP_l_6_c_23_r_*dt;
nextP(10,23) = OP_l_10_c_23_r_ + OP_l_7_c_23_r_*dt;
nextP(11,23) = OP_l_11_c_23_r_;
nextP(12,23) = OP_l_12_c_23_r_;
nextP(13,23) = OP_l_13_c_23_r_;
nextP(14,23) = OP_l_14_c_23_r_;
nextP(15,23) = OP_l_15_c_23_r_;
nextP(16,23) = OP_l_16_c_23_r_;
nextP(17,23) = OP_l_17_c_23_r_;
nextP(18,23) = OP_l_18_c_23_r_;
nextP(19,23) = OP_l_19_c_23_r_;
nextP(20,23) = OP_l_20_c_23_r_;
nextP(21,23) = OP_l_21_c_23_r_;
nextP(22,23) = OP_l_22_c_23_r_;
nextP(23,23) = OP_l_23_c_23_r_;
nextP(1,24) = OP_l_1_c_24_r_ + OP_l_2_c_24_r_*SF(10) + OP_l_3_c_24_r_*SF(12) + OP_l_4_c_24_r_*SF(11) + OP_l_11_c_24_r_*SF(15) + OP_l_12_c_24_r_*SF(16) + OP_l_13_c_24_r_*SPP(11);
nextP(2,24) = OP_l_2_c_24_r_ + OP_l_1_c_24_r_*SF(9) + OP_l_3_c_24_r_*SF(8) + OP_l_4_c_24_r_*SF(12) - OP_l_13_c_24_r_*SF(16) + OP_l_12_c_24_r_*SPP(11) - (OP_l_11_c_24_r_*q0)/2;
nextP(3,24) = OP_l_3_c_24_r_ + OP_l_1_c_24_r_*SF(7) + OP_l_2_c_24_r_*SF(11) + OP_l_4_c_24_r_*SF(9) + OP_l_13_c_24_r_*SF(15) - OP_l_11_c_24_r_*SPP(11) - (OP_l_12_c_24_r_*q0)/2;
nextP(4,24) = OP_l_4_c_24_r_ + OP_l_1_c_24_r_*SF(8) + OP_l_2_c_24_r_*SF(7) + OP_l_3_c_24_r_*SF(10) + OP_l_11_c_24_r_*SF(16) - OP_l_12_c_24_r_*SF(15) - (OP_l_13_c_24_r_*q0)/2;
nextP(5,24) = OP_l_5_c_24_r_ + OP_l_1_c_24_r_*SF(6) + OP_l_2_c_24_r_*SF(4) - OP_l_4_c_24_r_*SF(5) + OP_l_3_c_24_r_*SPP(1) + OP_l_14_c_24_r_*SPP(4) + OP_l_15_c_24_r_*SPP(7) - OP_l_16_c_24_r_*SPP(10);
nextP(6,24) = OP_l_6_c_24_r_ + OP_l_1_c_24_r_*SF(5) + OP_l_3_c_24_r_*SF(4) + OP_l_4_c_24_r_*SF(6) - OP_l_2_c_24_r_*SPP(1) - OP_l_14_c_24_r_*SPP(9) + OP_l_15_c_24_r_*SPP(3) + OP_l_16_c_24_r_*SPP(6);
nextP(7,24) = OP_l_7_c_24_r_ + OP_l_2_c_24_r_*SF(5) - OP_l_3_c_24_r_*SF(6) + OP_l_4_c_24_r_*SF(4) + OP_l_1_c_24_r_*SPP(1) + OP_l_14_c_24_r_*SPP(5) - OP_l_15_c_24_r_*SPP(8) - OP_l_16_c_24_r_*SPP(2);
nextP(8,24) = OP_l_8_c_24_r_ + OP_l_5_c_24_r_*dt;
nextP(9,24) = OP_l_9_c_24_r_ + OP_l_6_c_24_r_*dt;
nextP(10,24) = OP_l_10_c_24_r_ + OP_l_7_c_24_r_*dt;
nextP(11,24) = OP_l_11_c_24_r_;
nextP(12,24) = OP_l_12_c_24_r_;
nextP(13,24) = OP_l_13_c_24_r_;
nextP(14,24) = OP_l_14_c_24_r_;
nextP(15,24) = OP_l_15_c_24_r_;
nextP(16,24) = OP_l_16_c_24_r_;
nextP(17,24) = OP_l_17_c_24_r_;
nextP(18,24) = OP_l_18_c_24_r_;
nextP(19,24) = OP_l_19_c_24_r_;
nextP(20,24) = OP_l_20_c_24_r_;
nextP(21,24) = OP_l_21_c_24_r_;
nextP(22,24) = OP_l_22_c_24_r_;
nextP(23,24) = OP_l_23_c_24_r_;
nextP(24,24) = OP_l_24_c_24_r_;


SH_TAS = zeros(3,1);
SH_TAS(1) = 1/((ve - vwe)^2 + (vn - vwn)^2 + vd^2)^(1/2);
SH_TAS(2) = (SH_TAS(1)*(2*ve - 2*vwe))/2;
SH_TAS(3) = (SH_TAS(1)*(2*vn - 2*vwn))/2;

H_TAS = zeros(1,24);
H_TAS(1,5) = SH_TAS(3);
H_TAS(1,6) = SH_TAS(2);
H_TAS(1,7) = vd*SH_TAS(1);
H_TAS(1,23) = -SH_TAS(3);
H_TAS(1,24) = -SH_TAS(2);


SK_TAS = zeros(2,1);
SK_TAS(1) = 1/(R_TAS + SH_TAS(3)*(OP_l_5_c_5_r_*SH_TAS(3) + OP_l_6_c_5_r_*SH_TAS(2) - OP_l_23_c_5_r_*SH_TAS(3) - OP_l_24_c_5_r_*SH_TAS(2) + OP_l_7_c_5_r_*vd*SH_TAS(1)) + SH_TAS(2)*(OP_l_5_c_6_r_*SH_TAS(3) + OP_l_6_c_6_r_*SH_TAS(2) - OP_l_23_c_6_r_*SH_TAS(3) - OP_l_24_c_6_r_*SH_TAS(2) + OP_l_7_c_6_r_*vd*SH_TAS(1)) - SH_TAS(3)*(OP_l_5_c_23_r_*SH_TAS(3) + OP_l_6_c_23_r_*SH_TAS(2) - OP_l_23_c_23_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(2) + OP_l_7_c_23_r_*vd*SH_TAS(1)) - SH_TAS(2)*(OP_l_5_c_24_r_*SH_TAS(3) + OP_l_6_c_24_r_*SH_TAS(2) - OP_l_23_c_24_r_*SH_TAS(3) - OP_l_24_c_24_r_*SH_TAS(2) + OP_l_7_c_24_r_*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP_l_5_c_7_r_*SH_TAS(3) + OP_l_6_c_7_r_*SH_TAS(2) - OP_l_23_c_7_r_*SH_TAS(3) - OP_l_24_c_7_r_*SH_TAS(2) + OP_l_7_c_7_r_*vd*SH_TAS(1)));
SK_TAS(2) = SH_TAS(2);


Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_TAS(1)*(OP_l_1_c_5_r_*SH_TAS(3) - OP_l_1_c_23_r_*SH_TAS(3) + OP_l_1_c_6_r_*SK_TAS(2) - OP_l_1_c_24_r_*SK_TAS(2) + OP_l_1_c_7_r_*vd*SH_TAS(1));
Kfusion(2) = SK_TAS(1)*(OP_l_2_c_5_r_*SH_TAS(3) - OP_l_2_c_23_r_*SH_TAS(3) + OP_l_2_c_6_r_*SK_TAS(2) - OP_l_2_c_24_r_*SK_TAS(2) + OP_l_2_c_7_r_*vd*SH_TAS(1));
Kfusion(3) = SK_TAS(1)*(OP_l_3_c_5_r_*SH_TAS(3) - OP_l_3_c_23_r_*SH_TAS(3) + OP_l_3_c_6_r_*SK_TAS(2) - OP_l_3_c_24_r_*SK_TAS(2) + OP_l_3_c_7_r_*vd*SH_TAS(1));
Kfusion(4) = SK_TAS(1)*(OP_l_4_c_5_r_*SH_TAS(3) - OP_l_4_c_23_r_*SH_TAS(3) + OP_l_4_c_6_r_*SK_TAS(2) - OP_l_4_c_24_r_*SK_TAS(2) + OP_l_4_c_7_r_*vd*SH_TAS(1));
Kfusion(5) = SK_TAS(1)*(OP_l_5_c_5_r_*SH_TAS(3) - OP_l_5_c_23_r_*SH_TAS(3) + OP_l_5_c_6_r_*SK_TAS(2) - OP_l_5_c_24_r_*SK_TAS(2) + OP_l_5_c_7_r_*vd*SH_TAS(1));
Kfusion(6) = SK_TAS(1)*(OP_l_6_c_5_r_*SH_TAS(3) - OP_l_6_c_23_r_*SH_TAS(3) + OP_l_6_c_6_r_*SK_TAS(2) - OP_l_6_c_24_r_*SK_TAS(2) + OP_l_6_c_7_r_*vd*SH_TAS(1));
Kfusion(7) = SK_TAS(1)*(OP_l_7_c_5_r_*SH_TAS(3) - OP_l_7_c_23_r_*SH_TAS(3) + OP_l_7_c_6_r_*SK_TAS(2) - OP_l_7_c_24_r_*SK_TAS(2) + OP_l_7_c_7_r_*vd*SH_TAS(1));
Kfusion(8) = SK_TAS(1)*(OP_l_8_c_5_r_*SH_TAS(3) - OP_l_8_c_23_r_*SH_TAS(3) + OP_l_8_c_6_r_*SK_TAS(2) - OP_l_8_c_24_r_*SK_TAS(2) + OP_l_8_c_7_r_*vd*SH_TAS(1));
Kfusion(9) = SK_TAS(1)*(OP_l_9_c_5_r_*SH_TAS(3) - OP_l_9_c_23_r_*SH_TAS(3) + OP_l_9_c_6_r_*SK_TAS(2) - OP_l_9_c_24_r_*SK_TAS(2) + OP_l_9_c_7_r_*vd*SH_TAS(1));
Kfusion(10) = SK_TAS(1)*(OP_l_10_c_5_r_*SH_TAS(3) - OP_l_10_c_23_r_*SH_TAS(3) + OP_l_10_c_6_r_*SK_TAS(2) - OP_l_10_c_24_r_*SK_TAS(2) + OP_l_10_c_7_r_*vd*SH_TAS(1));
Kfusion(11) = SK_TAS(1)*(OP_l_11_c_5_r_*SH_TAS(3) - OP_l_11_c_23_r_*SH_TAS(3) + OP_l_11_c_6_r_*SK_TAS(2) - OP_l_11_c_24_r_*SK_TAS(2) + OP_l_11_c_7_r_*vd*SH_TAS(1));
Kfusion(12) = SK_TAS(1)*(OP_l_12_c_5_r_*SH_TAS(3) - OP_l_12_c_23_r_*SH_TAS(3) + OP_l_12_c_6_r_*SK_TAS(2) - OP_l_12_c_24_r_*SK_TAS(2) + OP_l_12_c_7_r_*vd*SH_TAS(1));
Kfusion(13) = SK_TAS(1)*(OP_l_13_c_5_r_*SH_TAS(3) - OP_l_13_c_23_r_*SH_TAS(3) + OP_l_13_c_6_r_*SK_TAS(2) - OP_l_13_c_24_r_*SK_TAS(2) + OP_l_13_c_7_r_*vd*SH_TAS(1));
Kfusion(14) = SK_TAS(1)*(OP_l_14_c_5_r_*SH_TAS(3) - OP_l_14_c_23_r_*SH_TAS(3) + OP_l_14_c_6_r_*SK_TAS(2) - OP_l_14_c_24_r_*SK_TAS(2) + OP_l_14_c_7_r_*vd*SH_TAS(1));
Kfusion(15) = SK_TAS(1)*(OP_l_15_c_5_r_*SH_TAS(3) - OP_l_15_c_23_r_*SH_TAS(3) + OP_l_15_c_6_r_*SK_TAS(2) - OP_l_15_c_24_r_*SK_TAS(2) + OP_l_15_c_7_r_*vd*SH_TAS(1));
Kfusion(16) = SK_TAS(1)*(OP_l_16_c_5_r_*SH_TAS(3) - OP_l_16_c_23_r_*SH_TAS(3) + OP_l_16_c_6_r_*SK_TAS(2) - OP_l_16_c_24_r_*SK_TAS(2) + OP_l_16_c_7_r_*vd*SH_TAS(1));
Kfusion(17) = SK_TAS(1)*(OP_l_17_c_5_r_*SH_TAS(3) - OP_l_17_c_23_r_*SH_TAS(3) + OP_l_17_c_6_r_*SK_TAS(2) - OP_l_17_c_24_r_*SK_TAS(2) + OP_l_17_c_7_r_*vd*SH_TAS(1));
Kfusion(18) = SK_TAS(1)*(OP_l_18_c_5_r_*SH_TAS(3) - OP_l_18_c_23_r_*SH_TAS(3) + OP_l_18_c_6_r_*SK_TAS(2) - OP_l_18_c_24_r_*SK_TAS(2) + OP_l_18_c_7_r_*vd*SH_TAS(1));
Kfusion(19) = SK_TAS(1)*(OP_l_19_c_5_r_*SH_TAS(3) - OP_l_19_c_23_r_*SH_TAS(3) + OP_l_19_c_6_r_*SK_TAS(2) - OP_l_19_c_24_r_*SK_TAS(2) + OP_l_19_c_7_r_*vd*SH_TAS(1));
Kfusion(20) = SK_TAS(1)*(OP_l_20_c_5_r_*SH_TAS(3) - OP_l_20_c_23_r_*SH_TAS(3) + OP_l_20_c_6_r_*SK_TAS(2) - OP_l_20_c_24_r_*SK_TAS(2) + OP_l_20_c_7_r_*vd*SH_TAS(1));
Kfusion(21) = SK_TAS(1)*(OP_l_21_c_5_r_*SH_TAS(3) - OP_l_21_c_23_r_*SH_TAS(3) + OP_l_21_c_6_r_*SK_TAS(2) - OP_l_21_c_24_r_*SK_TAS(2) + OP_l_21_c_7_r_*vd*SH_TAS(1));
Kfusion(22) = SK_TAS(1)*(OP_l_22_c_5_r_*SH_TAS(3) - OP_l_22_c_23_r_*SH_TAS(3) + OP_l_22_c_6_r_*SK_TAS(2) - OP_l_22_c_24_r_*SK_TAS(2) + OP_l_22_c_7_r_*vd*SH_TAS(1));
Kfusion(23) = SK_TAS(1)*(OP_l_23_c_5_r_*SH_TAS(3) - OP_l_23_c_23_r_*SH_TAS(3) + OP_l_23_c_6_r_*SK_TAS(2) - OP_l_23_c_24_r_*SK_TAS(2) + OP_l_23_c_7_r_*vd*SH_TAS(1));
Kfusion(24) = SK_TAS(1)*(OP_l_24_c_5_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(3) + OP_l_24_c_6_r_*SK_TAS(2) - OP_l_24_c_24_r_*SK_TAS(2) + OP_l_24_c_7_r_*vd*SH_TAS(1));


SH_BETA = zeros(13,1);
SH_BETA(1) = (vn - vwn)*(q0^2 + q1^2 - q2^2 - q3^2) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
SH_BETA(2) = (ve - vwe)*(q0^2 - q1^2 + q2^2 - q3^2) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2);
SH_BETA(3) = vn - vwn;
SH_BETA(4) = ve - vwe;
SH_BETA(5) = 1/SH_BETA(1)^2;
SH_BETA(6) = 1/SH_BETA(1);
SH_BETA(7) = SH_BETA(6)*(q0^2 - q1^2 + q2^2 - q3^2);
SH_BETA(8) = q0^2 + q1^2 - q2^2 - q3^2;
SH_BETA(9) = 2*q0*SH_BETA(4) - 2*q3*SH_BETA(3) + 2*q1*vd;
SH_BETA(10) = 2*q0*SH_BETA(3) + 2*q3*SH_BETA(4) - 2*q2*vd;
SH_BETA(11) = 2*q2*SH_BETA(3) - 2*q1*SH_BETA(4) + 2*q0*vd;
SH_BETA(12) = 2*q1*SH_BETA(3) + 2*q2*SH_BETA(4) + 2*q3*vd;
SH_BETA(13) = 2*q0*q3;

H_BETA = zeros(1,24);
H_BETA(1,1) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10);
H_BETA(1,2) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12);
H_BETA(1,3) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11);
H_BETA(1,4) = - SH_BETA(6)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9);
H_BETA(1,5) = - SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) - SH_BETA(2)*SH_BETA(5)*SH_BETA(8);
H_BETA(1,6) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2);
H_BETA(1,7) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3);
H_BETA(1,23) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8);
H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2) - SH_BETA(7);


SK_BETA = zeros(8,1);
SK_BETA(1) = 1/(R_BETA - (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP_l_23_c_5_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_5_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_5_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_5_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_5_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_5_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_5_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_5_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_5_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP_l_23_c_23_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_23_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_23_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_23_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_23_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_23_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_23_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_23_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_23_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP_l_23_c_6_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_6_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_6_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_6_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_6_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_6_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_6_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_6_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_6_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP_l_23_c_24_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_24_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_24_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_24_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_24_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_24_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_24_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_24_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_24_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10))*(OP_l_23_c_1_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_1_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_1_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_1_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_1_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_1_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_1_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_1_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_1_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12))*(OP_l_23_c_2_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_2_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_2_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_2_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_2_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_2_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_2_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_2_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_2_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11))*(OP_l_23_c_3_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_3_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_3_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_3_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_3_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_3_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_3_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_3_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_3_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP_l_23_c_4_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_4_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_4_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_4_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_4_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_4_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_4_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_4_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_4_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))*(OP_l_23_c_7_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_7_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_7_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_7_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_7_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_7_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_7_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_7_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_7_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))));
SK_BETA(2) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8);
SK_BETA(3) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2);
SK_BETA(4) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3);
SK_BETA(5) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12);
SK_BETA(6) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10);
SK_BETA(7) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11);
SK_BETA(8) = SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9);


Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_BETA(1)*(OP_l_1_c_1_r_*SK_BETA(6) + OP_l_1_c_2_r_*SK_BETA(5) - OP_l_1_c_5_r_*SK_BETA(2) + OP_l_1_c_6_r_*SK_BETA(3) + OP_l_1_c_3_r_*SK_BETA(7) + OP_l_1_c_7_r_*SK_BETA(4) - OP_l_1_c_4_r_*SK_BETA(8) + OP_l_1_c_23_r_*SK_BETA(2) - OP_l_1_c_24_r_*SK_BETA(3));
Kfusion(2) = SK_BETA(1)*(OP_l_2_c_1_r_*SK_BETA(6) + OP_l_2_c_2_r_*SK_BETA(5) - OP_l_2_c_5_r_*SK_BETA(2) + OP_l_2_c_6_r_*SK_BETA(3) + OP_l_2_c_3_r_*SK_BETA(7) + OP_l_2_c_7_r_*SK_BETA(4) - OP_l_2_c_4_r_*SK_BETA(8) + OP_l_2_c_23_r_*SK_BETA(2) - OP_l_2_c_24_r_*SK_BETA(3));
Kfusion(3) = SK_BETA(1)*(OP_l_3_c_1_r_*SK_BETA(6) + OP_l_3_c_2_r_*SK_BETA(5) - OP_l_3_c_5_r_*SK_BETA(2) + OP_l_3_c_6_r_*SK_BETA(3) + OP_l_3_c_3_r_*SK_BETA(7) + OP_l_3_c_7_r_*SK_BETA(4) - OP_l_3_c_4_r_*SK_BETA(8) + OP_l_3_c_23_r_*SK_BETA(2) - OP_l_3_c_24_r_*SK_BETA(3));
Kfusion(4) = SK_BETA(1)*(OP_l_4_c_1_r_*SK_BETA(6) + OP_l_4_c_2_r_*SK_BETA(5) - OP_l_4_c_5_r_*SK_BETA(2) + OP_l_4_c_6_r_*SK_BETA(3) + OP_l_4_c_3_r_*SK_BETA(7) + OP_l_4_c_7_r_*SK_BETA(4) - OP_l_4_c_4_r_*SK_BETA(8) + OP_l_4_c_23_r_*SK_BETA(2) - OP_l_4_c_24_r_*SK_BETA(3));
Kfusion(5) = SK_BETA(1)*(OP_l_5_c_1_r_*SK_BETA(6) + OP_l_5_c_2_r_*SK_BETA(5) - OP_l_5_c_5_r_*SK_BETA(2) + OP_l_5_c_6_r_*SK_BETA(3) + OP_l_5_c_3_r_*SK_BETA(7) + OP_l_5_c_7_r_*SK_BETA(4) - OP_l_5_c_4_r_*SK_BETA(8) + OP_l_5_c_23_r_*SK_BETA(2) - OP_l_5_c_24_r_*SK_BETA(3));
Kfusion(6) = SK_BETA(1)*(OP_l_6_c_1_r_*SK_BETA(6) + OP_l_6_c_2_r_*SK_BETA(5) - OP_l_6_c_5_r_*SK_BETA(2) + OP_l_6_c_6_r_*SK_BETA(3) + OP_l_6_c_3_r_*SK_BETA(7) + OP_l_6_c_7_r_*SK_BETA(4) - OP_l_6_c_4_r_*SK_BETA(8) + OP_l_6_c_23_r_*SK_BETA(2) - OP_l_6_c_24_r_*SK_BETA(3));
Kfusion(7) = SK_BETA(1)*(OP_l_7_c_1_r_*SK_BETA(6) + OP_l_7_c_2_r_*SK_BETA(5) - OP_l_7_c_5_r_*SK_BETA(2) + OP_l_7_c_6_r_*SK_BETA(3) + OP_l_7_c_3_r_*SK_BETA(7) + OP_l_7_c_7_r_*SK_BETA(4) - OP_l_7_c_4_r_*SK_BETA(8) + OP_l_7_c_23_r_*SK_BETA(2) - OP_l_7_c_24_r_*SK_BETA(3));
Kfusion(8) = SK_BETA(1)*(OP_l_8_c_1_r_*SK_BETA(6) + OP_l_8_c_2_r_*SK_BETA(5) - OP_l_8_c_5_r_*SK_BETA(2) + OP_l_8_c_6_r_*SK_BETA(3) + OP_l_8_c_3_r_*SK_BETA(7) + OP_l_8_c_7_r_*SK_BETA(4) - OP_l_8_c_4_r_*SK_BETA(8) + OP_l_8_c_23_r_*SK_BETA(2) - OP_l_8_c_24_r_*SK_BETA(3));
Kfusion(9) = SK_BETA(1)*(OP_l_9_c_1_r_*SK_BETA(6) + OP_l_9_c_2_r_*SK_BETA(5) - OP_l_9_c_5_r_*SK_BETA(2) + OP_l_9_c_6_r_*SK_BETA(3) + OP_l_9_c_3_r_*SK_BETA(7) + OP_l_9_c_7_r_*SK_BETA(4) - OP_l_9_c_4_r_*SK_BETA(8) + OP_l_9_c_23_r_*SK_BETA(2) - OP_l_9_c_24_r_*SK_BETA(3));
Kfusion(10) = SK_BETA(1)*(OP_l_10_c_1_r_*SK_BETA(6) + OP_l_10_c_2_r_*SK_BETA(5) - OP_l_10_c_5_r_*SK_BETA(2) + OP_l_10_c_6_r_*SK_BETA(3) + OP_l_10_c_3_r_*SK_BETA(7) + OP_l_10_c_7_r_*SK_BETA(4) - OP_l_10_c_4_r_*SK_BETA(8) + OP_l_10_c_23_r_*SK_BETA(2) - OP_l_10_c_24_r_*SK_BETA(3));
Kfusion(11) = SK_BETA(1)*(OP_l_11_c_1_r_*SK_BETA(6) + OP_l_11_c_2_r_*SK_BETA(5) - OP_l_11_c_5_r_*SK_BETA(2) + OP_l_11_c_6_r_*SK_BETA(3) + OP_l_11_c_3_r_*SK_BETA(7) + OP_l_11_c_7_r_*SK_BETA(4) - OP_l_11_c_4_r_*SK_BETA(8) + OP_l_11_c_23_r_*SK_BETA(2) - OP_l_11_c_24_r_*SK_BETA(3));
Kfusion(12) = SK_BETA(1)*(OP_l_12_c_1_r_*SK_BETA(6) + OP_l_12_c_2_r_*SK_BETA(5) - OP_l_12_c_5_r_*SK_BETA(2) + OP_l_12_c_6_r_*SK_BETA(3) + OP_l_12_c_3_r_*SK_BETA(7) + OP_l_12_c_7_r_*SK_BETA(4) - OP_l_12_c_4_r_*SK_BETA(8) + OP_l_12_c_23_r_*SK_BETA(2) - OP_l_12_c_24_r_*SK_BETA(3));
Kfusion(13) = SK_BETA(1)*(OP_l_13_c_1_r_*SK_BETA(6) + OP_l_13_c_2_r_*SK_BETA(5) - OP_l_13_c_5_r_*SK_BETA(2) + OP_l_13_c_6_r_*SK_BETA(3) + OP_l_13_c_3_r_*SK_BETA(7) + OP_l_13_c_7_r_*SK_BETA(4) - OP_l_13_c_4_r_*SK_BETA(8) + OP_l_13_c_23_r_*SK_BETA(2) - OP_l_13_c_24_r_*SK_BETA(3));
Kfusion(14) = SK_BETA(1)*(OP_l_14_c_1_r_*SK_BETA(6) + OP_l_14_c_2_r_*SK_BETA(5) - OP_l_14_c_5_r_*SK_BETA(2) + OP_l_14_c_6_r_*SK_BETA(3) + OP_l_14_c_3_r_*SK_BETA(7) + OP_l_14_c_7_r_*SK_BETA(4) - OP_l_14_c_4_r_*SK_BETA(8) + OP_l_14_c_23_r_*SK_BETA(2) - OP_l_14_c_24_r_*SK_BETA(3));
Kfusion(15) = SK_BETA(1)*(OP_l_15_c_1_r_*SK_BETA(6) + OP_l_15_c_2_r_*SK_BETA(5) - OP_l_15_c_5_r_*SK_BETA(2) + OP_l_15_c_6_r_*SK_BETA(3) + OP_l_15_c_3_r_*SK_BETA(7) + OP_l_15_c_7_r_*SK_BETA(4) - OP_l_15_c_4_r_*SK_BETA(8) + OP_l_15_c_23_r_*SK_BETA(2) - OP_l_15_c_24_r_*SK_BETA(3));
Kfusion(16) = SK_BETA(1)*(OP_l_16_c_1_r_*SK_BETA(6) + OP_l_16_c_2_r_*SK_BETA(5) - OP_l_16_c_5_r_*SK_BETA(2) + OP_l_16_c_6_r_*SK_BETA(3) + OP_l_16_c_3_r_*SK_BETA(7) + OP_l_16_c_7_r_*SK_BETA(4) - OP_l_16_c_4_r_*SK_BETA(8) + OP_l_16_c_23_r_*SK_BETA(2) - OP_l_16_c_24_r_*SK_BETA(3));
Kfusion(17) = SK_BETA(1)*(OP_l_17_c_1_r_*SK_BETA(6) + OP_l_17_c_2_r_*SK_BETA(5) - OP_l_17_c_5_r_*SK_BETA(2) + OP_l_17_c_6_r_*SK_BETA(3) + OP_l_17_c_3_r_*SK_BETA(7) + OP_l_17_c_7_r_*SK_BETA(4) - OP_l_17_c_4_r_*SK_BETA(8) + OP_l_17_c_23_r_*SK_BETA(2) - OP_l_17_c_24_r_*SK_BETA(3));
Kfusion(18) = SK_BETA(1)*(OP_l_18_c_1_r_*SK_BETA(6) + OP_l_18_c_2_r_*SK_BETA(5) - OP_l_18_c_5_r_*SK_BETA(2) + OP_l_18_c_6_r_*SK_BETA(3) + OP_l_18_c_3_r_*SK_BETA(7) + OP_l_18_c_7_r_*SK_BETA(4) - OP_l_18_c_4_r_*SK_BETA(8) + OP_l_18_c_23_r_*SK_BETA(2) - OP_l_18_c_24_r_*SK_BETA(3));
Kfusion(19) = SK_BETA(1)*(OP_l_19_c_1_r_*SK_BETA(6) + OP_l_19_c_2_r_*SK_BETA(5) - OP_l_19_c_5_r_*SK_BETA(2) + OP_l_19_c_6_r_*SK_BETA(3) + OP_l_19_c_3_r_*SK_BETA(7) + OP_l_19_c_7_r_*SK_BETA(4) - OP_l_19_c_4_r_*SK_BETA(8) + OP_l_19_c_23_r_*SK_BETA(2) - OP_l_19_c_24_r_*SK_BETA(3));
Kfusion(20) = SK_BETA(1)*(OP_l_20_c_1_r_*SK_BETA(6) + OP_l_20_c_2_r_*SK_BETA(5) - OP_l_20_c_5_r_*SK_BETA(2) + OP_l_20_c_6_r_*SK_BETA(3) + OP_l_20_c_3_r_*SK_BETA(7) + OP_l_20_c_7_r_*SK_BETA(4) - OP_l_20_c_4_r_*SK_BETA(8) + OP_l_20_c_23_r_*SK_BETA(2) - OP_l_20_c_24_r_*SK_BETA(3));
Kfusion(21) = SK_BETA(1)*(OP_l_21_c_1_r_*SK_BETA(6) + OP_l_21_c_2_r_*SK_BETA(5) - OP_l_21_c_5_r_*SK_BETA(2) + OP_l_21_c_6_r_*SK_BETA(3) + OP_l_21_c_3_r_*SK_BETA(7) + OP_l_21_c_7_r_*SK_BETA(4) - OP_l_21_c_4_r_*SK_BETA(8) + OP_l_21_c_23_r_*SK_BETA(2) - OP_l_21_c_24_r_*SK_BETA(3));
Kfusion(22) = SK_BETA(1)*(OP_l_22_c_1_r_*SK_BETA(6) + OP_l_22_c_2_r_*SK_BETA(5) - OP_l_22_c_5_r_*SK_BETA(2) + OP_l_22_c_6_r_*SK_BETA(3) + OP_l_22_c_3_r_*SK_BETA(7) + OP_l_22_c_7_r_*SK_BETA(4) - OP_l_22_c_4_r_*SK_BETA(8) + OP_l_22_c_23_r_*SK_BETA(2) - OP_l_22_c_24_r_*SK_BETA(3));
Kfusion(23) = SK_BETA(1)*(OP_l_23_c_1_r_*SK_BETA(6) + OP_l_23_c_2_r_*SK_BETA(5) - OP_l_23_c_5_r_*SK_BETA(2) + OP_l_23_c_6_r_*SK_BETA(3) + OP_l_23_c_3_r_*SK_BETA(7) + OP_l_23_c_7_r_*SK_BETA(4) - OP_l_23_c_4_r_*SK_BETA(8) + OP_l_23_c_23_r_*SK_BETA(2) - OP_l_23_c_24_r_*SK_BETA(3));
Kfusion(24) = SK_BETA(1)*(OP_l_24_c_1_r_*SK_BETA(6) + OP_l_24_c_2_r_*SK_BETA(5) - OP_l_24_c_5_r_*SK_BETA(2) + OP_l_24_c_6_r_*SK_BETA(3) + OP_l_24_c_3_r_*SK_BETA(7) + OP_l_24_c_7_r_*SK_BETA(4) - OP_l_24_c_4_r_*SK_BETA(8) + OP_l_24_c_23_r_*SK_BETA(2) - OP_l_24_c_24_r_*SK_BETA(3));


SH_MAG = zeros(9,1);
SH_MAG(1) = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
SH_MAG(2) = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
SH_MAG(3) = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
SH_MAG(4) = q3^2;
SH_MAG(5) = q2^2;
SH_MAG(6) = q1^2;
SH_MAG(7) = q0^2;
SH_MAG(8) = 2*magN*q0;
SH_MAG(9) = 2*magE*q3;


H_MAG = zeros(1,24);
H_MAG(1) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
H_MAG(2) = SH_MAG(1);
H_MAG(3) = -SH_MAG(2);
H_MAG(4) = SH_MAG(3);
H_MAG(17) = SH_MAG(6) - SH_MAG(5) - SH_MAG(4) + SH_MAG(7);
H_MAG(18) = 2*q0*q3 + 2*q1*q2;
H_MAG(19) = 2*q1*q3 - 2*q0*q2;
H_MAG(20) = 1;


SK_MX = zeros(5,1);
SK_MX(1) = 1/(OP_l_20_c_20_r_ + R_MAG + OP_l_2_c_20_r_*SH_MAG(1) - OP_l_3_c_20_r_*SH_MAG(2) + OP_l_4_c_20_r_*SH_MAG(3) - OP_l_17_c_20_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + (2*q0*q3 + 2*q1*q2)*(OP_l_20_c_18_r_ + OP_l_2_c_18_r_*SH_MAG(1) - OP_l_3_c_18_r_*SH_MAG(2) + OP_l_4_c_18_r_*SH_MAG(3) - OP_l_17_c_18_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_18_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_18_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(OP_l_20_c_19_r_ + OP_l_2_c_19_r_*SH_MAG(1) - OP_l_3_c_19_r_*SH_MAG(2) + OP_l_4_c_19_r_*SH_MAG(3) - OP_l_17_c_19_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_19_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_19_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_20_c_1_r_ + OP_l_2_c_1_r_*SH_MAG(1) - OP_l_3_c_1_r_*SH_MAG(2) + OP_l_4_c_1_r_*SH_MAG(3) - OP_l_17_c_1_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_1_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_1_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_18_c_20_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_20_r_*(2*q0*q2 - 2*q1*q3) + SH_MAG(1)*(OP_l_20_c_2_r_ + OP_l_2_c_2_r_*SH_MAG(1) - OP_l_3_c_2_r_*SH_MAG(2) + OP_l_4_c_2_r_*SH_MAG(3) - OP_l_17_c_2_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_2_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_2_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(2)*(OP_l_20_c_3_r_ + OP_l_2_c_3_r_*SH_MAG(1) - OP_l_3_c_3_r_*SH_MAG(2) + OP_l_4_c_3_r_*SH_MAG(3) - OP_l_17_c_3_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_3_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_3_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(3)*(OP_l_20_c_4_r_ + OP_l_2_c_4_r_*SH_MAG(1) - OP_l_3_c_4_r_*SH_MAG(2) + OP_l_4_c_4_r_*SH_MAG(3) - OP_l_17_c_4_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_4_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_4_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7))*(OP_l_20_c_17_r_ + OP_l_2_c_17_r_*SH_MAG(1) - OP_l_3_c_17_r_*SH_MAG(2) + OP_l_4_c_17_r_*SH_MAG(3) - OP_l_17_c_17_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_17_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_17_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_1_c_20_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2));
SK_MX(2) = SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7);
SK_MX(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
SK_MX(4) = 2*q0*q2 - 2*q1*q3;
SK_MX(5) = 2*q0*q3 + 2*q1*q2;


Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_MX(1)*(OP_l_1_c_20_r_ + OP_l_1_c_2_r_*SH_MAG(1) - OP_l_1_c_3_r_*SH_MAG(2) + OP_l_1_c_4_r_*SH_MAG(3) + OP_l_1_c_1_r_*SK_MX(3) - OP_l_1_c_17_r_*SK_MX(2) + OP_l_1_c_18_r_*SK_MX(5) - OP_l_1_c_19_r_*SK_MX(4));
Kfusion(2) = SK_MX(1)*(OP_l_2_c_20_r_ + OP_l_2_c_2_r_*SH_MAG(1) - OP_l_2_c_3_r_*SH_MAG(2) + OP_l_2_c_4_r_*SH_MAG(3) + OP_l_2_c_1_r_*SK_MX(3) - OP_l_2_c_17_r_*SK_MX(2) + OP_l_2_c_18_r_*SK_MX(5) - OP_l_2_c_19_r_*SK_MX(4));
Kfusion(3) = SK_MX(1)*(OP_l_3_c_20_r_ + OP_l_3_c_2_r_*SH_MAG(1) - OP_l_3_c_3_r_*SH_MAG(2) + OP_l_3_c_4_r_*SH_MAG(3) + OP_l_3_c_1_r_*SK_MX(3) - OP_l_3_c_17_r_*SK_MX(2) + OP_l_3_c_18_r_*SK_MX(5) - OP_l_3_c_19_r_*SK_MX(4));
Kfusion(4) = SK_MX(1)*(OP_l_4_c_20_r_ + OP_l_4_c_2_r_*SH_MAG(1) - OP_l_4_c_3_r_*SH_MAG(2) + OP_l_4_c_4_r_*SH_MAG(3) + OP_l_4_c_1_r_*SK_MX(3) - OP_l_4_c_17_r_*SK_MX(2) + OP_l_4_c_18_r_*SK_MX(5) - OP_l_4_c_19_r_*SK_MX(4));
Kfusion(5) = SK_MX(1)*(OP_l_5_c_20_r_ + OP_l_5_c_2_r_*SH_MAG(1) - OP_l_5_c_3_r_*SH_MAG(2) + OP_l_5_c_4_r_*SH_MAG(3) + OP_l_5_c_1_r_*SK_MX(3) - OP_l_5_c_17_r_*SK_MX(2) + OP_l_5_c_18_r_*SK_MX(5) - OP_l_5_c_19_r_*SK_MX(4));
Kfusion(6) = SK_MX(1)*(OP_l_6_c_20_r_ + OP_l_6_c_2_r_*SH_MAG(1) - OP_l_6_c_3_r_*SH_MAG(2) + OP_l_6_c_4_r_*SH_MAG(3) + OP_l_6_c_1_r_*SK_MX(3) - OP_l_6_c_17_r_*SK_MX(2) + OP_l_6_c_18_r_*SK_MX(5) - OP_l_6_c_19_r_*SK_MX(4));
Kfusion(7) = SK_MX(1)*(OP_l_7_c_20_r_ + OP_l_7_c_2_r_*SH_MAG(1) - OP_l_7_c_3_r_*SH_MAG(2) + OP_l_7_c_4_r_*SH_MAG(3) + OP_l_7_c_1_r_*SK_MX(3) - OP_l_7_c_17_r_*SK_MX(2) + OP_l_7_c_18_r_*SK_MX(5) - OP_l_7_c_19_r_*SK_MX(4));
Kfusion(8) = SK_MX(1)*(OP_l_8_c_20_r_ + OP_l_8_c_2_r_*SH_MAG(1) - OP_l_8_c_3_r_*SH_MAG(2) + OP_l_8_c_4_r_*SH_MAG(3) + OP_l_8_c_1_r_*SK_MX(3) - OP_l_8_c_17_r_*SK_MX(2) + OP_l_8_c_18_r_*SK_MX(5) - OP_l_8_c_19_r_*SK_MX(4));
Kfusion(9) = SK_MX(1)*(OP_l_9_c_20_r_ + OP_l_9_c_2_r_*SH_MAG(1) - OP_l_9_c_3_r_*SH_MAG(2) + OP_l_9_c_4_r_*SH_MAG(3) + OP_l_9_c_1_r_*SK_MX(3) - OP_l_9_c_17_r_*SK_MX(2) + OP_l_9_c_18_r_*SK_MX(5) - OP_l_9_c_19_r_*SK_MX(4));
Kfusion(10) = SK_MX(1)*(OP_l_10_c_20_r_ + OP_l_10_c_2_r_*SH_MAG(1) - OP_l_10_c_3_r_*SH_MAG(2) + OP_l_10_c_4_r_*SH_MAG(3) + OP_l_10_c_1_r_*SK_MX(3) - OP_l_10_c_17_r_*SK_MX(2) + OP_l_10_c_18_r_*SK_MX(5) - OP_l_10_c_19_r_*SK_MX(4));
Kfusion(11) = SK_MX(1)*(OP_l_11_c_20_r_ + OP_l_11_c_2_r_*SH_MAG(1) - OP_l_11_c_3_r_*SH_MAG(2) + OP_l_11_c_4_r_*SH_MAG(3) + OP_l_11_c_1_r_*SK_MX(3) - OP_l_11_c_17_r_*SK_MX(2) + OP_l_11_c_18_r_*SK_MX(5) - OP_l_11_c_19_r_*SK_MX(4));
Kfusion(12) = SK_MX(1)*(OP_l_12_c_20_r_ + OP_l_12_c_2_r_*SH_MAG(1) - OP_l_12_c_3_r_*SH_MAG(2) + OP_l_12_c_4_r_*SH_MAG(3) + OP_l_12_c_1_r_*SK_MX(3) - OP_l_12_c_17_r_*SK_MX(2) + OP_l_12_c_18_r_*SK_MX(5) - OP_l_12_c_19_r_*SK_MX(4));
Kfusion(13) = SK_MX(1)*(OP_l_13_c_20_r_ + OP_l_13_c_2_r_*SH_MAG(1) - OP_l_13_c_3_r_*SH_MAG(2) + OP_l_13_c_4_r_*SH_MAG(3) + OP_l_13_c_1_r_*SK_MX(3) - OP_l_13_c_17_r_*SK_MX(2) + OP_l_13_c_18_r_*SK_MX(5) - OP_l_13_c_19_r_*SK_MX(4));
Kfusion(14) = SK_MX(1)*(OP_l_14_c_20_r_ + OP_l_14_c_2_r_*SH_MAG(1) - OP_l_14_c_3_r_*SH_MAG(2) + OP_l_14_c_4_r_*SH_MAG(3) + OP_l_14_c_1_r_*SK_MX(3) - OP_l_14_c_17_r_*SK_MX(2) + OP_l_14_c_18_r_*SK_MX(5) - OP_l_14_c_19_r_*SK_MX(4));
Kfusion(15) = SK_MX(1)*(OP_l_15_c_20_r_ + OP_l_15_c_2_r_*SH_MAG(1) - OP_l_15_c_3_r_*SH_MAG(2) + OP_l_15_c_4_r_*SH_MAG(3) + OP_l_15_c_1_r_*SK_MX(3) - OP_l_15_c_17_r_*SK_MX(2) + OP_l_15_c_18_r_*SK_MX(5) - OP_l_15_c_19_r_*SK_MX(4));
Kfusion(16) = SK_MX(1)*(OP_l_16_c_20_r_ + OP_l_16_c_2_r_*SH_MAG(1) - OP_l_16_c_3_r_*SH_MAG(2) + OP_l_16_c_4_r_*SH_MAG(3) + OP_l_16_c_1_r_*SK_MX(3) - OP_l_16_c_17_r_*SK_MX(2) + OP_l_16_c_18_r_*SK_MX(5) - OP_l_16_c_19_r_*SK_MX(4));
Kfusion(17) = SK_MX(1)*(OP_l_17_c_20_r_ + OP_l_17_c_2_r_*SH_MAG(1) - OP_l_17_c_3_r_*SH_MAG(2) + OP_l_17_c_4_r_*SH_MAG(3) + OP_l_17_c_1_r_*SK_MX(3) - OP_l_17_c_17_r_*SK_MX(2) + OP_l_17_c_18_r_*SK_MX(5) - OP_l_17_c_19_r_*SK_MX(4));
Kfusion(18) = SK_MX(1)*(OP_l_18_c_20_r_ + OP_l_18_c_2_r_*SH_MAG(1) - OP_l_18_c_3_r_*SH_MAG(2) + OP_l_18_c_4_r_*SH_MAG(3) + OP_l_18_c_1_r_*SK_MX(3) - OP_l_18_c_17_r_*SK_MX(2) + OP_l_18_c_18_r_*SK_MX(5) - OP_l_18_c_19_r_*SK_MX(4));
Kfusion(19) = SK_MX(1)*(OP_l_19_c_20_r_ + OP_l_19_c_2_r_*SH_MAG(1) - OP_l_19_c_3_r_*SH_MAG(2) + OP_l_19_c_4_r_*SH_MAG(3) + OP_l_19_c_1_r_*SK_MX(3) - OP_l_19_c_17_r_*SK_MX(2) + OP_l_19_c_18_r_*SK_MX(5) - OP_l_19_c_19_r_*SK_MX(4));
Kfusion(20) = SK_MX(1)*(OP_l_20_c_20_r_ + OP_l_20_c_2_r_*SH_MAG(1) - OP_l_20_c_3_r_*SH_MAG(2) + OP_l_20_c_4_r_*SH_MAG(3) + OP_l_20_c_1_r_*SK_MX(3) - OP_l_20_c_17_r_*SK_MX(2) + OP_l_20_c_18_r_*SK_MX(5) - OP_l_20_c_19_r_*SK_MX(4));
Kfusion(21) = SK_MX(1)*(OP_l_21_c_20_r_ + OP_l_21_c_2_r_*SH_MAG(1) - OP_l_21_c_3_r_*SH_MAG(2) + OP_l_21_c_4_r_*SH_MAG(3) + OP_l_21_c_1_r_*SK_MX(3) - OP_l_21_c_17_r_*SK_MX(2) + OP_l_21_c_18_r_*SK_MX(5) - OP_l_21_c_19_r_*SK_MX(4));
Kfusion(22) = SK_MX(1)*(OP_l_22_c_20_r_ + OP_l_22_c_2_r_*SH_MAG(1) - OP_l_22_c_3_r_*SH_MAG(2) + OP_l_22_c_4_r_*SH_MAG(3) + OP_l_22_c_1_r_*SK_MX(3) - OP_l_22_c_17_r_*SK_MX(2) + OP_l_22_c_18_r_*SK_MX(5) - OP_l_22_c_19_r_*SK_MX(4));
Kfusion(23) = SK_MX(1)*(OP_l_23_c_20_r_ + OP_l_23_c_2_r_*SH_MAG(1) - OP_l_23_c_3_r_*SH_MAG(2) + OP_l_23_c_4_r_*SH_MAG(3) + OP_l_23_c_1_r_*SK_MX(3) - OP_l_23_c_17_r_*SK_MX(2) + OP_l_23_c_18_r_*SK_MX(5) - OP_l_23_c_19_r_*SK_MX(4));
Kfusion(24) = SK_MX(1)*(OP_l_24_c_20_r_ + OP_l_24_c_2_r_*SH_MAG(1) - OP_l_24_c_3_r_*SH_MAG(2) + OP_l_24_c_4_r_*SH_MAG(3) + OP_l_24_c_1_r_*SK_MX(3) - OP_l_24_c_17_r_*SK_MX(2) + OP_l_24_c_18_r_*SK_MX(5) - OP_l_24_c_19_r_*SK_MX(4));


H_MAG = zeros(1,24);
H_MAG(1) = SH_MAG(3);
H_MAG(2) = SH_MAG(2);
H_MAG(3) = SH_MAG(1);
H_MAG(4) = 2*magD*q2 - SH_MAG(9) - SH_MAG(8);
H_MAG(17) = 2*q1*q2 - 2*q0*q3;
H_MAG(18) = SH_MAG(5) - SH_MAG(4) - SH_MAG(6) + SH_MAG(7);
H_MAG(19) = 2*q0*q1 + 2*q2*q3;
H_MAG(21) = 1;


SK_MY = zeros(5,1);
SK_MY(1) = 1/(OP_l_21_c_21_r_ + R_MAG + OP_l_1_c_21_r_*SH_MAG(3) + OP_l_2_c_21_r_*SH_MAG(2) + OP_l_3_c_21_r_*SH_MAG(1) - OP_l_18_c_21_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - (2*q0*q3 - 2*q1*q2)*(OP_l_21_c_17_r_ + OP_l_1_c_17_r_*SH_MAG(3) + OP_l_2_c_17_r_*SH_MAG(2) + OP_l_3_c_17_r_*SH_MAG(1) - OP_l_18_c_17_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_17_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_17_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(OP_l_21_c_19_r_ + OP_l_1_c_19_r_*SH_MAG(3) + OP_l_2_c_19_r_*SH_MAG(2) + OP_l_3_c_19_r_*SH_MAG(1) - OP_l_18_c_19_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_19_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_19_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_21_c_4_r_ + OP_l_1_c_4_r_*SH_MAG(3) + OP_l_2_c_4_r_*SH_MAG(2) + OP_l_3_c_4_r_*SH_MAG(1) - OP_l_18_c_4_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_4_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_4_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP_l_17_c_21_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_21_r_*(2*q0*q1 + 2*q2*q3) + SH_MAG(3)*(OP_l_21_c_1_r_ + OP_l_1_c_1_r_*SH_MAG(3) + OP_l_2_c_1_r_*SH_MAG(2) + OP_l_3_c_1_r_*SH_MAG(1) - OP_l_18_c_1_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_1_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_1_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(2)*(OP_l_21_c_2_r_ + OP_l_1_c_2_r_*SH_MAG(3) + OP_l_2_c_2_r_*SH_MAG(2) + OP_l_3_c_2_r_*SH_MAG(1) - OP_l_18_c_2_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_2_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_2_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP_l_21_c_3_r_ + OP_l_1_c_3_r_*SH_MAG(3) + OP_l_2_c_3_r_*SH_MAG(2) + OP_l_3_c_3_r_*SH_MAG(1) - OP_l_18_c_3_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_3_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_3_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7))*(OP_l_21_c_18_r_ + OP_l_1_c_18_r_*SH_MAG(3) + OP_l_2_c_18_r_*SH_MAG(2) + OP_l_3_c_18_r_*SH_MAG(1) - OP_l_18_c_18_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_18_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_18_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP_l_4_c_21_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2));
SK_MY(2) = SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7);
SK_MY(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
SK_MY(4) = 2*q0*q3 - 2*q1*q2;
SK_MY(5) = 2*q0*q1 + 2*q2*q3;


Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_MY(1)*(OP_l_1_c_21_r_ + OP_l_1_c_1_r_*SH_MAG(3) + OP_l_1_c_2_r_*SH_MAG(2) + OP_l_1_c_3_r_*SH_MAG(1) - OP_l_1_c_4_r_*SK_MY(3) - OP_l_1_c_18_r_*SK_MY(2) - OP_l_1_c_17_r_*SK_MY(4) + OP_l_1_c_19_r_*SK_MY(5));
Kfusion(2) = SK_MY(1)*(OP_l_2_c_21_r_ + OP_l_2_c_1_r_*SH_MAG(3) + OP_l_2_c_2_r_*SH_MAG(2) + OP_l_2_c_3_r_*SH_MAG(1) - OP_l_2_c_4_r_*SK_MY(3) - OP_l_2_c_18_r_*SK_MY(2) - OP_l_2_c_17_r_*SK_MY(4) + OP_l_2_c_19_r_*SK_MY(5));
Kfusion(3) = SK_MY(1)*(OP_l_3_c_21_r_ + OP_l_3_c_1_r_*SH_MAG(3) + OP_l_3_c_2_r_*SH_MAG(2) + OP_l_3_c_3_r_*SH_MAG(1) - OP_l_3_c_4_r_*SK_MY(3) - OP_l_3_c_18_r_*SK_MY(2) - OP_l_3_c_17_r_*SK_MY(4) + OP_l_3_c_19_r_*SK_MY(5));
Kfusion(4) = SK_MY(1)*(OP_l_4_c_21_r_ + OP_l_4_c_1_r_*SH_MAG(3) + OP_l_4_c_2_r_*SH_MAG(2) + OP_l_4_c_3_r_*SH_MAG(1) - OP_l_4_c_4_r_*SK_MY(3) - OP_l_4_c_18_r_*SK_MY(2) - OP_l_4_c_17_r_*SK_MY(4) + OP_l_4_c_19_r_*SK_MY(5));
Kfusion(5) = SK_MY(1)*(OP_l_5_c_21_r_ + OP_l_5_c_1_r_*SH_MAG(3) + OP_l_5_c_2_r_*SH_MAG(2) + OP_l_5_c_3_r_*SH_MAG(1) - OP_l_5_c_4_r_*SK_MY(3) - OP_l_5_c_18_r_*SK_MY(2) - OP_l_5_c_17_r_*SK_MY(4) + OP_l_5_c_19_r_*SK_MY(5));
Kfusion(6) = SK_MY(1)*(OP_l_6_c_21_r_ + OP_l_6_c_1_r_*SH_MAG(3) + OP_l_6_c_2_r_*SH_MAG(2) + OP_l_6_c_3_r_*SH_MAG(1) - OP_l_6_c_4_r_*SK_MY(3) - OP_l_6_c_18_r_*SK_MY(2) - OP_l_6_c_17_r_*SK_MY(4) + OP_l_6_c_19_r_*SK_MY(5));
Kfusion(7) = SK_MY(1)*(OP_l_7_c_21_r_ + OP_l_7_c_1_r_*SH_MAG(3) + OP_l_7_c_2_r_*SH_MAG(2) + OP_l_7_c_3_r_*SH_MAG(1) - OP_l_7_c_4_r_*SK_MY(3) - OP_l_7_c_18_r_*SK_MY(2) - OP_l_7_c_17_r_*SK_MY(4) + OP_l_7_c_19_r_*SK_MY(5));
Kfusion(8) = SK_MY(1)*(OP_l_8_c_21_r_ + OP_l_8_c_1_r_*SH_MAG(3) + OP_l_8_c_2_r_*SH_MAG(2) + OP_l_8_c_3_r_*SH_MAG(1) - OP_l_8_c_4_r_*SK_MY(3) - OP_l_8_c_18_r_*SK_MY(2) - OP_l_8_c_17_r_*SK_MY(4) + OP_l_8_c_19_r_*SK_MY(5));
Kfusion(9) = SK_MY(1)*(OP_l_9_c_21_r_ + OP_l_9_c_1_r_*SH_MAG(3) + OP_l_9_c_2_r_*SH_MAG(2) + OP_l_9_c_3_r_*SH_MAG(1) - OP_l_9_c_4_r_*SK_MY(3) - OP_l_9_c_18_r_*SK_MY(2) - OP_l_9_c_17_r_*SK_MY(4) + OP_l_9_c_19_r_*SK_MY(5));
Kfusion(10) = SK_MY(1)*(OP_l_10_c_21_r_ + OP_l_10_c_1_r_*SH_MAG(3) + OP_l_10_c_2_r_*SH_MAG(2) + OP_l_10_c_3_r_*SH_MAG(1) - OP_l_10_c_4_r_*SK_MY(3) - OP_l_10_c_18_r_*SK_MY(2) - OP_l_10_c_17_r_*SK_MY(4) + OP_l_10_c_19_r_*SK_MY(5));
Kfusion(11) = SK_MY(1)*(OP_l_11_c_21_r_ + OP_l_11_c_1_r_*SH_MAG(3) + OP_l_11_c_2_r_*SH_MAG(2) + OP_l_11_c_3_r_*SH_MAG(1) - OP_l_11_c_4_r_*SK_MY(3) - OP_l_11_c_18_r_*SK_MY(2) - OP_l_11_c_17_r_*SK_MY(4) + OP_l_11_c_19_r_*SK_MY(5));
Kfusion(12) = SK_MY(1)*(OP_l_12_c_21_r_ + OP_l_12_c_1_r_*SH_MAG(3) + OP_l_12_c_2_r_*SH_MAG(2) + OP_l_12_c_3_r_*SH_MAG(1) - OP_l_12_c_4_r_*SK_MY(3) - OP_l_12_c_18_r_*SK_MY(2) - OP_l_12_c_17_r_*SK_MY(4) + OP_l_12_c_19_r_*SK_MY(5));
Kfusion(13) = SK_MY(1)*(OP_l_13_c_21_r_ + OP_l_13_c_1_r_*SH_MAG(3) + OP_l_13_c_2_r_*SH_MAG(2) + OP_l_13_c_3_r_*SH_MAG(1) - OP_l_13_c_4_r_*SK_MY(3) - OP_l_13_c_18_r_*SK_MY(2) - OP_l_13_c_17_r_*SK_MY(4) + OP_l_13_c_19_r_*SK_MY(5));
Kfusion(14) = SK_MY(1)*(OP_l_14_c_21_r_ + OP_l_14_c_1_r_*SH_MAG(3) + OP_l_14_c_2_r_*SH_MAG(2) + OP_l_14_c_3_r_*SH_MAG(1) - OP_l_14_c_4_r_*SK_MY(3) - OP_l_14_c_18_r_*SK_MY(2) - OP_l_14_c_17_r_*SK_MY(4) + OP_l_14_c_19_r_*SK_MY(5));
Kfusion(15) = SK_MY(1)*(OP_l_15_c_21_r_ + OP_l_15_c_1_r_*SH_MAG(3) + OP_l_15_c_2_r_*SH_MAG(2) + OP_l_15_c_3_r_*SH_MAG(1) - OP_l_15_c_4_r_*SK_MY(3) - OP_l_15_c_18_r_*SK_MY(2) - OP_l_15_c_17_r_*SK_MY(4) + OP_l_15_c_19_r_*SK_MY(5));
Kfusion(16) = SK_MY(1)*(OP_l_16_c_21_r_ + OP_l_16_c_1_r_*SH_MAG(3) + OP_l_16_c_2_r_*SH_MAG(2) + OP_l_16_c_3_r_*SH_MAG(1) - OP_l_16_c_4_r_*SK_MY(3) - OP_l_16_c_18_r_*SK_MY(2) - OP_l_16_c_17_r_*SK_MY(4) + OP_l_16_c_19_r_*SK_MY(5));
Kfusion(17) = SK_MY(1)*(OP_l_17_c_21_r_ + OP_l_17_c_1_r_*SH_MAG(3) + OP_l_17_c_2_r_*SH_MAG(2) + OP_l_17_c_3_r_*SH_MAG(1) - OP_l_17_c_4_r_*SK_MY(3) - OP_l_17_c_18_r_*SK_MY(2) - OP_l_17_c_17_r_*SK_MY(4) + OP_l_17_c_19_r_*SK_MY(5));
Kfusion(18) = SK_MY(1)*(OP_l_18_c_21_r_ + OP_l_18_c_1_r_*SH_MAG(3) + OP_l_18_c_2_r_*SH_MAG(2) + OP_l_18_c_3_r_*SH_MAG(1) - OP_l_18_c_4_r_*SK_MY(3) - OP_l_18_c_18_r_*SK_MY(2) - OP_l_18_c_17_r_*SK_MY(4) + OP_l_18_c_19_r_*SK_MY(5));
Kfusion(19) = SK_MY(1)*(OP_l_19_c_21_r_ + OP_l_19_c_1_r_*SH_MAG(3) + OP_l_19_c_2_r_*SH_MAG(2) + OP_l_19_c_3_r_*SH_MAG(1) - OP_l_19_c_4_r_*SK_MY(3) - OP_l_19_c_18_r_*SK_MY(2) - OP_l_19_c_17_r_*SK_MY(4) + OP_l_19_c_19_r_*SK_MY(5));
Kfusion(20) = SK_MY(1)*(OP_l_20_c_21_r_ + OP_l_20_c_1_r_*SH_MAG(3) + OP_l_20_c_2_r_*SH_MAG(2) + OP_l_20_c_3_r_*SH_MAG(1) - OP_l_20_c_4_r_*SK_MY(3) - OP_l_20_c_18_r_*SK_MY(2) - OP_l_20_c_17_r_*SK_MY(4) + OP_l_20_c_19_r_*SK_MY(5));
Kfusion(21) = SK_MY(1)*(OP_l_21_c_21_r_ + OP_l_21_c_1_r_*SH_MAG(3) + OP_l_21_c_2_r_*SH_MAG(2) + OP_l_21_c_3_r_*SH_MAG(1) - OP_l_21_c_4_r_*SK_MY(3) - OP_l_21_c_18_r_*SK_MY(2) - OP_l_21_c_17_r_*SK_MY(4) + OP_l_21_c_19_r_*SK_MY(5));
Kfusion(22) = SK_MY(1)*(OP_l_22_c_21_r_ + OP_l_22_c_1_r_*SH_MAG(3) + OP_l_22_c_2_r_*SH_MAG(2) + OP_l_22_c_3_r_*SH_MAG(1) - OP_l_22_c_4_r_*SK_MY(3) - OP_l_22_c_18_r_*SK_MY(2) - OP_l_22_c_17_r_*SK_MY(4) + OP_l_22_c_19_r_*SK_MY(5));
Kfusion(23) = SK_MY(1)*(OP_l_23_c_21_r_ + OP_l_23_c_1_r_*SH_MAG(3) + OP_l_23_c_2_r_*SH_MAG(2) + OP_l_23_c_3_r_*SH_MAG(1) - OP_l_23_c_4_r_*SK_MY(3) - OP_l_23_c_18_r_*SK_MY(2) - OP_l_23_c_17_r_*SK_MY(4) + OP_l_23_c_19_r_*SK_MY(5));
Kfusion(24) = SK_MY(1)*(OP_l_24_c_21_r_ + OP_l_24_c_1_r_*SH_MAG(3) + OP_l_24_c_2_r_*SH_MAG(2) + OP_l_24_c_3_r_*SH_MAG(1) - OP_l_24_c_4_r_*SK_MY(3) - OP_l_24_c_18_r_*SK_MY(2) - OP_l_24_c_17_r_*SK_MY(4) + OP_l_24_c_19_r_*SK_MY(5));


H_MAG = zeros(1,24);
H_MAG(1) = SH_MAG(2);
H_MAG(2) = -SH_MAG(3);
H_MAG(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
H_MAG(4) = SH_MAG(1);
H_MAG(17) = 2*q0*q2 + 2*q1*q3;
H_MAG(18) = 2*q2*q3 - 2*q0*q1;
H_MAG(19) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7);
H_MAG(22) = 1;


SK_MZ = zeros(5,1);
SK_MZ(1) = 1/(OP_l_22_c_22_r_ + R_MAG + OP_l_1_c_22_r_*SH_MAG(2) - OP_l_2_c_22_r_*SH_MAG(3) + OP_l_4_c_22_r_*SH_MAG(1) + OP_l_19_c_22_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + (2*q0*q2 + 2*q1*q3)*(OP_l_22_c_17_r_ + OP_l_1_c_17_r_*SH_MAG(2) - OP_l_2_c_17_r_*SH_MAG(3) + OP_l_4_c_17_r_*SH_MAG(1) + OP_l_19_c_17_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_17_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_17_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(OP_l_22_c_18_r_ + OP_l_1_c_18_r_*SH_MAG(2) - OP_l_2_c_18_r_*SH_MAG(3) + OP_l_4_c_18_r_*SH_MAG(1) + OP_l_19_c_18_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_18_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_18_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_22_c_3_r_ + OP_l_1_c_3_r_*SH_MAG(2) - OP_l_2_c_3_r_*SH_MAG(3) + OP_l_4_c_3_r_*SH_MAG(1) + OP_l_19_c_3_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_3_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_3_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_17_c_22_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_22_r_*(2*q0*q1 - 2*q2*q3) + SH_MAG(2)*(OP_l_22_c_1_r_ + OP_l_1_c_1_r_*SH_MAG(2) - OP_l_2_c_1_r_*SH_MAG(3) + OP_l_4_c_1_r_*SH_MAG(1) + OP_l_19_c_1_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_1_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_1_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(3)*(OP_l_22_c_2_r_ + OP_l_1_c_2_r_*SH_MAG(2) - OP_l_2_c_2_r_*SH_MAG(3) + OP_l_4_c_2_r_*SH_MAG(1) + OP_l_19_c_2_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_2_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_2_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP_l_22_c_4_r_ + OP_l_1_c_4_r_*SH_MAG(2) - OP_l_2_c_4_r_*SH_MAG(3) + OP_l_4_c_4_r_*SH_MAG(1) + OP_l_19_c_4_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_4_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_4_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7))*(OP_l_22_c_19_r_ + OP_l_1_c_19_r_*SH_MAG(2) - OP_l_2_c_19_r_*SH_MAG(3) + OP_l_4_c_19_r_*SH_MAG(1) + OP_l_19_c_19_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_19_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_19_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_3_c_22_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2));
SK_MZ(2) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7);
SK_MZ(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
SK_MZ(4) = 2*q0*q1 - 2*q2*q3;
SK_MZ(5) = 2*q0*q2 + 2*q1*q3;


Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = SK_MZ(1)*(OP_l_1_c_22_r_ + OP_l_1_c_1_r_*SH_MAG(2) - OP_l_1_c_2_r_*SH_MAG(3) + OP_l_1_c_4_r_*SH_MAG(1) + OP_l_1_c_3_r_*SK_MZ(3) + OP_l_1_c_19_r_*SK_MZ(2) + OP_l_1_c_17_r_*SK_MZ(5) - OP_l_1_c_18_r_*SK_MZ(4));
Kfusion(2) = SK_MZ(1)*(OP_l_2_c_22_r_ + OP_l_2_c_1_r_*SH_MAG(2) - OP_l_2_c_2_r_*SH_MAG(3) + OP_l_2_c_4_r_*SH_MAG(1) + OP_l_2_c_3_r_*SK_MZ(3) + OP_l_2_c_19_r_*SK_MZ(2) + OP_l_2_c_17_r_*SK_MZ(5) - OP_l_2_c_18_r_*SK_MZ(4));
Kfusion(3) = SK_MZ(1)*(OP_l_3_c_22_r_ + OP_l_3_c_1_r_*SH_MAG(2) - OP_l_3_c_2_r_*SH_MAG(3) + OP_l_3_c_4_r_*SH_MAG(1) + OP_l_3_c_3_r_*SK_MZ(3) + OP_l_3_c_19_r_*SK_MZ(2) + OP_l_3_c_17_r_*SK_MZ(5) - OP_l_3_c_18_r_*SK_MZ(4));
Kfusion(4) = SK_MZ(1)*(OP_l_4_c_22_r_ + OP_l_4_c_1_r_*SH_MAG(2) - OP_l_4_c_2_r_*SH_MAG(3) + OP_l_4_c_4_r_*SH_MAG(1) + OP_l_4_c_3_r_*SK_MZ(3) + OP_l_4_c_19_r_*SK_MZ(2) + OP_l_4_c_17_r_*SK_MZ(5) - OP_l_4_c_18_r_*SK_MZ(4));
Kfusion(5) = SK_MZ(1)*(OP_l_5_c_22_r_ + OP_l_5_c_1_r_*SH_MAG(2) - OP_l_5_c_2_r_*SH_MAG(3) + OP_l_5_c_4_r_*SH_MAG(1) + OP_l_5_c_3_r_*SK_MZ(3) + OP_l_5_c_19_r_*SK_MZ(2) + OP_l_5_c_17_r_*SK_MZ(5) - OP_l_5_c_18_r_*SK_MZ(4));
Kfusion(6) = SK_MZ(1)*(OP_l_6_c_22_r_ + OP_l_6_c_1_r_*SH_MAG(2) - OP_l_6_c_2_r_*SH_MAG(3) + OP_l_6_c_4_r_*SH_MAG(1) + OP_l_6_c_3_r_*SK_MZ(3) + OP_l_6_c_19_r_*SK_MZ(2) + OP_l_6_c_17_r_*SK_MZ(5) - OP_l_6_c_18_r_*SK_MZ(4));
Kfusion(7) = SK_MZ(1)*(OP_l_7_c_22_r_ + OP_l_7_c_1_r_*SH_MAG(2) - OP_l_7_c_2_r_*SH_MAG(3) + OP_l_7_c_4_r_*SH_MAG(1) + OP_l_7_c_3_r_*SK_MZ(3) + OP_l_7_c_19_r_*SK_MZ(2) + OP_l_7_c_17_r_*SK_MZ(5) - OP_l_7_c_18_r_*SK_MZ(4));
Kfusion(8) = SK_MZ(1)*(OP_l_8_c_22_r_ + OP_l_8_c_1_r_*SH_MAG(2) - OP_l_8_c_2_r_*SH_MAG(3) + OP_l_8_c_4_r_*SH_MAG(1) + OP_l_8_c_3_r_*SK_MZ(3) + OP_l_8_c_19_r_*SK_MZ(2) + OP_l_8_c_17_r_*SK_MZ(5) - OP_l_8_c_18_r_*SK_MZ(4));
Kfusion(9) = SK_MZ(1)*(OP_l_9_c_22_r_ + OP_l_9_c_1_r_*SH_MAG(2) - OP_l_9_c_2_r_*SH_MAG(3) + OP_l_9_c_4_r_*SH_MAG(1) + OP_l_9_c_3_r_*SK_MZ(3) + OP_l_9_c_19_r_*SK_MZ(2) + OP_l_9_c_17_r_*SK_MZ(5) - OP_l_9_c_18_r_*SK_MZ(4));
Kfusion(10) = SK_MZ(1)*(OP_l_10_c_22_r_ + OP_l_10_c_1_r_*SH_MAG(2) - OP_l_10_c_2_r_*SH_MAG(3) + OP_l_10_c_4_r_*SH_MAG(1) + OP_l_10_c_3_r_*SK_MZ(3) + OP_l_10_c_19_r_*SK_MZ(2) + OP_l_10_c_17_r_*SK_MZ(5) - OP_l_10_c_18_r_*SK_MZ(4));
Kfusion(11) = SK_MZ(1)*(OP_l_11_c_22_r_ + OP_l_11_c_1_r_*SH_MAG(2) - OP_l_11_c_2_r_*SH_MAG(3) + OP_l_11_c_4_r_*SH_MAG(1) + OP_l_11_c_3_r_*SK_MZ(3) + OP_l_11_c_19_r_*SK_MZ(2) + OP_l_11_c_17_r_*SK_MZ(5) - OP_l_11_c_18_r_*SK_MZ(4));
Kfusion(12) = SK_MZ(1)*(OP_l_12_c_22_r_ + OP_l_12_c_1_r_*SH_MAG(2) - OP_l_12_c_2_r_*SH_MAG(3) + OP_l_12_c_4_r_*SH_MAG(1) + OP_l_12_c_3_r_*SK_MZ(3) + OP_l_12_c_19_r_*SK_MZ(2) + OP_l_12_c_17_r_*SK_MZ(5) - OP_l_12_c_18_r_*SK_MZ(4));
Kfusion(13) = SK_MZ(1)*(OP_l_13_c_22_r_ + OP_l_13_c_1_r_*SH_MAG(2) - OP_l_13_c_2_r_*SH_MAG(3) + OP_l_13_c_4_r_*SH_MAG(1) + OP_l_13_c_3_r_*SK_MZ(3) + OP_l_13_c_19_r_*SK_MZ(2) + OP_l_13_c_17_r_*SK_MZ(5) - OP_l_13_c_18_r_*SK_MZ(4));
Kfusion(14) = SK_MZ(1)*(OP_l_14_c_22_r_ + OP_l_14_c_1_r_*SH_MAG(2) - OP_l_14_c_2_r_*SH_MAG(3) + OP_l_14_c_4_r_*SH_MAG(1) + OP_l_14_c_3_r_*SK_MZ(3) + OP_l_14_c_19_r_*SK_MZ(2) + OP_l_14_c_17_r_*SK_MZ(5) - OP_l_14_c_18_r_*SK_MZ(4));
Kfusion(15) = SK_MZ(1)*(OP_l_15_c_22_r_ + OP_l_15_c_1_r_*SH_MAG(2) - OP_l_15_c_2_r_*SH_MAG(3) + OP_l_15_c_4_r_*SH_MAG(1) + OP_l_15_c_3_r_*SK_MZ(3) + OP_l_15_c_19_r_*SK_MZ(2) + OP_l_15_c_17_r_*SK_MZ(5) - OP_l_15_c_18_r_*SK_MZ(4));
Kfusion(16) = SK_MZ(1)*(OP_l_16_c_22_r_ + OP_l_16_c_1_r_*SH_MAG(2) - OP_l_16_c_2_r_*SH_MAG(3) + OP_l_16_c_4_r_*SH_MAG(1) + OP_l_16_c_3_r_*SK_MZ(3) + OP_l_16_c_19_r_*SK_MZ(2) + OP_l_16_c_17_r_*SK_MZ(5) - OP_l_16_c_18_r_*SK_MZ(4));
Kfusion(17) = SK_MZ(1)*(OP_l_17_c_22_r_ + OP_l_17_c_1_r_*SH_MAG(2) - OP_l_17_c_2_r_*SH_MAG(3) + OP_l_17_c_4_r_*SH_MAG(1) + OP_l_17_c_3_r_*SK_MZ(3) + OP_l_17_c_19_r_*SK_MZ(2) + OP_l_17_c_17_r_*SK_MZ(5) - OP_l_17_c_18_r_*SK_MZ(4));
Kfusion(18) = SK_MZ(1)*(OP_l_18_c_22_r_ + OP_l_18_c_1_r_*SH_MAG(2) - OP_l_18_c_2_r_*SH_MAG(3) + OP_l_18_c_4_r_*SH_MAG(1) + OP_l_18_c_3_r_*SK_MZ(3) + OP_l_18_c_19_r_*SK_MZ(2) + OP_l_18_c_17_r_*SK_MZ(5) - OP_l_18_c_18_r_*SK_MZ(4));
Kfusion(19) = SK_MZ(1)*(OP_l_19_c_22_r_ + OP_l_19_c_1_r_*SH_MAG(2) - OP_l_19_c_2_r_*SH_MAG(3) + OP_l_19_c_4_r_*SH_MAG(1) + OP_l_19_c_3_r_*SK_MZ(3) + OP_l_19_c_19_r_*SK_MZ(2) + OP_l_19_c_17_r_*SK_MZ(5) - OP_l_19_c_18_r_*SK_MZ(4));
Kfusion(20) = SK_MZ(1)*(OP_l_20_c_22_r_ + OP_l_20_c_1_r_*SH_MAG(2) - OP_l_20_c_2_r_*SH_MAG(3) + OP_l_20_c_4_r_*SH_MAG(1) + OP_l_20_c_3_r_*SK_MZ(3) + OP_l_20_c_19_r_*SK_MZ(2) + OP_l_20_c_17_r_*SK_MZ(5) - OP_l_20_c_18_r_*SK_MZ(4));
Kfusion(21) = SK_MZ(1)*(OP_l_21_c_22_r_ + OP_l_21_c_1_r_*SH_MAG(2) - OP_l_21_c_2_r_*SH_MAG(3) + OP_l_21_c_4_r_*SH_MAG(1) + OP_l_21_c_3_r_*SK_MZ(3) + OP_l_21_c_19_r_*SK_MZ(2) + OP_l_21_c_17_r_*SK_MZ(5) - OP_l_21_c_18_r_*SK_MZ(4));
Kfusion(22) = SK_MZ(1)*(OP_l_22_c_22_r_ + OP_l_22_c_1_r_*SH_MAG(2) - OP_l_22_c_2_r_*SH_MAG(3) + OP_l_22_c_4_r_*SH_MAG(1) + OP_l_22_c_3_r_*SK_MZ(3) + OP_l_22_c_19_r_*SK_MZ(2) + OP_l_22_c_17_r_*SK_MZ(5) - OP_l_22_c_18_r_*SK_MZ(4));
Kfusion(23) = SK_MZ(1)*(OP_l_23_c_22_r_ + OP_l_23_c_1_r_*SH_MAG(2) - OP_l_23_c_2_r_*SH_MAG(3) + OP_l_23_c_4_r_*SH_MAG(1) + OP_l_23_c_3_r_*SK_MZ(3) + OP_l_23_c_19_r_*SK_MZ(2) + OP_l_23_c_17_r_*SK_MZ(5) - OP_l_23_c_18_r_*SK_MZ(4));
Kfusion(24) = SK_MZ(1)*(OP_l_24_c_22_r_ + OP_l_24_c_1_r_*SH_MAG(2) - OP_l_24_c_2_r_*SH_MAG(3) + OP_l_24_c_4_r_*SH_MAG(1) + OP_l_24_c_3_r_*SK_MZ(3) + OP_l_24_c_19_r_*SK_MZ(2) + OP_l_24_c_17_r_*SK_MZ(5) - OP_l_24_c_18_r_*SK_MZ(4));


SH_ACCX = zeros(4,1);
SH_ACCX(1) = q0^2 + q1^2 - q2^2 - q3^2;
SH_ACCX(2) = vn - vwn;
SH_ACCX(3) = ve - vwe;
SH_ACCX(4) = 2*q0*q3 + 2*q1*q2;

H_ACCX = zeros(1,24);
H_ACCX(1,1) = -Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd);
H_ACCX(1,2) = -Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd);
H_ACCX(1,3) = Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd);
H_ACCX(1,4) = -Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd);
H_ACCX(1,5) = -Kaccx*SH_ACCX(1);
H_ACCX(1,6) = -Kaccx*SH_ACCX(4);
H_ACCX(1,7) = Kaccx*(2*q0*q2 - 2*q1*q3);
H_ACCX(1,23) = Kaccx*SH_ACCX(1);
H_ACCX(1,24) = Kaccx*SH_ACCX(4);


SK_ACCX = zeros(7,1);
SK_ACCX(1) = 1/(R_ACC + Kaccx*SH_ACCX(1)*(Kaccx*OP_l_5_c_5_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_5_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_5_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_5_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_5_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_5_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_5_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_5_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*SH_ACCX(4)*(Kaccx*OP_l_5_c_6_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_6_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_6_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_6_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_6_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_6_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_6_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_6_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_6_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(1)*(Kaccx*OP_l_5_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_23_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_23_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_23_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_23_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_23_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_23_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_23_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(4)*(Kaccx*OP_l_5_c_24_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_24_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_24_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_24_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_24_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_24_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_24_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_24_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_24_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*OP_l_5_c_7_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_7_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_7_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_7_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_7_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_7_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_7_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_7_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_7_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd)*(Kaccx*OP_l_5_c_1_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_1_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_1_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_1_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_1_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_1_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_1_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_1_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_1_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd)*(Kaccx*OP_l_5_c_2_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_2_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_2_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_2_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_2_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_2_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_2_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_2_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_2_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd)*(Kaccx*OP_l_5_c_3_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_3_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_3_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_3_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_3_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_3_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_3_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_3_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_3_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)*(Kaccx*OP_l_5_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_4_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_4_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_4_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_4_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_4_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_4_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_4_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_4_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)));
SK_ACCX(2) = 2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd;
SK_ACCX(3) = 2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd;
SK_ACCX(4) = 2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd;
SK_ACCX(5) = 2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd;
SK_ACCX(6) = 2*q0*q2 - 2*q1*q3;
SK_ACCX(7) = SH_ACCX(4);


Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = -SK_ACCX(1)*(Kaccx*OP_l_1_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_1_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_1_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_1_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_1_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_1_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_1_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_1_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_1_c_24_r_*SK_ACCX(7));
Kfusion(2) = -SK_ACCX(1)*(Kaccx*OP_l_2_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_2_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_2_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_2_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_2_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_2_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_2_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_2_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_2_c_24_r_*SK_ACCX(7));
Kfusion(3) = -SK_ACCX(1)*(Kaccx*OP_l_3_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_3_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_3_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_3_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_3_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_3_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_3_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_3_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_3_c_24_r_*SK_ACCX(7));
Kfusion(4) = -SK_ACCX(1)*(Kaccx*OP_l_4_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_4_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_4_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_4_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_4_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_4_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_4_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_4_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_4_c_24_r_*SK_ACCX(7));
Kfusion(5) = -SK_ACCX(1)*(Kaccx*OP_l_5_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_5_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_5_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_5_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_5_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_5_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_5_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_5_c_24_r_*SK_ACCX(7));
Kfusion(6) = -SK_ACCX(1)*(Kaccx*OP_l_6_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_6_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_6_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_6_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_6_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_6_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_6_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_6_c_24_r_*SK_ACCX(7));
Kfusion(7) = -SK_ACCX(1)*(Kaccx*OP_l_7_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_7_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_7_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_7_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_7_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_7_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_7_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_7_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_7_c_24_r_*SK_ACCX(7));
Kfusion(8) = -SK_ACCX(1)*(Kaccx*OP_l_8_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_8_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_8_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_8_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_8_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_8_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_8_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_8_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_8_c_24_r_*SK_ACCX(7));
Kfusion(9) = -SK_ACCX(1)*(Kaccx*OP_l_9_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_9_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_9_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_9_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_9_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_9_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_9_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_9_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_9_c_24_r_*SK_ACCX(7));
Kfusion(10) = -SK_ACCX(1)*(Kaccx*OP_l_10_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_10_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_10_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_10_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_10_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_10_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_10_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_10_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_10_c_24_r_*SK_ACCX(7));
Kfusion(11) = -SK_ACCX(1)*(Kaccx*OP_l_11_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_11_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_11_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_11_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_11_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_11_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_11_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_11_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_11_c_24_r_*SK_ACCX(7));
Kfusion(12) = -SK_ACCX(1)*(Kaccx*OP_l_12_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_12_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_12_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_12_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_12_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_12_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_12_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_12_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_12_c_24_r_*SK_ACCX(7));
Kfusion(13) = -SK_ACCX(1)*(Kaccx*OP_l_13_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_13_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_13_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_13_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_13_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_13_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_13_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_13_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_13_c_24_r_*SK_ACCX(7));
Kfusion(14) = -SK_ACCX(1)*(Kaccx*OP_l_14_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_14_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_14_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_14_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_14_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_14_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_14_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_14_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_14_c_24_r_*SK_ACCX(7));
Kfusion(15) = -SK_ACCX(1)*(Kaccx*OP_l_15_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_15_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_15_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_15_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_15_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_15_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_15_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_15_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_15_c_24_r_*SK_ACCX(7));
Kfusion(16) = -SK_ACCX(1)*(Kaccx*OP_l_16_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_16_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_16_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_16_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_16_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_16_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_16_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_16_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_16_c_24_r_*SK_ACCX(7));
Kfusion(17) = -SK_ACCX(1)*(Kaccx*OP_l_17_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_17_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_17_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_17_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_17_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_17_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_17_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_17_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_17_c_24_r_*SK_ACCX(7));
Kfusion(18) = -SK_ACCX(1)*(Kaccx*OP_l_18_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_18_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_18_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_18_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_18_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_18_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_18_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_18_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_18_c_24_r_*SK_ACCX(7));
Kfusion(19) = -SK_ACCX(1)*(Kaccx*OP_l_19_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_19_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_19_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_19_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_19_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_19_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_19_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_19_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_19_c_24_r_*SK_ACCX(7));
Kfusion(20) = -SK_ACCX(1)*(Kaccx*OP_l_20_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_20_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_20_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_20_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_20_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_20_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_20_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_20_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_20_c_24_r_*SK_ACCX(7));
Kfusion(21) = -SK_ACCX(1)*(Kaccx*OP_l_21_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_21_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_21_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_21_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_21_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_21_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_21_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_21_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_21_c_24_r_*SK_ACCX(7));
Kfusion(22) = -SK_ACCX(1)*(Kaccx*OP_l_22_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_22_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_22_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_22_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_22_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_22_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_22_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_22_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_22_c_24_r_*SK_ACCX(7));
Kfusion(23) = -SK_ACCX(1)*(Kaccx*OP_l_23_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_23_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_23_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_23_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_23_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_23_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_23_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_23_c_24_r_*SK_ACCX(7));
Kfusion(24) = -SK_ACCX(1)*(Kaccx*OP_l_24_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_24_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_24_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_24_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_24_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_24_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_24_c_24_r_*SK_ACCX(7));


SH_ACCY = zeros(3,1);
SH_ACCY(1) = q0^2 - q1^2 + q2^2 - q3^2;
SH_ACCY(2) = vn - vwn;
SH_ACCY(3) = ve - vwe;

H_ACCY = zeros(1,24);
H_ACCY(1,1) = -Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd);
H_ACCY(1,2) = -Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd);
H_ACCY(1,3) = -Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd);
H_ACCY(1,4) = Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd);
H_ACCY(1,5) = Kaccy*(2*q0*q3 - 2*q1*q2);
H_ACCY(1,6) = -Kaccy*SH_ACCY(1);
H_ACCY(1,7) = -Kaccy*(2*q0*q1 + 2*q2*q3);
H_ACCY(1,23) = -2*Kaccy*(q0*q3 - q1*q2);
H_ACCY(1,24) = Kaccy*SH_ACCY(1);


SK_ACCY = zeros(9,1);
SK_ACCY(1) = 1/(R_ACC + Kaccy*SH_ACCY(1)*(Kaccy*OP_l_6_c_6_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_6_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_6_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_6_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_6_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_6_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_6_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_6_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_6_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*SH_ACCY(1)*(Kaccy*OP_l_6_c_24_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_24_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_24_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_24_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_24_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_24_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_24_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_24_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_24_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*OP_l_6_c_5_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_5_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_5_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_5_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_5_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_5_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_5_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_5_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_5_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*OP_l_6_c_7_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_7_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_7_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_7_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_7_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_7_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_7_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_7_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_7_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP_l_6_c_23_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_23_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_23_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_23_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_23_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_23_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_23_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_23_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_23_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd)*(Kaccy*OP_l_6_c_1_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_1_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_1_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_1_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_1_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_1_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_1_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_1_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_1_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd)*(Kaccy*OP_l_6_c_2_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_2_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_2_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_2_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_2_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_2_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_2_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_2_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_2_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd)*(Kaccy*OP_l_6_c_3_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_3_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_3_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_3_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_3_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_3_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_3_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_3_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_3_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)*(Kaccy*OP_l_6_c_4_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_4_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_4_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_4_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_4_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_4_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_4_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_4_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_4_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)));
SK_ACCY(2) = 2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd;
SK_ACCY(3) = 2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd;
SK_ACCY(4) = 2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd;
SK_ACCY(5) = 2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd;
SK_ACCY(6) = 2*q0*q3 - 2*q1*q2;
SK_ACCY(7) = q0*q3 - q1*q2;
SK_ACCY(8) = 2*q0*q1 + 2*q2*q3;
SK_ACCY(9) = SH_ACCY(1);


Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
Kfusion(1) = -SK_ACCY(1)*(Kaccy*OP_l_1_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_1_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_1_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_1_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_1_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_1_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_1_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_1_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_1_c_24_r_*SK_ACCY(9));
Kfusion(2) = -SK_ACCY(1)*(Kaccy*OP_l_2_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_2_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_2_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_2_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_2_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_2_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_2_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_2_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_2_c_24_r_*SK_ACCY(9));
Kfusion(3) = -SK_ACCY(1)*(Kaccy*OP_l_3_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_3_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_3_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_3_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_3_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_3_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_3_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_3_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_3_c_24_r_*SK_ACCY(9));
Kfusion(4) = -SK_ACCY(1)*(Kaccy*OP_l_4_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_4_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_4_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_4_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_4_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_4_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_4_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_4_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_4_c_24_r_*SK_ACCY(9));
Kfusion(5) = -SK_ACCY(1)*(Kaccy*OP_l_5_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_5_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_5_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_5_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_5_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_5_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_5_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_5_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_5_c_24_r_*SK_ACCY(9));
Kfusion(6) = -SK_ACCY(1)*(Kaccy*OP_l_6_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_6_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_6_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_6_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_6_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_6_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_6_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_6_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_6_c_24_r_*SK_ACCY(9));
Kfusion(7) = -SK_ACCY(1)*(Kaccy*OP_l_7_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_7_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_7_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_7_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_7_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_7_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_7_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_7_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_7_c_24_r_*SK_ACCY(9));
Kfusion(8) = -SK_ACCY(1)*(Kaccy*OP_l_8_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_8_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_8_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_8_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_8_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_8_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_8_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_8_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_8_c_24_r_*SK_ACCY(9));
Kfusion(9) = -SK_ACCY(1)*(Kaccy*OP_l_9_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_9_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_9_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_9_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_9_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_9_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_9_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_9_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_9_c_24_r_*SK_ACCY(9));
Kfusion(10) = -SK_ACCY(1)*(Kaccy*OP_l_10_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_10_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_10_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_10_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_10_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_10_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_10_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_10_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_10_c_24_r_*SK_ACCY(9));
Kfusion(11) = -SK_ACCY(1)*(Kaccy*OP_l_11_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_11_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_11_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_11_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_11_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_11_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_11_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_11_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_11_c_24_r_*SK_ACCY(9));
Kfusion(12) = -SK_ACCY(1)*(Kaccy*OP_l_12_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_12_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_12_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_12_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_12_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_12_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_12_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_12_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_12_c_24_r_*SK_ACCY(9));
Kfusion(13) = -SK_ACCY(1)*(Kaccy*OP_l_13_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_13_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_13_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_13_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_13_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_13_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_13_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_13_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_13_c_24_r_*SK_ACCY(9));
Kfusion(14) = -SK_ACCY(1)*(Kaccy*OP_l_14_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_14_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_14_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_14_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_14_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_14_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_14_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_14_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_14_c_24_r_*SK_ACCY(9));
Kfusion(15) = -SK_ACCY(1)*(Kaccy*OP_l_15_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_15_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_15_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_15_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_15_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_15_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_15_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_15_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_15_c_24_r_*SK_ACCY(9));
Kfusion(16) = -SK_ACCY(1)*(Kaccy*OP_l_16_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_16_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_16_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_16_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_16_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_16_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_16_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_16_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_16_c_24_r_*SK_ACCY(9));
Kfusion(17) = -SK_ACCY(1)*(Kaccy*OP_l_17_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_17_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_17_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_17_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_17_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_17_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_17_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_17_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_17_c_24_r_*SK_ACCY(9));
Kfusion(18) = -SK_ACCY(1)*(Kaccy*OP_l_18_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_18_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_18_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_18_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_18_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_18_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_18_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_18_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_18_c_24_r_*SK_ACCY(9));
Kfusion(19) = -SK_ACCY(1)*(Kaccy*OP_l_19_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_19_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_19_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_19_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_19_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_19_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_19_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_19_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_19_c_24_r_*SK_ACCY(9));
Kfusion(20) = -SK_ACCY(1)*(Kaccy*OP_l_20_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_20_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_20_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_20_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_20_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_20_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_20_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_20_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_20_c_24_r_*SK_ACCY(9));
Kfusion(21) = -SK_ACCY(1)*(Kaccy*OP_l_21_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_21_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_21_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_21_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_21_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_21_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_21_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_21_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_21_c_24_r_*SK_ACCY(9));
Kfusion(22) = -SK_ACCY(1)*(Kaccy*OP_l_22_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_22_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_22_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_22_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_22_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_22_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_22_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_22_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_22_c_24_r_*SK_ACCY(9));
Kfusion(23) = -SK_ACCY(1)*(Kaccy*OP_l_23_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_23_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_23_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_23_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_23_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_23_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_23_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_23_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_23_c_24_r_*SK_ACCY(9));
Kfusion(24) = -SK_ACCY(1)*(Kaccy*OP_l_24_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_24_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_24_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_24_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_24_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_24_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_24_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_24_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_24_c_24_r_*SK_ACCY(9));