// Sub Expressions
const float HK0 = magE*q3;
const float HK1 = magN*q0;
const float HK2 = magD*q2;
const float HK3 = HK0 + HK1 - HK2;
const float HK4 = 2*HK3;
const float HK5 = magD*q3 + magE*q2 + magN*q1;
const float HK6 = 2*HK5;
const float HK7 = magE*q1;
const float HK8 = magD*q0;
const float HK9 = magN*q2;
const float HK10 = magD*q1;
const float HK11 = magE*q0;
const float HK12 = magN*q3;
const float HK13 = HK10 + HK11 - HK12;
const float HK14 = 2*HK13;
const float HK15 = powf(q1, 2);
const float HK16 = powf(q2, 2);
const float HK17 = -HK16;
const float HK18 = powf(q0, 2);
const float HK19 = powf(q3, 2);
const float HK20 = HK18 - HK19;
const float HK21 = HK15 + HK17 + HK20;
const float HK22 = q0*q3;
const float HK23 = q1*q2;
const float HK24 = HK22 + HK23;
const float HK25 = q1*q3;
const float HK26 = q0*q2;
const float HK27 = 2*HK24;
const float HK28 = -2*HK25 + 2*HK26;
const float HK29 = 2*HK5;
const float HK30 = 2*HK3;
const float HK31 = -HK7 + HK8 + HK9;
const float HK32 = 2*HK31;
const float HK33 = HK32*P(0,2);
const float HK34 = 2*HK13;
const float HK35 = HK34*P(0,3);
const float HK36 = HK21*P(0,16) + HK27*P(0,17) - HK28*P(0,18) + HK29*P(0,1) + HK30*P(0,0) - HK33 + HK35 + P(0,19);
const float HK37 = HK21*P(16,16) + HK27*P(16,17) - HK28*P(16,18) + HK29*P(1,16) + HK30*P(0,16) - HK32*P(2,16) + HK34*P(3,16) + P(16,19);
const float HK38 = HK21*P(16,18) + HK27*P(17,18) - HK28*P(18,18) + HK29*P(1,18) + HK30*P(0,18) - HK32*P(2,18) + HK34*P(3,18) + P(18,19);
const float HK39 = HK29*P(1,2);
const float HK40 = HK30*P(0,2);
const float HK41 = HK21*P(2,16) + HK27*P(2,17) - HK28*P(2,18) - HK32*P(2,2) + HK34*P(2,3) + HK39 + HK40 + P(2,19);
const float HK42 = HK21*P(16,17) + HK27*P(17,17) - HK28*P(17,18) + HK29*P(1,17) + HK30*P(0,17) - HK32*P(2,17) + HK34*P(3,17) + P(17,19);
const float HK43 = HK29*P(1,3);
const float HK44 = HK30*P(0,3);
const float HK45 = HK21*P(3,16) + HK27*P(3,17) - HK28*P(3,18) - HK32*P(2,3) + HK34*P(3,3) + HK43 + HK44 + P(3,19);
const float HK46 = HK32*P(1,2);
const float HK47 = HK34*P(1,3);
const float HK48 = HK21*P(1,16) + HK27*P(1,17) - HK28*P(1,18) + HK29*P(1,1) + HK30*P(0,1) - HK46 + HK47 + P(1,19);
const float HK49 = HK21*P(16,19) + HK27*P(17,19) - HK28*P(18,19) + HK29*P(1,19) + HK30*P(0,19) - HK32*P(2,19) + HK34*P(3,19) + P(19,19);
const float HK50 = 1.0F/(HK21*HK37 + HK27*HK42 - HK28*HK38 + HK29*HK48 + HK30*HK36 - HK32*HK41 + HK34*HK45 + HK49 + R_MAG);
const float HK51 = 2*HK31;
const float HK52 = -HK15;
const float HK53 = HK16 + HK20 + HK52;
const float HK54 = q0*q1;
const float HK55 = q2*q3;
const float HK56 = HK54 + HK55;
const float HK57 = 2*HK56;
const float HK58 = 2*HK22 - 2*HK23;
const float HK59 = HK32*P(0,1);
const float HK60 = HK29*P(0,2) + HK34*P(0,0) - HK44 + HK53*P(0,17) + HK57*P(0,18) - HK58*P(0,16) + HK59 + P(0,20);
const float HK61 = HK29*P(2,17) - HK30*P(3,17) + HK32*P(1,17) + HK34*P(0,17) + HK53*P(17,17) + HK57*P(17,18) - HK58*P(16,17) + P(17,20);
const float HK62 = HK29*P(2,16) - HK30*P(3,16) + HK32*P(1,16) + HK34*P(0,16) + HK53*P(16,17) + HK57*P(16,18) - HK58*P(16,16) + P(16,20);
const float HK63 = HK29*P(2,3);
const float HK64 = -HK30*P(3,3) + HK32*P(1,3) + HK35 + HK53*P(3,17) + HK57*P(3,18) - HK58*P(3,16) + HK63 + P(3,20);
const float HK65 = HK29*P(2,18) - HK30*P(3,18) + HK32*P(1,18) + HK34*P(0,18) + HK53*P(17,18) + HK57*P(18,18) - HK58*P(16,18) + P(18,20);
const float HK66 = HK34*P(0,1);
const float HK67 = -HK30*P(1,3) + HK32*P(1,1) + HK39 + HK53*P(1,17) + HK57*P(1,18) - HK58*P(1,16) + HK66 + P(1,20);
const float HK68 = HK30*P(2,3);
const float HK69 = HK29*P(2,2) + HK34*P(0,2) + HK46 + HK53*P(2,17) + HK57*P(2,18) - HK58*P(2,16) - HK68 + P(2,20);
const float HK70 = HK29*P(2,20) - HK30*P(3,20) + HK32*P(1,20) + HK34*P(0,20) + HK53*P(17,20) + HK57*P(18,20) - HK58*P(16,20) + P(20,20);
const float HK71 = 1.0F/(HK29*HK69 - HK30*HK64 + HK32*HK67 + HK34*HK60 + HK53*HK61 + HK57*HK65 - HK58*HK62 + HK70 + R_MAG);
const float HK72 = HK25 + HK26;
const float HK73 = HK17 + HK18 + HK19 + HK52;
const float HK74 = 2*HK72;
const float HK75 = 2*HK54 - 2*HK55;
const float HK76 = HK29*P(0,3) + HK32*P(0,0) + HK40 - HK66 + HK73*P(0,18) + HK74*P(0,16) - HK75*P(0,17) + P(0,21);
const float HK77 = HK29*P(3,18) + HK30*P(2,18) + HK32*P(0,18) - HK34*P(1,18) + HK73*P(18,18) + HK74*P(16,18) - HK75*P(17,18) + P(18,21);
const float HK78 = HK29*P(3,17) + HK30*P(2,17) + HK32*P(0,17) - HK34*P(1,17) + HK73*P(17,18) + HK74*P(16,17) - HK75*P(17,17) + P(17,21);
const float HK79 = HK30*P(1,2) - HK34*P(1,1) + HK43 + HK59 + HK73*P(1,18) + HK74*P(1,16) - HK75*P(1,17) + P(1,21);
const float HK80 = HK29*P(3,16) + HK30*P(2,16) + HK32*P(0,16) - HK34*P(1,16) + HK73*P(16,18) + HK74*P(16,16) - HK75*P(16,17) + P(16,21);
const float HK81 = HK29*P(3,3) + HK32*P(0,3) - HK47 + HK68 + HK73*P(3,18) + HK74*P(3,16) - HK75*P(3,17) + P(3,21);
const float HK82 = HK30*P(2,2) + HK33 - HK34*P(1,2) + HK63 + HK73*P(2,18) + HK74*P(2,16) - HK75*P(2,17) + P(2,21);
const float HK83 = HK29*P(3,21) + HK30*P(2,21) + HK32*P(0,21) - HK34*P(1,21) + HK73*P(18,21) + HK74*P(16,21) - HK75*P(17,21) + P(21,21);
const float HK84 = 1.0F/(HK29*HK81 + HK30*HK82 + HK32*HK76 - HK34*HK79 + HK73*HK77 + HK74*HK80 - HK75*HK78 + HK83 + R_MAG);


