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

// Axis 0 equations
// Sub Expressions
const float HKX0 = magE*q3;
const float HKX1 = magD*q2;
const float HKX2 = magD*q3 + magE*q2;
const float HKX3 = magE*q1;
const float HKX4 = magD*q0;
const float HKX5 = 2*magN;
const float HKX6 = HKX5*q2;
const float HKX7 = -HKX5*q3 + magD*q1 + magE*q0;
const float HKX8 = 2*powf(q2, 2);
const float HKX9 = 2*powf(q3, 2);
const float HKX10 = q0*q3 + q1*q2;
const float HKX11 = q1*q3;
const float HKX12 = q0*q2;
const float HKX13 = 2*HKX2;
const float HKX14 = HKX13*P(0,1);
const float HKX15 = 2*HKX10;
const float HKX16 = HKX15*P(0,17);
const float HKX17 = -2*HKX0 + 2*HKX1;
const float HKX18 = HKX17*P(0,0);
const float HKX19 = -2*HKX11 + 2*HKX12;
const float HKX20 = HKX19*P(0,18);
const float HKX21 = HKX8 + HKX9 - 1;
const float HKX22 = HKX21*P(0,16);
const float HKX23 = 2*HKX7;
const float HKX24 = HKX23*P(0,3);
const float HKX25 = -2*HKX3 + 2*HKX4 + 2*HKX6;
const float HKX26 = HKX25*P(0,2);
const float HKX27 = HKX13*P(1,16);
const float HKX28 = HKX15*P(16,17);
const float HKX29 = HKX17*P(0,16);
const float HKX30 = HKX19*P(16,18);
const float HKX31 = HKX23*P(3,16);
const float HKX32 = HKX21*P(16,16);
const float HKX33 = HKX25*P(2,16);
const float HKX34 = HKX13*P(1,1);
const float HKX35 = HKX15*P(1,17);
const float HKX36 = HKX17*P(0,1);
const float HKX37 = HKX19*P(1,18);
const float HKX38 = HKX23*P(1,3);
const float HKX39 = HKX21*P(1,16);
const float HKX40 = HKX25*P(1,2);
const float HKX41 = HKX13*P(1,17);
const float HKX42 = HKX15*P(17,17);
const float HKX43 = HKX17*P(0,17);
const float HKX44 = HKX19*P(17,18);
const float HKX45 = HKX23*P(3,17);
const float HKX46 = HKX21*P(16,17);
const float HKX47 = HKX25*P(2,17);
const float HKX48 = HKX13*P(1,3);
const float HKX49 = HKX15*P(3,17);
const float HKX50 = HKX17*P(0,3);
const float HKX51 = HKX19*P(3,18);
const float HKX52 = HKX23*P(3,3);
const float HKX53 = HKX25*P(2,3);
const float HKX54 = HKX21*P(3,16);
const float HKX55 = HKX13*P(1,18);
const float HKX56 = HKX15*P(17,18);
const float HKX57 = HKX17*P(0,18);
const float HKX58 = HKX19*P(18,18);
const float HKX59 = HKX23*P(3,18);
const float HKX60 = HKX21*P(16,18);
const float HKX61 = HKX25*P(2,18);
const float HKX62 = HKX13*P(1,2);
const float HKX63 = HKX15*P(2,17);
const float HKX64 = HKX17*P(0,2);
const float HKX65 = HKX19*P(2,18);
const float HKX66 = HKX23*P(2,3);
const float HKX67 = HKX21*P(2,16);
const float HKX68 = HKX25*P(2,2);
const float HKX69 = -HKX13*P(1,19) - HKX15*P(17,19) + HKX17*P(0,19) + HKX19*P(18,19) + HKX21*P(16,19) - HKX23*P(3,19) + HKX25*P(2,19) - P(19,19);
const float HKX70 = 1.0F/(-HKX13*(HKX34 + HKX35 - HKX36 - HKX37 + HKX38 - HKX39 - HKX40 + P(1,19)) - HKX15*(HKX41 + HKX42 - HKX43 - HKX44 + HKX45 - HKX46 - HKX47 + P(17,19)) + HKX17*(HKX14 + HKX16 - HKX18 - HKX20 - HKX22 + HKX24 - HKX26 + P(0,19)) + HKX19*(HKX55 + HKX56 - HKX57 - HKX58 + HKX59 - HKX60 - HKX61 + P(18,19)) + HKX21*(HKX27 + HKX28 - HKX29 - HKX30 + HKX31 - HKX32 - HKX33 + P(16,19)) - HKX23*(HKX48 + HKX49 - HKX50 - HKX51 + HKX52 - HKX53 - HKX54 + P(3,19)) + HKX25*(HKX62 + HKX63 - HKX64 - HKX65 + HKX66 - HKX67 - HKX68 + P(2,19)) + HKX69 - R_MAG);


