zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
Paul Riseborough 9 years ago
parent
ffebaf384f
commit
e9ccfdd484
7 changed files with 488 additions and 489 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 281
      matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt
  2. 319
      matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt
  3. 2
      matlab/scripts/Inertial Nav EKF/ConvertToC.m
  4. 2
      matlab/scripts/Inertial Nav EKF/ConvertToM.m
  5. 258
      matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
  6. 23
      matlab/scripts/Inertial Nav EKF/SaveScriptCode.m
  7. 92
      matlab/scripts/Inertial Nav EKF/fix_c_code.m

281
matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt
Unescape 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);

319
matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt
Unescape View File

@ -3,114 +3,219 @@
// Observations are body modtion compensated optica flow rates about the X and Y body axis // 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) // Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated)
// intermediate variable from algebraic optimisation float H_LOS[2][24];
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;
// Calculate the observation jacobians for the LOS rate about the X body axis // calculate X axis observation Jacobian
float H_LOS[24]; float t2 = 1.0f / range;
H_LOS[0] = SH_LOS[2]*SH_LOS[3]*SH_LOS[6] - SH_LOS[0]*SH_LOS[3]*SH_LOS[4]; float t3 = q0 * q0;
H_LOS[1] = SH_LOS[2]*SH_LOS[3]*SH_LOS[5]; float t4 = q1 * q1;
H_LOS[2] = SH_LOS[0]*SH_LOS[1]*SH_LOS[3]; float t5 = q2 * q2;
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2); float t6 = q3 * q3;
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); float t7 = q0 * q2 * 2.0f;
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[6]; float t8 = q1 * q3 * 2.0f;
H_LOS[8] = SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]); 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 // calculate X axis Kalman gains
float H_LOS[24]; t2 = 1.0/range;
H_LOS[0] = -SH_LOS[1]*SH_LOS[3]*SH_LOS[6]; t3 = q0*q1*2.0;
H_LOS[1] = - SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[1]*SH_LOS[3]*SH_LOS[5]; t4 = q2*q3*2.0;
H_LOS[2] = SH_LOS[0]*SH_LOS[2]*SH_LOS[3]; t5 = q0*q0;
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)); t6 = q1*q1;
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2); t7 = q2*q2;
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[5]; t8 = q3*q3;
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]); 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 // calculate Y axis Kalman gains
float SK_LOS[22]; t2 = 1.0f/range;
// this is 1/(innovation variance) for the X axis measurement t3 = q0*q2*2.0f;
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)))); t4 = q0*q0;
// this is 1/(innovation variance) for the Y axis measurement t5 = q1*q1;
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)))); t6 = q2*q2;
SK_LOS[2] = sq(q0) + sq(q1) - sq(q2) - sq(q3); t7 = q3*q3;
SK_LOS[3] = sq(q0) - sq(q1) + sq(q2) - sq(q3); t8 = q0*q1*2.0f;
SK_LOS[4] = SH_LOS[3]; t9 = q0*q3*2.0f;
SK_LOS[5] = SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]); t10 = q1*q2*2.0f;
SK_LOS[6] = SH_LOS[0]*SH_LOS[4]*SK_LOS[4]; float t11 = t9+t10;
SK_LOS[7] = SH_LOS[2]*SH_LOS[6]*SK_LOS[4]; float t12 = q1*q3*2.0f;
SK_LOS[8] = SH_LOS[0]*SK_LOS[4]*(2.0f*q0*q3 - 2.0f*q1*q2); float t13 = t4-t5-t6+t7;
SK_LOS[9] = SH_LOS[0]*SH_LOS[1]*SK_LOS[4]; float t14 = t13*vd;
SK_LOS[10] = SH_LOS[2]*SH_LOS[5]*SK_LOS[4]; float t15 = q2*q3*2.0f;
SK_LOS[11] = SH_LOS[0]*SH_LOS[6]*SK_LOS[4]; float t16 = t3+t12;
SK_LOS[12] = SH_LOS[0]*SK_LOS[3]*SK_LOS[4]; float t17 = t16*vn;
SK_LOS[13] = SH_LOS[1]*SH_LOS[5]*SK_LOS[4]; float t18 = t4-t5+t6-t7;
SK_LOS[14] = SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]); float t19 = t18*ve;
SK_LOS[15] = SH_LOS[0]*SK_LOS[4]*(2.0f*q0*q3 + 2.0f*q1*q2); float t20 = t8+t15;
SK_LOS[16] = SH_LOS[0]*SH_LOS[2]*SK_LOS[4]; float t21 = t20*vd;
SK_LOS[17] = SH_LOS[1]*SH_LOS[6]*SK_LOS[4]; float t22 = t9-t10;
SK_LOS[18] = SH_LOS[0]*SH_LOS[5]*SK_LOS[4]; float t28 = t22*vn;
SK_LOS[19] = SH_LOS[0]*SK_LOS[2]*SK_LOS[4]; float t23 = t19+t21-t28;
SK_LOS[20] = SK_LOS[6] - SK_LOS[7]; float t24 = t4+t5-t6-t7;
SK_LOS[21] = SK_LOS[6] + SK_LOS[13]; float t25 = t3-t12;
float t26 = t8-t15;
// Calculate the Kalman gain matrix for the X axis measurement float t29 = t26*ve;
float Kfusion[24]; float t27 = t14+t17-t29;
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]); float t30 = P[4][4]*t2*t11;
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]); float t31 = P[2][4]*t2*t23;
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]); float t32 = P[3][4]*t2*t24;
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]); float t56 = P[5][4]*t2*t25;
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]); float t57 = P[1][4]*t2*t27;
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]); float t33 = t30+t31+t32-t56-t57;
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]); float t34 = t2*t11*t33;
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]); float t35 = P[4][5]*t2*t11;
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]); float t36 = P[2][5]*t2*t23;
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]); float t37 = P[3][5]*t2*t24;
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]); float t58 = P[5][5]*t2*t25;
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]); float t59 = P[1][5]*t2*t27;
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]); float t38 = t35+t36+t37-t58-t59;
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]); float t39 = P[4][1]*t2*t11;
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]); float t40 = P[2][1]*t2*t23;
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]); float t41 = P[3][1]*t2*t24;
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]); float t55 = P[1][1]*t2*t27;
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]); float t61 = P[5][1]*t2*t25;
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]); float t42 = t39+t40+t41-t55-t61;
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]); float t43 = P[4][2]*t2*t11;
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]); float t44 = P[2][2]*t2*t23;
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]); float t45 = P[3][2]*t2*t24;
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]); float t63 = P[5][2]*t2*t25;
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]); float t64 = P[1][2]*t2*t27;
float t46 = t43+t44+t45-t63-t64;
// Calculate the Kalman gain matrix for the Y axis measurement float t47 = t2*t23*t46;
float Kfusion[24]; float t48 = P[4][3]*t2*t11;
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]); float t49 = P[2][3]*t2*t23;
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]); float t50 = P[3][3]*t2*t24;
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]); float t65 = P[5][3]*t2*t25;
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]); float t66 = P[1][3]*t2*t27;
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]); float t51 = t48+t49+t50-t65-t66;
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]); float t52 = t2*t24*t51;
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]); float t60 = t2*t25*t38;
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]); float t62 = t2*t27*t42;
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]); float t53 = R_LOS+t34+t47+t52-t60-t62;
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]); float t54 = 1.0f/t53;
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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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]); 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);

