mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 19:19:06 +08:00
EKF: Update derivation scripts and outputs
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
parent
ffebaf384f
commit
e9ccfdd484
@ -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);
|
||||
@ -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);
|
||||
|
||||
@ -15,7 +15,7 @@ fileID = fopen(fileName,'r');
|
||||
% This call is based on the structure of the file used to generate this
|
||||
% code. If an error occurs for a different file, try regenerating the code
|
||||
% from the Import Tool.
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false, 'Bufsize', 65535);
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
|
||||
|
||||
%% Close the text file.
|
||||
fclose(fileID);
|
||||
|
||||
@ -15,7 +15,7 @@ fileID = fopen(fileName,'r');
|
||||
% This call is based on the structure of the file used to generate this
|
||||
% code. If an error occurs for a different file, try regenerating the code
|
||||
% from the Import Tool.
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false,'Bufsize',65535);
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
|
||||
|
||||
%% Close the text file.
|
||||
fclose(fileID);
|
||||
|
||||
@ -1,13 +1,13 @@
|
||||
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
|
||||
|
||||
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
|
||||
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
|
||||
% XYZ body fixed frame
|
||||
% Sequential fusion of velocity and position measurements
|
||||
% Fusion of true airspeed
|
||||
% Sequential fusion of magnetic flux measurements
|
||||
% 24 state architecture.
|
||||
% IMU data is assumed to arrive at a constant rate with a time step of dt
|
||||
% IMU delta angle and velocity data are used as time varying parameters,
|
||||
% IMU delta angle and velocity data are used as control inputs,
|
||||
% not observations
|
||||
|
||||
% Author: Paul Riseborough
|
||||
@ -15,8 +15,8 @@
|
||||
% Based on use of a rotation vector for attitude estimation as described
|
||||
% here:
|
||||
|
||||
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
|
||||
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
|
||||
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
|
||||
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
|
||||
% pp. 855-860.
|
||||
|
||||
% State vector:
|
||||
@ -52,7 +52,7 @@ syms vn ve vd real % NED velocity - m/sec
|
||||
syms pn pe pd real % NED position - m
|
||||
syms dax_b day_b daz_b real % delta angle bias - rad
|
||||
syms dax_s day_s daz_s real % delta angle scale factor
|
||||
syms dvz_b real % delta velocity bias - m/sec
|
||||
syms dvz_b dvy_b dvz_b real % delta velocity bias - m/sec
|
||||
syms dt real % IMU time step - sec
|
||||
syms gravity real % gravity - m/sec^2
|
||||
syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real; % IMU delta angle and delta velocity measurement noise
|
||||
@ -72,9 +72,9 @@ syms R_DECL R_YAW real; % variance of declination or yaw angle observation
|
||||
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
|
||||
syms rho real % air density (kg/m^3)
|
||||
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
|
||||
syms Kacc real % ratio of horizontal acceleration to top speed for a multirotor
|
||||
syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s)
|
||||
|
||||
%% define the process equations
|
||||
%% define the state prediction equations
|
||||
|
||||
% define the measured Delta angle and delta velocity vectors
|
||||
dAngMeas = [dax; day; daz];
|
||||
@ -101,7 +101,7 @@ truthQuat = QuatMult(estQuat, errQuat);
|
||||
Tbn = Quat2Tbn(truthQuat);
|
||||
|
||||
% define the truth delta angle
|
||||
% ignore coning compensation as these effects are negligible in terms of
|
||||
% ignore coning compensation as these effects are negligible in terms of
|
||||
% covariance growth for our application and grade of sensor
|
||||
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise];
|
||||
|
||||
@ -157,7 +157,27 @@ nStates=numel(stateVector);
|
||||
% Define vector of process equations
|
||||
newStateVector = [errRotNew;vNew;pNew;dabNew;dasNew;dvbNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
|
||||
|
||||
%% derive the covariance prediction equation
|
||||
% derive the state transition matrix
|
||||
F = jacobian(newStateVector, stateVector);
|
||||
% set the rotation error states to zero
|
||||
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[F,SF]=OptimiseAlgebra(F,'SF');
|
||||
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
% '_l_' to represent '( '
|
||||
% '_c_' to represent ,
|
||||
% '_r_' to represent ')'
|
||||
% these can be substituted later to create executable code
|
||||
for rowIndex = 1:nStates
|
||||
for colIndex = 1:nStates
|
||||
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
|
||||
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
|
||||
end
|
||||
end
|
||||
|
||||
save 'StatePrediction.mat';
|
||||
|
||||
%% derive the covariance prediction equations
|
||||
% This reduces the number of floating point operations by a factor of 6 or
|
||||
% more compared to using the standard matrix operations in code
|
||||
|
||||
@ -179,26 +199,8 @@ Q = G*distMatrix*transpose(G);
|
||||
|
||||
% remove the disturbance noise from the process equations as it is only
|
||||
% needed when calculating the disturbance influence matrix
|
||||
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0);
|
||||
errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0);
|
||||
|
||||
% derive the state transition matrix
|
||||
F = jacobian(newStateVector, stateVector);
|
||||
% set the rotation error states to zero
|
||||
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[F,SF]=OptimiseAlgebra(F,'SF');
|
||||
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
% '_l_' to represent '( '
|
||||
% '_c_' to represent ,
|
||||
% '_r_' to represent ')'
|
||||
% these can be substituted later to create executable code
|
||||
for rowIndex = 1:nStates
|
||||
for colIndex = 1:nStates
|
||||
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
|
||||
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
|
||||
end
|
||||
end
|
||||
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
|
||||
errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
|
||||
|
||||
% Derive the predicted covariance matrix using the standard equation
|
||||
PP = F*P*transpose(F) + Q;
|
||||
@ -206,14 +208,27 @@ PP = F*P*transpose(F) + Q;
|
||||
% Collect common expressions to optimise processing
|
||||
[PP,SPP]=OptimiseAlgebra(PP,'SPP');
|
||||
|
||||
save('StateAndCovariancePrediction.mat');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of true airspeed measurements
|
||||
load('StatePrediction.mat');
|
||||
VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
|
||||
H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
|
||||
H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing
|
||||
K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
|
||||
K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);
|
||||
[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
|
||||
|
||||
% save equations and reset workspace
|
||||
save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of angle of sideslip measurements
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% calculate wind relative velocities in nav frame and rotate into body frame
|
||||
Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd];
|
||||
% calculate predicted angle of sideslip using small angle assumption
|
||||
@ -223,7 +238,14 @@ H_BETA = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
|
||||
K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector
|
||||
|
||||
% save equations and reset workspace
|
||||
save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of magnetic field measurement
|
||||
load('StatePrediction.mat');
|
||||
|
||||
magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
|
||||
H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
|
||||
H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
@ -236,99 +258,128 @@ K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); %
|
||||
K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector
|
||||
[K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');
|
||||
|
||||
%% derive equations for sequential fusion of optical flow measurements
|
||||
% save equations and reset workspace
|
||||
save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for sequential fusion of optical flow measurements
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% range is defined as distance from camera focal point to centre of sensor fov
|
||||
syms range real;
|
||||
|
||||
% calculate range from plane to centre of sensor fov assuming flat earth
|
||||
% and camera axes aligned with body axes
|
||||
range = ((ptd - pd)/Tbn(3,3));
|
||||
% calculate relative velocity in body frame
|
||||
relVelBody = transpose(Tbn)*[vn;ve;vd];
|
||||
|
||||
% divide by range to get predicted angular LOS rates relative to X and Y
|
||||
% axes. Note these are body angular rate motion compensated optical flow rates
|
||||
losRateX = +relVelBody(2)/range;
|
||||
losRateY = -relVelBody(1)/range;
|
||||
|
||||
H_LOS = jacobian([losRateX;losRateY],stateVector); % measurement Jacobian
|
||||
H_LOS = subs(H_LOS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_LOS = simplify(H_LOS);
|
||||
[H_LOS,SH_LOS] = OptimiseAlgebra(H_LOS,'SH_LOS');
|
||||
save('temp1.mat','losRateX','losRateY');
|
||||
|
||||
% combine into a single K matrix to enable common expressions to be found
|
||||
% note this matrix cannot be used in a single step fusion
|
||||
K_LOSX = (P*transpose(H_LOS(1,:)))/(H_LOS(1,:)*P*transpose(H_LOS(1,:)) + R_LOS); % Kalman gain vector
|
||||
% calculate the observation Jacobian for the X axis
|
||||
H_LOSX = jacobian(losRateX,stateVector); % measurement Jacobian
|
||||
H_LOSX = subs(H_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_LOSX = simplify(H_LOSX);
|
||||
save('temp2.mat','H_LOSX');
|
||||
ccode(H_LOSX,'file','H_LOSX.c');
|
||||
fix_c_code('H_LOSX.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
|
||||
% calculate the observation Jacobian for the Y axis
|
||||
H_LOSY = jacobian(losRateY,stateVector); % measurement Jacobian
|
||||
H_LOSY = subs(H_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_LOSY = simplify(H_LOSY);
|
||||
save('temp3.mat','H_LOSY');
|
||||
ccode(H_LOSY,'file','H_LOSY.c');
|
||||
fix_c_code('H_LOSY.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
load('temp2.mat');
|
||||
|
||||
% calculate Kalman gain vector for the X axis
|
||||
K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector
|
||||
K_LOSX = subs(K_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
K_LOSY = (P*transpose(H_LOS(2,:)))/(H_LOS(2,:)*P*transpose(H_LOS(2,:)) + R_LOS); % Kalman gain vector
|
||||
K_LOSX = simplify(K_LOSX);
|
||||
ccode(K_LOSX,'file','K_LOSX.c');
|
||||
fix_c_code('K_LOSX.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
load('temp3.mat');
|
||||
|
||||
% calculate Kalman gain vector for the Y axis
|
||||
K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector
|
||||
K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
K_LOS = [K_LOSX,K_LOSY];
|
||||
simplify(K_LOS);
|
||||
[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS');
|
||||
K_LOSY = simplify(K_LOSY);
|
||||
ccode(K_LOSY,'file','K_LOSY.c');
|
||||
fix_c_code('K_LOSY.c');
|
||||
|
||||
% Use matlab c code converter for an alternate method of
|
||||
ccode(H_LOS,'file','H_LOS.txt');
|
||||
ccode(K_LOSX,'file','K_LOSX.txt');
|
||||
ccode(K_LOSY,'file','K_LOSY.txt');
|
||||
|
||||
%% derive equations for simple fusion of 2-D magnetic heading measurements
|
||||
|
||||
% rotate magnetic field into earth axes
|
||||
magMeasNED = Tbn*[magX;magY;magZ];
|
||||
% the predicted measurement is the angle wrt true north of the horizontal
|
||||
% component of the measured field
|
||||
angMeas = atan(magMeasNED(2)/magMeasNED(1));
|
||||
simpleStateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b];
|
||||
Psimple = P(1:16,1:16);
|
||||
H_MAGS = jacobian(angMeas,simpleStateVector); % measurement Jacobian
|
||||
%H_MAGS = H_MAGS(1:3);
|
||||
H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
%H_MAGS = simplify(H_MAGS);
|
||||
%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
|
||||
ccode(H_MAGS,'file','calcH_MAGS.c');
|
||||
% Calculate Kalman gain vector
|
||||
K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL);
|
||||
%K_MAGS = simplify(K_MAGS);
|
||||
%[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS');
|
||||
ccode(K_MAGS,'file','calcK_MAGS.c');
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of 321 sequence yaw measurement
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% Calculate the yaw (first rotation) angle from the 321 rotation sequence
|
||||
angMeas = atan(Tbn(2,1)/Tbn(1,1));
|
||||
H_YAW = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
H_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
ccode(H_YAW,'file','calcH_YAW321.c');
|
||||
% Calculate Kalman gain vector
|
||||
K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW);
|
||||
ccode([K_YAW;H_YAW'],'file','calcYAW321.c');
|
||||
H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
H_YAW321 = subs(H_YAW321, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_YAW321 = simplify(H_YAW321);
|
||||
ccode(H_YAW321,'file','calcH_YAW321.c');
|
||||
fix_c_code('calcH_YAW321.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of 312 sequence yaw measurement
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% Calculate the yaw (first rotation) angle from an Euler 312 sequence
|
||||
angMeas = atan(-Tbn(1,2)/Tbn(2,2));
|
||||
H_YAW2 = jacobian(angMeas,stateVector); % measurement Jacobianclea
|
||||
H_YAW2 = subs(H_YAW2, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
ccode(H_YAW2,'file','calcH_YAW312.c');
|
||||
% Calculate Kalman gain vector
|
||||
K_YAW2 = (P*transpose(H_YAW2))/(H_YAW2*P*transpose(H_YAW2) + R_YAW);
|
||||
ccode([K_YAW2;H_YAW2'],'file','calcYAW312.c');
|
||||
H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea
|
||||
H_YAW312 = subs(H_YAW312, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_YAW312 = simplify(H_YAW312);
|
||||
ccode(H_YAW312,'file','calcH_YAW312.c');
|
||||
fix_c_code('calcH_YAW312.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of declination
|
||||
load('StatePrediction.mat');
|
||||
|
||||
%% derive equations for fusion of synthetic deviation measurement
|
||||
% used to keep correct heading when operating without absolute position or
|
||||
% velocity measurements - eg when using optical flow
|
||||
% rotate magnetic field into earth axes
|
||||
magMeasNED = [magN;magE;magD];
|
||||
% the predicted measurement is the angle wrt magnetic north of the horizontal
|
||||
% component of the measured field
|
||||
angMeas = atan(magMeasNED(2)/magMeasNED(1));
|
||||
angMeas = atan(magE/magN);
|
||||
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_MAGD = simplify(H_MAGD);
|
||||
%[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD');
|
||||
ccode(H_MAGD,'file','calcH_MAGD.c');
|
||||
% Calculate Kalman gain vector
|
||||
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
|
||||
ccode([H_MAGD',K_MAGD],'file','calcMAGD.c');
|
||||
K_MAGD = simplify(K_MAGD);
|
||||
ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c');
|
||||
fix_c_code('calcMAGD.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of lateral body acceleration (multirotors only)
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% use relationship between airspeed along the X and Y body axis and the
|
||||
% drag to predict the lateral acceleration for a multirotor vehicle type
|
||||
@ -344,32 +395,45 @@ vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
|
||||
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
|
||||
|
||||
% Use a simple viscous drag model for the linear estimator equations
|
||||
% Use the the derivative from speed to acceleration averaged across the
|
||||
% Use the the derivative from speed to acceleration averaged across the
|
||||
% speed range
|
||||
% The nonlinear equation will be used to calculate the predicted
|
||||
% measurement in implementation
|
||||
accXpred = -Kacc*vrel(1); % predicted acceleration measured along X body axis
|
||||
accYpred = -Kacc*vrel(2); % predicted acceleration measured along Y body axis
|
||||
accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis
|
||||
accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis
|
||||
|
||||
% Derive observation Jacobian and Kalman gain matrix for X accel fusion
|
||||
H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
|
||||
H_ACCX = subs(H_ACCX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_ACCX = simplify(H_ACCX);
|
||||
[H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing
|
||||
K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
|
||||
ccode([H_ACCX',K_ACCX],'file','calcACCX.c');
|
||||
[K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector
|
||||
|
||||
% Derive observation Jacobian and Kalman gain matrix for Y accel fusion
|
||||
H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
|
||||
H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_ACCY = simplify(H_ACCY);
|
||||
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
|
||||
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
|
||||
ccode([H_ACCY',K_ACCY],'file','calcACCY.c');
|
||||
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
|
||||
|
||||
% save equations and reset workspace
|
||||
save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% Save output and convert to m and c code fragments
|
||||
|
||||
% load equations for predictions and updates
|
||||
load('StateAndCovariancePrediction.mat');
|
||||
load('Airspeed.mat');
|
||||
load('Sideslip.mat');
|
||||
load('Magnetometer.mat');
|
||||
load('Drag.mat');
|
||||
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||
save(fileName);
|
||||
SaveScriptCode(nStates);
|
||||
ConvertToM(nStates);
|
||||
ConvertToC(nStates);
|
||||
ConvertToC(nStates);
|
||||
@ -409,7 +409,7 @@ if exist('SH_LOS','var')
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_LOS(rowIndex,1));
|
||||
string = char(K_LOSX(rowIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
@ -421,7 +421,7 @@ if exist('SH_LOS','var')
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_LOS(rowIndex,2));
|
||||
string = char(K_LOSY(rowIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
@ -547,6 +547,25 @@ if exist('SH_MAGS','var')
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_MAGS = zeros(%d,1);\n',numel(SK_MAGS));
|
||||
for rowIndex = 1:numel(SK_MAGS)
|
||||
string = char(SK_MAGS(rowIndex,1));
|
||||
fprintf(fid,'SK_MAGS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_MAGS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_MAGS(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
end
|
||||
|
||||
%% Write equations for X accel fusion
|
||||
|
||||
92
matlab/scripts/Inertial Nav EKF/fix_c_code.m
Normal file
92
matlab/scripts/Inertial Nav EKF/fix_c_code.m
Normal file
@ -0,0 +1,92 @@
|
||||
function fix_c_code(fileName)
|
||||
%% Initialize variables
|
||||
delimiter = '';
|
||||
|
||||
%% Format string for each line of text:
|
||||
% column1: text (%s)
|
||||
% For more information, see the TEXTSCAN documentation.
|
||||
formatSpec = '%s%[^\n\r]';
|
||||
|
||||
%% Open the text file.
|
||||
fileID = fopen(fileName,'r');
|
||||
|
||||
%% Read columns of data according to format string.
|
||||
% This call is based on the structure of the file used to generate this
|
||||
% code. If an error occurs for a different file, try regenerating the code
|
||||
% from the Import Tool.
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
|
||||
|
||||
%% Close the text file.
|
||||
fclose(fileID);
|
||||
|
||||
%% Create output variable
|
||||
SymbolicOutput = [dataArray{1:end-1}];
|
||||
|
||||
%% Clear temporary variables
|
||||
clearvars filename delimiter formatSpec fileID dataArray ans;
|
||||
|
||||
%% replace brackets and commas
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '(');
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ',');
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')');
|
||||
end
|
||||
|
||||
%% Convert indexing and replace brackets
|
||||
|
||||
% replace 1-D indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf('[%d]',(arrayIndex-1));
|
||||
strPat = strcat('\(',strIndex,'\)');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace 2-D left indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf('[%d,',(arrayIndex-1));
|
||||
strPat = strcat('\(',strIndex,'\,');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace 2-D right indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf(',%d]',(arrayIndex-1));
|
||||
strPat = strcat('\,',strIndex,'\)');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace commas
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')};
|
||||
end
|
||||
|
||||
%% Change covariance matrix variable name to P
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
strIn = char(SymbolicOutput(lineIndex));
|
||||
strIn = regexprep(strIn,'OP\[','P[');
|
||||
SymbolicOutput(lineIndex) = cellstr(strIn);
|
||||
end
|
||||
|
||||
%% Write to file
|
||||
fid = fopen(fileName,'wt');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
fprintf(fid,char(SymbolicOutput(lineIndex)));
|
||||
fprintf(fid,'\n');
|
||||
end
|
||||
fclose(fid);
|
||||
clear all;
|
||||
|
||||
end
|
||||
Loading…
x
Reference in New Issue
Block a user