// Observation Jacobians
Hfusion.at<0>() = 2*HKX0 - 2*HKX1;
Hfusion.at<1>() = 2*HKX2;
Hfusion.at<2>() = 2*HKX3 - 2*HKX4 - 2*HKX6;
Hfusion.at<3>() = 2*HKX7;
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>() = -HKX8 - HKX9 + 1;
Hfusion.at<17>() = 2*HKX10;
Hfusion.at<18>() = 2*HKX11 - 2*HKX12;
Hfusion.at<19>() = 1;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;


// Kalman gains
Kfusion(0) = HKX70*(-HKX14 - HKX16 + HKX18 + HKX20 + HKX22 - HKX24 + HKX26 - P(0,19));
Kfusion(1) = HKX70*(-HKX34 - HKX35 + HKX36 + HKX37 - HKX38 + HKX39 + HKX40 - P(1,19));
Kfusion(2) = HKX70*(-HKX62 - HKX63 + HKX64 + HKX65 - HKX66 + HKX67 + HKX68 - P(2,19));
Kfusion(3) = HKX70*(-HKX48 - HKX49 + HKX50 + HKX51 - HKX52 + HKX53 + HKX54 - P(3,19));
Kfusion(4) = HKX70*(-HKX13*P(1,4) - HKX15*P(4,17) + HKX17*P(0,4) + HKX19*P(4,18) + HKX21*P(4,16) - HKX23*P(3,4) + HKX25*P(2,4) - P(4,19));
Kfusion(5) = HKX70*(-HKX13*P(1,5) - HKX15*P(5,17) + HKX17*P(0,5) + HKX19*P(5,18) + HKX21*P(5,16) - HKX23*P(3,5) + HKX25*P(2,5) - P(5,19));
Kfusion(6) = HKX70*(-HKX13*P(1,6) - HKX15*P(6,17) + HKX17*P(0,6) + HKX19*P(6,18) + HKX21*P(6,16) - HKX23*P(3,6) + HKX25*P(2,6) - P(6,19));
Kfusion(7) = HKX70*(-HKX13*P(1,7) - HKX15*P(7,17) + HKX17*P(0,7) + HKX19*P(7,18) + HKX21*P(7,16) - HKX23*P(3,7) + HKX25*P(2,7) - P(7,19));
Kfusion(8) = HKX70*(-HKX13*P(1,8) - HKX15*P(8,17) + HKX17*P(0,8) + HKX19*P(8,18) + HKX21*P(8,16) - HKX23*P(3,8) + HKX25*P(2,8) - P(8,19));
Kfusion(9) = HKX70*(-HKX13*P(1,9) - HKX15*P(9,17) + HKX17*P(0,9) + HKX19*P(9,18) + HKX21*P(9,16) - HKX23*P(3,9) + HKX25*P(2,9) - P(9,19));
Kfusion(10) = HKX70*(-HKX13*P(1,10) - HKX15*P(10,17) + HKX17*P(0,10) + HKX19*P(10,18) + HKX21*P(10,16) - HKX23*P(3,10) + HKX25*P(2,10) - P(10,19));
Kfusion(11) = HKX70*(-HKX13*P(1,11) - HKX15*P(11,17) + HKX17*P(0,11) + HKX19*P(11,18) + HKX21*P(11,16) - HKX23*P(3,11) + HKX25*P(2,11) - P(11,19));
Kfusion(12) = HKX70*(-HKX13*P(1,12) - HKX15*P(12,17) + HKX17*P(0,12) + HKX19*P(12,18) + HKX21*P(12,16) - HKX23*P(3,12) + HKX25*P(2,12) - P(12,19));
Kfusion(13) = HKX70*(-HKX13*P(1,13) - HKX15*P(13,17) + HKX17*P(0,13) + HKX19*P(13,18) + HKX21*P(13,16) - HKX23*P(3,13) + HKX25*P(2,13) - P(13,19));
Kfusion(14) = HKX70*(-HKX13*P(1,14) - HKX15*P(14,17) + HKX17*P(0,14) + HKX19*P(14,18) + HKX21*P(14,16) - HKX23*P(3,14) + HKX25*P(2,14) - P(14,19));
Kfusion(15) = HKX70*(-HKX13*P(1,15) - HKX15*P(15,17) + HKX17*P(0,15) + HKX19*P(15,18) + HKX21*P(15,16) - HKX23*P(3,15) + HKX25*P(2,15) - P(15,19));
Kfusion(16) = HKX70*(-HKX27 - HKX28 + HKX29 + HKX30 - HKX31 + HKX32 + HKX33 - P(16,19));
Kfusion(17) = HKX70*(-HKX41 - HKX42 + HKX43 + HKX44 - HKX45 + HKX46 + HKX47 - P(17,19));
Kfusion(18) = HKX70*(-HKX55 - HKX56 + HKX57 + HKX58 - HKX59 + HKX60 + HKX61 - P(18,19));
Kfusion(19) = HKX69*HKX70;
Kfusion(20) = HKX70*(-HKX13*P(1,20) - HKX15*P(17,20) + HKX17*P(0,20) + HKX19*P(18,20) + HKX21*P(16,20) - HKX23*P(3,20) + HKX25*P(2,20) - P(19,20));
Kfusion(21) = HKX70*(-HKX13*P(1,21) - HKX15*P(17,21) + HKX17*P(0,21) + HKX19*P(18,21) + HKX21*P(16,21) - HKX23*P(3,21) + HKX25*P(2,21) - P(19,21));
Kfusion(22) = HKX70*(-HKX13*P(1,22) - HKX15*P(17,22) + HKX17*P(0,22) + HKX19*P(18,22) + HKX21*P(16,22) - HKX23*P(3,22) + HKX25*P(2,22) - P(19,22));
Kfusion(23) = HKX70*(-HKX13*P(1,23) - HKX15*P(17,23) + HKX17*P(0,23) + HKX19*P(18,23) + HKX21*P(16,23) - HKX23*P(3,23) + HKX25*P(2,23) - P(19,23));


