/* * Fuse angular motion compensated optical flow rates using explicit algebraic equations generated with Matlab symbolic toolbox. * The script file used to generate these and other equations in this filter can be found here: * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m * Requires a valid terrain height estimate. */ void NavEKF2_core::FuseOptFlow() { Vector24 H_LOS; Vector3f relVelSensor; Vector14 SH_LOS; Vector2 losPred; // Copy required states to local variable names float q0 = stateStruct.quat[0]; float q1 = stateStruct.quat[1]; float q2 = stateStruct.quat[2]; float q3 = stateStruct.quat[3]; float vn = stateStruct.velocity.x; float ve = stateStruct.velocity.y; float vd = stateStruct.velocity.z; float pd = stateStruct.position.z; // constrain height above ground to be above range measured on ground float heightAboveGndEst = max((terrainState - pd), rngOnGnd); float ptd = pd + heightAboveGndEst; // Calculate common expressions for observation jacobians 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]); // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first // calculate range from ground plain to centre of sensor fov assuming flat earth float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),rngOnGnd,1000.0f); // calculate relative velocity in sensor frame relVelSensor = Tnb_flow*stateStruct.velocity; // divide velocity by range to get predicted angular LOS rates relative to X and Y axes losPred[0] = relVelSensor.y/range; losPred[1] = -relVelSensor.x/range; // calculate observation jacobians and Kalman gains memset(&H_LOS[0], 0, sizeof(H_LOS)); if (obsIndex == 0) { 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]; 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; float t62 = 1.0f/t61; // calculate innovation variance for X axis observation and protect against a badly conditioned calculation if (t61 > R_LOS) { t62 = 1.0f/t61; } else { t61 = 0.0f; t62 = 1.0f/R_LOS; } varInnovOptFlow[0] = t61; // calculate innovation for X axis observation innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x; // calculate Kalman gains for X-axis observation 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); if (!inhibitWindStates) { 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); } else { Kfusion[22] = 0.0f; Kfusion[23] = 0.0f; } if (!inhibitMagStates) { 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); } else { for (uint8_t i = 16; i <= 21; i++) { Kfusion[i] = 0.0f; } } } else { 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]; 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; float t62 = 1.0f/t61; // calculate innovation variance for X axis observation and protect against a badly conditioned calculation if (t61 > R_LOS) { t62 = 1.0f/t61; } else { t61 = 0.0f; t62 = 1.0f/R_LOS; } varInnovOptFlow[1] = t61; // calculate innovation for Y observation innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y; // calculate Kalman gains for the Y-axis observation 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); if (!inhibitWindStates) { 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); } else { Kfusion[22] = 0.0f; Kfusion[23] = 0.0f; } if (!inhibitMagStates) { 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); } else { for (uint8_t i = 16; i <= 21; i++) { Kfusion[i] = 0.0f; } } } // calculate the innovation consistency test ratio flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(frontend._flowInnovGate) * varInnovOptFlow[obsIndex]); // Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend._maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend._maxFlowRate)) { // record the last time observations were accepted for fusion prevFlowFuseTime_ms = imuSampleTime_ms; // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion stateStruct.angErr.zero(); // correct the state vector for (uint8_t j= 0; j<=stateIndexLim; j++) { statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex]; } // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion on the current time step stateStruct.quat.rotate(stateStruct.angErr); // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations for (unsigned i = 0; i<=stateIndexLim; i++) { for (unsigned j = 0; j<=5; j++) { KH[i][j] = Kfusion[i] * H_LOS[j]; } for (unsigned j = 6; j<=7; j++) { KH[i][j] = 0.0f; } KH[i][8] = Kfusion[i] * H_LOS[8]; for (unsigned j = 9; j<=23; j++) { KH[i][j] = 0.0f; } } for (unsigned j = 0; j<=stateIndexLim; j++) { for (unsigned i = 0; i<=stateIndexLim; i++) { ftype res = 0; res += KH[i][0] * P[0][j]; res += KH[i][1] * P[1][j]; res += KH[i][2] * P[2][j]; res += KH[i][3] * P[3][j]; res += KH[i][4] * P[4][j]; res += KH[i][5] * P[5][j]; res += KH[i][8] * P[8][j]; KHP[i][j] = res; } } for (unsigned i = 0; i<=stateIndexLim; i++) { for (unsigned j = 0; j<=stateIndexLim; j++) { P[i][j] = P[i][j] - KHP[i][j]; } } } // fix basic numerical errors ForceSymmetry(); ConstrainVariances(); } }
// fuse selected position, velocity and height measurements void NavEKF2_core::FuseVelPosNED() { // start performance timer hal.util->perf_begin(_perf_FuseVelPosNED); // health is set bad until test passed velHealth = false; posHealth = false; hgtHealth = false; // declare variables used to check measurement errors Vector3f velInnov; // declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; // declare variables used by state and covariance update calculations float posErr; Vector6 R_OBS; // Measurement variances used for fusion Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only Vector6 observation; float SK; // perform sequential fusion of GPS measurements. This assumes that the // errors in the different velocity and position components are // uncorrelated which is not true, however in the absence of covariance // data from the GPS receiver it is the only assumption we can make // so we might as well take advantage of the computational efficiencies // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { // set the GPS data timeout depending on whether airspeed data is present uint32_t gpsRetryTime; if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms; else gpsRetryTime = frontend->gpsRetryTimeNoTAS_ms; // form the observation vector observation[0] = gpsDataDelayed.vel.x; observation[1] = gpsDataDelayed.vel.y; observation[2] = gpsDataDelayed.vel.z; observation[3] = gpsDataDelayed.pos.x; observation[4] = gpsDataDelayed.pos.y; observation[5] = -hgtMea; // calculate additional error in GPS position caused by manoeuvring posErr = frontend->gpsPosVarAccScale * accNavMag; // estimate the GPS Velocity, GPS horiz position and height measurement variances. // if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity // otherwise we scale it using manoeuvre acceleration // Use different errors if flying without GPS using synthetic position and velocity data if (PV_AidingMode == AID_NONE && inFlight) { // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use) // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres R_OBS[0] = sq(10.0f); R_OBS[1] = R_OBS[0]; R_OBS[2] = R_OBS[0]; // Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres R_OBS[3] = sq(25.0f); R_OBS[4] = R_OBS[3]; } else { if (gpsSpdAccuracy > 0.0f) { // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f)); R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f)); } else { // calculate additional error in GPS velocity caused by manoeuvring R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag); } R_OBS[1] = R_OBS[0]; R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); R_OBS[4] = R_OBS[3]; } R_OBS[5] = posDownObsNoise; // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; // if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting // the accelerometers and we should disable the GPS and barometer innovation consistency checks. if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) { // calculate innovations for height and vertical GPS vel measurements float hgtErr = stateStruct.position.z - observation[5]; float velDErr = stateStruct.velocity.z - observation[2]; // check if they are the same sign and both more than 3-sigma out of bounds if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) { badIMUdata = true; } else { badIMUdata = false; } } // calculate innovations and check GPS data validity using an innovation consistency check // test position measurements if (fusePosData) { // test horizontal position measurements innovVelPos[3] = stateStruct.position.x - observation[3]; innovVelPos[4] = stateStruct.position.y - observation[4]; varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3]; varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4]; // apply an innovation consistency threshold test, but don't fail if bad IMU data float maxPosInnov2 = sq(max(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]); posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); // declare a timeout condition if we have been too long without data or not aiding posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE); // use position data if healthy, timed out, or in constant position mode if (posHealth || posTimeout || (PV_AidingMode == AID_NONE)) { posHealth = true; // only reset the failed time and do glitch timeout checks if we are doing full aiding if (PV_AidingMode == AID_ABSOLUTE) { lastPosPassTime_ms = imuSampleTime_ms; // if timed out or outside the specified uncertainty radius, reset to the GPS if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) { // reset the position to the current GPS position ResetPosition(); // reset the velocity to the GPS velocity ResetVelocity(); // don't fuse GPS data on this time step fusePosData = false; fuseVelData = false; // Reset the position variances and corresponding covariances to a value that will pass the checks zeroRows(P,6,7); zeroCols(P,6,7); P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax)); P[7][7] = P[6][6]; // Reset the normalised innovation to avoid failing the bad fusion tests posTestRatio = 0.0f; velTestRatio = 0.0f; } } } else { posHealth = false; } } // test velocity measurements if (fuseVelData) { // test velocity measurements uint8_t imax = 2; // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) { imax = 1; } float innovVelSumSq = 0; // sum of squares of velocity innovations float varVelSum = 0; // sum of velocity innovation variances for (uint8_t i = 0; i<=imax; i++) { // velocity states start at index 3 stateIndex = i + 3; // calculate innovations using blended and single IMU predicted states velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended // calculate innovation variance varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i]; // sum the innovation and innovation variances innovVelSumSq += sq(velInnov[i]); varVelSum += varInnovVelPos[i]; } // apply an innovation consistency threshold test, but don't fail if bad IMU data // calculate the test ratio velTestRatio = innovVelSumSq / (varVelSum * sq(max(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f))); // fail if the ratio is greater than 1 velHealth = ((velTestRatio < 1.0f) || badIMUdata); // declare a timeout if we have not fused velocity data for too long or not aiding velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE); // use velocity data if healthy, timed out, or in constant position mode if (velHealth || velTimeout) { velHealth = true; // restart the timeout count lastVelPassTime_ms = imuSampleTime_ms; // If we are doing full aiding and velocity fusion times out, reset to the GPS velocity if (PV_AidingMode == AID_ABSOLUTE && velTimeout) { // reset the velocity to the GPS velocity ResetVelocity(); // don't fuse GPS velocity data on this time step fuseVelData = false; // Reset the normalised innovation to avoid failing the bad fusion tests velTestRatio = 0.0f; } } else { velHealth = false; } } // test height measurements if (fuseHgtData) { // calculate height innovations innovVelPos[5] = stateStruct.position.z - observation[5]; varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio hgtTestRatio = sq(innovVelPos[5]) / (sq(max(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); // fail if the ratio is > 1, but don't fail if bad IMU data hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); // Fuse height data if healthy or timed out or in constant position mode if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) { // Calculate a filtered value to be used by pre-flight health checks // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution if (onGround) { float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f; const float hgtInnovFiltTC = 2.0f; float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f); hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha; } else { hgtInnovFiltState = 0.0f; } // if timed out, reset the height if (hgtTimeout) { ResetHeight(); hgtTimeout = false; } // If we have got this far then declare the height data as healthy and reset the timeout counter hgtHealth = true; lastHgtPassTime_ms = imuSampleTime_ms; } } // set range for sequential fusion of velocity and position measurements depending on which data is available and its health if (fuseVelData && velHealth) { fuseData[0] = true; fuseData[1] = true; if (useGpsVertVel) { fuseData[2] = true; } tiltErrVec.zero(); } if (fusePosData && posHealth) { fuseData[3] = true; fuseData[4] = true; tiltErrVec.zero(); } if (fuseHgtData && hgtHealth) { fuseData[5] = true; } // fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) { if (fuseData[obsIndex]) { stateIndex = 3 + obsIndex; // calculate the measurement innovation, using states from a different time coordinate if fusing height data // adjust scaling on GPS measurement noise variances if not enough satellites if (obsIndex <= 2) { innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 3 || obsIndex == 4) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 5) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; const float gndMaxBaroErr = 4.0f; const float gndBaroInnovFloor = -0.5f; if(getTouchdownExpected()) { // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr // this function looks like this: // |/ //---------|--------- // ____/| // / | // / | innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); } } // calculate the Kalman gain and calculate innovation variances varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; SK = 1.0f/varInnovVelPos[obsIndex]; for (uint8_t i= 0; i<=15; i++) { Kfusion[i] = P[i][stateIndex]*SK; } // inhibit magnetic field state estimation by setting Kalman gains to zero if (!inhibitMagStates) { for (uint8_t i = 16; i<=21; i++) { Kfusion[i] = P[i][stateIndex]*SK; } } else { for (uint8_t i = 16; i<=21; i++) { Kfusion[i] = 0.0f; } } // inhibit wind state estimation by setting Kalman gains to zero if (!inhibitWindStates) { Kfusion[22] = P[22][stateIndex]*SK; Kfusion[23] = P[23][stateIndex]*SK; } else { Kfusion[22] = 0.0f; Kfusion[23] = 0.0f; } // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion stateStruct.angErr.zero(); // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data for (uint8_t i = 0; i<=stateIndexLim; i++) { statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex]; } // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion stateStruct.quat.rotate(stateStruct.angErr); // sum the attitude error from velocity and position fusion only // used as a metric for convergence monitoring if (obsIndex != 5) { tiltErrVec += stateStruct.angErr; } // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations // this is a numerically optimised implementation of standard equation P = (I - K*H)*P; for (uint8_t i= 0; i<=stateIndexLim; i++) { for (uint8_t j= 0; j<=stateIndexLim; j++) { KHP[i][j] = Kfusion[i] * P[stateIndex][j]; } } for (uint8_t i= 0; i<=stateIndexLim; i++) { for (uint8_t j= 0; j<=stateIndexLim; j++) { P[i][j] = P[i][j] - KHP[i][j]; } } } } } // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. ForceSymmetry(); ConstrainVariances(); // stop performance timer hal.util->perf_end(_perf_FuseVelPosNED); }
void NavEKF3_core::FuseRngBcn() { // declarations float pn; float pe; float pd; float bcn_pn; float bcn_pe; float bcn_pd; const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); float rngPred; // health is set bad until test passed rngBcnHealth = false; if (activeHgtSource != HGT_SOURCE_BCN) { // calculate the vertical offset from EKF datum to beacon datum CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false); } else { bcnPosOffsetNED.z = 0.0f; } // copy required states to local variable names pn = stateStruct.position.x; pe = stateStruct.position.y; pd = stateStruct.position.z; bcn_pn = rngBcnDataDelayed.beacon_posNED.x; bcn_pe = rngBcnDataDelayed.beacon_posNED.y; bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z; // predicted range Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; rngPred = deltaPosNED.length(); // calculate measurement innovation innovRngBcn = rngPred - rngBcnDataDelayed.rng; // perform fusion of range measurement if (rngPred > 0.1f) { // calculate observation jacobians float H_BCN[24]; memset(H_BCN, 0, sizeof(H_BCN)); float t2 = bcn_pd-pd; float t3 = bcn_pe-pe; float t4 = bcn_pn-pn; float t5 = t2*t2; float t6 = t3*t3; float t7 = t4*t4; float t8 = t5+t6+t7; float t9 = 1.0f/sqrtf(t8); H_BCN[7] = -t4*t9; H_BCN[8] = -t3*t9; // If we are not using the beacons as a height reference, we pretend that the beacons // are at the same height as the flight vehicle when calculating the observation derivatives // and Kalman gains // TODO - less hacky way of achieving this, preferably using an alternative derivation if (activeHgtSource != HGT_SOURCE_BCN) { t2 = 0.0f; } H_BCN[9] = -t2*t9; // calculate Kalman gains float t10 = P[9][9]*t2*t9; float t11 = P[8][9]*t3*t9; float t12 = P[7][9]*t4*t9; float t13 = t10+t11+t12; float t14 = t2*t9*t13; float t15 = P[9][8]*t2*t9; float t16 = P[8][8]*t3*t9; float t17 = P[7][8]*t4*t9; float t18 = t15+t16+t17; float t19 = t3*t9*t18; float t20 = P[9][7]*t2*t9; float t21 = P[8][7]*t3*t9; float t22 = P[7][7]*t4*t9; float t23 = t20+t21+t22; float t24 = t4*t9*t23; varInnovRngBcn = R_BCN+t14+t19+t24; float t26; if (varInnovRngBcn >= R_BCN) { t26 = 1.0f/varInnovRngBcn; faultStatus.bad_rngbcn = false; } else { // the calculation is badly conditioned, so we cannot perform fusion on this step // we reset the covariance matrix and try again next measurement CovarianceInit(); faultStatus.bad_rngbcn = true; return; } Kfusion[0] = -t26*(P[0][7]*t4*t9+P[0][8]*t3*t9+P[0][9]*t2*t9); Kfusion[1] = -t26*(P[1][7]*t4*t9+P[1][8]*t3*t9+P[1][9]*t2*t9); Kfusion[2] = -t26*(P[2][7]*t4*t9+P[2][8]*t3*t9+P[2][9]*t2*t9); Kfusion[3] = -t26*(P[3][7]*t4*t9+P[3][8]*t3*t9+P[3][9]*t2*t9); Kfusion[4] = -t26*(P[4][7]*t4*t9+P[4][8]*t3*t9+P[4][9]*t2*t9); Kfusion[5] = -t26*(P[5][7]*t4*t9+P[5][8]*t3*t9+P[5][9]*t2*t9); Kfusion[7] = -t26*(t22+P[7][8]*t3*t9+P[7][9]*t2*t9); Kfusion[8] = -t26*(t16+P[8][7]*t4*t9+P[8][9]*t2*t9); if (!inhibitDelAngBiasStates) { Kfusion[10] = -t26*(P[10][7]*t4*t9+P[10][8]*t3*t9+P[10][9]*t2*t9); Kfusion[11] = -t26*(P[11][7]*t4*t9+P[11][8]*t3*t9+P[11][9]*t2*t9); Kfusion[12] = -t26*(P[12][7]*t4*t9+P[12][8]*t3*t9+P[12][9]*t2*t9); } else { // zero indexes 10 to 12 = 3*4 bytes memset(&Kfusion[10], 0, 12); } if (!inhibitDelVelBiasStates) { Kfusion[13] = -t26*(P[13][7]*t4*t9+P[13][8]*t3*t9+P[13][9]*t2*t9); Kfusion[14] = -t26*(P[14][7]*t4*t9+P[14][8]*t3*t9+P[14][9]*t2*t9); Kfusion[15] = -t26*(P[15][7]*t4*t9+P[15][8]*t3*t9+P[15][9]*t2*t9); } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); } // only allow the range observations to modify the vertical states if we are using it as a height reference if (activeHgtSource == HGT_SOURCE_BCN) { Kfusion[6] = -t26*(P[6][7]*t4*t9+P[6][8]*t3*t9+P[6][9]*t2*t9); Kfusion[9] = -t26*(t10+P[9][7]*t4*t9+P[9][8]*t3*t9); } else { Kfusion[6] = 0.0f; Kfusion[9] = 0.0f; } if (!inhibitMagStates) { Kfusion[16] = -t26*(P[16][7]*t4*t9+P[16][8]*t3*t9+P[16][9]*t2*t9); Kfusion[17] = -t26*(P[17][7]*t4*t9+P[17][8]*t3*t9+P[17][9]*t2*t9); Kfusion[18] = -t26*(P[18][7]*t4*t9+P[18][8]*t3*t9+P[18][9]*t2*t9); Kfusion[19] = -t26*(P[19][7]*t4*t9+P[19][8]*t3*t9+P[19][9]*t2*t9); Kfusion[20] = -t26*(P[20][7]*t4*t9+P[20][8]*t3*t9+P[20][9]*t2*t9); Kfusion[21] = -t26*(P[21][7]*t4*t9+P[21][8]*t3*t9+P[21][9]*t2*t9); } else { // zero indexes 16 to 21 = 6*4 bytes memset(&Kfusion[16], 0, 24); } if (!inhibitWindStates) { Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9); Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9); } else { // zero indexes 22 to 23 = 2*4 bytes memset(&Kfusion[22], 0, 8); } // Calculate innovation using the selected offset value Vector3f delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED; innovRngBcn = delta.length() - rngBcnDataDelayed.rng; // calculate the innovation consistency test ratio rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn); // fail if the ratio is > 1, but don't fail if bad IMU data rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata); // test the ratio before fusing data if (rngBcnHealth) { // restart the counter lastRngBcnPassTime_ms = imuSampleTime_ms; // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations for (unsigned i = 0; i<=stateIndexLim; i++) { for (unsigned j = 0; j<=6; j++) { KH[i][j] = 0.0f; } for (unsigned j = 7; j<=9; j++) { KH[i][j] = Kfusion[i] * H_BCN[j]; } for (unsigned j = 10; j<=23; j++) { KH[i][j] = 0.0f; } } for (unsigned j = 0; j<=stateIndexLim; j++) { for (unsigned i = 0; i<=stateIndexLim; i++) { ftype res = 0; res += KH[i][7] * P[7][j]; res += KH[i][8] * P[8][j]; res += KH[i][9] * P[9][j]; KHP[i][j] = res; } } // Check that we are not going to drive any variances negative and skip the update if so bool healthyFusion = true; for (uint8_t i= 0; i<=stateIndexLim; i++) { if (KHP[i][i] > P[i][i]) { healthyFusion = false; } } if (healthyFusion) { // update the covariance matrix for (uint8_t i= 0; i<=stateIndexLim; i++) { for (uint8_t j= 0; j<=stateIndexLim; j++) { P[i][j] = P[i][j] - KHP[i][j]; } } // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); // correct the state vector for (uint8_t j= 0; j<=stateIndexLim; j++) { statesArray[j] = statesArray[j] - Kfusion[j] * innovRngBcn; } // record healthy fusion faultStatus.bad_rngbcn = false; } else { // record bad fusion faultStatus.bad_rngbcn = true; } } // Update the fusion report rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED; rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn; rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn; rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng; rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio; } }