fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 14:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Update derivation scripts and outputs

Browse Source 
Work around limitation in Symbolic toolbox environment space by incrementally saving and clearing workspace
Remove vehicle vertical position from optical flow prediction equations (use range measurement instead) and regenerate auto-code
Remove legacy optical flow auto-code conversion method as it is not required now a work around for symbolic toolbox limitations has been developed
Fix out of date syntax
This commit is contained in:
Paul Riseborough
2016-03-11 11:03:43 +11:00
parent ffebaf384f
commit e9ccfdd484
7 changed files with 489 additions and 490 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt
-281
View File
@@ -1,281 +0,0 @@
// Auto code for fusion of line of sight rate massurements from a optical flow camera aligned with the Z body axis
// Generated using MAtlab in-built symbolic toolbox to c-code function
// Observations are body modtion compensated optica flow rates about the X and Y body axis
// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated)
// intermediate variable from algebraic optimisation
float SH_LOS[14];
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
SH_LOS[3] = 1/(pd - ptd);
SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3);
SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3;
SH_LOS[7] = q0*q0;
SH_LOS[8] = q1*q1;
SH_LOS[9] = q2*q2;
SH_LOS[10] = q3*q3;
SH_LOS[11] = q0*q3*2.0f;
SH_LOS[12] = pd-ptd;
SH_LOS[13] = 1.0f/(SH_LOS[12]*SH_LOS[12]);
// Calculate the observation jacobians for the LOS rate about the X body axis
float H_LOS[24];
H_LOS[0] = SH_LOS[3]*SH_LOS[2]*SH_LOS[6]-SH_LOS[3]*SH_LOS[0]*SH_LOS[4];
H_LOS[1] = SH_LOS[3]*SH_LOS[2]*SH_LOS[5];
H_LOS[2] = SH_LOS[3]*SH_LOS[0]*SH_LOS[1];
H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]-q1*q2*2.0f);
H_LOS[4] = -SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]-SH_LOS[8]+SH_LOS[9]-SH_LOS[10]);
H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[6];
H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13];
// Calculate the observation jacobians for the LOS rate about the Y body axis
float H_LOS[24];
H_LOS[0] = -SH_LOS[3]*SH_LOS[6]*SH_LOS[1];
H_LOS[1] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[4]-SH_LOS[3]*SH_LOS[1]*SH_LOS[5];
H_LOS[2] = SH_LOS[3]*SH_LOS[2]*SH_LOS[0];
H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]+SH_LOS[8]-SH_LOS[9]-SH_LOS[10]);
H_LOS[4] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]+q1*q2*2.0f);
H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[5];
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13];
// Intermediate variables used to calculate the Kalman gain matrices for the LOS rate about the X body axis
float t2 = SH_LOS[3];
float t3 = SH_LOS[0];
float t4 = SH_LOS[2];
float t5 = SH_LOS[6];
float t100 = t2 * t3 * t5;
float t6 = SH_LOS[4];
float t7 = t2*t3*t6;
float t9 = t2*t4*t5;
float t8 = t7-t9;
float t10 = q0*q3*2.0f;
float t21 = q1*q2*2.0f;
float t11 = t10-t21;
float t101 = t2 * t3 * t11;
float t12 = pd-ptd;
float t13 = 1.0f/(t12*t12);
float t104 = t3 * t4 * t13;
float t14 = SH_LOS[5];
float t102 = t2 * t4 * t14;
float t15 = SH_LOS[1];
float t103 = t2 * t3 * t15;
float t16 = q0*q0;
float t17 = q1*q1;
float t18 = q2*q2;
float t19 = q3*q3;
float t20 = t16-t17+t18-t19;
float t105 = t2 * t3 * t20;
float t22 = P[1][1]*t102;
float t23 = P[3][0]*t101;
float t24 = P[8][0]*t104;
float t25 = P[1][0]*t102;
float t26 = P[2][0]*t103;
float t63 = P[0][0]*t8;
float t64 = P[5][0]*t100;
float t65 = P[4][0]*t105;
float t27 = t23+t24+t25+t26-t63-t64-t65;
float t28 = P[3][3]*t101;
float t29 = P[8][3]*t104;
float t30 = P[1][3]*t102;
float t31 = P[2][3]*t103;
float t67 = P[0][3]*t8;
float t68 = P[5][3]*t100;
float t69 = P[4][3]*t105;
float t32 = t28+t29+t30+t31-t67-t68-t69;
float t33 = t101*t32;
float t34 = P[3][8]*t101;
float t35 = P[8][8]*t104;
float t36 = P[1][8]*t102;
float t37 = P[2][8]*t103;
float t70 = P[0][8]*t8;
float t71 = P[5][8]*t100;
float t72 = P[4][8]*t105;
float t38 = t34+t35+t36+t37-t70-t71-t72;
float t39 = t104*t38;
float t40 = P[3][1]*t101;
float t41 = P[8][1]*t104;
float t42 = P[2][1]*t103;
float t73 = P[0][1]*t8;
float t74 = P[5][1]*t100;
float t75 = P[4][1]*t105;
float t43 = t22+t40+t41+t42-t73-t74-t75;
float t44 = t102*t43;
float t45 = P[3][2]*t101;
float t46 = P[8][2]*t104;
float t47 = P[1][2]*t102;
float t48 = P[2][2]*t103;
float t76 = P[0][2]*t8;
float t77 = P[5][2]*t100;
float t78 = P[4][2]*t105;
float t49 = t45+t46+t47+t48-t76-t77-t78;
float t50 = t103*t49;
float t51 = P[3][5]*t101;
float t52 = P[8][5]*t104;
float t53 = P[1][5]*t102;
float t54 = P[2][5]*t103;
float t79 = P[0][5]*t8;
float t80 = P[5][5]*t100;
float t81 = P[4][5]*t105;
float t55 = t51+t52+t53+t54-t79-t80-t81;
float t56 = P[3][4]*t101;
float t57 = P[8][4]*t104;
float t58 = P[1][4]*t102;
float t59 = P[2][4]*t103;
float t83 = P[0][4]*t8;
float t84 = P[5][4]*t100;
float t85 = P[4][4]*t105;
float t60 = t56+t57+t58+t59-t83-t84-t85;
float t66 = t8*t27;
float t82 = t100*t55;
float t86 = t105*t60;
float t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86; // innovation variance - should always be >= R_LOS
float t62 = 1.0f/t61;
// Calculate the Kalman gain matrix for the LOS rate about the X body axis
float Kfusion[24];
Kfusion[0] = t62*(-P[0][0]*t8-P[0][5]*t100+P[0][3]*t101+P[0][1]*t102+P[0][2]*t103+P[0][8]*t104-P[0][4]*t105);
Kfusion[1] = t62*(t22-P[1][0]*t8-P[1][5]*t100+P[1][3]*t101+P[1][2]*t103+P[1][8]*t104-P[1][4]*t105);
Kfusion[2] = t62*(t48-P[2][0]*t8-P[2][5]*t100+P[2][3]*t101+P[2][1]*t102+P[2][8]*t104-P[2][4]*t105);
Kfusion[3] = t62*(t28-P[3][0]*t8-P[3][5]*t100+P[3][1]*t102+P[3][2]*t103+P[3][8]*t104-P[3][4]*t105);
Kfusion[4] = t62*(-t85-P[4][0]*t8-P[4][5]*t100+P[4][3]*t101+P[4][1]*t102+P[4][2]*t103+P[4][8]*t104);
Kfusion[5] = t62*(-t80-P[5][0]*t8+P[5][3]*t101+P[5][1]*t102+P[5][2]*t103+P[5][8]*t104-P[5][4]*t105);
Kfusion[6] = t62*(-P[6][0]*t8-P[6][5]*t100+P[6][3]*t101+P[6][1]*t102+P[6][2]*t103+P[6][8]*t104-P[6][4]*t105);
Kfusion[7] = t62*(-P[7][0]*t8-P[7][5]*t100+P[7][3]*t101+P[7][1]*t102+P[7][2]*t103+P[7][8]*t104-P[7][4]*t105);
Kfusion[8] = t62*(t35-P[8][0]*t8-P[8][5]*t100+P[8][3]*t101+P[8][1]*t102+P[8][2]*t103-P[8][4]*t105);
Kfusion[9] = t62*(-P[9][0]*t8-P[9][5]*t100+P[9][3]*t101+P[9][1]*t102+P[9][2]*t103+P[9][8]*t104-P[9][4]*t105);
Kfusion[10] = t62*(-P[10][0]*t8-P[10][5]*t100+P[10][3]*t101+P[10][1]*t102+P[10][2]*t103+P[10][8]*t104-P[10][4]*t105);
Kfusion[11] = t62*(-P[11][0]*t8-P[11][5]*t100+P[11][3]*t101+P[11][1]*t102+P[11][2]*t103+P[11][8]*t104-P[11][4]*t105);
Kfusion[12] = t62*(-P[12][0]*t8-P[12][5]*t100+P[12][3]*t101+P[12][1]*t102+P[12][2]*t103+P[12][8]*t104-P[12][4]*t105);
Kfusion[13] = t62*(-P[13][0]*t8-P[13][5]*t100+P[13][3]*t101+P[13][1]*t102+P[13][2]*t103+P[13][8]*t104-P[13][4]*t105);
Kfusion[14] = t62*(-P[14][0]*t8-P[14][5]*t100+P[14][3]*t101+P[14][1]*t102+P[14][2]*t103+P[14][8]*t104-P[14][4]*t105);
Kfusion[15] = t62*(-P[15][0]*t8-P[15][5]*t100+P[15][3]*t101+P[15][1]*t102+P[15][2]*t103+P[15][8]*t104-P[15][4]*t105);
Kfusion[16] = t62*(-P[16][0]*t8-P[16][5]*t100+P[16][3]*t101+P[16][1]*t102+P[16][2]*t103+P[16][8]*t104-P[16][4]*t105);
Kfusion[17] = t62*(-P[17][0]*t8-P[17][5]*t100+P[17][3]*t101+P[17][1]*t102+P[17][2]*t103+P[17][8]*t104-P[17][4]*t105);
Kfusion[18] = t62*(-P[18][0]*t8-P[18][5]*t100+P[18][3]*t101+P[18][1]*t102+P[18][2]*t103+P[18][8]*t104-P[18][4]*t105);
Kfusion[19] = t62*(-P[19][0]*t8-P[19][5]*t100+P[19][3]*t101+P[19][1]*t102+P[19][2]*t103+P[19][8]*t104-P[19][4]*t105);
Kfusion[20] = t62*(-P[20][0]*t8-P[20][5]*t100+P[20][3]*t101+P[20][1]*t102+P[20][2]*t103+P[20][8]*t104-P[20][4]*t105);
Kfusion[21] = t62*(-P[21][0]*t8-P[21][5]*t100+P[21][3]*t101+P[21][1]*t102+P[21][2]*t103+P[21][8]*t104-P[21][4]*t105);
Kfusion[22] = t62*(-P[22][0]*t8-P[22][5]*t100+P[22][3]*t101+P[22][1]*t102+P[22][2]*t103+P[22][8]*t104-P[22][4]*t105);
Kfusion[23] = t62*(-P[23][0]*t8-P[23][5]*t100+P[23][3]*t101+P[23][1]*t102+P[23][2]*t103+P[23][8]*t104-P[23][4]*t105);
// Intermediate variables used to calculate the Kalman gain matrices for the LOS rate about the Y body axis
float t2 = SH_LOS[3];
float t3 = SH_LOS[0];
float t4 = SH_LOS[1];
float t5 = SH_LOS[5];
float t100 = t2 * t3 * t5;
float t6 = SH_LOS[4];
float t7 = t2*t3*t6;
float t8 = t2*t4*t5;
float t9 = t7+t8;
float t10 = q0*q3*2.0f;
float t11 = q1*q2*2.0f;
float t12 = t10+t11;
float t101 = t2 * t3 * t12;
float t13 = pd-ptd;
float t14 = 1.0f/(t13*t13);
float t104 = t3 * t4 * t14;
float t15 = SH_LOS[6];
float t105 = t2 * t4 * t15;
float t16 = SH_LOS[2];
float t102 = t2 * t3 * t16;
float t17 = q0*q0;
float t18 = q1*q1;
float t19 = q2*q2;
float t20 = q3*q3;
float t21 = t17+t18-t19-t20;
float t103 = t2 * t3 * t21;
float t22 = P[0][0]*t105;
float t23 = P[1][1]*t9;
float t24 = P[8][1]*t104;
float t25 = P[0][1]*t105;
float t26 = P[5][1]*t100;
float t64 = P[4][1]*t101;
float t65 = P[2][1]*t102;
float t66 = P[3][1]*t103;
float t27 = t23+t24+t25+t26-t64-t65-t66;
float t28 = t9*t27;
float t29 = P[1][4]*t9;
float t30 = P[8][4]*t104;
float t31 = P[0][4]*t105;
float t32 = P[5][4]*t100;
float t67 = P[4][4]*t101;
float t68 = P[2][4]*t102;
float t69 = P[3][4]*t103;
float t33 = t29+t30+t31+t32-t67-t68-t69;
float t34 = P[1][8]*t9;
float t35 = P[8][8]*t104;
float t36 = P[0][8]*t105;
float t37 = P[5][8]*t100;
float t71 = P[4][8]*t101;
float t72 = P[2][8]*t102;
float t73 = P[3][8]*t103;
float t38 = t34+t35+t36+t37-t71-t72-t73;
float t39 = t104*t38;
float t40 = P[1][0]*t9;
float t41 = P[8][0]*t104;
float t42 = P[5][0]*t100;
float t74 = P[4][0]*t101;
float t75 = P[2][0]*t102;
float t76 = P[3][0]*t103;
float t43 = t22+t40+t41+t42-t74-t75-t76;
float t44 = t105*t43;
float t45 = P[1][2]*t9;
float t46 = P[8][2]*t104;
float t47 = P[0][2]*t105;
float t48 = P[5][2]*t100;
float t63 = P[2][2]*t102;
float t77 = P[4][2]*t101;
float t78 = P[3][2]*t103;
float t49 = t45+t46+t47+t48-t63-t77-t78;
float t50 = P[1][5]*t9;
float t51 = P[8][5]*t104;
float t52 = P[0][5]*t105;
float t53 = P[5][5]*t100;
float t80 = P[4][5]*t101;
float t81 = P[2][5]*t102;
float t82 = P[3][5]*t103;
float t54 = t50+t51+t52+t53-t80-t81-t82;
float t55 = t100*t54;
float t56 = P[1][3]*t9;
float t57 = P[8][3]*t104;
float t58 = P[0][3]*t105;
float t59 = P[5][3]*t100;
float t83 = P[4][3]*t101;
float t84 = P[2][3]*t102;
float t85 = P[3][3]*t103;
float t60 = t56+t57+t58+t59-t83-t84-t85;
float t70 = t101*t33;
float t79 = t102*t49;
float t86 = t103*t60;
float t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86; // innovation variance - should always be >= R_LOS
float t62 = 1.0f/t61;
// Calculate the Kalman gain matrix for the LOS rate about the Y body axis
float Kfusion[24];
Kfusion[0] = -t62*(t22+P[0][1]*t9+P[0][5]*t100-P[0][4]*t101-P[0][2]*t102-P[0][3]*t103+P[0][8]*t104);
Kfusion[1] = -t62*(t23+P[1][5]*t100+P[1][0]*t105-P[1][4]*t101-P[1][2]*t102-P[1][3]*t103+P[1][8]*t104);
Kfusion[2] = -t62*(-t63+P[2][1]*t9+P[2][5]*t100+P[2][0]*t105-P[2][4]*t101-P[2][3]*t103+P[2][8]*t104);
Kfusion[3] = -t62*(-t85+P[3][1]*t9+P[3][5]*t100+P[3][0]*t105-P[3][4]*t101-P[3][2]*t102+P[3][8]*t104);
Kfusion[4] = -t62*(-t67+P[4][1]*t9+P[4][5]*t100+P[4][0]*t105-P[4][2]*t102-P[4][3]*t103+P[4][8]*t104);
Kfusion[5] = -t62*(t53+P[5][1]*t9+P[5][0]*t105-P[5][4]*t101-P[5][2]*t102-P[5][3]*t103+P[5][8]*t104);
Kfusion[6] = -t62*(P[6][1]*t9+P[6][5]*t100+P[6][0]*t105-P[6][4]*t101-P[6][2]*t102-P[6][3]*t103+P[6][8]*t104);
Kfusion[7] = -t62*(P[7][1]*t9+P[7][5]*t100+P[7][0]*t105-P[7][4]*t101-P[7][2]*t102-P[7][3]*t103+P[7][8]*t104);
Kfusion[8] = -t62*(t35+P[8][1]*t9+P[8][5]*t100+P[8][0]*t105-P[8][4]*t101-P[8][2]*t102-P[8][3]*t103);
Kfusion[9] = -t62*(P[9][1]*t9+P[9][5]*t100+P[9][0]*t105-P[9][4]*t101-P[9][2]*t102-P[9][3]*t103+P[9][8]*t104);
Kfusion[10] = -t62*(P[10][1]*t9+P[10][5]*t100+P[10][0]*t105-P[10][4]*t101-P[10][2]*t102-P[10][3]*t103+P[10][8]*t104);
Kfusion[11] = -t62*(P[11][1]*t9+P[11][5]*t100+P[11][0]*t105-P[11][4]*t101-P[11][2]*t102-P[11][3]*t103+P[11][8]*t104);
Kfusion[12] = -t62*(P[12][1]*t9+P[12][5]*t100+P[12][0]*t105-P[12][4]*t101-P[12][2]*t102-P[12][3]*t103+P[12][8]*t104);
Kfusion[13] = -t62*(P[13][1]*t9+P[13][5]*t100+P[13][0]*t105-P[13][4]*t101-P[13][2]*t102-P[13][3]*t103+P[13][8]*t104);
Kfusion[14] = -t62*(P[14][1]*t9+P[14][5]*t100+P[14][0]*t105-P[14][4]*t101-P[14][2]*t102-P[14][3]*t103+P[14][8]*t104);
Kfusion[15] = -t62*(P[15][1]*t9+P[15][5]*t100+P[15][0]*t105-P[15][4]*t101-P[15][2]*t102-P[15][3]*t103+P[15][8]*t104);
Kfusion[16] = -t62*(P[16][1]*t9+P[16][5]*t100+P[16][0]*t105-P[16][4]*t101-P[16][2]*t102-P[16][3]*t103+P[16][8]*t104);
Kfusion[17] = -t62*(P[17][1]*t9+P[17][5]*t100+P[17][0]*t105-P[17][4]*t101-P[17][2]*t102-P[17][3]*t103+P[17][8]*t104);
Kfusion[18] = -t62*(P[18][1]*t9+P[18][5]*t100+P[18][0]*t105-P[18][4]*t101-P[18][2]*t102-P[18][3]*t103+P[18][8]*t104);
Kfusion[19] = -t62*(P[19][1]*t9+P[19][5]*t100+P[19][0]*t105-P[19][4]*t101-P[19][2]*t102-P[19][3]*t103+P[19][8]*t104);
Kfusion[20] = -t62*(P[20][1]*t9+P[20][5]*t100+P[20][0]*t105-P[20][4]*t101-P[20][2]*t102-P[20][3]*t103+P[20][8]*t104);
Kfusion[21] = -t62*(P[21][1]*t9+P[21][5]*t100+P[21][0]*t105-P[21][4]*t101-P[21][2]*t102-P[21][3]*t103+P[21][8]*t104);
Kfusion[22] = -t62*(P[22][1]*t9+P[22][5]*t100+P[22][0]*t105-P[22][4]*t101-P[22][2]*t102-P[22][3]*t103+P[22][8]*t104);
Kfusion[23] = -t62*(P[23][1]*t9+P[23][5]*t100+P[23][0]*t105-P[23][4]*t101-P[23][2]*t102-P[23][3]*t103+P[23][8]*t104);
matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt
+212 -107
View File
@@ -3,114 +3,219 @@
// Observations are body modtion compensated optica flow rates about the X and Y body axis
// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated)
// intermediate variable from algebraic optimisation
float SH_LOS[7];
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + ve*(2.0f*q0*q3 + 2.0f*q1*q2);
SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - vn*(2.0f*q0*q3 - 2.0f*q1*q2);
SH_LOS[3] = 1.0f/(pd - ptd);
SH_LOS[4] = vd*SH_LOS[0] - ve*(2.0f*q0*q1 - 2.0f*q2*q3) + vn*(2.0f*q0*q2 + 2.0f*q1*q3);
SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3;
float H_LOS[2][24];
// Calculate the observation jacobians for the LOS rate about the X body axis
float H_LOS[24];
H_LOS[0] = SH_LOS[2]*SH_LOS[3]*SH_LOS[6] - SH_LOS[0]*SH_LOS[3]*SH_LOS[4];
H_LOS[1] = SH_LOS[2]*SH_LOS[3]*SH_LOS[5];
H_LOS[2] = SH_LOS[0]*SH_LOS[1]*SH_LOS[3];
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2);
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[6];
H_LOS[8] = SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]);
// calculate X axis observation Jacobian
float t2 = 1.0f / range;
float t3 = q0 * q0;
float t4 = q1 * q1;
float t5 = q2 * q2;
float t6 = q3 * q3;
float t7 = q0 * q2 * 2.0f;
float t8 = q1 * q3 * 2.0f;
float t9 = q0 * q3 * 2.0f;
float t10 = q1 * q2 * 2.0f;
float t11 = q0 * q1 * 2.0f;
H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f));
H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6));
H_LOS[0][3] = -t2 * (t9 - t10);
H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6);
H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f);
// Calculate the observation jacobians for the LOS rate about the Y body axis
float H_LOS[24];
H_LOS[0] = -SH_LOS[1]*SH_LOS[3]*SH_LOS[6];
H_LOS[1] = - SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[1]*SH_LOS[3]*SH_LOS[5];
H_LOS[2] = SH_LOS[0]*SH_LOS[2]*SH_LOS[3];
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3));
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2);
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[5];
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]);
// calculate X axis Kalman gains
t2 = 1.0/range;
t3 = q0*q1*2.0;
t4 = q2*q3*2.0;
t5 = q0*q0;
t6 = q1*q1;
t7 = q2*q2;
t8 = q3*q3;
t9 = q0*q2*2.0;
t10 = q1*q3*2.0;
t11 = q0*q3*2.0;
float t12 = q1*q2*2.0;
float t13 = t11-t12;
float t14 = t3+t4;
float t15 = t5-t6-t7+t8;
float t16 = t15*vd;
float t17 = t3-t4;
float t18 = t9+t10;
float t19 = t18*vn;
float t28 = t17*ve;
float t20 = t16+t19-t28;
float t21 = t5+t6-t7-t8;
float t22 = t21*vn;
float t23 = t9-t10;
float t24 = t11+t12;
float t25 = t24*ve;
float t29 = t23*vd;
float t26 = t22+t25-t29;
float t27 = t5-t6+t7-t8;
float t30 = P[0][0]*t2*t20;
float t31 = P[5][3]*t2*t14;
float t32 = P[0][3]*t2*t20;
float t33 = P[4][3]*t2*t27;
float t56 = P[3][3]*t2*t13;
float t57 = P[2][3]*t2*t26;
float t34 = t31+t32+t33-t56-t57;
float t35 = P[5][5]*t2*t14;
float t36 = P[0][5]*t2*t20;
float t37 = P[4][5]*t2*t27;
float t59 = P[3][5]*t2*t13;
float t60 = P[2][5]*t2*t26;
float t38 = t35+t36+t37-t59-t60;
float t39 = t2*t14*t38;
float t40 = P[5][0]*t2*t14;
float t41 = P[4][0]*t2*t27;
float t61 = P[3][0]*t2*t13;
float t62 = P[2][0]*t2*t26;
float t42 = t30+t40+t41-t61-t62;
float t43 = t2*t20*t42;
float t44 = P[5][2]*t2*t14;
float t45 = P[0][2]*t2*t20;
float t46 = P[4][2]*t2*t27;
float t55 = P[2][2]*t2*t26;
float t63 = P[3][2]*t2*t13;
float t47 = t44+t45+t46-t55-t63;
float t48 = P[5][4]*t2*t14;
float t49 = P[0][4]*t2*t20;
float t50 = P[4][4]*t2*t27;
float t65 = P[3][4]*t2*t13;
float t66 = P[2][4]*t2*t26;
float t51 = t48+t49+t50-t65-t66;
float t52 = t2*t27*t51;
float t58 = t2*t13*t34;
float t64 = t2*t26*t47;
float t53 = R_LOS+t39+t43+t52-t58-t64;
float t54 = 1.0/t53;
Kfusion[0][0] = t54*(t30-P[0][3]*t2*(t11-q1*q2*2.0)+P[0][5]*t2*t14-P[0][2]*t2*t26+P[0][4]*t2*t27);
Kfusion[1][0] = t54*(-P[1][3]*t2*t13+P[1][5]*t2*t14+P[1][0]*t2*t20-P[1][2]*t2*t26+P[1][4]*t2*t27);
Kfusion[2][0] = t54*(-t55-P[2][3]*t2*t13+P[2][5]*t2*t14+P[2][0]*t2*t20+P[2][4]*t2*t27);
Kfusion[3][0] = t54*(-t56+P[3][5]*t2*t14+P[3][0]*t2*t20-P[3][2]*t2*t26+P[3][4]*t2*t27);
Kfusion[4][0] = t54*(t50-P[4][3]*t2*t13+P[4][5]*t2*t14+P[4][0]*t2*t20-P[4][2]*t2*t26);
Kfusion[5][0] = t54*(t35-P[5][3]*t2*t13+P[5][0]*t2*t20-P[5][2]*t2*t26+P[5][4]*t2*t27);
Kfusion[6][0] = t54*(-P[6][3]*t2*t13+P[6][5]*t2*t14+P[6][0]*t2*t20-P[6][2]*t2*t26+P[6][4]*t2*t27);
Kfusion[7][0] = t54*(-P[7][3]*t2*t13+P[7][5]*t2*t14+P[7][0]*t2*t20-P[7][2]*t2*t26+P[7][4]*t2*t27);
Kfusion[8][0] = t54*(-P[8][3]*t2*t13+P[8][5]*t2*t14+P[8][0]*t2*t20-P[8][2]*t2*t26+P[8][4]*t2*t27);
Kfusion[9][0] = t54*(-P[9][3]*t2*t13+P[9][5]*t2*t14+P[9][0]*t2*t20-P[9][2]*t2*t26+P[9][4]*t2*t27);
Kfusion[10][0] = t54*(-P[10][3]*t2*t13+P[10][5]*t2*t14+P[10][0]*t2*t20-P[10][2]*t2*t26+P[10][4]*t2*t27);
Kfusion[11][0] = t54*(-P[11][3]*t2*t13+P[11][5]*t2*t14+P[11][0]*t2*t20-P[11][2]*t2*t26+P[11][4]*t2*t27);
Kfusion[12][0] = t54*(-P[12][3]*t2*t13+P[12][5]*t2*t14+P[12][0]*t2*t20-P[12][2]*t2*t26+P[12][4]*t2*t27);
Kfusion[13][0] = t54*(-P[13][3]*t2*t13+P[13][5]*t2*t14+P[13][0]*t2*t20-P[13][2]*t2*t26+P[13][4]*t2*t27);
Kfusion[14][0] = t54*(-P[14][3]*t2*t13+P[14][5]*t2*t14+P[14][0]*t2*t20-P[14][2]*t2*t26+P[14][4]*t2*t27);
Kfusion[15][0] = t54*(-P[15][3]*t2*t13+P[15][5]*t2*t14+P[15][0]*t2*t20-P[15][2]*t2*t26+P[15][4]*t2*t27);
Kfusion[16][0] = t54*(-P[16][3]*t2*t13+P[16][5]*t2*t14+P[16][0]*t2*t20-P[16][2]*t2*t26+P[16][4]*t2*t27);
Kfusion[17][0] = t54*(-P[17][3]*t2*t13+P[17][5]*t2*t14+P[17][0]*t2*t20-P[17][2]*t2*t26+P[17][4]*t2*t27);
Kfusion[18][0] = t54*(-P[18][3]*t2*t13+P[18][5]*t2*t14+P[18][0]*t2*t20-P[18][2]*t2*t26+P[18][4]*t2*t27);
Kfusion[19][0] = t54*(-P[19][3]*t2*t13+P[19][5]*t2*t14+P[19][0]*t2*t20-P[19][2]*t2*t26+P[19][4]*t2*t27);
Kfusion[20][0] = t54*(-P[20][3]*t2*t13+P[20][5]*t2*t14+P[20][0]*t2*t20-P[20][2]*t2*t26+P[20][4]*t2*t27);
Kfusion[21][0] = t54*(-P[21][3]*t2*t13+P[21][5]*t2*t14+P[21][0]*t2*t20-P[21][2]*t2*t26+P[21][4]*t2*t27);
Kfusion[22][0] = t54*(-P[22][3]*t2*t13+P[22][5]*t2*t14+P[22][0]*t2*t20-P[22][2]*t2*t26+P[22][4]*t2*t27);
Kfusion[23][0] = t54*(-P[23][3]*t2*t13+P[23][5]*t2*t14+P[23][0]*t2*t20-P[23][2]*t2*t26+P[23][4]*t2*t27);
// calculate Y axis observation jacobian
float t2 = 1.0f/range;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = q0*q1*2.0f;
float t8 = q0*q3*2.0f;
float t9 = q0*q2*2.0f;
float t10 = q1*q3*2.0f;
H_LOS[1][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0f));
H_LOS[1][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0f)-vn*(t8-q1*q2*2.0f));
H_LOS[1][3] = -t2*(t3+t4-t5-t6);
H_LOS[1][4] = -t2*(t8+q1*q2*2.0f);
H_LOS[1][5] = t2*(t9-t10);
// Intermediate variables used to calculate the Kalman gain matrices
float SK_LOS[22];
// this is 1/(innovation variance) for the X axis measurement
SK_LOS[0] = 1.0f/(R_LOS - (SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6])*(P[8][0]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][0]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][0]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][0]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][0]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[2]*SH_LOS[3]*SH_LOS[5]*(P[8][1]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][1]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][1]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][1]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][1]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[3]*(P[8][2]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][2]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][2]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][2]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][2]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*SH_LOS[6]*(P[8][5]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][5]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][5]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][5]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][5]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[8][4]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][4]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][4]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][4]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][4]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3])*(P[8][8]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][8]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][8]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][8]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][8]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][8]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][8]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2)*(P[8][3]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][3]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][3]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][3]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][3]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))));
// this is 1/(innovation variance) for the Y axis measurement
SK_LOS[1] = 1.0f/(R_LOS + (SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5])*(P[1][1]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][1]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][1]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][1]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[1]*SH_LOS[3]*SH_LOS[6]*(P[1][0]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][0]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][0]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][0]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[3]*(P[1][2]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][2]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][2]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][2]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*SH_LOS[5]*(P[1][5]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][5]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][5]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][5]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[1][3]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][3]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][3]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][3]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3])*(P[1][8]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][8]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][8]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][8]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][8]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][8]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][8]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2)*(P[1][4]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][4]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][4]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][4]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))));
SK_LOS[2] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
SK_LOS[3] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
SK_LOS[4] = SH_LOS[3];
SK_LOS[5] = SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]);
SK_LOS[6] = SH_LOS[0]*SH_LOS[4]*SK_LOS[4];
SK_LOS[7] = SH_LOS[2]*SH_LOS[6]*SK_LOS[4];
SK_LOS[8] = SH_LOS[0]*SK_LOS[4]*(2.0f*q0*q3 - 2.0f*q1*q2);
SK_LOS[9] = SH_LOS[0]*SH_LOS[1]*SK_LOS[4];
SK_LOS[10] = SH_LOS[2]*SH_LOS[5]*SK_LOS[4];
SK_LOS[11] = SH_LOS[0]*SH_LOS[6]*SK_LOS[4];
SK_LOS[12] = SH_LOS[0]*SK_LOS[3]*SK_LOS[4];
SK_LOS[13] = SH_LOS[1]*SH_LOS[5]*SK_LOS[4];
SK_LOS[14] = SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]);
SK_LOS[15] = SH_LOS[0]*SK_LOS[4]*(2.0f*q0*q3 + 2.0f*q1*q2);
SK_LOS[16] = SH_LOS[0]*SH_LOS[2]*SK_LOS[4];
SK_LOS[17] = SH_LOS[1]*SH_LOS[6]*SK_LOS[4];
SK_LOS[18] = SH_LOS[0]*SH_LOS[5]*SK_LOS[4];
SK_LOS[19] = SH_LOS[0]*SK_LOS[2]*SK_LOS[4];
SK_LOS[20] = SK_LOS[6] - SK_LOS[7];
SK_LOS[21] = SK_LOS[6] + SK_LOS[13];
// Calculate the Kalman gain matrix for the X axis measurement
float Kfusion[24];
Kfusion[0] = SK_LOS[0]*(P[0][8]*SK_LOS[5] - P[0][0]*SK_LOS[20] + P[0][3]*SK_LOS[8] + P[0][2]*SK_LOS[9] + P[0][1]*SK_LOS[10] - P[0][5]*SK_LOS[11] - P[0][4]*SK_LOS[12]);
Kfusion[1] = SK_LOS[0]*(P[1][8]*SK_LOS[5] - P[1][0]*SK_LOS[20] + P[1][3]*SK_LOS[8] + P[1][2]*SK_LOS[9] + P[1][1]*SK_LOS[10] - P[1][5]*SK_LOS[11] - P[1][4]*SK_LOS[12]);
Kfusion[2] = SK_LOS[0]*(P[2][8]*SK_LOS[5] - P[2][0]*SK_LOS[20] + P[2][3]*SK_LOS[8] + P[2][2]*SK_LOS[9] + P[2][1]*SK_LOS[10] - P[2][5]*SK_LOS[11] - P[2][4]*SK_LOS[12]);
Kfusion[3] = SK_LOS[0]*(P[3][8]*SK_LOS[5] - P[3][0]*SK_LOS[20] + P[3][3]*SK_LOS[8] + P[3][2]*SK_LOS[9] + P[3][1]*SK_LOS[10] - P[3][5]*SK_LOS[11] - P[3][4]*SK_LOS[12]);
Kfusion[4] = SK_LOS[0]*(P[4][8]*SK_LOS[5] - P[4][0]*SK_LOS[20] + P[4][3]*SK_LOS[8] + P[4][2]*SK_LOS[9] + P[4][1]*SK_LOS[10] - P[4][5]*SK_LOS[11] - P[4][4]*SK_LOS[12]);
Kfusion[5] = SK_LOS[0]*(P[5][8]*SK_LOS[5] - P[5][0]*SK_LOS[20] + P[5][3]*SK_LOS[8] + P[5][2]*SK_LOS[9] + P[5][1]*SK_LOS[10] - P[5][5]*SK_LOS[11] - P[5][4]*SK_LOS[12]);
Kfusion[6] = SK_LOS[0]*(P[6][8]*SK_LOS[5] - P[6][0]*SK_LOS[20] + P[6][3]*SK_LOS[8] + P[6][2]*SK_LOS[9] + P[6][1]*SK_LOS[10] - P[6][5]*SK_LOS[11] - P[6][4]*SK_LOS[12]);
Kfusion[7] = SK_LOS[0]*(P[7][8]*SK_LOS[5] - P[7][0]*SK_LOS[20] + P[7][3]*SK_LOS[8] + P[7][2]*SK_LOS[9] + P[7][1]*SK_LOS[10] - P[7][5]*SK_LOS[11] - P[7][4]*SK_LOS[12]);
Kfusion[8] = SK_LOS[0]*(P[8][8]*SK_LOS[5] - P[8][0]*SK_LOS[20] + P[8][3]*SK_LOS[8] + P[8][2]*SK_LOS[9] + P[8][1]*SK_LOS[10] - P[8][5]*SK_LOS[11] - P[8][4]*SK_LOS[12]);
Kfusion[9] = SK_LOS[0]*(P[9][8]*SK_LOS[5] - P[9][0]*SK_LOS[20] + P[9][3]*SK_LOS[8] + P[9][2]*SK_LOS[9] + P[9][1]*SK_LOS[10] - P[9][5]*SK_LOS[11] - P[9][4]*SK_LOS[12]);
Kfusion[10] = SK_LOS[0]*(P[10][8]*SK_LOS[5] - P[10][0]*SK_LOS[20] + P[10][3]*SK_LOS[8] + P[10][2]*SK_LOS[9] + P[10][1]*SK_LOS[10] - P[10][5]*SK_LOS[11] - P[10][4]*SK_LOS[12]);
Kfusion[11] = SK_LOS[0]*(P[11][8]*SK_LOS[5] - P[11][0]*SK_LOS[20] + P[11][3]*SK_LOS[8] + P[11][2]*SK_LOS[9] + P[11][1]*SK_LOS[10] - P[11][5]*SK_LOS[11] - P[11][4]*SK_LOS[12]);
Kfusion[12] = SK_LOS[0]*(P[12][8]*SK_LOS[5] - P[12][0]*SK_LOS[20] + P[12][3]*SK_LOS[8] + P[12][2]*SK_LOS[9] + P[12][1]*SK_LOS[10] - P[12][5]*SK_LOS[11] - P[12][4]*SK_LOS[12]);
Kfusion[13] = SK_LOS[0]*(P[13][8]*SK_LOS[5] - P[13][0]*SK_LOS[20] + P[13][3]*SK_LOS[8] + P[13][2]*SK_LOS[9] + P[13][1]*SK_LOS[10] - P[13][5]*SK_LOS[11] - P[13][4]*SK_LOS[12]);
Kfusion[14] = SK_LOS[0]*(P[14][8]*SK_LOS[5] - P[14][0]*SK_LOS[20] + P[14][3]*SK_LOS[8] + P[14][2]*SK_LOS[9] + P[14][1]*SK_LOS[10] - P[14][5]*SK_LOS[11] - P[14][4]*SK_LOS[12]);
Kfusion[15] = SK_LOS[0]*(P[15][8]*SK_LOS[5] - P[15][0]*SK_LOS[20] + P[15][3]*SK_LOS[8] + P[15][2]*SK_LOS[9] + P[15][1]*SK_LOS[10] - P[15][5]*SK_LOS[11] - P[15][4]*SK_LOS[12]);
Kfusion[16] = SK_LOS[0]*(P[16][8]*SK_LOS[5] - P[16][0]*SK_LOS[20] + P[16][3]*SK_LOS[8] + P[16][2]*SK_LOS[9] + P[16][1]*SK_LOS[10] - P[16][5]*SK_LOS[11] - P[16][4]*SK_LOS[12]);
Kfusion[17] = SK_LOS[0]*(P[17][8]*SK_LOS[5] - P[17][0]*SK_LOS[20] + P[17][3]*SK_LOS[8] + P[17][2]*SK_LOS[9] + P[17][1]*SK_LOS[10] - P[17][5]*SK_LOS[11] - P[17][4]*SK_LOS[12]);
Kfusion[18] = SK_LOS[0]*(P[18][8]*SK_LOS[5] - P[18][0]*SK_LOS[20] + P[18][3]*SK_LOS[8] + P[18][2]*SK_LOS[9] + P[18][1]*SK_LOS[10] - P[18][5]*SK_LOS[11] - P[18][4]*SK_LOS[12]);
Kfusion[19] = SK_LOS[0]*(P[19][8]*SK_LOS[5] - P[19][0]*SK_LOS[20] + P[19][3]*SK_LOS[8] + P[19][2]*SK_LOS[9] + P[19][1]*SK_LOS[10] - P[19][5]*SK_LOS[11] - P[19][4]*SK_LOS[12]);
Kfusion[20] = SK_LOS[0]*(P[20][8]*SK_LOS[5] - P[20][0]*SK_LOS[20] + P[20][3]*SK_LOS[8] + P[20][2]*SK_LOS[9] + P[20][1]*SK_LOS[10] - P[20][5]*SK_LOS[11] - P[20][4]*SK_LOS[12]);
Kfusion[21] = SK_LOS[0]*(P[21][8]*SK_LOS[5] - P[21][0]*SK_LOS[20] + P[21][3]*SK_LOS[8] + P[21][2]*SK_LOS[9] + P[21][1]*SK_LOS[10] - P[21][5]*SK_LOS[11] - P[21][4]*SK_LOS[12]);
Kfusion[22] = SK_LOS[0]*(P[22][8]*SK_LOS[5] - P[22][0]*SK_LOS[20] + P[22][3]*SK_LOS[8] + P[22][2]*SK_LOS[9] + P[22][1]*SK_LOS[10] - P[22][5]*SK_LOS[11] - P[22][4]*SK_LOS[12]);
Kfusion[23] = SK_LOS[0]*(P[23][8]*SK_LOS[5] - P[23][0]*SK_LOS[20] + P[23][3]*SK_LOS[8] + P[23][2]*SK_LOS[9] + P[23][1]*SK_LOS[10] - P[23][5]*SK_LOS[11] - P[23][4]*SK_LOS[12]);
// Calculate the Kalman gain matrix for the Y axis measurement
float Kfusion[24];
Kfusion[0] = -SK_LOS[1]*(P[0][1]*SK_LOS[21] + P[0][8]*SK_LOS[14] - P[0][4]*SK_LOS[15] - P[0][2]*SK_LOS[16] + P[0][0]*SK_LOS[17] + P[0][5]*SK_LOS[18] - P[0][3]*SK_LOS[19]);
Kfusion[1] = -SK_LOS[1]*(P[1][1]*SK_LOS[21] + P[1][8]*SK_LOS[14] - P[1][4]*SK_LOS[15] - P[1][2]*SK_LOS[16] + P[1][0]*SK_LOS[17] + P[1][5]*SK_LOS[18] - P[1][3]*SK_LOS[19]);
Kfusion[2] = -SK_LOS[1]*(P[2][1]*SK_LOS[21] + P[2][8]*SK_LOS[14] - P[2][4]*SK_LOS[15] - P[2][2]*SK_LOS[16] + P[2][0]*SK_LOS[17] + P[2][5]*SK_LOS[18] - P[2][3]*SK_LOS[19]);
Kfusion[3] = -SK_LOS[1]*(P[3][1]*SK_LOS[21] + P[3][8]*SK_LOS[14] - P[3][4]*SK_LOS[15] - P[3][2]*SK_LOS[16] + P[3][0]*SK_LOS[17] + P[3][5]*SK_LOS[18] - P[3][3]*SK_LOS[19]);
Kfusion[4] = -SK_LOS[1]*(P[4][1]*SK_LOS[21] + P[4][8]*SK_LOS[14] - P[4][4]*SK_LOS[15] - P[4][2]*SK_LOS[16] + P[4][0]*SK_LOS[17] + P[4][5]*SK_LOS[18] - P[4][3]*SK_LOS[19]);
Kfusion[5] = -SK_LOS[1]*(P[5][1]*SK_LOS[21] + P[5][8]*SK_LOS[14] - P[5][4]*SK_LOS[15] - P[5][2]*SK_LOS[16] + P[5][0]*SK_LOS[17] + P[5][5]*SK_LOS[18] - P[5][3]*SK_LOS[19]);
Kfusion[6] = -SK_LOS[1]*(P[6][1]*SK_LOS[21] + P[6][8]*SK_LOS[14] - P[6][4]*SK_LOS[15] - P[6][2]*SK_LOS[16] + P[6][0]*SK_LOS[17] + P[6][5]*SK_LOS[18] - P[6][3]*SK_LOS[19]);
Kfusion[7] = -SK_LOS[1]*(P[7][1]*SK_LOS[21] + P[7][8]*SK_LOS[14] - P[7][4]*SK_LOS[15] - P[7][2]*SK_LOS[16] + P[7][0]*SK_LOS[17] + P[7][5]*SK_LOS[18] - P[7][3]*SK_LOS[19]);
Kfusion[8] = -SK_LOS[1]*(P[8][1]*SK_LOS[21] + P[8][8]*SK_LOS[14] - P[8][4]*SK_LOS[15] - P[8][2]*SK_LOS[16] + P[8][0]*SK_LOS[17] + P[8][5]*SK_LOS[18] - P[8][3]*SK_LOS[19]);
Kfusion[9] = -SK_LOS[1]*(P[9][1]*SK_LOS[21] + P[9][8]*SK_LOS[14] - P[9][4]*SK_LOS[15] - P[9][2]*SK_LOS[16] + P[9][0]*SK_LOS[17] + P[9][5]*SK_LOS[18] - P[9][3]*SK_LOS[19]);
Kfusion[10] = -SK_LOS[1]*(P[10][1]*SK_LOS[21] + P[10][8]*SK_LOS[14] - P[10][4]*SK_LOS[15] - P[10][2]*SK_LOS[16] + P[10][0]*SK_LOS[17] + P[10][5]*SK_LOS[18] - P[10][3]*SK_LOS[19]);
Kfusion[11] = -SK_LOS[1]*(P[11][1]*SK_LOS[21] + P[11][8]*SK_LOS[14] - P[11][4]*SK_LOS[15] - P[11][2]*SK_LOS[16] + P[11][0]*SK_LOS[17] + P[11][5]*SK_LOS[18] - P[11][3]*SK_LOS[19]);
Kfusion[12] = -SK_LOS[1]*(P[12][1]*SK_LOS[21] + P[12][8]*SK_LOS[14] - P[12][4]*SK_LOS[15] - P[12][2]*SK_LOS[16] + P[12][0]*SK_LOS[17] + P[12][5]*SK_LOS[18] - P[12][3]*SK_LOS[19]);
Kfusion[13] = -SK_LOS[1]*(P[13][1]*SK_LOS[21] + P[13][8]*SK_LOS[14] - P[13][4]*SK_LOS[15] - P[13][2]*SK_LOS[16] + P[13][0]*SK_LOS[17] + P[13][5]*SK_LOS[18] - P[13][3]*SK_LOS[19]);
Kfusion[14] = -SK_LOS[1]*(P[14][1]*SK_LOS[21] + P[14][8]*SK_LOS[14] - P[14][4]*SK_LOS[15] - P[14][2]*SK_LOS[16] + P[14][0]*SK_LOS[17] + P[14][5]*SK_LOS[18] - P[14][3]*SK_LOS[19]);
Kfusion[15] = -SK_LOS[1]*(P[15][1]*SK_LOS[21] + P[15][8]*SK_LOS[14] - P[15][4]*SK_LOS[15] - P[15][2]*SK_LOS[16] + P[15][0]*SK_LOS[17] + P[15][5]*SK_LOS[18] - P[15][3]*SK_LOS[19]);
Kfusion[16] = -SK_LOS[1]*(P[16][1]*SK_LOS[21] + P[16][8]*SK_LOS[14] - P[16][4]*SK_LOS[15] - P[16][2]*SK_LOS[16] + P[16][0]*SK_LOS[17] + P[16][5]*SK_LOS[18] - P[16][3]*SK_LOS[19]);
Kfusion[17] = -SK_LOS[1]*(P[17][1]*SK_LOS[21] + P[17][8]*SK_LOS[14] - P[17][4]*SK_LOS[15] - P[17][2]*SK_LOS[16] + P[17][0]*SK_LOS[17] + P[17][5]*SK_LOS[18] - P[17][3]*SK_LOS[19]);
Kfusion[18] = -SK_LOS[1]*(P[18][1]*SK_LOS[21] + P[18][8]*SK_LOS[14] - P[18][4]*SK_LOS[15] - P[18][2]*SK_LOS[16] + P[18][0]*SK_LOS[17] + P[18][5]*SK_LOS[18] - P[18][3]*SK_LOS[19]);
Kfusion[19] = -SK_LOS[1]*(P[19][1]*SK_LOS[21] + P[19][8]*SK_LOS[14] - P[19][4]*SK_LOS[15] - P[19][2]*SK_LOS[16] + P[19][0]*SK_LOS[17] + P[19][5]*SK_LOS[18] - P[19][3]*SK_LOS[19]);
Kfusion[20] = -SK_LOS[1]*(P[20][1]*SK_LOS[21] + P[20][8]*SK_LOS[14] - P[20][4]*SK_LOS[15] - P[20][2]*SK_LOS[16] + P[20][0]*SK_LOS[17] + P[20][5]*SK_LOS[18] - P[20][3]*SK_LOS[19]);
Kfusion[21] = -SK_LOS[1]*(P[21][1]*SK_LOS[21] + P[21][8]*SK_LOS[14] - P[21][4]*SK_LOS[15] - P[21][2]*SK_LOS[16] + P[21][0]*SK_LOS[17] + P[21][5]*SK_LOS[18] - P[21][3]*SK_LOS[19]);
Kfusion[22] = -SK_LOS[1]*(P[22][1]*SK_LOS[21] + P[22][8]*SK_LOS[14] - P[22][4]*SK_LOS[15] - P[22][2]*SK_LOS[16] + P[22][0]*SK_LOS[17] + P[22][5]*SK_LOS[18] - P[22][3]*SK_LOS[19]);
Kfusion[23] = -SK_LOS[1]*(P[23][1]*SK_LOS[21] + P[23][8]*SK_LOS[14] - P[23][4]*SK_LOS[15] - P[23][2]*SK_LOS[16] + P[23][0]*SK_LOS[17] + P[23][5]*SK_LOS[18] - P[23][3]*SK_LOS[19]);
// calculate Y axis Kalman gains
t2 = 1.0f/range;
t3 = q0*q2*2.0f;
t4 = q0*q0;
t5 = q1*q1;
t6 = q2*q2;
t7 = q3*q3;
t8 = q0*q1*2.0f;
t9 = q0*q3*2.0f;
t10 = q1*q2*2.0f;
float t11 = t9+t10;
float t12 = q1*q3*2.0f;
float t13 = t4-t5-t6+t7;
float t14 = t13*vd;
float t15 = q2*q3*2.0f;
float t16 = t3+t12;
float t17 = t16*vn;
float t18 = t4-t5+t6-t7;
float t19 = t18*ve;
float t20 = t8+t15;
float t21 = t20*vd;
float t22 = t9-t10;
float t28 = t22*vn;
float t23 = t19+t21-t28;
float t24 = t4+t5-t6-t7;
float t25 = t3-t12;
float t26 = t8-t15;
float t29 = t26*ve;
float t27 = t14+t17-t29;
float t30 = P[4][4]*t2*t11;
float t31 = P[2][4]*t2*t23;
float t32 = P[3][4]*t2*t24;
float t56 = P[5][4]*t2*t25;
float t57 = P[1][4]*t2*t27;
float t33 = t30+t31+t32-t56-t57;
float t34 = t2*t11*t33;
float t35 = P[4][5]*t2*t11;
float t36 = P[2][5]*t2*t23;
float t37 = P[3][5]*t2*t24;
float t58 = P[5][5]*t2*t25;
float t59 = P[1][5]*t2*t27;
float t38 = t35+t36+t37-t58-t59;
float t39 = P[4][1]*t2*t11;
float t40 = P[2][1]*t2*t23;
float t41 = P[3][1]*t2*t24;
float t55 = P[1][1]*t2*t27;
float t61 = P[5][1]*t2*t25;
float t42 = t39+t40+t41-t55-t61;
float t43 = P[4][2]*t2*t11;
float t44 = P[2][2]*t2*t23;
float t45 = P[3][2]*t2*t24;
float t63 = P[5][2]*t2*t25;
float t64 = P[1][2]*t2*t27;
float t46 = t43+t44+t45-t63-t64;
float t47 = t2*t23*t46;
float t48 = P[4][3]*t2*t11;
float t49 = P[2][3]*t2*t23;
float t50 = P[3][3]*t2*t24;
float t65 = P[5][3]*t2*t25;
float t66 = P[1][3]*t2*t27;
float t51 = t48+t49+t50-t65-t66;
float t52 = t2*t24*t51;
float t60 = t2*t25*t38;
float t62 = t2*t27*t42;
float t53 = R_LOS+t34+t47+t52-t60-t62;
float t54 = 1.0f/t53;
Kfusion[0][1] = -t54*(P[0][4]*t2*t11+P[0][2]*t2*t23+P[0][3]*t2*t24-P[0][1]*t2*t27-P[0][5]*t2*t25);
Kfusion[1][1] = -t54*(-t55+P[1][4]*t2*t11+P[1][2]*t2*t23+P[1][3]*t2*t24-P[1][5]*t2*t25);
Kfusion[2][1] = -t54*(t44+P[2][4]*t2*t11+P[2][3]*t2*t24-P[2][1]*t2*t27-P[2][5]*t2*t25);
Kfusion[3][1] = -t54*(t50+P[3][4]*t2*t11+P[3][2]*t2*t23-P[3][1]*t2*t27-P[3][5]*t2*t25);
Kfusion[4][1] = -t54*(t30+P[4][2]*t2*t23+P[4][3]*t2*t24-P[4][1]*t2*t27-P[4][5]*t2*t25);
Kfusion[5][1] = -t54*(-t58+P[5][4]*t2*t11+P[5][2]*t2*t23+P[5][3]*t2*t24-P[5][1]*t2*t27);
Kfusion[6][1] = -t54*(P[6][4]*t2*t11+P[6][2]*t2*t23+P[6][3]*t2*t24-P[6][1]*t2*t27-P[6][5]*t2*t25);
Kfusion[7][1] = -t54*(P[7][4]*t2*t11+P[7][2]*t2*t23+P[7][3]*t2*t24-P[7][1]*t2*t27-P[7][5]*t2*t25);
Kfusion[8][1] = -t54*(P[8][4]*t2*t11+P[8][2]*t2*t23+P[8][3]*t2*t24-P[8][1]*t2*t27-P[8][5]*t2*t25);
Kfusion[9][1] = -t54*(P[9][4]*t2*t11+P[9][2]*t2*t23+P[9][3]*t2*t24-P[9][1]*t2*t27-P[9][5]*t2*t25);
Kfusion[10][1] = -t54*(P[10][4]*t2*t11+P[10][2]*t2*t23+P[10][3]*t2*t24-P[10][1]*t2*t27-P[10][5]*t2*t25);
Kfusion[11][1] = -t54*(P[11][4]*t2*t11+P[11][2]*t2*t23+P[11][3]*t2*t24-P[11][1]*t2*t27-P[11][5]*t2*t25);
Kfusion[12][1] = -t54*(P[12][4]*t2*t11+P[12][2]*t2*t23+P[12][3]*t2*t24-P[12][1]*t2*t27-P[12][5]*t2*t25);
Kfusion[13][1] = -t54*(P[13][4]*t2*t11+P[13][2]*t2*t23+P[13][3]*t2*t24-P[13][1]*t2*t27-P[13][5]*t2*t25);
Kfusion[14][1] = -t54*(P[14][4]*t2*t11+P[14][2]*t2*t23+P[14][3]*t2*t24-P[14][1]*t2*t27-P[14][5]*t2*t25);
Kfusion[15][1] = -t54*(P[15][4]*t2*t11+P[15][2]*t2*t23+P[15][3]*t2*t24-P[15][1]*t2*t27-P[15][5]*t2*t25);
Kfusion[16][1] = -t54*(P[16][4]*t2*t11+P[16][2]*t2*t23+P[16][3]*t2*t24-P[16][1]*t2*t27-P[16][5]*t2*t25);
Kfusion[17][1] = -t54*(P[17][4]*t2*t11+P[17][2]*t2*t23+P[17][3]*t2*t24-P[17][1]*t2*t27-P[17][5]*t2*t25);
Kfusion[18][1] = -t54*(P[18][4]*t2*t11+P[18][2]*t2*t23+P[18][3]*t2*t24-P[18][1]*t2*t27-P[18][5]*t2*t25);
Kfusion[19][1] = -t54*(P[19][4]*t2*t11+P[19][2]*t2*t23+P[19][3]*t2*t24-P[19][1]*t2*t27-P[19][5]*t2*t25);
Kfusion[20][1] = -t54*(P[20][4]*t2*t11+P[20][2]*t2*t23+P[20][3]*t2*t24-P[20][1]*t2*t27-P[20][5]*t2*t25);
Kfusion[21][1] = -t54*(P[21][4]*t2*t11+P[21][2]*t2*t23+P[21][3]*t2*t24-P[21][1]*t2*t27-P[21][5]*t2*t25);
Kfusion[22][1] = -t54*(P[22][4]*t2*t11+P[22][2]*t2*t23+P[22][3]*t2*t24-P[22][1]*t2*t27-P[22][5]*t2*t25);
Kfusion[23][1] = -t54*(P[23][4]*t2*t11+P[23][2]*t2*t23+P[23][3]*t2*t24-P[23][1]*t2*t27-P[23][5]*t2*t25);