// Axis 1 equations
// Sub Expressions
const float HKY0 = magD*q1 - magN*q3;
const float HKY1 = 2*magE;
const float HKY2 = -HKY1*q1 + magD*q0 + magN*q2;
const float HKY3 = magD*q3 + magN*q1;
const float HKY4 = magD*q2;
const float HKY5 = HKY1*q3;
const float HKY6 = magN*q0;
const float HKY7 = q1*q2;
const float HKY8 = q0*q3;
const float HKY9 = 2*powf(q1, 2);
const float HKY10 = 2*powf(q3, 2);
const float HKY11 = q0*q1 + q2*q3;
const float HKY12 = 2*HKY11;
const float HKY13 = 2*HKY3;
const float HKY14 = 2*HKY0;
const float HKY15 = -2*HKY7 + 2*HKY8;
const float HKY16 = 2*HKY2;
const float HKY17 = HKY10 + HKY9 - 1;
const float HKY18 = -2*HKY4 + 2*HKY5 + 2*HKY6;
const float HKY19 = HKY12*P(0,18) + HKY13*P(0,2) + HKY14*P(0,0) - HKY15*P(0,16) + HKY16*P(0,1) - HKY17*P(0,17) - HKY18*P(0,3) + P(0,20);
const float HKY20 = HKY12*P(17,18) + HKY13*P(2,17) + HKY14*P(0,17) - HKY15*P(16,17) + HKY16*P(1,17) - HKY17*P(17,17) - HKY18*P(3,17) + P(17,20);
const float HKY21 = HKY12*P(16,18) + HKY13*P(2,16) + HKY14*P(0,16) - HKY15*P(16,16) + HKY16*P(1,16) - HKY17*P(16,17) - HKY18*P(3,16) + P(16,20);
const float HKY22 = HKY12*P(3,18) + HKY13*P(2,3) + HKY14*P(0,3) - HKY15*P(3,16) + HKY16*P(1,3) - HKY17*P(3,17) - HKY18*P(3,3) + P(3,20);
const float HKY23 = HKY12*P(2,18) + HKY13*P(2,2) + HKY14*P(0,2) - HKY15*P(2,16) + HKY16*P(1,2) - HKY17*P(2,17) - HKY18*P(2,3) + P(2,20);
const float HKY24 = HKY12*P(18,18) + HKY13*P(2,18) + HKY14*P(0,18) - HKY15*P(16,18) + HKY16*P(1,18) - HKY17*P(17,18) - HKY18*P(3,18) + P(18,20);
const float HKY25 = HKY12*P(1,18) + HKY13*P(1,2) + HKY14*P(0,1) - HKY15*P(1,16) + HKY16*P(1,1) - HKY17*P(1,17) - HKY18*P(1,3) + P(1,20);
const float HKY26 = HKY12*P(18,20) + HKY13*P(2,20) + HKY14*P(0,20) - HKY15*P(16,20) + HKY16*P(1,20) - HKY17*P(17,20) - HKY18*P(3,20) + P(20,20);
const float HKY27 = 1.0F/(HKY12*HKY24 + HKY13*HKY23 + HKY14*HKY19 - HKY15*HKY21 + HKY16*HKY25 - HKY17*HKY20 - HKY18*HKY22 + HKY26 + R_MAG);


