PX4-Autopilot/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp

// Sub Expressions
const float HK0 = magE*q3;
const float HK1 = magD*q2;
const float HK2 = -HK1;
const float HK3 = magD*q3;
const float HK4 = magE*q2;
const float HK5 = HK3 + HK4;
const float HK6 = magE*q1;
const float HK7 = magD*q0;
const float HK8 = magN*q2;
const float HK9 = 2*HK8;
const float HK10 = magD*q1;
const float HK11 = magE*q0;
const float HK12 = magN*q3;
const float HK13 = HK10 + HK11 - 2*HK12;
const float HK14 = 2*powf(q2, 2);
const float HK15 = -HK14;
const float HK16 = 2*powf(q3, 2);
const float HK17 = 1 - HK16;
const float HK18 = q0*q3;
const float HK19 = q1*q2;
const float HK20 = HK18 + HK19;
const float HK21 = q1*q3;
const float HK22 = q0*q2;
const float HK23 = 2*HK5;
const float HK24 = HK23*P(0,1);
const float HK25 = 2*HK20;
const float HK26 = HK25*P(0,17);
const float HK27 = -2*HK0 + 2*HK1;
const float HK28 = HK27*P(0,0);
const float HK29 = -2*HK21 + 2*HK22;
const float HK30 = HK29*P(0,18);
const float HK31 = HK16 - 1;
const float HK32 = HK14 + HK31;
const float HK33 = HK32*P(0,16);
const float HK34 = 2*HK13;
const float HK35 = HK34*P(0,3);
const float HK36 = -HK6;
const float HK37 = 2*HK36 + 2*HK7 + 2*HK9;
const float HK38 = HK37*P(0,2);
const float HK39 = -R_MAG;
const float HK40 = HK23*P(1,16);
const float HK41 = HK25*P(16,17);
const float HK42 = HK27*P(0,16);
const float HK43 = HK29*P(16,18);
const float HK44 = HK34*P(3,16);
const float HK45 = HK32*P(16,16);
const float HK46 = HK37*P(2,16);
const float HK47 = HK23*P(1,1);
const float HK48 = HK25*P(1,17);
const float HK49 = HK27*P(0,1);
const float HK50 = HK29*P(1,18);
const float HK51 = HK34*P(1,3);
const float HK52 = HK32*P(1,16);
const float HK53 = HK37*P(1,2);
const float HK54 = HK23*P(1,17);
const float HK55 = HK25*P(17,17);
const float HK56 = HK27*P(0,17);
const float HK57 = HK29*P(17,18);
const float HK58 = HK34*P(3,17);
const float HK59 = HK32*P(16,17);
const float HK60 = HK37*P(2,17);
const float HK61 = HK23*P(1,3);
const float HK62 = HK25*P(3,17);
const float HK63 = HK27*P(0,3);
const float HK64 = HK29*P(3,18);
const float HK65 = HK34*P(3,3);
const float HK66 = HK37*P(2,3);
const float HK67 = HK32*P(3,16);
const float HK68 = HK23*P(1,18);
const float HK69 = HK25*P(17,18);
const float HK70 = HK27*P(0,18);
const float HK71 = HK29*P(18,18);
const float HK72 = HK34*P(3,18);
const float HK73 = HK32*P(16,18);
const float HK74 = HK37*P(2,18);
const float HK75 = HK23*P(1,2);
const float HK76 = HK25*P(2,17);
const float HK77 = HK27*P(0,2);
const float HK78 = HK29*P(2,18);
const float HK79 = HK34*P(2,3);
const float HK80 = HK32*P(2,16);
const float HK81 = HK37*P(2,2);
const float HK82 = -HK23*P(1,19) - HK25*P(17,19) + HK27*P(0,19) + HK29*P(18,19) + HK32*P(16,19) - HK34*P(3,19) + HK37*P(2,19) - P(19,19);
const float HK83 = 1.0F/(-HK23*(HK47 + HK48 - HK49 - HK50 + HK51 - HK52 - HK53 + P(1,19)) - HK25*(HK54 + HK55 - HK56 - HK57 + HK58 - HK59 - HK60 + P(17,19)) + HK27*(HK24 + HK26 - HK28 - HK30 - HK33 + HK35 - HK38 + P(0,19)) + HK29*(HK68 + HK69 - HK70 - HK71 + HK72 - HK73 - HK74 + P(18,19)) + HK32*(HK40 + HK41 - HK42 - HK43 + HK44 - HK45 - HK46 + P(16,19)) - HK34*(HK61 + HK62 - HK63 - HK64 + HK65 - HK66 - HK67 + P(3,19)) + HK37*(HK75 + HK76 - HK77 - HK78 + HK79 - HK80 - HK81 + P(2,19)) + HK39 + HK82);
const float HK84 = -P(19,21);
const float HK85 = -HK12;
const float HK86 = HK10 + HK85;
const float HK87 = -2*HK6 + HK7 + HK8;
const float HK88 = magN*q1;
const float HK89 = HK3 + HK88;
const float HK90 = 2*HK0;
const float HK91 = magN*q0;
const float HK92 = 2*powf(q1, 2);
const float HK93 = -HK92;
const float HK94 = q0*q1;
const float HK95 = q2*q3;
const float HK96 = HK94 + HK95;
const float HK97 = 2*HK96;
const float HK98 = 2*HK89;
const float HK99 = 2*HK86;
const float HK100 = 2*HK18 - 2*HK19;
const float HK101 = 2*HK87;
const float HK102 = HK31 + HK92;
const float HK103 = 2*HK2 + 2*HK90 + 2*HK91;
const float HK104 = -HK100*P(0,16) + HK101*P(0,1) - HK102*P(0,17) - HK103*P(0,3) + HK97*P(0,18) + HK98*P(0,2) + HK99*P(0,0) + P(0,20);
const float HK105 = -HK100*P(16,17) + HK101*P(1,17) - HK102*P(17,17) - HK103*P(3,17) + HK97*P(17,18) + HK98*P(2,17) + HK99*P(0,17) + P(17,20);
const float HK106 = -HK100*P(16,16) + HK101*P(1,16) - HK102*P(16,17) - HK103*P(3,16) + HK97*P(16,18) + HK98*P(2,16) + HK99*P(0,16) + P(16,20);
const float HK107 = -HK100*P(3,16) + HK101*P(1,3) - HK102*P(3,17) - HK103*P(3,3) + HK97*P(3,18) + HK98*P(2,3) + HK99*P(0,3) + P(3,20);
const float HK108 = -HK100*P(2,16) + HK101*P(1,2) - HK102*P(2,17) - HK103*P(2,3) + HK97*P(2,18) + HK98*P(2,2) + HK99*P(0,2) + P(2,20);
const float HK109 = -HK100*P(16,18) + HK101*P(1,18) - HK102*P(17,18) - HK103*P(3,18) + HK97*P(18,18) + HK98*P(2,18) + HK99*P(0,18) + P(18,20);
const float HK110 = -HK100*P(1,16) + HK101*P(1,1) - HK102*P(1,17) - HK103*P(1,3) + HK97*P(1,18) + HK98*P(1,2) + HK99*P(0,1) + P(1,20);
const float HK111 = -HK100*P(16,20) + HK101*P(1,20) - HK102*P(17,20) - HK103*P(3,20) + HK97*P(18,20) + HK98*P(2,20) + HK99*P(0,20) + P(20,20);
const float HK112 = 1.0F/(-HK100*HK106 + HK101*HK110 - HK102*HK105 - HK103*HK107 + HK104*HK99 + HK108*HK98 + HK109*HK97 + HK111 + R_MAG);
const float HK113 = 2*HK10;
const float HK114 = HK0 - 2*HK1 + HK91;
const float HK115 = HK4 + HK88;
const float HK116 = HK21 + HK22;
const float HK117 = 2*HK116;
const float HK118 = HK117*P(0,16);
const float HK119 = 2*HK115;
const float HK120 = HK119*P(0,3);
const float HK121 = 2*HK6 - 2*HK8;
const float HK122 = HK121*P(0,0);
const float HK123 = 2*HK94 - 2*HK95;
const float HK124 = HK123*P(0,17);
const float HK125 = HK14 + HK92 - 1;
const float HK126 = HK125*P(0,18);
const float HK127 = 2*HK114;
const float HK128 = HK127*P(0,2);
const float HK129 = 2*HK11 + 2*HK113 + 2*HK85;
const float HK130 = HK129*P(0,1);
const float HK131 = HK117*P(16,18);
const float HK132 = HK119*P(3,18);
const float HK133 = HK121*P(0,18);
const float HK134 = HK123*P(17,18);
const float HK135 = HK127*P(2,18);
const float HK136 = HK129*P(1,18);
const float HK137 = HK125*P(18,18);
const float HK138 = HK117*P(3,16);
const float HK139 = HK119*P(3,3);
const float HK140 = HK121*P(0,3);
const float HK141 = HK123*P(3,17);
const float HK142 = HK127*P(2,3);
const float HK143 = HK129*P(1,3);
const float HK144 = HK125*P(3,18);
const float HK145 = HK117*P(16,16);
const float HK146 = HK119*P(3,16);
const float HK147 = HK121*P(0,16);
const float HK148 = HK123*P(16,17);
const float HK149 = HK127*P(2,16);
const float HK150 = HK129*P(1,16);
const float HK151 = HK125*P(16,18);
const float HK152 = HK117*P(2,16);
const float HK153 = HK119*P(2,3);
const float HK154 = HK121*P(0,2);
const float HK155 = HK123*P(2,17);
const float HK156 = HK127*P(2,2);
const float HK157 = HK129*P(1,2);
const float HK158 = HK125*P(2,18);
const float HK159 = HK117*P(16,17);
const float HK160 = HK119*P(3,17);
const float HK161 = HK121*P(0,17);
const float HK162 = HK123*P(17,17);
const float HK163 = HK127*P(2,17);
const float HK164 = HK129*P(1,17);
const float HK165 = HK125*P(17,18);
const float HK166 = HK117*P(1,16);
const float HK167 = HK119*P(1,3);
const float HK168 = HK121*P(0,1);
const float HK169 = HK123*P(1,17);
const float HK170 = HK127*P(1,2);
const float HK171 = HK129*P(1,1);
const float HK172 = HK125*P(1,18);
const float HK173 = -HK117*P(16,21) - HK119*P(3,21) + HK121*P(0,21) + HK123*P(17,21) + HK125*P(18,21) - HK127*P(2,21) + HK129*P(1,21) - P(21,21);
const float HK174 = 1.0F/(-HK117*(HK145 + HK146 - HK147 - HK148 + HK149 - HK150 - HK151 + P(16,21)) - HK119*(HK138 + HK139 - HK140 - HK141 + HK142 - HK143 - HK144 + P(3,21)) + HK121*(HK118 + HK120 - HK122 - HK124 - HK126 + HK128 - HK130 + P(0,21)) + HK123*(HK159 + HK160 - HK161 - HK162 + HK163 - HK164 - HK165 + P(17,21)) + HK125*(HK131 + HK132 - HK133 - HK134 + HK135 - HK136 - HK137 + P(18,21)) - HK127*(HK152 + HK153 - HK154 - HK155 + HK156 - HK157 - HK158 + P(2,21)) + HK129*(HK166 + HK167 - HK168 - HK169 + HK170 - HK171 - HK172 + P(1,21)) + HK173 + HK39);


