diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index abdd2d1c3c..d5d33f09b1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1057,7 +1057,7 @@ void NavEKF3::updateCoreRelativeErrors() // reduce error for a core only if its better than the primary lane by at least the Relative Error Threshold, this should prevent unnecessary lane changes if (error > 0 || error < -MAX(_err_thresh, 0.05)) { coreRelativeErrors[i] += error; - coreRelativeErrors[i] = constrain_float(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM); + coreRelativeErrors[i] = constrain_ftype(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM); } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 13d6e94aea..b414893158 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -20,15 +20,15 @@ void NavEKF3_core::FuseAirspeed() { // declarations - float vn; - float ve; - float vd; - float vwn; - float vwe; - float SH_TAS[3]; - float SK_TAS[2]; + ftype vn; + ftype ve; + ftype vd; + ftype vwn; + ftype vwe; + ftype SH_TAS[3]; + ftype SK_TAS[2]; Vector24 H_TAS = {}; - float VtasPred; + ftype VtasPred; // copy required states to local variable names vn = stateStruct.velocity.x; @@ -55,7 +55,7 @@ void NavEKF3_core::FuseAirspeed() H_TAS[22] = -SH_TAS[2]; H_TAS[23] = -SH_TAS[1]; // calculate Kalman gains - float temp = (tasErrVar + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + ftype temp = (tasErrVar + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); if (temp >= tasErrVar) { SK_TAS[0] = 1.0f / temp; faultStatus.bad_airspeed = false; @@ -80,8 +80,8 @@ void NavEKF3_core::FuseAirspeed() Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); } else { - // zero indexes 0 to 9 = 10*4 bytes - memset(&Kfusion[0], 0, 40); + // zero indexes 0 to 9 + zero_range(&Kfusion[0], 0, 9); } if (!inhibitDelAngBiasStates && !airDataFusionWindOnly) { @@ -89,8 +89,8 @@ void NavEKF3_core::FuseAirspeed() Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) { @@ -103,8 +103,8 @@ void NavEKF3_core::FuseAirspeed() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } // zero Kalman gains to inhibit magnetic field state estimation @@ -116,23 +116,23 @@ void NavEKF3_core::FuseAirspeed() Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } if (!inhibitWindStates) { Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 = 2 + zero_range(&Kfusion[0], 22, 23); } // calculate measurement innovation variance varInnovVtas = 1.0f/SK_TAS[0]; // calculate the innovation consistency test ratio - tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); + tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); // fail if the ratio is > 1, but don't fail if bad IMU data bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); @@ -276,19 +276,19 @@ void NavEKF3_core::SelectBetaDragFusion() void NavEKF3_core::FuseSideslip() { // declarations - float q0; - float q1; - float q2; - float q3; - float vn; - float ve; - float vd; - float vwn; - float vwe; - const float R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg + ftype q0; + ftype q1; + ftype q2; + ftype q3; + ftype vn; + ftype ve; + ftype vd; + ftype vwn; + ftype vwe; + const ftype R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg Vector13 SH_BETA; Vector8 SK_BETA; - Vector3f vel_rel_wind; + Vector3F vel_rel_wind; Vector24 H_BETA; // copy required states to local variable names @@ -315,7 +315,7 @@ void NavEKF3_core::FuseSideslip() { // Calculate observation jacobians SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); - if (fabsf(SH_BETA[0]) <= 1e-9f) { + if (fabsF(SH_BETA[0]) <= 1e-9f) { faultStatus.bad_sideslip = true; return; } else { @@ -348,7 +348,7 @@ void NavEKF3_core::FuseSideslip() H_BETA[23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6]; // Calculate Kalman gains - float temp = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3)))); + ftype temp = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3)))); if (temp >= R_BETA) { SK_BETA[0] = 1.0f / temp; faultStatus.bad_sideslip = false; @@ -379,8 +379,8 @@ void NavEKF3_core::FuseSideslip() Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]); Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]); } else { - // zero indexes 0 to 9 = 10*4 bytes - memset(&Kfusion[0], 0, 40); + // zero indexes 0 to 9 + zero_range(&Kfusion[0], 0, 9); } if (!inhibitDelAngBiasStates && !airDataFusionWindOnly) { @@ -388,8 +388,8 @@ void NavEKF3_core::FuseSideslip() Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]); Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 = 3 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) { @@ -402,8 +402,8 @@ void NavEKF3_core::FuseSideslip() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } // zero Kalman gains to inhibit magnetic field state estimation @@ -415,16 +415,16 @@ void NavEKF3_core::FuseSideslip() Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]); Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } if (!inhibitWindStates) { Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } // calculate predicted sideslip angle and innovation using small angle approximation @@ -491,68 +491,68 @@ void NavEKF3_core::FuseSideslip() void NavEKF3_core::FuseDragForces() { // drag model parameters - const float bcoef_x = frontend->_ballisticCoef_x; - const float bcoef_y = frontend->_ballisticCoef_y; - const float mcoef = frontend->_momentumDragCoef.get(); + const ftype bcoef_x = frontend->_ballisticCoef_x; + const ftype bcoef_y = frontend->_ballisticCoef_y; + const ftype mcoef = frontend->_momentumDragCoef.get(); const bool using_bcoef_x = bcoef_x > 1.0f; const bool using_bcoef_y = bcoef_y > 1.0f; const bool using_mcoef = mcoef > 0.001f; - memset (&Kfusion, 0, sizeof(Kfusion)); + ZERO_FARRAY(Kfusion); Vector24 Hfusion; // Observation Jacobians - const float R_ACC = sq(fmaxf(frontend->_dragObsNoise, 0.5f)); - const float density_ratio = sqrtf(dal.get_EAS2TAS()); - const float rho = fmaxf(1.225f * density_ratio, 0.1f); // air density + const ftype R_ACC = sq(fmaxF(frontend->_dragObsNoise, 0.5f)); + const ftype density_ratio = sqrtF(dal.get_EAS2TAS()); + const ftype rho = fmaxF(1.225f * density_ratio, 0.1f); // air density // get latest estimated orientation - const float &q0 = stateStruct.quat[0]; - const float &q1 = stateStruct.quat[1]; - const float &q2 = stateStruct.quat[2]; - const float &q3 = stateStruct.quat[3]; + const ftype &q0 = stateStruct.quat[0]; + const ftype &q1 = stateStruct.quat[1]; + const ftype &q2 = stateStruct.quat[2]; + const ftype &q3 = stateStruct.quat[3]; // get latest velocity in earth frame - const float &vn = stateStruct.velocity.x; - const float &ve = stateStruct.velocity.y; - const float &vd = stateStruct.velocity.z; + const ftype &vn = stateStruct.velocity.x; + const ftype &ve = stateStruct.velocity.y; + const ftype &vd = stateStruct.velocity.z; // get latest wind velocity in earth frame - const float &vwn = stateStruct.wind_vel.x; - const float &vwe = stateStruct.wind_vel.y; + const ftype &vwn = stateStruct.wind_vel.x; + const ftype &vwe = stateStruct.wind_vel.y; // predicted specific forces // calculate relative wind velocity in earth frame and rotate into body frame - const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd); - const Vector3f rel_wind_body = prevTnb * rel_wind_earth; + const Vector3F rel_wind_earth(vn - vwn, ve - vwe, vd); + const Vector3F rel_wind_body = prevTnb * rel_wind_earth; // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { // correct accel data for bias - const float mea_acc = dragSampleDelayed.accelXY[axis_index] - stateStruct.accel_bias[axis_index] / dtEkfAvg; + const ftype mea_acc = dragSampleDelayed.accelXY[axis_index] - stateStruct.accel_bias[axis_index] / dtEkfAvg; // Acceleration in m/s/s predicfed using vehicle and wind velocity estimates // Initialised to measured value and updated later using available drag model - float predAccel = mea_acc; + ftype predAccel = mea_acc; // predicted sign of drag force - const float dragForceSign = is_positive(rel_wind_body[axis_index]) ? -1.0f : 1.0f; + const ftype dragForceSign = is_positive(rel_wind_body[axis_index]) ? -1.0f : 1.0f; if (axis_index == 0) { // drag can be modelled as an arbitrary combination of bluff body drag that proportional to // speed squared, and rotor momentum drag that is proportional to speed. - float Kacc; // Derivative of specific force wrt airspeed + ftype Kacc; // Derivative of specific force wrt airspeed if (using_mcoef && using_bcoef_x) { // mixed bluff body and propeller momentum drag - const float airSpd = (bcoef_x / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef_x) * fabsf(mea_acc))); - Kacc = fmaxf(1e-1f, (rho / bcoef_x) * airSpd + mcoef * density_ratio); + const ftype airSpd = (bcoef_x / rho) * (- mcoef + sqrtF(sq(mcoef) + 2.0f * (rho / bcoef_x) * fabsF(mea_acc))); + Kacc = fmaxF(1e-1f, (rho / bcoef_x) * airSpd + mcoef * density_ratio); predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio; } else if (using_mcoef) { // propeller momentum drag only - Kacc = fmaxf(1e-1f, mcoef * density_ratio); + Kacc = fmaxF(1e-1f, mcoef * density_ratio); predAccel = - rel_wind_body[0] * mcoef * density_ratio; } else if (using_bcoef_x) { // bluff body drag only - const float airSpd = sqrtf((2.0f * bcoef_x * fabsf(mea_acc)) / rho); - Kacc = fmaxf(1e-1f, (rho / bcoef_x) * airSpd); + const ftype airSpd = sqrtF((2.0f * bcoef_x * fabsF(mea_acc)) / rho); + Kacc = fmaxF(1e-1f, (rho / bcoef_x) * airSpd); predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign; } else { // skip this axis @@ -560,46 +560,46 @@ void NavEKF3_core::FuseDragForces() } // intermediate variables - const float HK0 = vn - vwn; - const float HK1 = ve - vwe; - const float HK2 = HK0*q0 + HK1*q3 - q2*vd; - const float HK3 = 2*Kacc; - const float HK4 = HK0*q1 + HK1*q2 + q3*vd; - const float HK5 = HK0*q2 - HK1*q1 + q0*vd; - const float HK6 = -HK0*q3 + HK1*q0 + q1*vd; - const float HK7 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); - const float HK8 = HK7*Kacc; - const float HK9 = q0*q3 + q1*q2; - const float HK10 = HK3*HK9; - const float HK11 = q0*q2 - q1*q3; - const float HK12 = 2*HK9; - const float HK13 = 2*HK11; - const float HK14 = 2*HK4; - const float HK15 = 2*HK2; - const float HK16 = 2*HK5; - const float HK17 = 2*HK6; - const float HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4]; - const float HK19 = HK12*P[5][23]; - const float HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23]; - const float HK21 = powf(Kacc, 2); - const float HK22 = HK12*HK21; - const float HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22]; - const float HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22]; - const float HK25 = HK7*P[4][22]; - const float HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4]; - const float HK27 = HK21*HK7; - const float HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22]; - const float HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4]; - const float HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4]; - const float HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4]; - // const float HK32 = Kacc/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); + const ftype HK0 = vn - vwn; + const ftype HK1 = ve - vwe; + const ftype HK2 = HK0*q0 + HK1*q3 - q2*vd; + const ftype HK3 = 2*Kacc; + const ftype HK4 = HK0*q1 + HK1*q2 + q3*vd; + const ftype HK5 = HK0*q2 - HK1*q1 + q0*vd; + const ftype HK6 = -HK0*q3 + HK1*q0 + q1*vd; + const ftype HK7 = powF(q0, 2) + powF(q1, 2) - powF(q2, 2) - powF(q3, 2); + const ftype HK8 = HK7*Kacc; + const ftype HK9 = q0*q3 + q1*q2; + const ftype HK10 = HK3*HK9; + const ftype HK11 = q0*q2 - q1*q3; + const ftype HK12 = 2*HK9; + const ftype HK13 = 2*HK11; + const ftype HK14 = 2*HK4; + const ftype HK15 = 2*HK2; + const ftype HK16 = 2*HK5; + const ftype HK17 = 2*HK6; + const ftype HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4]; + const ftype HK19 = HK12*P[5][23]; + const ftype HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23]; + const ftype HK21 = powF(Kacc, 2); + const ftype HK22 = HK12*HK21; + const ftype HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22]; + const ftype HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22]; + const ftype HK25 = HK7*P[4][22]; + const ftype HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4]; + const ftype HK27 = HK21*HK7; + const ftype HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22]; + const ftype HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4]; + const ftype HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4]; + const ftype HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4]; + // const ftype HK32 = Kacc/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); // calculate innovation variance and exit if badly conditioned innovDragVar.x = (-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); if (innovDragVar.x < R_ACC) { return; } - const float HK32 = Kacc / innovDragVar.x; + const ftype HK32 = Kacc / innovDragVar.x; // Observation Jacobians Hfusion[0] = -HK2*HK3; @@ -622,20 +622,20 @@ void NavEKF3_core::FuseDragForces() } else if (axis_index == 1) { // drag can be modelled as an arbitrary combination of bluff body drag that proportional to // speed squared, and rotor momentum drag that is proportional to speed. - float Kacc; // Derivative of specific force wrt airspeed + ftype Kacc; // Derivative of specific force wrt airspeed if (using_mcoef && using_bcoef_y) { // mixed bluff body and propeller momentum drag - const float airSpd = (bcoef_y / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef_y) * fabsf(mea_acc))); - Kacc = fmaxf(1e-1f, (rho / bcoef_y) * airSpd + mcoef * density_ratio); + const ftype airSpd = (bcoef_y / rho) * (- mcoef + sqrtF(sq(mcoef) + 2.0f * (rho / bcoef_y) * fabsF(mea_acc))); + Kacc = fmaxF(1e-1f, (rho / bcoef_y) * airSpd + mcoef * density_ratio); predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio; } else if (using_mcoef) { // propeller momentum drag only - Kacc = fmaxf(1e-1f, mcoef * density_ratio); + Kacc = fmaxF(1e-1f, mcoef * density_ratio); predAccel = - rel_wind_body[1] * mcoef * density_ratio; } else if (using_bcoef_y) { // bluff body drag only - const float airSpd = sqrtf((2.0f * bcoef_y * fabsf(mea_acc)) / rho); - Kacc = fmaxf(1e-1f, (rho / bcoef_y) * airSpd); + const ftype airSpd = sqrtF((2.0f * bcoef_y * fabsF(mea_acc)) / rho); + Kacc = fmaxF(1e-1f, (rho / bcoef_y) * airSpd); predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign; } else { // nothing more to do @@ -643,46 +643,46 @@ void NavEKF3_core::FuseDragForces() } // intermediate variables - const float HK0 = ve - vwe; - const float HK1 = vn - vwn; - const float HK2 = HK0*q0 - HK1*q3 + q1*vd; - const float HK3 = 2*Kacc; - const float HK4 = -HK0*q1 + HK1*q2 + q0*vd; - const float HK5 = HK0*q2 + HK1*q1 + q3*vd; - const float HK6 = HK0*q3 + HK1*q0 - q2*vd; - const float HK7 = q0*q3 - q1*q2; - const float HK8 = HK3*HK7; - const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2); - const float HK10 = HK9*Kacc; - const float HK11 = q0*q1 + q2*q3; - const float HK12 = 2*HK11; - const float HK13 = 2*HK7; - const float HK14 = 2*HK5; - const float HK15 = 2*HK2; - const float HK16 = 2*HK4; - const float HK17 = 2*HK6; - const float HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5]; - const float HK19 = powf(Kacc, 2); - const float HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23]; - const float HK21 = HK13*P[4][22]; - const float HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22]; - const float HK23 = HK13*HK19; - const float HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5]; - const float HK25 = HK9*P[5][23]; - const float HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5]; - const float HK27 = HK19*HK9; - const float HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23]; - const float HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5]; - const float HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5]; - const float HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5]; - // const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); + const ftype HK0 = ve - vwe; + const ftype HK1 = vn - vwn; + const ftype HK2 = HK0*q0 - HK1*q3 + q1*vd; + const ftype HK3 = 2*Kacc; + const ftype HK4 = -HK0*q1 + HK1*q2 + q0*vd; + const ftype HK5 = HK0*q2 + HK1*q1 + q3*vd; + const ftype HK6 = HK0*q3 + HK1*q0 - q2*vd; + const ftype HK7 = q0*q3 - q1*q2; + const ftype HK8 = HK3*HK7; + const ftype HK9 = powF(q0, 2) - powF(q1, 2) + powF(q2, 2) - powF(q3, 2); + const ftype HK10 = HK9*Kacc; + const ftype HK11 = q0*q1 + q2*q3; + const ftype HK12 = 2*HK11; + const ftype HK13 = 2*HK7; + const ftype HK14 = 2*HK5; + const ftype HK15 = 2*HK2; + const ftype HK16 = 2*HK4; + const ftype HK17 = 2*HK6; + const ftype HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5]; + const ftype HK19 = powF(Kacc, 2); + const ftype HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23]; + const ftype HK21 = HK13*P[4][22]; + const ftype HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22]; + const ftype HK23 = HK13*HK19; + const ftype HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5]; + const ftype HK25 = HK9*P[5][23]; + const ftype HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5]; + const ftype HK27 = HK19*HK9; + const ftype HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23]; + const ftype HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5]; + const ftype HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5]; + const ftype HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5]; + // const ftype HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); innovDragVar.y = (HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); if (innovDragVar.y < R_ACC) { // calculation is badly conditioned return; } - const float HK32 = Kacc / innovDragVar.y; + const ftype HK32 = Kacc / innovDragVar.y; // Observation Jacobians Hfusion[0] = -HK2*HK3; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 841743b3fa..82c7476653 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -71,15 +71,15 @@ void NavEKF3_core::setWindMagStateLearningMode() if (yawAlignComplete && useAirspeed()) { // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading // which assumes the vehicle has launched into the wind - Vector3f tempEuler; + Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); - float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; - stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z); - stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z); + ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; + stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z); + stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z); // set the wind state variances to the measurement uncertainty for (uint8_t index=22; index<=23; index++) { - P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(dal.get_EAS2TAS(), 0.9f, 10.0f)); + P[index][index] = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(dal.get_EAS2TAS(), 0.9f, 10.0f)); } } else { // set the variances using a typical wind speed @@ -232,7 +232,7 @@ void NavEKF3_core::setAidingMode() } } // keep the IMU bias state variances, but zero the covariances - float oldBiasVariance[6]; + ftype oldBiasVariance[6]; for (uint8_t row=0; row<6; row++) { oldBiasVariance[row] = P[row+10][row+10]; } @@ -407,7 +407,7 @@ void NavEKF3_core::setAidingMode() } // handle height reset as special case hgtMea = -extNavDataDelayed.pos.z; - posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); + posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f)); ResetHeight(); #endif // EK3_FEATURE_EXTERNAL_NAV } @@ -438,7 +438,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus() // Once the tilt variances have reduced, re-set the yaw and magnetic field states // and declare the tilt alignment complete if (!tiltAlignComplete) { - if (tiltErrorVariance < sq(radians(5.0f))) { + if (tiltErrorVariance < sq(radians(5.0))) { tiltAlignComplete = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index); } @@ -631,16 +631,16 @@ void NavEKF3_core::recordYawReset() void NavEKF3_core::checkGyroCalStatus(void) { // check delta angle bias variances - const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); + const ftype delAngBiasVarMax = sq(radians(0.15 * dtEkfAvg)); const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) { // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference // which can make this check fail - Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]); - Vector3f temp = prevTnb * delAngBiasVarVec; - delAngBiasLearned = (fabsf(temp.x) < delAngBiasVarMax) && - (fabsf(temp.y) < delAngBiasVarMax); + Vector3F delAngBiasVarVec = Vector3F(P[10][10],P[11][11],P[12][12]); + Vector3F temp = prevTnb * delAngBiasVarVec; + delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) && + (fabsF(temp.y) < delAngBiasVarMax); } else { delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) && (P[11][11] <= delAngBiasVarMax) && @@ -697,7 +697,7 @@ void NavEKF3_core::runYawEstimatorPrediction() return; } - float trueAirspeed; + ftype trueAirspeed; if (assume_zero_sideslip()) { trueAirspeed = MAX(tasDataDelayed.tas, 0.0f); } else { @@ -720,13 +720,13 @@ void NavEKF3_core::runYawEstimatorCorrection() if (EKFGSF_run_filterbank) { if (gpsDataToFuse) { - Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y); - float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise); + Vector2F gpsVelNE = Vector2F(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y); + ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise)); yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc); // after velocity data has been fused the yaw variance estimate will have been refreshed and // is used maintain a history of validity - float gsfYaw, gsfYawVariance; + ftype gsfYaw, gsfYawVariance; if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) { if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) { EKFGSF_yaw_valid_count++; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp index 6cecb869a3..6e3ed85a1d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp @@ -17,7 +17,7 @@ void NavEKF3_core::resetGyroBias(void) /* vehicle specific initial gyro bias uncertainty in deg/sec */ -float NavEKF3_core::InitialGyroBiasUncertainty(void) const +ftype NavEKF3_core::InitialGyroBiasUncertainty(void) const { return 2.5f; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index eb3a524741..307b3bf5c8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -6,6 +6,8 @@ #include +#pragma GCC diagnostic ignored "-Wnarrowing" + void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const { // Write first EKF packet @@ -147,7 +149,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const nav_filter_status solutionStatus {}; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); - float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); + float tempVar = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z); getFilterFaults(_faultStatus); getFilterStatus(solutionStatus); const struct log_NKF4 pkt4{ @@ -159,7 +161,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const sqrtvarH : (int16_t)(100*hgtVar), sqrtvarM : (int16_t)(100*tempVar), sqrtvarVT : (int16_t)(100*tasVar), - tiltErr : sqrtf(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians + tiltErr : sqrtF(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians offsetNorth : offset.x, offsetEast : offset.y, faults : _faultStatus, @@ -191,7 +193,7 @@ void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const offset : (int16_t)(100*terrainState), // filter ground offset state error RI : (int16_t)(100*innovRng), // range finder innovations meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range - errHAGL : (uint16_t)(100*sqrtf(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset() + errHAGL : (uint16_t)(100*sqrtF(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset() angErr : (float)outputTrackError.x, // output predictor angle error velErr : (float)outputTrackError.y, // output predictor velocity error posErr : (float)outputTrackError.z // output predictor position tracking error @@ -248,8 +250,8 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us) ID : rngBcnFuseDataReportIndex, rng : (int16_t)(100*report.rng), innov : (int16_t)(100*report.innov), - sqrtInnovVar : (uint16_t)(100*sqrtf(report.innovVar)), - testRatio : (uint16_t)(100*constrain_float(report.testRatio,0.0f,650.0f)), + sqrtInnovVar : (uint16_t)(100*sqrtF(report.innovVar)), + testRatio : (uint16_t)(100*constrain_ftype(report.testRatio,0.0f,650.0f)), beaconPosN : (int16_t)(100*report.beaconPosNED.x), beaconPosE : (int16_t)(100*report.beaconPosNED.y), beaconPosD : (int16_t)(100*report.beaconPosNED.z), diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 7c3d56b977..afb34a3b9d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -25,8 +25,8 @@ void NavEKF3_core::controlMagYawReset() } // Quaternion and delta rotation vector that are re-used for different calculations - Vector3f deltaRotVecTemp; - Quaternion deltaQuatTemp; + Vector3F deltaRotVecTemp; + QuaternionF deltaQuatTemp; bool flightResetAllowed = false; bool initialResetAllowed = false; @@ -61,7 +61,7 @@ void NavEKF3_core::controlMagYawReset() // check for increasing height bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f; - float yawInnovIncrease = fabsf(innovYaw) - fabsf(yawInnovAtLastMagReset); + ftype yawInnovIncrease = fabsF(innovYaw) - fabsF(yawInnovAtLastMagReset); // check for increasing yaw innovations bool yawInnovIncreasing = yawInnovIncrease > 0.25f; @@ -131,18 +131,18 @@ void NavEKF3_core::controlMagYawReset() void NavEKF3_core::realignYawGPS() { // get quaternion from existing filter states and calculate roll, pitch and yaw angles - Vector3f eulerAngles; + Vector3F eulerAngles; stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) { // calculate course yaw angle - float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x); + ftype velYaw = atan2F(stateStruct.velocity.y,stateStruct.velocity.x); // calculate course yaw angle from GPS velocity - float gpsYaw = atan2f(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x); + ftype gpsYaw = atan2F(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x); // Check the yaw angles for consistency - float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),fabsf(wrap_PI(gpsYaw - eulerAngles.z))); + ftype yawErr = MAX(fabsF(wrap_PI(gpsYaw - velYaw)),fabsF(wrap_PI(gpsYaw - eulerAngles.z))); // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete; @@ -180,7 +180,7 @@ void NavEKF3_core::realignYawGPS() void NavEKF3_core::alignYawAngle(const yaw_elements &yawAngData) { // update quaternion states and covariances - resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2f)), yawAngData.order); + resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2)), yawAngData.order); // send yaw alignment information to console GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index); @@ -206,14 +206,14 @@ void NavEKF3_core::SelectMagFusion() // Store yaw angle when moving for use as a static reference when not moving if (!onGroundNotMoving) { - if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { + if (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) { // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_321; - yawAngDataStatic.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]); + yawAngDataStatic.yawAng = atan2F(prevTnb[0][1], prevTnb[0][0]); } else { // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_312; - yawAngDataStatic.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]); + yawAngDataStatic.yawAng = atan2F(-prevTnb[1][0], prevTnb[1][1]); } yawAngDataStatic.yawAngErr = MAX(frontend->_yawNoise, 0.05f); yawAngDataStatic.time_ms = imuDataDelayed.time_ms; @@ -236,7 +236,7 @@ void NavEKF3_core::SelectMagFusion() if (imuSampleTime_ms - lastSynthYawTime_ms > 140) { // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement // for non fixed wing platform types - float gsfYaw, gsfYawVariance; + ftype gsfYaw, gsfYawVariance; const bool didUseEKFGSF = yawAlignComplete && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF); // fallback methods @@ -453,8 +453,8 @@ void NavEKF3_core::FuseMagnetometer() ftype &magXbias = mag_state.magXbias; ftype &magYbias = mag_state.magYbias; ftype &magZbias = mag_state.magZbias; - Matrix3f &DCM = mag_state.DCM; - Vector3f &MagPred = mag_state.MagPred; + Matrix3F &DCM = mag_state.DCM; + Vector3F &MagPred = mag_state.MagPred; ftype &R_MAG = mag_state.R_MAG; ftype *SH_MAG = &mag_state.SH_MAG[0]; Vector24 H_MAG; @@ -503,7 +503,7 @@ void NavEKF3_core::FuseMagnetometer() } // scale magnetometer observation error with total angular rate to allow for timing errors - R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT); + R_MAG = sq(constrain_ftype(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT); // calculate common expressions used to calculate observation jacobians an innovation variance for each component SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1; @@ -555,7 +555,7 @@ void NavEKF3_core::FuseMagnetometer() // calculate the innovation test ratios for (uint8_t i = 0; i<=2; i++) { - magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]); + magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (ftype)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]); } // check the last values from all components and set magnetometer health accordingly @@ -605,8 +605,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]); Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -619,8 +619,8 @@ void NavEKF3_core::FuseMagnetometer() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } // zero Kalman gains to inhibit magnetic field state estimation if (!inhibitMagStates) { @@ -631,8 +631,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } // zero Kalman gains to inhibit wind state estimation @@ -640,8 +640,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 = 2 + zero_range(&Kfusion[0], 22, 23); } // set flags to indicate to other processes that fusion has been performed and is required on the next frame @@ -685,8 +685,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -699,8 +699,8 @@ void NavEKF3_core::FuseMagnetometer() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } // zero Kalman gains to inhibit magnetic field state estimation @@ -712,8 +712,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } // zero Kalman gains to inhibit wind state estimation @@ -721,8 +721,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } // set flags to indicate to other processes that fusion has been performed and is required on the next frame @@ -767,8 +767,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]); Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -781,8 +781,8 @@ void NavEKF3_core::FuseMagnetometer() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } // zero Kalman gains to inhibit magnetic field state estimation @@ -794,8 +794,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]); Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } // zero Kalman gains to inhibit wind state estimation @@ -803,8 +803,8 @@ void NavEKF3_core::FuseMagnetometer() Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } // set flags to indicate to other processes that fusion has been performed and is required on the next frame @@ -897,12 +897,12 @@ void NavEKF3_core::FuseMagnetometer() */ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) { - const float &q0 = stateStruct.quat[0]; - const float &q1 = stateStruct.quat[1]; - const float &q2 = stateStruct.quat[2]; - const float &q3 = stateStruct.quat[3]; + const ftype &q0 = stateStruct.quat[0]; + const ftype &q1 = stateStruct.quat[1]; + const ftype &q2 = stateStruct.quat[2]; + const ftype &q3 = stateStruct.quat[3]; - float gsfYaw, gsfYawVariance; + ftype gsfYaw, gsfYawVariance; if (method == yawFusionMethod::GSF) { if (!EKFGSF_getYaw(gsfYaw, gsfYawVariance)) { return false; @@ -910,7 +910,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } // yaw measurement error variance (rad^2) - float R_YAW; + ftype R_YAW; switch (method) { case yawFusionMethod::GPS: R_YAW = sq(yawAngDataDelayed.yawAngErr); @@ -953,7 +953,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) case yawFusionMethod::PREDICTED: default: // determined automatically - order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312; + order = (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312; break; #if EK3_FEATURE_EXTERNAL_NAV @@ -964,53 +964,53 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix - float yawAngPredicted; - float H_YAW[4]; - Matrix3f Tbn_zeroYaw; + ftype yawAngPredicted; + ftype H_YAW[4]; + Matrix3F Tbn_zeroYaw; if (order == rotationOrder::TAIT_BRYAN_321) { // calculate 321 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw bool canUseA = false; - const float SA0 = 2*q3; - const float SA1 = 2*q2; - const float SA2 = SA0*q0 + SA1*q1; - const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); - float SA4, SA5_inv; + const ftype SA0 = 2*q3; + const ftype SA1 = 2*q2; + const ftype SA2 = SA0*q0 + SA1*q1; + const ftype SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); + ftype SA4, SA5_inv; if (is_positive(sq(SA3))) { SA4 = 1.0F/sq(SA3); SA5_inv = sq(SA2)*SA4 + 1; - canUseA = is_positive(fabsf(SA5_inv)); + canUseA = is_positive(fabsF(SA5_inv)); } bool canUseB = false; - const float SB0 = 2*q0; - const float SB1 = 2*q1; - const float SB2 = SB0*q3 + SB1*q2; - const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); - float SB3, SB5_inv; + const ftype SB0 = 2*q0; + const ftype SB1 = 2*q1; + const ftype SB2 = SB0*q3 + SB1*q2; + const ftype SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); + ftype SB3, SB5_inv; if (is_positive(sq(SB2))) { SB3 = 1.0F/sq(SB2); SB5_inv = SB3*sq(SB4) + 1; - canUseB = is_positive(fabsf(SB5_inv)); + canUseB = is_positive(fabsF(SB5_inv)); } - if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) { - const float SA5 = 1.0F/SA5_inv; - const float SA6 = 1.0F/SA3; - const float SA7 = SA2*SA4; - const float SA8 = 2*SA7; - const float SA9 = 2*SA6; + if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) { + const ftype SA5 = 1.0F/SA5_inv; + const ftype SA6 = 1.0F/SA3; + const ftype SA7 = SA2*SA4; + const ftype SA8 = 2*SA7; + const ftype SA9 = 2*SA6; H_YAW[0] = SA5*(SA0*SA6 - SA8*q0); H_YAW[1] = SA5*(SA1*SA6 - SA8*q1); H_YAW[2] = SA5*(SA1*SA7 + SA9*q1); H_YAW[3] = SA5*(SA0*SA7 + SA9*q0); - } else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) { - const float SB5 = 1.0F/SB5_inv; - const float SB6 = 1.0F/SB2; - const float SB7 = SB3*SB4; - const float SB8 = 2*SB7; - const float SB9 = 2*SB6; + } else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) { + const ftype SB5 = 1.0F/SB5_inv; + const ftype SB6 = 1.0F/SB2; + const ftype SB7 = SB3*SB4; + const ftype SB8 = 2*SB7; + const ftype SB9 = 2*SB6; H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3); H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2); @@ -1021,7 +1021,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } // Get the 321 euler angles - Vector3f euler321; + Vector3F euler321; stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z); yawAngPredicted = euler321.z; @@ -1031,46 +1031,46 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } else if (order == rotationOrder::TAIT_BRYAN_312) { // calculate 312 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw bool canUseA = false; - const float SA0 = 2*q3; - const float SA1 = 2*q2; - const float SA2 = SA0*q0 - SA1*q1; - const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3); - float SA4, SA5_inv; + const ftype SA0 = 2*q3; + const ftype SA1 = 2*q2; + const ftype SA2 = SA0*q0 - SA1*q1; + const ftype SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3); + ftype SA4, SA5_inv; if (is_positive(sq(SA3))) { SA4 = 1.0F/sq(SA3); SA5_inv = sq(SA2)*SA4 + 1; - canUseA = is_positive(fabsf(SA5_inv)); + canUseA = is_positive(fabsF(SA5_inv)); } bool canUseB = false; - const float SB0 = 2*q0; - const float SB1 = 2*q1; - const float SB2 = -SB0*q3 + SB1*q2; - const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3); - float SB3, SB5_inv; + const ftype SB0 = 2*q0; + const ftype SB1 = 2*q1; + const ftype SB2 = -SB0*q3 + SB1*q2; + const ftype SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3); + ftype SB3, SB5_inv; if (is_positive(sq(SB2))) { SB3 = 1.0F/sq(SB2); SB5_inv = SB3*sq(SB4) + 1; - canUseB = is_positive(fabsf(SB5_inv)); + canUseB = is_positive(fabsF(SB5_inv)); } - if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) { - const float SA5 = 1.0F/SA5_inv; - const float SA6 = 1.0F/SA3; - const float SA7 = SA2*SA4; - const float SA8 = 2*SA7; - const float SA9 = 2*SA6; + if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) { + const ftype SA5 = 1.0F/SA5_inv; + const ftype SA6 = 1.0F/SA3; + const ftype SA7 = SA2*SA4; + const ftype SA8 = 2*SA7; + const ftype SA9 = 2*SA6; H_YAW[0] = SA5*(SA0*SA6 - SA8*q0); H_YAW[1] = SA5*(-SA1*SA6 + SA8*q1); H_YAW[2] = SA5*(-SA1*SA7 - SA9*q1); H_YAW[3] = SA5*(SA0*SA7 + SA9*q0); - } else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) { - const float SB5 = 1.0F/SB5_inv; - const float SB6 = 1.0F/SB2; - const float SB7 = SB3*SB4; - const float SB8 = 2*SB7; - const float SB9 = 2*SB6; + } else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) { + const ftype SB5 = 1.0F/SB5_inv; + const ftype SB6 = 1.0F/SB2; + const ftype SB7 = SB3*SB4; + const ftype SB8 = 2*SB7; + const ftype SB9 = 2*SB6; H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3); H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2); @@ -1081,7 +1081,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } // Get the 312 Tait Bryan rotation angles - Vector3f euler312 = stateStruct.quat.to_vector312(); + Vector3F euler312 = stateStruct.quat.to_vector312(); yawAngPredicted = euler312.z; // set the yaw to zero and calculate the zero yaw rotation from body to earth frame @@ -1097,8 +1097,8 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) { // Use the difference between the horizontal projection and declination to give the measured yaw // rotate measured mag components into earth frame - Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); + Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + ftype yawAngMeasured = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination()); innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured); break; } @@ -1128,8 +1128,8 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero - float PH[4]; - float varInnov = R_YAW; + ftype PH[4]; + ftype varInnov = R_YAW; for (uint8_t rowIndex=0; rowIndex<=3; rowIndex++) { PH[rowIndex] = 0.0f; for (uint8_t colIndex=0; colIndex<=3; colIndex++) { @@ -1137,7 +1137,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } varInnov += H_YAW[rowIndex]*PH[rowIndex]; } - float varInnovInv; + ftype varInnovInv; if (varInnov >= R_YAW) { varInnovInv = 1.0f / varInnov; // output numerical health status @@ -1161,7 +1161,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } // calculate the innovation test ratio - yawTestRatio = sq(innovYaw) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov); + yawTestRatio = sq(innovYaw) / (sq(MAX(0.01f * (ftype)frontend->_yawInnovGate, 1.0f)) * varInnov); // Declare the magnetometer unhealthy if the innovation test fails if (yawTestRatio > 1.0f) { @@ -1184,7 +1184,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) } for (uint8_t row = 0; row <= stateIndexLim; row++) { for (uint8_t column = 0; column <= stateIndexLim; column++) { - float tmp = KH[row][0] * P[0][column]; + ftype tmp = KH[row][0] * P[0][column]; tmp += KH[row][1] * P[1][column]; tmp += KH[row][2] * P[2][column]; tmp += KH[row][3] * P[3][column]; @@ -1213,7 +1213,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) // correct the state vector for (uint8_t i=0; i<=stateIndexLim; i++) { - statesArray[i] -= Kfusion[i] * constrain_float(innovYaw, -0.5f, 0.5f); + statesArray[i] -= Kfusion[i] * constrain_ftype(innovYaw, -0.5f, 0.5f); } stateStruct.quat.normalize(); @@ -1234,14 +1234,14 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) * This is used to prevent the declination of the EKF earth field states from drifting during operation without GPS * or some other absolute position or velocity reference */ -void NavEKF3_core::FuseDeclination(float declErr) +void NavEKF3_core::FuseDeclination(ftype declErr) { // declination error variance (rad^2) - const float R_DECL = sq(declErr); + const ftype R_DECL = sq(declErr); // copy required states to local variables - float magN = stateStruct.earth_magfield.x; - float magE = stateStruct.earth_magfield.y; + ftype magN = stateStruct.earth_magfield.x; + ftype magE = stateStruct.earth_magfield.y; // prevent bad earth field states from causing numerical errors or exceptions if (magN < 1e-3f) { @@ -1250,34 +1250,34 @@ void NavEKF3_core::FuseDeclination(float declErr) // Calculate observation Jacobian and Kalman gains // Calculate intermediate variables - float t2 = magE*magE; - float t3 = magN*magN; - float t4 = t2+t3; + ftype t2 = magE*magE; + ftype t3 = magN*magN; + ftype t4 = t2+t3; // if the horizontal magnetic field is too small, this calculation will be badly conditioned if (t4 < 1e-4f) { return; } - float t5 = P[16][16]*t2; - float t6 = P[17][17]*t3; - float t7 = t2*t2; - float t8 = R_DECL*t7; - float t9 = t3*t3; - float t10 = R_DECL*t9; - float t11 = R_DECL*t2*t3*2.0f; - float t14 = P[16][17]*magE*magN; - float t15 = P[17][16]*magE*magN; - float t12 = t5+t6+t8+t10+t11-t14-t15; - float t13; - if (fabsf(t12) > 1e-6f) { + ftype t5 = P[16][16]*t2; + ftype t6 = P[17][17]*t3; + ftype t7 = t2*t2; + ftype t8 = R_DECL*t7; + ftype t9 = t3*t3; + ftype t10 = R_DECL*t9; + ftype t11 = R_DECL*t2*t3*2.0f; + ftype t14 = P[16][17]*magE*magN; + ftype t15 = P[17][16]*magE*magN; + ftype t12 = t5+t6+t8+t10+t11-t14-t15; + ftype t13; + if (fabsF(t12) > 1e-6f) { t13 = 1.0f / t12; } else { return; } - float t18 = magE*magE; - float t19 = magN*magN; - float t20 = t18+t19; - float t21; - if (fabsf(t20) > 1e-6f) { + ftype t18 = magE*magE; + ftype t19 = magN*magN; + ftype t20 = t18+t19; + ftype t21; + if (fabsF(t20) > 1e-6f) { t21 = 1.0f/t20; } else { return; @@ -1285,7 +1285,7 @@ void NavEKF3_core::FuseDeclination(float declErr) // Calculate the observation Jacobian // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost - float H_DECL[24] = {}; + ftype H_DECL[24] = {}; H_DECL[16] = -magE*t21; H_DECL[17] = magN*t21; @@ -1305,8 +1305,8 @@ void NavEKF3_core::FuseDeclination(float declErr) Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN); Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -1319,8 +1319,8 @@ void NavEKF3_core::FuseDeclination(float declErr) } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } if (!inhibitMagStates) { @@ -1331,23 +1331,23 @@ void NavEKF3_core::FuseDeclination(float declErr) Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN); Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } if (!inhibitWindStates) { Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } // get the magnetic declination - float magDecAng = MagDeclination(); + ftype magDecAng = MagDeclination(); // Calculate the innovation - float innovation = atan2f(magE , magN) - magDecAng; + ftype innovation = atan2F(magE , magN) - magDecAng; // limit the innovation to protect against data errors if (innovation > 0.5f) { @@ -1422,18 +1422,18 @@ void NavEKF3_core::alignMagStateDeclination() } // get the magnetic declination - float magDecAng = MagDeclination(); + ftype magDecAng = MagDeclination(); // rotate the NE values so that the declination matches the published value - Vector3f initMagNED = stateStruct.earth_magfield; - float magLengthNE = norm(initMagNED.x,initMagNED.y); - stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng); - stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng); + Vector3F initMagNED = stateStruct.earth_magfield; + ftype magLengthNE = norm(initMagNED.x,initMagNED.y); + stateStruct.earth_magfield.x = magLengthNE * cosF(magDecAng); + stateStruct.earth_magfield.y = magLengthNE * sinF(magDecAng); if (!inhibitMagStates) { // zero the corresponding state covariances if magnetic field state learning is active - float var_16 = P[16][16]; - float var_17 = P[17][17]; + ftype var_16 = P[16][16]; + ftype var_17 = P[17][17]; zeroRows(P,16,17); zeroCols(P,16,17); P[16][16] = var_16; @@ -1484,13 +1484,13 @@ bool NavEKF3_core::learnMagBiasFromGPS(void) } // combine yaw with current quaternion to get yaw corrected quaternion - Quaternion quat = stateStruct.quat; + QuaternionF quat = stateStruct.quat; if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_321) { - Vector3f euler321; + Vector3F euler321; quat.to_euler(euler321.x, euler321.y, euler321.z); quat.from_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng); } else if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_312) { - Vector3f euler312 = quat.to_vector312(); + Vector3F euler312 = quat.to_vector312(); quat.from_vector312(euler312.x, euler312.y, yawAngDataDelayed.yawAng); } else { // rotation order not supported @@ -1498,19 +1498,19 @@ bool NavEKF3_core::learnMagBiasFromGPS(void) } // build the expected body field from orientation and table earth field - Matrix3f dcm; + Matrix3F dcm; quat.rotation_matrix(dcm); - Vector3f expected_body_field = dcm.transposed() * table_earth_field_ga; + Vector3F expected_body_field = dcm.transposed() * table_earth_field_ga; // calculate error in field - Vector3f err = (expected_body_field - mag_data.mag) + stateStruct.body_magfield; + Vector3F err = (expected_body_field - mag_data.mag) + stateStruct.body_magfield; // learn body frame mag biases stateStruct.body_magfield -= err * EK3_GPS_MAG_LEARN_RATE; // check if error is below threshold. If it is then we can // fallback to magnetometer on failure of external yaw - float err_length = err.length(); + ftype err_length = err.length(); // we allow for yaw backback to compass if we have had 50 samples // in a row below the threshold. This corresponds to 10 seconds @@ -1540,7 +1540,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw() return false; }; - float yawEKFGSF, yawVarianceEKFGSF; + ftype yawEKFGSF, yawVarianceEKFGSF; if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) { // keep roll and pitch and reset yaw rotationOrder order; @@ -1582,14 +1582,14 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw() } // returns true on success and populates yaw (in radians) and yawVariance (rad^2) -bool NavEKF3_core::EKFGSF_getYaw(float &yaw, float &yawVariance) const +bool NavEKF3_core::EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const { // return immediately if no yaw estimator if (yawEstimator == nullptr) { return false; } - float velInnovLength; + ftype velInnovLength; if (yawEstimator->getYawData(yaw, yawVariance) && is_positive(yawVariance) && yawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) && @@ -1600,14 +1600,14 @@ bool NavEKF3_core::EKFGSF_getYaw(float &yaw, float &yawVariance) const return false; } -void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order) +void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order) { - Quaternion quatBeforeReset = stateStruct.quat; + QuaternionF quatBeforeReset = stateStruct.quat; // check if we should use a 321 or 312 Rotation order and update the quaternion // states using the preferred yaw definition stateStruct.quat.inverse().rotation_matrix(prevTnb); - Vector3f eulerAngles; + Vector3F eulerAngles; if (order == rotationOrder::TAIT_BRYAN_321) { // rolled more than pitched so use 321 rotation order stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); @@ -1624,14 +1624,14 @@ void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationO // Update the rotation matrix stateStruct.quat.inverse().rotation_matrix(prevTnb); - float deltaYaw = wrap_PI(yaw - eulerAngles.z); + ftype deltaYaw = wrap_PI(yaw - eulerAngles.z); // calculate the change in the quaternion state and apply it to the output history buffer - Quaternion quat_delta = stateStruct.quat / quatBeforeReset; + QuaternionF quat_delta = stateStruct.quat / quatBeforeReset; StoreQuatRotate(quat_delta); // assume tilt uncertainty split equally between roll and pitch - Vector3f angleErrVarVec = Vector3f(0.5f * tiltErrorVariance, 0.5f * tiltErrorVariance, yawVariance); + Vector3F angleErrVarVec = Vector3F(0.5 * tiltErrorVariance, 0.5 * tiltErrorVariance, yawVariance); CovariancePrediction(&angleErrVarVec); // record the yaw reset event diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 3432b605ec..507a60b24e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -128,10 +128,10 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con // subtract delay from timestamp timeStamp_ms -= delay_ms; - bodyOdmDataNew.body_offset = posOffset; - bodyOdmDataNew.vel = delPos * (1.0f/delTime); + bodyOdmDataNew.body_offset = posOffset.toftype(); + bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime); bodyOdmDataNew.time_ms = timeStamp_ms; - bodyOdmDataNew.angRate = delAng * (1.0f/delTime); + bodyOdmDataNew.angRate = (delAng * (1.0/delTime)).toftype(); bodyOdmMeasTime_ms = timeStamp_ms; // simple model of accuracy @@ -157,7 +157,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam } wheel_odm_elements wheelOdmDataNew = {}; - wheelOdmDataNew.hub_offset = posOffset; + wheelOdmDataNew.hub_offset = posOffset.toftype(); wheelOdmDataNew.delAng = delAng; wheelOdmDataNew.radius = radius; wheelOdmDataNew.delTime = delTime; @@ -190,8 +190,8 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f // reset the accumulated body delta angle and time // don't do the calculation if not enough time lapsed for a reliable body rate measurement if (delTimeOF > 0.01f) { - flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f); - flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f); + flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_ftype((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f); + flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_ftype((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f); delAngBodyOF.zero(); delTimeOF = 0.0f; } @@ -219,9 +219,9 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f } // write uncorrected flow rate measurements // note correction for different axis and sign conventions used by the px4flow sensor - ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) + ofDataNew.flowRadXY = - rawFlowRates.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) // write the flow sensor position in body frame - ofDataNew.body_offset = posOffset; + ofDataNew.body_offset = posOffset.toftype(); // write flow rate measurements corrected for body rates ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x; ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y; @@ -328,7 +328,7 @@ void NavEKF3_core::readMagData() ((compass.last_update_usec(magSelectIndex) - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) { // detect changes to magnetometer offset parameters and reset states - Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex); + Vector3F nowMagOffsets = compass.get_offsets(magSelectIndex).toftype(); bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets); if (changeDetected) { // zero the learned magnetometer bias states @@ -357,7 +357,7 @@ void NavEKF3_core::readMagData() magDataNew.time_ms -= localFilterTimeStep_ms/2; // read compass data and scale to improve numerical conditioning - magDataNew.mag = compass.get_field(magSelectIndex) * 0.001f; + magDataNew.mag = (compass.get_field(magSelectIndex) * 0.001f).toftype(); // check for consistent data between magnetometers consistentMagData = compass.consistent(); @@ -386,7 +386,7 @@ void NavEKF3_core::readIMUData() const auto &ins = dal.ins(); // calculate an averaged IMU update rate using a spike and lowpass filter combination - dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg; + dtIMUavg = 0.02f * constrain_ftype(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg; // the imu sample time is used as a common time reference throughout the filter imuSampleTime_ms = frontend->imuSampleTime_us / 1000; @@ -427,7 +427,7 @@ void NavEKF3_core::readIMUData() updateMovementCheck(); readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT); - accelPosOffset = ins.get_imu_pos_offset(accel_index_active); + accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype(); imuDataNew.accel_index = accel_index_active; // Get delta angle data from primary gyro or primary if not available @@ -454,7 +454,7 @@ void NavEKF3_core::readIMUData() imuQuatDownSampleNew.normalize(); // Rotate the latest delta velocity into body frame at the start of accumulation - Matrix3f deltaRotMat; + Matrix3F deltaRotMat; imuQuatDownSampleNew.rotation_matrix(deltaRotMat); // Apply the delta velocity to the delta velocity accumulator @@ -482,7 +482,7 @@ void NavEKF3_core::readIMUData() storedIMU.push_youngest_element(imuDataDownSampledNew); // calculate the achieved average time step rate for the EKF using a combination spike and LPF - float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg); + ftype dtNow = constrain_ftype(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg); dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow; // do an addtional down sampling for data used to sample XY body frame drag specific forces @@ -506,7 +506,7 @@ void NavEKF3_core::readIMUData() imuDataDelayed = storedIMU.get_oldest_element(); // protect against delta time going to zero - float minDT = 0.1f * dtEkfAvg; + ftype minDT = 0.1f * dtEkfAvg; imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); @@ -526,12 +526,16 @@ void NavEKF3_core::readIMUData() // read the delta velocity and corresponding time interval from the IMU // return false if data is not available -bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) { +bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt) { const auto &ins = dal.ins(); if (ins_index < ins.get_accel_count()) { - ins.get_delta_velocity(ins_index,dVel,dVel_dt); - dVel_dt = MAX(dVel_dt,1.0e-4f); + Vector3f dVelF; + float dVel_dtF; + ins.get_delta_velocity(ins_index, dVelF, dVel_dtF); + dVel = dVelF.toftype(); + dVel_dt = dVel_dtF; + dVel_dt = MAX(dVel_dt,1.0e-4); return true; } return false; @@ -585,7 +589,7 @@ void NavEKF3_core::readGpsData() gpsDataNew.sensor_idx = selected_gps; // read the NED velocity from the GPS - gpsDataNew.vel = gps.velocity(selected_gps); + gpsDataNew.vel = gps.velocity(selected_gps).toftype(); gpsDataNew.have_vz = gps.have_vertical_velocity(selected_gps); // position and velocity are not yet corrected for sensor position @@ -593,7 +597,7 @@ void NavEKF3_core::readGpsData() // Use the speed and position accuracy from the GPS if available, otherwise set it to zero. // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data - float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f); + ftype alpha = constrain_ftype(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f); gpsSpdAccuracy *= (1.0f - alpha); float gpsSpdAccRaw; if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) { @@ -686,7 +690,7 @@ void NavEKF3_core::readGpsData() if (gpsGoodToAlign && !have_table_earth_field) { const auto *compass = dal.get_compass(); if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) { - table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc); + table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype(); table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7, gpsloc.lng*1.0e-7)); have_table_earth_field = true; @@ -699,9 +703,9 @@ void NavEKF3_core::readGpsData() // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin if (validOrigin) { - gpsDataNew.pos = EKF_origin.get_distance_NE(gpsloc); + gpsDataNew.pos = EKF_origin.get_distance_NE_ftype(gpsloc); if ((frontend->_originHgtMode & (1<<2)) == 0) { - gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt); + gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt); } else { gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt); } @@ -717,7 +721,7 @@ void NavEKF3_core::readGpsData() // accuracy. Set to min of 5 degrees here to prevent // the user constantly receiving warnings about high // normalised yaw innovations - const float min_yaw_accuracy_deg = 5.0f; + const ftype min_yaw_accuracy_deg = 5.0f; yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg); writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2); } @@ -725,11 +729,15 @@ void NavEKF3_core::readGpsData() // read the delta angle and corresponding time interval from the IMU // return false if data is not available -bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAngDT) { +bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAngDT) { const auto &ins = dal.ins(); if (ins_index < ins.get_gyro_count()) { - ins.get_delta_angle(ins_index, dAng, dAngDT); + Vector3f dAngF; + float dAngDTF; + ins.get_delta_angle(ins_index, dAngF, dAngDTF); + dAng = dAngF.toftype(); + dAngDT = dAngDTF; return true; } return false; @@ -773,7 +781,7 @@ void NavEKF3_core::readBaroData() void NavEKF3_core::calcFiltBaroOffset() { // Apply a first order LPF with spike protection - baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f); + baroHgtOffset += 0.1f * constrain_ftype(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f); } // correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter. @@ -782,14 +790,14 @@ void NavEKF3_core::correctEkfOriginHeight() // Estimate the WGS-84 height of the EKF's origin using a Bayes filter // calculate the variance of our a-priori estimate of the ekf origin height - float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f); + ftype deltaTime = constrain_ftype(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0, 1.0); if (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) { // Use the baro drift rate - const float baroDriftRate = 0.05f; + const ftype baroDriftRate = 0.05; ekfOriginHgtVar += sq(baroDriftRate * deltaTime); } else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) { // use the worse case expected terrain gradient and vehicle horizontal speed - const float maxTerrGrad = 0.25f; + const ftype maxTerrGrad = 0.25; ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime); } else { // by definition our height source is absolute so cannot run this filter @@ -799,16 +807,16 @@ void NavEKF3_core::correctEkfOriginHeight() // calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error // when not using GPS as height source - float originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9]; + ftype originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9]; // calculate the correction gain - float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar); + ftype gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar); // calculate the innovation - float innovation = - stateStruct.position.z - gpsDataDelayed.hgt; + ftype innovation = - stateStruct.position.z - gpsDataDelayed.hgt; // check the innovation variance ratio - float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar); + ftype ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar); // correct the EKF origin and variance estimate if the innovation is less than 5-sigma if (ratio < 25.0f && gpsAccuracyGood) { @@ -910,7 +918,7 @@ void NavEKF3_core::readRngBcnData() rngBcnDataNew.rng = beacon->beacon_distance(index); // set the beacon position - rngBcnDataNew.beacon_posNED = beacon->beacon_position(index); + rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype(); // identify the beacon identifier rngBcnDataNew.beacon_ID = index; @@ -927,15 +935,19 @@ void NavEKF3_core::readRngBcnData() } // Check if the beacon system has returned a 3D fix - if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) { + Vector3f bp; + float bperr; + if (beacon->get_vehicle_position_ned(bp, bperr)) { rngBcnLast3DmeasTime_ms = imuSampleTime_ms; } + beaconVehiclePosNED = bp.toftype(); + beaconVehiclePosErr = bperr; // Check if the range beacon data can be used to align the vehicle position if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) && (beaconVehiclePosErr < 1.0f) && rngBcnAlignmentCompleted) { // check for consistency between the position reported by the beacon and the position from the 3-State alignment filter - const float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y); - const float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; + const ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y); + const ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; if (posDiffSq < 9.0f * posDiffVar) { rngBcnGoodToAlign = true; // Set the EKF origin and magnetic field declination if not previously set @@ -1035,7 +1047,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, extNavDataNew.posReset = false; } - extNavDataNew.pos = pos; + extNavDataNew.pos = pos.toftype(); extNavDataNew.posErr = posErr; // calculate timestamp @@ -1052,7 +1064,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, // protect against attitude or angle being NaN if (!quat.is_nan() && !isnan(angErr)) { // extract yaw from the attitude - float roll_rad, pitch_rad, yaw_rad; + ftype roll_rad, pitch_rad, yaw_rad; quat.to_euler(roll_rad, pitch_rad, yaw_rad); yaw_elements extNavYawAngDataNew; extNavYawAngDataNew.yawAng = yaw_rad; @@ -1087,7 +1099,7 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t ext_nav_vel_elements extNavVelNew; extNavVelNew.time_ms = timeStamp_ms; - extNavVelNew.vel = vel; + extNavVelNew.vel = vel.toftype(); extNavVelNew.err = err; extNavVelNew.corrected = false; @@ -1246,15 +1258,15 @@ void NavEKF3_core::learnInactiveBiases(void) // get filtered gyro and use the difference between the // corrected gyro on the active IMU and the inactive IMU // to move the inactive bias towards the right value - Vector3f filtered_gyro_active = ins.get_gyro(gyro_index_active) - (stateStruct.gyro_bias/dtEkfAvg); - Vector3f filtered_gyro_inactive = ins.get_gyro(i) - (inactiveBias[i].gyro_bias/dtEkfAvg); - Vector3f error = filtered_gyro_active - filtered_gyro_inactive; + Vector3F filtered_gyro_active = ins.get_gyro(gyro_index_active).toftype() - (stateStruct.gyro_bias/dtEkfAvg); + Vector3F filtered_gyro_inactive = ins.get_gyro(i).toftype() - (inactiveBias[i].gyro_bias/dtEkfAvg); + Vector3F error = filtered_gyro_active - filtered_gyro_inactive; // prevent a single large error from contaminating bias estimate - const float bias_limit = radians(5); - error.x = constrain_float(error.x, -bias_limit, bias_limit); - error.y = constrain_float(error.y, -bias_limit, bias_limit); - error.z = constrain_float(error.z, -bias_limit, bias_limit); + const ftype bias_limit = radians(5); + error.x = constrain_ftype(error.x, -bias_limit, bias_limit); + error.y = constrain_ftype(error.y, -bias_limit, bias_limit); + error.z = constrain_ftype(error.z, -bias_limit, bias_limit); // slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec // gyro bias error in around 1 minute @@ -1275,15 +1287,15 @@ void NavEKF3_core::learnInactiveBiases(void) // get filtered accel and use the difference between the // corrected accel on the active IMU and the inactive IMU // to move the inactive bias towards the right value - Vector3f filtered_accel_active = ins.get_accel(accel_index_active) - (stateStruct.accel_bias/dtEkfAvg); - Vector3f filtered_accel_inactive = ins.get_accel(i) - (inactiveBias[i].accel_bias/dtEkfAvg); - Vector3f error = filtered_accel_active - filtered_accel_inactive; + Vector3F filtered_accel_active = ins.get_accel(accel_index_active).toftype() - (stateStruct.accel_bias/dtEkfAvg); + Vector3F filtered_accel_inactive = ins.get_accel(i).toftype() - (inactiveBias[i].accel_bias/dtEkfAvg); + Vector3F error = filtered_accel_active - filtered_accel_inactive; // prevent a single large error from contaminating bias estimate - const float bias_limit = 1.0; // m/s/s - error.x = constrain_float(error.x, -bias_limit, bias_limit); - error.y = constrain_float(error.y, -bias_limit, bias_limit); - error.z = constrain_float(error.z, -bias_limit, bias_limit); + const ftype bias_limit = 1.0; // m/s/s + error.x = constrain_ftype(error.x, -bias_limit, bias_limit); + error.y = constrain_ftype(error.y, -bias_limit, bias_limit); + error.z = constrain_ftype(error.z, -bias_limit, bias_limit); // slowly bring the inactive accel in line with the active // accel. This corrects a 0.5 m/s/s accel bias error in @@ -1297,7 +1309,7 @@ void NavEKF3_core::learnInactiveBiases(void) /* return declination in radians */ -float NavEKF3_core::MagDeclination(void) const +ftype NavEKF3_core::MagDeclination(void) const { // if we are using the WMM tables then use the table declination // to ensure consistency with the table mag field. Otherwise use @@ -1327,17 +1339,17 @@ void NavEKF3_core::updateMovementCheck(void) return; } - const float gyro_limit = radians(3.0f); - const float gyro_diff_limit = 0.2f; - const float accel_limit = 1.0f; - const float accel_diff_limit = 5.0f; - const float hysteresis_ratio = 0.7f; - const float dtEkfAvgInv = 1.0f / dtEkfAvg; + const ftype gyro_limit = radians(3.0f); + const ftype gyro_diff_limit = 0.2f; + const ftype accel_limit = 1.0f; + const ftype accel_diff_limit = 5.0f; + const ftype hysteresis_ratio = 0.7f; + const ftype dtEkfAvgInv = 1.0f / dtEkfAvg; // get latest bias corrected gyro and accelerometer data const auto &ins = dal.ins(); - Vector3f gyro = ins.get_gyro(gyro_index_active) - stateStruct.gyro_bias * dtEkfAvgInv; - Vector3f accel = ins.get_accel(accel_index_active) - stateStruct.accel_bias * dtEkfAvgInv; + Vector3F gyro = ins.get_gyro(gyro_index_active).toftype() - stateStruct.gyro_bias * dtEkfAvgInv; + Vector3F accel = ins.get_accel(accel_index_active).toftype() - stateStruct.accel_bias * dtEkfAvgInv; if (!prevOnGround) { gyro_prev = gyro; @@ -1349,7 +1361,7 @@ void NavEKF3_core::updateMovementCheck(void) } // calculate a gyro rate of change metric - Vector3f temp = (gyro - gyro_prev) * dtEkfAvgInv; + Vector3F temp = (gyro - gyro_prev) * dtEkfAvgInv; gyro_prev = gyro; gyro_diff = 0.99f * gyro_diff + 0.01f * temp.length(); @@ -1358,14 +1370,14 @@ void NavEKF3_core::updateMovementCheck(void) accel_prev = accel; accel_diff = 0.99f * accel_diff + 0.01f * temp.length(); - const float gyro_length_ratio = gyro.length() / gyro_limit; - const float accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit; - const float gyro_diff_ratio = gyro_diff / gyro_diff_limit; - const float accel_diff_ratio = accel_diff / accel_diff_limit; + const ftype gyro_length_ratio = gyro.length() / gyro_limit; + const ftype accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit; + const ftype gyro_diff_ratio = gyro_diff / gyro_diff_limit; + const ftype accel_diff_ratio = accel_diff / accel_diff_limit; bool logStatusChange = false; if (onGroundNotMoving) { if (gyro_length_ratio > frontend->_ognmTestScaleFactor || - fabsf(accel_length_ratio) > frontend->_ognmTestScaleFactor || + fabsF(accel_length_ratio) > frontend->_ognmTestScaleFactor || gyro_diff_ratio > frontend->_ognmTestScaleFactor || accel_diff_ratio > frontend->_ognmTestScaleFactor) { @@ -1373,7 +1385,7 @@ void NavEKF3_core::updateMovementCheck(void) logStatusChange = true; } } else if (gyro_length_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio && - fabsf(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio && + fabsF(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio && gyro_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio && accel_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio) { @@ -1388,10 +1400,10 @@ void NavEKF3_core::updateMovementCheck(void) time_us : dal.micros64(), core : core_index, ongroundnotmoving : onGroundNotMoving, - gyro_length_ratio : gyro_length_ratio, - accel_length_ratio : accel_length_ratio, - gyro_diff_ratio : gyro_diff_ratio, - accel_diff_ratio : accel_diff_ratio, + gyro_length_ratio : float(gyro_length_ratio), + accel_length_ratio : float(accel_length_ratio), + gyro_diff_ratio : float(gyro_diff_ratio), + accel_diff_ratio : float(accel_diff_ratio), }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } @@ -1401,9 +1413,9 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu) { #if EK3_FEATURE_DRAG_FUSION // Average and down sample to 5Hz - const float bcoef_x = frontend->_ballisticCoef_x; - const float bcoef_y = frontend->_ballisticCoef_y; - const float mcoef = frontend->_momentumDragCoef.get(); + const ftype bcoef_x = frontend->_ballisticCoef_x; + const ftype bcoef_y = frontend->_ballisticCoef_y; + const ftype mcoef = frontend->_momentumDragCoef.get(); const bool using_bcoef_x = bcoef_x > 1.0f; const bool using_bcoef_y = bcoef_y > 1.0f; const bool using_mcoef = mcoef > 0.001f; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index ce395b6a34..b2a30a9cac 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -71,7 +71,7 @@ Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scri void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) { // horizontal velocity squared - float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); + ftype velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); // don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable // don't fuse flow data if it exceeds validity limits @@ -92,34 +92,34 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning - float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE); + ftype distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE); distanceTravelledSq = MIN(distanceTravelledSq, 100.0f); prevPosN = stateStruct.position[0]; prevPosE = stateStruct.position[1]; // in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity - float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); - float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6]; + ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); + ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6]; Popt += Pincrement; timeAtLastAuxEKF_ms = imuSampleTime_ms; // fuse range finder data if (rangeDataToFuse) { // predict range - float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z; + ftype predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z; // Copy required states to local variable names - float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time - float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time - float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time - float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time + ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time + ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time + ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time + ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time // Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors - float R_RNG = frontend->_rngNoise; + ftype R_RNG = frontend->_rngNoise; // calculate Kalman gain - float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3); - float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))); + ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3); + ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))); // Calculate the innovation variance for data logging varInnovRng = (R_RNG + Popt/sq(SK_RNG)); @@ -131,7 +131,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) innovRng = predRngMeas - rangeDataDelayed.rng; // calculate the innovation consistency test ratio - auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng); + auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f)) * varInnovRng); // Check the innovation test ratio and don't fuse if too large if (auxRngTestRatio < 1.0f) { @@ -152,18 +152,18 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) if (!cantFuseFlowData) { - Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes - Vector2f losPred; // predicted optical flow angular rate measurement - float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time - float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time - float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time - float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time - float K_OPT; - float H_OPT; - Vector2f auxFlowObsInnovVar; + Vector3F relVelSensor; // velocity of sensor relative to ground in sensor axes + Vector2F losPred; // predicted optical flow angular rate measurement + ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time + ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time + ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time + ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time + ftype K_OPT; + ftype H_OPT; + Vector2F auxFlowObsInnovVar; // predict range to centre of image - float flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z; + ftype flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z; // constrain terrain height to be below the vehicle terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); @@ -180,20 +180,20 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp; // calculate observation jacobians - float t2 = q0*q0; - float t3 = q1*q1; - float t4 = q2*q2; - float t5 = q3*q3; - float t6 = stateStruct.position.z - terrainState; - float t7 = 1.0f / (t6*t6); - float t8 = q0*q3*2.0f; - float t9 = t2-t3-t4+t5; + ftype t2 = q0*q0; + ftype t3 = q1*q1; + ftype t4 = q2*q2; + ftype t5 = q3*q3; + ftype t6 = stateStruct.position.z - terrainState; + ftype t7 = 1.0f / (t6*t6); + ftype t8 = q0*q3*2.0f; + ftype t9 = t2-t3-t4+t5; // prevent the state variances from becoming badly conditioned Popt = MAX(Popt,1E-6f); // calculate observation noise variance from parameter - float flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f)); + ftype flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f)); // Fuse Y axis data @@ -207,7 +207,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y; // calculate the innovation consistency test ratio - auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y); + auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y); // don't fuse if optical flow data is outside valid range if (auxFlowTestRatio.y < 1.0f) { @@ -239,7 +239,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x; // calculate the innovation consistency test ratio - auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x); + auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x); // don't fuse if optical flow data is outside valid range if (auxFlowTestRatio.x < 1.0f) { @@ -269,23 +269,23 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) { Vector24 H_LOS; - Vector3f relVelSensor; + 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; + ftype q0 = stateStruct.quat[0]; + ftype q1 = stateStruct.quat[1]; + ftype q2 = stateStruct.quat[2]; + ftype q3 = stateStruct.quat[3]; + ftype vn = stateStruct.velocity.x; + ftype ve = stateStruct.velocity.y; + ftype vd = stateStruct.velocity.z; + ftype 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; + ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd); + ftype ptd = pd + heightAboveGndEst; // Calculate common expressions for observation jacobians SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); @@ -306,14 +306,14 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) // 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/prevTnb.c.z),rngOnGnd,1000.0f); + ftype range = constrain_ftype((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f); // correct range for flow sensor offset body frame position offset // the corrected value is the predicted range from the sensor focal point to the // centre of the image on the ground assuming flat terrain - Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset; + Vector3F posOffsetBody = ofDataDelayed.body_offset - accelPosOffset; if (!posOffsetBody.is_zero()) { - Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); + Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); range -= posOffsetEarth.z / prevTnb.c.z; } @@ -328,7 +328,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) memset(&H_LOS[0], 0, sizeof(H_LOS)); if (obsIndex == 0) { // calculate X axis observation Jacobian - float t2 = 1.0f / range; + ftype t2 = 1.0f / range; H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); @@ -338,98 +338,98 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f); // calculate intermediate variables for the X observation innovation variance and Kalman gains - float t3 = q1*vd*2.0f; - float t4 = q0*ve*2.0f; - float t11 = q3*vn*2.0f; - float t5 = t3+t4-t11; - float t6 = q0*q3*2.0f; - float t29 = q1*q2*2.0f; - float t7 = t6-t29; - float t8 = q0*q1*2.0f; - float t9 = q2*q3*2.0f; - float t10 = t8+t9; - float t12 = P[0][0]*t2*t5; - float t13 = q0*vd*2.0f; - float t14 = q2*vn*2.0f; - float t28 = q1*ve*2.0f; - float t15 = t13+t14-t28; - float t16 = q3*vd*2.0f; - float t17 = q2*ve*2.0f; - float t18 = q1*vn*2.0f; - float t19 = t16+t17+t18; - float t20 = q3*ve*2.0f; - float t21 = q0*vn*2.0f; - float t30 = q2*vd*2.0f; - float t22 = t20+t21-t30; - float t23 = q0*q0; - float t24 = q1*q1; - float t25 = q2*q2; - float t26 = q3*q3; - float t27 = t23-t24+t25-t26; - float t31 = P[1][1]*t2*t15; - float t32 = P[6][0]*t2*t10; - float t33 = P[1][0]*t2*t15; - float t34 = P[2][0]*t2*t19; - float t35 = P[5][0]*t2*t27; - float t79 = P[4][0]*t2*t7; - float t80 = P[3][0]*t2*t22; - float t36 = t12+t32+t33+t34+t35-t79-t80; - float t37 = t2*t5*t36; - float t38 = P[6][1]*t2*t10; - float t39 = P[0][1]*t2*t5; - float t40 = P[2][1]*t2*t19; - float t41 = P[5][1]*t2*t27; - float t81 = P[4][1]*t2*t7; - float t82 = P[3][1]*t2*t22; - float t42 = t31+t38+t39+t40+t41-t81-t82; - float t43 = t2*t15*t42; - float t44 = P[6][2]*t2*t10; - float t45 = P[0][2]*t2*t5; - float t46 = P[1][2]*t2*t15; - float t47 = P[2][2]*t2*t19; - float t48 = P[5][2]*t2*t27; - float t83 = P[4][2]*t2*t7; - float t84 = P[3][2]*t2*t22; - float t49 = t44+t45+t46+t47+t48-t83-t84; - float t50 = t2*t19*t49; - float t51 = P[6][3]*t2*t10; - float t52 = P[0][3]*t2*t5; - float t53 = P[1][3]*t2*t15; - float t54 = P[2][3]*t2*t19; - float t55 = P[5][3]*t2*t27; - float t85 = P[4][3]*t2*t7; - float t86 = P[3][3]*t2*t22; - float t56 = t51+t52+t53+t54+t55-t85-t86; - float t57 = P[6][5]*t2*t10; - float t58 = P[0][5]*t2*t5; - float t59 = P[1][5]*t2*t15; - float t60 = P[2][5]*t2*t19; - float t61 = P[5][5]*t2*t27; - float t88 = P[4][5]*t2*t7; - float t89 = P[3][5]*t2*t22; - float t62 = t57+t58+t59+t60+t61-t88-t89; - float t63 = t2*t27*t62; - float t64 = P[6][4]*t2*t10; - float t65 = P[0][4]*t2*t5; - float t66 = P[1][4]*t2*t15; - float t67 = P[2][4]*t2*t19; - float t68 = P[5][4]*t2*t27; - float t90 = P[4][4]*t2*t7; - float t91 = P[3][4]*t2*t22; - float t69 = t64+t65+t66+t67+t68-t90-t91; - float t70 = P[6][6]*t2*t10; - float t71 = P[0][6]*t2*t5; - float t72 = P[1][6]*t2*t15; - float t73 = P[2][6]*t2*t19; - float t74 = P[5][6]*t2*t27; - float t93 = P[4][6]*t2*t7; - float t94 = P[3][6]*t2*t22; - float t75 = t70+t71+t72+t73+t74-t93-t94; - float t76 = t2*t10*t75; - float t87 = t2*t22*t56; - float t92 = t2*t7*t69; - float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; - float t78; + ftype t3 = q1*vd*2.0f; + ftype t4 = q0*ve*2.0f; + ftype t11 = q3*vn*2.0f; + ftype t5 = t3+t4-t11; + ftype t6 = q0*q3*2.0f; + ftype t29 = q1*q2*2.0f; + ftype t7 = t6-t29; + ftype t8 = q0*q1*2.0f; + ftype t9 = q2*q3*2.0f; + ftype t10 = t8+t9; + ftype t12 = P[0][0]*t2*t5; + ftype t13 = q0*vd*2.0f; + ftype t14 = q2*vn*2.0f; + ftype t28 = q1*ve*2.0f; + ftype t15 = t13+t14-t28; + ftype t16 = q3*vd*2.0f; + ftype t17 = q2*ve*2.0f; + ftype t18 = q1*vn*2.0f; + ftype t19 = t16+t17+t18; + ftype t20 = q3*ve*2.0f; + ftype t21 = q0*vn*2.0f; + ftype t30 = q2*vd*2.0f; + ftype t22 = t20+t21-t30; + ftype t23 = q0*q0; + ftype t24 = q1*q1; + ftype t25 = q2*q2; + ftype t26 = q3*q3; + ftype t27 = t23-t24+t25-t26; + ftype t31 = P[1][1]*t2*t15; + ftype t32 = P[6][0]*t2*t10; + ftype t33 = P[1][0]*t2*t15; + ftype t34 = P[2][0]*t2*t19; + ftype t35 = P[5][0]*t2*t27; + ftype t79 = P[4][0]*t2*t7; + ftype t80 = P[3][0]*t2*t22; + ftype t36 = t12+t32+t33+t34+t35-t79-t80; + ftype t37 = t2*t5*t36; + ftype t38 = P[6][1]*t2*t10; + ftype t39 = P[0][1]*t2*t5; + ftype t40 = P[2][1]*t2*t19; + ftype t41 = P[5][1]*t2*t27; + ftype t81 = P[4][1]*t2*t7; + ftype t82 = P[3][1]*t2*t22; + ftype t42 = t31+t38+t39+t40+t41-t81-t82; + ftype t43 = t2*t15*t42; + ftype t44 = P[6][2]*t2*t10; + ftype t45 = P[0][2]*t2*t5; + ftype t46 = P[1][2]*t2*t15; + ftype t47 = P[2][2]*t2*t19; + ftype t48 = P[5][2]*t2*t27; + ftype t83 = P[4][2]*t2*t7; + ftype t84 = P[3][2]*t2*t22; + ftype t49 = t44+t45+t46+t47+t48-t83-t84; + ftype t50 = t2*t19*t49; + ftype t51 = P[6][3]*t2*t10; + ftype t52 = P[0][3]*t2*t5; + ftype t53 = P[1][3]*t2*t15; + ftype t54 = P[2][3]*t2*t19; + ftype t55 = P[5][3]*t2*t27; + ftype t85 = P[4][3]*t2*t7; + ftype t86 = P[3][3]*t2*t22; + ftype t56 = t51+t52+t53+t54+t55-t85-t86; + ftype t57 = P[6][5]*t2*t10; + ftype t58 = P[0][5]*t2*t5; + ftype t59 = P[1][5]*t2*t15; + ftype t60 = P[2][5]*t2*t19; + ftype t61 = P[5][5]*t2*t27; + ftype t88 = P[4][5]*t2*t7; + ftype t89 = P[3][5]*t2*t22; + ftype t62 = t57+t58+t59+t60+t61-t88-t89; + ftype t63 = t2*t27*t62; + ftype t64 = P[6][4]*t2*t10; + ftype t65 = P[0][4]*t2*t5; + ftype t66 = P[1][4]*t2*t15; + ftype t67 = P[2][4]*t2*t19; + ftype t68 = P[5][4]*t2*t27; + ftype t90 = P[4][4]*t2*t7; + ftype t91 = P[3][4]*t2*t22; + ftype t69 = t64+t65+t66+t67+t68-t90-t91; + ftype t70 = P[6][6]*t2*t10; + ftype t71 = P[0][6]*t2*t5; + ftype t72 = P[1][6]*t2*t15; + ftype t73 = P[2][6]*t2*t19; + ftype t74 = P[5][6]*t2*t27; + ftype t93 = P[4][6]*t2*t7; + ftype t94 = P[3][6]*t2*t22; + ftype t75 = t70+t71+t72+t73+t74-t93-t94; + ftype t76 = t2*t10*t75; + ftype t87 = t2*t22*t56; + ftype t92 = t2*t7*t69; + ftype t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; + ftype t78; // calculate innovation variance for X axis observation and protect against a badly conditioned calculation if (t77 > R_LOS) { @@ -463,8 +463,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) Kfusion[11] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27); Kfusion[12] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -477,8 +477,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } if (!inhibitMagStates) { @@ -489,22 +489,22 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) Kfusion[20] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27); Kfusion[21] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } if (!inhibitWindStates) { Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } } else { // calculate Y axis observation Jacobian - float t2 = 1.0f / range; + ftype t2 = 1.0f / range; H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); @@ -514,98 +514,98 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f); // calculate intermediate variables for the Y observation innovation variance and Kalman gains - float t3 = q3*ve*2.0f; - float t4 = q0*vn*2.0f; - float t11 = q2*vd*2.0f; - float t5 = t3+t4-t11; - float t6 = q0*q3*2.0f; - float t7 = q1*q2*2.0f; - float t8 = t6+t7; - float t9 = q0*q2*2.0f; - float t28 = q1*q3*2.0f; - float t10 = t9-t28; - float t12 = P[0][0]*t2*t5; - float t13 = q3*vd*2.0f; - float t14 = q2*ve*2.0f; - float t15 = q1*vn*2.0f; - float t16 = t13+t14+t15; - float t17 = q0*vd*2.0f; - float t18 = q2*vn*2.0f; - float t29 = q1*ve*2.0f; - float t19 = t17+t18-t29; - float t20 = q1*vd*2.0f; - float t21 = q0*ve*2.0f; - float t30 = q3*vn*2.0f; - float t22 = t20+t21-t30; - float t23 = q0*q0; - float t24 = q1*q1; - float t25 = q2*q2; - float t26 = q3*q3; - float t27 = t23+t24-t25-t26; - float t31 = P[1][1]*t2*t16; - float t32 = P[5][0]*t2*t8; - float t33 = P[1][0]*t2*t16; - float t34 = P[3][0]*t2*t22; - float t35 = P[4][0]*t2*t27; - float t80 = P[6][0]*t2*t10; - float t81 = P[2][0]*t2*t19; - float t36 = t12+t32+t33+t34+t35-t80-t81; - float t37 = t2*t5*t36; - float t38 = P[5][1]*t2*t8; - float t39 = P[0][1]*t2*t5; - float t40 = P[3][1]*t2*t22; - float t41 = P[4][1]*t2*t27; - float t82 = P[6][1]*t2*t10; - float t83 = P[2][1]*t2*t19; - float t42 = t31+t38+t39+t40+t41-t82-t83; - float t43 = t2*t16*t42; - float t44 = P[5][2]*t2*t8; - float t45 = P[0][2]*t2*t5; - float t46 = P[1][2]*t2*t16; - float t47 = P[3][2]*t2*t22; - float t48 = P[4][2]*t2*t27; - float t79 = P[2][2]*t2*t19; - float t84 = P[6][2]*t2*t10; - float t49 = t44+t45+t46+t47+t48-t79-t84; - float t50 = P[5][3]*t2*t8; - float t51 = P[0][3]*t2*t5; - float t52 = P[1][3]*t2*t16; - float t53 = P[3][3]*t2*t22; - float t54 = P[4][3]*t2*t27; - float t86 = P[6][3]*t2*t10; - float t87 = P[2][3]*t2*t19; - float t55 = t50+t51+t52+t53+t54-t86-t87; - float t56 = t2*t22*t55; - float t57 = P[5][4]*t2*t8; - float t58 = P[0][4]*t2*t5; - float t59 = P[1][4]*t2*t16; - float t60 = P[3][4]*t2*t22; - float t61 = P[4][4]*t2*t27; - float t88 = P[6][4]*t2*t10; - float t89 = P[2][4]*t2*t19; - float t62 = t57+t58+t59+t60+t61-t88-t89; - float t63 = t2*t27*t62; - float t64 = P[5][5]*t2*t8; - float t65 = P[0][5]*t2*t5; - float t66 = P[1][5]*t2*t16; - float t67 = P[3][5]*t2*t22; - float t68 = P[4][5]*t2*t27; - float t90 = P[6][5]*t2*t10; - float t91 = P[2][5]*t2*t19; - float t69 = t64+t65+t66+t67+t68-t90-t91; - float t70 = t2*t8*t69; - float t71 = P[5][6]*t2*t8; - float t72 = P[0][6]*t2*t5; - float t73 = P[1][6]*t2*t16; - float t74 = P[3][6]*t2*t22; - float t75 = P[4][6]*t2*t27; - float t92 = P[6][6]*t2*t10; - float t93 = P[2][6]*t2*t19; - float t76 = t71+t72+t73+t74+t75-t92-t93; - float t85 = t2*t19*t49; - float t94 = t2*t10*t76; - float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; - float t78; + ftype t3 = q3*ve*2.0f; + ftype t4 = q0*vn*2.0f; + ftype t11 = q2*vd*2.0f; + ftype t5 = t3+t4-t11; + ftype t6 = q0*q3*2.0f; + ftype t7 = q1*q2*2.0f; + ftype t8 = t6+t7; + ftype t9 = q0*q2*2.0f; + ftype t28 = q1*q3*2.0f; + ftype t10 = t9-t28; + ftype t12 = P[0][0]*t2*t5; + ftype t13 = q3*vd*2.0f; + ftype t14 = q2*ve*2.0f; + ftype t15 = q1*vn*2.0f; + ftype t16 = t13+t14+t15; + ftype t17 = q0*vd*2.0f; + ftype t18 = q2*vn*2.0f; + ftype t29 = q1*ve*2.0f; + ftype t19 = t17+t18-t29; + ftype t20 = q1*vd*2.0f; + ftype t21 = q0*ve*2.0f; + ftype t30 = q3*vn*2.0f; + ftype t22 = t20+t21-t30; + ftype t23 = q0*q0; + ftype t24 = q1*q1; + ftype t25 = q2*q2; + ftype t26 = q3*q3; + ftype t27 = t23+t24-t25-t26; + ftype t31 = P[1][1]*t2*t16; + ftype t32 = P[5][0]*t2*t8; + ftype t33 = P[1][0]*t2*t16; + ftype t34 = P[3][0]*t2*t22; + ftype t35 = P[4][0]*t2*t27; + ftype t80 = P[6][0]*t2*t10; + ftype t81 = P[2][0]*t2*t19; + ftype t36 = t12+t32+t33+t34+t35-t80-t81; + ftype t37 = t2*t5*t36; + ftype t38 = P[5][1]*t2*t8; + ftype t39 = P[0][1]*t2*t5; + ftype t40 = P[3][1]*t2*t22; + ftype t41 = P[4][1]*t2*t27; + ftype t82 = P[6][1]*t2*t10; + ftype t83 = P[2][1]*t2*t19; + ftype t42 = t31+t38+t39+t40+t41-t82-t83; + ftype t43 = t2*t16*t42; + ftype t44 = P[5][2]*t2*t8; + ftype t45 = P[0][2]*t2*t5; + ftype t46 = P[1][2]*t2*t16; + ftype t47 = P[3][2]*t2*t22; + ftype t48 = P[4][2]*t2*t27; + ftype t79 = P[2][2]*t2*t19; + ftype t84 = P[6][2]*t2*t10; + ftype t49 = t44+t45+t46+t47+t48-t79-t84; + ftype t50 = P[5][3]*t2*t8; + ftype t51 = P[0][3]*t2*t5; + ftype t52 = P[1][3]*t2*t16; + ftype t53 = P[3][3]*t2*t22; + ftype t54 = P[4][3]*t2*t27; + ftype t86 = P[6][3]*t2*t10; + ftype t87 = P[2][3]*t2*t19; + ftype t55 = t50+t51+t52+t53+t54-t86-t87; + ftype t56 = t2*t22*t55; + ftype t57 = P[5][4]*t2*t8; + ftype t58 = P[0][4]*t2*t5; + ftype t59 = P[1][4]*t2*t16; + ftype t60 = P[3][4]*t2*t22; + ftype t61 = P[4][4]*t2*t27; + ftype t88 = P[6][4]*t2*t10; + ftype t89 = P[2][4]*t2*t19; + ftype t62 = t57+t58+t59+t60+t61-t88-t89; + ftype t63 = t2*t27*t62; + ftype t64 = P[5][5]*t2*t8; + ftype t65 = P[0][5]*t2*t5; + ftype t66 = P[1][5]*t2*t16; + ftype t67 = P[3][5]*t2*t22; + ftype t68 = P[4][5]*t2*t27; + ftype t90 = P[6][5]*t2*t10; + ftype t91 = P[2][5]*t2*t19; + ftype t69 = t64+t65+t66+t67+t68-t90-t91; + ftype t70 = t2*t8*t69; + ftype t71 = P[5][6]*t2*t8; + ftype t72 = P[0][6]*t2*t5; + ftype t73 = P[1][6]*t2*t16; + ftype t74 = P[3][6]*t2*t22; + ftype t75 = P[4][6]*t2*t27; + ftype t92 = P[6][6]*t2*t10; + ftype t93 = P[2][6]*t2*t19; + ftype t76 = t71+t72+t73+t74+t75-t92-t93; + ftype t85 = t2*t19*t49; + ftype t94 = t2*t10*t76; + ftype t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; + ftype t78; // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation if (t77 > R_LOS) { @@ -639,8 +639,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) Kfusion[11] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27); Kfusion[12] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -653,8 +653,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } if (!inhibitMagStates) { @@ -665,21 +665,21 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) Kfusion[20] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27); Kfusion[21] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } if (!inhibitWindStates) { Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } } // calculate the innovation consistency test ratio - flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]); + flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * 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)) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 625a9a7b76..c2ca8c5a4b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -23,7 +23,7 @@ bool NavEKF3_core::healthy(void) const } // position and height innovations must be within limits when on-ground and in a static mode of operation float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]); - if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) { + if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) { return false; } @@ -98,7 +98,7 @@ void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const gyroBias.zero(); return; } - gyroBias = stateStruct.gyro_bias / dtEkfAvg; + gyroBias = (stateStruct.gyro_bias / dtEkfAvg).tofloat(); } // return accelerometer bias in m/s/s @@ -108,7 +108,7 @@ void NavEKF3_core::getAccelBias(Vector3f &accelBias) const accelBias.zero(); return; } - accelBias = stateStruct.accel_bias / dtEkfAvg; + accelBias = (stateStruct.accel_bias / dtEkfAvg).tofloat(); } // return the transformation matrix from XYZ (body) to NED axes @@ -121,7 +121,7 @@ void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const // return the quaternions defining the rotation from NED to XYZ (body) axes void NavEKF3_core::getQuaternion(Quaternion& ret) const { - ret = outputDataNew.quat; + ret = outputDataNew.quat.tofloat(); } // return the amount of yaw angle change due to the last yaw angle reset in radians @@ -136,7 +136,7 @@ uint32_t NavEKF3_core::getLastYawResetAngle(float &yawAng) const // returns the time of the last reset or 0 if no reset has ever occurred uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const { - pos = posResetNE; + pos = posResetNE.tofloat(); return lastPosReset_ms; } @@ -152,7 +152,7 @@ uint32_t NavEKF3_core::getLastPosDownReset(float &posD) const // returns the time of the last reset or 0 if no reset has ever occurred uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const { - vel = velResetNE; + vel = velResetNE.tofloat(); return lastVelReset_ms; } @@ -171,7 +171,7 @@ bool NavEKF3_core::getWind(Vector3f &wind) const void NavEKF3_core::getVelNED(Vector3f &vel) const { // correct for the IMU position offset (EKF calculations are at the IMU) - vel = outputDataNew.velocity + velOffsetNED; + vel = (outputDataNew.velocity + velOffsetNED).tofloat(); } // return estimate of true airspeed vector in body frame in m/s @@ -181,7 +181,7 @@ bool NavEKF3_core::getAirSpdVec(Vector3f &vel) const if (inhibitWindStates || PV_AidingMode == AID_NONE) { return false; } - vel = outputDataNew.velocity + velOffsetNED; + vel = (outputDataNew.velocity + velOffsetNED).tofloat(); vel.x -= stateStruct.wind_vel.x; vel.y -= stateStruct.wind_vel.y; Matrix3f Tnb; // rotation from nav to body frame @@ -218,7 +218,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) { // If the origin has been set and we have GPS, then return the GPS position relative to the origin const struct Location &gpsloc = gps.location(selected_gps); - const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc); + const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc); posNE.x = tempPosNE.x; posNE.y = tempPosNE.y; return false; @@ -369,13 +369,13 @@ bool NavEKF3_core::getOriginLLH(struct Location &loc) const // return earth magnetic field estimates in measurement units / 1000 void NavEKF3_core::getMagNED(Vector3f &magNED) const { - magNED = stateStruct.earth_magfield * 1000.0f; + magNED = (stateStruct.earth_magfield * 1000.0f).tofloat(); } // return body magnetic field estimates in measurement units / 1000 void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const { - magXYZ = stateStruct.body_magfield*1000.0f; + magXYZ = (stateStruct.body_magfield*1000.0f).tofloat(); } // return magnetometer offsets @@ -394,7 +394,7 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const !inhibitMagStates && dal.get_compass()->healthy(magSelectIndex) && variancesConverged) { - magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f; + magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0; return true; } else { magOffsets = dal.get_compass()->get_offsets(magSelectIndex); @@ -440,15 +440,15 @@ void NavEKF3_core::getSynthAirDataInnovations(Vector2f &dragInnov, float &betaIn // also return the delta in position due to the last position reset void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { - velVar = sqrtf(velTestRatio); - posVar = sqrtf(posTestRatio); - hgtVar = sqrtf(hgtTestRatio); + velVar = sqrtF(velTestRatio); + posVar = sqrtF(posTestRatio); + hgtVar = sqrtF(hgtTestRatio); // If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output - magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio)); - magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio)); - magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio)); - tasVar = sqrtf(tasTestRatio); - offset = posResetNE; + magVar.x = sqrtF(MAX(magTestRatio.x,yawTestRatio)); + magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio)); + magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio)); + tasVar = sqrtF(tasTestRatio); + offset = posResetNE.tofloat(); } // get a particular source's velocity innovations @@ -461,8 +461,8 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour if (AP_HAL::millis() - gpsVelInnovTime_ms > 500) { return false; } - innovations = gpsVelInnov; - variances = gpsVelVarInnov; + innovations = gpsVelInnov.tofloat(); + variances = gpsVelVarInnov.tofloat(); return true; #if EK3_FEATURE_EXTERNAL_NAV case AP_NavEKF_Source::SourceXY::EXTNAV: @@ -470,8 +470,8 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) { return false; } - innovations = extNavVelInnov; - variances = extNavVelVarInnov; + innovations = extNavVelInnov.tofloat(); + variances = extNavVelVarInnov.tofloat(); return true; #endif // EK3_FEATURE_EXTERNAL_NAV default: @@ -565,11 +565,11 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const // range finder is fitted for other applications float temp; if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { - temp = sqrtf(auxRngTestRatio); + temp = sqrtF(auxRngTestRatio); } else { temp = 0.0f; } - const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); + const float mag_max = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z); // send message mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, mag_max, temp, tasVar); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 608f072779..1fa8dd990d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -96,8 +96,8 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource) // record the ID of the GPS for the data we are using for the reset last_gps_idx = gps_corrected.sensor_idx; // write to state vector and compensate for offset between last GPS measurement and the EKF time horizon - stateStruct.position.x = gps_corrected.pos.x + 0.001f*gps_corrected.vel.x*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms)); - stateStruct.position.y = gps_corrected.pos.y + 0.001f*gps_corrected.vel.y*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms)); + stateStruct.position.x = gps_corrected.pos.x + 0.001*gps_corrected.vel.x*(ftype(imuDataDelayed.time_ms) - ftype(gps_corrected.time_ms)); + stateStruct.position.y = gps_corrected.pos.y + 0.001*gps_corrected.vel.y*(ftype(imuDataDelayed.time_ms) - ftype(gps_corrected.time_ms)); // set the variances using the position measurement noise parameter P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise)); } else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) { @@ -142,10 +142,10 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource) // posResetNE is updated to hold the change in position // storedOutput, outputDataNew and outputDataDelayed are updated with the change in position // lastPosReset_ms is updated with the time of the reset -void NavEKF3_core::ResetPositionNE(float posN, float posE) +void NavEKF3_core::ResetPositionNE(ftype posN, ftype posE) { // Store the position before the reset so that we can record the reset delta - const Vector3f posOrig = stateStruct.position; + const Vector3F posOrig = stateStruct.position; // Set the position states to the new position stateStruct.position.x = posN; @@ -173,10 +173,10 @@ void NavEKF3_core::ResetPositionNE(float posN, float posE) // posResetD is updated to hold the change in position // storedOutput, outputDataNew and outputDataDelayed are updated with the change in position // lastPosResetD_ms is updated with the time of the reset -void NavEKF3_core::ResetPositionD(float posD) +void NavEKF3_core::ResetPositionD(ftype posD) { // Store the position before the reset so that we can record the reset delta - const float posDOrig = stateStruct.position.z; + const ftype posDOrig = stateStruct.position.z; // write to the state vector stateStruct.position.z = posD; @@ -283,7 +283,7 @@ bool NavEKF3_core::resetHeightDatum(void) return false; } // record the old height estimate - float oldHgt = -stateStruct.position.z; + ftype oldHgt = -stateStruct.position.z; // reset the barometer so that it reads zero at the current height dal.baro().update_calibration(); // reset the height state @@ -323,18 +323,18 @@ void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const } gps_data.corrected = true; - const Vector3f &posOffsetBody = dal.gps().get_antenna_offset(gps_data.sensor_idx) - accelPosOffset; + const Vector3F posOffsetBody = dal.gps().get_antenna_offset(gps_data.sensor_idx).toftype() - accelPosOffset; if (posOffsetBody.is_zero()) { return; } // TODO use a filtered angular rate with a group delay that matches the GPS delay - Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); - Vector3f velOffsetBody = angRate % posOffsetBody; - Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); + Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); + Vector3F velOffsetBody = angRate % posOffsetBody; + Vector3F velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); gps_data.vel -= velOffsetEarth; - Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); + Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); gps_data.pos.x -= posOffsetEarth.x; gps_data.pos.y -= posOffsetEarth.y; gps_data.hgt += posOffsetEarth.z; @@ -354,11 +354,11 @@ void NavEKF3_core::CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data) if (visual_odom == nullptr) { return; } - const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset; + const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset; if (posOffsetBody.is_zero()) { return; } - Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); + Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); ext_nav_data.pos.x -= posOffsetEarth.x; ext_nav_data.pos.y -= posOffsetEarth.y; ext_nav_data.pos.z -= posOffsetEarth.z; @@ -379,23 +379,23 @@ void NavEKF3_core::CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav if (visual_odom == nullptr) { return; } - const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset; + const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset; if (posOffsetBody.is_zero()) { return; } // TODO use a filtered angular rate with a group delay that matches the sensor delay - const Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); + const Vector3F angRate = imuDataDelayed.delAng * (1.0/imuDataDelayed.delAngDT); ext_nav_vel_data.vel += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate); #endif } // calculate velocity variance helper function -void NavEKF3_core::CalculateVelInnovationsAndVariances(const Vector3f &velocity, float noise, float accel_scale, Vector3f &innovations, Vector3f &variances) const +void NavEKF3_core::CalculateVelInnovationsAndVariances(const Vector3F &velocity, ftype noise, ftype accel_scale, Vector3F &innovations, Vector3F &variances) const { // innovations are latest estimate - latest observation innovations = stateStruct.velocity - velocity; - const float obs_data_chk = sq(constrain_float(noise, 0.05f, 5.0f)) + sq(accel_scale * accNavMag); + const ftype obs_data_chk = sq(constrain_ftype(noise, 0.05, 5.0)) + sq(accel_scale * accNavMag); // calculate innovation variance. velocity states start at index 4 variances.x = P[4][4] + obs_data_chk; @@ -597,7 +597,7 @@ void NavEKF3_core::FuseVelPosNED() // declare variables used by state and covariance update calculations Vector6 R_OBS; // Measurement variances used for fusion Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only - float SK; + ftype SK; // perform sequential fusion of GPS measurements. This assumes that the // errors in the different velocity and position components are @@ -607,7 +607,7 @@ void NavEKF3_core::FuseVelPosNED() // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { // calculate additional error in GPS position caused by manoeuvring - float posErr = frontend->gpsPosVarAccScale * accNavMag; + ftype posErr = frontend->gpsPosVarAccScale * accNavMag; // To-Do: this posErr should come from external nav when fusing external nav position @@ -616,7 +616,7 @@ void NavEKF3_core::FuseVelPosNED() if (PV_AidingMode == AID_NONE) { if (tiltAlignComplete && motorsArmed) { // This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate - R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f)); + R_OBS[0] = sq(constrain_ftype(frontend->_noaidHorizNoise, 0.5f, 50.0f)); } else { // Use a smaller value to give faster initial alignment R_OBS[0] = sq(0.5f); @@ -629,40 +629,40 @@ void NavEKF3_core::FuseVelPosNED() } else { if (gpsSpdAccuracy > 0.0f) { // use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter - R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f)); - R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f)); + R_OBS[0] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f)); + R_OBS[2] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f)); #if EK3_FEATURE_EXTERNAL_NAV } else if (extNavVelToFuse) { - R_OBS[2] = R_OBS[0] = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f)); + R_OBS[2] = R_OBS[0] = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f)); #endif } 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[0] = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); + R_OBS[2] = sq(constrain_ftype(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag); } R_OBS[1] = R_OBS[0]; // Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter if (gpsPosAccuracy > 0.0f) { - R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f)); + R_OBS[3] = sq(constrain_ftype(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f)); #if EK3_FEATURE_EXTERNAL_NAV } else if (extNavUsedForPos) { - R_OBS[3] = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f)); + R_OBS[3] = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f)); #endif } else { - R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); + R_OBS[3] = sq(constrain_ftype(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); } R_OBS[4] = R_OBS[3]; // 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 performance // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early - float obs_data_chk; + ftype obs_data_chk; #if EK3_FEATURE_EXTERNAL_NAV if (extNavVelToFuse) { - obs_data_chk = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag); + obs_data_chk = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag); } else #endif { - obs_data_chk = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); + obs_data_chk = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); } R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk; } @@ -674,8 +674,8 @@ void NavEKF3_core::FuseVelPosNED() // the accelerometers and we should disable the GPS and barometer innovation consistency checks. if (gpsDataDelayed.have_vz && fuseVelData && (frontend->sources.getPosZSource() != AP_NavEKF_Source::SourceZ::GPS)) { // calculate innovations for height and vertical GPS vel measurements - const float hgtErr = stateStruct.position.z - velPosObs[5]; - const float velDErr = stateStruct.velocity.z - velPosObs[2]; + const ftype hgtErr = stateStruct.position.z - velPosObs[5]; + const ftype velDErr = stateStruct.velocity.z - velPosObs[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[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) { badIMUdata = true; @@ -695,7 +695,8 @@ void NavEKF3_core::FuseVelPosNED() // Don't allow test to fail if not navigating and using a constant position // assumption to constrain tilt errors because innovations can become large // due to vehicle motion. - float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]); + ftype maxPosInnov2 = sq(MAX(0.01 * (ftype)frontend->_gpsPosInnovGate, 1.0))*(varInnovVelPos[3] + varInnovVelPos[4]); + posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; if (posTestRatio < 1.0f || (PV_AidingMode == AID_NONE)) { posCheckPassed = true; @@ -707,7 +708,7 @@ void NavEKF3_core::FuseVelPosNED() // from the measurement un-opposed if test threshold is exceeded. if (posCheckPassed || posTimeout || badIMUdata) { // if timed out or outside the specified uncertainty radius, reset to the external sensor - if (posTimeout || ((P[8][8] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) { + if (posTimeout || ((P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax)))) { // reset the position to the current external sensor position ResetPosition(resetDataSource::DEFAULT); @@ -717,7 +718,7 @@ void NavEKF3_core::FuseVelPosNED() // Reset the position variances and corresponding covariances to a value that will pass the checks zeroRows(P,7,8); zeroCols(P,7,8); - P[7][7] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax)); + P[7][7] = sq(ftype(0.5f*frontend->_gpsGlitchRadiusMax)); P[8][8] = P[7][7]; // Reset the normalised innovation to avoid failing the bad fusion tests @@ -749,8 +750,9 @@ void NavEKF3_core::FuseVelPosNED() } // Apply an innovation consistency threshold test - float innovVelSumSq = 0; // sum of squares of velocity innovations - float varVelSum = 0; // sum of velocity innovation variances + ftype innovVelSumSq = 0; // sum of squares of velocity innovations + ftype varVelSum = 0; // sum of velocity innovation variances + for (uint8_t i = 0; i<=imax; i++) { stateIndex = i + 4; const float innovation = stateStruct.velocity[i] - velPosObs[i]; @@ -758,8 +760,8 @@ void NavEKF3_core::FuseVelPosNED() varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i]; varVelSum += varInnovVelPos[i]; } - velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f))); - if (velTestRatio < 1.0f) { + velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01 * (ftype)frontend->_gpsVelInnovGate, 1.0))); + if (velTestRatio < 1.0) { velCheckPassed = true; lastVelPassTime_ms = imuSampleTime_ms; } @@ -790,7 +792,7 @@ void NavEKF3_core::FuseVelPosNED() varInnovVelPos[5] = P[9][9] + 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]); + hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01 * (ftype)frontend->_hgtInnovGate, 1.0)) * varInnovVelPos[5]); // When on ground we accept a larger test ratio to allow the filter to handle large switch on IMU // bias errors without rejecting the height sensor. @@ -807,9 +809,9 @@ void NavEKF3_core::FuseVelPosNED() // 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); + ftype dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms) * 1.0e-3; + const ftype hgtInnovFiltTC = 2.0; + ftype alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC), 0.0, 1.0); hgtInnovFiltState += (innovVelPos[5] - hgtInnovFiltState)*alpha; } else { hgtInnovFiltState = 0.0f; @@ -857,8 +859,8 @@ void NavEKF3_core::FuseVelPosNED() R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 5) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex]; - const float gndMaxBaroErr = MAX(frontend->_baroGndEffectDeadZone, 0.0f); - const float gndBaroInnovFloor = -0.5f; + const ftype gndMaxBaroErr = MAX(frontend->_baroGndEffectDeadZone, 0.0); + const ftype gndBaroInnovFloor = -0.5; if ((dal.get_touchdown_expected() || dal.get_takeoff_expected()) && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) { // when baro positive pressure error due to ground effect is expected, @@ -870,7 +872,7 @@ void NavEKF3_core::FuseVelPosNED() // ____/| // / | // / | - innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); + innovVelPos[5] += constrain_ftype(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); } } @@ -890,11 +892,11 @@ void NavEKF3_core::FuseVelPosNED() if (PV_AidingMode == AID_NONE) { const uint8_t axisIndex = i - 10; if (axisIndex == 0) { - poorObservability = fabsf(prevTnb.a.z) > M_SQRT1_2; + poorObservability = fabsF(prevTnb.a.z) > M_SQRT1_2; } else if (axisIndex == 1) { - poorObservability = fabsf(prevTnb.b.z) > M_SQRT1_2; + poorObservability = fabsF(prevTnb.b.z) > M_SQRT1_2; } else { - poorObservability = fabsf(prevTnb.c.z) > M_SQRT1_2; + poorObservability = fabsF(prevTnb.c.z) > M_SQRT1_2; } } if (poorObservability) { @@ -904,8 +906,8 @@ void NavEKF3_core::FuseVelPosNED() } } } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } // Inhibit delta velocity bias state estimation by setting Kalman gains to zero @@ -921,8 +923,8 @@ void NavEKF3_core::FuseVelPosNED() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } // inhibit magnetic field state estimation by setting Kalman gains to zero @@ -931,8 +933,8 @@ void NavEKF3_core::FuseVelPosNED() Kfusion[i] = P[i][stateIndex]*SK; } } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } // inhibit wind state estimation by setting Kalman gains to zero @@ -940,8 +942,8 @@ void NavEKF3_core::FuseVelPosNED() Kfusion[22] = P[22][stateIndex]*SK; Kfusion[23] = P[23][stateIndex]*SK; } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations @@ -1030,9 +1032,9 @@ void NavEKF3_core::selectHeightForFusion() if (_rng && rangeDataToFuse) { auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx); if (sensor != nullptr) { - Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset; + Vector3F posOffsetBody = sensor->get_pos_offset().toftype() - accelPosOffset; if (!posOffsetBody.is_zero()) { - Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); + Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z; } } @@ -1052,7 +1054,7 @@ void NavEKF3_core::selectHeightForFusion() activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) || (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) { // determine if we are above or below the height switch region - float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt; + ftype rangeMaxUse = 1e-4 * (ftype)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; @@ -1062,9 +1064,9 @@ void NavEKF3_core::selectHeightForFusion() bool dontTrustTerrain, trustTerrain; if (filterStatus.flags.horiz_vel) { // We can use the velocity estimate - float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y); + ftype horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y); dontTrustTerrain = (horizSpeed > frontend->_useRngSwSpd) || !terrainHgtStable; - float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f)); + ftype trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f)); trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable; } else { // We can't use the velocity estimate @@ -1122,9 +1124,9 @@ void NavEKF3_core::selectHeightForFusion() // filtered baro data used to provide a reference for takeoff // it is is reset to last height measurement on disarming in performArmingChecks() if (!dal.get_takeoff_expected()) { - const float gndHgtFiltTC = 0.5f; - const float dtBaro = frontend->hgtAvg_ms*1.0e-3f; - float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f); + const ftype gndHgtFiltTC = 0.5; + const ftype dtBaro = frontend->hgtAvg_ms*1.0e-3; + ftype alpha = constrain_ftype(dtBaro / (dtBaro+gndHgtFiltTC),0.0,1.0); meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha; } } @@ -1144,7 +1146,7 @@ void NavEKF3_core::selectHeightForFusion() if (extNavDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) { hgtMea = -extNavDataDelayed.pos.z; velPosObs[5] = -hgtMea; - posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); + posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f)); fuseHgtData = true; } else #endif // EK3_FEATURE_EXTERNAL_NAV @@ -1160,7 +1162,7 @@ void NavEKF3_core::selectHeightForFusion() // enable fusion fuseHgtData = true; // set the observation noise - posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f)); + posDownObsNoise = sq(constrain_ftype(frontend->_rngNoise, 0.1f, 10.0f)); // add uncertainty created by terrain gradient and vehicle tilt posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z))); } else { @@ -1175,9 +1177,9 @@ void NavEKF3_core::selectHeightForFusion() fuseHgtData = true; // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio if (gpsHgtAccuracy > 0.0f) { - posDownObsNoise = sq(constrain_float(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f)); + posDownObsNoise = sq(constrain_ftype(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f)); } else { - posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f)); + posDownObsNoise = sq(constrain_ftype(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f)); } } else if (baroDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) { // using Baro data @@ -1185,7 +1187,7 @@ void NavEKF3_core::selectHeightForFusion() // enable fusion fuseHgtData = true; // set the observation noise - posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f)); + posDownObsNoise = sq(constrain_ftype(frontend->_baroAltNoise, 0.1f, 10.0f)); // reduce weighting (increase observation noise) on baro if we are likely to be experiencing rotor wash ground interaction if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) { posDownObsNoise *= frontend->gndEffectBaroScaler; @@ -1220,16 +1222,16 @@ void NavEKF3_core::selectHeightForFusion() void NavEKF3_core::FuseBodyVel() { Vector24 H_VEL; - Vector3f bodyVelPred; + Vector3F bodyVelPred; // 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; + ftype q0 = stateStruct.quat[0]; + ftype q1 = stateStruct.quat[1]; + ftype q2 = stateStruct.quat[2]; + ftype q3 = stateStruct.quat[3]; + ftype vn = stateStruct.velocity.x; + ftype ve = stateStruct.velocity.y; + ftype vd = stateStruct.velocity.z; // Fuse X, Y and Z axis measurements sequentially assuming observation errors are uncorrelated for (uint8_t obsIndex=0; obsIndex<=2; obsIndex++) { @@ -1238,7 +1240,7 @@ void NavEKF3_core::FuseBodyVel() bodyVelPred = (prevTnb * stateStruct.velocity); // correct sensor offset body frame position offset relative to IMU - Vector3f posOffsetBody = bodyOdmDataDelayed.body_offset - accelPosOffset; + Vector3F posOffsetBody = bodyOdmDataDelayed.body_offset - accelPosOffset; // correct prediction for relative motion due to rotation // note - % operator overloaded for cross product @@ -1261,99 +1263,99 @@ void NavEKF3_core::FuseBodyVel() } // calculate intermediate expressions for X axis Kalman gains - float R_VEL = sq(bodyOdmDataDelayed.velErr); - float t2 = q0*q3*2.0f; - float t3 = q1*q2*2.0f; - float t4 = t2+t3; - float t5 = q0*q0; - float t6 = q1*q1; - float t7 = q2*q2; - float t8 = q3*q3; - float t9 = t5+t6-t7-t8; - float t10 = q0*q2*2.0f; - float t25 = q1*q3*2.0f; - float t11 = t10-t25; - float t12 = q3*ve*2.0f; - float t13 = q0*vn*2.0f; - float t26 = q2*vd*2.0f; - float t14 = t12+t13-t26; - float t15 = q3*vd*2.0f; - float t16 = q2*ve*2.0f; - float t17 = q1*vn*2.0f; - float t18 = t15+t16+t17; - float t19 = q0*vd*2.0f; - float t20 = q2*vn*2.0f; - float t27 = q1*ve*2.0f; - float t21 = t19+t20-t27; - float t22 = q1*vd*2.0f; - float t23 = q0*ve*2.0f; - float t28 = q3*vn*2.0f; - float t24 = t22+t23-t28; - float t29 = P[0][0]*t14; - float t30 = P[1][1]*t18; - float t31 = P[4][5]*t9; - float t32 = P[5][5]*t4; - float t33 = P[0][5]*t14; - float t34 = P[1][5]*t18; - float t35 = P[3][5]*t24; - float t79 = P[6][5]*t11; - float t80 = P[2][5]*t21; - float t36 = t31+t32+t33+t34+t35-t79-t80; - float t37 = t4*t36; - float t38 = P[4][6]*t9; - float t39 = P[5][6]*t4; - float t40 = P[0][6]*t14; - float t41 = P[1][6]*t18; - float t42 = P[3][6]*t24; - float t81 = P[6][6]*t11; - float t82 = P[2][6]*t21; - float t43 = t38+t39+t40+t41+t42-t81-t82; - float t44 = P[4][0]*t9; - float t45 = P[5][0]*t4; - float t46 = P[1][0]*t18; - float t47 = P[3][0]*t24; - float t84 = P[6][0]*t11; - float t85 = P[2][0]*t21; - float t48 = t29+t44+t45+t46+t47-t84-t85; - float t49 = t14*t48; - float t50 = P[4][1]*t9; - float t51 = P[5][1]*t4; - float t52 = P[0][1]*t14; - float t53 = P[3][1]*t24; - float t86 = P[6][1]*t11; - float t87 = P[2][1]*t21; - float t54 = t30+t50+t51+t52+t53-t86-t87; - float t55 = t18*t54; - float t56 = P[4][2]*t9; - float t57 = P[5][2]*t4; - float t58 = P[0][2]*t14; - float t59 = P[1][2]*t18; - float t60 = P[3][2]*t24; - float t78 = P[2][2]*t21; - float t88 = P[6][2]*t11; - float t61 = t56+t57+t58+t59+t60-t78-t88; - float t62 = P[4][3]*t9; - float t63 = P[5][3]*t4; - float t64 = P[0][3]*t14; - float t65 = P[1][3]*t18; - float t66 = P[3][3]*t24; - float t90 = P[6][3]*t11; - float t91 = P[2][3]*t21; - float t67 = t62+t63+t64+t65+t66-t90-t91; - float t68 = t24*t67; - float t69 = P[4][4]*t9; - float t70 = P[5][4]*t4; - float t71 = P[0][4]*t14; - float t72 = P[1][4]*t18; - float t73 = P[3][4]*t24; - float t92 = P[6][4]*t11; - float t93 = P[2][4]*t21; - float t74 = t69+t70+t71+t72+t73-t92-t93; - float t75 = t9*t74; - float t83 = t11*t43; - float t89 = t21*t61; - float t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89; - float t77; + ftype R_VEL = sq(bodyOdmDataDelayed.velErr); + ftype t2 = q0*q3*2.0f; + ftype t3 = q1*q2*2.0f; + ftype t4 = t2+t3; + ftype t5 = q0*q0; + ftype t6 = q1*q1; + ftype t7 = q2*q2; + ftype t8 = q3*q3; + ftype t9 = t5+t6-t7-t8; + ftype t10 = q0*q2*2.0f; + ftype t25 = q1*q3*2.0f; + ftype t11 = t10-t25; + ftype t12 = q3*ve*2.0f; + ftype t13 = q0*vn*2.0f; + ftype t26 = q2*vd*2.0f; + ftype t14 = t12+t13-t26; + ftype t15 = q3*vd*2.0f; + ftype t16 = q2*ve*2.0f; + ftype t17 = q1*vn*2.0f; + ftype t18 = t15+t16+t17; + ftype t19 = q0*vd*2.0f; + ftype t20 = q2*vn*2.0f; + ftype t27 = q1*ve*2.0f; + ftype t21 = t19+t20-t27; + ftype t22 = q1*vd*2.0f; + ftype t23 = q0*ve*2.0f; + ftype t28 = q3*vn*2.0f; + ftype t24 = t22+t23-t28; + ftype t29 = P[0][0]*t14; + ftype t30 = P[1][1]*t18; + ftype t31 = P[4][5]*t9; + ftype t32 = P[5][5]*t4; + ftype t33 = P[0][5]*t14; + ftype t34 = P[1][5]*t18; + ftype t35 = P[3][5]*t24; + ftype t79 = P[6][5]*t11; + ftype t80 = P[2][5]*t21; + ftype t36 = t31+t32+t33+t34+t35-t79-t80; + ftype t37 = t4*t36; + ftype t38 = P[4][6]*t9; + ftype t39 = P[5][6]*t4; + ftype t40 = P[0][6]*t14; + ftype t41 = P[1][6]*t18; + ftype t42 = P[3][6]*t24; + ftype t81 = P[6][6]*t11; + ftype t82 = P[2][6]*t21; + ftype t43 = t38+t39+t40+t41+t42-t81-t82; + ftype t44 = P[4][0]*t9; + ftype t45 = P[5][0]*t4; + ftype t46 = P[1][0]*t18; + ftype t47 = P[3][0]*t24; + ftype t84 = P[6][0]*t11; + ftype t85 = P[2][0]*t21; + ftype t48 = t29+t44+t45+t46+t47-t84-t85; + ftype t49 = t14*t48; + ftype t50 = P[4][1]*t9; + ftype t51 = P[5][1]*t4; + ftype t52 = P[0][1]*t14; + ftype t53 = P[3][1]*t24; + ftype t86 = P[6][1]*t11; + ftype t87 = P[2][1]*t21; + ftype t54 = t30+t50+t51+t52+t53-t86-t87; + ftype t55 = t18*t54; + ftype t56 = P[4][2]*t9; + ftype t57 = P[5][2]*t4; + ftype t58 = P[0][2]*t14; + ftype t59 = P[1][2]*t18; + ftype t60 = P[3][2]*t24; + ftype t78 = P[2][2]*t21; + ftype t88 = P[6][2]*t11; + ftype t61 = t56+t57+t58+t59+t60-t78-t88; + ftype t62 = P[4][3]*t9; + ftype t63 = P[5][3]*t4; + ftype t64 = P[0][3]*t14; + ftype t65 = P[1][3]*t18; + ftype t66 = P[3][3]*t24; + ftype t90 = P[6][3]*t11; + ftype t91 = P[2][3]*t21; + ftype t67 = t62+t63+t64+t65+t66-t90-t91; + ftype t68 = t24*t67; + ftype t69 = P[4][4]*t9; + ftype t70 = P[5][4]*t4; + ftype t71 = P[0][4]*t14; + ftype t72 = P[1][4]*t18; + ftype t73 = P[3][4]*t24; + ftype t92 = P[6][4]*t11; + ftype t93 = P[2][4]*t21; + ftype t74 = t69+t70+t71+t72+t73-t92-t93; + ftype t75 = t9*t74; + ftype t83 = t11*t43; + ftype t89 = t21*t61; + ftype t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89; + ftype t77; // calculate innovation variance for X axis observation and protect against a badly conditioned calculation if (t76 > R_VEL) { @@ -1387,8 +1389,8 @@ void NavEKF3_core::FuseBodyVel() Kfusion[11] = t77*(P[11][5]*t4+P[11][4]*t9+P[11][0]*t14-P[11][6]*t11+P[11][1]*t18-P[11][2]*t21+P[11][3]*t24); Kfusion[12] = t77*(P[12][5]*t4+P[12][4]*t9+P[12][0]*t14-P[12][6]*t11+P[12][1]*t18-P[12][2]*t21+P[12][3]*t24); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -1401,8 +1403,8 @@ void NavEKF3_core::FuseBodyVel() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 = 3 + zero_range(&Kfusion[0], 13, 15); } if (!inhibitMagStates) { @@ -1413,16 +1415,16 @@ void NavEKF3_core::FuseBodyVel() Kfusion[20] = t77*(P[20][5]*t4+P[20][4]*t9+P[20][0]*t14-P[20][6]*t11+P[20][1]*t18-P[20][2]*t21+P[20][3]*t24); Kfusion[21] = t77*(P[21][5]*t4+P[21][4]*t9+P[21][0]*t14-P[21][6]*t11+P[21][1]*t18-P[21][2]*t21+P[21][3]*t24); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } if (!inhibitWindStates) { Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24); Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } } else if (obsIndex == 1) { // calculate Y axis observation Jacobian @@ -1438,99 +1440,99 @@ void NavEKF3_core::FuseBodyVel() } // calculate intermediate expressions for Y axis Kalman gains - float R_VEL = sq(bodyOdmDataDelayed.velErr); - float t2 = q0*q3*2.0f; - float t9 = q1*q2*2.0f; - float t3 = t2-t9; - float t4 = q0*q0; - float t5 = q1*q1; - float t6 = q2*q2; - float t7 = q3*q3; - float t8 = t4-t5+t6-t7; - float t10 = q0*q1*2.0f; - float t11 = q2*q3*2.0f; - float t12 = t10+t11; - float t13 = q1*vd*2.0f; - float t14 = q0*ve*2.0f; - float t26 = q3*vn*2.0f; - float t15 = t13+t14-t26; - float t16 = q0*vd*2.0f; - float t17 = q2*vn*2.0f; - float t27 = q1*ve*2.0f; - float t18 = t16+t17-t27; - float t19 = q3*vd*2.0f; - float t20 = q2*ve*2.0f; - float t21 = q1*vn*2.0f; - float t22 = t19+t20+t21; - float t23 = q3*ve*2.0f; - float t24 = q0*vn*2.0f; - float t28 = q2*vd*2.0f; - float t25 = t23+t24-t28; - float t29 = P[0][0]*t15; - float t30 = P[1][1]*t18; - float t31 = P[5][4]*t8; - float t32 = P[6][4]*t12; - float t33 = P[0][4]*t15; - float t34 = P[1][4]*t18; - float t35 = P[2][4]*t22; - float t78 = P[4][4]*t3; - float t79 = P[3][4]*t25; - float t36 = t31+t32+t33+t34+t35-t78-t79; - float t37 = P[5][6]*t8; - float t38 = P[6][6]*t12; - float t39 = P[0][6]*t15; - float t40 = P[1][6]*t18; - float t41 = P[2][6]*t22; - float t81 = P[4][6]*t3; - float t82 = P[3][6]*t25; - float t42 = t37+t38+t39+t40+t41-t81-t82; - float t43 = t12*t42; - float t44 = P[5][0]*t8; - float t45 = P[6][0]*t12; - float t46 = P[1][0]*t18; - float t47 = P[2][0]*t22; - float t83 = P[4][0]*t3; - float t84 = P[3][0]*t25; - float t48 = t29+t44+t45+t46+t47-t83-t84; - float t49 = t15*t48; - float t50 = P[5][1]*t8; - float t51 = P[6][1]*t12; - float t52 = P[0][1]*t15; - float t53 = P[2][1]*t22; - float t85 = P[4][1]*t3; - float t86 = P[3][1]*t25; - float t54 = t30+t50+t51+t52+t53-t85-t86; - float t55 = t18*t54; - float t56 = P[5][2]*t8; - float t57 = P[6][2]*t12; - float t58 = P[0][2]*t15; - float t59 = P[1][2]*t18; - float t60 = P[2][2]*t22; - float t87 = P[4][2]*t3; - float t88 = P[3][2]*t25; - float t61 = t56+t57+t58+t59+t60-t87-t88; - float t62 = t22*t61; - float t63 = P[5][3]*t8; - float t64 = P[6][3]*t12; - float t65 = P[0][3]*t15; - float t66 = P[1][3]*t18; - float t67 = P[2][3]*t22; - float t89 = P[4][3]*t3; - float t90 = P[3][3]*t25; - float t68 = t63+t64+t65+t66+t67-t89-t90; - float t69 = P[5][5]*t8; - float t70 = P[6][5]*t12; - float t71 = P[0][5]*t15; - float t72 = P[1][5]*t18; - float t73 = P[2][5]*t22; - float t92 = P[4][5]*t3; - float t93 = P[3][5]*t25; - float t74 = t69+t70+t71+t72+t73-t92-t93; - float t75 = t8*t74; - float t80 = t3*t36; - float t91 = t25*t68; - float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91; - float t77; + ftype R_VEL = sq(bodyOdmDataDelayed.velErr); + ftype t2 = q0*q3*2.0f; + ftype t9 = q1*q2*2.0f; + ftype t3 = t2-t9; + ftype t4 = q0*q0; + ftype t5 = q1*q1; + ftype t6 = q2*q2; + ftype t7 = q3*q3; + ftype t8 = t4-t5+t6-t7; + ftype t10 = q0*q1*2.0f; + ftype t11 = q2*q3*2.0f; + ftype t12 = t10+t11; + ftype t13 = q1*vd*2.0f; + ftype t14 = q0*ve*2.0f; + ftype t26 = q3*vn*2.0f; + ftype t15 = t13+t14-t26; + ftype t16 = q0*vd*2.0f; + ftype t17 = q2*vn*2.0f; + ftype t27 = q1*ve*2.0f; + ftype t18 = t16+t17-t27; + ftype t19 = q3*vd*2.0f; + ftype t20 = q2*ve*2.0f; + ftype t21 = q1*vn*2.0f; + ftype t22 = t19+t20+t21; + ftype t23 = q3*ve*2.0f; + ftype t24 = q0*vn*2.0f; + ftype t28 = q2*vd*2.0f; + ftype t25 = t23+t24-t28; + ftype t29 = P[0][0]*t15; + ftype t30 = P[1][1]*t18; + ftype t31 = P[5][4]*t8; + ftype t32 = P[6][4]*t12; + ftype t33 = P[0][4]*t15; + ftype t34 = P[1][4]*t18; + ftype t35 = P[2][4]*t22; + ftype t78 = P[4][4]*t3; + ftype t79 = P[3][4]*t25; + ftype t36 = t31+t32+t33+t34+t35-t78-t79; + ftype t37 = P[5][6]*t8; + ftype t38 = P[6][6]*t12; + ftype t39 = P[0][6]*t15; + ftype t40 = P[1][6]*t18; + ftype t41 = P[2][6]*t22; + ftype t81 = P[4][6]*t3; + ftype t82 = P[3][6]*t25; + ftype t42 = t37+t38+t39+t40+t41-t81-t82; + ftype t43 = t12*t42; + ftype t44 = P[5][0]*t8; + ftype t45 = P[6][0]*t12; + ftype t46 = P[1][0]*t18; + ftype t47 = P[2][0]*t22; + ftype t83 = P[4][0]*t3; + ftype t84 = P[3][0]*t25; + ftype t48 = t29+t44+t45+t46+t47-t83-t84; + ftype t49 = t15*t48; + ftype t50 = P[5][1]*t8; + ftype t51 = P[6][1]*t12; + ftype t52 = P[0][1]*t15; + ftype t53 = P[2][1]*t22; + ftype t85 = P[4][1]*t3; + ftype t86 = P[3][1]*t25; + ftype t54 = t30+t50+t51+t52+t53-t85-t86; + ftype t55 = t18*t54; + ftype t56 = P[5][2]*t8; + ftype t57 = P[6][2]*t12; + ftype t58 = P[0][2]*t15; + ftype t59 = P[1][2]*t18; + ftype t60 = P[2][2]*t22; + ftype t87 = P[4][2]*t3; + ftype t88 = P[3][2]*t25; + ftype t61 = t56+t57+t58+t59+t60-t87-t88; + ftype t62 = t22*t61; + ftype t63 = P[5][3]*t8; + ftype t64 = P[6][3]*t12; + ftype t65 = P[0][3]*t15; + ftype t66 = P[1][3]*t18; + ftype t67 = P[2][3]*t22; + ftype t89 = P[4][3]*t3; + ftype t90 = P[3][3]*t25; + ftype t68 = t63+t64+t65+t66+t67-t89-t90; + ftype t69 = P[5][5]*t8; + ftype t70 = P[6][5]*t12; + ftype t71 = P[0][5]*t15; + ftype t72 = P[1][5]*t18; + ftype t73 = P[2][5]*t22; + ftype t92 = P[4][5]*t3; + ftype t93 = P[3][5]*t25; + ftype t74 = t69+t70+t71+t72+t73-t92-t93; + ftype t75 = t8*t74; + ftype t80 = t3*t36; + ftype t91 = t25*t68; + ftype t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91; + ftype t77; // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation if (t76 > R_VEL) { @@ -1564,8 +1566,8 @@ void NavEKF3_core::FuseBodyVel() Kfusion[11] = t77*(-P[11][4]*t3+P[11][5]*t8+P[11][0]*t15+P[11][6]*t12+P[11][1]*t18+P[11][2]*t22-P[11][3]*t25); Kfusion[12] = t77*(-P[12][4]*t3+P[12][5]*t8+P[12][0]*t15+P[12][6]*t12+P[12][1]*t18+P[12][2]*t22-P[12][3]*t25); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 = 3 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -1578,8 +1580,8 @@ void NavEKF3_core::FuseBodyVel() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } if (!inhibitMagStates) { @@ -1590,16 +1592,16 @@ void NavEKF3_core::FuseBodyVel() Kfusion[20] = t77*(-P[20][4]*t3+P[20][5]*t8+P[20][0]*t15+P[20][6]*t12+P[20][1]*t18+P[20][2]*t22-P[20][3]*t25); Kfusion[21] = t77*(-P[21][4]*t3+P[21][5]*t8+P[21][0]*t15+P[21][6]*t12+P[21][1]*t18+P[21][2]*t22-P[21][3]*t25); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } if (!inhibitWindStates) { Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25); Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } } else if (obsIndex == 2) { // calculate Z axis observation Jacobian @@ -1615,99 +1617,99 @@ void NavEKF3_core::FuseBodyVel() } // calculate intermediate expressions for Z axis Kalman gains - float R_VEL = sq(bodyOdmDataDelayed.velErr); - float t2 = q0*q2*2.0f; - float t3 = q1*q3*2.0f; - float t4 = t2+t3; - float t5 = q0*q0; - float t6 = q1*q1; - float t7 = q2*q2; - float t8 = q3*q3; - float t9 = t5-t6-t7+t8; - float t10 = q0*q1*2.0f; - float t25 = q2*q3*2.0f; - float t11 = t10-t25; - float t12 = q0*vd*2.0f; - float t13 = q2*vn*2.0f; - float t26 = q1*ve*2.0f; - float t14 = t12+t13-t26; - float t15 = q1*vd*2.0f; - float t16 = q0*ve*2.0f; - float t27 = q3*vn*2.0f; - float t17 = t15+t16-t27; - float t18 = q3*ve*2.0f; - float t19 = q0*vn*2.0f; - float t28 = q2*vd*2.0f; - float t20 = t18+t19-t28; - float t21 = q3*vd*2.0f; - float t22 = q2*ve*2.0f; - float t23 = q1*vn*2.0f; - float t24 = t21+t22+t23; - float t29 = P[0][0]*t14; - float t30 = P[6][4]*t9; - float t31 = P[4][4]*t4; - float t32 = P[0][4]*t14; - float t33 = P[2][4]*t20; - float t34 = P[3][4]*t24; - float t78 = P[5][4]*t11; - float t79 = P[1][4]*t17; - float t35 = t30+t31+t32+t33+t34-t78-t79; - float t36 = t4*t35; - float t37 = P[6][5]*t9; - float t38 = P[4][5]*t4; - float t39 = P[0][5]*t14; - float t40 = P[2][5]*t20; - float t41 = P[3][5]*t24; - float t80 = P[5][5]*t11; - float t81 = P[1][5]*t17; - float t42 = t37+t38+t39+t40+t41-t80-t81; - float t43 = P[6][0]*t9; - float t44 = P[4][0]*t4; - float t45 = P[2][0]*t20; - float t46 = P[3][0]*t24; - float t83 = P[5][0]*t11; - float t84 = P[1][0]*t17; - float t47 = t29+t43+t44+t45+t46-t83-t84; - float t48 = t14*t47; - float t49 = P[6][1]*t9; - float t50 = P[4][1]*t4; - float t51 = P[0][1]*t14; - float t52 = P[2][1]*t20; - float t53 = P[3][1]*t24; - float t85 = P[5][1]*t11; - float t86 = P[1][1]*t17; - float t54 = t49+t50+t51+t52+t53-t85-t86; - float t55 = P[6][2]*t9; - float t56 = P[4][2]*t4; - float t57 = P[0][2]*t14; - float t58 = P[2][2]*t20; - float t59 = P[3][2]*t24; - float t88 = P[5][2]*t11; - float t89 = P[1][2]*t17; - float t60 = t55+t56+t57+t58+t59-t88-t89; - float t61 = t20*t60; - float t62 = P[6][3]*t9; - float t63 = P[4][3]*t4; - float t64 = P[0][3]*t14; - float t65 = P[2][3]*t20; - float t66 = P[3][3]*t24; - float t90 = P[5][3]*t11; - float t91 = P[1][3]*t17; - float t67 = t62+t63+t64+t65+t66-t90-t91; - float t68 = t24*t67; - float t69 = P[6][6]*t9; - float t70 = P[4][6]*t4; - float t71 = P[0][6]*t14; - float t72 = P[2][6]*t20; - float t73 = P[3][6]*t24; - float t92 = P[5][6]*t11; - float t93 = P[1][6]*t17; - float t74 = t69+t70+t71+t72+t73-t92-t93; - float t75 = t9*t74; - float t82 = t11*t42; - float t87 = t17*t54; - float t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87; - float t77; + ftype R_VEL = sq(bodyOdmDataDelayed.velErr); + ftype t2 = q0*q2*2.0f; + ftype t3 = q1*q3*2.0f; + ftype t4 = t2+t3; + ftype t5 = q0*q0; + ftype t6 = q1*q1; + ftype t7 = q2*q2; + ftype t8 = q3*q3; + ftype t9 = t5-t6-t7+t8; + ftype t10 = q0*q1*2.0f; + ftype t25 = q2*q3*2.0f; + ftype t11 = t10-t25; + ftype t12 = q0*vd*2.0f; + ftype t13 = q2*vn*2.0f; + ftype t26 = q1*ve*2.0f; + ftype t14 = t12+t13-t26; + ftype t15 = q1*vd*2.0f; + ftype t16 = q0*ve*2.0f; + ftype t27 = q3*vn*2.0f; + ftype t17 = t15+t16-t27; + ftype t18 = q3*ve*2.0f; + ftype t19 = q0*vn*2.0f; + ftype t28 = q2*vd*2.0f; + ftype t20 = t18+t19-t28; + ftype t21 = q3*vd*2.0f; + ftype t22 = q2*ve*2.0f; + ftype t23 = q1*vn*2.0f; + ftype t24 = t21+t22+t23; + ftype t29 = P[0][0]*t14; + ftype t30 = P[6][4]*t9; + ftype t31 = P[4][4]*t4; + ftype t32 = P[0][4]*t14; + ftype t33 = P[2][4]*t20; + ftype t34 = P[3][4]*t24; + ftype t78 = P[5][4]*t11; + ftype t79 = P[1][4]*t17; + ftype t35 = t30+t31+t32+t33+t34-t78-t79; + ftype t36 = t4*t35; + ftype t37 = P[6][5]*t9; + ftype t38 = P[4][5]*t4; + ftype t39 = P[0][5]*t14; + ftype t40 = P[2][5]*t20; + ftype t41 = P[3][5]*t24; + ftype t80 = P[5][5]*t11; + ftype t81 = P[1][5]*t17; + ftype t42 = t37+t38+t39+t40+t41-t80-t81; + ftype t43 = P[6][0]*t9; + ftype t44 = P[4][0]*t4; + ftype t45 = P[2][0]*t20; + ftype t46 = P[3][0]*t24; + ftype t83 = P[5][0]*t11; + ftype t84 = P[1][0]*t17; + ftype t47 = t29+t43+t44+t45+t46-t83-t84; + ftype t48 = t14*t47; + ftype t49 = P[6][1]*t9; + ftype t50 = P[4][1]*t4; + ftype t51 = P[0][1]*t14; + ftype t52 = P[2][1]*t20; + ftype t53 = P[3][1]*t24; + ftype t85 = P[5][1]*t11; + ftype t86 = P[1][1]*t17; + ftype t54 = t49+t50+t51+t52+t53-t85-t86; + ftype t55 = P[6][2]*t9; + ftype t56 = P[4][2]*t4; + ftype t57 = P[0][2]*t14; + ftype t58 = P[2][2]*t20; + ftype t59 = P[3][2]*t24; + ftype t88 = P[5][2]*t11; + ftype t89 = P[1][2]*t17; + ftype t60 = t55+t56+t57+t58+t59-t88-t89; + ftype t61 = t20*t60; + ftype t62 = P[6][3]*t9; + ftype t63 = P[4][3]*t4; + ftype t64 = P[0][3]*t14; + ftype t65 = P[2][3]*t20; + ftype t66 = P[3][3]*t24; + ftype t90 = P[5][3]*t11; + ftype t91 = P[1][3]*t17; + ftype t67 = t62+t63+t64+t65+t66-t90-t91; + ftype t68 = t24*t67; + ftype t69 = P[6][6]*t9; + ftype t70 = P[4][6]*t4; + ftype t71 = P[0][6]*t14; + ftype t72 = P[2][6]*t20; + ftype t73 = P[3][6]*t24; + ftype t92 = P[5][6]*t11; + ftype t93 = P[1][6]*t17; + ftype t74 = t69+t70+t71+t72+t73-t92-t93; + ftype t75 = t9*t74; + ftype t82 = t11*t42; + ftype t87 = t17*t54; + ftype t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87; + ftype t77; // calculate innovation variance for Z axis observation and protect against a badly conditioned calculation if (t76 > R_VEL) { @@ -1741,8 +1743,8 @@ void NavEKF3_core::FuseBodyVel() Kfusion[11] = t77*(P[11][4]*t4+P[11][0]*t14+P[11][6]*t9-P[11][5]*t11-P[11][1]*t17+P[11][2]*t20+P[11][3]*t24); Kfusion[12] = t77*(P[12][4]*t4+P[12][0]*t14+P[12][6]*t9-P[12][5]*t11-P[12][1]*t17+P[12][2]*t20+P[12][3]*t24); } else { - // zero indexes 10 to 12 = 3*4 bytes - memset(&Kfusion[10], 0, 12); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } @@ -1756,8 +1758,8 @@ void NavEKF3_core::FuseBodyVel() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } if (!inhibitMagStates) { @@ -1768,16 +1770,16 @@ void NavEKF3_core::FuseBodyVel() Kfusion[20] = t77*(P[20][4]*t4+P[20][0]*t14+P[20][6]*t9-P[20][5]*t11-P[20][1]*t17+P[20][2]*t20+P[20][3]*t24); Kfusion[21] = t77*(P[21][4]*t4+P[21][0]*t14+P[21][6]*t9-P[21][5]*t11-P[21][1]*t17+P[21][2]*t20+P[21][3]*t24); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } if (!inhibitWindStates) { Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24); Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24); } else { - // zero indexes 22 to 23 = 2*4 bytes - memset(&Kfusion[22], 0, 8); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } } else { return; @@ -1894,17 +1896,17 @@ void NavEKF3_core::SelectBodyOdomFusion() if (wheelOdmDataDelayed.delTime > EKF_TARGET_DT) { // get the forward velocity - float fwdSpd = wheelOdmDataDelayed.delAng * wheelOdmDataDelayed.radius * (1.0f / wheelOdmDataDelayed.delTime); + ftype fwdSpd = wheelOdmDataDelayed.delAng * wheelOdmDataDelayed.radius * (1.0f / wheelOdmDataDelayed.delTime); // get the unit vector from the projection of the X axis onto the horizontal - Vector3f unitVec; + Vector3F unitVec; unitVec.x = prevTnb.a.x; unitVec.y = prevTnb.a.y; unitVec.z = 0.0f; unitVec.normalize(); // multiply by forward speed to get velocity vector measured by wheel encoders - Vector3f velNED = unitVec * fwdSpd; + Vector3F velNED = unitVec * fwdSpd; // This is a hack to enable use of the existing body frame velocity fusion method // TODO write a dedicated observation model for wheel encoders diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index bbc8a9d694..324e0eff5d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -42,14 +42,14 @@ void NavEKF3_core::SelectRngBcnFusion() 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; + ftype pn; + ftype pe; + ftype pd; + ftype bcn_pn; + ftype bcn_pe; + ftype bcn_pd; + const ftype R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); + ftype rngPred; // health is set bad until test passed rngBcnHealth = false; @@ -70,7 +70,7 @@ void NavEKF3_core::FuseRngBcn() bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z; // predicted range - Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; + Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; rngPred = deltaPosNED.length(); // calculate measurement innovation @@ -80,16 +80,16 @@ void NavEKF3_core::FuseRngBcn() if (rngPred > 0.1f) { // calculate observation jacobians - float H_BCN[24]; + ftype 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); + ftype t2 = bcn_pd-pd; + ftype t3 = bcn_pe-pe; + ftype t4 = bcn_pn-pn; + ftype t5 = t2*t2; + ftype t6 = t3*t3; + ftype t7 = t4*t4; + ftype t8 = t5+t6+t7; + ftype 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 @@ -102,23 +102,23 @@ void NavEKF3_core::FuseRngBcn() 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; + ftype t10 = P[9][9]*t2*t9; + ftype t11 = P[8][9]*t3*t9; + ftype t12 = P[7][9]*t4*t9; + ftype t13 = t10+t11+t12; + ftype t14 = t2*t9*t13; + ftype t15 = P[9][8]*t2*t9; + ftype t16 = P[8][8]*t3*t9; + ftype t17 = P[7][8]*t4*t9; + ftype t18 = t15+t16+t17; + ftype t19 = t3*t9*t18; + ftype t20 = P[9][7]*t2*t9; + ftype t21 = P[8][7]*t3*t9; + ftype t22 = P[7][7]*t4*t9; + ftype t23 = t20+t21+t22; + ftype t24 = t4*t9*t23; varInnovRngBcn = R_BCN+t14+t19+t24; - float t26; + ftype t26; if (varInnovRngBcn >= R_BCN) { t26 = 1.0f/varInnovRngBcn; faultStatus.bad_rngbcn = false; @@ -144,8 +144,8 @@ void NavEKF3_core::FuseRngBcn() 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); + // zero indexes 10 to 12 + zero_range(&Kfusion[0], 10, 12); } if (!inhibitDelVelBiasStates) { @@ -158,8 +158,8 @@ void NavEKF3_core::FuseRngBcn() } } } else { - // zero indexes 13 to 15 = 3*4 bytes - memset(&Kfusion[13], 0, 12); + // zero indexes 13 to 15 + zero_range(&Kfusion[0], 13, 15); } // only allow the range observations to modify the vertical states if we are using it as a height reference @@ -179,24 +179,24 @@ void NavEKF3_core::FuseRngBcn() 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); + // zero indexes 16 to 21 + zero_range(&Kfusion[0], 16, 21); } 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); + // zero indexes 22 to 23 + zero_range(&Kfusion[0], 22, 23); } // Calculate innovation using the selected offset value - Vector3f delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED; + 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); + rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn); // fail if the ratio is > 1, but don't fail if bad IMU data rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata); @@ -283,7 +283,7 @@ https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon void NavEKF3_core::FuseRngBcnStatic() { // get the estimated range measurement variance - const float R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); + const ftype R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); /* The first thing to do is to check if we have started the alignment and if not, initialise the @@ -307,7 +307,7 @@ void NavEKF3_core::FuseRngBcnStatic() } if (numBcnMeas >= 100) { rngBcnAlignmentStarted = true; - float tempVar = 1.0f / (float)numBcnMeas; + ftype tempVar = 1.0f / (ftype)numBcnMeas; // initialise the receiver position to the centre of the beacons and at zero height receiverPos.x = rngBcnPosSum.x * tempVar; receiverPos.y = rngBcnPosSum.y * tempVar; @@ -336,7 +336,7 @@ void NavEKF3_core::FuseRngBcnStatic() // position offset to be applied to the beacon system that minimises the range innovations // The position estimate should be stable after 100 iterations so we use a simple dual // hypothesis 1-state EKF to estimate the offset - Vector3f refPosNED; + Vector3F refPosNED; refPosNED.x = receiverPos.x; refPosNED.y = receiverPos.y; refPosNED.z = stateStruct.position.z; @@ -355,14 +355,14 @@ void NavEKF3_core::FuseRngBcnStatic() // and the main EKF vertical position // Calculate the mid vertical position of all beacons - float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD); + ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD); // calculate the delta to the estimated receiver position - float delta = receiverPos.z - bcnMidPosD; + ftype delta = receiverPos.z - bcnMidPosD; // calcuate the two hypothesis for our vertical position - float receverPosDownMax; - float receverPosDownMin; + ftype receverPosDownMax; + ftype receverPosDownMin; if (delta >= 0.0f) { receverPosDownMax = receiverPos.z; receverPosDownMin = receiverPos.z - 2.0f * delta; @@ -385,62 +385,62 @@ void NavEKF3_core::FuseRngBcnStatic() } // calculate the observation jacobian - float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z; - float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y; - float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x; - float t5 = t2*t2; - float t6 = t3*t3; - float t7 = t4*t4; - float t8 = t5+t6+t7; + ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z; + ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y; + ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x; + ftype t5 = t2*t2; + ftype t6 = t3*t3; + ftype t7 = t4*t4; + ftype t8 = t5+t6+t7; if (t8 < 0.1f) { // calculation will be badly conditioned return; } - float t9 = 1.0f/sqrtf(t8); - float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f; - float t15 = receiverPos.x*2.0f; - float t11 = t10-t15; - float t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f; - float t14 = receiverPos.y*2.0f; - float t13 = t12-t14; - float t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f; - float t18 = receiverPos.z*2.0f; - float t17 = t16-t18; - float H_RNG[3]; + ftype t9 = 1.0f/sqrtF(t8); + ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f; + ftype t15 = receiverPos.x*2.0f; + ftype t11 = t10-t15; + ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f; + ftype t14 = receiverPos.y*2.0f; + ftype t13 = t12-t14; + ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f; + ftype t18 = receiverPos.z*2.0f; + ftype t17 = t16-t18; + ftype H_RNG[3]; H_RNG[0] = -t9*t11*0.5f; H_RNG[1] = -t9*t13*0.5f; H_RNG[2] = -t9*t17*0.5f; // calculate the Kalman gains - float t19 = receiverPosCov[0][0]*t9*t11*0.5f; - float t20 = receiverPosCov[1][1]*t9*t13*0.5f; - float t21 = receiverPosCov[0][1]*t9*t11*0.5f; - float t22 = receiverPosCov[2][1]*t9*t17*0.5f; - float t23 = t20+t21+t22; - float t24 = t9*t13*t23*0.5f; - float t25 = receiverPosCov[1][2]*t9*t13*0.5f; - float t26 = receiverPosCov[0][2]*t9*t11*0.5f; - float t27 = receiverPosCov[2][2]*t9*t17*0.5f; - float t28 = t25+t26+t27; - float t29 = t9*t17*t28*0.5f; - float t30 = receiverPosCov[1][0]*t9*t13*0.5f; - float t31 = receiverPosCov[2][0]*t9*t17*0.5f; - float t32 = t19+t30+t31; - float t33 = t9*t11*t32*0.5f; + ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f; + ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f; + ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f; + ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f; + ftype t23 = t20+t21+t22; + ftype t24 = t9*t13*t23*0.5f; + ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f; + ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f; + ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f; + ftype t28 = t25+t26+t27; + ftype t29 = t9*t17*t28*0.5f; + ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f; + ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f; + ftype t32 = t19+t30+t31; + ftype t33 = t9*t11*t32*0.5f; varInnovRngBcn = R_RNG+t24+t29+t33; - float t35 = 1.0f/varInnovRngBcn; - float K_RNG[3]; + ftype t35 = 1.0f/varInnovRngBcn; + ftype K_RNG[3]; K_RNG[0] = -t35*(t19+receiverPosCov[0][1]*t9*t13*0.5f+receiverPosCov[0][2]*t9*t17*0.5f); K_RNG[1] = -t35*(t20+receiverPosCov[1][0]*t9*t11*0.5f+receiverPosCov[1][2]*t9*t17*0.5f); K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f); // calculate range measurement innovation - Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED; + Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED; deltaPosNED.z -= bcnPosOffsetNED.z; innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng; // calculate the innovation consistency test ratio - rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn); + rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn); // fail if the ratio is > 1, but don't fail if bad IMU data rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata || !rngBcnAlignmentCompleted); @@ -489,7 +489,7 @@ void NavEKF3_core::FuseRngBcnStatic() // ensure the covariance matrix is symmetric for (uint8_t i=1; i<=2; i++) { for (uint8_t j=0; j<=i-1; j++) { - float temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]); + ftype temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]); receiverPosCov[i][j] = temp; receiverPosCov[j][i] = temp; } @@ -517,7 +517,7 @@ void NavEKF3_core::FuseRngBcnStatic() Run a single state Kalman filter to estimate the vertical position offset of the range beacon constellation Calculate using a high and low hypothesis and select the hypothesis with the lowest innovation sequence */ -void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning) +void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning) { // Handle height offsets between the primary height source and the range beacons by estimating // the beacon systems global vertical position offset using a single state Kalman filter @@ -525,32 +525,32 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP // A high and low estimate is calculated to handle the ambiguity in height associated with beacon positions that are co-planar // The main filter then uses the offset with the smaller innovations - float innov; // range measurement innovation (m) - float innovVar; // range measurement innovation variance (m^2) - float gain; // Kalman gain - float obsDeriv; // derivative of observation relative to state + ftype innov; // range measurement innovation (m) + ftype innovVar; // range measurement innovation variance (m^2) + ftype gain; // Kalman gain + ftype obsDeriv; // derivative of observation relative to state - const float stateNoiseVar = 0.1f; // State process noise variance - const float filtAlpha = 0.1f; // LPF constant - const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std + const ftype stateNoiseVar = 0.1f; // State process noise variance + const ftype filtAlpha = 0.1f; // LPF constant + const ftype innovGateWidth = 5.0f; // width of innovation consistency check gate in std // estimate upper value for offset // calculate observation derivative - float t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax; - float t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y; - float t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x; - float t5 = t2*t2; - float t6 = t3*t3; - float t7 = t4*t4; - float t8 = t5+t6+t7; - float t9; + ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax; + ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y; + ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x; + ftype t5 = t2*t2; + ftype t6 = t3*t3; + ftype t7 = t4*t4; + ftype t8 = t5+t6+t7; + ftype t9; if (t8 > 0.1f) { - t9 = 1.0f/sqrtf(t8); + t9 = 1.0f/sqrtF(t8); obsDeriv = t2*t9; // Calculate innovation - innov = sqrtf(t8) - rngBcnDataDelayed.rng; + innov = sqrtF(t8) - rngBcnDataDelayed.rng; // covariance prediction bcnPosOffsetMaxVar += stateNoiseVar; @@ -563,8 +563,8 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar; // calculate a filtered state change magnitude to be used to select between the high or low offset - float stateChange = innov * gain; - maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f); + ftype stateChange = innov * gain; + maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f); // Reject range innovation spikes using a 5-sigma threshold unless aligning if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { @@ -585,11 +585,11 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP t5 = t2*t2; t8 = t5+t6+t7; if (t8 > 0.1f) { - t9 = 1.0f/sqrtf(t8); + t9 = 1.0f/sqrtF(t8); obsDeriv = t2*t9; // Calculate innovation - innov = sqrtf(t8) - rngBcnDataDelayed.rng; + innov = sqrtF(t8) - rngBcnDataDelayed.rng; // covariance prediction bcnPosOffsetMinVar += stateNoiseVar; @@ -602,8 +602,8 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar; // calculate a filtered state change magnitude to be used to select between the high or low offset - float stateChange = innov * gain; - minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f); + ftype stateChange = innov * gain; + minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f); // Reject range innovation spikes using a 5-sigma threshold unless aligning if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { @@ -618,7 +618,7 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP } // calculate the mid vertical position of all beacons - float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD); + ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD); // ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 5d962e15f6..d927486a53 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -22,7 +22,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void) } // User defined multiplier to be applied to check thresholds - float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler; + ftype checkScaler = 0.01f*(ftype)frontend->_gpsCheckScaler; if (gpsGoodToAlign) { /* @@ -51,9 +51,9 @@ void NavEKF3_core::calcGpsGoodToAlign(void) const auto &gps = dal.gps(); const struct Location &gpsloc = gps.location(preferred_gps); // Current location - const float posFiltTimeConst = 10.0f; // time constant used to decay position drift + const ftype posFiltTimeConst = 10.0; // time constant used to decay position drift // calculate time lapsed since last update and limit to prevent numerical errors - float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst); + ftype deltaTime = constrain_ftype(ftype(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst); lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms; // Sum distance moved gpsDriftNE += gpsloc_prev.get_distance(gpsloc); @@ -81,8 +81,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void) if (gpsDataNew.have_vz && onGround) { // check that the average vertical GPS velocity is close to zero gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; - gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); - gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); + gpsVertVelFilt = constrain_ftype(gpsVertVelFilt,-10.0f,10.0f); + gpsVertVelFail = (fabsF(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); } else { gpsVertVelFail = false; } @@ -91,7 +91,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void) if (gpsVertVelFail) { dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler)); + "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsF(gpsVertVelFilt), (double)(0.3f*checkScaler)); gpsCheckStatus.bad_vert_vel = true; } else { gpsCheckStatus.bad_vert_vel = false; @@ -102,8 +102,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void) bool gpsHorizVelFail; if (onGround) { gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; - gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f); - gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD); + gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f); + gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD); } else { gpsHorizVelFail = false; } @@ -240,14 +240,14 @@ void NavEKF3_core::calcGpsGoodForFlight(void) // use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks // set up varaibles and constants used by filter that is applied to GPS speed accuracy - const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data - const float tau = 10.0f; // time constant (sec) of peak hold decay + const ftype alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data + const ftype tau = 10.0f; // time constant (sec) of peak hold decay if (lastGpsCheckTime_ms == 0) { lastGpsCheckTime_ms = imuSampleTime_ms; } - float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f; + ftype dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f; lastGpsCheckTime_ms = imuSampleTime_ms; - float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f); + ftype alpha2 = constrain_ftype(dtLPF/tau,0.0f,1.0f); // get the receivers reported speed accuracy float gpsSpdAccRaw; @@ -256,7 +256,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void) } // filter the raw speed accuracy using a LPF - sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f); + sAccFilterState1 = constrain_ftype((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f); // apply a peak hold filter to the LPF output sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2)); @@ -305,7 +305,7 @@ void NavEKF3_core::detectFlight() if (assume_zero_sideslip()) { // To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change - float gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y); + ftype gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y); bool highGndSpd = false; bool highAirSpd = false; bool largeHgtChange = false; @@ -319,13 +319,13 @@ void NavEKF3_core::detectFlight() } // trigger on ground speed - const float gndSpdThresholdSq = sq(5.0f); + const ftype gndSpdThresholdSq = sq(5.0f); if (gndSpdSq > gndSpdThresholdSq + sq(gpsSpdAccuracy)) { highGndSpd = true; } // trigger if more than 10m away from initial height - if (fabsf(hgtMea) > 10.0f) { + if (fabsF(hgtMea) > 10.0f) { largeHgtChange = true; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 8297470e64..52aae85638 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -89,7 +89,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) // set the observation buffer length to handle the minimum time of arrival between observations in combination // with the worst case delay from current time to ekf fusion time // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter - uint16_t ekf_delay_ms = maxTimeDelay_ms + (int)(ceilf((float)maxTimeDelay_ms * 0.5f)); + uint16_t ekf_delay_ms = maxTimeDelay_ms + (int)(ceilF((float)maxTimeDelay_ms * 0.5f)); obs_buffer_length = (ekf_delay_ms / frontend->sensorIntervalMin_ms) + 1; // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) @@ -302,7 +302,7 @@ void NavEKF3_core::InitialiseVariables() gpsDriftNE = 0.0f; gpsVertVelFilt = 0.0f; gpsHorizVelFilt = 0.0f; - memset(&statesArray, 0, sizeof(statesArray)); + ZERO_FARRAY(statesArray); memset(&vertCompFiltState, 0, sizeof(vertCompFiltState)); posVelFusionDelayed = false; optFlowFusionDelayed = false; @@ -335,7 +335,7 @@ void NavEKF3_core::InitialiseVariables() ekfGpsRefHgt = 0.0; velOffsetNED.zero(); posOffsetNED.zero(); - memset(&velPosObs, 0, sizeof(velPosObs)); + ZERO_FARRAY(velPosObs); // range beacon fusion variables memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed)); @@ -383,9 +383,9 @@ void NavEKF3_core::InitialiseVariables() memset((void *)&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed)); #endif lastbodyVelPassTime_ms = 0; - memset(&bodyVelTestRatio, 0, sizeof(bodyVelTestRatio)); - memset(&varInnovBodyVel, 0, sizeof(varInnovBodyVel)); - memset(&innovBodyVel, 0, sizeof(innovBodyVel)); + ZERO_FARRAY(bodyVelTestRatio); + ZERO_FARRAY(varInnovBodyVel); + ZERO_FARRAY(innovBodyVel); prevBodyVelFuseTime_ms = 0; bodyOdmMeasTime_ms = 0; bodyVelFusionDelayed = false; @@ -519,21 +519,21 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) InitialiseVariables(); // acceleration vector in XYZ body axes measured by the IMU (m/s^2) - Vector3f initAccVec; + Vector3F initAccVec; // TODO we should average accel readings over several cycles - initAccVec = dal.ins().get_accel(accel_index_active); + initAccVec = dal.ins().get_accel(accel_index_active).toftype(); // normalise the acceleration vector - float pitch=0, roll=0; + ftype pitch=0, roll=0; if (initAccVec.length() > 0.001f) { initAccVec.normalize(); // calculate initial pitch angle - pitch = asinf(initAccVec.x); + pitch = asinF(initAccVec.x); // calculate initial roll angle - roll = atan2f(-initAccVec.y , -initAccVec.z); + roll = atan2F(-initAccVec.y , -initAccVec.z); } // calculate initial roll and pitch orientation @@ -590,7 +590,7 @@ void NavEKF3_core::CovarianceInit() memset(&P[0][0], 0, sizeof(P)); // define the initial angle uncertainty as variances for a rotation vector - Vector3f rot_vec_var; + Vector3F rot_vec_var; rot_vec_var.x = rot_vec_var.y = rot_vec_var.z = sq(0.1f); // reset the quaternion state covariances @@ -727,12 +727,12 @@ void NavEKF3_core::UpdateFilter(bool predict) } } -void NavEKF3_core::correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index) +void NavEKF3_core::correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index) { delAng -= inactiveBias[gyro_index].gyro_bias * (delAngDT / dtEkfAvg); } -void NavEKF3_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index) +void NavEKF3_core::correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index) { delVel -= inactiveBias[accel_index].accel_bias * (delVelDT / dtEkfAvg); } @@ -758,7 +758,7 @@ void NavEKF3_core::UpdateStrapdownEquationsNED() // use the nav frame from previous time step as the delta velocities // have been rotated into that frame // * and + operators have been overloaded - Vector3f delVelNav; // delta velocity vector in earth axes + Vector3F delVelNav; // delta velocity vector in earth axes delVelNav = prevTnb.mul_transpose(delVelCorrected); delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT; @@ -779,13 +779,13 @@ void NavEKF3_core::UpdateStrapdownEquationsNED() // if we are not aiding, then limit the horizontal magnitude of acceleration // to prevent large manoeuvre transients disturbing the attitude if ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) { - float gain = 5.0f/accNavMagHoriz; + ftype gain = 5.0f/accNavMagHoriz; delVelNav.x *= gain; delVelNav.y *= gain; } // save velocity for use in trapezoidal integration for position calcuation - Vector3f lastVelocity = stateStruct.velocity; + Vector3F lastVelocity = stateStruct.velocity; // sum delta velocities to get velocity stateStruct.velocity += delVelNav; @@ -821,16 +821,16 @@ void NavEKF3_core::UpdateStrapdownEquationsNED() void NavEKF3_core::calcOutputStates() { // apply corrections to the IMU data - Vector3f delAngNewCorrected = imuDataNew.delAng; - Vector3f delVelNewCorrected = imuDataNew.delVel; + Vector3F delAngNewCorrected = imuDataNew.delAng; + Vector3F delVelNewCorrected = imuDataNew.delVel; correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT, imuDataNew.gyro_index); correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT, imuDataNew.accel_index); // apply corrections to track EKF solution - Vector3f delAng = delAngNewCorrected + delAngCorrection; + Vector3F delAng = delAngNewCorrected + delAngCorrection; // convert the rotation vector to its equivalent quaternion - Quaternion deltaQuat; + QuaternionF deltaQuat; deltaQuat.from_axis_angle(delAng); // update the quaternion states by rotating from the previous attitude through @@ -839,15 +839,15 @@ void NavEKF3_core::calcOutputStates() outputDataNew.quat.normalize(); // calculate the body to nav cosine matrix - Matrix3f Tbn_temp; + Matrix3F Tbn_temp; outputDataNew.quat.rotation_matrix(Tbn_temp); // transform body delta velocities to delta velocities in the nav frame - Vector3f delVelNav = Tbn_temp*delVelNewCorrected; + Vector3F delVelNav = Tbn_temp*delVelNewCorrected; delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT; // save velocity for use in trapezoidal integration for position calcuation - Vector3f lastVelocity = outputDataNew.velocity; + Vector3F lastVelocity = outputDataNew.velocity; // sum delta velocities to get velocity outputDataNew.velocity += delVelNav; @@ -860,14 +860,14 @@ void NavEKF3_core::calcOutputStates() // Perform filter calculation using backwards Euler integration // Coefficients selected to place all three filter poles at omega - const float CompFiltOmega = M_2PI * constrain_float(frontend->_hrt_filt_freq, 0.1f, 30.0f); - float omega2 = CompFiltOmega * CompFiltOmega; - float pos_err = constrain_float(outputDataNew.position.z - vertCompFiltState.pos, -1e5, 1e5); - float integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT; + const ftype CompFiltOmega = M_2PI * constrain_ftype(frontend->_hrt_filt_freq, 0.1f, 30.0f); + ftype omega2 = CompFiltOmega * CompFiltOmega; + ftype pos_err = constrain_ftype(outputDataNew.position.z - vertCompFiltState.pos, -1e5, 1e5); + ftype integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT; vertCompFiltState.acc += integ1_input; - float integ2_input = delVelNav.z + (vertCompFiltState.acc + pos_err * omega2 * 3.0f) * imuDataNew.delVelDT; + ftype integ2_input = delVelNav.z + (vertCompFiltState.acc + pos_err * omega2 * 3.0f) * imuDataNew.delVelDT; vertCompFiltState.vel += integ2_input; - float integ3_input = (vertCompFiltState.vel + pos_err * CompFiltOmega * 3.0f) * imuDataNew.delVelDT; + ftype integ3_input = (vertCompFiltState.vel + pos_err * CompFiltOmega * 3.0f) * imuDataNew.delVelDT; vertCompFiltState.pos += integ3_input; // apply a trapezoidal integration to velocities to calculate position @@ -880,11 +880,11 @@ void NavEKF3_core::calcOutputStates() if (!accelPosOffset.is_zero()) { // calculate the average angular rate across the last IMU update // note delAngDT is prevented from being zero in readIMUData() - Vector3f angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT); + Vector3F angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT); // Calculate the velocity of the body frame origin relative to the IMU in body frame // and rotate into earth frame. Note % operator has been overloaded to perform a cross product - Vector3f velBodyRelIMU = angRate % (- accelPosOffset); + Vector3F velBodyRelIMU = angRate % (- accelPosOffset); velOffsetNED = Tbn_temp * velBodyRelIMU; // calculate the earth frame position of the body frame origin relative to the IMU @@ -897,7 +897,7 @@ void NavEKF3_core::calcOutputStates() // Detect fixed wing launch acceleration using latest data from IMU to enable early startup of filter functions // that use launch acceleration to detect start of flight if (!inFlight && !dal.get_takeoff_expected() && assume_zero_sideslip()) { - const float launchDelVel = imuDataNew.delVel.x + GRAVITY_MSS * imuDataNew.delVelDT * Tbn_temp.c.x; + const ftype launchDelVel = imuDataNew.delVel.x + GRAVITY_MSS * imuDataNew.delVelDT * Tbn_temp.c.x; if (launchDelVel > GRAVITY_MSS * imuDataNew.delVelDT) { dal.set_takeoff_expected(); } @@ -914,12 +914,12 @@ void NavEKF3_core::calcOutputStates() // compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction // divide the demanded quaternion by the estimated to get the error - Quaternion quatErr = stateStruct.quat / outputDataDelayed.quat; + QuaternionF quatErr = stateStruct.quat / outputDataDelayed.quat; // Convert to a delta rotation using a small angle approximation quatErr.normalize(); - Vector3f deltaAngErr; - float scaler; + Vector3F deltaAngErr; + ftype scaler; if (quatErr[0] >= 0.0f) { scaler = 2.0f; } else { @@ -931,17 +931,17 @@ void NavEKF3_core::calcOutputStates() // calculate a gain that provides tight tracking of the estimator states and // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 - float timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms); + ftype timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms); timeDelay = MAX(timeDelay, dtIMUavg); - float errorGain = 0.5f / timeDelay; + ftype errorGain = 0.5f / timeDelay; // calculate a correction to the delta angle // that will cause the INS to track the EKF quaternions delAngCorrection = deltaAngErr * errorGain * dtIMUavg; // calculate velocity and position tracking errors - Vector3f velErr = (stateStruct.velocity - outputDataDelayed.velocity); - Vector3f posErr = (stateStruct.position - outputDataDelayed.position); + Vector3F velErr = (stateStruct.velocity - outputDataDelayed.velocity); + Vector3F posErr = (stateStruct.position - outputDataDelayed.position); // collect magnitude tracking error for diagnostics outputTrackError.x = deltaAngErr.length(); @@ -949,16 +949,16 @@ void NavEKF3_core::calcOutputStates() outputTrackError.z = posErr.length(); // convert user specified time constant from centi-seconds to seconds - float tauPosVel = constrain_float(0.01f*(float)frontend->_tauVelPosOutput, 0.1f, 0.5f); + ftype tauPosVel = constrain_ftype(0.01f*(ftype)frontend->_tauVelPosOutput, 0.1f, 0.5f); // calculate a gain to track the EKF position states with the specified time constant - float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f); + ftype velPosGain = dtEkfAvg / constrain_ftype(tauPosVel, dtEkfAvg, 10.0f); // use a PI feedback to calculate a correction that will be applied to the output state history posErrintegral += posErr; velErrintegral += velErr; - Vector3f velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f; - Vector3f posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1f; + Vector3F velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f; + Vector3F posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1f; // loop through the output filter state history and apply the corrections to the velocity and position states // this method is too expensive to use for the attitude states due to the quaternion operations required @@ -991,38 +991,38 @@ void NavEKF3_core::calcOutputStates() * Argument rotVarVecPtr is pointer to a vector defining the earth frame uncertainty variance of the quaternion states * used to perform a reset of the quaternion state covariances only. Set to null for normal operation. */ -void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) +void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) { - float daxVar; // X axis delta angle noise variance rad^2 - float dayVar; // Y axis delta angle noise variance rad^2 - float dazVar; // Z axis delta angle noise variance rad^2 - float dvxVar; // X axis delta velocity variance noise (m/s)^2 - float dvyVar; // Y axis delta velocity variance noise (m/s)^2 - float dvzVar; // Z axis delta velocity variance noise (m/s)^2 - float dvx; // X axis delta velocity (m/s) - float dvy; // Y axis delta velocity (m/s) - float dvz; // Z axis delta velocity (m/s) - float dax; // X axis delta angle (rad) - float day; // Y axis delta angle (rad) - float daz; // Z axis delta angle (rad) - float q0; // attitude quaternion - float q1; // attitude quaternion - float q2; // attitude quaternion - float q3; // attitude quaternion - float dax_b; // X axis delta angle measurement bias (rad) - float day_b; // Y axis delta angle measurement bias (rad) - float daz_b; // Z axis delta angle measurement bias (rad) - float dvx_b; // X axis delta velocity measurement bias (rad) - float dvy_b; // Y axis delta velocity measurement bias (rad) - float dvz_b; // Z axis delta velocity measurement bias (rad) + ftype daxVar; // X axis delta angle noise variance rad^2 + ftype dayVar; // Y axis delta angle noise variance rad^2 + ftype dazVar; // Z axis delta angle noise variance rad^2 + ftype dvxVar; // X axis delta velocity variance noise (m/s)^2 + ftype dvyVar; // Y axis delta velocity variance noise (m/s)^2 + ftype dvzVar; // Z axis delta velocity variance noise (m/s)^2 + ftype dvx; // X axis delta velocity (m/s) + ftype dvy; // Y axis delta velocity (m/s) + ftype dvz; // Z axis delta velocity (m/s) + ftype dax; // X axis delta angle (rad) + ftype day; // Y axis delta angle (rad) + ftype daz; // Z axis delta angle (rad) + ftype q0; // attitude quaternion + ftype q1; // attitude quaternion + ftype q2; // attitude quaternion + ftype q3; // attitude quaternion + ftype dax_b; // X axis delta angle measurement bias (rad) + ftype day_b; // Y axis delta angle measurement bias (rad) + ftype daz_b; // Z axis delta angle measurement bias (rad) + ftype dvx_b; // X axis delta velocity measurement bias (rad) + ftype dvy_b; // Y axis delta velocity measurement bias (rad) + ftype dvz_b; // Z axis delta velocity measurement bias (rad) // Calculate the time step used by the covariance prediction as an average of the gyro and accel integration period // Constrain to prevent bad timing jitter causing numerical conditioning problems with the covariance prediction - dt = constrain_float(0.5f*(imuDataDelayed.delAngDT+imuDataDelayed.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg); + dt = constrain_ftype(0.5f*(imuDataDelayed.delAngDT+imuDataDelayed.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg); // use filtered height rate to increase wind process noise when climbing or descending // this allows for wind gradient effects.Filter height rate using a 10 second time constant filter - float alpha = 0.1f * dt; + ftype alpha = 0.1f * dt; hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha; // calculate covariance prediction process noise added to diagonals of predicted covariance matrix @@ -1030,13 +1030,13 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) Vector14 processNoiseVariance = {}; if (!inhibitDelAngBiasStates) { - float dAngBiasVar = sq(sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f)); + ftype dAngBiasVar = sq(sq(dt) * constrain_ftype(frontend->_gyroBiasProcessNoise, 0.0, 1.0)); for (uint8_t i=0; i<=2; i++) processNoiseVariance[i] = dAngBiasVar; } if (!inhibitDelVelBiasStates) { // default process noise (m/s)^2 - float dVelBiasVar = sq(sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f)); + ftype dVelBiasVar = sq(sq(dt) * constrain_ftype(frontend->_accelBiasProcessNoise, 0.0, 1.0)); for (uint8_t i=3; i<=5; i++) { processNoiseVariance[i] = dVelBiasVar; } @@ -1072,15 +1072,15 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) } if (!inhibitMagStates) { - float magEarthVar = sq(dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f)); - float magBodyVar = sq(dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f)); + ftype magEarthVar = sq(dt * constrain_ftype(frontend->_magEarthProcessNoise, 0.0f, 1.0f)); + ftype magBodyVar = sq(dt * constrain_ftype(frontend->_magBodyProcessNoise, 0.0f, 1.0f)); for (uint8_t i=6; i<=8; i++) processNoiseVariance[i] = magEarthVar; for (uint8_t i=9; i<=11; i++) processNoiseVariance[i] = magBodyVar; } lastInhibitMagStates = inhibitMagStates; if (!inhibitWindStates) { - float windVelVar = sq(dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate))); + ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate))); for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar; } @@ -1108,14 +1108,14 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) // vector defining the variance of the angular alignment uncertainty. Convert he varaince vector // to a matrix and rotate into body frame. Use the exisiting gyro error propagation mechanism to // propagate the body frame angular uncertainty variances. - const Vector3f &rotVarVec = *rotVarVecPtr; - Matrix3f R_ef = Matrix3f ( + const Vector3F &rotVarVec = *rotVarVecPtr; + Matrix3F R_ef = Matrix3F ( rotVarVec.x, 0.0f, 0.0f, 0.0f, rotVarVec.y, 0.0f, 0.0f, 0.0f, rotVarVec.z); - Matrix3f Tnb; + Matrix3F Tnb; stateStruct.quat.inverse().rotation_matrix(Tnb); - Matrix3f R_bf = Tnb * R_ef * Tnb.transposed(); + Matrix3F R_bf = Tnb * R_ef * Tnb.transposed(); daxVar = R_bf.a.x; dayVar = R_bf.b.y; dazVar = R_bf.c.z; @@ -1123,10 +1123,10 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) zeroRows(P,0,3); zeroCols(P,0,3); } else { - float _gyrNoise = constrain_float(frontend->_gyrNoise, 0.0f, 1.0f); + ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f); daxVar = dayVar = dazVar = sq(dt*_gyrNoise); } - float _accNoise = constrain_float(frontend->_accNoise, 0.0f, 10.0f); + ftype _accNoise = constrain_ftype(frontend->_accNoise, 0.0f, 10.0f); dvxVar = dvyVar = dvzVar = sq(dt*_accNoise); if (!inhibitDelVelBiasStates) { @@ -1134,7 +1134,7 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) const uint8_t index = stateIndex - 13; // Don't attempt learning of IMU delta velocty bias if on ground and not aligned with the gravity vector - const bool is_bias_observable = (fabsf(prevTnb[index][2]) > 0.8f) && onGround; + const bool is_bias_observable = (fabsF(prevTnb[index][2]) > 0.8f) && onGround; if (!is_bias_observable && !dvelBiasAxisInhibit[index]) { // store variances to be reinstated wben learning can commence later @@ -1151,193 +1151,193 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) // we calculate the lower diagonal and copy to take advantage of symmetry // intermediate calculations - const float PS0 = powf(q1, 2); - const float PS1 = 0.25F*daxVar; - const float PS2 = powf(q2, 2); - const float PS3 = 0.25F*dayVar; - const float PS4 = powf(q3, 2); - const float PS5 = 0.25F*dazVar; - const float PS6 = 0.5F*q1; - const float PS7 = 0.5F*q2; - const float PS8 = PS7*P[10][11]; - const float PS9 = 0.5F*q3; - const float PS10 = PS9*P[10][12]; - const float PS11 = 0.5F*dax - 0.5F*dax_b; - const float PS12 = 0.5F*day - 0.5F*day_b; - const float PS13 = 0.5F*daz - 0.5F*daz_b; - const float PS14 = PS10 - PS11*P[1][10] - PS12*P[2][10] - PS13*P[3][10] + PS6*P[10][10] + PS8 + P[0][10]; - const float PS15 = PS6*P[10][11]; - const float PS16 = PS9*P[11][12]; - const float PS17 = -PS11*P[1][11] - PS12*P[2][11] - PS13*P[3][11] + PS15 + PS16 + PS7*P[11][11] + P[0][11]; - const float PS18 = PS6*P[10][12]; - const float PS19 = PS7*P[11][12]; - const float PS20 = -PS11*P[1][12] - PS12*P[2][12] - PS13*P[3][12] + PS18 + PS19 + PS9*P[12][12] + P[0][12]; - const float PS21 = PS12*P[1][2]; - const float PS22 = -PS13*P[1][3]; - const float PS23 = -PS11*P[1][1] - PS21 + PS22 + PS6*P[1][10] + PS7*P[1][11] + PS9*P[1][12] + P[0][1]; - const float PS24 = -PS11*P[1][2]; - const float PS25 = PS13*P[2][3]; - const float PS26 = -PS12*P[2][2] + PS24 - PS25 + PS6*P[2][10] + PS7*P[2][11] + PS9*P[2][12] + P[0][2]; - const float PS27 = PS11*P[1][3]; - const float PS28 = -PS12*P[2][3]; - const float PS29 = -PS13*P[3][3] - PS27 + PS28 + PS6*P[3][10] + PS7*P[3][11] + PS9*P[3][12] + P[0][3]; - const float PS30 = PS11*P[0][1]; - const float PS31 = PS12*P[0][2]; - const float PS32 = PS13*P[0][3]; - const float PS33 = -PS30 - PS31 - PS32 + PS6*P[0][10] + PS7*P[0][11] + PS9*P[0][12] + P[0][0]; - const float PS34 = 0.5F*q0; - const float PS35 = q2*q3; - const float PS36 = q0*q1; - const float PS37 = q1*q3; - const float PS38 = q0*q2; - const float PS39 = q1*q2; - const float PS40 = q0*q3; - const float PS41 = -PS2; - const float PS42 = powf(q0, 2); - const float PS43 = -PS4 + PS42; - const float PS44 = PS0 + PS41 + PS43; - const float PS45 = -PS11*P[1][13] - PS12*P[2][13] - PS13*P[3][13] + PS6*P[10][13] + PS7*P[11][13] + PS9*P[12][13] + P[0][13]; - const float PS46 = PS37 + PS38; - const float PS47 = -PS11*P[1][15] - PS12*P[2][15] - PS13*P[3][15] + PS6*P[10][15] + PS7*P[11][15] + PS9*P[12][15] + P[0][15]; - const float PS48 = 2*PS47; - const float PS49 = dvy - dvy_b; - const float PS50 = dvx - dvx_b; - const float PS51 = dvz - dvz_b; - const float PS52 = PS49*q0 + PS50*q3 - PS51*q1; - const float PS53 = 2*PS29; - const float PS54 = -PS39 + PS40; - const float PS55 = -PS11*P[1][14] - PS12*P[2][14] - PS13*P[3][14] + PS6*P[10][14] + PS7*P[11][14] + PS9*P[12][14] + P[0][14]; - const float PS56 = 2*PS55; - const float PS57 = -PS49*q3 + PS50*q0 + PS51*q2; - const float PS58 = 2*PS33; - const float PS59 = PS49*q1 - PS50*q2 + PS51*q0; - const float PS60 = 2*PS59; - const float PS61 = PS49*q2 + PS50*q1 + PS51*q3; - const float PS62 = 2*PS61; - const float PS63 = -PS11*P[1][4] - PS12*P[2][4] - PS13*P[3][4] + PS6*P[4][10] + PS7*P[4][11] + PS9*P[4][12] + P[0][4]; - const float PS64 = -PS0; - const float PS65 = PS2 + PS43 + PS64; - const float PS66 = PS39 + PS40; - const float PS67 = 2*PS45; - const float PS68 = -PS35 + PS36; - const float PS69 = -PS11*P[1][5] - PS12*P[2][5] - PS13*P[3][5] + PS6*P[5][10] + PS7*P[5][11] + PS9*P[5][12] + P[0][5]; - const float PS70 = PS4 + PS41 + PS42 + PS64; - const float PS71 = PS35 + PS36; - const float PS72 = 2*PS57; - const float PS73 = -PS37 + PS38; - const float PS74 = 2*PS52; - const float PS75 = -PS11*P[1][6] - PS12*P[2][6] - PS13*P[3][6] + PS6*P[6][10] + PS7*P[6][11] + PS9*P[6][12] + P[0][6]; - const float PS76 = -PS34*P[10][11]; - const float PS77 = PS11*P[0][11] - PS12*P[3][11] + PS13*P[2][11] - PS19 + PS76 + PS9*P[11][11] + P[1][11]; - const float PS78 = PS13*P[0][2]; - const float PS79 = PS12*P[0][3]; - const float PS80 = PS11*P[0][0] - PS34*P[0][10] - PS7*P[0][12] + PS78 - PS79 + PS9*P[0][11] + P[0][1]; - const float PS81 = PS11*P[0][2]; - const float PS82 = PS13*P[2][2] + PS28 - PS34*P[2][10] - PS7*P[2][12] + PS81 + PS9*P[2][11] + P[1][2]; - const float PS83 = PS9*P[10][11]; - const float PS84 = PS7*P[10][12]; - const float PS85 = PS11*P[0][10] - PS12*P[3][10] + PS13*P[2][10] - PS34*P[10][10] + PS83 - PS84 + P[1][10]; - const float PS86 = -PS34*P[10][12]; - const float PS87 = PS11*P[0][12] - PS12*P[3][12] + PS13*P[2][12] + PS16 - PS7*P[12][12] + PS86 + P[1][12]; - const float PS88 = PS11*P[0][3]; - const float PS89 = -PS12*P[3][3] + PS25 - PS34*P[3][10] - PS7*P[3][12] + PS88 + PS9*P[3][11] + P[1][3]; - const float PS90 = PS13*P[1][2]; - const float PS91 = PS12*P[1][3]; - const float PS92 = PS30 - PS34*P[1][10] - PS7*P[1][12] + PS9*P[1][11] + PS90 - PS91 + P[1][1]; - const float PS93 = PS11*P[0][13] - PS12*P[3][13] + PS13*P[2][13] - PS34*P[10][13] - PS7*P[12][13] + PS9*P[11][13] + P[1][13]; - const float PS94 = PS11*P[0][15] - PS12*P[3][15] + PS13*P[2][15] - PS34*P[10][15] - PS7*P[12][15] + PS9*P[11][15] + P[1][15]; - const float PS95 = 2*PS94; - const float PS96 = PS11*P[0][14] - PS12*P[3][14] + PS13*P[2][14] - PS34*P[10][14] - PS7*P[12][14] + PS9*P[11][14] + P[1][14]; - const float PS97 = 2*PS96; - const float PS98 = PS11*P[0][4] - PS12*P[3][4] + PS13*P[2][4] - PS34*P[4][10] - PS7*P[4][12] + PS9*P[4][11] + P[1][4]; - const float PS99 = 2*PS93; - const float PS100 = PS11*P[0][5] - PS12*P[3][5] + PS13*P[2][5] - PS34*P[5][10] - PS7*P[5][12] + PS9*P[5][11] + P[1][5]; - const float PS101 = PS11*P[0][6] - PS12*P[3][6] + PS13*P[2][6] - PS34*P[6][10] - PS7*P[6][12] + PS9*P[6][11] + P[1][6]; - const float PS102 = -PS34*P[11][12]; - const float PS103 = -PS10 + PS102 + PS11*P[3][12] + PS12*P[0][12] - PS13*P[1][12] + PS6*P[12][12] + P[2][12]; - const float PS104 = PS11*P[3][3] + PS22 - PS34*P[3][11] + PS6*P[3][12] + PS79 - PS9*P[3][10] + P[2][3]; - const float PS105 = PS13*P[0][1]; - const float PS106 = -PS105 + PS12*P[0][0] - PS34*P[0][11] + PS6*P[0][12] + PS88 - PS9*P[0][10] + P[0][2]; - const float PS107 = PS6*P[11][12]; - const float PS108 = PS107 + PS11*P[3][11] + PS12*P[0][11] - PS13*P[1][11] - PS34*P[11][11] - PS83 + P[2][11]; - const float PS109 = PS11*P[3][10] + PS12*P[0][10] - PS13*P[1][10] + PS18 + PS76 - PS9*P[10][10] + P[2][10]; - const float PS110 = PS12*P[0][1]; - const float PS111 = PS110 - PS13*P[1][1] + PS27 - PS34*P[1][11] + PS6*P[1][12] - PS9*P[1][10] + P[1][2]; - const float PS112 = PS11*P[2][3]; - const float PS113 = PS112 + PS31 - PS34*P[2][11] + PS6*P[2][12] - PS9*P[2][10] - PS90 + P[2][2]; - const float PS114 = PS11*P[3][13] + PS12*P[0][13] - PS13*P[1][13] - PS34*P[11][13] + PS6*P[12][13] - PS9*P[10][13] + P[2][13]; - const float PS115 = PS11*P[3][15] + PS12*P[0][15] - PS13*P[1][15] - PS34*P[11][15] + PS6*P[12][15] - PS9*P[10][15] + P[2][15]; - const float PS116 = 2*PS115; - const float PS117 = PS11*P[3][14] + PS12*P[0][14] - PS13*P[1][14] - PS34*P[11][14] + PS6*P[12][14] - PS9*P[10][14] + P[2][14]; - const float PS118 = 2*PS117; - const float PS119 = PS11*P[3][4] + PS12*P[0][4] - PS13*P[1][4] - PS34*P[4][11] + PS6*P[4][12] - PS9*P[4][10] + P[2][4]; - const float PS120 = 2*PS114; - const float PS121 = PS11*P[3][5] + PS12*P[0][5] - PS13*P[1][5] - PS34*P[5][11] + PS6*P[5][12] - PS9*P[5][10] + P[2][5]; - const float PS122 = PS11*P[3][6] + PS12*P[0][6] - PS13*P[1][6] - PS34*P[6][11] + PS6*P[6][12] - PS9*P[6][10] + P[2][6]; - const float PS123 = -PS11*P[2][10] + PS12*P[1][10] + PS13*P[0][10] - PS15 + PS7*P[10][10] + PS86 + P[3][10]; - const float PS124 = PS105 + PS12*P[1][1] + PS24 - PS34*P[1][12] - PS6*P[1][11] + PS7*P[1][10] + P[1][3]; - const float PS125 = PS110 + PS13*P[0][0] - PS34*P[0][12] - PS6*P[0][11] + PS7*P[0][10] - PS81 + P[0][3]; - const float PS126 = -PS107 - PS11*P[2][12] + PS12*P[1][12] + PS13*P[0][12] - PS34*P[12][12] + PS84 + P[3][12]; - const float PS127 = PS102 - PS11*P[2][11] + PS12*P[1][11] + PS13*P[0][11] - PS6*P[11][11] + PS8 + P[3][11]; - const float PS128 = -PS11*P[2][2] + PS21 - PS34*P[2][12] - PS6*P[2][11] + PS7*P[2][10] + PS78 + P[2][3]; - const float PS129 = -PS112 + PS32 - PS34*P[3][12] - PS6*P[3][11] + PS7*P[3][10] + PS91 + P[3][3]; - const float PS130 = -PS11*P[2][13] + PS12*P[1][13] + PS13*P[0][13] - PS34*P[12][13] - PS6*P[11][13] + PS7*P[10][13] + P[3][13]; - const float PS131 = -PS11*P[2][15] + PS12*P[1][15] + PS13*P[0][15] - PS34*P[12][15] - PS6*P[11][15] + PS7*P[10][15] + P[3][15]; - const float PS132 = 2*PS131; - const float PS133 = -PS11*P[2][14] + PS12*P[1][14] + PS13*P[0][14] - PS34*P[12][14] - PS6*P[11][14] + PS7*P[10][14] + P[3][14]; - const float PS134 = 2*PS133; - const float PS135 = -PS11*P[2][4] + PS12*P[1][4] + PS13*P[0][4] - PS34*P[4][12] - PS6*P[4][11] + PS7*P[4][10] + P[3][4]; - const float PS136 = 2*PS130; - const float PS137 = -PS11*P[2][5] + PS12*P[1][5] + PS13*P[0][5] - PS34*P[5][12] - PS6*P[5][11] + PS7*P[5][10] + P[3][5]; - const float PS138 = -PS11*P[2][6] + PS12*P[1][6] + PS13*P[0][6] - PS34*P[6][12] - PS6*P[6][11] + PS7*P[6][10] + P[3][6]; - const float PS139 = 2*PS46; - const float PS140 = 2*PS54; - const float PS141 = -PS139*P[13][15] + PS140*P[13][14] - PS44*P[13][13] + PS60*P[2][13] + PS62*P[1][13] + PS72*P[0][13] - PS74*P[3][13] + P[4][13]; - const float PS142 = -PS139*P[15][15] + PS140*P[14][15] - PS44*P[13][15] + PS60*P[2][15] + PS62*P[1][15] + PS72*P[0][15] - PS74*P[3][15] + P[4][15]; - const float PS143 = PS62*P[1][3]; - const float PS144 = PS72*P[0][3]; - const float PS145 = -PS139*P[3][15] + PS140*P[3][14] + PS143 + PS144 - PS44*P[3][13] + PS60*P[2][3] - PS74*P[3][3] + P[3][4]; - const float PS146 = -PS139*P[14][15] + PS140*P[14][14] - PS44*P[13][14] + PS60*P[2][14] + PS62*P[1][14] + PS72*P[0][14] - PS74*P[3][14] + P[4][14]; - const float PS147 = PS60*P[0][2]; - const float PS148 = PS74*P[0][3]; - const float PS149 = -PS139*P[0][15] + PS140*P[0][14] + PS147 - PS148 - PS44*P[0][13] + PS62*P[0][1] + PS72*P[0][0] + P[0][4]; - const float PS150 = PS62*P[1][2]; - const float PS151 = PS72*P[0][2]; - const float PS152 = -PS139*P[2][15] + PS140*P[2][14] + PS150 + PS151 - PS44*P[2][13] + PS60*P[2][2] - PS74*P[2][3] + P[2][4]; - const float PS153 = PS60*P[1][2]; - const float PS154 = PS74*P[1][3]; - const float PS155 = -PS139*P[1][15] + PS140*P[1][14] + PS153 - PS154 - PS44*P[1][13] + PS62*P[1][1] + PS72*P[0][1] + P[1][4]; - const float PS156 = 4*dvyVar; - const float PS157 = 4*dvzVar; - const float PS158 = -PS139*P[4][15] + PS140*P[4][14] - PS44*P[4][13] + PS60*P[2][4] + PS62*P[1][4] + PS72*P[0][4] - PS74*P[3][4] + P[4][4]; - const float PS159 = 2*PS141; - const float PS160 = 2*PS68; - const float PS161 = PS65*dvyVar; - const float PS162 = 2*PS66; - const float PS163 = PS44*dvxVar; - const float PS164 = -PS139*P[5][15] + PS140*P[5][14] - PS44*P[5][13] + PS60*P[2][5] + PS62*P[1][5] + PS72*P[0][5] - PS74*P[3][5] + P[4][5]; - const float PS165 = 2*PS71; - const float PS166 = 2*PS73; - const float PS167 = PS70*dvzVar; - const float PS168 = -PS139*P[6][15] + PS140*P[6][14] - PS44*P[6][13] + PS60*P[2][6] + PS62*P[1][6] + PS72*P[0][6] - PS74*P[3][6] + P[4][6]; - const float PS169 = PS160*P[14][15] - PS162*P[13][14] - PS60*P[1][14] + PS62*P[2][14] - PS65*P[14][14] + PS72*P[3][14] + PS74*P[0][14] + P[5][14]; - const float PS170 = PS160*P[13][15] - PS162*P[13][13] - PS60*P[1][13] + PS62*P[2][13] - PS65*P[13][14] + PS72*P[3][13] + PS74*P[0][13] + P[5][13]; - const float PS171 = PS74*P[0][1]; - const float PS172 = PS150 + PS160*P[1][15] - PS162*P[1][13] + PS171 - PS60*P[1][1] - PS65*P[1][14] + PS72*P[1][3] + P[1][5]; - const float PS173 = PS160*P[15][15] - PS162*P[13][15] - PS60*P[1][15] + PS62*P[2][15] - PS65*P[14][15] + PS72*P[3][15] + PS74*P[0][15] + P[5][15]; - const float PS174 = PS62*P[2][3]; - const float PS175 = PS148 + PS160*P[3][15] - PS162*P[3][13] + PS174 - PS60*P[1][3] - PS65*P[3][14] + PS72*P[3][3] + P[3][5]; - const float PS176 = PS60*P[0][1]; - const float PS177 = PS144 + PS160*P[0][15] - PS162*P[0][13] - PS176 + PS62*P[0][2] - PS65*P[0][14] + PS74*P[0][0] + P[0][5]; - const float PS178 = PS72*P[2][3]; - const float PS179 = -PS153 + PS160*P[2][15] - PS162*P[2][13] + PS178 + PS62*P[2][2] - PS65*P[2][14] + PS74*P[0][2] + P[2][5]; - const float PS180 = 4*dvxVar; - const float PS181 = PS160*P[5][15] - PS162*P[5][13] - PS60*P[1][5] + PS62*P[2][5] - PS65*P[5][14] + PS72*P[3][5] + PS74*P[0][5] + P[5][5]; - const float PS182 = PS160*P[6][15] - PS162*P[6][13] - PS60*P[1][6] + PS62*P[2][6] - PS65*P[6][14] + PS72*P[3][6] + PS74*P[0][6] + P[5][6]; - const float PS183 = -PS165*P[14][15] + PS166*P[13][15] + PS60*P[0][15] + PS62*P[3][15] - PS70*P[15][15] - PS72*P[2][15] + PS74*P[1][15] + P[6][15]; - const float PS184 = -PS165*P[14][14] + PS166*P[13][14] + PS60*P[0][14] + PS62*P[3][14] - PS70*P[14][15] - PS72*P[2][14] + PS74*P[1][14] + P[6][14]; - const float PS185 = -PS165*P[13][14] + PS166*P[13][13] + PS60*P[0][13] + PS62*P[3][13] - PS70*P[13][15] - PS72*P[2][13] + PS74*P[1][13] + P[6][13]; - const float PS186 = -PS165*P[6][14] + PS166*P[6][13] + PS60*P[0][6] + PS62*P[3][6] - PS70*P[6][15] - PS72*P[2][6] + PS74*P[1][6] + P[6][6]; + const ftype PS0 = powF(q1, 2); + const ftype PS1 = 0.25F*daxVar; + const ftype PS2 = powF(q2, 2); + const ftype PS3 = 0.25F*dayVar; + const ftype PS4 = powF(q3, 2); + const ftype PS5 = 0.25F*dazVar; + const ftype PS6 = 0.5F*q1; + const ftype PS7 = 0.5F*q2; + const ftype PS8 = PS7*P[10][11]; + const ftype PS9 = 0.5F*q3; + const ftype PS10 = PS9*P[10][12]; + const ftype PS11 = 0.5F*dax - 0.5F*dax_b; + const ftype PS12 = 0.5F*day - 0.5F*day_b; + const ftype PS13 = 0.5F*daz - 0.5F*daz_b; + const ftype PS14 = PS10 - PS11*P[1][10] - PS12*P[2][10] - PS13*P[3][10] + PS6*P[10][10] + PS8 + P[0][10]; + const ftype PS15 = PS6*P[10][11]; + const ftype PS16 = PS9*P[11][12]; + const ftype PS17 = -PS11*P[1][11] - PS12*P[2][11] - PS13*P[3][11] + PS15 + PS16 + PS7*P[11][11] + P[0][11]; + const ftype PS18 = PS6*P[10][12]; + const ftype PS19 = PS7*P[11][12]; + const ftype PS20 = -PS11*P[1][12] - PS12*P[2][12] - PS13*P[3][12] + PS18 + PS19 + PS9*P[12][12] + P[0][12]; + const ftype PS21 = PS12*P[1][2]; + const ftype PS22 = -PS13*P[1][3]; + const ftype PS23 = -PS11*P[1][1] - PS21 + PS22 + PS6*P[1][10] + PS7*P[1][11] + PS9*P[1][12] + P[0][1]; + const ftype PS24 = -PS11*P[1][2]; + const ftype PS25 = PS13*P[2][3]; + const ftype PS26 = -PS12*P[2][2] + PS24 - PS25 + PS6*P[2][10] + PS7*P[2][11] + PS9*P[2][12] + P[0][2]; + const ftype PS27 = PS11*P[1][3]; + const ftype PS28 = -PS12*P[2][3]; + const ftype PS29 = -PS13*P[3][3] - PS27 + PS28 + PS6*P[3][10] + PS7*P[3][11] + PS9*P[3][12] + P[0][3]; + const ftype PS30 = PS11*P[0][1]; + const ftype PS31 = PS12*P[0][2]; + const ftype PS32 = PS13*P[0][3]; + const ftype PS33 = -PS30 - PS31 - PS32 + PS6*P[0][10] + PS7*P[0][11] + PS9*P[0][12] + P[0][0]; + const ftype PS34 = 0.5F*q0; + const ftype PS35 = q2*q3; + const ftype PS36 = q0*q1; + const ftype PS37 = q1*q3; + const ftype PS38 = q0*q2; + const ftype PS39 = q1*q2; + const ftype PS40 = q0*q3; + const ftype PS41 = -PS2; + const ftype PS42 = powF(q0, 2); + const ftype PS43 = -PS4 + PS42; + const ftype PS44 = PS0 + PS41 + PS43; + const ftype PS45 = -PS11*P[1][13] - PS12*P[2][13] - PS13*P[3][13] + PS6*P[10][13] + PS7*P[11][13] + PS9*P[12][13] + P[0][13]; + const ftype PS46 = PS37 + PS38; + const ftype PS47 = -PS11*P[1][15] - PS12*P[2][15] - PS13*P[3][15] + PS6*P[10][15] + PS7*P[11][15] + PS9*P[12][15] + P[0][15]; + const ftype PS48 = 2*PS47; + const ftype PS49 = dvy - dvy_b; + const ftype PS50 = dvx - dvx_b; + const ftype PS51 = dvz - dvz_b; + const ftype PS52 = PS49*q0 + PS50*q3 - PS51*q1; + const ftype PS53 = 2*PS29; + const ftype PS54 = -PS39 + PS40; + const ftype PS55 = -PS11*P[1][14] - PS12*P[2][14] - PS13*P[3][14] + PS6*P[10][14] + PS7*P[11][14] + PS9*P[12][14] + P[0][14]; + const ftype PS56 = 2*PS55; + const ftype PS57 = -PS49*q3 + PS50*q0 + PS51*q2; + const ftype PS58 = 2*PS33; + const ftype PS59 = PS49*q1 - PS50*q2 + PS51*q0; + const ftype PS60 = 2*PS59; + const ftype PS61 = PS49*q2 + PS50*q1 + PS51*q3; + const ftype PS62 = 2*PS61; + const ftype PS63 = -PS11*P[1][4] - PS12*P[2][4] - PS13*P[3][4] + PS6*P[4][10] + PS7*P[4][11] + PS9*P[4][12] + P[0][4]; + const ftype PS64 = -PS0; + const ftype PS65 = PS2 + PS43 + PS64; + const ftype PS66 = PS39 + PS40; + const ftype PS67 = 2*PS45; + const ftype PS68 = -PS35 + PS36; + const ftype PS69 = -PS11*P[1][5] - PS12*P[2][5] - PS13*P[3][5] + PS6*P[5][10] + PS7*P[5][11] + PS9*P[5][12] + P[0][5]; + const ftype PS70 = PS4 + PS41 + PS42 + PS64; + const ftype PS71 = PS35 + PS36; + const ftype PS72 = 2*PS57; + const ftype PS73 = -PS37 + PS38; + const ftype PS74 = 2*PS52; + const ftype PS75 = -PS11*P[1][6] - PS12*P[2][6] - PS13*P[3][6] + PS6*P[6][10] + PS7*P[6][11] + PS9*P[6][12] + P[0][6]; + const ftype PS76 = -PS34*P[10][11]; + const ftype PS77 = PS11*P[0][11] - PS12*P[3][11] + PS13*P[2][11] - PS19 + PS76 + PS9*P[11][11] + P[1][11]; + const ftype PS78 = PS13*P[0][2]; + const ftype PS79 = PS12*P[0][3]; + const ftype PS80 = PS11*P[0][0] - PS34*P[0][10] - PS7*P[0][12] + PS78 - PS79 + PS9*P[0][11] + P[0][1]; + const ftype PS81 = PS11*P[0][2]; + const ftype PS82 = PS13*P[2][2] + PS28 - PS34*P[2][10] - PS7*P[2][12] + PS81 + PS9*P[2][11] + P[1][2]; + const ftype PS83 = PS9*P[10][11]; + const ftype PS84 = PS7*P[10][12]; + const ftype PS85 = PS11*P[0][10] - PS12*P[3][10] + PS13*P[2][10] - PS34*P[10][10] + PS83 - PS84 + P[1][10]; + const ftype PS86 = -PS34*P[10][12]; + const ftype PS87 = PS11*P[0][12] - PS12*P[3][12] + PS13*P[2][12] + PS16 - PS7*P[12][12] + PS86 + P[1][12]; + const ftype PS88 = PS11*P[0][3]; + const ftype PS89 = -PS12*P[3][3] + PS25 - PS34*P[3][10] - PS7*P[3][12] + PS88 + PS9*P[3][11] + P[1][3]; + const ftype PS90 = PS13*P[1][2]; + const ftype PS91 = PS12*P[1][3]; + const ftype PS92 = PS30 - PS34*P[1][10] - PS7*P[1][12] + PS9*P[1][11] + PS90 - PS91 + P[1][1]; + const ftype PS93 = PS11*P[0][13] - PS12*P[3][13] + PS13*P[2][13] - PS34*P[10][13] - PS7*P[12][13] + PS9*P[11][13] + P[1][13]; + const ftype PS94 = PS11*P[0][15] - PS12*P[3][15] + PS13*P[2][15] - PS34*P[10][15] - PS7*P[12][15] + PS9*P[11][15] + P[1][15]; + const ftype PS95 = 2*PS94; + const ftype PS96 = PS11*P[0][14] - PS12*P[3][14] + PS13*P[2][14] - PS34*P[10][14] - PS7*P[12][14] + PS9*P[11][14] + P[1][14]; + const ftype PS97 = 2*PS96; + const ftype PS98 = PS11*P[0][4] - PS12*P[3][4] + PS13*P[2][4] - PS34*P[4][10] - PS7*P[4][12] + PS9*P[4][11] + P[1][4]; + const ftype PS99 = 2*PS93; + const ftype PS100 = PS11*P[0][5] - PS12*P[3][5] + PS13*P[2][5] - PS34*P[5][10] - PS7*P[5][12] + PS9*P[5][11] + P[1][5]; + const ftype PS101 = PS11*P[0][6] - PS12*P[3][6] + PS13*P[2][6] - PS34*P[6][10] - PS7*P[6][12] + PS9*P[6][11] + P[1][6]; + const ftype PS102 = -PS34*P[11][12]; + const ftype PS103 = -PS10 + PS102 + PS11*P[3][12] + PS12*P[0][12] - PS13*P[1][12] + PS6*P[12][12] + P[2][12]; + const ftype PS104 = PS11*P[3][3] + PS22 - PS34*P[3][11] + PS6*P[3][12] + PS79 - PS9*P[3][10] + P[2][3]; + const ftype PS105 = PS13*P[0][1]; + const ftype PS106 = -PS105 + PS12*P[0][0] - PS34*P[0][11] + PS6*P[0][12] + PS88 - PS9*P[0][10] + P[0][2]; + const ftype PS107 = PS6*P[11][12]; + const ftype PS108 = PS107 + PS11*P[3][11] + PS12*P[0][11] - PS13*P[1][11] - PS34*P[11][11] - PS83 + P[2][11]; + const ftype PS109 = PS11*P[3][10] + PS12*P[0][10] - PS13*P[1][10] + PS18 + PS76 - PS9*P[10][10] + P[2][10]; + const ftype PS110 = PS12*P[0][1]; + const ftype PS111 = PS110 - PS13*P[1][1] + PS27 - PS34*P[1][11] + PS6*P[1][12] - PS9*P[1][10] + P[1][2]; + const ftype PS112 = PS11*P[2][3]; + const ftype PS113 = PS112 + PS31 - PS34*P[2][11] + PS6*P[2][12] - PS9*P[2][10] - PS90 + P[2][2]; + const ftype PS114 = PS11*P[3][13] + PS12*P[0][13] - PS13*P[1][13] - PS34*P[11][13] + PS6*P[12][13] - PS9*P[10][13] + P[2][13]; + const ftype PS115 = PS11*P[3][15] + PS12*P[0][15] - PS13*P[1][15] - PS34*P[11][15] + PS6*P[12][15] - PS9*P[10][15] + P[2][15]; + const ftype PS116 = 2*PS115; + const ftype PS117 = PS11*P[3][14] + PS12*P[0][14] - PS13*P[1][14] - PS34*P[11][14] + PS6*P[12][14] - PS9*P[10][14] + P[2][14]; + const ftype PS118 = 2*PS117; + const ftype PS119 = PS11*P[3][4] + PS12*P[0][4] - PS13*P[1][4] - PS34*P[4][11] + PS6*P[4][12] - PS9*P[4][10] + P[2][4]; + const ftype PS120 = 2*PS114; + const ftype PS121 = PS11*P[3][5] + PS12*P[0][5] - PS13*P[1][5] - PS34*P[5][11] + PS6*P[5][12] - PS9*P[5][10] + P[2][5]; + const ftype PS122 = PS11*P[3][6] + PS12*P[0][6] - PS13*P[1][6] - PS34*P[6][11] + PS6*P[6][12] - PS9*P[6][10] + P[2][6]; + const ftype PS123 = -PS11*P[2][10] + PS12*P[1][10] + PS13*P[0][10] - PS15 + PS7*P[10][10] + PS86 + P[3][10]; + const ftype PS124 = PS105 + PS12*P[1][1] + PS24 - PS34*P[1][12] - PS6*P[1][11] + PS7*P[1][10] + P[1][3]; + const ftype PS125 = PS110 + PS13*P[0][0] - PS34*P[0][12] - PS6*P[0][11] + PS7*P[0][10] - PS81 + P[0][3]; + const ftype PS126 = -PS107 - PS11*P[2][12] + PS12*P[1][12] + PS13*P[0][12] - PS34*P[12][12] + PS84 + P[3][12]; + const ftype PS127 = PS102 - PS11*P[2][11] + PS12*P[1][11] + PS13*P[0][11] - PS6*P[11][11] + PS8 + P[3][11]; + const ftype PS128 = -PS11*P[2][2] + PS21 - PS34*P[2][12] - PS6*P[2][11] + PS7*P[2][10] + PS78 + P[2][3]; + const ftype PS129 = -PS112 + PS32 - PS34*P[3][12] - PS6*P[3][11] + PS7*P[3][10] + PS91 + P[3][3]; + const ftype PS130 = -PS11*P[2][13] + PS12*P[1][13] + PS13*P[0][13] - PS34*P[12][13] - PS6*P[11][13] + PS7*P[10][13] + P[3][13]; + const ftype PS131 = -PS11*P[2][15] + PS12*P[1][15] + PS13*P[0][15] - PS34*P[12][15] - PS6*P[11][15] + PS7*P[10][15] + P[3][15]; + const ftype PS132 = 2*PS131; + const ftype PS133 = -PS11*P[2][14] + PS12*P[1][14] + PS13*P[0][14] - PS34*P[12][14] - PS6*P[11][14] + PS7*P[10][14] + P[3][14]; + const ftype PS134 = 2*PS133; + const ftype PS135 = -PS11*P[2][4] + PS12*P[1][4] + PS13*P[0][4] - PS34*P[4][12] - PS6*P[4][11] + PS7*P[4][10] + P[3][4]; + const ftype PS136 = 2*PS130; + const ftype PS137 = -PS11*P[2][5] + PS12*P[1][5] + PS13*P[0][5] - PS34*P[5][12] - PS6*P[5][11] + PS7*P[5][10] + P[3][5]; + const ftype PS138 = -PS11*P[2][6] + PS12*P[1][6] + PS13*P[0][6] - PS34*P[6][12] - PS6*P[6][11] + PS7*P[6][10] + P[3][6]; + const ftype PS139 = 2*PS46; + const ftype PS140 = 2*PS54; + const ftype PS141 = -PS139*P[13][15] + PS140*P[13][14] - PS44*P[13][13] + PS60*P[2][13] + PS62*P[1][13] + PS72*P[0][13] - PS74*P[3][13] + P[4][13]; + const ftype PS142 = -PS139*P[15][15] + PS140*P[14][15] - PS44*P[13][15] + PS60*P[2][15] + PS62*P[1][15] + PS72*P[0][15] - PS74*P[3][15] + P[4][15]; + const ftype PS143 = PS62*P[1][3]; + const ftype PS144 = PS72*P[0][3]; + const ftype PS145 = -PS139*P[3][15] + PS140*P[3][14] + PS143 + PS144 - PS44*P[3][13] + PS60*P[2][3] - PS74*P[3][3] + P[3][4]; + const ftype PS146 = -PS139*P[14][15] + PS140*P[14][14] - PS44*P[13][14] + PS60*P[2][14] + PS62*P[1][14] + PS72*P[0][14] - PS74*P[3][14] + P[4][14]; + const ftype PS147 = PS60*P[0][2]; + const ftype PS148 = PS74*P[0][3]; + const ftype PS149 = -PS139*P[0][15] + PS140*P[0][14] + PS147 - PS148 - PS44*P[0][13] + PS62*P[0][1] + PS72*P[0][0] + P[0][4]; + const ftype PS150 = PS62*P[1][2]; + const ftype PS151 = PS72*P[0][2]; + const ftype PS152 = -PS139*P[2][15] + PS140*P[2][14] + PS150 + PS151 - PS44*P[2][13] + PS60*P[2][2] - PS74*P[2][3] + P[2][4]; + const ftype PS153 = PS60*P[1][2]; + const ftype PS154 = PS74*P[1][3]; + const ftype PS155 = -PS139*P[1][15] + PS140*P[1][14] + PS153 - PS154 - PS44*P[1][13] + PS62*P[1][1] + PS72*P[0][1] + P[1][4]; + const ftype PS156 = 4*dvyVar; + const ftype PS157 = 4*dvzVar; + const ftype PS158 = -PS139*P[4][15] + PS140*P[4][14] - PS44*P[4][13] + PS60*P[2][4] + PS62*P[1][4] + PS72*P[0][4] - PS74*P[3][4] + P[4][4]; + const ftype PS159 = 2*PS141; + const ftype PS160 = 2*PS68; + const ftype PS161 = PS65*dvyVar; + const ftype PS162 = 2*PS66; + const ftype PS163 = PS44*dvxVar; + const ftype PS164 = -PS139*P[5][15] + PS140*P[5][14] - PS44*P[5][13] + PS60*P[2][5] + PS62*P[1][5] + PS72*P[0][5] - PS74*P[3][5] + P[4][5]; + const ftype PS165 = 2*PS71; + const ftype PS166 = 2*PS73; + const ftype PS167 = PS70*dvzVar; + const ftype PS168 = -PS139*P[6][15] + PS140*P[6][14] - PS44*P[6][13] + PS60*P[2][6] + PS62*P[1][6] + PS72*P[0][6] - PS74*P[3][6] + P[4][6]; + const ftype PS169 = PS160*P[14][15] - PS162*P[13][14] - PS60*P[1][14] + PS62*P[2][14] - PS65*P[14][14] + PS72*P[3][14] + PS74*P[0][14] + P[5][14]; + const ftype PS170 = PS160*P[13][15] - PS162*P[13][13] - PS60*P[1][13] + PS62*P[2][13] - PS65*P[13][14] + PS72*P[3][13] + PS74*P[0][13] + P[5][13]; + const ftype PS171 = PS74*P[0][1]; + const ftype PS172 = PS150 + PS160*P[1][15] - PS162*P[1][13] + PS171 - PS60*P[1][1] - PS65*P[1][14] + PS72*P[1][3] + P[1][5]; + const ftype PS173 = PS160*P[15][15] - PS162*P[13][15] - PS60*P[1][15] + PS62*P[2][15] - PS65*P[14][15] + PS72*P[3][15] + PS74*P[0][15] + P[5][15]; + const ftype PS174 = PS62*P[2][3]; + const ftype PS175 = PS148 + PS160*P[3][15] - PS162*P[3][13] + PS174 - PS60*P[1][3] - PS65*P[3][14] + PS72*P[3][3] + P[3][5]; + const ftype PS176 = PS60*P[0][1]; + const ftype PS177 = PS144 + PS160*P[0][15] - PS162*P[0][13] - PS176 + PS62*P[0][2] - PS65*P[0][14] + PS74*P[0][0] + P[0][5]; + const ftype PS178 = PS72*P[2][3]; + const ftype PS179 = -PS153 + PS160*P[2][15] - PS162*P[2][13] + PS178 + PS62*P[2][2] - PS65*P[2][14] + PS74*P[0][2] + P[2][5]; + const ftype PS180 = 4*dvxVar; + const ftype PS181 = PS160*P[5][15] - PS162*P[5][13] - PS60*P[1][5] + PS62*P[2][5] - PS65*P[5][14] + PS72*P[3][5] + PS74*P[0][5] + P[5][5]; + const ftype PS182 = PS160*P[6][15] - PS162*P[6][13] - PS60*P[1][6] + PS62*P[2][6] - PS65*P[6][14] + PS72*P[3][6] + PS74*P[0][6] + P[5][6]; + const ftype PS183 = -PS165*P[14][15] + PS166*P[13][15] + PS60*P[0][15] + PS62*P[3][15] - PS70*P[15][15] - PS72*P[2][15] + PS74*P[1][15] + P[6][15]; + const ftype PS184 = -PS165*P[14][14] + PS166*P[13][14] + PS60*P[0][14] + PS62*P[3][14] - PS70*P[14][15] - PS72*P[2][14] + PS74*P[1][14] + P[6][14]; + const ftype PS185 = -PS165*P[13][14] + PS166*P[13][13] + PS60*P[0][13] + PS62*P[3][13] - PS70*P[13][15] - PS72*P[2][13] + PS74*P[1][13] + P[6][13]; + const ftype PS186 = -PS165*P[6][14] + PS166*P[6][13] + PS60*P[0][6] + PS62*P[3][6] - PS70*P[6][15] - PS72*P[2][6] + PS74*P[1][6] + P[6][6]; nextP[0][0] = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5; nextP[0][1] = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5; @@ -1355,7 +1355,7 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) // to lower and upper half in P for (uint8_t row = 0; row <= 3; row++) { // copy diagonals - P[row][row] = constrain_float(nextP[row][row], 0.0f, 1.0f); + P[row][row] = constrain_ftype(nextP[row][row], 0.0f, 1.0f); // copy off diagonals for (uint8_t column = 0 ; column < row; column++) { P[row][column] = P[column][row] = nextP[column][row]; @@ -1369,20 +1369,20 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) nextP[1][4] = -PS44*PS93 - PS46*PS95 + PS54*PS97 + PS60*PS82 + PS62*PS92 + PS72*PS80 - PS74*PS89 + PS98; nextP[2][4] = -PS104*PS74 + PS106*PS72 + PS111*PS62 + PS113*PS60 - PS114*PS44 - PS116*PS46 + PS118*PS54 + PS119; nextP[3][4] = PS124*PS62 + PS125*PS72 + PS128*PS60 - PS129*PS74 - PS130*PS44 - PS132*PS46 + PS134*PS54 + PS135; - nextP[4][4] = -PS139*PS142 + PS140*PS146 - PS141*PS44 - PS145*PS74 + PS149*PS72 + PS152*PS60 + PS155*PS62 + PS156*powf(PS54, 2) + PS157*powf(PS46, 2) + PS158 + powf(PS44, 2)*dvxVar; + nextP[4][4] = -PS139*PS142 + PS140*PS146 - PS141*PS44 - PS145*PS74 + PS149*PS72 + PS152*PS60 + PS155*PS62 + PS156*powF(PS54, 2) + PS157*powF(PS46, 2) + PS158 + powF(PS44, 2)*dvxVar; nextP[0][5] = -PS23*PS60 + PS26*PS62 + PS48*PS68 + PS52*PS58 + PS53*PS57 - PS55*PS65 - PS66*PS67 + PS69; nextP[1][5] = PS100 - PS60*PS92 + PS62*PS82 - PS65*PS96 - PS66*PS99 + PS68*PS95 + PS72*PS89 + PS74*PS80; nextP[2][5] = PS104*PS72 + PS106*PS74 - PS111*PS60 + PS113*PS62 + PS116*PS68 - PS117*PS65 - PS120*PS66 + PS121; nextP[3][5] = -PS124*PS60 + PS125*PS74 + PS128*PS62 + PS129*PS72 + PS132*PS68 - PS133*PS65 - PS136*PS66 + PS137; nextP[4][5] = -PS140*PS161 + PS142*PS160 + PS145*PS72 - PS146*PS65 + PS149*PS74 + PS152*PS62 - PS155*PS60 - PS157*PS46*PS68 - PS159*PS66 + PS162*PS163 + PS164; - nextP[5][5] = PS157*powf(PS68, 2) + PS160*PS173 - PS162*PS170 - PS169*PS65 - PS172*PS60 + PS175*PS72 + PS177*PS74 + PS179*PS62 + PS180*powf(PS66, 2) + PS181 + powf(PS65, 2)*dvyVar; + nextP[5][5] = PS157*powF(PS68, 2) + PS160*PS173 - PS162*PS170 - PS169*PS65 - PS172*PS60 + PS175*PS72 + PS177*PS74 + PS179*PS62 + PS180*powF(PS66, 2) + PS181 + powF(PS65, 2)*dvyVar; nextP[0][6] = PS23*PS74 - PS26*PS72 - PS47*PS70 + PS53*PS61 - PS56*PS71 + PS58*PS59 + PS67*PS73 + PS75; nextP[1][6] = PS101 + PS60*PS80 + PS62*PS89 - PS70*PS94 - PS71*PS97 - PS72*PS82 + PS73*PS99 + PS74*PS92; nextP[2][6] = PS104*PS62 + PS106*PS60 + PS111*PS74 - PS113*PS72 - PS115*PS70 - PS118*PS71 + PS120*PS73 + PS122; nextP[3][6] = PS124*PS74 + PS125*PS60 - PS128*PS72 + PS129*PS62 - PS131*PS70 - PS134*PS71 + PS136*PS73 + PS138; nextP[4][6] = PS139*PS167 - PS142*PS70 + PS145*PS62 - PS146*PS165 + PS149*PS60 - PS152*PS72 + PS155*PS74 - PS156*PS54*PS71 + PS159*PS73 - PS163*PS166 + PS168; nextP[5][6] = -PS160*PS167 + PS161*PS165 - PS165*PS169 + PS166*PS170 + PS172*PS74 - PS173*PS70 + PS175*PS62 + PS177*PS60 - PS179*PS72 - PS180*PS66*PS73 + PS182; - nextP[6][6] = PS156*powf(PS71, 2) - PS165*PS184 + PS166*PS185 + PS180*powf(PS73, 2) - PS183*PS70 + PS186 + PS60*(-PS151 - PS165*P[0][14] + PS166*P[0][13] + PS171 + PS60*P[0][0] + PS62*P[0][3] - PS70*P[0][15] + P[0][6]) + PS62*(PS154 - PS165*P[3][14] + PS166*P[3][13] - PS178 + PS60*P[0][3] + PS62*P[3][3] - PS70*P[3][15] + P[3][6]) + powf(PS70, 2)*dvzVar - PS72*(PS147 - PS165*P[2][14] + PS166*P[2][13] + PS174 - PS70*P[2][15] - PS72*P[2][2] + PS74*P[1][2] + P[2][6]) + PS74*(PS143 - PS165*P[1][14] + PS166*P[1][13] + PS176 - PS70*P[1][15] - PS72*P[1][2] + PS74*P[1][1] + P[1][6]); + nextP[6][6] = PS156*powF(PS71, 2) - PS165*PS184 + PS166*PS185 + PS180*powF(PS73, 2) - PS183*PS70 + PS186 + PS60*(-PS151 - PS165*P[0][14] + PS166*P[0][13] + PS171 + PS60*P[0][0] + PS62*P[0][3] - PS70*P[0][15] + P[0][6]) + PS62*(PS154 - PS165*P[3][14] + PS166*P[3][13] - PS178 + PS60*P[0][3] + PS62*P[3][3] - PS70*P[3][15] + P[3][6]) + powF(PS70, 2)*dvzVar - PS72*(PS147 - PS165*P[2][14] + PS166*P[2][13] + PS174 - PS70*P[2][15] - PS72*P[2][2] + PS74*P[1][2] + P[2][6]) + PS74*(PS143 - PS165*P[1][14] + PS166*P[1][13] + PS176 - PS70*P[1][15] - PS72*P[1][2] + PS74*P[1][1] + P[1][6]); nextP[0][7] = -PS11*P[1][7] - PS12*P[2][7] - PS13*P[3][7] + PS6*P[7][10] + PS63*dt + PS7*P[7][11] + PS9*P[7][12] + P[0][7]; nextP[1][7] = PS11*P[0][7] - PS12*P[3][7] + PS13*P[2][7] - PS34*P[7][10] - PS7*P[7][12] + PS9*P[7][11] + PS98*dt + P[1][7]; nextP[2][7] = PS11*P[3][7] + PS119*dt + PS12*P[0][7] - PS13*P[1][7] - PS34*P[7][11] + PS6*P[7][12] - PS9*P[7][10] + P[2][7]; @@ -1733,7 +1733,7 @@ void NavEKF3_core::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last) uint8_t row; for (row=first; row<=last; row++) { - memset(&covMat[row][0], 0, sizeof(covMat[0][0])*24); + zero_range(&covMat[row][0], 0, 23); } } @@ -1743,7 +1743,7 @@ void NavEKF3_core::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last) uint8_t row; for (row=0; row<=23; row++) { - memset(&covMat[row][first], 0, sizeof(covMat[0][0])*(1+last-first)); + zero_range(&covMat[row][0], first, last); } } @@ -1775,7 +1775,7 @@ void NavEKF3_core::StoreQuatReset() } // Rotate the stored output quaternion history through a quaternion rotation -void NavEKF3_core::StoreQuatRotate(const Quaternion &deltaQuat) +void NavEKF3_core::StoreQuatRotate(const QuaternionF &deltaQuat) { outputDataNew.quat = outputDataNew.quat*deltaQuat; // write current measurement to entire table @@ -1792,7 +1792,7 @@ void NavEKF3_core::ForceSymmetry() { for (uint8_t j=0; j<=i-1; j++) { - float temp = 0.5f*(P[i][j] + P[j][i]); + ftype temp = 0.5f*(P[i][j] + P[j][i]); P[i][j] = temp; P[j][i] = temp; } @@ -1803,8 +1803,8 @@ void NavEKF3_core::ForceSymmetry() // if states are inactive, zero the corresponding off-diagonals void NavEKF3_core::ConstrainVariances() { - for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error - for (uint8_t i=4; i<=5; i++) P[i][i] = constrain_float(P[i][i], VEL_STATE_MIN_VARIANCE, 1.0e3f); // NE velocity + for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_ftype(P[i][i],0.0,1.0); // attitude error + for (uint8_t i=4; i<=5; i++) P[i][i] = constrain_ftype(P[i][i], VEL_STATE_MIN_VARIANCE, 1.0e3); // NE velocity // check for collapse of the vertical velocity variance if (P[6][6] < VEL_STATE_MIN_VARIANCE) { @@ -1831,21 +1831,21 @@ void NavEKF3_core::ConstrainVariances() } } - for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_float(P[i][i], POS_STATE_MIN_VARIANCE, 1.0e6f); // NED position + for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_ftype(P[i][i], POS_STATE_MIN_VARIANCE, 1.0e6); // NED position if (!inhibitDelAngBiasStates) { - for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); + for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,sq(0.175 * dtEkfAvg)); } else { zeroCols(P,10,12); zeroRows(P,10,12); } - const float minStateVarTarget = 1E-8f; + const ftype minStateVarTarget = 1E-8; if (!inhibitDelVelBiasStates) { // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum - const float minSafeStateVar = 1e-9f; - float maxStateVar = minSafeStateVar; + const ftype minSafeStateVar = 1e-9; + ftype maxStateVar = minSafeStateVar; bool resetRequired = false; for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { if (P[stateIndex][stateIndex] > maxStateVar) { @@ -1857,14 +1857,14 @@ void NavEKF3_core::ConstrainVariances() // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must // not exceed 100 and the minimum variance must not fall below the target minimum - float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); + ftype minAllowedStateVar = fmaxF(0.01f * maxStateVar, minStateVarTarget); for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { - P[stateIndex][stateIndex] = constrain_float(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * dtEkfAvg)); + P[stateIndex][stateIndex] = constrain_ftype(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * dtEkfAvg)); } // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { - float delVelBiasVar[3]; + ftype delVelBiasVar[3]; // store all delta velocity bias variances for (uint8_t i=0; i<=2; i++) { delVelBiasVar[i] = P[i+13][i+13]; @@ -1882,20 +1882,20 @@ void NavEKF3_core::ConstrainVariances() zeroRows(P,13,15); for (uint8_t i=0; i<=2; i++) { const uint8_t stateIndex = 1 + 13; - P[stateIndex][stateIndex] = fmaxf(P[stateIndex][stateIndex], minStateVarTarget); + P[stateIndex][stateIndex] = fmaxF(P[stateIndex][stateIndex], minStateVarTarget); } } if (!inhibitMagStates) { - for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // earth magnetic field - for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body magnetic field + for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // earth magnetic field + for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // body magnetic field } else { zeroCols(P,16,21); zeroRows(P,16,21); } if (!inhibitWindStates) { - for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); + for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,1.0e3f); } else { zeroCols(P,22,23); zeroRows(P,22,23); @@ -1906,14 +1906,14 @@ void NavEKF3_core::ConstrainVariances() void NavEKF3_core::MagTableConstrain(void) { // constrain to error from table earth field - float limit_ga = frontend->_mag_ef_limit * 0.001f; - stateStruct.earth_magfield.x = constrain_float(stateStruct.earth_magfield.x, + ftype limit_ga = frontend->_mag_ef_limit * 0.001f; + stateStruct.earth_magfield.x = constrain_ftype(stateStruct.earth_magfield.x, table_earth_field_ga.x-limit_ga, table_earth_field_ga.x+limit_ga); - stateStruct.earth_magfield.y = constrain_float(stateStruct.earth_magfield.y, + stateStruct.earth_magfield.y = constrain_ftype(stateStruct.earth_magfield.y, table_earth_field_ga.y-limit_ga, table_earth_field_ga.y+limit_ga); - stateStruct.earth_magfield.z = constrain_float(stateStruct.earth_magfield.z, + stateStruct.earth_magfield.z = constrain_ftype(stateStruct.earth_magfield.z, table_earth_field_ga.z-limit_ga, table_earth_field_ga.z+limit_ga); } @@ -1922,29 +1922,29 @@ void NavEKF3_core::MagTableConstrain(void) void NavEKF3_core::ConstrainStates() { // quaternions are limited between +-1 - for (uint8_t i=0; i<=3; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f); + for (uint8_t i=0; i<=3; i++) statesArray[i] = constrain_ftype(statesArray[i],-1.0f,1.0f); // velocity limit 500 m/sec (could set this based on some multiple of max airspeed * EAS2TAS) - for (uint8_t i=4; i<=6; i++) statesArray[i] = constrain_float(statesArray[i],-5.0e2f,5.0e2f); - // position limit 1000 km - TODO apply circular limit - for (uint8_t i=7; i<=8; i++) statesArray[i] = constrain_float(statesArray[i],-1.0e6f,1.0e6f); + for (uint8_t i=4; i<=6; i++) statesArray[i] = constrain_ftype(statesArray[i],-5.0e2f,5.0e2f); + // position limit TODO apply circular limit + for (uint8_t i=7; i<=8; i++) statesArray[i] = constrain_ftype(statesArray[i],-EK3_POSXY_STATE_LIMIT,EK3_POSXY_STATE_LIMIT); // height limit covers home alt on everest through to home alt at SL and balloon drop - stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f); + stateStruct.position.z = constrain_ftype(stateStruct.position.z,-4.0e4f,1.0e4f); // gyro bias limit (this needs to be set based on manufacturers specs) - for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg); + for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_ftype(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg); // the accelerometer bias limit is controlled by a user adjustable parameter - for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_float(statesArray[i],-frontend->_accBiasLim*dtEkfAvg,frontend->_accBiasLim*dtEkfAvg); + for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_ftype(statesArray[i],-frontend->_accBiasLim*dtEkfAvg,frontend->_accBiasLim*dtEkfAvg); // earth magnetic field limit if (frontend->_mag_ef_limit <= 0 || !have_table_earth_field) { // constrain to +/-1Ga - for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f); + for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_ftype(statesArray[i],-1.0f,1.0f); } else { // use table constrain MagTableConstrain(); } // body magnetic field limit - for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_float(statesArray[i],-0.5f,0.5f); + for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_ftype(statesArray[i],-0.5f,0.5f); // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit - for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_float(statesArray[i],-100.0f,100.0f); + for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_ftype(statesArray[i],-100.0f,100.0f); // constrain the terrain state to be below the vehicle height unless we are using terrain as the height datum if (!inhibitGndState) { terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); @@ -1952,12 +1952,12 @@ void NavEKF3_core::ConstrainStates() } // calculate the NED earth spin vector in rad/sec -void NavEKF3_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const +void NavEKF3_core::calcEarthRateNED(Vector3F &omega, int32_t latitude) const { - float lat_rad = radians(latitude*1.0e-7f); - omega.x = earthRate*cosf(lat_rad); + ftype lat_rad = radians(latitude*1.0e-7f); + omega.x = earthRate*cosF(lat_rad); omega.y = 0; - omega.z = -earthRate*sinf(lat_rad); + omega.z = -earthRate*sinF(lat_rad); } // set yaw from a single magnetometer sample @@ -1973,8 +1973,8 @@ void NavEKF3_core::setYawFromMag() // use best of either 312 or 321 rotation sequence when calculating yaw rotationOrder order; bestRotationOrder(order); - Vector3f eulerAngles; - Matrix3f Tbn_zeroYaw; + Vector3F eulerAngles; + Matrix3F Tbn_zeroYaw; if (order == rotationOrder::TAIT_BRYAN_321) { // rolled more than pitched so use 321 rotation order stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); @@ -1988,8 +1988,8 @@ void NavEKF3_core::setYawFromMag() return; } - Vector3f magMeasNED = Tbn_zeroYaw * magDataDelayed.mag; - float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); + Vector3F magMeasNED = Tbn_zeroYaw * magDataDelayed.mag; + ftype yawAngMeasured = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination()); // update quaternion states and covariances resetQuatStateYawOnly(yawAngMeasured, sq(MAX(frontend->_yawNoise, 1.0e-2f)), order); @@ -2028,7 +2028,7 @@ void NavEKF3_core::resetMagFieldStates() // zero the attitude covariances, but preserve the variances void NavEKF3_core::zeroAttCovOnly() { - float varTemp[4]; + ftype varTemp[4]; for (uint8_t index=0; index<=3; index++) { varTemp[index] = P[index][index]; } @@ -2042,52 +2042,52 @@ void NavEKF3_core::zeroAttCovOnly() // calculate the tilt error variance void NavEKF3_core::calcTiltErrorVariance() { - const float &q0 = stateStruct.quat[0]; - const float &q1 = stateStruct.quat[1]; - const float &q2 = stateStruct.quat[2]; - const float &q3 = stateStruct.quat[3]; + const ftype &q0 = stateStruct.quat[0]; + const ftype &q1 = stateStruct.quat[1]; + const ftype &q2 = stateStruct.quat[2]; + const ftype &q3 = stateStruct.quat[3]; // equations generated by quaternion_error_propagation(): in AP_NavEKF3/derivation/main.py // only diagonals have been used // dq0 ... dq3 terms have been zeroed - const float PS1 = q0*q1 + q2*q3; - const float PS2 = q1*PS1; - const float PS4 = sq(q0) - sq(q1) - sq(q2) + sq(q3); - const float PS5 = q0*PS4; - const float PS6 = 2*PS2 + PS5; - const float PS8 = PS1*q2; - const float PS10 = PS4*q3; - const float PS11 = PS10 + 2*PS8; - const float PS12 = PS1*q3; - const float PS13 = PS4*q2; - const float PS14 = -2*PS12 + PS13; - const float PS15 = PS1*q0; - const float PS16 = q1*PS4; - const float PS17 = 2*PS15 - PS16; - const float PS18 = q0*q2 - q1*q3; - const float PS19 = PS18*q2; - const float PS20 = 2*PS19 + PS5; - const float PS22 = q1*PS18; - const float PS23 = -PS10 + 2*PS22; - const float PS25 = PS18*q3; - const float PS26 = PS16 + 2*PS25; - const float PS28 = PS18*q0; - const float PS29 = -PS13 + 2*PS28; - const float PS32 = PS12 + PS28; - const float PS33 = PS19 + PS2; - const float PS34 = PS15 - PS25; - const float PS35 = PS22 - PS8; + const ftype PS1 = q0*q1 + q2*q3; + const ftype PS2 = q1*PS1; + const ftype PS4 = sq(q0) - sq(q1) - sq(q2) + sq(q3); + const ftype PS5 = q0*PS4; + const ftype PS6 = 2*PS2 + PS5; + const ftype PS8 = PS1*q2; + const ftype PS10 = PS4*q3; + const ftype PS11 = PS10 + 2*PS8; + const ftype PS12 = PS1*q3; + const ftype PS13 = PS4*q2; + const ftype PS14 = -2*PS12 + PS13; + const ftype PS15 = PS1*q0; + const ftype PS16 = q1*PS4; + const ftype PS17 = 2*PS15 - PS16; + const ftype PS18 = q0*q2 - q1*q3; + const ftype PS19 = PS18*q2; + const ftype PS20 = 2*PS19 + PS5; + const ftype PS22 = q1*PS18; + const ftype PS23 = -PS10 + 2*PS22; + const ftype PS25 = PS18*q3; + const ftype PS26 = PS16 + 2*PS25; + const ftype PS28 = PS18*q0; + const ftype PS29 = -PS13 + 2*PS28; + const ftype PS32 = PS12 + PS28; + const ftype PS33 = PS19 + PS2; + const ftype PS34 = PS15 - PS25; + const ftype PS35 = PS22 - PS8; tiltErrorVariance = 4*sq(PS11)*P[2][2] + 4*sq(PS14)*P[3][3] + 4*sq(PS17)*P[0][0] + 4*sq(PS6)*P[1][1]; tiltErrorVariance += 4*sq(PS20)*P[2][2] + 4*sq(PS23)*P[1][1] + 4*sq(PS26)*P[3][3] + 4*sq(PS29)*P[0][0]; tiltErrorVariance += 16*sq(PS32)*P[1][1] + 16*sq(PS33)*P[3][3] + 16*sq(PS34)*P[2][2] + 16*sq(PS35)*P[0][0]; - tiltErrorVariance = constrain_float(tiltErrorVariance, 0.0f, sq(radians(30.0f))); + tiltErrorVariance = constrain_ftype(tiltErrorVariance, 0.0f, sq(radians(30.0f))); } void NavEKF3_core::bestRotationOrder(rotationOrder &order) { - if (fabsf(prevTnb[2][0]) < fabsf(prevTnb[2][1])) { + if (fabsF(prevTnb[2][0]) < fabsF(prevTnb[2][1])) { // rolled more than pitched so use 321 sequence order = rotationOrder::TAIT_BRYAN_321; } else { @@ -2106,7 +2106,7 @@ void NavEKF3_core::verifyTiltErrorVariance() const float quat_delta = 0.001f; float tiltErrorVarianceAlt = 0.0f; for (uint8_t index = 0; index<4; index++) { - Quaternion quat = stateStruct.quat; + QuaternionF quat = stateStruct.quat; // Add a positive increment to the quaternion element and calculate the tilt error vector quat[index] = stateStruct.quat[index] + quat_delta; @@ -2136,8 +2136,8 @@ void NavEKF3_core::verifyTiltErrorVariance() LOG_PACKET_HEADER_INIT(LOG_XKTV_MSG), time_us : dal.micros64(), core : core_index, - tvs : tiltErrorVariance, - tvd : tiltErrorVarianceAlt, + tvs : float(tiltErrorVariance), + tvd : float(tiltErrorVarianceAlt), }; AP::logger().WriteBlock(&msg, sizeof(msg)); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 36607ceab5..704810558e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -81,14 +81,21 @@ #define GSF_YAW_VALID_HISTORY_THRESHOLD 5 // minimum variances allowed for velocity and position states -#define VEL_STATE_MIN_VARIANCE 1E-4f -#define POS_STATE_MIN_VARIANCE 1E-4f +#define VEL_STATE_MIN_VARIANCE 1E-4 +#define POS_STATE_MIN_VARIANCE 1E-4 // maximum number of times the vertical velocity variance can hit the lower limit before the // associated states, variances and covariances are reset -#define EKF_TARGET_RATE_HZ uint32_t(1.0f / EKF_TARGET_DT) +#define EKF_TARGET_RATE_HZ uint32_t(1.0 / EKF_TARGET_DT) #define VERT_VEL_VAR_CLIP_COUNT_LIM (5 * EKF_TARGET_RATE_HZ) +// limit on horizontal position states +#if HAL_WITH_EKF_DOUBLE +#define EK3_POSXY_STATE_LIMIT 50.0e6 +#else +#define EK3_POSXY_STATE_LIMIT 1.0e6 +#endif + class NavEKF3_core : public NavEKF_core_common { public: @@ -407,7 +414,6 @@ private: uint8_t imu_buffer_length; uint8_t obs_buffer_length; - typedef float ftype; #if MATH_CHECK_INDEXES typedef VectorN Vector2; typedef VectorN Vector3; @@ -461,14 +467,14 @@ private: // broken down as individual elements. Both are equivalent (same // memory) struct state_elements { - Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame 0..3 - Vector3f velocity; // velocity of IMU in local NED earth frame (m/sec) 4..6 - Vector3f position; // position of IMU in local NED earth frame (m) 7..9 - Vector3f gyro_bias; // body frame delta angle IMU bias vector (rad) 10..12 - Vector3f accel_bias; // body frame delta velocity IMU bias vector (m/sec) 13..15 - Vector3f earth_magfield; // earth frame magnetic field vector (Gauss) 16..18 - Vector3f body_magfield; // body frame magnetic field vector (Gauss) 19..21 - Vector2f wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23 + QuaternionF quat; // quaternion defining rotation from local NED earth frame to body frame 0..3 + Vector3F velocity; // velocity of IMU in local NED earth frame (m/sec) 4..6 + Vector3F position; // position of IMU in local NED earth frame (m) 7..9 + Vector3F gyro_bias; // body frame delta angle IMU bias vector (rad) 10..12 + Vector3F accel_bias; // body frame delta velocity IMU bias vector (m/sec) 13..15 + Vector3F earth_magfield; // earth frame magnetic field vector (Gauss) 16..18 + Vector3F body_magfield; // body frame magnetic field vector (Gauss) 19..21 + Vector2F wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23 }; union { @@ -477,73 +483,73 @@ private: }; struct output_elements { - Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame - Vector3f velocity; // velocity of body frame origin in local NED earth frame (m/sec) - Vector3f position; // position of body frame origin in local NED earth frame (m) + QuaternionF quat; // quaternion defining rotation from local NED earth frame to body frame + Vector3F velocity; // velocity of body frame origin in local NED earth frame (m/sec) + Vector3F position; // position of body frame origin in local NED earth frame (m) }; struct imu_elements { - Vector3f delAng; // IMU delta angle measurements in body frame (rad) - Vector3f delVel; // IMU delta velocity measurements in body frame (m/sec) - float delAngDT; // time interval over which delAng has been measured (sec) - float delVelDT; // time interval over which delVelDT has been measured (sec) + Vector3F delAng; // IMU delta angle measurements in body frame (rad) + Vector3F delVel; // IMU delta velocity measurements in body frame (m/sec) + ftype delAngDT; // time interval over which delAng has been measured (sec) + ftype delVelDT; // time interval over which delVelDT has been measured (sec) uint32_t time_ms; // measurement timestamp (msec) uint8_t gyro_index; uint8_t accel_index; }; struct gps_elements : EKF_obs_element_t { - Vector2f pos; // horizontal North East position of the GPS antenna in local NED earth frame (m) - float hgt; // height of the GPS antenna in local NED earth frame (m) - Vector3f vel; // velocity of the GPS antenna in local NED earth frame (m/sec) + Vector2F pos; // horizontal North East position of the GPS antenna in local NED earth frame (m) + ftype hgt; // height of the GPS antenna in local NED earth frame (m) + Vector3F vel; // velocity of the GPS antenna in local NED earth frame (m/sec) uint8_t sensor_idx; // unique integer identifying the GPS sensor bool corrected; // true when the position and velocity have been corrected for sensor position bool have_vz; // true when vertical velocity is valid }; struct mag_elements : EKF_obs_element_t { - Vector3f mag; // body frame magnetic field measurements (Gauss) + Vector3F mag; // body frame magnetic field measurements (Gauss) }; struct baro_elements : EKF_obs_element_t { - float hgt; // height of the pressure sensor in local NED earth frame (m) + ftype hgt; // height of the pressure sensor in local NED earth frame (m) }; struct range_elements : EKF_obs_element_t { - float rng; // distance measured by the range sensor (m) + ftype rng; // distance measured by the range sensor (m) uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors }; struct rng_bcn_elements : EKF_obs_element_t { - float rng; // range measurement to each beacon (m) - Vector3f beacon_posNED; // NED position of the beacon (m) - float rngErr; // range measurement error 1-std (m) + ftype rng; // range measurement to each beacon (m) + Vector3F beacon_posNED; // NED position of the beacon (m) + ftype rngErr; // range measurement error 1-std (m) uint8_t beacon_ID; // beacon identification number }; struct tas_elements : EKF_obs_element_t { - float tas; // true airspeed measurement (m/sec) + ftype tas; // true airspeed measurement (m/sec) }; struct of_elements : EKF_obs_element_t { - Vector2f flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec) - Vector2f flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec) - Vector3f bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec) - Vector3f body_offset; // XYZ position of the optical flow sensor in body frame (m) + Vector2F flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec) + Vector2F flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec) + Vector3F bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec) + Vector3F body_offset; // XYZ position of the optical flow sensor in body frame (m) }; struct vel_odm_elements : EKF_obs_element_t { - Vector3f vel; // XYZ velocity measured in body frame (m/s) - float velErr; // velocity measurement error 1-std (m/s) - Vector3f body_offset;// XYZ position of the velocity sensor in body frame (m) - Vector3f angRate; // angular rate estimated from odometry (rad/sec) + Vector3F vel; // XYZ velocity measured in body frame (m/s) + ftype velErr; // velocity measurement error 1-std (m/s) + Vector3F body_offset;// XYZ position of the velocity sensor in body frame (m) + Vector3F angRate; // angular rate estimated from odometry (rad/sec) }; struct wheel_odm_elements : EKF_obs_element_t { - float delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s) - float radius; // wheel radius (m) - Vector3f hub_offset; // XYZ position of the wheel hub in body frame (m) - float delTime; // time interval that the measurement was accumulated over (sec) + ftype delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s) + ftype radius; // wheel radius (m) + Vector3F hub_offset; // XYZ position of the wheel hub in body frame (m) + ftype delTime; // time interval that the measurement was accumulated over (sec) }; // Specifies the rotation order used for the Tait-Bryan or Euler angles where alternative rotation orders are available @@ -553,21 +559,21 @@ private: }; struct yaw_elements : EKF_obs_element_t { - float yawAng; // yaw angle measurement (rad) - float yawAngErr; // yaw angle 1SD measurement accuracy (rad) + ftype yawAng; // yaw angle measurement (rad) + ftype yawAngErr; // yaw angle 1SD measurement accuracy (rad) rotationOrder order; // type specifiying Euler rotation order used, 0 = 321 (ZYX), 1 = 312 (ZXY) }; struct ext_nav_elements : EKF_obs_element_t { - Vector3f pos; // XYZ position measured in a RH navigation frame (m) - float posErr; // spherical position measurement error 1-std (m) + Vector3F pos; // XYZ position measured in a RH navigation frame (m) + ftype posErr; // spherical position measurement error 1-std (m) bool posReset; // true when the position measurement has been reset bool corrected; // true when the position has been corrected for sensor position }; struct ext_nav_vel_elements : EKF_obs_element_t { - Vector3f vel; // velocity in NED (m/s) - float err; // velocity measurement error (m/s) + Vector3F vel; // velocity in NED (m/s) + ftype err; // velocity measurement error (m/s) bool corrected; // true when the velocity has been corrected for sensor position }; @@ -578,8 +584,8 @@ private: // bias estimates for the IMUs that are enabled but not being used // by this core. struct { - Vector3f gyro_bias; - Vector3f accel_bias; + Vector3F gyro_bias; + Vector3F accel_bias; } inactiveBias[INS_MAX_INSTANCES]; // Specify source of data to be used for a partial state reset @@ -614,7 +620,7 @@ private: // calculate the predicted state covariance matrix // Argument rotVarVecPtr is pointer to a vector defining the earth frame uncertainty variance of the quaternion states // used to perform a reset of the quaternion state covariances only. Set to null for normal operation. - void CovariancePrediction(Vector3f *rotVarVecPtr); + void CovariancePrediction(Vector3F *rotVarVecPtr); // force symmetry on the state covariance matrix void ForceSymmetry(); @@ -641,7 +647,7 @@ private: void FuseRngBcnStatic(); // calculate the offset from EKF vertical position datum to the range beacon system datum - void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning); + void CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning); // fuse magnetometer measurements void FuseMagnetometer(); @@ -665,21 +671,21 @@ private: void StoreQuatReset(void); // Rotate the stored output quaternion history through a quaternion rotation - void StoreQuatRotate(const Quaternion &deltaQuat); + void StoreQuatRotate(const QuaternionF &deltaQuat); // calculate the NED earth spin vector in rad/sec - void calcEarthRateNED(Vector3f &omega, int32_t latitude) const; + void calcEarthRateNED(Vector3F &omega, int32_t latitude) const; // initialise the covariance matrix void CovarianceInit(); // helper functions for readIMUData - bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt); - bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt); + bool readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt); + bool readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt); // helper functions for correcting IMU data - void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index); - void correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index); + void correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index); + void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index); // update IMU delta angle and delta velocity measurements void readIMUData(); @@ -744,10 +750,10 @@ private: void ResetPosition(resetDataSource posResetSource); // reset the stateStruct's NE position to the specified position - void ResetPositionNE(float posN, float posE); + void ResetPositionNE(ftype posN, ftype posE); // reset the stateStruct's D position - void ResetPositionD(float posD); + void ResetPositionD(ftype posD); // reset velocity states using the last GPS measurement void ResetVelocity(resetDataSource velResetSource); @@ -768,7 +774,7 @@ private: void checkDivergence(void); // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2 - void calcIMU_Weighting(float K1, float K2); + void calcIMU_Weighting(ftype K1, ftype K2); // return true if the filter is ready to start using optical flow measurements for position and velocity estimation bool readyToUseOptFlow(void) const; @@ -847,10 +853,10 @@ private: // Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations. // Input is 1-sigma uncertainty in published declination - void FuseDeclination(float declErr); + void FuseDeclination(ftype declErr); // return magnetic declination in radians - float MagDeclination(void) const; + ftype MagDeclination(void) const; // Propagate PVA solution forward from the fusion time horizon to the current time horizon // using a simple observer @@ -905,7 +911,7 @@ private: // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration // variances argument is updated with variances for each axis - void CalculateVelInnovationsAndVariances(const Vector3f &velocity, float noise, float accel_scale, Vector3f &innovations, Vector3f &variances) const; + void CalculateVelInnovationsAndVariances(const Vector3F &velocity, ftype noise, ftype accel_scale, Vector3F &innovations, Vector3F &variances) const; // Runs the IMU prediction step for an independent GSF yaw estimator algorithm // that uses IMU, GPS horizontal velocity and optionally true airspeed data. @@ -921,14 +927,14 @@ private: // yaw : new yaw angle (rad) // yaw_variance : variance of new yaw angle (rad^2) // order : enum defining Tait-Bryan rotation order used in calculation of the yaw angle - void resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order); + void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order); // attempt to reset the yaw to the EKF-GSF value // returns false if unsuccessful bool EKFGSF_resetMainFilterYaw(); // returns true on success and populates yaw (in radians) and yawVariance (rad^2) - bool EKFGSF_getYaw(float &yaw, float &yawVariance) const; + bool EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const; // Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation void FuseDragForces(); @@ -948,7 +954,7 @@ private: bool badIMUdata; // boolean true if the bad IMU data is detected uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit - float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts + ftype gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts Matrix24 P; // covariance matrix EKF_IMU_buffer_t storedIMU; // IMU data buffer EKF_obs_buffer_t storedGPS; // GPS data buffer @@ -957,10 +963,10 @@ private: EKF_obs_buffer_t storedTAS; // TAS data buffer EKF_obs_buffer_t storedRange; // Range finder data buffer EKF_IMU_buffer_t storedOutput;// output state buffer - Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation + Matrix3F prevTnb; // previous nav to body transformation used for INS earth rotation compensation ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2) ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2) - Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) + Vector3F earthRateNED; // earths angular rate vector in NED (rad/s) ftype dtIMUavg; // expected time between IMU measurements (sec) ftype dtEkfAvg; // expected time between EKF updates (sec) ftype dt; // time lapsed since the last covariance prediction (sec) @@ -976,12 +982,12 @@ private: bool fuseVelData; // this boolean causes the velNED measurements to be fused bool fusePosData; // this boolean causes the posNE measurements to be fused bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused - Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements - Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements + Vector3F innovMag; // innovation output from fusion of X,Y,Z compass measurements + Vector3F varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements ftype innovVtas; // innovation output from fusion of airspeed measurements ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements - float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive. - float defaultAirSpeedVariance; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified. + ftype defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive. + ftype defaultAirSpeedVariance; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified. bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step MagCal effectiveMagCal; // the actual mag calibration being used as the default uint32_t prevTasStep_ms; // time stamp of last TAS fusion step @@ -989,8 +995,8 @@ private: ftype innovBeta; // synthetic sideslip innovation (rad) uint32_t lastMagUpdate_us; // last time compass was updated in usec uint32_t lastMagRead_ms; // last time compass data was successfully read - Vector3f velDotNED; // rate of change of velocity in NED frame - Vector3f velDotNEDfilt; // low pass filtered velDotNED + Vector3F velDotNED; // rate of change of velocity in NED frame + Vector3F velDotNEDfilt; // low pass filtered velDotNED uint32_t imuSampleTime_ms; // time that the last IMU value was taken bool tasDataToFuse; // true when new airspeed data is waiting to be fused uint32_t lastBaroReceived_ms; // time last time we received baro height data @@ -1005,13 +1011,13 @@ private: bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec) uint32_t ekfStartTime_ms; // time the EKF was started (msec) - Vector2f lastKnownPositionNE; // last known position + Vector2F lastKnownPositionNE; // last known position uint32_t lastLaunchAccelTime_ms; - float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold - float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold - float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold - Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold - float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold + ftype velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold + ftype posTestRatio; // sum of squares of GPS position innovation divided by fail threshold + ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold + Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold + ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold bool inhibitWindStates; // true when wind states and covariances are to remain constant bool inhibitMagStates; // true when magnetic field states are inactive bool lastInhibitMagStates; // previous inhibitMagStates @@ -1021,15 +1027,15 @@ private: bool gpsNotAvailable; // bool true when valid GPS data is not available struct Location EKF_origin; // LLH origin of the NED axis system bool validOrigin; // true when the EKF origin is valid - float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver - float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver - float gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver + ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver + ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver + ftype gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail uint32_t lastGpsVelPass_ms; // time of last GPS vertical velocity consistency check pass uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad - float posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m) + ftype posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m) bool useGpsVertVel; // true if GPS vertical velocity should be used - float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased. + ftype yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased. uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle bool tiltAlignComplete; // true when tilt alignment is complete bool yawAlignComplete; // true when yaw alignment is complete @@ -1038,14 +1044,14 @@ private: imu_elements imuDataDelayed; // IMU data at the fusion time horizon imu_elements imuDataNew; // IMU data at the current time horizon imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate - Quaternion imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame + QuaternionF imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame baro_elements baroDataNew; // Baro data at the current time horizon baro_elements baroDataDelayed; // Baro data at the fusion time horizon range_elements rangeDataNew; // Range finder data at the current time horizon range_elements rangeDataDelayed;// Range finder data at the fusion time horizon tas_elements tasDataNew; // TAS data at the current time horizon tas_elements tasDataDelayed; // TAS data at the fusion time horizon - float tasErrVar; // TAS error variance (m/s)**2 + ftype tasErrVar; // TAS error variance (m/s)**2 bool usingDefaultAirspeed; // true when a default airspeed is being used instead of a measured value mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon gps_elements gpsDataNew; // GPS data at the current time horizon @@ -1053,10 +1059,10 @@ private: uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset output_elements outputDataNew; // output state data at the current time step output_elements outputDataDelayed; // output state data at the current time step - Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF - Vector3f velErrintegral; // integral of output predictor NED velocity tracking error (m) - Vector3f posErrintegral; // integral of output predictor NED position tracking error (m.sec) - float innovYaw; // compass yaw angle innovation (rad) + Vector3F delAngCorrection; // correction applied to delta angles used by output observer to track the EKF + Vector3F velErrintegral; // integral of output predictor NED velocity tracking error (m) + Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec) + ftype innovYaw; // compass yaw angle innovation (rad) uint32_t timeTasReceived_ms; // time last TAS data was received (msec) bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks @@ -1068,94 +1074,94 @@ private: bool airSpdFusionDelayed; // true when the air speed fusion has been delayed bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed bool airDataFusionWindOnly; // true when sideslip and airspeed fusion is only allowed to modify the wind states - Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes. + Vector3F lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes. bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized - Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset + Vector2F posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset uint32_t lastPosReset_ms; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset - Vector2f velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset + Vector2F velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset - float posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset + ftype posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset uint32_t lastPosResetD_ms; // System time at which the last position reset occurred. Returned by getLastPosDownReset - float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold - Quaternion prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed - float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks + ftype yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold + QuaternionF prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed + ftype hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF bool runUpdates; // boolean true when the EKF updates can be run uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycle uint8_t localFilterTimeStep_ms; // average number of msec between filter updates - float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2) - Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad) - Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s) + ftype posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2) + Vector3F delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad) + Vector3F delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s) bool magFieldLearned; // true when the magnetic field has been learned uint32_t wasLearningCompass_ms; // time when we were last waiting for compass learn to complete - Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2) - Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2) + Vector3F earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2) + Vector3F bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2) bool delAngBiasLearned; // true when the gyro bias has been learned nav_filter_status filterStatus; // contains the status of various filter outputs - float ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2) + ftype ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2) double ekfGpsRefHgt; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m) uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected - Vector3f outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer - Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s) - Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m) + Vector3F outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer + Vector3F velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s) + Vector3F posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m) uint32_t firstInitTime_ms; // First time the initialise function was called (msec) uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec) - float tiltErrorVariance; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2) + ftype tiltErrorVariance; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2) // variables used to calculate a vertical velocity that is kinematically consistent with the vertical position struct { - float pos; - float vel; - float acc; + ftype pos; + ftype vel; + ftype acc; } vertCompFiltState; // variables used by the pre-initialisation GPS checks struct Location gpsloc_prev; // LLH location of previous GPS measurement uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks - float gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks - float gpsVertVelFilt; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks - float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks + ftype gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks + ftype gpsVertVelFilt; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks + ftype gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks // variable used by the in-flight GPS quality check bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks - float sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy - float sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed + ftype sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy + ftype sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight. - Vector3f gpsVelInnov; // gps velocity innovations - Vector3f gpsVelVarInnov; // gps velocity innovation variances + Vector3F gpsVelInnov; // gps velocity innovations + Vector3F gpsVelVarInnov; // gps velocity innovation variances uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts) // variables added for optical flow fusion EKF_obs_buffer_t storedOF; // OF data buffer of_elements ofDataNew; // OF data at the current time horizon bool flowDataValid; // true while optical flow data is still fresh - Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator + Vector2F auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec) uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec) uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec) - Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period + Matrix3F Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2 Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec) - float Popt; // Optical flow terrain height state covariance (m^2) - float terrainState; // terrain position state (m) - float prevPosN; // north position at last measurement - float prevPosE; // east position at last measurement - float varInnovRng; // range finder observation innovation variance (m^2) - float innovRng; // range finder observation innovation (m) - float hgtMea; // height measurement derived from either baro, gps or range finder data (m) + ftype Popt; // Optical flow terrain height state covariance (m^2) + ftype terrainState; // terrain position state (m) + ftype prevPosN; // north position at last measurement + ftype prevPosE; // east position at last measurement + ftype varInnovRng; // range finder observation innovation variance (m^2) + ftype innovRng; // range finder observation innovation (m) + ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m) bool inhibitGndState; // true when the terrain position state is to remain constant uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail - Vector2f auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator - float R_LOS; // variance of optical flow rate measurements (rad/sec)^2 - float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail - Vector2f flowGyroBias; // bias error of optical flow sensor gyro output + Vector2F auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator + ftype R_LOS; // variance of optical flow rate measurements (rad/sec)^2 + ftype auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail + Vector2F flowGyroBias; // bias error of optical flow sensor gyro output bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon. bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon. bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon. @@ -1167,16 +1173,16 @@ private: AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions bool gndOffsetValid; // true when the ground offset state can still be considered valid - Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement - float delTimeOF; // time that delAngBodyOF is summed across + Vector3F delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement + ftype delTimeOF; // time that delAngBodyOF is summed across bool flowFusionActive; // true when optical flow fusion is active - Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m) + Vector3F accelPosOffset; // position of IMU accelerometer unit in body frame (m) // Range finder - float baroHgtOffset; // offset applied when when switching to use of Baro height - float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground - float storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors + ftype baroHgtOffset; // offset applied when when switching to use of Baro height + ftype rngOnGnd; // Expected range finder reading in metres when vehicle is on ground + ftype storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors uint32_t storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors @@ -1214,49 +1220,49 @@ private: EKF_obs_buffer_t storedRangeBeacon; // Beacon range buffer rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec) - float rngBcnTestRatio; // Innovation test ratio for range beacon measurements + ftype rngBcnTestRatio; // Innovation test ratio for range beacon measurements bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check - float varInnovRngBcn; // range beacon observation innovation variance (m^2) - float innovRngBcn; // range beacon observation innovation (m) + ftype varInnovRngBcn; // range beacon observation innovation variance (m^2) + ftype innovRngBcn; // range beacon observation innovation (m) uint32_t lastTimeRngBcn_ms[4]; // last time we received a range beacon measurement (msec) bool rngBcnDataToFuse; // true when there is new range beacon data to fuse - Vector3f beaconVehiclePosNED; // NED position estimate from the beacon system (NED) - float beaconVehiclePosErr; // estimated position error from the beacon system (m) + Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED) + ftype beaconVehiclePosErr; // estimated position error from the beacon system (m) uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec) bool rngBcnGoodToAlign; // true when the range beacon systems 3D fix can be used to align the filter uint8_t lastRngBcnChecked; // index of the last range beacon checked for data - Vector3f receiverPos; // receiver NED position (m) - alignment 3 state filter - float receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter ( + Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter + ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter ( bool rngBcnAlignmentStarted; // True when the initial position alignment using range measurements has started bool rngBcnAlignmentCompleted; // True when the initial position alignment using range measurements has finished uint8_t lastBeaconIndex; // Range beacon index last read - used during initialisation of the 3-state filter - Vector3f rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter + Vector3F rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter uint8_t numBcnMeas; // Number of beacon measurements - used during initialisation of the 3-state filter - float rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter + ftype rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter uint8_t N_beacons; // Number of range beacons in use - float maxBcnPosD; // maximum position of all beacons in the down direction (m) - float minBcnPosD; // minimum position of all beacons in the down direction (m) + ftype maxBcnPosD; // maximum position of all beacons in the down direction (m) + ftype minBcnPosD; // minimum position of all beacons in the down direction (m) bool usingMinHypothesis; // true when the min beacon constellation offset hypothesis is being used - float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) - float bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m) - float maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh + ftype bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) + ftype bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m) + ftype maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh - float bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) - float bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m) - float minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow + ftype bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) + ftype bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m) + ftype minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow - Vector3f bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m) + Vector3F bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m) bool bcnOriginEstInit; // True when the beacon origin has been initialised // Range Beacon Fusion Debug Reporting uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported struct rngBcnFusionReport_t { - float rng; // measured range to beacon (m) - float innov; // range innovation (m) - float innovVar; // innovation variance (m^2) - float testRatio; // innovation consistency test ratio - Vector3f beaconPosNED; // beacon NED position + ftype rng; // measured range to beacon (m) + ftype innov; // range innovation (m) + ftype innovVar; // innovation variance (m^2) + ftype testRatio; // innovation consistency test ratio + Vector3F beaconPosNED; // beacon NED position } *rngBcnFusionReport; #if EK3_FEATURE_DRAG_FUSION @@ -1265,10 +1271,10 @@ private: drag_elements dragSampleDelayed; drag_elements dragDownSampled; // down sampled from filter prediction rate to observation rate uint8_t dragSampleCount; // number of drag specific force samples accumulated at the filter prediction rate - float dragSampleTimeDelta; // time integral across all samples used to form _drag_down_sampled (sec) - Vector2f innovDrag; // multirotor drag measurement innovation (m/sec**2) - Vector2f innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2) - Vector2f dragTestRatio; // drag innovation consistency check ratio + ftype dragSampleTimeDelta; // time integral across all samples used to form _drag_down_sampled (sec) + Vector2F innovDrag; // multirotor drag measurement innovation (m/sec**2) + Vector2F innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2) + Vector2F dragTestRatio; // drag innovation consistency check ratio #endif bool dragFusionEnabled; @@ -1278,11 +1284,11 @@ private: // Movement detector bool takeOffDetected; // true when takeoff for optical flow navigation has been detected - float rngAtStartOfFlight; // range finder measurement at start of flight + ftype rngAtStartOfFlight; // range finder measurement at start of flight uint32_t timeAtArming_ms; // time in msec that the vehicle armed // baro ground effect - float meaHgtAtTakeOff; // height measured at commencement of takeoff + ftype meaHgtAtTakeOff; // height measured at commencement of takeoff // control of post takeoff magnetic field and heading resets bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed @@ -1291,22 +1297,22 @@ private: bool magStateResetRequest; // true if magnetic field states need to be reset using the magnetomter measurements bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course - float posDownAtLastMagReset; // vertical position last time the mag states were reset (m) - float yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad) - Quaternion quatAtLastMagReset; // quaternion states last time the mag states were reset + ftype posDownAtLastMagReset; // vertical position last time the mag states were reset (m) + ftype yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad) + QuaternionF quatAtLastMagReset; // quaternion states last time the mag states were reset // Used by on ground movement check required when operating on ground without a yaw reference - float gyro_diff; // filtered gyro difference (rad/s) - float accel_diff; // filtered acceerometer difference (m/s/s) - Vector3f gyro_prev; // gyro vector from previous time step (rad/s) - Vector3f accel_prev; // accelerometer vector from previous time step (m/s/s) + ftype gyro_diff; // filtered gyro difference (rad/s) + ftype accel_diff; // filtered acceerometer difference (m/s/s) + Vector3F gyro_prev; // gyro vector from previous time step (rad/s) + Vector3F accel_prev; // accelerometer vector from previous time step (m/s/s) bool onGroundNotMoving; // true when on the ground and not moving uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec) // variables used to inhibit accel bias learning bool inhibitDelVelBiasStates; // true when all IMU delta velocity bias states are de-activated bool dvelBiasAxisInhibit[3] {}; // true when IMU delta velocity bias states for a specific axis is de-activated - Vector3f dvelBiasAxisVarPrev; // saved delta velocity XYZ bias variances (m/sec)**2 + Vector3F dvelBiasAxisVarPrev; // saved delta velocity XYZ bias variances (m/sec)**2 #if EK3_FEATURE_EXTERNAL_NAV // external navigation fusion @@ -1320,8 +1326,8 @@ private: ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec) bool extNavVelToFuse; // true when there is new external navigation velocity to fuse - Vector3f extNavVelInnov; // external nav velocity innovations - Vector3f extNavVelVarInnov; // external nav velocity innovation variances + Vector3F extNavVelInnov; // external nav velocity innovations + Vector3F extNavVelVarInnov; // external nav velocity innovation variances uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts) EKF_obs_buffer_t storedExtNavYawAng; // external navigation yaw angle buffer yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon @@ -1384,8 +1390,8 @@ private: ftype magXbias; ftype magYbias; ftype magZbias; - Matrix3f DCM; - Vector3f MagPred; + Matrix3F DCM; + Vector3F MagPred; ftype R_MAG; Vector9 SH_MAG; } mag_state; @@ -1395,8 +1401,8 @@ private: // earth field from WMM tables bool have_table_earth_field; // true when we have initialised table_earth_field_ga - Vector3f table_earth_field_ga; // earth field from WMM tables - float table_declination; // declination in radians from the tables + Vector3F table_earth_field_ga; // earth field from WMM tables + ftype table_declination; // declination in radians from the tables // timing statistics struct ekf_timing timing; @@ -1408,7 +1414,7 @@ private: bool assume_zero_sideslip(void) const; // vehicle specific initial gyro bias uncertainty - float InitialGyroBiasUncertainty(void) const; + ftype InitialGyroBiasUncertainty(void) const; /* learn magnetometer biases from GPS yaw. Return true if the