// Observation Jacobians
Hfusion.at<0>() = 2*HKY0;
Hfusion.at<1>() = 2*HKY2;
Hfusion.at<2>() = 2*HKY3;
Hfusion.at<3>() = 2*HKY4 - 2*HKY5 - 2*HKY6;
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*HKY7 - 2*HKY8;
Hfusion.at<17>() = -HKY10 - HKY9 + 1;
Hfusion.at<18>() = 2*HKY11;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 1;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;


// Kalman gains
Kfusion(0) = HKY19*HKY27;
Kfusion(1) = HKY25*HKY27;
Kfusion(2) = HKY23*HKY27;
Kfusion(3) = HKY22*HKY27;
Kfusion(4) = HKY27*(HKY12*P(4,18) + HKY13*P(2,4) + HKY14*P(0,4) - HKY15*P(4,16) + HKY16*P(1,4) - HKY17*P(4,17) - HKY18*P(3,4) + P(4,20));
Kfusion(5) = HKY27*(HKY12*P(5,18) + HKY13*P(2,5) + HKY14*P(0,5) - HKY15*P(5,16) + HKY16*P(1,5) - HKY17*P(5,17) - HKY18*P(3,5) + P(5,20));
Kfusion(6) = HKY27*(HKY12*P(6,18) + HKY13*P(2,6) + HKY14*P(0,6) - HKY15*P(6,16) + HKY16*P(1,6) - HKY17*P(6,17) - HKY18*P(3,6) + P(6,20));
Kfusion(7) = HKY27*(HKY12*P(7,18) + HKY13*P(2,7) + HKY14*P(0,7) - HKY15*P(7,16) + HKY16*P(1,7) - HKY17*P(7,17) - HKY18*P(3,7) + P(7,20));
Kfusion(8) = HKY27*(HKY12*P(8,18) + HKY13*P(2,8) + HKY14*P(0,8) - HKY15*P(8,16) + HKY16*P(1,8) - HKY17*P(8,17) - HKY18*P(3,8) + P(8,20));
Kfusion(9) = HKY27*(HKY12*P(9,18) + HKY13*P(2,9) + HKY14*P(0,9) - HKY15*P(9,16) + HKY16*P(1,9) - HKY17*P(9,17) - HKY18*P(3,9) + P(9,20));
Kfusion(10) = HKY27*(HKY12*P(10,18) + HKY13*P(2,10) + HKY14*P(0,10) - HKY15*P(10,16) + HKY16*P(1,10) - HKY17*P(10,17) - HKY18*P(3,10) + P(10,20));
Kfusion(11) = HKY27*(HKY12*P(11,18) + HKY13*P(2,11) + HKY14*P(0,11) - HKY15*P(11,16) + HKY16*P(1,11) - HKY17*P(11,17) - HKY18*P(3,11) + P(11,20));
Kfusion(12) = HKY27*(HKY12*P(12,18) + HKY13*P(2,12) + HKY14*P(0,12) - HKY15*P(12,16) + HKY16*P(1,12) - HKY17*P(12,17) - HKY18*P(3,12) + P(12,20));
Kfusion(13) = HKY27*(HKY12*P(13,18) + HKY13*P(2,13) + HKY14*P(0,13) - HKY15*P(13,16) + HKY16*P(1,13) - HKY17*P(13,17) - HKY18*P(3,13) + P(13,20));
Kfusion(14) = HKY27*(HKY12*P(14,18) + HKY13*P(2,14) + HKY14*P(0,14) - HKY15*P(14,16) + HKY16*P(1,14) - HKY17*P(14,17) - HKY18*P(3,14) + P(14,20));
Kfusion(15) = HKY27*(HKY12*P(15,18) + HKY13*P(2,15) + HKY14*P(0,15) - HKY15*P(15,16) + HKY16*P(1,15) - HKY17*P(15,17) - HKY18*P(3,15) + P(15,20));
Kfusion(16) = HKY21*HKY27;
Kfusion(17) = HKY20*HKY27;
Kfusion(18) = HKY24*HKY27;
Kfusion(19) = HKY27*(HKY12*P(18,19) + HKY13*P(2,19) + HKY14*P(0,19) - HKY15*P(16,19) + HKY16*P(1,19) - HKY17*P(17,19) - HKY18*P(3,19) + P(19,20));
Kfusion(20) = HKY26*HKY27;
Kfusion(21) = HKY27*(HKY12*P(18,21) + HKY13*P(2,21) + HKY14*P(0,21) - HKY15*P(16,21) + HKY16*P(1,21) - HKY17*P(17,21) - HKY18*P(3,21) + P(20,21));
Kfusion(22) = HKY27*(HKY12*P(18,22) + HKY13*P(2,22) + HKY14*P(0,22) - HKY15*P(16,22) + HKY16*P(1,22) - HKY17*P(17,22) - HKY18*P(3,22) + P(20,22));
Kfusion(23) = HKY27*(HKY12*P(18,23) + HKY13*P(2,23) + HKY14*P(0,23) - HKY15*P(16,23) + HKY16*P(1,23) - HKY17*P(17,23) - HKY18*P(3,23) + P(20,23));