// Observation Jacobians - axis 0
Hfusion.at<0>() = 2*HK0 + 2*HK2;
Hfusion.at<1>() = 2*HK5;
Hfusion.at<2>() = 2*HK6 - 2*HK7 - 2*HK9;
Hfusion.at<3>() = 2*HK13;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = HK15 + HK17;
Hfusion.at<17>() = 2*HK20;
Hfusion.at<18>() = 2*HK21 - 2*HK22;
Hfusion.at<19>() = 1;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;


// Kalman gains - axis 0
Kfusion(0) = HK83*(-HK24 - HK26 + HK28 + HK30 + HK33 - HK35 + HK38 - P(0,19));
Kfusion(1) = HK83*(-HK47 - HK48 + HK49 + HK50 - HK51 + HK52 + HK53 - P(1,19));
Kfusion(2) = HK83*(-HK75 - HK76 + HK77 + HK78 - HK79 + HK80 + HK81 - P(2,19));
Kfusion(3) = HK83*(-HK61 - HK62 + HK63 + HK64 - HK65 + HK66 + HK67 - P(3,19));
Kfusion(4) = HK83*(-HK23*P(1,4) - HK25*P(4,17) + HK27*P(0,4) + HK29*P(4,18) + HK32*P(4,16) - HK34*P(3,4) + HK37*P(2,4) - P(4,19));
Kfusion(5) = HK83*(-HK23*P(1,5) - HK25*P(5,17) + HK27*P(0,5) + HK29*P(5,18) + HK32*P(5,16) - HK34*P(3,5) + HK37*P(2,5) - P(5,19));
Kfusion(6) = HK83*(-HK23*P(1,6) - HK25*P(6,17) + HK27*P(0,6) + HK29*P(6,18) + HK32*P(6,16) - HK34*P(3,6) + HK37*P(2,6) - P(6,19));
Kfusion(7) = HK83*(-HK23*P(1,7) - HK25*P(7,17) + HK27*P(0,7) + HK29*P(7,18) + HK32*P(7,16) - HK34*P(3,7) + HK37*P(2,7) - P(7,19));
Kfusion(8) = HK83*(-HK23*P(1,8) - HK25*P(8,17) + HK27*P(0,8) + HK29*P(8,18) + HK32*P(8,16) - HK34*P(3,8) + HK37*P(2,8) - P(8,19));
Kfusion(9) = HK83*(-HK23*P(1,9) - HK25*P(9,17) + HK27*P(0,9) + HK29*P(9,18) + HK32*P(9,16) - HK34*P(3,9) + HK37*P(2,9) - P(9,19));
Kfusion(10) = HK83*(-HK23*P(1,10) - HK25*P(10,17) + HK27*P(0,10) + HK29*P(10,18) + HK32*P(10,16) - HK34*P(3,10) + HK37*P(2,10) - P(10,19));
Kfusion(11) = HK83*(-HK23*P(1,11) - HK25*P(11,17) + HK27*P(0,11) + HK29*P(11,18) + HK32*P(11,16) - HK34*P(3,11) + HK37*P(2,11) - P(11,19));
Kfusion(12) = HK83*(-HK23*P(1,12) - HK25*P(12,17) + HK27*P(0,12) + HK29*P(12,18) + HK32*P(12,16) - HK34*P(3,12) + HK37*P(2,12) - P(12,19));
Kfusion(13) = HK83*(-HK23*P(1,13) - HK25*P(13,17) + HK27*P(0,13) + HK29*P(13,18) + HK32*P(13,16) - HK34*P(3,13) + HK37*P(2,13) - P(13,19));
Kfusion(14) = HK83*(-HK23*P(1,14) - HK25*P(14,17) + HK27*P(0,14) + HK29*P(14,18) + HK32*P(14,16) - HK34*P(3,14) + HK37*P(2,14) - P(14,19));
Kfusion(15) = HK83*(-HK23*P(1,15) - HK25*P(15,17) + HK27*P(0,15) + HK29*P(15,18) + HK32*P(15,16) - HK34*P(3,15) + HK37*P(2,15) - P(15,19));
Kfusion(16) = HK83*(-HK40 - HK41 + HK42 + HK43 - HK44 + HK45 + HK46 - P(16,19));
Kfusion(17) = HK83*(-HK54 - HK55 + HK56 + HK57 - HK58 + HK59 + HK60 - P(17,19));
Kfusion(18) = HK83*(-HK68 - HK69 + HK70 + HK71 - HK72 + HK73 + HK74 - P(18,19));
Kfusion(19) = HK82*HK83;
Kfusion(20) = HK83*(-HK23*P(1,20) - HK25*P(17,20) + HK27*P(0,20) + HK29*P(18,20) + HK32*P(16,20) - HK34*P(3,20) + HK37*P(2,20) - P(19,20));
Kfusion(21) = HK83*(-HK23*P(1,21) - HK25*P(17,21) + HK27*P(0,21) + HK29*P(18,21) + HK32*P(16,21) - HK34*P(3,21) + HK37*P(2,21) + HK84);
Kfusion(22) = HK83*(-HK23*P(1,22) - HK25*P(17,22) + HK27*P(0,22) + HK29*P(18,22) + HK32*P(16,22) - HK34*P(3,22) + HK37*P(2,22) - P(19,22));
Kfusion(23) = HK83*(-HK23*P(1,23) - HK25*P(17,23) + HK27*P(0,23) + HK29*P(18,23) + HK32*P(16,23) - HK34*P(3,23) + HK37*P(2,23) - P(19,23));