// Observation Jacobians - axis 0
Hfusion.at<0>() = HK4;
Hfusion.at<1>() = HK6;
Hfusion.at<2>() = 2*HK7 - 2*HK8 - 2*HK9;
Hfusion.at<3>() = HK14;
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>() = HK21;
Hfusion.at<17>() = 2*HK24;
Hfusion.at<18>() = 2*HK25 - 2*HK26;
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) = HK36*HK50;
Kfusion(1) = HK48*HK50;
Kfusion(2) = HK41*HK50;
Kfusion(3) = HK45*HK50;
Kfusion(4) = HK50*(HK21*P(4,16) + HK27*P(4,17) - HK28*P(4,18) + HK29*P(1,4) + HK30*P(0,4) - HK32*P(2,4) + HK34*P(3,4) + P(4,19));
Kfusion(5) = HK50*(HK21*P(5,16) + HK27*P(5,17) - HK28*P(5,18) + HK29*P(1,5) + HK30*P(0,5) - HK32*P(2,5) + HK34*P(3,5) + P(5,19));
Kfusion(6) = HK50*(HK21*P(6,16) + HK27*P(6,17) - HK28*P(6,18) + HK29*P(1,6) + HK30*P(0,6) - HK32*P(2,6) + HK34*P(3,6) + P(6,19));
Kfusion(7) = HK50*(HK21*P(7,16) + HK27*P(7,17) - HK28*P(7,18) + HK29*P(1,7) + HK30*P(0,7) - HK32*P(2,7) + HK34*P(3,7) + P(7,19));
Kfusion(8) = HK50*(HK21*P(8,16) + HK27*P(8,17) - HK28*P(8,18) + HK29*P(1,8) + HK30*P(0,8) - HK32*P(2,8) + HK34*P(3,8) + P(8,19));
Kfusion(9) = HK50*(HK21*P(9,16) + HK27*P(9,17) - HK28*P(9,18) + HK29*P(1,9) + HK30*P(0,9) - HK32*P(2,9) + HK34*P(3,9) + P(9,19));
Kfusion(10) = HK50*(HK21*P(10,16) + HK27*P(10,17) - HK28*P(10,18) + HK29*P(1,10) + HK30*P(0,10) - HK32*P(2,10) + HK34*P(3,10) + P(10,19));
Kfusion(11) = HK50*(HK21*P(11,16) + HK27*P(11,17) - HK28*P(11,18) + HK29*P(1,11) + HK30*P(0,11) - HK32*P(2,11) + HK34*P(3,11) + P(11,19));
Kfusion(12) = HK50*(HK21*P(12,16) + HK27*P(12,17) - HK28*P(12,18) + HK29*P(1,12) + HK30*P(0,12) - HK32*P(2,12) + HK34*P(3,12) + P(12,19));
Kfusion(13) = HK50*(HK21*P(13,16) + HK27*P(13,17) - HK28*P(13,18) + HK29*P(1,13) + HK30*P(0,13) - HK32*P(2,13) + HK34*P(3,13) + P(13,19));
Kfusion(14) = HK50*(HK21*P(14,16) + HK27*P(14,17) - HK28*P(14,18) + HK29*P(1,14) + HK30*P(0,14) - HK32*P(2,14) + HK34*P(3,14) + P(14,19));
Kfusion(15) = HK50*(HK21*P(15,16) + HK27*P(15,17) - HK28*P(15,18) + HK29*P(1,15) + HK30*P(0,15) - HK32*P(2,15) + HK34*P(3,15) + P(15,19));
Kfusion(16) = HK37*HK50;
Kfusion(17) = HK42*HK50;
Kfusion(18) = HK38*HK50;
Kfusion(19) = HK49*HK50;
Kfusion(20) = HK50*(HK21*P(16,20) + HK27*P(17,20) - HK28*P(18,20) + HK29*P(1,20) + HK30*P(0,20) - HK32*P(2,20) + HK34*P(3,20) + P(19,20));
Kfusion(21) = HK50*(HK21*P(16,21) + HK27*P(17,21) - HK28*P(18,21) + HK29*P(1,21) + HK30*P(0,21) - HK32*P(2,21) + HK34*P(3,21) + P(19,21));
Kfusion(22) = HK50*(HK21*P(16,22) + HK27*P(17,22) - HK28*P(18,22) + HK29*P(1,22) + HK30*P(0,22) - HK32*P(2,22) + HK34*P(3,22) + P(19,22));
Kfusion(23) = HK50*(HK21*P(16,23) + HK27*P(17,23) - HK28*P(18,23) + HK29*P(1,23) + HK30*P(0,23) - HK32*P(2,23) + HK34*P(3,23) + P(19,23));