2
matlab/scripts/Inertial Nav EKF/ConvertToC.m
Unescape View File

@ -15,7 +15,7 @@ fileID = fopen(fileName,'r');
% This call is based on the structure of the file used to generate this % 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 % code. If an error occurs for a different file, try regenerating the code
% from the Import Tool. % 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. %% Close the text file.
fclose(fileID); fclose(fileID);

2
matlab/scripts/Inertial Nav EKF/ConvertToM.m
Unescape View File

@ -15,7 +15,7 @@ fileID = fopen(fileName,'r');
% This call is based on the structure of the file used to generate this % 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 % code. If an error occurs for a different file, try regenerating the code
% from the Import Tool. % 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. %% Close the text file.
fclose(fileID); fclose(fileID);

258
matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
Unescape View File

@ -1,13 +1,13 @@
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run % 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 % XYZ body fixed frame
% Sequential fusion of velocity and position measurements % Sequential fusion of velocity and position measurements
% Fusion of true airspeed % Fusion of true airspeed
% Sequential fusion of magnetic flux measurements % Sequential fusion of magnetic flux measurements
% 24 state architecture. % 24 state architecture.
% IMU data is assumed to arrive at a constant rate with a time step of dt % 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 % not observations
% Author: Paul Riseborough % Author: Paul Riseborough
@ -15,8 +15,8 @@
% Based on use of a rotation vector for attitude estimation as described % Based on use of a rotation vector for attitude estimation as described
% here: % here:
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation", % Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003), % Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
% pp. 855-860. % pp. 855-860.
% State vector: % State vector:
@ -52,7 +52,7 @@ syms vn ve vd real % NED velocity - m/sec
syms pn pe pd real % NED position - m syms pn pe pd real % NED position - m
syms dax_b day_b daz_b real % delta angle bias - rad 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 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 dt real % IMU time step - sec
syms gravity real % gravity - m/sec^2 syms gravity real % gravity - m/sec^2
syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real; % IMU delta angle and delta velocity measurement noise 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 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 rho real % air density (kg/m^3)
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2 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 % define the measured Delta angle and delta velocity vectors
dAngMeas = [dax; day; daz]; dAngMeas = [dax; day; daz];
@ -101,7 +101,7 @@ truthQuat = QuatMult(estQuat, errQuat);
Tbn = Quat2Tbn(truthQuat); Tbn = Quat2Tbn(truthQuat);
% define the truth delta angle % 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 % covariance growth for our application and grade of sensor
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise]; dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise];
@ -157,7 +157,27 @@ nStates=numel(stateVector);
% Define vector of process equations % Define vector of process equations
newStateVector = [errRotNew;vNew;pNew;dabNew;dasNew;dvbNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew]; 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 % This reduces the number of floating point operations by a factor of 6 or
% more compared to using the standard matrix operations in code % 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 % remove the disturbance noise from the process equations as it is only
% needed when calculating the disturbance influence matrix % needed when calculating the disturbance influence matrix
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0); 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},0); errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {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
% Derive the predicted covariance matrix using the standard equation % Derive the predicted covariance matrix using the standard equation
PP = F*P*transpose(F) + Q; PP = F*P*transpose(F) + Q;
@ -206,14 +208,27 @@ PP = F*P*transpose(F) + Q;
% Collect common expressions to optimise processing % Collect common expressions to optimise processing
[PP,SPP]=OptimiseAlgebra(PP,'SPP'); [PP,SPP]=OptimiseAlgebra(PP,'SPP');
save('StateAndCovariancePrediction.mat');
clear all;
reset(symengine);
%% derive equations for fusion of true airspeed measurements %% derive equations for fusion of true airspeed measurements
load('StatePrediction.mat');
VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing [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 %% derive equations for fusion of angle of sideslip measurements
load('StatePrediction.mat');
% calculate wind relative velocities in nav frame and rotate into body frame % calculate wind relative velocities in nav frame and rotate into body frame
Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd]; Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd];
% calculate predicted angle of sideslip using small angle assumption % 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 [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 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 %% derive equations for fusion of magnetic field measurement
load('StatePrediction.mat');
magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); 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 = (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'); [K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');
% 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 %% 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 % calculate relative velocity in body frame
relVelBody = transpose(Tbn)*[vn;ve;vd]; relVelBody = transpose(Tbn)*[vn;ve;vd];
% divide by range to get predicted angular LOS rates relative to X and Y % 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 % axes. Note these are body angular rate motion compensated optical flow rates
losRateX = +relVelBody(2)/range; losRateX = +relVelBody(2)/range;
losRateY = -relVelBody(1)/range; losRateY = -relVelBody(1)/range;
H_LOS = jacobian([losRateX;losRateY],stateVector); % measurement Jacobian save('temp1.mat','losRateX','losRateY');
H_LOS = subs(H_LOS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_LOS = simplify(H_LOS); % calculate the observation Jacobian for the X axis
[H_LOS,SH_LOS] = OptimiseAlgebra(H_LOS,'SH_LOS'); 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');
% combine into a single K matrix to enable common expressions to be found clear all;
% note this matrix cannot be used in a single step fusion reset(symengine);
K_LOSX = (P*transpose(H_LOS(1,:)))/(H_LOS(1,:)*P*transpose(H_LOS(1,:)) + R_LOS); % Kalman gain vector 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_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);
K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); ccode(K_LOSX,'file','K_LOSX.c');
K_LOS = [K_LOSX,K_LOSY]; fix_c_code('K_LOSX.c');
simplify(K_LOS);
[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS');
% Use matlab c code converter for an alternate method of clear all;
ccode(H_LOS,'file','H_LOS.txt'); reset(symengine);
ccode(K_LOSX,'file','K_LOSX.txt'); load('StatePrediction.mat');
ccode(K_LOSY,'file','K_LOSY.txt'); load('temp1.mat');
load('temp3.mat');
%% derive equations for simple fusion of 2-D magnetic heading measurements % 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_LOSY = simplify(K_LOSY);
ccode(K_LOSY,'file','K_LOSY.c');
fix_c_code('K_LOSY.c');
% rotate magnetic field into earth axes % reset workspace
magMeasNED = Tbn*[magX;magY;magZ]; clear all;
% the predicted measurement is the angle wrt true north of the horizontal reset(symengine);
% 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');
%% derive equations for fusion of 321 sequence yaw measurement %% derive equations for fusion of 321 sequence yaw measurement
load('StatePrediction.mat');
% Calculate the yaw (first rotation) angle from the 321 rotation sequence % Calculate the yaw (first rotation) angle from the 321 rotation sequence
angMeas = atan(Tbn(2,1)/Tbn(1,1)); angMeas = atan(Tbn(2,1)/Tbn(1,1));
H_YAW = jacobian(angMeas,stateVector); % measurement Jacobian H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian
H_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_YAW321 = subs(H_YAW321, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
ccode(H_YAW,'file','calcH_YAW321.c'); H_YAW321 = simplify(H_YAW321);
% Calculate Kalman gain vector ccode(H_YAW321,'file','calcH_YAW321.c');
K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW); fix_c_code('calcH_YAW321.c');
ccode([K_YAW;H_YAW'],'file','calcYAW321.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of 312 sequence yaw measurement %% derive equations for fusion of 312 sequence yaw measurement
load('StatePrediction.mat');
% Calculate the yaw (first rotation) angle from an Euler 312 sequence % Calculate the yaw (first rotation) angle from an Euler 312 sequence
angMeas = atan(-Tbn(1,2)/Tbn(2,2)); angMeas = atan(-Tbn(1,2)/Tbn(2,2));
H_YAW2 = jacobian(angMeas,stateVector); % measurement Jacobianclea H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea
H_YAW2 = subs(H_YAW2, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_YAW312 = subs(H_YAW312, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
ccode(H_YAW2,'file','calcH_YAW312.c'); H_YAW312 = simplify(H_YAW312);
% Calculate Kalman gain vector ccode(H_YAW312,'file','calcH_YAW312.c');
K_YAW2 = (P*transpose(H_YAW2))/(H_YAW2*P*transpose(H_YAW2) + R_YAW); fix_c_code('calcH_YAW312.c');
ccode([K_YAW2;H_YAW2'],'file','calcYAW312.c');
% reset workspace
%% derive equations for fusion of synthetic deviation measurement clear all;
% used to keep correct heading when operating without absolute position or reset(symengine);
% velocity measurements - eg when using optical flow
% rotate magnetic field into earth axes %% derive equations for fusion of declination
magMeasNED = [magN;magE;magD]; load('StatePrediction.mat');
% the predicted measurement is the angle wrt magnetic north of the horizontal % the predicted measurement is the angle wrt magnetic north of the horizontal
% component of the measured field % component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1)); angMeas = atan(magE/magN);
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_MAGD = simplify(H_MAGD); 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); 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) %% 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 % use relationship between airspeed along the X and Y body axis and the
% drag to predict the lateral acceleration for a multirotor vehicle type % 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 % 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 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 % speed range
% The nonlinear equation will be used to calculate the predicted % The nonlinear equation will be used to calculate the predicted
% measurement in implementation % measurement in implementation
accXpred = -Kacc*vrel(1); % predicted acceleration measured along X body axis accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis
accYpred = -Kacc*vrel(2); % predicted acceleration measured along Y body axis accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis
% Derive observation Jacobian and Kalman gain matrix for X accel fusion % Derive observation Jacobian and Kalman gain matrix for X accel fusion
H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
H_ACCX = subs(H_ACCX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); 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 [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); 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 [K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector
% Derive observation Jacobian and Kalman gain matrix for Y accel fusion % Derive observation Jacobian and Kalman gain matrix for Y accel fusion
H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); 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 [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); 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 [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 %% 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'); fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName); save(fileName);
SaveScriptCode(nStates); SaveScriptCode(nStates);
ConvertToM(nStates); ConvertToM(nStates);
ConvertToC(nStates); ConvertToC(nStates);

23
matlab/scripts/Inertial Nav EKF/SaveScriptCode.m
Unescape View File

@ -409,7 +409,7 @@ if exist('SH_LOS','var')
fprintf(fid,'\n'); fprintf(fid,'\n');
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
for rowIndex = 1:nRow for rowIndex = 1:nRow
string = char(K_LOS(rowIndex,1)); string = char(K_LOSX(rowIndex));
% don't write out a zero-assignment % don't write out a zero-assignment
if ~strcmpi(string,'0') if ~strcmpi(string,'0')
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
@ -421,7 +421,7 @@ if exist('SH_LOS','var')
fprintf(fid,'\n'); fprintf(fid,'\n');
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
for rowIndex = 1:nRow for rowIndex = 1:nRow
string = char(K_LOS(rowIndex,2)); string = char(K_LOSY(rowIndex));
% don't write out a zero-assignment % don't write out a zero-assignment
if ~strcmpi(string,'0') if ~strcmpi(string,'0')
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
@ -547,6 +547,25 @@ if exist('SH_MAGS','var')
end end
fprintf(fid,'\n'); 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 end
%% Write equations for X accel fusion %% Write equations for X accel fusion

92
matlab/scripts/Inertial Nav EKF/fix_c_code.m
Unescape View 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
Write Preview
Loading…
Cancel
Save