// Observation Jacobians - axis 1
Hfusion.at<0>() = 2*HK86;
Hfusion.at<1>() = 2*HK87;
Hfusion.at<2>() = 2*HK89;
Hfusion.at<3>() = 2*HK1 - 2*HK90 - 2*HK91;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = -2*HK18 + 2*HK19;
Hfusion.at<17>() = HK17 + HK93;
Hfusion.at<18>() = 2*HK96;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 1;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;


// Kalman gains - axis 1
Kfusion(0) = HK104*HK112;
Kfusion(1) = HK110*HK112;
Kfusion(2) = HK108*HK112;
Kfusion(3) = HK107*HK112;
Kfusion(4) = HK112*(-HK100*P(4,16) + HK101*P(1,4) - HK102*P(4,17) - HK103*P(3,4) + HK97*P(4,18) + HK98*P(2,4) + HK99*P(0,4) + P(4,20));
Kfusion(5) = HK112*(-HK100*P(5,16) + HK101*P(1,5) - HK102*P(5,17) - HK103*P(3,5) + HK97*P(5,18) + HK98*P(2,5) + HK99*P(0,5) + P(5,20));
Kfusion(6) = HK112*(-HK100*P(6,16) + HK101*P(1,6) - HK102*P(6,17) - HK103*P(3,6) + HK97*P(6,18) + HK98*P(2,6) + HK99*P(0,6) + P(6,20));
Kfusion(7) = HK112*(-HK100*P(7,16) + HK101*P(1,7) - HK102*P(7,17) - HK103*P(3,7) + HK97*P(7,18) + HK98*P(2,7) + HK99*P(0,7) + P(7,20));
Kfusion(8) = HK112*(-HK100*P(8,16) + HK101*P(1,8) - HK102*P(8,17) - HK103*P(3,8) + HK97*P(8,18) + HK98*P(2,8) + HK99*P(0,8) + P(8,20));
Kfusion(9) = HK112*(-HK100*P(9,16) + HK101*P(1,9) - HK102*P(9,17) - HK103*P(3,9) + HK97*P(9,18) + HK98*P(2,9) + HK99*P(0,9) + P(9,20));
Kfusion(10) = HK112*(-HK100*P(10,16) + HK101*P(1,10) - HK102*P(10,17) - HK103*P(3,10) + HK97*P(10,18) + HK98*P(2,10) + HK99*P(0,10) + P(10,20));
Kfusion(11) = HK112*(-HK100*P(11,16) + HK101*P(1,11) - HK102*P(11,17) - HK103*P(3,11) + HK97*P(11,18) + HK98*P(2,11) + HK99*P(0,11) + P(11,20));
Kfusion(12) = HK112*(-HK100*P(12,16) + HK101*P(1,12) - HK102*P(12,17) - HK103*P(3,12) + HK97*P(12,18) + HK98*P(2,12) + HK99*P(0,12) + P(12,20));
Kfusion(13) = HK112*(-HK100*P(13,16) + HK101*P(1,13) - HK102*P(13,17) - HK103*P(3,13) + HK97*P(13,18) + HK98*P(2,13) + HK99*P(0,13) + P(13,20));
Kfusion(14) = HK112*(-HK100*P(14,16) + HK101*P(1,14) - HK102*P(14,17) - HK103*P(3,14) + HK97*P(14,18) + HK98*P(2,14) + HK99*P(0,14) + P(14,20));
Kfusion(15) = HK112*(-HK100*P(15,16) + HK101*P(1,15) - HK102*P(15,17) - HK103*P(3,15) + HK97*P(15,18) + HK98*P(2,15) + HK99*P(0,15) + P(15,20));
Kfusion(16) = HK106*HK112;
Kfusion(17) = HK105*HK112;
Kfusion(18) = HK109*HK112;
Kfusion(19) = HK112*(-HK100*P(16,19) + HK101*P(1,19) - HK102*P(17,19) - HK103*P(3,19) + HK97*P(18,19) + HK98*P(2,19) + HK99*P(0,19) + P(19,20));
Kfusion(20) = HK111*HK112;
Kfusion(21) = HK112*(-HK100*P(16,21) + HK101*P(1,21) - HK102*P(17,21) - HK103*P(3,21) + HK97*P(18,21) + HK98*P(2,21) + HK99*P(0,21) + P(20,21));
Kfusion(22) = HK112*(-HK100*P(16,22) + HK101*P(1,22) - HK102*P(17,22) - HK103*P(3,22) + HK97*P(18,22) + HK98*P(2,22) + HK99*P(0,22) + P(20,22));
Kfusion(23) = HK112*(-HK100*P(16,23) + HK101*P(1,23) - HK102*P(17,23) - HK103*P(3,23) + HK97*P(18,23) + HK98*P(2,23) + HK99*P(0,23) + P(20,23));