// Axis 2 equations
// Sub Expressions
const float HKZ0 = magN*q2;
const float HKZ1 = magE*q1;
const float HKZ2 = magN*q3;
const float HKZ3 = 2*magD;
const float HKZ4 = HKZ3*q1;
const float HKZ5 = magE*q0;
const float HKZ6 = -HKZ3*q2 + magE*q3 + magN*q0;
const float HKZ7 = magE*q2 + magN*q1;
const float HKZ8 = q0*q2 + q1*q3;
const float HKZ9 = q2*q3;
const float HKZ10 = q0*q1;
const float HKZ11 = 2*powf(q1, 2);
const float HKZ12 = 2*powf(q2, 2);
const float HKZ13 = 2*HKZ8;
const float HKZ14 = HKZ13*P(0,16);
const float HKZ15 = 2*HKZ7;
const float HKZ16 = HKZ15*P(0,3);
const float HKZ17 = -2*HKZ0 + 2*HKZ1;
const float HKZ18 = HKZ17*P(0,0);
const float HKZ19 = 2*HKZ10 - 2*HKZ9;
const float HKZ20 = HKZ19*P(0,17);
const float HKZ21 = HKZ11 + HKZ12 - 1;
const float HKZ22 = HKZ21*P(0,18);
const float HKZ23 = 2*HKZ6;
const float HKZ24 = HKZ23*P(0,2);
const float HKZ25 = -2*HKZ2 + 2*HKZ4 + 2*HKZ5;
const float HKZ26 = HKZ25*P(0,1);
const float HKZ27 = HKZ13*P(16,18);
const float HKZ28 = HKZ15*P(3,18);
const float HKZ29 = HKZ17*P(0,18);
const float HKZ30 = HKZ19*P(17,18);
const float HKZ31 = HKZ23*P(2,18);
const float HKZ32 = HKZ25*P(1,18);
const float HKZ33 = HKZ21*P(18,18);
const float HKZ34 = HKZ13*P(3,16);
const float HKZ35 = HKZ15*P(3,3);
const float HKZ36 = HKZ17*P(0,3);
const float HKZ37 = HKZ19*P(3,17);
const float HKZ38 = HKZ23*P(2,3);
const float HKZ39 = HKZ25*P(1,3);
const float HKZ40 = HKZ21*P(3,18);
const float HKZ41 = HKZ13*P(16,16);
const float HKZ42 = HKZ15*P(3,16);
const float HKZ43 = HKZ17*P(0,16);
const float HKZ44 = HKZ19*P(16,17);
const float HKZ45 = HKZ23*P(2,16);
const float HKZ46 = HKZ25*P(1,16);
const float HKZ47 = HKZ21*P(16,18);
const float HKZ48 = HKZ13*P(2,16);
const float HKZ49 = HKZ15*P(2,3);
const float HKZ50 = HKZ17*P(0,2);
const float HKZ51 = HKZ19*P(2,17);
const float HKZ52 = HKZ23*P(2,2);
const float HKZ53 = HKZ25*P(1,2);
const float HKZ54 = HKZ21*P(2,18);
const float HKZ55 = HKZ13*P(16,17);
const float HKZ56 = HKZ15*P(3,17);
const float HKZ57 = HKZ17*P(0,17);
const float HKZ58 = HKZ19*P(17,17);
const float HKZ59 = HKZ23*P(2,17);
const float HKZ60 = HKZ25*P(1,17);
const float HKZ61 = HKZ21*P(17,18);
const float HKZ62 = HKZ13*P(1,16);
const float HKZ63 = HKZ15*P(1,3);
const float HKZ64 = HKZ17*P(0,1);
const float HKZ65 = HKZ19*P(1,17);
const float HKZ66 = HKZ23*P(1,2);
const float HKZ67 = HKZ25*P(1,1);
const float HKZ68 = HKZ21*P(1,18);
const float HKZ69 = -HKZ13*P(16,21) - HKZ15*P(3,21) + HKZ17*P(0,21) + HKZ19*P(17,21) + HKZ21*P(18,21) - HKZ23*P(2,21) + HKZ25*P(1,21) - P(21,21);
const float HKZ70 = 1.0F/(-HKZ13*(HKZ41 + HKZ42 - HKZ43 - HKZ44 + HKZ45 - HKZ46 - HKZ47 + P(16,21)) - HKZ15*(HKZ34 + HKZ35 - HKZ36 - HKZ37 + HKZ38 - HKZ39 - HKZ40 + P(3,21)) + HKZ17*(HKZ14 + HKZ16 - HKZ18 - HKZ20 - HKZ22 + HKZ24 - HKZ26 + P(0,21)) + HKZ19*(HKZ55 + HKZ56 - HKZ57 - HKZ58 + HKZ59 - HKZ60 - HKZ61 + P(17,21)) + HKZ21*(HKZ27 + HKZ28 - HKZ29 - HKZ30 + HKZ31 - HKZ32 - HKZ33 + P(18,21)) - HKZ23*(HKZ48 + HKZ49 - HKZ50 - HKZ51 + HKZ52 - HKZ53 - HKZ54 + P(2,21)) + HKZ25*(HKZ62 + HKZ63 - HKZ64 - HKZ65 + HKZ66 - HKZ67 - HKZ68 + P(1,21)) + HKZ69 - R_MAG);


