diff --git a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt b/matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt deleted file mode 100644 index fbe105bf32..0000000000 --- a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt +++ /dev/null @@ -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); diff --git a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt b/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt index 5320af7b77..c1f76bf491 100644 --- a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt @@ -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); diff --git a/matlab/scripts/Inertial Nav EKF/ConvertToC.m b/matlab/scripts/Inertial Nav EKF/ConvertToC.m index 7c18441859..23176f36ca 100644 --- a/matlab/scripts/Inertial Nav EKF/ConvertToC.m +++ b/matlab/scripts/Inertial Nav EKF/ConvertToC.m @@ -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); diff --git a/matlab/scripts/Inertial Nav EKF/ConvertToM.m b/matlab/scripts/Inertial Nav EKF/ConvertToM.m index 1e5f7f4388..a9b0489de7 100644 --- a/matlab/scripts/Inertial Nav EKF/ConvertToM.m +++ b/matlab/scripts/Inertial Nav EKF/ConvertToM.m @@ -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); diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m index 44d0eeeb40..99741cb303 100644 --- a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m +++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m @@ -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); \ No newline at end of file diff --git a/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m b/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m index a1f755136c..b86f59eb04 100644 --- a/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m +++ b/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m @@ -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 diff --git a/matlab/scripts/Inertial Nav EKF/fix_c_code.m b/matlab/scripts/Inertial Nav EKF/fix_c_code.m new file mode 100644 index 0000000000..0eb2874843 --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/fix_c_code.m @@ -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 \ No newline at end of file