// Observation Jacobians - axis 1
Hfusion.at<0>() = HK14;
Hfusion.at<1>() = HK51;
Hfusion.at<2>() = HK6;
Hfusion.at<3>() = -2*HK0 - 2*HK1 + 2*HK2;
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*HK22 + 2*HK23;
Hfusion.at<17>() = HK53;
Hfusion.at<18>() = 2*HK56;
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) = HK60*HK71;
Kfusion(1) = HK67*HK71;
Kfusion(2) = HK69*HK71;
Kfusion(3) = HK64*HK71;
Kfusion(4) = HK71*(HK29*P(2,4) - HK30*P(3,4) + HK32*P(1,4) + HK34*P(0,4) + HK53*P(4,17) + HK57*P(4,18) - HK58*P(4,16) + P(4,20));
Kfusion(5) = HK71*(HK29*P(2,5) - HK30*P(3,5) + HK32*P(1,5) + HK34*P(0,5) + HK53*P(5,17) + HK57*P(5,18) - HK58*P(5,16) + P(5,20));
Kfusion(6) = HK71*(HK29*P(2,6) - HK30*P(3,6) + HK32*P(1,6) + HK34*P(0,6) + HK53*P(6,17) + HK57*P(6,18) - HK58*P(6,16) + P(6,20));
Kfusion(7) = HK71*(HK29*P(2,7) - HK30*P(3,7) + HK32*P(1,7) + HK34*P(0,7) + HK53*P(7,17) + HK57*P(7,18) - HK58*P(7,16) + P(7,20));
Kfusion(8) = HK71*(HK29*P(2,8) - HK30*P(3,8) + HK32*P(1,8) + HK34*P(0,8) + HK53*P(8,17) + HK57*P(8,18) - HK58*P(8,16) + P(8,20));
Kfusion(9) = HK71*(HK29*P(2,9) - HK30*P(3,9) + HK32*P(1,9) + HK34*P(0,9) + HK53*P(9,17) + HK57*P(9,18) - HK58*P(9,16) + P(9,20));
Kfusion(10) = HK71*(HK29*P(2,10) - HK30*P(3,10) + HK32*P(1,10) + HK34*P(0,10) + HK53*P(10,17) + HK57*P(10,18) - HK58*P(10,16) + P(10,20));
Kfusion(11) = HK71*(HK29*P(2,11) - HK30*P(3,11) + HK32*P(1,11) + HK34*P(0,11) + HK53*P(11,17) + HK57*P(11,18) - HK58*P(11,16) + P(11,20));
Kfusion(12) = HK71*(HK29*P(2,12) - HK30*P(3,12) + HK32*P(1,12) + HK34*P(0,12) + HK53*P(12,17) + HK57*P(12,18) - HK58*P(12,16) + P(12,20));
Kfusion(13) = HK71*(HK29*P(2,13) - HK30*P(3,13) + HK32*P(1,13) + HK34*P(0,13) + HK53*P(13,17) + HK57*P(13,18) - HK58*P(13,16) + P(13,20));
Kfusion(14) = HK71*(HK29*P(2,14) - HK30*P(3,14) + HK32*P(1,14) + HK34*P(0,14) + HK53*P(14,17) + HK57*P(14,18) - HK58*P(14,16) + P(14,20));
Kfusion(15) = HK71*(HK29*P(2,15) - HK30*P(3,15) + HK32*P(1,15) + HK34*P(0,15) + HK53*P(15,17) + HK57*P(15,18) - HK58*P(15,16) + P(15,20));
Kfusion(16) = HK62*HK71;
Kfusion(17) = HK61*HK71;
Kfusion(18) = HK65*HK71;
Kfusion(19) = HK71*(HK29*P(2,19) - HK30*P(3,19) + HK32*P(1,19) + HK34*P(0,19) + HK53*P(17,19) + HK57*P(18,19) - HK58*P(16,19) + P(19,20));
Kfusion(20) = HK70*HK71;
Kfusion(21) = HK71*(HK29*P(2,21) - HK30*P(3,21) + HK32*P(1,21) + HK34*P(0,21) + HK53*P(17,21) + HK57*P(18,21) - HK58*P(16,21) + P(20,21));
Kfusion(22) = HK71*(HK29*P(2,22) - HK30*P(3,22) + HK32*P(1,22) + HK34*P(0,22) + HK53*P(17,22) + HK57*P(18,22) - HK58*P(16,22) + P(20,22));
Kfusion(23) = HK71*(HK29*P(2,23) - HK30*P(3,23) + HK32*P(1,23) + HK34*P(0,23) + HK53*P(17,23) + HK57*P(18,23) - HK58*P(16,23) + P(20,23));


