diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 36904d55c9..20c8f9c4ad 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -21,17 +21,17 @@ extern const AP_HAL::HAL& hal; void NavEKF2_core::FuseAirspeed() { // declarations - float vn; - float ve; - float vd; - float vwn; - float vwe; - float EAS2TAS = dal.get_EAS2TAS(); - const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f)); + ftype vn; + ftype ve; + ftype vd; + ftype vwn; + ftype vwe; + ftype EAS2TAS = dal.get_EAS2TAS(); + const ftype R_TAS = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(EAS2TAS, 0.9f, 10.0f)); Vector3 SH_TAS; - float SK_TAS; + ftype SK_TAS; Vector24 H_TAS; - float VtasPred; + ftype VtasPred; // copy required states to local variable names vn = stateStruct.velocity.x; @@ -46,7 +46,7 @@ void NavEKF2_core::FuseAirspeed() if (VtasPred > 1.0f) { // calculate observation jacobians - SH_TAS[0] = 1.0f/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[0] = 1.0f/(sqrtF(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; for (uint8_t i=0; i<=2; i++) H_TAS[i] = 0.0f; @@ -57,7 +57,7 @@ void NavEKF2_core::FuseAirspeed() H_TAS[23] = -SH_TAS[1]; for (uint8_t i=6; i<=21; i++) H_TAS[i] = 0.0f; // calculate Kalman gains - float temp = (R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0])); + ftype temp = (R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0])); if (temp >= R_TAS) { SK_TAS = 1.0f / temp; faultStatus.bad_airspeed = false; @@ -107,7 +107,7 @@ void NavEKF2_core::FuseAirspeed() innovVtas = VtasPred - tasDataDelayed.tas; // 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); @@ -238,21 +238,21 @@ void NavEKF2_core::SelectBetaFusion() void NavEKF2_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 Vector10 SH_BETA; Vector5 SK_BETA; - Vector3f vel_rel_wind; + Vector3F vel_rel_wind; Vector24 H_BETA; - float innovBeta; + ftype innovBeta; // copy required states to local variable names q0 = stateStruct.quat[0]; @@ -278,7 +278,7 @@ void NavEKF2_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 { @@ -307,7 +307,7 @@ void NavEKF2_core::FuseSideslip() H_BETA[23] = SH_BETA[1]*SH_BETA[3]*SH_BETA[8] - SH_BETA[4]; // Calculate Kalman gains - float temp = (R_BETA + (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][4]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[6] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][23]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[6] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][3]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[6] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][22]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[6] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (sq(SH_BETA[1])*SH_BETA[3] + 1)*(P[22][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][2]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[6] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3))*(P[22][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][5]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[6] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[2]*SH_BETA[6]*(P[22][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][0]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[6] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[3]*(P[22][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][1]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[6] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3])); + ftype temp = (R_BETA + (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][4]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[6] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][23]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[6] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][3]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[6] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][22]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[6] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (sq(SH_BETA[1])*SH_BETA[3] + 1)*(P[22][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][2]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[6] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3))*(P[22][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][5]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[6] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[2]*SH_BETA[6]*(P[22][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][0]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[6] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[3]*(P[22][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][1]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[6] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3])); if (temp >= R_BETA) { SK_BETA[0] = 1.0f / temp; faultStatus.bad_sideslip = false; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 4ce13dde76..c575b8b5c0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -61,15 +61,15 @@ void NavEKF2_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 sate 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 @@ -325,7 +325,7 @@ void NavEKF2_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(); } // reset the last fusion accepted times to prevent unwanted activation of timeout logic @@ -348,8 +348,8 @@ void NavEKF2_core::setAidingMode() void NavEKF2_core::checkAttitudeAlignmentStatus() { // Check for tilt convergence - used during initial alignment - float alpha = 1.0f*imuDataDelayed.delAngDT; - float temp=tiltErrVec.length(); + ftype alpha = 1.0f*imuDataDelayed.delAngDT; + ftype temp=tiltErrVec.length(); tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt; if (tiltErrFilt < 0.005f && !tiltAlignComplete) { tiltAlignComplete = true; @@ -470,14 +470,14 @@ void NavEKF2_core::recordYawReset() bool NavEKF2_core::checkGyroCalStatus(void) { // check delta angle bias variances - const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); + const ftype delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); if (!use_compass()) { // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass // which can make this check fail - Vector3f delAngBiasVarVec = Vector3f(P[9][9],P[10][10],P[11][11]); - Vector3f temp = prevTnb * delAngBiasVarVec; - delAngBiasLearned = (fabsf(temp.x) < delAngBiasVarMax) && - (fabsf(temp.y) < delAngBiasVarMax); + Vector3F delAngBiasVarVec = Vector3F(P[9][9],P[10][10],P[11][11]); + Vector3F temp = prevTnb * delAngBiasVarVec; + delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) && + (fabsF(temp.y) < delAngBiasVarMax); } else { delAngBiasLearned = (P[9][9] <= delAngBiasVarMax) && (P[10][10] <= delAngBiasVarMax) && @@ -525,7 +525,7 @@ void NavEKF2_core::updateFilterStatus(void) void NavEKF2_core::runYawEstimatorPrediction() { if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) { - float trueAirspeed; + ftype trueAirspeed; if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) { if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) { trueAirspeed = tasDataDelayed.tas; @@ -544,8 +544,8 @@ void NavEKF2_core::runYawEstimatorCorrection() { if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1 && 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); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp index dbaee98a5e..684d22ca4c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp @@ -129,7 +129,7 @@ void NavEKF2_core::Log_Write_NKF4(uint64_t time_us) const nav_filter_status solutionStatus {}; nav_gps_status gpsStatus {}; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); - float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); + ftype tempVar = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z); getFilterFaults(_faultStatus); getFilterStatus(solutionStatus); getFilterGpsStatus(gpsStatus); @@ -142,7 +142,7 @@ void NavEKF2_core::Log_Write_NKF4(uint64_t time_us) const sqrtvarH : (int16_t)(100*hgtVar), sqrtvarM : (int16_t)(100*tempVar), sqrtvarVT : (int16_t)(100*tasVar), - tiltErr : tiltErrFilt, // tilt error convergence metric + tiltErr : float(tiltErrFilt), // tilt error convergence metric offsetNorth : offset.x, offsetEast : offset.y, faults : _faultStatus, @@ -174,10 +174,10 @@ void NavEKF2_core::Log_Write_NKF5(uint64_t time_us) const offset : (int16_t)(100*terrainState), // // estimated vertical position of the terrain relative to the nav filter zero datum RI : (int16_t)(100*innovRng), // range finder innovations meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range - errHAGL : (uint16_t)(100*sqrtf(Popt)), // filter ground offset state error - angErr : outputTrackError.x, - velErr : outputTrackError.y, - posErr : outputTrackError.z + errHAGL : (uint16_t)(100*sqrtF(Popt)), // filter ground offset state error + angErr : float(outputTrackError.x), + velErr : float(outputTrackError.y), + posErr : float(outputTrackError.z) }; AP::logger().WriteBlock(&pkt5, sizeof(pkt5)); } @@ -235,7 +235,7 @@ void NavEKF2_core::Log_Write_Beacon(uint64_t time_us) rng : (int16_t)(100*report.rng), innov : (int16_t)(100*report.innov), sqrtInnovVar : (uint16_t)(100*safe_sqrt(report.innovVar)), - testRatio : (uint16_t)(100*constrain_float(report.testRatio,0.0f,650.0f)), + 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_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index f27d34626c..2552a20230 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -24,9 +24,9 @@ void NavEKF2_core::controlMagYawReset() gpsYawResetRequest = false; } - // Quaternion and delta rotation vector that are re-used for different calculations - Vector3f deltaRotVecTemp; - Quaternion deltaQuatTemp; + // QuaternionF and delta rotation vector that are re-used for different calculations + Vector3F deltaRotVecTemp; + QuaternionF deltaQuatTemp; bool flightResetAllowed = false; bool initialResetAllowed = false; @@ -61,7 +61,7 @@ void NavEKF2_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; @@ -96,14 +96,14 @@ void NavEKF2_core::controlMagYawReset() // if a yaw reset has been requested, apply the updated quaternion to the current state if (extNavYawResetRequest) { // get the euler angles from the current state estimate - Vector3f eulerAnglesOld; + Vector3F eulerAnglesOld; stateStruct.quat.to_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesOld.z); // previous value used to calculate a reset delta - Quaternion prevQuat = stateStruct.quat; + QuaternionF prevQuat = stateStruct.quat; // Get the Euler angles from the external vision data - Vector3f eulerAnglesNew; + Vector3F eulerAnglesNew; extNavDataDelayed.quat.to_euler(eulerAnglesNew.x, eulerAnglesNew.y, eulerAnglesNew.z); // the new quaternion uses the old roll/pitch and new yaw angle @@ -128,16 +128,16 @@ void NavEKF2_core::controlMagYawReset() } else if (magYawResetRequest || magStateResetRequest) { // get the euler angles from the current state estimate - Vector3f eulerAngles; + Vector3F eulerAngles; stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); // Use the Euler angles and magnetometer measurement to update the magnetic field states // and get an updated quaternion - Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); + QuaternionF newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); if (magYawResetRequest) { // previous value used to calculate a reset delta - Quaternion prevQuat = stateStruct.quat; + QuaternionF prevQuat = stateStruct.quat; // update the quaternion states using the new yaw angle stateStruct.quat = newQuat; @@ -181,17 +181,17 @@ void NavEKF2_core::realignYawGPS() { if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) { // 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); // 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; @@ -332,8 +332,8 @@ void NavEKF2_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; @@ -374,7 +374,7 @@ void NavEKF2_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*delAngCorrected.length() / imuDataDelayed.delAngDT); + R_MAG = sq(constrain_ftype(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*delAngCorrected.length() / imuDataDelayed.delAngDT); // calculate common expressions used to calculate observation jacobians an innovation variance for each component SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); @@ -426,7 +426,7 @@ void NavEKF2_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 @@ -448,7 +448,7 @@ void NavEKF2_core::FuseMagnetometer() if (obsIndex == 0) { // calculate observation jacobians - memset(&H_MAG, 0, sizeof(H_MAG)); + ZERO_FARRAY(H_MAG); H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5]; H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2); H_MAG[16] = SH_MAG[1]; @@ -508,7 +508,7 @@ void NavEKF2_core::FuseMagnetometer() else if (obsIndex == 1) // we are now fusing the Y measurement { // calculate observation jacobians - memset(&H_MAG, 0, sizeof(H_MAG)); + ZERO_FARRAY(H_MAG); H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1]; H_MAG[16] = 2.0f*q1*q2 - SH_MAG[8]; @@ -567,7 +567,7 @@ void NavEKF2_core::FuseMagnetometer() else if (obsIndex == 2) // we are now fusing the Z measurement { // calculate observation jacobians - memset(&H_MAG, 0, sizeof(H_MAG)); + ZERO_FARRAY(H_MAG); H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0]; H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; H_MAG[16] = SH_MAG[5]; @@ -720,60 +720,60 @@ void NavEKF2_core::FuseMagnetometer() */ void NavEKF2_core::fuseEulerYaw() { - float q0 = stateStruct.quat[0]; - float q1 = stateStruct.quat[1]; - float q2 = stateStruct.quat[2]; - float q3 = stateStruct.quat[3]; + ftype q0 = stateStruct.quat[0]; + ftype q1 = stateStruct.quat[1]; + ftype q2 = stateStruct.quat[2]; + ftype q3 = stateStruct.quat[3]; // compass measurement error variance (rad^2) set to parameter value as a default - float R_YAW = sq(frontend->_yawNoise); + ftype R_YAW = sq(frontend->_yawNoise); // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix // determine if a 321 or 312 Euler sequence is best - float predicted_yaw; - float measured_yaw; - float H_YAW[3]; - Matrix3f Tbn_zeroYaw; + ftype predicted_yaw; + ftype measured_yaw; + ftype H_YAW[3]; + Matrix3F Tbn_zeroYaw; - if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { + if (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) { // calculate observation jacobian when we are observing the first rotation in a 321 sequence - float t2 = q0*q0; - float t3 = q1*q1; - float t4 = q2*q2; - float t5 = q3*q3; - float t6 = t2+t3-t4-t5; - float t7 = q0*q3*2.0f; - float t8 = q1*q2*2.0f; - float t9 = t7+t8; - float t10 = sq(t6); + ftype t2 = q0*q0; + ftype t3 = q1*q1; + ftype t4 = q2*q2; + ftype t5 = q3*q3; + ftype t6 = t2+t3-t4-t5; + ftype t7 = q0*q3*2.0f; + ftype t8 = q1*q2*2.0f; + ftype t9 = t7+t8; + ftype t10 = sq(t6); if (t10 > 1e-6f) { t10 = 1.0f / t10; } else { return; } - float t11 = t9*t9; - float t12 = t10*t11; - float t13 = t12+1.0f; - float t14; - if (fabsf(t13) > 1e-3f) { + ftype t11 = t9*t9; + ftype t12 = t10*t11; + ftype t13 = t12+1.0f; + ftype t14; + if (fabsF(t13) > 1e-3f) { t14 = 1.0f/t13; } else { return; } - float t15 = 1.0f/t6; + ftype t15 = 1.0f/t6; H_YAW[0] = 0.0f; H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f)); H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8)); // calculate predicted and measured yaw angle - Vector3f euler321; + Vector3F euler321; stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z); predicted_yaw = euler321.z; if (use_compass() && yawAlignComplete && magStateInitComplete) { // Use measured mag components rotated into earth frame to measure yaw Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); - Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); + Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + measured_yaw = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination()); } else if (extNavUsedForYaw) { // Get the yaw angle from the external vision data R_YAW = sq(extNavDataDelayed.angErr); @@ -781,7 +781,7 @@ void NavEKF2_core::fuseEulerYaw() measured_yaw = euler321.z; } else { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) { - float gsfYaw, gsfYawVariance, velInnovLength; + ftype gsfYaw, gsfYawVariance, velInnovLength; if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) && is_positive(gsfYawVariance) && gsfYawVariance < sq(radians(15.0f)) && @@ -799,42 +799,42 @@ void NavEKF2_core::fuseEulerYaw() } } else { // calculate observation jacobian when we are observing a rotation in a 312 sequence - float t2 = q0*q0; - float t3 = q1*q1; - float t4 = q2*q2; - float t5 = q3*q3; - float t6 = t2-t3+t4-t5; - float t7 = q0*q3*2.0f; - float t10 = q1*q2*2.0f; - float t8 = t7-t10; - float t9 = sq(t6); + ftype t2 = q0*q0; + ftype t3 = q1*q1; + ftype t4 = q2*q2; + ftype t5 = q3*q3; + ftype t6 = t2-t3+t4-t5; + ftype t7 = q0*q3*2.0f; + ftype t10 = q1*q2*2.0f; + ftype t8 = t7-t10; + ftype t9 = sq(t6); if (t9 > 1e-6f) { t9 = 1.0f/t9; } else { return; } - float t11 = t8*t8; - float t12 = t9*t11; - float t13 = t12+1.0f; - float t14; - if (fabsf(t13) > 1e-3f) { + ftype t11 = t8*t8; + ftype t12 = t9*t11; + ftype t13 = t12+1.0f; + ftype t14; + if (fabsF(t13) > 1e-3f) { t14 = 1.0f/t13; } else { return; } - float t15 = 1.0f/t6; + ftype t15 = 1.0f/t6; H_YAW[0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0)); H_YAW[1] = 0.0f; H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10)); // calculate predicted and measured yaw angle - Vector3f euler312 = stateStruct.quat.to_vector312(); + Vector3F euler312 = stateStruct.quat.to_vector312(); predicted_yaw = euler312.z; if (use_compass() && yawAlignComplete && magStateInitComplete) { // Use measured mag components rotated into earth frame to measure yaw Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f); - Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); + Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + measured_yaw = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination()); } else if (extNavUsedForYaw) { // Get the yaw angle from the external vision data R_YAW = sq(extNavDataDelayed.angErr); @@ -842,7 +842,7 @@ void NavEKF2_core::fuseEulerYaw() measured_yaw = euler312.z; } else { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) { - float gsfYaw, gsfYawVariance, velInnovLength; + ftype gsfYaw, gsfYawVariance, velInnovLength; if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) && is_positive(gsfYawVariance) && gsfYawVariance < sq(radians(15.0f)) && @@ -861,14 +861,14 @@ void NavEKF2_core::fuseEulerYaw() } // Calculate the innovation - float innovation = wrap_PI(predicted_yaw - measured_yaw); + ftype innovation = wrap_PI(predicted_yaw - measured_yaw); // Copy raw value to output variable used for data logging innovYaw = innovation; // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero - float PH[3]; - float varInnov = R_YAW; + ftype PH[3]; + ftype varInnov = R_YAW; for (uint8_t rowIndex=0; rowIndex<=2; rowIndex++) { PH[rowIndex] = 0.0f; for (uint8_t colIndex=0; colIndex<=2; colIndex++) { @@ -876,7 +876,7 @@ void NavEKF2_core::fuseEulerYaw() } varInnov += H_YAW[rowIndex]*PH[rowIndex]; } - float varInnovInv; + ftype varInnovInv; if (varInnov >= R_YAW) { varInnovInv = 1.0f / varInnov; // output numerical health status @@ -900,7 +900,7 @@ void NavEKF2_core::fuseEulerYaw() } // calculate the innovation test ratio - yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov); + yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (ftype)frontend->_yawInnovGate, 1.0f)) * varInnov); // Declare the magnetometer unhealthy if the innovation test fails if (yawTestRatio > 1.0f) { @@ -930,7 +930,7 @@ void NavEKF2_core::fuseEulerYaw() } 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]; KHP[row][column] = tmp; @@ -986,14 +986,14 @@ void NavEKF2_core::fuseEulerYaw() * 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 NavEKF2_core::FuseDeclination(float declErr) +void NavEKF2_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) { @@ -1001,27 +1001,27 @@ void NavEKF2_core::FuseDeclination(float declErr) } // Calculate observation Jacobian and Kalman gains - float t2 = magE*magE; - float t3 = magN*magN; - float t4 = t2+t3; - float t5 = 1.0f/t4; - float t22 = magE*t5; - float t23 = magN*t5; - float t6 = P[16][16]*t22; - float t13 = P[17][16]*t23; - float t7 = t6-t13; - float t8 = t22*t7; - float t9 = P[16][17]*t22; - float t14 = P[17][17]*t23; - float t10 = t9-t14; - float t15 = t23*t10; - float t11 = R_DECL+t8-t15; // innovation variance + ftype t2 = magE*magE; + ftype t3 = magN*magN; + ftype t4 = t2+t3; + ftype t5 = 1.0f/t4; + ftype t22 = magE*t5; + ftype t23 = magN*t5; + ftype t6 = P[16][16]*t22; + ftype t13 = P[17][16]*t23; + ftype t7 = t6-t13; + ftype t8 = t22*t7; + ftype t9 = P[16][17]*t22; + ftype t14 = P[17][17]*t23; + ftype t10 = t9-t14; + ftype t15 = t23*t10; + ftype t11 = R_DECL+t8-t15; // innovation variance if (t11 < R_DECL) { return; } - float t12 = 1.0f/t11; + ftype t12 = 1.0f/t11; - float H_MAG[24]; + ftype H_MAG[24]; H_MAG[16] = -magE*t5; H_MAG[17] = magN*t5; @@ -1036,10 +1036,10 @@ void NavEKF2_core::FuseDeclination(float declErr) } // 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) { @@ -1120,18 +1120,18 @@ void NavEKF2_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; @@ -1168,7 +1168,7 @@ bool NavEKF2_core::EKFGSF_resetMainFilterYaw() return false; }; - float yawEKFGSF, yawVarianceEKFGSF, velInnovLength; + ftype yawEKFGSF, yawVarianceEKFGSF, velInnovLength; if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f)) && @@ -1207,15 +1207,15 @@ bool NavEKF2_core::EKFGSF_resetMainFilterYaw() } -void NavEKF2_core::resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw) +void NavEKF2_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, bool isDeltaYaw) { - Quaternion quatBeforeReset = stateStruct.quat; + QuaternionF quatBeforeReset = stateStruct.quat; // check if we should use a 321 or 312 Rotation sequence and update the quaternion // states using the preferred yaw definition stateStruct.quat.inverse().rotation_matrix(prevTnb); - Vector3f eulerAngles; - if (fabsf(prevTnb[2][0]) < fabsf(prevTnb[2][1])) { + Vector3F eulerAngles; + if (fabsF(prevTnb[2][0]) < fabsF(prevTnb[2][1])) { // rolled more than pitched so use 321 rotation order stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); if (isDeltaYaw) { @@ -1234,15 +1234,15 @@ void NavEKF2_core::resetQuatStateYawOnly(float yaw, float yawVariance, bool isDe // 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); // rotate attitude error variances into earth frame - Vector3f bf_variances = Vector3f(P[0][0], P[1][1], P[2][2]); - Vector3f ef_variances = prevTnb.transposed() * bf_variances; + Vector3F bf_variances = Vector3F(P[0][0], P[1][1], P[2][2]); + Vector3F ef_variances = prevTnb.transposed() * bf_variances; // reset vertical component to supplied value ef_variances.z = yawVariance; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index ef69378e20..b7aeec6440 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -132,8 +132,8 @@ void NavEKF2_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; } @@ -161,9 +161,9 @@ void NavEKF2_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; @@ -274,7 +274,7 @@ void NavEKF2_core::readMagData() compass.last_update_usec(magSelectIndex) - lastMagUpdate_us > 70000) { // 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 @@ -302,7 +302,7 @@ void NavEKF2_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).toftype() * 0.001f; // check for consistent data between magnetometers consistentMagData = compass.consistent(); @@ -373,7 +373,7 @@ void NavEKF2_core::readIMUData() learnInactiveBiases(); 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 @@ -399,7 +399,7 @@ void NavEKF2_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 @@ -427,7 +427,7 @@ void NavEKF2_core::readIMUData() storedIMU.push_youngest_element(imuDataDownSampledNew); // calculate the achieved average time step rate for the EKF - float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT); + ftype dtNow = constrain_ftype(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT); dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow; // zero the accumulated IMU data and quaternion @@ -451,7 +451,7 @@ void NavEKF2_core::readIMUData() // protect against delta time going to zero // TODO - check if calculations can tolerate 0 - float minDT = 0.1f*dtEkfAvg; + ftype minDT = 0.1f*dtEkfAvg; imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); @@ -471,11 +471,15 @@ void NavEKF2_core::readIMUData() // read the delta velocity and corresponding time interval from the IMU // return false if data is not available -bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) { +bool NavEKF2_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); + 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-4f); dVel_dt = MIN(dVel_dt,1.0e-1f); return true; @@ -526,11 +530,11 @@ void NavEKF2_core::readGpsData() gpsDataNew.sensor_idx = gps.primary_sensor(); // read the NED velocity from the GPS - gpsDataNew.vel = gps.velocity(); + gpsDataNew.vel = gps.velocity().toftype(); // 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(gpsSpdAccRaw)) { @@ -625,7 +629,7 @@ void NavEKF2_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; @@ -638,7 +642,7 @@ void NavEKF2_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); } else { @@ -659,11 +663,15 @@ void NavEKF2_core::readGpsData() // read the delta angle and corresponding time interval from the IMU // return false if data is not available -bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt) { +bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt) { const auto &ins = dal.ins(); if (ins_index < ins.get_gyro_count()) { - ins.get_delta_angle(ins_index, dAng, dAng_dt); + Vector3f dAngF; + float dAng_dtF; + ins.get_delta_angle(ins_index, dAngF, dAng_dtF); + dAng = dAngF.toftype(); + dAng_dt = dAng_dtF; dAng_dt = MAX(dAng_dt,1.0e-4f); dAng_dt = MIN(dAng_dt,1.0e-1f); return true; @@ -715,7 +723,7 @@ void NavEKF2_core::readBaroData() void NavEKF2_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. @@ -724,14 +732,14 @@ void NavEKF2_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.0f, 1.0f); if (activeHgtSource == HGT_SOURCE_BARO) { // Use the baro drift rate - const float baroDriftRate = 0.05f; + const ftype baroDriftRate = 0.05f; ekfOriginHgtVar += sq(baroDriftRate * deltaTime); } else if (activeHgtSource == HGT_SOURCE_RNG) { // use the worse case expected terrain gradient and vehicle horizontal speed - const float maxTerrGrad = 0.25f; + const ftype maxTerrGrad = 0.25f; ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime); } else { // by definition our height source is absolute so cannot run this filter @@ -741,16 +749,16 @@ void NavEKF2_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[8][8]; + ftype originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8]; // 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) { @@ -837,7 +845,7 @@ void NavEKF2_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; @@ -851,15 +859,20 @@ void NavEKF2_core::readRngBcnData() } // Check if the beacon system has returned a 3D fix - if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) { + Vector3f beaconVehiclePosNEDF; + float beaconVehiclePosErrF; + if (beacon->get_vehicle_position_ned(beaconVehiclePosNEDF, beaconVehiclePosErrF)) { rngBcnLast3DmeasTime_ms = imuSampleTime_ms; } + beaconVehiclePosNED = beaconVehiclePosNEDF.toftype(); + beaconVehiclePosErr = beaconVehiclePosErrF; + // 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 - float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y); - float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; + ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y); + 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 @@ -943,8 +956,8 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, extNavDataNew.posReset = false; } - extNavDataNew.pos = pos; - extNavDataNew.quat = quat; + extNavDataNew.pos = pos.toftype(); + extNavDataNew.quat = quat.toftype(); extNavDataNew.posErr = posErr; extNavDataNew.angErr = angErr; timeStamp_ms = timeStamp_ms - delay_ms; @@ -960,7 +973,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, /* return declination in radians */ -float NavEKF2_core::MagDeclination(void) const +ftype NavEKF2_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 @@ -1002,15 +1015,15 @@ void NavEKF2_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 @@ -1031,13 +1044,13 @@ void NavEKF2_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 - float filtered_accel_active = ins.get_accel(accel_index_active).z - (stateStruct.accel_zbias/dtEkfAvg); - float filtered_accel_inactive = ins.get_accel(i).z - (inactiveBias[i].accel_zbias/dtEkfAvg); - float error = filtered_accel_active - filtered_accel_inactive; + ftype filtered_accel_active = ins.get_accel(accel_index_active).z - (stateStruct.accel_zbias/dtEkfAvg); + ftype filtered_accel_inactive = ins.get_accel(i).z - (inactiveBias[i].accel_zbias/dtEkfAvg); + ftype error = filtered_accel_active - filtered_accel_inactive; // prevent a single large error from contaminating bias estimate - const float bias_limit = 1; // m/s/s - error = constrain_float(error, -bias_limit, bias_limit); + const ftype bias_limit = 1; // m/s/s + error = constrain_ftype(error, -bias_limit, bias_limit); // slowly bring the inactive accel in line with the active accel // this learns 0.5m/s/s bias in about 1 minute @@ -1066,7 +1079,7 @@ void NavEKF2_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t extNavVelMeasTime_ms = timeStamp_ms; useExtNavVel = true; - extNavVelNew.vel = vel; + extNavVelNew.vel = vel.toftype(); extNavVelNew.err = err; timeStamp_ms = timeStamp_ms - delay_ms; // Correct for the average intersampling delay due to the filter updaterate diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index a31e106610..d06277ee4e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -69,7 +69,7 @@ Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scri void NavEKF2_core::EstimateTerrainOffset() { // 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 @@ -90,34 +90,34 @@ void NavEKF2_core::EstimateTerrainOffset() // 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[5][5]; + ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); + ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5]; 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)); @@ -129,7 +129,7 @@ void NavEKF2_core::EstimateTerrainOffset() 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) { @@ -150,18 +150,18 @@ void NavEKF2_core::EstimateTerrainOffset() 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); @@ -178,20 +178,20 @@ void NavEKF2_core::EstimateTerrainOffset() 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 @@ -205,7 +205,7 @@ void NavEKF2_core::EstimateTerrainOffset() 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) { @@ -237,7 +237,7 @@ void NavEKF2_core::EstimateTerrainOffset() 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) { @@ -267,23 +267,23 @@ void NavEKF2_core::EstimateTerrainOffset() void NavEKF2_core::FuseOptFlow() { 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); @@ -304,14 +304,14 @@ void NavEKF2_core::FuseOptFlow() // 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; } @@ -333,97 +333,97 @@ void NavEKF2_core::FuseOptFlow() H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[6]; H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13]; - float t2 = SH_LOS[3]; - float t3 = SH_LOS[0]; - float t4 = SH_LOS[2]; - float t5 = SH_LOS[6]; - float t100 = t2 * t3 * t5; - float t6 = SH_LOS[4]; - float t7 = t2*t3*t6; - float t9 = t2*t4*t5; - float t8 = t7-t9; - float t10 = q0*q3*2.0f; - float t21 = q1*q2*2.0f; - float t11 = t10-t21; - float t101 = t2 * t3 * t11; - float t12 = pd-ptd; - float t13 = 1.0f/(t12*t12); - float t104 = t3 * t4 * t13; - float t14 = SH_LOS[5]; - float t102 = t2 * t4 * t14; - float t15 = SH_LOS[1]; - float t103 = t2 * t3 * t15; - float t16 = q0*q0; - float t17 = q1*q1; - float t18 = q2*q2; - float t19 = q3*q3; - float t20 = t16-t17+t18-t19; - float t105 = t2 * t3 * t20; - float t22 = P[1][1]*t102; - float t23 = P[3][0]*t101; - float t24 = P[8][0]*t104; - float t25 = P[1][0]*t102; - float t26 = P[2][0]*t103; - float t63 = P[0][0]*t8; - float t64 = P[5][0]*t100; - float t65 = P[4][0]*t105; - float t27 = t23+t24+t25+t26-t63-t64-t65; - float t28 = P[3][3]*t101; - float t29 = P[8][3]*t104; - float t30 = P[1][3]*t102; - float t31 = P[2][3]*t103; - float t67 = P[0][3]*t8; - float t68 = P[5][3]*t100; - float t69 = P[4][3]*t105; - float t32 = t28+t29+t30+t31-t67-t68-t69; - float t33 = t101*t32; - float t34 = P[3][8]*t101; - float t35 = P[8][8]*t104; - float t36 = P[1][8]*t102; - float t37 = P[2][8]*t103; - float t70 = P[0][8]*t8; - float t71 = P[5][8]*t100; - float t72 = P[4][8]*t105; - float t38 = t34+t35+t36+t37-t70-t71-t72; - float t39 = t104*t38; - float t40 = P[3][1]*t101; - float t41 = P[8][1]*t104; - float t42 = P[2][1]*t103; - float t73 = P[0][1]*t8; - float t74 = P[5][1]*t100; - float t75 = P[4][1]*t105; - float t43 = t22+t40+t41+t42-t73-t74-t75; - float t44 = t102*t43; - float t45 = P[3][2]*t101; - float t46 = P[8][2]*t104; - float t47 = P[1][2]*t102; - float t48 = P[2][2]*t103; - float t76 = P[0][2]*t8; - float t77 = P[5][2]*t100; - float t78 = P[4][2]*t105; - float t49 = t45+t46+t47+t48-t76-t77-t78; - float t50 = t103*t49; - float t51 = P[3][5]*t101; - float t52 = P[8][5]*t104; - float t53 = P[1][5]*t102; - float t54 = P[2][5]*t103; - float t79 = P[0][5]*t8; - float t80 = P[5][5]*t100; - float t81 = P[4][5]*t105; - float t55 = t51+t52+t53+t54-t79-t80-t81; - float t56 = P[3][4]*t101; - float t57 = P[8][4]*t104; - float t58 = P[1][4]*t102; - float t59 = P[2][4]*t103; - float t83 = P[0][4]*t8; - float t84 = P[5][4]*t100; - float t85 = P[4][4]*t105; - float t60 = t56+t57+t58+t59-t83-t84-t85; - float t66 = t8*t27; - float t82 = t100*t55; - float t86 = t105*t60; - float t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86; - float t62 = 1.0f/t61; + ftype t2 = SH_LOS[3]; + ftype t3 = SH_LOS[0]; + ftype t4 = SH_LOS[2]; + ftype t5 = SH_LOS[6]; + ftype t100 = t2 * t3 * t5; + ftype t6 = SH_LOS[4]; + ftype t7 = t2*t3*t6; + ftype t9 = t2*t4*t5; + ftype t8 = t7-t9; + ftype t10 = q0*q3*2.0f; + ftype t21 = q1*q2*2.0f; + ftype t11 = t10-t21; + ftype t101 = t2 * t3 * t11; + ftype t12 = pd-ptd; + ftype t13 = 1.0f/(t12*t12); + ftype t104 = t3 * t4 * t13; + ftype t14 = SH_LOS[5]; + ftype t102 = t2 * t4 * t14; + ftype t15 = SH_LOS[1]; + ftype t103 = t2 * t3 * t15; + ftype t16 = q0*q0; + ftype t17 = q1*q1; + ftype t18 = q2*q2; + ftype t19 = q3*q3; + ftype t20 = t16-t17+t18-t19; + ftype t105 = t2 * t3 * t20; + ftype t22 = P[1][1]*t102; + ftype t23 = P[3][0]*t101; + ftype t24 = P[8][0]*t104; + ftype t25 = P[1][0]*t102; + ftype t26 = P[2][0]*t103; + ftype t63 = P[0][0]*t8; + ftype t64 = P[5][0]*t100; + ftype t65 = P[4][0]*t105; + ftype t27 = t23+t24+t25+t26-t63-t64-t65; + ftype t28 = P[3][3]*t101; + ftype t29 = P[8][3]*t104; + ftype t30 = P[1][3]*t102; + ftype t31 = P[2][3]*t103; + ftype t67 = P[0][3]*t8; + ftype t68 = P[5][3]*t100; + ftype t69 = P[4][3]*t105; + ftype t32 = t28+t29+t30+t31-t67-t68-t69; + ftype t33 = t101*t32; + ftype t34 = P[3][8]*t101; + ftype t35 = P[8][8]*t104; + ftype t36 = P[1][8]*t102; + ftype t37 = P[2][8]*t103; + ftype t70 = P[0][8]*t8; + ftype t71 = P[5][8]*t100; + ftype t72 = P[4][8]*t105; + ftype t38 = t34+t35+t36+t37-t70-t71-t72; + ftype t39 = t104*t38; + ftype t40 = P[3][1]*t101; + ftype t41 = P[8][1]*t104; + ftype t42 = P[2][1]*t103; + ftype t73 = P[0][1]*t8; + ftype t74 = P[5][1]*t100; + ftype t75 = P[4][1]*t105; + ftype t43 = t22+t40+t41+t42-t73-t74-t75; + ftype t44 = t102*t43; + ftype t45 = P[3][2]*t101; + ftype t46 = P[8][2]*t104; + ftype t47 = P[1][2]*t102; + ftype t48 = P[2][2]*t103; + ftype t76 = P[0][2]*t8; + ftype t77 = P[5][2]*t100; + ftype t78 = P[4][2]*t105; + ftype t49 = t45+t46+t47+t48-t76-t77-t78; + ftype t50 = t103*t49; + ftype t51 = P[3][5]*t101; + ftype t52 = P[8][5]*t104; + ftype t53 = P[1][5]*t102; + ftype t54 = P[2][5]*t103; + ftype t79 = P[0][5]*t8; + ftype t80 = P[5][5]*t100; + ftype t81 = P[4][5]*t105; + ftype t55 = t51+t52+t53+t54-t79-t80-t81; + ftype t56 = P[3][4]*t101; + ftype t57 = P[8][4]*t104; + ftype t58 = P[1][4]*t102; + ftype t59 = P[2][4]*t103; + ftype t83 = P[0][4]*t8; + ftype t84 = P[5][4]*t100; + ftype t85 = P[4][4]*t105; + ftype t60 = t56+t57+t58+t59-t83-t84-t85; + ftype t66 = t8*t27; + ftype t82 = t100*t55; + ftype t86 = t105*t60; + ftype t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86; + ftype t62 = 1.0f/t61; // calculate innovation variance for X axis observation and protect against a badly conditioned calculation if (t61 > R_LOS) { @@ -487,97 +487,97 @@ void NavEKF2_core::FuseOptFlow() H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[5]; H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13]; - float t2 = SH_LOS[3]; - float t3 = SH_LOS[0]; - float t4 = SH_LOS[1]; - float t5 = SH_LOS[5]; - float t100 = t2 * t3 * t5; - float t6 = SH_LOS[4]; - float t7 = t2*t3*t6; - float t8 = t2*t4*t5; - float t9 = t7+t8; - float t10 = q0*q3*2.0f; - float t11 = q1*q2*2.0f; - float t12 = t10+t11; - float t101 = t2 * t3 * t12; - float t13 = pd-ptd; - float t14 = 1.0f/(t13*t13); - float t104 = t3 * t4 * t14; - float t15 = SH_LOS[6]; - float t105 = t2 * t4 * t15; - float t16 = SH_LOS[2]; - float t102 = t2 * t3 * t16; - float t17 = q0*q0; - float t18 = q1*q1; - float t19 = q2*q2; - float t20 = q3*q3; - float t21 = t17+t18-t19-t20; - float t103 = t2 * t3 * t21; - float t22 = P[0][0]*t105; - float t23 = P[1][1]*t9; - float t24 = P[8][1]*t104; - float t25 = P[0][1]*t105; - float t26 = P[5][1]*t100; - float t64 = P[4][1]*t101; - float t65 = P[2][1]*t102; - float t66 = P[3][1]*t103; - float t27 = t23+t24+t25+t26-t64-t65-t66; - float t28 = t9*t27; - float t29 = P[1][4]*t9; - float t30 = P[8][4]*t104; - float t31 = P[0][4]*t105; - float t32 = P[5][4]*t100; - float t67 = P[4][4]*t101; - float t68 = P[2][4]*t102; - float t69 = P[3][4]*t103; - float t33 = t29+t30+t31+t32-t67-t68-t69; - float t34 = P[1][8]*t9; - float t35 = P[8][8]*t104; - float t36 = P[0][8]*t105; - float t37 = P[5][8]*t100; - float t71 = P[4][8]*t101; - float t72 = P[2][8]*t102; - float t73 = P[3][8]*t103; - float t38 = t34+t35+t36+t37-t71-t72-t73; - float t39 = t104*t38; - float t40 = P[1][0]*t9; - float t41 = P[8][0]*t104; - float t42 = P[5][0]*t100; - float t74 = P[4][0]*t101; - float t75 = P[2][0]*t102; - float t76 = P[3][0]*t103; - float t43 = t22+t40+t41+t42-t74-t75-t76; - float t44 = t105*t43; - float t45 = P[1][2]*t9; - float t46 = P[8][2]*t104; - float t47 = P[0][2]*t105; - float t48 = P[5][2]*t100; - float t63 = P[2][2]*t102; - float t77 = P[4][2]*t101; - float t78 = P[3][2]*t103; - float t49 = t45+t46+t47+t48-t63-t77-t78; - float t50 = P[1][5]*t9; - float t51 = P[8][5]*t104; - float t52 = P[0][5]*t105; - float t53 = P[5][5]*t100; - float t80 = P[4][5]*t101; - float t81 = P[2][5]*t102; - float t82 = P[3][5]*t103; - float t54 = t50+t51+t52+t53-t80-t81-t82; - float t55 = t100*t54; - float t56 = P[1][3]*t9; - float t57 = P[8][3]*t104; - float t58 = P[0][3]*t105; - float t59 = P[5][3]*t100; - float t83 = P[4][3]*t101; - float t84 = P[2][3]*t102; - float t85 = P[3][3]*t103; - float t60 = t56+t57+t58+t59-t83-t84-t85; - float t70 = t101*t33; - float t79 = t102*t49; - float t86 = t103*t60; - float t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86; - float t62 = 1.0f/t61; + ftype t2 = SH_LOS[3]; + ftype t3 = SH_LOS[0]; + ftype t4 = SH_LOS[1]; + ftype t5 = SH_LOS[5]; + ftype t100 = t2 * t3 * t5; + ftype t6 = SH_LOS[4]; + ftype t7 = t2*t3*t6; + ftype t8 = t2*t4*t5; + ftype t9 = t7+t8; + ftype t10 = q0*q3*2.0f; + ftype t11 = q1*q2*2.0f; + ftype t12 = t10+t11; + ftype t101 = t2 * t3 * t12; + ftype t13 = pd-ptd; + ftype t14 = 1.0f/(t13*t13); + ftype t104 = t3 * t4 * t14; + ftype t15 = SH_LOS[6]; + ftype t105 = t2 * t4 * t15; + ftype t16 = SH_LOS[2]; + ftype t102 = t2 * t3 * t16; + ftype t17 = q0*q0; + ftype t18 = q1*q1; + ftype t19 = q2*q2; + ftype t20 = q3*q3; + ftype t21 = t17+t18-t19-t20; + ftype t103 = t2 * t3 * t21; + ftype t22 = P[0][0]*t105; + ftype t23 = P[1][1]*t9; + ftype t24 = P[8][1]*t104; + ftype t25 = P[0][1]*t105; + ftype t26 = P[5][1]*t100; + ftype t64 = P[4][1]*t101; + ftype t65 = P[2][1]*t102; + ftype t66 = P[3][1]*t103; + ftype t27 = t23+t24+t25+t26-t64-t65-t66; + ftype t28 = t9*t27; + ftype t29 = P[1][4]*t9; + ftype t30 = P[8][4]*t104; + ftype t31 = P[0][4]*t105; + ftype t32 = P[5][4]*t100; + ftype t67 = P[4][4]*t101; + ftype t68 = P[2][4]*t102; + ftype t69 = P[3][4]*t103; + ftype t33 = t29+t30+t31+t32-t67-t68-t69; + ftype t34 = P[1][8]*t9; + ftype t35 = P[8][8]*t104; + ftype t36 = P[0][8]*t105; + ftype t37 = P[5][8]*t100; + ftype t71 = P[4][8]*t101; + ftype t72 = P[2][8]*t102; + ftype t73 = P[3][8]*t103; + ftype t38 = t34+t35+t36+t37-t71-t72-t73; + ftype t39 = t104*t38; + ftype t40 = P[1][0]*t9; + ftype t41 = P[8][0]*t104; + ftype t42 = P[5][0]*t100; + ftype t74 = P[4][0]*t101; + ftype t75 = P[2][0]*t102; + ftype t76 = P[3][0]*t103; + ftype t43 = t22+t40+t41+t42-t74-t75-t76; + ftype t44 = t105*t43; + ftype t45 = P[1][2]*t9; + ftype t46 = P[8][2]*t104; + ftype t47 = P[0][2]*t105; + ftype t48 = P[5][2]*t100; + ftype t63 = P[2][2]*t102; + ftype t77 = P[4][2]*t101; + ftype t78 = P[3][2]*t103; + ftype t49 = t45+t46+t47+t48-t63-t77-t78; + ftype t50 = P[1][5]*t9; + ftype t51 = P[8][5]*t104; + ftype t52 = P[0][5]*t105; + ftype t53 = P[5][5]*t100; + ftype t80 = P[4][5]*t101; + ftype t81 = P[2][5]*t102; + ftype t82 = P[3][5]*t103; + ftype t54 = t50+t51+t52+t53-t80-t81-t82; + ftype t55 = t100*t54; + ftype t56 = P[1][3]*t9; + ftype t57 = P[8][3]*t104; + ftype t58 = P[0][3]*t105; + ftype t59 = P[5][3]*t100; + ftype t83 = P[4][3]*t101; + ftype t84 = P[2][3]*t102; + ftype t85 = P[3][3]*t103; + ftype t60 = t56+t57+t58+t59-t83-t84-t85; + ftype t70 = t101*t33; + ftype t79 = t102*t49; + ftype t86 = t103*t60; + ftype t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86; + ftype t62 = 1.0f/t61; // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation if (t61 > R_LOS) { @@ -633,7 +633,7 @@ void NavEKF2_core::FuseOptFlow() } // 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_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 49b46647d6..d7716e85e3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -25,8 +25,8 @@ bool NavEKF2_core::healthy(void) const return false; } // 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))) { + ftype horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]); + if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) { return false; } @@ -36,16 +36,16 @@ bool NavEKF2_core::healthy(void) const // Return a consolidated error score where higher numbers represent larger errors // Intended to be used by the front-end to determine which is the primary EKF -float NavEKF2_core::errorScore() const +ftype NavEKF2_core::errorScore() const { - float score = 0.0f; + ftype score = 0.0f; if (tiltAlignComplete && yawAlignComplete) { // Check GPS fusion performance score = MAX(score, 0.5f * (velTestRatio + posTestRatio)); // Check altimeter fusion performance score = MAX(score, hgtTestRatio); // Check attitude corrections - const float tiltErrThreshold = 0.05f; + const ftype tiltErrThreshold = 0.05f; score = MAX(score, tiltErrFilt / tiltErrThreshold); } return score; @@ -90,7 +90,7 @@ void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const gyroBias.zero(); return; } - gyroBias = stateStruct.gyro_bias / dtEkfAvg; + gyroBias = stateStruct.gyro_bias.tofloat() / dtEkfAvg; } // return body axis gyro scale factor error as a percentage @@ -115,7 +115,7 @@ void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const // return the quaternions defining the rotation from NED to XYZ (body) axes void NavEKF2_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 @@ -130,7 +130,7 @@ uint32_t NavEKF2_core::getLastYawResetAngle(float &yawAng) const // returns the time of the last reset or 0 if no reset has ever occurred uint32_t NavEKF2_core::getLastPosNorthEastReset(Vector2f &pos) const { - pos = posResetNE; + pos = posResetNE.tofloat(); return lastPosReset_ms; } @@ -146,7 +146,7 @@ uint32_t NavEKF2_core::getLastPosDownReset(float &posD) const // returns the time of the last reset or 0 if no reset has ever occurred uint32_t NavEKF2_core::getLastVelNorthEastReset(Vector2f &vel) const { - vel = velResetNE; + vel = velResetNE.tofloat(); return lastVelReset_ms; } @@ -164,7 +164,7 @@ void NavEKF2_core::getWind(Vector3f &wind) const void NavEKF2_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 @@ -174,7 +174,7 @@ bool NavEKF2_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 NavEKF2_core::getPosNE(Vector2f &posNE) const if ((dal.gps().status(dal.gps().primary_sensor()) >= 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 = dal.gps().location(); - 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; @@ -364,13 +364,13 @@ bool NavEKF2_core::getOriginLLH(struct Location &loc) const // return earth magnetic field estimates in measurement units / 1000 void NavEKF2_core::getMagNED(Vector3f &magNED) const { - magNED = stateStruct.earth_magfield * 1000.0f; + magNED = (stateStruct.earth_magfield * 1000.0).tofloat(); } // return body magnetic field estimates in measurement units / 1000 void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const { - magXYZ = stateStruct.body_magfield*1000.0f; + magXYZ = (stateStruct.body_magfield*1000.0).tofloat(); } // return magnetometer offsets @@ -383,14 +383,14 @@ bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const // compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited, // primary compass is valid and state variances have converged - const float maxMagVar = 5E-6f; + const ftype maxMagVar = 5E-6f; bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar); if ((mag_idx == magSelectIndex) && finalInflightMagInit && !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*1000.0).tofloat(); return true; } else { magOffsets = dal.get_compass()->get_offsets(magSelectIndex); @@ -419,15 +419,15 @@ void NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vecto // also return the delta in position due to the last position reset void NavEKF2_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(); } @@ -537,14 +537,14 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) const Vector2f offset; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); - const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); + const float mag_max = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z); // Only report range finder normalised innovation levels if the EKF needs the data for primary // height estimation or optical flow operation. This prevents false alarms at the GCS if a // range finder is fitted for other applications float temp; if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { - temp = sqrtf(auxRngTestRatio); + temp = sqrtF(auxRngTestRatio); } else { temp = 0.0f; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 118a890e83..9190c65574 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -144,10 +144,10 @@ void NavEKF2_core::ResetPosition(void) // 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 NavEKF2_core::ResetPositionNE(float posN, float posE) +void NavEKF2_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; @@ -245,10 +245,10 @@ void NavEKF2_core::ResetHeight(void) // 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 NavEKF2_core::ResetPositionD(float posD) +void NavEKF2_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; @@ -279,7 +279,7 @@ bool NavEKF2_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 @@ -313,7 +313,7 @@ bool NavEKF2_core::resetHeightDatum(void) */ void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const { - const Vector3f &posOffsetBody = dal.gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; + const Vector3F posOffsetBody = dal.gps().get_antenna_offset(gpsDataDelayed.sensor_idx).toftype() - accelPosOffset; if (posOffsetBody.is_zero()) { return; } @@ -321,33 +321,33 @@ void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const // Don't fuse velocity data if GPS doesn't support it if (fuseVelData) { // 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.x -= velOffsetEarth.x; gps_data.vel.y -= velOffsetEarth.y; gps_data.vel.z -= velOffsetEarth.z; } - 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; } // correct external navigation earth-frame position using sensor body-frame offset -void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const +void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3F &ext_position) const { #if HAL_VISUALODOM_ENABLED const auto *visual_odom = dal.visualodom(); 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_position.x -= posOffsetEarth.x; ext_position.y -= posOffsetEarth.y; ext_position.z -= posOffsetEarth.z; @@ -355,19 +355,19 @@ void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const } // correct external navigation earth-frame velocity using sensor body-frame offset -void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const +void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3F &ext_velocity) const { #if HAL_VISUALODOM_ENABLED const auto *visual_odom = dal.visualodom(); 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.0f/imuDataDelayed.delAngDT); ext_velocity += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate); #endif } @@ -565,7 +565,7 @@ void NavEKF2_core::FuseVelPosNED() bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check // declare variables used to check measurement errors - Vector3f velInnov; + Vector3F velInnov; // declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; @@ -575,7 +575,7 @@ void NavEKF2_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 @@ -586,14 +586,14 @@ void NavEKF2_core::FuseVelPosNED() if (fuseVelData || fusePosData || fuseHgtData) { // calculate additional error in GPS position caused by manoeuvring - float posErr = frontend->gpsPosVarAccScale * accNavMag; + ftype posErr = frontend->gpsPosVarAccScale * accNavMag; // estimate the GPS Velocity, GPS horiz position and height measurement variances. // Use different errors if operating without external aiding using an assumed position or velocity of zero 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); @@ -606,33 +606,33 @@ void NavEKF2_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)); } 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)); } 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)); } 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)); } 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 perfomrance // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early - float obs_data_chk; + ftype obs_data_chk; 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 { - 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; } @@ -644,8 +644,8 @@ void NavEKF2_core::FuseVelPosNED() // the accelerometers and we should disable the GPS and barometer innovation consistency checks. if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) { // calculate innovations for height and vertical GPS vel measurements - float hgtErr = stateStruct.position.z - velPosObs[5]; - float velDErr = stateStruct.velocity.z - velPosObs[2]; + ftype hgtErr = stateStruct.position.z - velPosObs[5]; + 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[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) { badIMUdata = true; @@ -663,7 +663,7 @@ void NavEKF2_core::FuseVelPosNED() varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3]; varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4]; // apply an innovation consistency threshold test, but don't fail if bad IMU data - float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]); + ftype maxPosInnov2 = sq(MAX(0.01f * (ftype)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]); posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); // use position data if healthy or timed out @@ -703,8 +703,8 @@ void NavEKF2_core::FuseVelPosNED() !dal.gps().have_vertical_velocity())) { imax = 1; } - 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++) { // velocity states start at index 3 stateIndex = i + 3; @@ -718,7 +718,7 @@ void NavEKF2_core::FuseVelPosNED() } // apply an innovation consistency threshold test, but don't fail if bad IMU data // calculate the test ratio - velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f))); + velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (ftype)frontend->_gpsVelInnovGate, 1.0f))); // fail if the ratio is greater than 1 velHealth = ((velTestRatio < 1.0f) || badIMUdata); // use velocity data if healthy, timed out, or in constant position mode @@ -744,12 +744,12 @@ void NavEKF2_core::FuseVelPosNED() innovVelPos[5] = stateStruct.position.z - velPosObs[5]; varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio - hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); + hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (ftype)frontend->_hgtInnovGate, 1.0f)) * 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 - const float maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0 : 1.0; + const ftype maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0 : 1.0; // fail if the ratio is > maxTestRatio, but don't fail if bad IMU data hgtHealth = (hgtTestRatio < maxTestRatio) || badIMUdata; @@ -759,9 +759,9 @@ void NavEKF2_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-3f; + const ftype hgtInnovFiltTC = 2.0f; + ftype alpha = constrain_ftype(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f); hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha; } else { hgtInnovFiltState = 0.0f; @@ -812,8 +812,8 @@ void NavEKF2_core::FuseVelPosNED() R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 5) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex]; - const float gndMaxBaroErr = 4.0f; - const float gndBaroInnovFloor = -0.5f; + const ftype gndMaxBaroErr = 4.0f; + const ftype gndBaroInnovFloor = -0.5f; if(dal.get_touchdown_expected() && activeHgtSource == HGT_SOURCE_BARO) { // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor @@ -824,7 +824,7 @@ void NavEKF2_core::FuseVelPosNED() // ____/| // / | // / | - innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); + innovVelPos[5] += constrain_ftype(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); } } @@ -954,9 +954,9 @@ void NavEKF2_core::selectHeightForFusion() if (_rng && rangeDataToFuse) { const 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; } } @@ -976,16 +976,16 @@ void NavEKF2_core::selectHeightForFusion() activeHgtSource = HGT_SOURCE_RNG; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _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-4f * (float)_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; // If the terrain height is consistent and we are moving slowly, then it can be // used as a height reference in combination with a range finder // apply a hysteresis to the speed check to prevent rapid switching - float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y); + ftype horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y); bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !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)); bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable; /* @@ -1030,9 +1030,9 @@ void NavEKF2_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.5f; + const ftype dtBaro = frontend->hgtAvg_ms*1.0e-3; + ftype alpha = constrain_ftype(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f); meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha; } } @@ -1050,7 +1050,7 @@ void NavEKF2_core::selectHeightForFusion() // Select the height measurement source if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) { hgtMea = -extNavDataDelayed.pos.z; - posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f)); + posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f)); } else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) { // using range finder data // correct for tilt using a flat earth model @@ -1063,7 +1063,7 @@ void NavEKF2_core::selectHeightForFusion() fuseHgtData = true; velPosObs[5] = -hgtMea; // 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 { @@ -1078,9 +1078,9 @@ void NavEKF2_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 == HGT_SOURCE_BARO)) { // using Baro data @@ -1089,7 +1089,7 @@ void NavEKF2_core::selectHeightForFusion() velPosObs[5] = -hgtMea; 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 in ground effect if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) { posDownObsNoise *= frontend->gndEffectBaroScaler; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp index 9d757ef779..b218c711d6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp @@ -26,14 +26,14 @@ void NavEKF2_core::SelectRngBcnFusion() void NavEKF2_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; @@ -54,7 +54,7 @@ void NavEKF2_core::FuseRngBcn() bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffset; // predicted range - Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; + Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; rngPred = deltaPosNED.length(); // calculate measurement innovation @@ -64,37 +64,37 @@ void NavEKF2_core::FuseRngBcn() if (rngPred > 0.1f) { // calculate observation jacobians - float H_BCN[24] = {}; - 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 H_BCN[24] = {}; + 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[6] = -t4*t9; H_BCN[7] = -t3*t9; H_BCN[8] = -t2*t9; // calculate Kalman gains - float t10 = P[8][8]*t2*t9; - float t11 = P[7][8]*t3*t9; - float t12 = P[6][8]*t4*t9; - float t13 = t10+t11+t12; - float t14 = t2*t9*t13; - float t15 = P[8][7]*t2*t9; - float t16 = P[7][7]*t3*t9; - float t17 = P[6][7]*t4*t9; - float t18 = t15+t16+t17; - float t19 = t3*t9*t18; - float t20 = P[8][6]*t2*t9; - float t21 = P[7][6]*t3*t9; - float t22 = P[6][6]*t4*t9; - float t23 = t20+t21+t22; - float t24 = t4*t9*t23; + ftype t10 = P[8][8]*t2*t9; + ftype t11 = P[7][8]*t3*t9; + ftype t12 = P[6][8]*t4*t9; + ftype t13 = t10+t11+t12; + ftype t14 = t2*t9*t13; + ftype t15 = P[8][7]*t2*t9; + ftype t16 = P[7][7]*t3*t9; + ftype t17 = P[6][7]*t4*t9; + ftype t18 = t15+t16+t17; + ftype t19 = t3*t9*t18; + ftype t20 = P[8][6]*t2*t9; + ftype t21 = P[7][6]*t3*t9; + ftype t22 = P[6][6]*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.0/varInnovRngBcn; faultStatus.bad_rngbcn = false; @@ -141,18 +141,18 @@ void NavEKF2_core::FuseRngBcn() Kfusion[20] = -t26*(P[20][6]*t4*t9+P[20][7]*t3*t9+P[20][8]*t2*t9); Kfusion[21] = -t26*(P[21][6]*t4*t9+P[21][7]*t3*t9+P[21][8]*t2*t9); } else { - // zero indexes 16 to 21 = 6*4 bytes - memset(&Kfusion[16], 0, 24); + // zero indexes 16 to 21 = 6 + zero_range(&Kfusion[0], 16, 21); } Kfusion[22] = -t26*(P[22][6]*t4*t9+P[22][7]*t3*t9+P[22][8]*t2*t9); Kfusion[23] = -t26*(P[23][6]*t4*t9+P[23][7]*t3*t9+P[23][8]*t2*t9); // 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); @@ -245,7 +245,7 @@ https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon void NavEKF2_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 @@ -269,7 +269,7 @@ void NavEKF2_core::FuseRngBcnStatic() } if (numBcnMeas >= 100) { rngBcnAlignmentStarted = true; - float tempVar = 1.0f / (float)numBcnMeas; + ftype tempVar = 1.0f / (float)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; @@ -298,7 +298,7 @@ void NavEKF2_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; @@ -317,14 +317,14 @@ void NavEKF2_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; @@ -347,57 +347,57 @@ void NavEKF2_core::FuseRngBcnStatic() } // calculate the observation jacobian - float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset; - 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 + bcnPosOffset; + 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 -= bcnPosOffset; innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng; @@ -440,7 +440,7 @@ void NavEKF2_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; } @@ -458,7 +458,7 @@ void NavEKF2_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 NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning) +void NavEKF2_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 @@ -466,35 +466,35 @@ void NavEKF2_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.01f; // 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.01f; // 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 + bcnPosOffsetMax; - 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 + bcnPosOffsetMax; + 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; // calculate a filtered innovation magnitude to be used to select between the high or low offset - OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsf(innov); + OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsF(innov); // covariance prediction bcnPosOffsetMaxVar += stateNoiseVar; @@ -524,14 +524,14 @@ void NavEKF2_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; // calculate a filtered innovation magnitude to be used to select between the high or low offset - OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsf(innov); + OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsF(innov); // covariance prediction bcnPosOffsetMinVar += stateNoiseVar; @@ -555,7 +555,7 @@ void NavEKF2_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 bcnPosOffsetMax = MAX(bcnPosOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 19a0e8e740..23121106d5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -28,7 +28,7 @@ void NavEKF2_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) { /* @@ -55,9 +55,9 @@ void NavEKF2_core::calcGpsGoodToAlign(void) // Check for significant change in GPS position if disarmed which indicates bad GPS // This check can only be used when the vehicle is stationary const struct Location &gpsloc = gps.location(); // Current location - const float posFiltTimeConst = 10.0f; // time constant used to decay position drift + const ftype posFiltTimeConst = 10.0f; // 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(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst); lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms; // Sum distance moved gpsDriftNE += gpsloc_prev.get_distance(gpsloc); @@ -85,8 +85,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void) if (gps.have_vertical_velocity() && 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; } @@ -95,7 +95,7 @@ void NavEKF2_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; @@ -106,8 +106,8 @@ void NavEKF2_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; } @@ -123,7 +123,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void) } // fail if horiziontal position accuracy not sufficient - float hAcc = 0.0f; + float hAcc = 0.0; bool hAccFail; if (gps.horizontal_accuracy(hAcc)) { hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); @@ -243,14 +243,14 @@ void NavEKF2_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; @@ -259,7 +259,7 @@ void NavEKF2_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)); @@ -308,7 +308,7 @@ void NavEKF2_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(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y); + ftype gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y); bool highGndSpd = false; bool highAirSpd = false; bool largeHgtChange = false; @@ -327,7 +327,7 @@ void NavEKF2_core::detectFlight() } // 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_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index e651d1c11d..e53d9cc9d8 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -219,7 +219,7 @@ void NavEKF2_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; @@ -249,7 +249,7 @@ void NavEKF2_core::InitialiseVariables() ekfGpsRefHgt = 0.0; velOffsetNED.zero(); posOffsetNED.zero(); - memset(&velPosObs, 0, sizeof(velPosObs)); + ZERO_FARRAY(velPosObs); // range beacon fusion variables memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew)); @@ -397,24 +397,24 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) imuDataDelayed = imuDataNew; // 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 = ins.get_accel(accel_index_active); + initAccVec = ins.get_accel(accel_index_active).toftype(); // read the magnetometer data readMagData(); // 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 @@ -626,7 +626,7 @@ void NavEKF2_core::UpdateFilter(bool predict) } -void NavEKF2_core::correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index) +void NavEKF2_core::correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index) { delAng.x = delAng.x * stateStruct.gyro_scale.x; delAng.y = delAng.y * stateStruct.gyro_scale.y; @@ -634,7 +634,7 @@ void NavEKF2_core::correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t g delAng -= inactiveBias[gyro_index].gyro_bias * (delAngDT / dtEkfAvg); } -void NavEKF2_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index) +void NavEKF2_core::correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index) { delVel.z -= inactiveBias[accel_index].accel_zbias * (delVelDT / dtEkfAvg); } @@ -659,7 +659,7 @@ void NavEKF2_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; @@ -680,13 +680,13 @@ void NavEKF2_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; @@ -717,16 +717,16 @@ void NavEKF2_core::UpdateStrapdownEquationsNED() void NavEKF2_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 corections 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 @@ -735,15 +735,15 @@ void NavEKF2_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; @@ -756,14 +756,14 @@ void NavEKF2_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 @@ -776,11 +776,11 @@ void NavEKF2_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 @@ -801,12 +801,12 @@ void NavEKF2_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 { @@ -818,17 +818,17 @@ void NavEKF2_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); - timeDelay = fmaxf(timeDelay, dtIMUavg); - float errorGain = 0.5f / timeDelay; + ftype timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms); + timeDelay = fmaxF(timeDelay, dtIMUavg); + ftype errorGain = 0.5f / timeDelay; // calculate a corrrection 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(); @@ -836,16 +836,16 @@ void NavEKF2_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 @@ -878,35 +878,35 @@ void NavEKF2_core::calcOutputStates() */ void NavEKF2_core::CovariancePrediction() { - float windVelSigma; // wind velocity 1-sigma process noise - m/s - float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s - float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s - float dAngScaleSigma;// delta angle scale factor 1-Sigma process noise - float magEarthSigma;// earth magnetic field 1-sigma process noise - float magBodySigma; // body magnetic field 1-sigma process noise - float daxNoise; // X axis delta angle noise variance rad^2 - float dayNoise; // Y axis delta angle noise variance rad^2 - float dazNoise; // Z axis delta angle noise variance rad^2 - float dvxNoise; // X axis delta velocity variance noise (m/s)^2 - float dvyNoise; // Y axis delta velocity variance noise (m/s)^2 - float dvzNoise; // 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 dax_s; // X axis delta angle measurement scale factor - float day_s; // Y axis delta angle measurement scale factor - float daz_s; // Z axis delta angle measurement scale factor - float dvz_b; // Z axis delta velocity measurement bias (rad) + ftype windVelSigma; // wind velocity 1-sigma process noise - m/s + ftype dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s + ftype dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s + ftype dAngScaleSigma;// delta angle scale factor 1-Sigma process noise + ftype magEarthSigma;// earth magnetic field 1-sigma process noise + ftype magBodySigma; // body magnetic field 1-sigma process noise + ftype daxNoise; // X axis delta angle noise variance rad^2 + ftype dayNoise; // Y axis delta angle noise variance rad^2 + ftype dazNoise; // Z axis delta angle noise variance rad^2 + ftype dvxNoise; // X axis delta velocity variance noise (m/s)^2 + ftype dvyNoise; // Y axis delta velocity variance noise (m/s)^2 + ftype dvzNoise; // 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 dax_s; // X axis delta angle measurement scale factor + ftype day_s; // Y axis delta angle measurement scale factor + ftype daz_s; // Z axis delta angle measurement scale factor + ftype dvz_b; // Z axis delta velocity measurement bias (rad) Vector25 SF; Vector5 SG; Vector8 SQ; @@ -917,17 +917,17 @@ void NavEKF2_core::CovariancePrediction() // this allows for wind gradient effects. // filter height rate using a 10 second time constant filter dt = imuDataDelayed.delAngDT; - float alpha = 0.1f * dt; + ftype alpha = 0.1f * dt; hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha; // use filtered height rate to increase wind process noise when climbing or descending // this allows for wind gradient effects. - windVelSigma = dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)); - dAngBiasSigma = sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f); - dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f); - dAngScaleSigma = dt * constrain_float(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f); - magEarthSigma = dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f); - magBodySigma = dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f); + windVelSigma = dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate)); + dAngBiasSigma = sq(dt) * constrain_ftype(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f); + dVelBiasSigma = sq(dt) * constrain_ftype(frontend->_accelBiasProcessNoise, 0.0f, 1.0f); + dAngScaleSigma = dt * constrain_ftype(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f); + magEarthSigma = dt * constrain_ftype(frontend->_magEarthProcessNoise, 0.0f, 1.0f); + magBodySigma = dt * constrain_ftype(frontend->_magBodyProcessNoise, 0.0f, 1.0f); for (uint8_t i= 0; i<=8; i++) processNoise[i] = 0.0f; for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma; for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma; @@ -960,9 +960,9 @@ void NavEKF2_core::CovariancePrediction() day_s = stateStruct.gyro_scale.y; daz_s = stateStruct.gyro_scale.z; dvz_b = stateStruct.accel_zbias; - float _gyrNoise = constrain_float(frontend->_gyrNoise, 0.0f, 1.0f); + ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f); daxNoise = dayNoise = dazNoise = sq(dt*_gyrNoise); - float _accNoise = constrain_float(frontend->_accNoise, 0.0f, 10.0f); + ftype _accNoise = constrain_ftype(frontend->_accNoise, 0.0f, 10.0f); dvxNoise = dvyNoise = dvzNoise = sq(dt*_accNoise); // calculate the predicted covariance due to inertial sensor error propagation @@ -1409,7 +1409,7 @@ void NavEKF2_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); } } @@ -1419,7 +1419,7 @@ void NavEKF2_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); } } @@ -1451,7 +1451,7 @@ void NavEKF2_core::StoreQuatReset() } // Rotate the stored output quaternion history through a quaternion rotation -void NavEKF2_core::StoreQuatRotate(const Quaternion &deltaQuat) +void NavEKF2_core::StoreQuatRotate(const QuaternionF &deltaQuat) { outputDataNew.quat = outputDataNew.quat*deltaQuat; // write current measurement to entire table @@ -1468,7 +1468,7 @@ void NavEKF2_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; } @@ -1490,37 +1490,37 @@ void NavEKF2_core::CopyCovariances() // constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning void NavEKF2_core::ConstrainVariances() { - for (uint8_t i=0; i<=2; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error - for (uint8_t i=3; i<=5; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities - for (uint8_t i=6; i<=7; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f); - P[8][8] = constrain_float(P[8][8],0.0f,1.0e6f); // vertical position - for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases + for (uint8_t i=0; i<=2; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,1.0f); // attitude error + for (uint8_t i=3; i<=5; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,1.0e3f); // velocities + for (uint8_t i=6; i<=7; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,EK2_POSXY_STATE_LIMIT); + P[8][8] = constrain_ftype(P[8][8],0.0f,1.0e6f); // vertical position + for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases if (PV_AidingMode != AID_NONE) { - for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // delta angle scale factors + for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // delta angle scale factors } else { // we can't reliably estimate scale factors when there is no aiding data due to transient manoeuvre induced innovations // so inhibit estimation by keeping covariance elements at zero zeroRows(P,12,14); zeroCols(P,12,14); } - P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias - 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=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // wind velocity + P[15][15] = constrain_ftype(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias + 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 + for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,1.0e3f); // wind velocity } // constrain states using WMM tables and specified limit void NavEKF2_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); } @@ -1529,33 +1529,33 @@ void NavEKF2_core::MagTableConstrain(void) void NavEKF2_core::ConstrainStates() { // attitude errors are limited between +-1 - for (uint8_t i=0; i<=2; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f); + for (uint8_t i=0; i<=2; 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=3; i<=5; i++) statesArray[i] = constrain_float(statesArray[i],-5.0e2f,5.0e2f); - // position limit 1000 km - TODO apply circular limit - for (uint8_t i=6; i<=7; i++) statesArray[i] = constrain_float(statesArray[i],-1.0e6f,1.0e6f); + for (uint8_t i=3; i<=5; i++) statesArray[i] = constrain_ftype(statesArray[i],-5.0e2f,5.0e2f); + // position limit - TODO apply circular limit + for (uint8_t i=6; i<=7; i++) statesArray[i] = constrain_ftype(statesArray[i],-EK2_POSXY_STATE_LIMIT,EK2_POSXY_STATE_LIMIT); // height limit covers home alt on everest through to home alt at SL and ballon 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=9; i<=11; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg); + for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_ftype(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg); // gyro scale factor limit of +-5% (this needs to be set based on manufacturers specs) - for (uint8_t i=12; i<=14; i++) statesArray[i] = constrain_float(statesArray[i],0.95f,1.05f); + for (uint8_t i=12; i<=14; i++) statesArray[i] = constrain_ftype(statesArray[i],0.95f,1.05f); // Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data) - stateStruct.accel_zbias = constrain_float(stateStruct.accel_zbias,-1.0f*dtEkfAvg,1.0f*dtEkfAvg); + stateStruct.accel_zbias = constrain_ftype(stateStruct.accel_zbias,-1.0f*dtEkfAvg,1.0f*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); @@ -1563,23 +1563,23 @@ void NavEKF2_core::ConstrainStates() } // calculate the NED earth spin vector in rad/sec -void NavEKF2_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const +void NavEKF2_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); } // initialise the earth magnetic field states using declination, suppled roll/pitch // and magnetometer measurements and return initial attitude quaternion -Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) +QuaternionF NavEKF2_core::calcQuatAndFieldStates(ftype roll, ftype pitch) { // declare local variables required to calculate initial orientation and magnetic field - float yaw; - Matrix3f Tbn; - Vector3f initMagNED; - Quaternion initQuat; + ftype yaw; + Matrix3F Tbn; + Vector3F initMagNED; + QuaternionF initQuat; if (use_compass()) { // calculate rotation matrix from body to NED frame @@ -1592,17 +1592,17 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) initMagNED = Tbn * magDataDelayed.mag; // calculate heading of mag field rel to body heading - float magHeading = atan2f(initMagNED.y, initMagNED.x); + ftype magHeading = atan2F(initMagNED.y, initMagNED.x); // get the magnetic declination - float magDecAng = MagDeclination(); + ftype magDecAng = MagDeclination(); // calculate yaw angle rel to true north yaw = magDecAng - magHeading; // calculate initial filter quaternion states using yaw from magnetometer // store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset - Vector3f tempEuler; + Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); // this check ensures we accumulate the resets that occur within a single iteration of the EKF if (imuSampleTime_ms != lastYawReset_ms) { @@ -1659,7 +1659,7 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) // zero the attitude covariances, but preserve the variances void NavEKF2_core::zeroAttCovOnly() { - float varTemp[3]; + ftype varTemp[3]; for (uint8_t index=0; index<=2; index++) { varTemp[index] = P[index][index]; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 9fed911eec..3ff0952b34 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -63,6 +63,13 @@ // number of seconds a request to reset the yaw to the GSF estimate is active before it times out #define YAW_RESET_TO_GSF_TIMEOUT_MS 5000 +// limit on horizontal position states +#if HAL_WITH_EKF_DOUBLE +#define EK2_POSXY_STATE_LIMIT 50.0e6 +#else +#define EK2_POSXY_STATE_LIMIT 1.0e6 +#endif + class AP_AHRS; class NavEKF2_core : public NavEKF_core_common @@ -87,7 +94,7 @@ public: // Return a consolidated error score where higher numbers are less healthy // Intended to be used by the front-end to determine which is the primary EKF - float errorScore(void) const; + ftype errorScore(void) const; // Write the last calculated NE position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false @@ -383,16 +390,16 @@ private: // broken down as individual elements. Both are equivalent (same // memory) struct state_elements { - Vector3f angErr; // 0..2 - Vector3f velocity; // 3..5 - Vector3f position; // 6..8 - Vector3f gyro_bias; // 9..11 - Vector3f gyro_scale; // 12..14 - float accel_zbias; // 15 - Vector3f earth_magfield; // 16..18 - Vector3f body_magfield; // 19..21 - Vector2f wind_vel; // 22..23 - Quaternion quat; // 24..27 + Vector3F angErr; // 0..2 + Vector3F velocity; // 3..5 + Vector3F position; // 6..8 + Vector3F gyro_bias; // 9..11 + Vector3F gyro_scale; // 12..14 + ftype accel_zbias; // 15 + Vector3F earth_magfield; // 16..18 + Vector3F body_magfield; // 19..21 + Vector2F wind_vel; // 22..23 + QuaternionF quat; // 24..27 }; union { @@ -401,78 +408,78 @@ private: }; struct output_elements { - Quaternion quat; // 0..3 - Vector3f velocity; // 4..6 - Vector3f position; // 7..9 + QuaternionF quat; // 0..3 + Vector3F velocity; // 4..6 + Vector3F position; // 7..9 }; struct imu_elements { - Vector3f delAng; // 0..2 - Vector3f delVel; // 3..5 - float delAngDT; // 6 - float delVelDT; // 7 + Vector3F delAng; // 0..2 + Vector3F delVel; // 3..5 + ftype delAngDT; // 6 + ftype delVelDT; // 7 uint32_t time_ms; // 8 uint8_t gyro_index; uint8_t accel_index; }; struct gps_elements : EKF_obs_element_t { - Vector2f pos; - float hgt; - Vector3f vel; + Vector2F pos; + ftype hgt; + Vector3F vel; uint8_t sensor_idx; }; struct mag_elements : EKF_obs_element_t { - Vector3f mag; + Vector3F mag; }; struct baro_elements : EKF_obs_element_t { - float hgt; + ftype hgt; }; struct range_elements : EKF_obs_element_t { - float rng; + ftype rng; uint8_t sensor_idx; }; 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; + ftype tas; }; struct of_elements : EKF_obs_element_t { - Vector2f flowRadXY; - Vector2f flowRadXYcomp; - Vector3f bodyRadXYZ; - Vector3f body_offset; + Vector2F flowRadXY; + Vector2F flowRadXYcomp; + Vector3F bodyRadXYZ; + Vector3F body_offset; }; struct ext_nav_elements : EKF_obs_element_t { - Vector3f pos; // XYZ position measured in a RH navigation frame (m) - Quaternion quat; // quaternion describing the rotation from navigation to body frame - float posErr; // spherical poition measurement error 1-std (m) - float angErr; // spherical angular measurement error 1-std (rad) + Vector3F pos; // XYZ position measured in a RH navigation frame (m) + QuaternionF quat; // quaternion describing the rotation from navigation to body frame + ftype posErr; // spherical poition measurement error 1-std (m) + ftype angErr; // spherical angular measurement error 1-std (rad) bool posReset; // true when the position measurement has been reset }; // bias estimates for the IMUs that are enabled but not being used // by this core. struct { - Vector3f gyro_bias; - Vector3f gyro_scale; - float accel_zbias; + Vector3F gyro_bias; + Vector3F gyro_scale; + ftype accel_zbias; } inactiveBias[INS_MAX_INSTANCES]; struct ext_nav_vel_elements : EKF_obs_element_t { - Vector3f vel; // velocity in NED (m) - float err; // velocity measurement error (m/s) + Vector3F vel; // velocity in NED (m) + ftype err; // velocity measurement error (m/s) }; // update the navigation filter status @@ -509,7 +516,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(); @@ -533,21 +540,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(); @@ -593,7 +600,7 @@ private: // initialise the earth magnetic field states using declination and current attitude and magnetometer measurements // and return attitude quaternion - Quaternion calcQuatAndFieldStates(float roll, float pitch); + QuaternionF calcQuatAndFieldStates(ftype roll, ftype pitch); // zero stored variables void InitialiseVariables(); @@ -604,7 +611,7 @@ private: void ResetPosition(void); // reset the stateStruct's NE position to the specified position - void ResetPositionNE(float posN, float posE); + void ResetPositionNE(ftype posN, ftype posE); // reset velocity states using the last GPS measurement void ResetVelocity(void); @@ -613,7 +620,7 @@ private: void ResetHeight(void); // reset the stateStruct's D position - void ResetPositionD(float posD); + void ResetPositionD(ftype posD); // return true if we should use the airspeed sensor bool useAirspeed(void) const; @@ -631,7 +638,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 optical flow data is available bool optFlowDataPresent(void) const; @@ -697,10 +704,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 @@ -734,10 +741,10 @@ private: void CorrectGPSForAntennaOffset(gps_elements &gps_data) const; // correct external navigation earth-frame position using sensor body-frame offset - void CorrectExtNavForSensorOffset(Vector3f &ext_position) const; + void CorrectExtNavForSensorOffset(Vector3F &ext_position) const; // correct external navigation earth-frame velocity using sensor body-frame offset - void CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const; + void CorrectExtNavVelForSensorOffset(Vector3F &ext_velocity) const; // Runs the IMU prediction step for an independent GSF yaw estimator algorithm // that uses IMU, GPS horizontal velocity and optionally true airspeed data. @@ -753,7 +760,7 @@ private: // yaw : new yaw angle (rad) // yaw_variance : variance of new yaw angle (rad^2) // isDeltaYaw : true when the yaw should be added to the existing yaw angle - void resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw); + void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, bool isDeltaYaw); // attempt to reset the yaw to the EKF-GSF value // returns false if unsuccessful @@ -775,7 +782,7 @@ private: bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out bool badIMUdata; // boolean true if the bad IMU data is detected - 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 @@ -784,10 +791,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) @@ -804,8 +811,8 @@ 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 bool magFusePerformed; // boolean set to true when magnetometer fusion has been performed in that time step @@ -813,8 +820,8 @@ private: uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step 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 @@ -829,13 +836,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 lastYawTime_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 - 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 - float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive. + Vector2F lastKnownPositionNE; // last known position + 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 + ftype defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive. bool inhibitWindStates; // true when wind states and covariances are to remain constant bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant bool lastInhibitMagStates; // previous inhibitMagStates @@ -844,18 +851,18 @@ private: uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset 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 - Vector3f tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion - float tiltErrFilt; // Filtered tilt error metric + Vector3F tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion + ftype tiltErrFilt; // Filtered tilt error metric bool tiltAlignComplete; // true when tilt alignment is complete bool yawAlignComplete; // true when yaw alignment is complete bool magStateInitComplete; // true when the magnetic field sttes have been initialised @@ -863,7 +870,7 @@ 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 @@ -875,10 +882,10 @@ private: gps_elements gpsDataDelayed; // GPS data at the fusion time horizon 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 @@ -889,57 +896,57 @@ private: bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed bool airSpdFusionDelayed; // true when the air speed fusion has been delayed bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed - 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 cycele 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) // variables used to calculate a vertical velocity that is kinematically consistent with the verical 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 filterred vertical GPS velocity detected durng 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 filterred vertical GPS velocity detected durng 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 applid to reported GPS speed accuracy - float sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed + ftype sAccFilterState1; // state variable for LPF applid 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 @@ -951,28 +958,28 @@ private: of_elements ofDataDelayed; // OF data at the fusion time horizon bool flowDataToFuse; // true when optical flow data is ready for fusion 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. @@ -984,15 +991,15 @@ 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 accelPosOffset; // position of IMU accelerometer unit in body frame (m) + 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 + 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 @@ -1003,47 +1010,47 @@ private: rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon 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 innvovation 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 bool rngBcnTimeout; // boolean true if range beacon measurements have faled innovation consistency checks for too long - 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[10]; // 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) - float bcnPosOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (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) + ftype bcnPosOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) - float bcnPosOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) - float bcnPosOffsetMaxVar; // Variance of the bcnPosOffsetHigh state (m) - float OffsetMaxInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetHigh + ftype bcnPosOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) + ftype bcnPosOffsetMaxVar; // Variance of the bcnPosOffsetHigh state (m) + ftype OffsetMaxInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetHigh - float bcnPosOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) - float bcnPosOffsetMinVar; // Variance of the bcnPosoffset state (m) - float OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow + ftype bcnPosOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) + ftype bcnPosOffsetMinVar; // Variance of the bcnPosoffset state (m) + ftype OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow // 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[10]; // height source selection logic @@ -1051,11 +1058,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 @@ -1063,9 +1070,9 @@ private: bool magStateResetRequest; // true if magnetic field states need to be reset using the magneteomter 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 uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent // external navigation fusion @@ -1136,8 +1143,8 @@ private: ftype magXbias; ftype magYbias; ftype magZbias; - Matrix3f DCM; - Vector3f MagPred; + Matrix3F DCM; + Vector3F MagPred; ftype R_MAG; Vector9 SH_MAG; } mag_state; @@ -1147,8 +1154,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; @@ -1160,7 +1167,7 @@ private: bool assume_zero_sideslip(void) const; // vehicle specific initial gyro bias uncertainty - float InitialGyroBiasUncertainty(void) const; + ftype InitialGyroBiasUncertainty(void) const; // The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF uint32_t EKFGSF_yaw_reset_ms; // timestamp of last emergency yaw reset (uSec) diff --git a/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp b/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp index f2b73f110a..ab21a8a937 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp @@ -16,7 +16,7 @@ void NavEKF2_core::resetGyroBias(void) zeroRows(P,9,11); zeroCols(P,9,11); - P[9][9] = sq(radians(0.5f * dtIMUavg)); + P[9][9] = sq(radians(0.5 * dtIMUavg)); P[10][10] = P[9][9]; P[11][11] = P[9][9]; } @@ -24,8 +24,8 @@ void NavEKF2_core::resetGyroBias(void) /* vehicle specific initial gyro bias uncertainty in deg/sec */ -float NavEKF2_core::InitialGyroBiasUncertainty(void) const +ftype NavEKF2_core::InitialGyroBiasUncertainty(void) const { - return 2.5f; + return 2.5; }