// Observation Jacobians - axis 2
Hfusion.at<0>() = 2*HK36 + 2*HK8;
Hfusion.at<1>() = -2*HK11 - 2*HK113 + 2*HK12;
Hfusion.at<2>() = 2*HK114;
Hfusion.at<3>() = 2*HK115;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 2*HK116;
Hfusion.at<17>() = -2*HK94 + 2*HK95;
Hfusion.at<18>() = HK15 + HK93 + 1;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 1;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;


// Kalman gains - axis 2
Kfusion(0) = HK174*(-HK118 - HK120 + HK122 + HK124 + HK126 - HK128 + HK130 - P(0,21));
Kfusion(1) = HK174*(-HK166 - HK167 + HK168 + HK169 - HK170 + HK171 + HK172 - P(1,21));
Kfusion(2) = HK174*(-HK152 - HK153 + HK154 + HK155 - HK156 + HK157 + HK158 - P(2,21));
Kfusion(3) = HK174*(-HK138 - HK139 + HK140 + HK141 - HK142 + HK143 + HK144 - P(3,21));
Kfusion(4) = HK174*(-HK117*P(4,16) - HK119*P(3,4) + HK121*P(0,4) + HK123*P(4,17) + HK125*P(4,18) - HK127*P(2,4) + HK129*P(1,4) - P(4,21));
Kfusion(5) = HK174*(-HK117*P(5,16) - HK119*P(3,5) + HK121*P(0,5) + HK123*P(5,17) + HK125*P(5,18) - HK127*P(2,5) + HK129*P(1,5) - P(5,21));
Kfusion(6) = HK174*(-HK117*P(6,16) - HK119*P(3,6) + HK121*P(0,6) + HK123*P(6,17) + HK125*P(6,18) - HK127*P(2,6) + HK129*P(1,6) - P(6,21));
Kfusion(7) = HK174*(-HK117*P(7,16) - HK119*P(3,7) + HK121*P(0,7) + HK123*P(7,17) + HK125*P(7,18) - HK127*P(2,7) + HK129*P(1,7) - P(7,21));
Kfusion(8) = HK174*(-HK117*P(8,16) - HK119*P(3,8) + HK121*P(0,8) + HK123*P(8,17) + HK125*P(8,18) - HK127*P(2,8) + HK129*P(1,8) - P(8,21));
Kfusion(9) = HK174*(-HK117*P(9,16) - HK119*P(3,9) + HK121*P(0,9) + HK123*P(9,17) + HK125*P(9,18) - HK127*P(2,9) + HK129*P(1,9) - P(9,21));
Kfusion(10) = HK174*(-HK117*P(10,16) - HK119*P(3,10) + HK121*P(0,10) + HK123*P(10,17) + HK125*P(10,18) - HK127*P(2,10) + HK129*P(1,10) - P(10,21));
Kfusion(11) = HK174*(-HK117*P(11,16) - HK119*P(3,11) + HK121*P(0,11) + HK123*P(11,17) + HK125*P(11,18) - HK127*P(2,11) + HK129*P(1,11) - P(11,21));
Kfusion(12) = HK174*(-HK117*P(12,16) - HK119*P(3,12) + HK121*P(0,12) + HK123*P(12,17) + HK125*P(12,18) - HK127*P(2,12) + HK129*P(1,12) - P(12,21));
Kfusion(13) = HK174*(-HK117*P(13,16) - HK119*P(3,13) + HK121*P(0,13) + HK123*P(13,17) + HK125*P(13,18) - HK127*P(2,13) + HK129*P(1,13) - P(13,21));
Kfusion(14) = HK174*(-HK117*P(14,16) - HK119*P(3,14) + HK121*P(0,14) + HK123*P(14,17) + HK125*P(14,18) - HK127*P(2,14) + HK129*P(1,14) - P(14,21));
Kfusion(15) = HK174*(-HK117*P(15,16) - HK119*P(3,15) + HK121*P(0,15) + HK123*P(15,17) + HK125*P(15,18) - HK127*P(2,15) + HK129*P(1,15) - P(15,21));
Kfusion(16) = HK174*(-HK145 - HK146 + HK147 + HK148 - HK149 + HK150 + HK151 - P(16,21));
Kfusion(17) = HK174*(-HK159 - HK160 + HK161 + HK162 - HK163 + HK164 + HK165 - P(17,21));
Kfusion(18) = HK174*(-HK131 - HK132 + HK133 + HK134 - HK135 + HK136 + HK137 - P(18,21));
Kfusion(19) = HK174*(-HK117*P(16,19) - HK119*P(3,19) + HK121*P(0,19) + HK123*P(17,19) + HK125*P(18,19) - HK127*P(2,19) + HK129*P(1,19) + HK84);
Kfusion(20) = HK174*(-HK117*P(16,20) - HK119*P(3,20) + HK121*P(0,20) + HK123*P(17,20) + HK125*P(18,20) - HK127*P(2,20) + HK129*P(1,20) - P(20,21));
Kfusion(21) = HK173*HK174;
Kfusion(22) = HK174*(-HK117*P(16,22) - HK119*P(3,22) + HK121*P(0,22) + HK123*P(17,22) + HK125*P(18,22) - HK127*P(2,22) + HK129*P(1,22) - P(21,22));
Kfusion(23) = HK174*(-HK117*P(16,23) - HK119*P(3,23) + HK121*P(0,23) + HK123*P(17,23) + HK125*P(18,23) - HK127*P(2,23) + HK129*P(1,23) - P(21,23));