// Observation Jacobians - axis 2
Hfusion.at<0>() = HK51;
Hfusion.at<1>() = -2*HK10 - 2*HK11 + 2*HK12;
Hfusion.at<2>() = HK4;
Hfusion.at<3>() = HK6;
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*HK72;
Hfusion.at<17>() = -2*HK54 + 2*HK55;
Hfusion.at<18>() = HK73;
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) = HK76*HK84;
Kfusion(1) = HK79*HK84;
Kfusion(2) = HK82*HK84;
Kfusion(3) = HK81*HK84;
Kfusion(4) = HK84*(HK29*P(3,4) + HK30*P(2,4) + HK32*P(0,4) - HK34*P(1,4) + HK73*P(4,18) + HK74*P(4,16) - HK75*P(4,17) + P(4,21));
Kfusion(5) = HK84*(HK29*P(3,5) + HK30*P(2,5) + HK32*P(0,5) - HK34*P(1,5) + HK73*P(5,18) + HK74*P(5,16) - HK75*P(5,17) + P(5,21));
Kfusion(6) = HK84*(HK29*P(3,6) + HK30*P(2,6) + HK32*P(0,6) - HK34*P(1,6) + HK73*P(6,18) + HK74*P(6,16) - HK75*P(6,17) + P(6,21));
Kfusion(7) = HK84*(HK29*P(3,7) + HK30*P(2,7) + HK32*P(0,7) - HK34*P(1,7) + HK73*P(7,18) + HK74*P(7,16) - HK75*P(7,17) + P(7,21));
Kfusion(8) = HK84*(HK29*P(3,8) + HK30*P(2,8) + HK32*P(0,8) - HK34*P(1,8) + HK73*P(8,18) + HK74*P(8,16) - HK75*P(8,17) + P(8,21));
Kfusion(9) = HK84*(HK29*P(3,9) + HK30*P(2,9) + HK32*P(0,9) - HK34*P(1,9) + HK73*P(9,18) + HK74*P(9,16) - HK75*P(9,17) + P(9,21));
Kfusion(10) = HK84*(HK29*P(3,10) + HK30*P(2,10) + HK32*P(0,10) - HK34*P(1,10) + HK73*P(10,18) + HK74*P(10,16) - HK75*P(10,17) + P(10,21));
Kfusion(11) = HK84*(HK29*P(3,11) + HK30*P(2,11) + HK32*P(0,11) - HK34*P(1,11) + HK73*P(11,18) + HK74*P(11,16) - HK75*P(11,17) + P(11,21));
Kfusion(12) = HK84*(HK29*P(3,12) + HK30*P(2,12) + HK32*P(0,12) - HK34*P(1,12) + HK73*P(12,18) + HK74*P(12,16) - HK75*P(12,17) + P(12,21));
Kfusion(13) = HK84*(HK29*P(3,13) + HK30*P(2,13) + HK32*P(0,13) - HK34*P(1,13) + HK73*P(13,18) + HK74*P(13,16) - HK75*P(13,17) + P(13,21));
Kfusion(14) = HK84*(HK29*P(3,14) + HK30*P(2,14) + HK32*P(0,14) - HK34*P(1,14) + HK73*P(14,18) + HK74*P(14,16) - HK75*P(14,17) + P(14,21));
Kfusion(15) = HK84*(HK29*P(3,15) + HK30*P(2,15) + HK32*P(0,15) - HK34*P(1,15) + HK73*P(15,18) + HK74*P(15,16) - HK75*P(15,17) + P(15,21));
Kfusion(16) = HK80*HK84;
Kfusion(17) = HK78*HK84;
Kfusion(18) = HK77*HK84;
Kfusion(19) = HK84*(HK29*P(3,19) + HK30*P(2,19) + HK32*P(0,19) - HK34*P(1,19) + HK73*P(18,19) + HK74*P(16,19) - HK75*P(17,19) + P(19,21));
Kfusion(20) = HK84*(HK29*P(3,20) + HK30*P(2,20) + HK32*P(0,20) - HK34*P(1,20) + HK73*P(18,20) + HK74*P(16,20) - HK75*P(17,20) + P(20,21));
Kfusion(21) = HK83*HK84;
Kfusion(22) = HK84*(HK29*P(3,22) + HK30*P(2,22) + HK32*P(0,22) - HK34*P(1,22) + HK73*P(18,22) + HK74*P(16,22) - HK75*P(17,22) + P(21,22));
Kfusion(23) = HK84*(HK29*P(3,23) + HK30*P(2,23) + HK32*P(0,23) - HK34*P(1,23) + HK73*P(18,23) + HK74*P(16,23) - HK75*P(17,23) + P(21,23));