// Observation Jacobians
Hfusion.at<0>() = 2*HKZ0 - 2*HKZ1;
Hfusion.at<1>() = 2*HKZ2 - 2*HKZ4 - 2*HKZ5;
Hfusion.at<2>() = 2*HKZ6;
Hfusion.at<3>() = 2*HKZ7;
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*HKZ8;
Hfusion.at<17>() = -2*HKZ10 + 2*HKZ9;
Hfusion.at<18>() = -HKZ11 - HKZ12 + 1;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 1;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;


// Kalman gains
Kfusion(0) = HKZ70*(-HKZ14 - HKZ16 + HKZ18 + HKZ20 + HKZ22 - HKZ24 + HKZ26 - P(0,21));
Kfusion(1) = HKZ70*(-HKZ62 - HKZ63 + HKZ64 + HKZ65 - HKZ66 + HKZ67 + HKZ68 - P(1,21));
Kfusion(2) = HKZ70*(-HKZ48 - HKZ49 + HKZ50 + HKZ51 - HKZ52 + HKZ53 + HKZ54 - P(2,21));
Kfusion(3) = HKZ70*(-HKZ34 - HKZ35 + HKZ36 + HKZ37 - HKZ38 + HKZ39 + HKZ40 - P(3,21));
Kfusion(4) = HKZ70*(-HKZ13*P(4,16) - HKZ15*P(3,4) + HKZ17*P(0,4) + HKZ19*P(4,17) + HKZ21*P(4,18) - HKZ23*P(2,4) + HKZ25*P(1,4) - P(4,21));
Kfusion(5) = HKZ70*(-HKZ13*P(5,16) - HKZ15*P(3,5) + HKZ17*P(0,5) + HKZ19*P(5,17) + HKZ21*P(5,18) - HKZ23*P(2,5) + HKZ25*P(1,5) - P(5,21));
Kfusion(6) = HKZ70*(-HKZ13*P(6,16) - HKZ15*P(3,6) + HKZ17*P(0,6) + HKZ19*P(6,17) + HKZ21*P(6,18) - HKZ23*P(2,6) + HKZ25*P(1,6) - P(6,21));
Kfusion(7) = HKZ70*(-HKZ13*P(7,16) - HKZ15*P(3,7) + HKZ17*P(0,7) + HKZ19*P(7,17) + HKZ21*P(7,18) - HKZ23*P(2,7) + HKZ25*P(1,7) - P(7,21));
Kfusion(8) = HKZ70*(-HKZ13*P(8,16) - HKZ15*P(3,8) + HKZ17*P(0,8) + HKZ19*P(8,17) + HKZ21*P(8,18) - HKZ23*P(2,8) + HKZ25*P(1,8) - P(8,21));
Kfusion(9) = HKZ70*(-HKZ13*P(9,16) - HKZ15*P(3,9) + HKZ17*P(0,9) + HKZ19*P(9,17) + HKZ21*P(9,18) - HKZ23*P(2,9) + HKZ25*P(1,9) - P(9,21));
Kfusion(10) = HKZ70*(-HKZ13*P(10,16) - HKZ15*P(3,10) + HKZ17*P(0,10) + HKZ19*P(10,17) + HKZ21*P(10,18) - HKZ23*P(2,10) + HKZ25*P(1,10) - P(10,21));
Kfusion(11) = HKZ70*(-HKZ13*P(11,16) - HKZ15*P(3,11) + HKZ17*P(0,11) + HKZ19*P(11,17) + HKZ21*P(11,18) - HKZ23*P(2,11) + HKZ25*P(1,11) - P(11,21));
Kfusion(12) = HKZ70*(-HKZ13*P(12,16) - HKZ15*P(3,12) + HKZ17*P(0,12) + HKZ19*P(12,17) + HKZ21*P(12,18) - HKZ23*P(2,12) + HKZ25*P(1,12) - P(12,21));
Kfusion(13) = HKZ70*(-HKZ13*P(13,16) - HKZ15*P(3,13) + HKZ17*P(0,13) + HKZ19*P(13,17) + HKZ21*P(13,18) - HKZ23*P(2,13) + HKZ25*P(1,13) - P(13,21));
Kfusion(14) = HKZ70*(-HKZ13*P(14,16) - HKZ15*P(3,14) + HKZ17*P(0,14) + HKZ19*P(14,17) + HKZ21*P(14,18) - HKZ23*P(2,14) + HKZ25*P(1,14) - P(14,21));
Kfusion(15) = HKZ70*(-HKZ13*P(15,16) - HKZ15*P(3,15) + HKZ17*P(0,15) + HKZ19*P(15,17) + HKZ21*P(15,18) - HKZ23*P(2,15) + HKZ25*P(1,15) - P(15,21));
Kfusion(16) = HKZ70*(-HKZ41 - HKZ42 + HKZ43 + HKZ44 - HKZ45 + HKZ46 + HKZ47 - P(16,21));
Kfusion(17) = HKZ70*(-HKZ55 - HKZ56 + HKZ57 + HKZ58 - HKZ59 + HKZ60 + HKZ61 - P(17,21));
Kfusion(18) = HKZ70*(-HKZ27 - HKZ28 + HKZ29 + HKZ30 - HKZ31 + HKZ32 + HKZ33 - P(18,21));
Kfusion(19) = HKZ70*(-HKZ13*P(16,19) - HKZ15*P(3,19) + HKZ17*P(0,19) + HKZ19*P(17,19) + HKZ21*P(18,19) - HKZ23*P(2,19) + HKZ25*P(1,19) - P(19,21));
Kfusion(20) = HKZ70*(-HKZ13*P(16,20) - HKZ15*P(3,20) + HKZ17*P(0,20) + HKZ19*P(17,20) + HKZ21*P(18,20) - HKZ23*P(2,20) + HKZ25*P(1,20) - P(20,21));
Kfusion(21) = HKZ69*HKZ70;
Kfusion(22) = HKZ70*(-HKZ13*P(16,22) - HKZ15*P(3,22) + HKZ17*P(0,22) + HKZ19*P(17,22) + HKZ21*P(18,22) - HKZ23*P(2,22) + HKZ25*P(1,22) - P(21,22));
Kfusion(23) = HKZ70*(-HKZ13*P(16,23) - HKZ15*P(3,23) + HKZ17*P(0,23) + HKZ19*P(17,23) + HKZ21*P(18,23) - HKZ23*P(2,23) + HKZ25*P(1,23) - P(21,23));