Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-20 23:04:09 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: allow for double EKF build

Browse Source
This commit is contained in:
Andrew Tridgell 2021-05-04 21:12:23 +10:00
parent 3235747ef8
commit 6aca0bb08a
13 changed files with 946 additions and 926 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -21,17 +21,17 @@ extern const AP_HAL::HAL& hal;
void NavEKF2_core::FuseAirspeed() void NavEKF2_core::FuseAirspeed()
{ {
// declarations // declarations
float vn; ftype vn;
float ve; ftype ve;
float vd; ftype vd;
float vwn; ftype vwn;
float vwe; ftype vwe;
float EAS2TAS = dal.get_EAS2TAS(); ftype EAS2TAS = dal.get_EAS2TAS();
const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f)); const ftype R_TAS = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(EAS2TAS, 0.9f, 10.0f));
Vector3 SH_TAS; Vector3 SH_TAS;
float SK_TAS; ftype SK_TAS;
Vector24 H_TAS; Vector24 H_TAS;
float VtasPred; ftype VtasPred;
// copy required states to local variable names // copy required states to local variable names
vn = stateStruct.velocity.x; vn = stateStruct.velocity.x;
@ -46,7 +46,7 @@ void NavEKF2_core::FuseAirspeed()
if (VtasPred > 1.0f) if (VtasPred > 1.0f)
{ {
// calculate observation jacobians // 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[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/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; 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]; H_TAS[23] = -SH_TAS[1];
for (uint8_t i=6; i<=21; i++) H_TAS[i] = 0.0f; for (uint8_t i=6; i<=21; i++) H_TAS[i] = 0.0f;
// calculate Kalman gains // 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) { if (temp >= R_TAS) {
SK_TAS = 1.0f / temp; SK_TAS = 1.0f / temp;
faultStatus.bad_airspeed = false; faultStatus.bad_airspeed = false;
@ -107,7 +107,7 @@ void NavEKF2_core::FuseAirspeed()
innovVtas = VtasPred - tasDataDelayed.tas; innovVtas = VtasPred - tasDataDelayed.tas;
// calculate the innovation consistency test ratio // 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 // fail if the ratio is > 1, but don't fail if bad IMU data
bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
@ -238,21 +238,21 @@ void NavEKF2_core::SelectBetaFusion()
void NavEKF2_core::FuseSideslip() void NavEKF2_core::FuseSideslip()
{ {
// declarations // declarations
float q0; ftype q0;
float q1; ftype q1;
float q2; ftype q2;
float q3; ftype q3;
float vn; ftype vn;
float ve; ftype ve;
float vd; ftype vd;
float vwn; ftype vwn;
float vwe; ftype vwe;
const float R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg const ftype R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg
Vector10 SH_BETA; Vector10 SH_BETA;
Vector5 SK_BETA; Vector5 SK_BETA;
Vector3f vel_rel_wind; Vector3F vel_rel_wind;
Vector24 H_BETA; Vector24 H_BETA;
float innovBeta; ftype innovBeta;
// copy required states to local variable names // copy required states to local variable names
q0 = stateStruct.quat[0]; q0 = stateStruct.quat[0];
@ -278,7 +278,7 @@ void NavEKF2_core::FuseSideslip()
{ {
// Calculate observation jacobians // 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); 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; faultStatus.bad_sideslip = true;
return; return;
} else { } else {
@ -307,7 +307,7 @@ void NavEKF2_core::FuseSideslip()
H_BETA[23] = SH_BETA[1]*SH_BETA[3]*SH_BETA[8] - SH_BETA[4]; H_BETA[23] = SH_BETA[1]*SH_BETA[3]*SH_BETA[8] - SH_BETA[4];
// Calculate Kalman gains // 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) { if (temp >= R_BETA) {
SK_BETA[0] = 1.0f / temp; SK_BETA[0] = 1.0f / temp;
faultStatus.bad_sideslip = false; faultStatus.bad_sideslip = false;

32
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -61,15 +61,15 @@ void NavEKF2_core::setWindMagStateLearningMode()
if (yawAlignComplete && useAirspeed()) { if (yawAlignComplete && useAirspeed()) {
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading // 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 // which assumes the vehicle has launched into the wind
Vector3f tempEuler; Vector3F tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z); stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z);
stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z); stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z);
// set the wind sate variances to the measurement uncertainty // set the wind sate variances to the measurement uncertainty
for (uint8_t index=22; index<=23; index++) { 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 { } else {
// set the variances using a typical wind speed // set the variances using a typical wind speed
@ -325,7 +325,7 @@ void NavEKF2_core::setAidingMode()
} }
// handle height reset as special case // handle height reset as special case
hgtMea = -extNavDataDelayed.pos.z; 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(); ResetHeight();
} }
// reset the last fusion accepted times to prevent unwanted activation of timeout logic // 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() void NavEKF2_core::checkAttitudeAlignmentStatus()
{ {
// Check for tilt convergence - used during initial alignment // Check for tilt convergence - used during initial alignment
float alpha = 1.0f*imuDataDelayed.delAngDT; ftype alpha = 1.0f*imuDataDelayed.delAngDT;
float temp=tiltErrVec.length(); ftype temp=tiltErrVec.length();
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt; tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
if (tiltErrFilt < 0.005f && !tiltAlignComplete) { if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
tiltAlignComplete = true; tiltAlignComplete = true;
@ -470,14 +470,14 @@ void NavEKF2_core::recordYawReset()
bool NavEKF2_core::checkGyroCalStatus(void) bool NavEKF2_core::checkGyroCalStatus(void)
{ {
// check delta angle bias variances // check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); const ftype delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
if (!use_compass()) { if (!use_compass()) {
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a 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 // which can make this check fail
Vector3f delAngBiasVarVec = Vector3f(P[9][9],P[10][10],P[11][11]); Vector3F delAngBiasVarVec = Vector3F(P[9][9],P[10][10],P[11][11]);
Vector3f temp = prevTnb * delAngBiasVarVec; Vector3F temp = prevTnb * delAngBiasVarVec;
delAngBiasLearned = (fabsf(temp.x) < delAngBiasVarMax) && delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) &&
(fabsf(temp.y) < delAngBiasVarMax); (fabsF(temp.y) < delAngBiasVarMax);
} else { } else {
delAngBiasLearned = (P[9][9] <= delAngBiasVarMax) && delAngBiasLearned = (P[9][9] <= delAngBiasVarMax) &&
(P[10][10] <= delAngBiasVarMax) && (P[10][10] <= delAngBiasVarMax) &&
@ -525,7 +525,7 @@ void NavEKF2_core::updateFilterStatus(void)
void NavEKF2_core::runYawEstimatorPrediction() void NavEKF2_core::runYawEstimatorPrediction()
{ {
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) { if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
float trueAirspeed; ftype trueAirspeed;
if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) { if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {
if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) { if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
trueAirspeed = tasDataDelayed.tas; trueAirspeed = tasDataDelayed.tas;
@ -544,8 +544,8 @@ void NavEKF2_core::runYawEstimatorCorrection()
{ {
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1 && EKFGSF_run_filterbank) { if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1 && EKFGSF_run_filterbank) {
if (gpsDataToFuse) { if (gpsDataToFuse) {
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y); Vector2F gpsVelNE = Vector2F(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise); ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc); yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
} }

14
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -129,7 +129,7 @@ void NavEKF2_core::Log_Write_NKF4(uint64_t time_us) const
nav_filter_status solutionStatus {}; nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {}; nav_gps_status gpsStatus {};
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); 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); getFilterFaults(_faultStatus);
getFilterStatus(solutionStatus); getFilterStatus(solutionStatus);
getFilterGpsStatus(gpsStatus); getFilterGpsStatus(gpsStatus);
@ -142,7 +142,7 @@ void NavEKF2_core::Log_Write_NKF4(uint64_t time_us) const
sqrtvarH : (int16_t)(100*hgtVar), sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*tempVar), sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar), sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : tiltErrFilt, // tilt error convergence metric tiltErr : float(tiltErrFilt), // tilt error convergence metric
offsetNorth : offset.x, offsetNorth : offset.x,
offsetEast : offset.y, offsetEast : offset.y,
faults : _faultStatus, 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 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 RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
errHAGL : (uint16_t)(100*sqrtf(Popt)), // filter ground offset state error errHAGL : (uint16_t)(100*sqrtF(Popt)), // filter ground offset state error
angErr : outputTrackError.x, angErr : float(outputTrackError.x),
velErr : outputTrackError.y, velErr : float(outputTrackError.y),
posErr : outputTrackError.z posErr : float(outputTrackError.z)
}; };
AP::logger().WriteBlock(&pkt5, sizeof(pkt5)); 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), rng : (int16_t)(100*report.rng),
innov : (int16_t)(100*report.innov), innov : (int16_t)(100*report.innov),
sqrtInnovVar : (uint16_t)(100*safe_sqrt(report.innovVar)), 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), beaconPosN : (int16_t)(100*report.beaconPosNED.x),
beaconPosE : (int16_t)(100*report.beaconPosNED.y), beaconPosE : (int16_t)(100*report.beaconPosNED.y),
beaconPosD : (int16_t)(100*report.beaconPosNED.z), beaconPosD : (int16_t)(100*report.beaconPosNED.z),

228
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -24,9 +24,9 @@ void NavEKF2_core::controlMagYawReset()
gpsYawResetRequest = false; gpsYawResetRequest = false;
} }
// Quaternion and delta rotation vector that are re-used for different calculations // QuaternionF and delta rotation vector that are re-used for different calculations
Vector3f deltaRotVecTemp; Vector3F deltaRotVecTemp;
Quaternion deltaQuatTemp; QuaternionF deltaQuatTemp;
bool flightResetAllowed = false; bool flightResetAllowed = false;
bool initialResetAllowed = false; bool initialResetAllowed = false;
@ -61,7 +61,7 @@ void NavEKF2_core::controlMagYawReset()
// check for increasing height // check for increasing height
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f; 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 // check for increasing yaw innovations
bool yawInnovIncreasing = yawInnovIncrease > 0.25f; 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 a yaw reset has been requested, apply the updated quaternion to the current state
if (extNavYawResetRequest) { if (extNavYawResetRequest) {
// get the euler angles from the current state estimate // get the euler angles from the current state estimate
Vector3f eulerAnglesOld; Vector3F eulerAnglesOld;
stateStruct.quat.to_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesOld.z); stateStruct.quat.to_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesOld.z);
// previous value used to calculate a reset delta // 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 // Get the Euler angles from the external vision data
Vector3f eulerAnglesNew; Vector3F eulerAnglesNew;
extNavDataDelayed.quat.to_euler(eulerAnglesNew.x, eulerAnglesNew.y, eulerAnglesNew.z); extNavDataDelayed.quat.to_euler(eulerAnglesNew.x, eulerAnglesNew.y, eulerAnglesNew.z);
// the new quaternion uses the old roll/pitch and new yaw angle // the new quaternion uses the old roll/pitch and new yaw angle
@ -128,16 +128,16 @@ void NavEKF2_core::controlMagYawReset()
} else if (magYawResetRequest || magStateResetRequest) { } else if (magYawResetRequest || magStateResetRequest) {
// get the euler angles from the current state estimate // get the euler angles from the current state estimate
Vector3f eulerAngles; Vector3F eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
// Use the Euler angles and magnetometer measurement to update the magnetic field states // Use the Euler angles and magnetometer measurement to update the magnetic field states
// and get an updated quaternion // and get an updated quaternion
Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); QuaternionF newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
if (magYawResetRequest) { if (magYawResetRequest) {
// previous value used to calculate a reset delta // 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 // update the quaternion states using the new yaw angle
stateStruct.quat = newQuat; stateStruct.quat = newQuat;
@ -181,17 +181,17 @@ void NavEKF2_core::realignYawGPS()
{ {
if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) { if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
// get quaternion from existing filter states and calculate roll, pitch and yaw angles // 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); stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
// calculate course yaw angle // 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 // 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 // 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 // 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; 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 &magXbias = mag_state.magXbias;
ftype &magYbias = mag_state.magYbias; ftype &magYbias = mag_state.magYbias;
ftype &magZbias = mag_state.magZbias; ftype &magZbias = mag_state.magZbias;
Matrix3f &DCM = mag_state.DCM; Matrix3F &DCM = mag_state.DCM;
Vector3f &MagPred = mag_state.MagPred; Vector3F &MagPred = mag_state.MagPred;
ftype &R_MAG = mag_state.R_MAG; ftype &R_MAG = mag_state.R_MAG;
ftype *SH_MAG = &mag_state.SH_MAG[0]; ftype *SH_MAG = &mag_state.SH_MAG[0];
Vector24 H_MAG; Vector24 H_MAG;
@ -374,7 +374,7 @@ void NavEKF2_core::FuseMagnetometer()
} }
// scale magnetometer observation error with total angular rate to allow for timing errors // 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 // 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); SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
@ -426,7 +426,7 @@ void NavEKF2_core::FuseMagnetometer()
// calculate the innovation test ratios // calculate the innovation test ratios
for (uint8_t i = 0; i<=2; i++) { 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 // check the last values from all components and set magnetometer health accordingly
@ -448,7 +448,7 @@ void NavEKF2_core::FuseMagnetometer()
if (obsIndex == 0) if (obsIndex == 0)
{ {
// calculate observation jacobians // 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[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[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
H_MAG[16] = SH_MAG[1]; 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 else if (obsIndex == 1) // we are now fusing the Y measurement
{ {
// calculate observation jacobians // 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[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[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
H_MAG[16] = 2.0f*q1*q2 - SH_MAG[8]; 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 else if (obsIndex == 2) // we are now fusing the Z measurement
{ {
// calculate observation jacobians // 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[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[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
H_MAG[16] = SH_MAG[5]; H_MAG[16] = SH_MAG[5];
@ -720,60 +720,60 @@ void NavEKF2_core::FuseMagnetometer()
*/ */
void NavEKF2_core::fuseEulerYaw() void NavEKF2_core::fuseEulerYaw()
{ {
float q0 = stateStruct.quat[0]; ftype q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1]; ftype q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2]; ftype q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3]; ftype q3 = stateStruct.quat[3];
// compass measurement error variance (rad^2) set to parameter value as a default // 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 // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
// determine if a 321 or 312 Euler sequence is best // determine if a 321 or 312 Euler sequence is best
float predicted_yaw; ftype predicted_yaw;
float measured_yaw; ftype measured_yaw;
float H_YAW[3]; ftype H_YAW[3];
Matrix3f Tbn_zeroYaw; 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 // calculate observation jacobian when we are observing the first rotation in a 321 sequence
float t2 = q0*q0; ftype t2 = q0*q0;
float t3 = q1*q1; ftype t3 = q1*q1;
float t4 = q2*q2; ftype t4 = q2*q2;
float t5 = q3*q3; ftype t5 = q3*q3;
float t6 = t2+t3-t4-t5; ftype t6 = t2+t3-t4-t5;
float t7 = q0*q3*2.0f; ftype t7 = q0*q3*2.0f;
float t8 = q1*q2*2.0f; ftype t8 = q1*q2*2.0f;
float t9 = t7+t8; ftype t9 = t7+t8;
float t10 = sq(t6); ftype t10 = sq(t6);
if (t10 > 1e-6f) { if (t10 > 1e-6f) {
t10 = 1.0f / t10; t10 = 1.0f / t10;
} else { } else {
return; return;
} }
float t11 = t9*t9; ftype t11 = t9*t9;
float t12 = t10*t11; ftype t12 = t10*t11;
float t13 = t12+1.0f; ftype t13 = t12+1.0f;
float t14; ftype t14;
if (fabsf(t13) > 1e-3f) { if (fabsF(t13) > 1e-3f) {
t14 = 1.0f/t13; t14 = 1.0f/t13;
} else { } else {
return; return;
} }
float t15 = 1.0f/t6; ftype t15 = 1.0f/t6;
H_YAW[0] = 0.0f; 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[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)); H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
// calculate predicted and measured yaw angle // calculate predicted and measured yaw angle
Vector3f euler321; Vector3F euler321;
stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z); stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
predicted_yaw = euler321.z; predicted_yaw = euler321.z;
if (use_compass() && yawAlignComplete && magStateInitComplete) { if (use_compass() && yawAlignComplete && magStateInitComplete) {
// Use measured mag components rotated into earth frame to measure yaw // Use measured mag components rotated into earth frame to measure yaw
Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); measured_yaw = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
} else if (extNavUsedForYaw) { } else if (extNavUsedForYaw) {
// Get the yaw angle from the external vision data // Get the yaw angle from the external vision data
R_YAW = sq(extNavDataDelayed.angErr); R_YAW = sq(extNavDataDelayed.angErr);
@ -781,7 +781,7 @@ void NavEKF2_core::fuseEulerYaw()
measured_yaw = euler321.z; measured_yaw = euler321.z;
} else { } else {
if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
float gsfYaw, gsfYawVariance, velInnovLength; ftype gsfYaw, gsfYawVariance, velInnovLength;
if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) && if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
is_positive(gsfYawVariance) && is_positive(gsfYawVariance) &&
gsfYawVariance < sq(radians(15.0f)) && gsfYawVariance < sq(radians(15.0f)) &&
@ -799,42 +799,42 @@ void NavEKF2_core::fuseEulerYaw()
} }
} else { } else {
// calculate observation jacobian when we are observing a rotation in a 312 sequence // calculate observation jacobian when we are observing a rotation in a 312 sequence
float t2 = q0*q0; ftype t2 = q0*q0;
float t3 = q1*q1; ftype t3 = q1*q1;
float t4 = q2*q2; ftype t4 = q2*q2;
float t5 = q3*q3; ftype t5 = q3*q3;
float t6 = t2-t3+t4-t5; ftype t6 = t2-t3+t4-t5;
float t7 = q0*q3*2.0f; ftype t7 = q0*q3*2.0f;
float t10 = q1*q2*2.0f; ftype t10 = q1*q2*2.0f;
float t8 = t7-t10; ftype t8 = t7-t10;
float t9 = sq(t6); ftype t9 = sq(t6);
if (t9 > 1e-6f) { if (t9 > 1e-6f) {
t9 = 1.0f/t9; t9 = 1.0f/t9;
} else { } else {
return; return;
} }
float t11 = t8*t8; ftype t11 = t8*t8;
float t12 = t9*t11; ftype t12 = t9*t11;
float t13 = t12+1.0f; ftype t13 = t12+1.0f;
float t14; ftype t14;
if (fabsf(t13) > 1e-3f) { if (fabsF(t13) > 1e-3f) {
t14 = 1.0f/t13; t14 = 1.0f/t13;
} else { } else {
return; 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[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[1] = 0.0f;
H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10)); H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
// calculate predicted and measured yaw angle // calculate predicted and measured yaw angle
Vector3f euler312 = stateStruct.quat.to_vector312(); Vector3F euler312 = stateStruct.quat.to_vector312();
predicted_yaw = euler312.z; predicted_yaw = euler312.z;
if (use_compass() && yawAlignComplete && magStateInitComplete) { if (use_compass() && yawAlignComplete && magStateInitComplete) {
// Use measured mag components rotated into earth frame to measure yaw // Use measured mag components rotated into earth frame to measure yaw
Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f); Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f);
Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); measured_yaw = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
} else if (extNavUsedForYaw) { } else if (extNavUsedForYaw) {
// Get the yaw angle from the external vision data // Get the yaw angle from the external vision data
R_YAW = sq(extNavDataDelayed.angErr); R_YAW = sq(extNavDataDelayed.angErr);
@ -842,7 +842,7 @@ void NavEKF2_core::fuseEulerYaw()
measured_yaw = euler312.z; measured_yaw = euler312.z;
} else { } else {
if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
float gsfYaw, gsfYawVariance, velInnovLength; ftype gsfYaw, gsfYawVariance, velInnovLength;
if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) && if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
is_positive(gsfYawVariance) && is_positive(gsfYawVariance) &&
gsfYawVariance < sq(radians(15.0f)) && gsfYawVariance < sq(radians(15.0f)) &&
@ -861,14 +861,14 @@ void NavEKF2_core::fuseEulerYaw()
} }
// Calculate the innovation // 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 // Copy raw value to output variable used for data logging
innovYaw = innovation; innovYaw = innovation;
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero // 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]; ftype PH[3];
float varInnov = R_YAW; ftype varInnov = R_YAW;
for (uint8_t rowIndex=0; rowIndex<=2; rowIndex++) { for (uint8_t rowIndex=0; rowIndex<=2; rowIndex++) {
PH[rowIndex] = 0.0f; PH[rowIndex] = 0.0f;
for (uint8_t colIndex=0; colIndex<=2; colIndex++) { for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
@ -876,7 +876,7 @@ void NavEKF2_core::fuseEulerYaw()
} }
varInnov += H_YAW[rowIndex]*PH[rowIndex]; varInnov += H_YAW[rowIndex]*PH[rowIndex];
} }
float varInnovInv; ftype varInnovInv;
if (varInnov >= R_YAW) { if (varInnov >= R_YAW) {
varInnovInv = 1.0f / varInnov; varInnovInv = 1.0f / varInnov;
// output numerical health status // output numerical health status
@ -900,7 +900,7 @@ void NavEKF2_core::fuseEulerYaw()
} }
// calculate the innovation test ratio // 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 // Declare the magnetometer unhealthy if the innovation test fails
if (yawTestRatio > 1.0f) { if (yawTestRatio > 1.0f) {
@ -930,7 +930,7 @@ void NavEKF2_core::fuseEulerYaw()
} }
for (uint8_t row = 0; row <= stateIndexLim; row++) { for (uint8_t row = 0; row <= stateIndexLim; row++) {
for (uint8_t column = 0; column <= stateIndexLim; column++) { 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][1] * P[1][column];
tmp += KH[row][2] * P[2][column]; tmp += KH[row][2] * P[2][column];
KHP[row][column] = tmp; 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 * 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 * 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) // declination error variance (rad^2)
const float R_DECL = sq(declErr); const ftype R_DECL = sq(declErr);
// copy required states to local variables // copy required states to local variables
float magN = stateStruct.earth_magfield.x; ftype magN = stateStruct.earth_magfield.x;
float magE = stateStruct.earth_magfield.y; ftype magE = stateStruct.earth_magfield.y;
// prevent bad earth field states from causing numerical errors or exceptions // prevent bad earth field states from causing numerical errors or exceptions
if (magN < 1e-3f) { if (magN < 1e-3f) {
@ -1001,27 +1001,27 @@ void NavEKF2_core::FuseDeclination(float declErr)
} }
// Calculate observation Jacobian and Kalman gains // Calculate observation Jacobian and Kalman gains
float t2 = magE*magE; ftype t2 = magE*magE;
float t3 = magN*magN; ftype t3 = magN*magN;
float t4 = t2+t3; ftype t4 = t2+t3;
float t5 = 1.0f/t4; ftype t5 = 1.0f/t4;
float t22 = magE*t5; ftype t22 = magE*t5;
float t23 = magN*t5; ftype t23 = magN*t5;
float t6 = P[16][16]*t22; ftype t6 = P[16][16]*t22;
float t13 = P[17][16]*t23; ftype t13 = P[17][16]*t23;
float t7 = t6-t13; ftype t7 = t6-t13;
float t8 = t22*t7; ftype t8 = t22*t7;
float t9 = P[16][17]*t22; ftype t9 = P[16][17]*t22;
float t14 = P[17][17]*t23; ftype t14 = P[17][17]*t23;
float t10 = t9-t14; ftype t10 = t9-t14;
float t15 = t23*t10; ftype t15 = t23*t10;
float t11 = R_DECL+t8-t15; // innovation variance ftype t11 = R_DECL+t8-t15; // innovation variance
if (t11 < R_DECL) { if (t11 < R_DECL) {
return; 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[16] = -magE*t5;
H_MAG[17] = magN*t5; H_MAG[17] = magN*t5;
@ -1036,10 +1036,10 @@ void NavEKF2_core::FuseDeclination(float declErr)
} }
// get the magnetic declination // get the magnetic declination
float magDecAng = MagDeclination(); ftype magDecAng = MagDeclination();
// Calculate the innovation // Calculate the innovation
float innovation = atan2f(magE , magN) - magDecAng; ftype innovation = atan2F(magE , magN) - magDecAng;
// limit the innovation to protect against data errors // limit the innovation to protect against data errors
if (innovation > 0.5f) { if (innovation > 0.5f) {
@ -1120,18 +1120,18 @@ void NavEKF2_core::alignMagStateDeclination()
} }
// get the magnetic declination // get the magnetic declination
float magDecAng = MagDeclination(); ftype magDecAng = MagDeclination();
// rotate the NE values so that the declination matches the published value // rotate the NE values so that the declination matches the published value
Vector3f initMagNED = stateStruct.earth_magfield; Vector3F initMagNED = stateStruct.earth_magfield;
float magLengthNE = norm(initMagNED.x,initMagNED.y); ftype magLengthNE = norm(initMagNED.x,initMagNED.y);
stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng); stateStruct.earth_magfield.x = magLengthNE * cosF(magDecAng);
stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng); stateStruct.earth_magfield.y = magLengthNE * sinF(magDecAng);
if (!inhibitMagStates) { if (!inhibitMagStates) {
// zero the corresponding state covariances if magnetic field state learning is active // zero the corresponding state covariances if magnetic field state learning is active
float var_16 = P[16][16]; ftype var_16 = P[16][16];
float var_17 = P[17][17]; ftype var_17 = P[17][17];
zeroRows(P,16,17); zeroRows(P,16,17);
zeroCols(P,16,17); zeroCols(P,16,17);
P[16][16] = var_16; P[16][16] = var_16;
@ -1168,7 +1168,7 @@ bool NavEKF2_core::EKFGSF_resetMainFilterYaw()
return false; return false;
}; };
float yawEKFGSF, yawVarianceEKFGSF, velInnovLength; ftype yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) &&
yawVarianceEKFGSF < sq(radians(15.0f)) && 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 // check if we should use a 321 or 312 Rotation sequence and update the quaternion
// states using the preferred yaw definition // states using the preferred yaw definition
stateStruct.quat.inverse().rotation_matrix(prevTnb); stateStruct.quat.inverse().rotation_matrix(prevTnb);
Vector3f eulerAngles; Vector3F eulerAngles;
if (fabsf(prevTnb[2][0]) < fabsf(prevTnb[2][1])) { if (fabsF(prevTnb[2][0]) < fabsF(prevTnb[2][1])) {
// rolled more than pitched so use 321 rotation order // rolled more than pitched so use 321 rotation order
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
if (isDeltaYaw) { if (isDeltaYaw) {
@ -1234,15 +1234,15 @@ void NavEKF2_core::resetQuatStateYawOnly(float yaw, float yawVariance, bool isDe
// Update the rotation matrix // Update the rotation matrix
stateStruct.quat.inverse().rotation_matrix(prevTnb); 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 // 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); StoreQuatRotate(quat_delta);
// rotate attitude error variances into earth frame // rotate attitude error variances into earth frame
Vector3f bf_variances = Vector3f(P[0][0], P[1][1], P[2][2]); Vector3F bf_variances = Vector3F(P[0][0], P[1][1], P[2][2]);
Vector3f ef_variances = prevTnb.transposed() * bf_variances; Vector3F ef_variances = prevTnb.transposed() * bf_variances;
// reset vertical component to supplied value // reset vertical component to supplied value
ef_variances.z = yawVariance; ef_variances.z = yawVariance;

105
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -132,8 +132,8 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
// reset the accumulated body delta angle and time // reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement // don't do the calculation if not enough time lapsed for a reliable body rate measurement
if (delTimeOF > 0.01f) { if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/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_float((rawGyroRates.y - delAngBodyOF.y/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(); delAngBodyOF.zero();
delTimeOF = 0.0f; delTimeOF = 0.0f;
} }
@ -161,9 +161,9 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
} }
// write uncorrected flow rate measurements // write uncorrected flow rate measurements
// note correction for different axis and sign conventions used by the px4flow sensor // 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 // 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 // write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x; ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y; 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) { compass.last_update_usec(magSelectIndex) - lastMagUpdate_us > 70000) {
// detect changes to magnetometer offset parameters and reset states // 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); bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
if (changeDetected) { if (changeDetected) {
// zero the learned magnetometer bias states // zero the learned magnetometer bias states
@ -302,7 +302,7 @@ void NavEKF2_core::readMagData()
magDataNew.time_ms -= localFilterTimeStep_ms/2; magDataNew.time_ms -= localFilterTimeStep_ms/2;
// read compass data and scale to improve numerical conditioning // 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 // check for consistent data between magnetometers
consistentMagData = compass.consistent(); consistentMagData = compass.consistent();
@ -373,7 +373,7 @@ void NavEKF2_core::readIMUData()
learnInactiveBiases(); learnInactiveBiases();
readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT); 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; imuDataNew.accel_index = accel_index_active;
// Get delta angle data from primary gyro or primary if not available // Get delta angle data from primary gyro or primary if not available
@ -399,7 +399,7 @@ void NavEKF2_core::readIMUData()
imuQuatDownSampleNew.normalize(); imuQuatDownSampleNew.normalize();
// Rotate the latest delta velocity into body frame at the start of accumulation // Rotate the latest delta velocity into body frame at the start of accumulation
Matrix3f deltaRotMat; Matrix3F deltaRotMat;
imuQuatDownSampleNew.rotation_matrix(deltaRotMat); imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
// Apply the delta velocity to the delta velocity accumulator // Apply the delta velocity to the delta velocity accumulator
@ -427,7 +427,7 @@ void NavEKF2_core::readIMUData()
storedIMU.push_youngest_element(imuDataDownSampledNew); storedIMU.push_youngest_element(imuDataDownSampledNew);
// calculate the achieved average time step rate for the EKF // 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; dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
// zero the accumulated IMU data and quaternion // zero the accumulated IMU data and quaternion
@ -451,7 +451,7 @@ void NavEKF2_core::readIMUData()
// protect against delta time going to zero // protect against delta time going to zero
// TODO - check if calculations can tolerate 0 // TODO - check if calculations can tolerate 0
float minDT = 0.1f*dtEkfAvg; ftype minDT = 0.1f*dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,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 // read the delta velocity and corresponding time interval from the IMU
// return false if data is not available // 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(); const auto &ins = dal.ins();
if (ins_index < ins.get_accel_count()) { 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 = MAX(dVel_dt,1.0e-4f);
dVel_dt = MIN(dVel_dt,1.0e-1f); dVel_dt = MIN(dVel_dt,1.0e-1f);
return true; return true;
@ -526,11 +530,11 @@ void NavEKF2_core::readGpsData()
gpsDataNew.sensor_idx = gps.primary_sensor(); gpsDataNew.sensor_idx = gps.primary_sensor();
// read the NED velocity from the GPS // 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. // 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 // 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); gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw; float gpsSpdAccRaw;
if (!gps.speed_accuracy(gpsSpdAccRaw)) { if (!gps.speed_accuracy(gpsSpdAccRaw)) {
@ -625,7 +629,7 @@ void NavEKF2_core::readGpsData()
if (gpsGoodToAlign && !have_table_earth_field) { if (gpsGoodToAlign && !have_table_earth_field) {
const auto *compass = dal.get_compass(); const auto *compass = dal.get_compass();
if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) { 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, table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
gpsloc.lng*1.0e-7)); gpsloc.lng*1.0e-7));
have_table_earth_field = true; 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 // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) { 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) { if ((frontend->_originHgtMode & (1<<2)) == 0) {
gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt); gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
} else { } else {
@ -659,11 +663,15 @@ void NavEKF2_core::readGpsData()
// read the delta angle and corresponding time interval from the IMU // read the delta angle and corresponding time interval from the IMU
// return false if data is not available // 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(); const auto &ins = dal.ins();
if (ins_index < ins.get_gyro_count()) { 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 = MAX(dAng_dt,1.0e-4f);
dAng_dt = MIN(dAng_dt,1.0e-1f); dAng_dt = MIN(dAng_dt,1.0e-1f);
return true; return true;
@ -715,7 +723,7 @@ void NavEKF2_core::readBaroData()
void NavEKF2_core::calcFiltBaroOffset() void NavEKF2_core::calcFiltBaroOffset()
{ {
// Apply a first order LPF with spike protection // 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. // 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 // 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 // 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) { if (activeHgtSource == HGT_SOURCE_BARO) {
// Use the baro drift rate // Use the baro drift rate
const float baroDriftRate = 0.05f; const ftype baroDriftRate = 0.05f;
ekfOriginHgtVar += sq(baroDriftRate * deltaTime); ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
} else if (activeHgtSource == HGT_SOURCE_RNG) { } else if (activeHgtSource == HGT_SOURCE_RNG) {
// use the worse case expected terrain gradient and vehicle horizontal speed // 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); ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
} else { } else {
// by definition our height source is absolute so cannot run this filter // 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 // calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
// when not using GPS as height source // 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 // calculate the correction gain
float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar); ftype gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
// calculate the innovation // calculate the innovation
float innovation = - stateStruct.position.z - gpsDataDelayed.hgt; ftype innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
// check the innovation variance ratio // 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 // correct the EKF origin and variance estimate if the innovation is less than 5-sigma
if (ratio < 25.0f && gpsAccuracyGood) { if (ratio < 25.0f && gpsAccuracyGood) {
@ -837,7 +845,7 @@ void NavEKF2_core::readRngBcnData()
rngBcnDataNew.rng = beacon->beacon_distance(index); rngBcnDataNew.rng = beacon->beacon_distance(index);
// set the beacon position // set the beacon position
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index); rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();
// identify the beacon identifier // identify the beacon identifier
rngBcnDataNew.beacon_ID = index; rngBcnDataNew.beacon_ID = index;
@ -851,15 +859,20 @@ void NavEKF2_core::readRngBcnData()
} }
// Check if the beacon system has returned a 3D fix // 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; rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
} }
beaconVehiclePosNED = beaconVehiclePosNEDF.toftype();
beaconVehiclePosErr = beaconVehiclePosErrF;
// Check if the range beacon data can be used to align the vehicle position // Check if the range beacon data can be used to align the vehicle position
if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) { 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 // 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); ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
if (posDiffSq < 9.0f*posDiffVar) { if (posDiffSq < 9.0f*posDiffVar) {
rngBcnGoodToAlign = true; rngBcnGoodToAlign = true;
// Set the EKF origin and magnetic field declination if not previously set // 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.posReset = false;
} }
extNavDataNew.pos = pos; extNavDataNew.pos = pos.toftype();
extNavDataNew.quat = quat; extNavDataNew.quat = quat.toftype();
extNavDataNew.posErr = posErr; extNavDataNew.posErr = posErr;
extNavDataNew.angErr = angErr; extNavDataNew.angErr = angErr;
timeStamp_ms = timeStamp_ms - delay_ms; timeStamp_ms = timeStamp_ms - delay_ms;
@ -960,7 +973,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
/* /*
return declination in radians 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 // if we are using the WMM tables then use the table declination
// to ensure consistency with the table mag field. Otherwise use // 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 // get filtered gyro and use the difference between the
// corrected gyro on the active IMU and the inactive IMU // corrected gyro on the active IMU and the inactive IMU
// to move the inactive bias towards the right value // 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_active = ins.get_gyro(gyro_index_active).toftype() - (stateStruct.gyro_bias/dtEkfAvg);
Vector3f filtered_gyro_inactive = ins.get_gyro(i) - (inactiveBias[i].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; Vector3F error = filtered_gyro_active - filtered_gyro_inactive;
// prevent a single large error from contaminating bias estimate // prevent a single large error from contaminating bias estimate
const float bias_limit = radians(5); const ftype bias_limit = radians(5);
error.x = constrain_float(error.x, -bias_limit, bias_limit); error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
error.y = constrain_float(error.y, -bias_limit, bias_limit); error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
error.z = constrain_float(error.z, -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 // slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
// gyro bias error in around 1 minute // 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 // get filtered accel and use the difference between the
// corrected accel on the active IMU and the inactive IMU // corrected accel on the active IMU and the inactive IMU
// to move the inactive bias towards the right value // to move the inactive bias towards the right value
float filtered_accel_active = ins.get_accel(accel_index_active).z - (stateStruct.accel_zbias/dtEkfAvg); ftype 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); ftype filtered_accel_inactive = ins.get_accel(i).z - (inactiveBias[i].accel_zbias/dtEkfAvg);
float error = filtered_accel_active - filtered_accel_inactive; ftype error = filtered_accel_active - filtered_accel_inactive;
// prevent a single large error from contaminating bias estimate // prevent a single large error from contaminating bias estimate
const float bias_limit = 1; // m/s/s const ftype bias_limit = 1; // m/s/s
error = constrain_float(error, -bias_limit, bias_limit); error = constrain_ftype(error, -bias_limit, bias_limit);
// slowly bring the inactive accel in line with the active accel // slowly bring the inactive accel in line with the active accel
// this learns 0.5m/s/s bias in about 1 minute // 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; extNavVelMeasTime_ms = timeStamp_ms;
useExtNavVel = true; useExtNavVel = true;
extNavVelNew.vel = vel; extNavVelNew.vel = vel.toftype();
extNavVelNew.err = err; extNavVelNew.err = err;
timeStamp_ms = timeStamp_ms - delay_ms; timeStamp_ms = timeStamp_ms - delay_ms;
// Correct for the average intersampling delay due to the filter updaterate // Correct for the average intersampling delay due to the filter updaterate

462
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -69,7 +69,7 @@ Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scri
void NavEKF2_core::EstimateTerrainOffset() void NavEKF2_core::EstimateTerrainOffset()
{ {
// horizontal velocity squared // 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 LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
// don't fuse flow data if it exceeds validity limits // 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 // 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 // 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); distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
prevPosN = stateStruct.position[0]; prevPosN = stateStruct.position[0];
prevPosE = stateStruct.position[1]; 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 // 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); ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5]; ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5];
Popt += Pincrement; Popt += Pincrement;
timeAtLastAuxEKF_ms = imuSampleTime_ms; timeAtLastAuxEKF_ms = imuSampleTime_ms;
// fuse range finder data // fuse range finder data
if (rangeDataToFuse) { if (rangeDataToFuse) {
// predict range // 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 // Copy required states to local variable names
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
float q3 = stateStruct.quat[3]; // 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 // 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 // calculate Kalman gain
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3); ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))); ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
// Calculate the innovation variance for data logging // Calculate the innovation variance for data logging
varInnovRng = (R_RNG + Popt/sq(SK_RNG)); varInnovRng = (R_RNG + Popt/sq(SK_RNG));
@ -129,7 +129,7 @@ void NavEKF2_core::EstimateTerrainOffset()
innovRng = predRngMeas - rangeDataDelayed.rng; innovRng = predRngMeas - rangeDataDelayed.rng;
// calculate the innovation consistency test ratio // 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 // Check the innovation test ratio and don't fuse if too large
if (auxRngTestRatio < 1.0f) { if (auxRngTestRatio < 1.0f) {
@ -150,18 +150,18 @@ void NavEKF2_core::EstimateTerrainOffset()
if (!cantFuseFlowData) { if (!cantFuseFlowData) {
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes Vector3F relVelSensor; // velocity of sensor relative to ground in sensor axes
Vector2f losPred; // predicted optical flow angular rate measurement Vector2F losPred; // predicted optical flow angular rate measurement
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
float K_OPT; ftype K_OPT;
float H_OPT; ftype H_OPT;
Vector2f auxFlowObsInnovVar; Vector2F auxFlowObsInnovVar;
// predict range to centre of image // 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 // constrain terrain height to be below the vehicle
terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
@ -178,20 +178,20 @@ void NavEKF2_core::EstimateTerrainOffset()
auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp; auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp;
// calculate observation jacobians // calculate observation jacobians
float t2 = q0*q0; ftype t2 = q0*q0;
float t3 = q1*q1; ftype t3 = q1*q1;
float t4 = q2*q2; ftype t4 = q2*q2;
float t5 = q3*q3; ftype t5 = q3*q3;
float t6 = stateStruct.position.z - terrainState; ftype t6 = stateStruct.position.z - terrainState;
float t7 = 1.0f / (t6*t6); ftype t7 = 1.0f / (t6*t6);
float t8 = q0*q3*2.0f; ftype t8 = q0*q3*2.0f;
float t9 = t2-t3-t4+t5; ftype t9 = t2-t3-t4+t5;
// prevent the state variances from becoming badly conditioned // prevent the state variances from becoming badly conditioned
Popt = MAX(Popt,1E-6f); Popt = MAX(Popt,1E-6f);
// calculate observation noise variance from parameter // 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 // Fuse Y axis data
@ -205,7 +205,7 @@ void NavEKF2_core::EstimateTerrainOffset()
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y; K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y;
// calculate the innovation consistency test ratio // 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 // don't fuse if optical flow data is outside valid range
if (auxFlowTestRatio.y < 1.0f) { if (auxFlowTestRatio.y < 1.0f) {
@ -237,7 +237,7 @@ void NavEKF2_core::EstimateTerrainOffset()
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x; K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x;
// calculate the innovation consistency test ratio // 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 // don't fuse if optical flow data is outside valid range
if (auxFlowTestRatio.x < 1.0f) { if (auxFlowTestRatio.x < 1.0f) {
@ -267,23 +267,23 @@ void NavEKF2_core::EstimateTerrainOffset()
void NavEKF2_core::FuseOptFlow() void NavEKF2_core::FuseOptFlow()
{ {
Vector24 H_LOS; Vector24 H_LOS;
Vector3f relVelSensor; Vector3F relVelSensor;
Vector14 SH_LOS; Vector14 SH_LOS;
Vector2 losPred; Vector2 losPred;
// Copy required states to local variable names // Copy required states to local variable names
float q0 = stateStruct.quat[0]; ftype q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1]; ftype q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2]; ftype q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3]; ftype q3 = stateStruct.quat[3];
float vn = stateStruct.velocity.x; ftype vn = stateStruct.velocity.x;
float ve = stateStruct.velocity.y; ftype ve = stateStruct.velocity.y;
float vd = stateStruct.velocity.z; ftype vd = stateStruct.velocity.z;
float pd = stateStruct.position.z; ftype pd = stateStruct.position.z;
// constrain height above ground to be above range measured on ground // constrain height above ground to be above range measured on ground
float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd); ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
float ptd = pd + heightAboveGndEst; ftype ptd = pd + heightAboveGndEst;
// Calculate common expressions for observation jacobians // Calculate common expressions for observation jacobians
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); 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 // 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 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 // 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 // correct range for flow sensor offset body frame position offset
// the corrected value is the predicted range from the sensor focal point to the // the corrected value is the predicted range from the sensor focal point to the
// centre of the image on the ground assuming flat terrain // 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()) { if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
range -= posOffsetEarth.z / prevTnb.c.z; 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[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[6];
H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13]; H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13];
float t2 = SH_LOS[3]; ftype t2 = SH_LOS[3];
float t3 = SH_LOS[0]; ftype t3 = SH_LOS[0];
float t4 = SH_LOS[2]; ftype t4 = SH_LOS[2];
float t5 = SH_LOS[6]; ftype t5 = SH_LOS[6];
float t100 = t2 * t3 * t5; ftype t100 = t2 * t3 * t5;
float t6 = SH_LOS[4]; ftype t6 = SH_LOS[4];
float t7 = t2*t3*t6; ftype t7 = t2*t3*t6;
float t9 = t2*t4*t5; ftype t9 = t2*t4*t5;
float t8 = t7-t9; ftype t8 = t7-t9;
float t10 = q0*q3*2.0f; ftype t10 = q0*q3*2.0f;
float t21 = q1*q2*2.0f; ftype t21 = q1*q2*2.0f;
float t11 = t10-t21; ftype t11 = t10-t21;
float t101 = t2 * t3 * t11; ftype t101 = t2 * t3 * t11;
float t12 = pd-ptd; ftype t12 = pd-ptd;
float t13 = 1.0f/(t12*t12); ftype t13 = 1.0f/(t12*t12);
float t104 = t3 * t4 * t13; ftype t104 = t3 * t4 * t13;
float t14 = SH_LOS[5]; ftype t14 = SH_LOS[5];
float t102 = t2 * t4 * t14; ftype t102 = t2 * t4 * t14;
float t15 = SH_LOS[1]; ftype t15 = SH_LOS[1];
float t103 = t2 * t3 * t15; ftype t103 = t2 * t3 * t15;
float t16 = q0*q0; ftype t16 = q0*q0;
float t17 = q1*q1; ftype t17 = q1*q1;
float t18 = q2*q2; ftype t18 = q2*q2;
float t19 = q3*q3; ftype t19 = q3*q3;
float t20 = t16-t17+t18-t19; ftype t20 = t16-t17+t18-t19;
float t105 = t2 * t3 * t20; ftype t105 = t2 * t3 * t20;
float t22 = P[1][1]*t102; ftype t22 = P[1][1]*t102;
float t23 = P[3][0]*t101; ftype t23 = P[3][0]*t101;
float t24 = P[8][0]*t104; ftype t24 = P[8][0]*t104;
float t25 = P[1][0]*t102; ftype t25 = P[1][0]*t102;
float t26 = P[2][0]*t103; ftype t26 = P[2][0]*t103;
float t63 = P[0][0]*t8; ftype t63 = P[0][0]*t8;
float t64 = P[5][0]*t100; ftype t64 = P[5][0]*t100;
float t65 = P[4][0]*t105; ftype t65 = P[4][0]*t105;
float t27 = t23+t24+t25+t26-t63-t64-t65; ftype t27 = t23+t24+t25+t26-t63-t64-t65;
float t28 = P[3][3]*t101; ftype t28 = P[3][3]*t101;
float t29 = P[8][3]*t104; ftype t29 = P[8][3]*t104;
float t30 = P[1][3]*t102; ftype t30 = P[1][3]*t102;
float t31 = P[2][3]*t103; ftype t31 = P[2][3]*t103;
float t67 = P[0][3]*t8; ftype t67 = P[0][3]*t8;
float t68 = P[5][3]*t100; ftype t68 = P[5][3]*t100;
float t69 = P[4][3]*t105; ftype t69 = P[4][3]*t105;
float t32 = t28+t29+t30+t31-t67-t68-t69; ftype t32 = t28+t29+t30+t31-t67-t68-t69;
float t33 = t101*t32; ftype t33 = t101*t32;
float t34 = P[3][8]*t101; ftype t34 = P[3][8]*t101;
float t35 = P[8][8]*t104; ftype t35 = P[8][8]*t104;
float t36 = P[1][8]*t102; ftype t36 = P[1][8]*t102;
float t37 = P[2][8]*t103; ftype t37 = P[2][8]*t103;
float t70 = P[0][8]*t8; ftype t70 = P[0][8]*t8;
float t71 = P[5][8]*t100; ftype t71 = P[5][8]*t100;
float t72 = P[4][8]*t105; ftype t72 = P[4][8]*t105;
float t38 = t34+t35+t36+t37-t70-t71-t72; ftype t38 = t34+t35+t36+t37-t70-t71-t72;
float t39 = t104*t38; ftype t39 = t104*t38;
float t40 = P[3][1]*t101; ftype t40 = P[3][1]*t101;
float t41 = P[8][1]*t104; ftype t41 = P[8][1]*t104;
float t42 = P[2][1]*t103; ftype t42 = P[2][1]*t103;
float t73 = P[0][1]*t8; ftype t73 = P[0][1]*t8;
float t74 = P[5][1]*t100; ftype t74 = P[5][1]*t100;
float t75 = P[4][1]*t105; ftype t75 = P[4][1]*t105;
float t43 = t22+t40+t41+t42-t73-t74-t75; ftype t43 = t22+t40+t41+t42-t73-t74-t75;
float t44 = t102*t43; ftype t44 = t102*t43;
float t45 = P[3][2]*t101; ftype t45 = P[3][2]*t101;
float t46 = P[8][2]*t104; ftype t46 = P[8][2]*t104;
float t47 = P[1][2]*t102; ftype t47 = P[1][2]*t102;
float t48 = P[2][2]*t103; ftype t48 = P[2][2]*t103;
float t76 = P[0][2]*t8; ftype t76 = P[0][2]*t8;
float t77 = P[5][2]*t100; ftype t77 = P[5][2]*t100;
float t78 = P[4][2]*t105; ftype t78 = P[4][2]*t105;
float t49 = t45+t46+t47+t48-t76-t77-t78; ftype t49 = t45+t46+t47+t48-t76-t77-t78;
float t50 = t103*t49; ftype t50 = t103*t49;
float t51 = P[3][5]*t101; ftype t51 = P[3][5]*t101;
float t52 = P[8][5]*t104; ftype t52 = P[8][5]*t104;
float t53 = P[1][5]*t102; ftype t53 = P[1][5]*t102;
float t54 = P[2][5]*t103; ftype t54 = P[2][5]*t103;
float t79 = P[0][5]*t8; ftype t79 = P[0][5]*t8;
float t80 = P[5][5]*t100; ftype t80 = P[5][5]*t100;
float t81 = P[4][5]*t105; ftype t81 = P[4][5]*t105;
float t55 = t51+t52+t53+t54-t79-t80-t81; ftype t55 = t51+t52+t53+t54-t79-t80-t81;
float t56 = P[3][4]*t101; ftype t56 = P[3][4]*t101;
float t57 = P[8][4]*t104; ftype t57 = P[8][4]*t104;
float t58 = P[1][4]*t102; ftype t58 = P[1][4]*t102;
float t59 = P[2][4]*t103; ftype t59 = P[2][4]*t103;
float t83 = P[0][4]*t8; ftype t83 = P[0][4]*t8;
float t84 = P[5][4]*t100; ftype t84 = P[5][4]*t100;
float t85 = P[4][4]*t105; ftype t85 = P[4][4]*t105;
float t60 = t56+t57+t58+t59-t83-t84-t85; ftype t60 = t56+t57+t58+t59-t83-t84-t85;
float t66 = t8*t27; ftype t66 = t8*t27;
float t82 = t100*t55; ftype t82 = t100*t55;
float t86 = t105*t60; ftype t86 = t105*t60;
float t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86; ftype t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86;
float t62 = 1.0f/t61; ftype t62 = 1.0f/t61;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t61 > R_LOS) { 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[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[5];
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13]; H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13];
float t2 = SH_LOS[3]; ftype t2 = SH_LOS[3];
float t3 = SH_LOS[0]; ftype t3 = SH_LOS[0];
float t4 = SH_LOS[1]; ftype t4 = SH_LOS[1];
float t5 = SH_LOS[5]; ftype t5 = SH_LOS[5];
float t100 = t2 * t3 * t5; ftype t100 = t2 * t3 * t5;
float t6 = SH_LOS[4]; ftype t6 = SH_LOS[4];
float t7 = t2*t3*t6; ftype t7 = t2*t3*t6;
float t8 = t2*t4*t5; ftype t8 = t2*t4*t5;
float t9 = t7+t8; ftype t9 = t7+t8;
float t10 = q0*q3*2.0f; ftype t10 = q0*q3*2.0f;
float t11 = q1*q2*2.0f; ftype t11 = q1*q2*2.0f;
float t12 = t10+t11; ftype t12 = t10+t11;
float t101 = t2 * t3 * t12; ftype t101 = t2 * t3 * t12;
float t13 = pd-ptd; ftype t13 = pd-ptd;
float t14 = 1.0f/(t13*t13); ftype t14 = 1.0f/(t13*t13);
float t104 = t3 * t4 * t14; ftype t104 = t3 * t4 * t14;
float t15 = SH_LOS[6]; ftype t15 = SH_LOS[6];
float t105 = t2 * t4 * t15; ftype t105 = t2 * t4 * t15;
float t16 = SH_LOS[2]; ftype t16 = SH_LOS[2];
float t102 = t2 * t3 * t16; ftype t102 = t2 * t3 * t16;
float t17 = q0*q0; ftype t17 = q0*q0;
float t18 = q1*q1; ftype t18 = q1*q1;
float t19 = q2*q2; ftype t19 = q2*q2;
float t20 = q3*q3; ftype t20 = q3*q3;
float t21 = t17+t18-t19-t20; ftype t21 = t17+t18-t19-t20;
float t103 = t2 * t3 * t21; ftype t103 = t2 * t3 * t21;
float t22 = P[0][0]*t105; ftype t22 = P[0][0]*t105;
float t23 = P[1][1]*t9; ftype t23 = P[1][1]*t9;
float t24 = P[8][1]*t104; ftype t24 = P[8][1]*t104;
float t25 = P[0][1]*t105; ftype t25 = P[0][1]*t105;
float t26 = P[5][1]*t100; ftype t26 = P[5][1]*t100;
float t64 = P[4][1]*t101; ftype t64 = P[4][1]*t101;
float t65 = P[2][1]*t102; ftype t65 = P[2][1]*t102;
float t66 = P[3][1]*t103; ftype t66 = P[3][1]*t103;
float t27 = t23+t24+t25+t26-t64-t65-t66; ftype t27 = t23+t24+t25+t26-t64-t65-t66;
float t28 = t9*t27; ftype t28 = t9*t27;
float t29 = P[1][4]*t9; ftype t29 = P[1][4]*t9;
float t30 = P[8][4]*t104; ftype t30 = P[8][4]*t104;
float t31 = P[0][4]*t105; ftype t31 = P[0][4]*t105;
float t32 = P[5][4]*t100; ftype t32 = P[5][4]*t100;
float t67 = P[4][4]*t101; ftype t67 = P[4][4]*t101;
float t68 = P[2][4]*t102; ftype t68 = P[2][4]*t102;
float t69 = P[3][4]*t103; ftype t69 = P[3][4]*t103;
float t33 = t29+t30+t31+t32-t67-t68-t69; ftype t33 = t29+t30+t31+t32-t67-t68-t69;
float t34 = P[1][8]*t9; ftype t34 = P[1][8]*t9;
float t35 = P[8][8]*t104; ftype t35 = P[8][8]*t104;
float t36 = P[0][8]*t105; ftype t36 = P[0][8]*t105;
float t37 = P[5][8]*t100; ftype t37 = P[5][8]*t100;
float t71 = P[4][8]*t101; ftype t71 = P[4][8]*t101;
float t72 = P[2][8]*t102; ftype t72 = P[2][8]*t102;
float t73 = P[3][8]*t103; ftype t73 = P[3][8]*t103;
float t38 = t34+t35+t36+t37-t71-t72-t73; ftype t38 = t34+t35+t36+t37-t71-t72-t73;
float t39 = t104*t38; ftype t39 = t104*t38;
float t40 = P[1][0]*t9; ftype t40 = P[1][0]*t9;
float t41 = P[8][0]*t104; ftype t41 = P[8][0]*t104;
float t42 = P[5][0]*t100; ftype t42 = P[5][0]*t100;
float t74 = P[4][0]*t101; ftype t74 = P[4][0]*t101;
float t75 = P[2][0]*t102; ftype t75 = P[2][0]*t102;
float t76 = P[3][0]*t103; ftype t76 = P[3][0]*t103;
float t43 = t22+t40+t41+t42-t74-t75-t76; ftype t43 = t22+t40+t41+t42-t74-t75-t76;
float t44 = t105*t43; ftype t44 = t105*t43;
float t45 = P[1][2]*t9; ftype t45 = P[1][2]*t9;
float t46 = P[8][2]*t104; ftype t46 = P[8][2]*t104;
float t47 = P[0][2]*t105; ftype t47 = P[0][2]*t105;
float t48 = P[5][2]*t100; ftype t48 = P[5][2]*t100;
float t63 = P[2][2]*t102; ftype t63 = P[2][2]*t102;
float t77 = P[4][2]*t101; ftype t77 = P[4][2]*t101;
float t78 = P[3][2]*t103; ftype t78 = P[3][2]*t103;
float t49 = t45+t46+t47+t48-t63-t77-t78; ftype t49 = t45+t46+t47+t48-t63-t77-t78;
float t50 = P[1][5]*t9; ftype t50 = P[1][5]*t9;
float t51 = P[8][5]*t104; ftype t51 = P[8][5]*t104;
float t52 = P[0][5]*t105; ftype t52 = P[0][5]*t105;
float t53 = P[5][5]*t100; ftype t53 = P[5][5]*t100;
float t80 = P[4][5]*t101; ftype t80 = P[4][5]*t101;
float t81 = P[2][5]*t102; ftype t81 = P[2][5]*t102;
float t82 = P[3][5]*t103; ftype t82 = P[3][5]*t103;
float t54 = t50+t51+t52+t53-t80-t81-t82; ftype t54 = t50+t51+t52+t53-t80-t81-t82;
float t55 = t100*t54; ftype t55 = t100*t54;
float t56 = P[1][3]*t9; ftype t56 = P[1][3]*t9;
float t57 = P[8][3]*t104; ftype t57 = P[8][3]*t104;
float t58 = P[0][3]*t105; ftype t58 = P[0][3]*t105;
float t59 = P[5][3]*t100; ftype t59 = P[5][3]*t100;
float t83 = P[4][3]*t101; ftype t83 = P[4][3]*t101;
float t84 = P[2][3]*t102; ftype t84 = P[2][3]*t102;
float t85 = P[3][3]*t103; ftype t85 = P[3][3]*t103;
float t60 = t56+t57+t58+t59-t83-t84-t85; ftype t60 = t56+t57+t58+t59-t83-t84-t85;
float t70 = t101*t33; ftype t70 = t101*t33;
float t79 = t102*t49; ftype t79 = t102*t49;
float t86 = t103*t60; ftype t86 = t103*t60;
float t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86; ftype t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86;
float t62 = 1.0f/t61; ftype t62 = 1.0f/t61;
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if (t61 > R_LOS) { if (t61 > R_LOS) {
@ -633,7 +633,7 @@ void NavEKF2_core::FuseOptFlow()
} }
// calculate the innovation consistency test ratio // 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 // 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)) { if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {

52
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -25,8 +25,8 @@ bool NavEKF2_core::healthy(void) const
return false; return false;
} }
// position and height innovations must be within limits when on-ground and in a static mode of operation // 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]); ftype horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) { if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
return false; return false;
} }
@ -36,16 +36,16 @@ bool NavEKF2_core::healthy(void) const
// Return a consolidated error score where higher numbers represent larger errors // 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 // 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) { if (tiltAlignComplete && yawAlignComplete) {
// Check GPS fusion performance // Check GPS fusion performance
score = MAX(score, 0.5f * (velTestRatio + posTestRatio)); score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
// Check altimeter fusion performance // Check altimeter fusion performance
score = MAX(score, hgtTestRatio); score = MAX(score, hgtTestRatio);
// Check attitude corrections // Check attitude corrections
const float tiltErrThreshold = 0.05f; const ftype tiltErrThreshold = 0.05f;
score = MAX(score, tiltErrFilt / tiltErrThreshold); score = MAX(score, tiltErrFilt / tiltErrThreshold);
} }
return score; return score;
@ -90,7 +90,7 @@ void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
gyroBias.zero(); gyroBias.zero();
return; return;
} }
gyroBias = stateStruct.gyro_bias / dtEkfAvg; gyroBias = stateStruct.gyro_bias.tofloat() / dtEkfAvg;
} }
// return body axis gyro scale factor error as a percentage // 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 // return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF2_core::getQuaternion(Quaternion& ret) const 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 // 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 // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF2_core::getLastPosNorthEastReset(Vector2f &pos) const uint32_t NavEKF2_core::getLastPosNorthEastReset(Vector2f &pos) const
{ {
pos = posResetNE; pos = posResetNE.tofloat();
return lastPosReset_ms; 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 // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF2_core::getLastVelNorthEastReset(Vector2f &vel) const uint32_t NavEKF2_core::getLastVelNorthEastReset(Vector2f &vel) const
{ {
vel = velResetNE; vel = velResetNE.tofloat();
return lastVelReset_ms; return lastVelReset_ms;
} }
@ -164,7 +164,7 @@ void NavEKF2_core::getWind(Vector3f &wind) const
void NavEKF2_core::getVelNED(Vector3f &vel) const void NavEKF2_core::getVelNED(Vector3f &vel) const
{ {
// correct for the IMU position offset (EKF calculations are at the IMU) // 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 // 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) { if (inhibitWindStates || PV_AidingMode == AID_NONE) {
return false; return false;
} }
vel = outputDataNew.velocity + velOffsetNED; vel = (outputDataNew.velocity + velOffsetNED).tofloat();
vel.x -= stateStruct.wind_vel.x; vel.x -= stateStruct.wind_vel.x;
vel.y -= stateStruct.wind_vel.y; vel.y -= stateStruct.wind_vel.y;
Matrix3f Tnb; // rotation from nav to body frame 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 ((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 // 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 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.x = tempPosNE.x;
posNE.y = tempPosNE.y; posNE.y = tempPosNE.y;
return false; return false;
@ -364,13 +364,13 @@ bool NavEKF2_core::getOriginLLH(struct Location &loc) const
// return earth magnetic field estimates in measurement units / 1000 // return earth magnetic field estimates in measurement units / 1000
void NavEKF2_core::getMagNED(Vector3f &magNED) const 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 // return body magnetic field estimates in measurement units / 1000
void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
{ {
magXYZ = stateStruct.body_magfield*1000.0f; magXYZ = (stateStruct.body_magfield*1000.0).tofloat();
} }
// return magnetometer offsets // 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, // 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 // 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); bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
if ((mag_idx == magSelectIndex) && if ((mag_idx == magSelectIndex) &&
finalInflightMagInit && finalInflightMagInit &&
!inhibitMagStates && !inhibitMagStates &&
dal.get_compass()->healthy(magSelectIndex) && dal.get_compass()->healthy(magSelectIndex) &&
variancesConverged) { 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; return true;
} else { } else {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex); 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 // 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 void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{ {
velVar = sqrtf(velTestRatio); velVar = sqrtF(velTestRatio);
posVar = sqrtf(posTestRatio); posVar = sqrtF(posTestRatio);
hgtVar = sqrtf(hgtTestRatio); 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 // 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.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio)); magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio)); magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtf(tasTestRatio); tasVar = sqrtF(tasTestRatio);
offset = posResetNE; offset = posResetNE.tofloat();
} }
@ -537,14 +537,14 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) const
Vector2f offset; Vector2f offset;
getVariances(velVar, posVar, hgtVar, magVar, tasVar, 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 // 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 // height estimation or optical flow operation. This prevents false alarms at the GCS if a
// range finder is fitted for other applications // range finder is fitted for other applications
float temp; float temp;
if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
temp = sqrtf(auxRngTestRatio); temp = sqrtF(auxRngTestRatio);
} else { } else {
temp = 0.0f; temp = 0.0f;
} }

116
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -144,10 +144,10 @@ void NavEKF2_core::ResetPosition(void)
// posResetNE is updated to hold the change in position // posResetNE is updated to hold the change in position
// storedOutput, outputDataNew and outputDataDelayed are updated with 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 // 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 // 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 // Set the position states to the new position
stateStruct.position.x = posN; stateStruct.position.x = posN;
@ -245,10 +245,10 @@ void NavEKF2_core::ResetHeight(void)
// posResetD is updated to hold the change in position // posResetD is updated to hold the change in position
// storedOutput, outputDataNew and outputDataDelayed are updated with 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 // 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 // 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 // write to the state vector
stateStruct.position.z = posD; stateStruct.position.z = posD;
@ -279,7 +279,7 @@ bool NavEKF2_core::resetHeightDatum(void)
return false; return false;
} }
// record the old height estimate // 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 // reset the barometer so that it reads zero at the current height
dal.baro().update_calibration(); dal.baro().update_calibration();
// reset the height state // reset the height state
@ -313,7 +313,7 @@ bool NavEKF2_core::resetHeightDatum(void)
*/ */
void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const 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()) { if (posOffsetBody.is_zero()) {
return; 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 // Don't fuse velocity data if GPS doesn't support it
if (fuseVelData) { if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay // TODO use a filtered angular rate with a group delay that matches the GPS delay
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
Vector3f velOffsetBody = angRate % posOffsetBody; Vector3F velOffsetBody = angRate % posOffsetBody;
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); Vector3F velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
gps_data.vel.x -= velOffsetEarth.x; gps_data.vel.x -= velOffsetEarth.x;
gps_data.vel.y -= velOffsetEarth.y; gps_data.vel.y -= velOffsetEarth.y;
gps_data.vel.z -= velOffsetEarth.z; 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.x -= posOffsetEarth.x;
gps_data.pos.y -= posOffsetEarth.y; gps_data.pos.y -= posOffsetEarth.y;
gps_data.hgt += posOffsetEarth.z; gps_data.hgt += posOffsetEarth.z;
} }
// correct external navigation earth-frame position using sensor body-frame offset // 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 #if HAL_VISUALODOM_ENABLED
const auto *visual_odom = dal.visualodom(); const auto *visual_odom = dal.visualodom();
if (visual_odom == nullptr) { if (visual_odom == nullptr) {
return; return;
} }
const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset; const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
if (posOffsetBody.is_zero()) { if (posOffsetBody.is_zero()) {
return; return;
} }
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
ext_position.x -= posOffsetEarth.x; ext_position.x -= posOffsetEarth.x;
ext_position.y -= posOffsetEarth.y; ext_position.y -= posOffsetEarth.y;
ext_position.z -= posOffsetEarth.z; 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 // 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 #if HAL_VISUALODOM_ENABLED
const auto *visual_odom = dal.visualodom(); const auto *visual_odom = dal.visualodom();
if (visual_odom == nullptr) { if (visual_odom == nullptr) {
return; return;
} }
const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset; const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
if (posOffsetBody.is_zero()) { if (posOffsetBody.is_zero()) {
return; return;
} }
// TODO use a filtered angular rate with a group delay that matches the sensor delay // 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); ext_velocity += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
#endif #endif
} }
@ -565,7 +565,7 @@ void NavEKF2_core::FuseVelPosNED()
bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check
// declare variables used to check measurement errors // declare variables used to check measurement errors
Vector3f velInnov; Vector3F velInnov;
// declare variables used to control access to arrays // declare variables used to control access to arrays
bool fuseData[6] = {false,false,false,false,false,false}; 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 // declare variables used by state and covariance update calculations
Vector6 R_OBS; // Measurement variances used for fusion Vector6 R_OBS; // Measurement variances used for fusion
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only 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 // perform sequential fusion of GPS measurements. This assumes that the
// errors in the different velocity and position components are // errors in the different velocity and position components are
@ -586,14 +586,14 @@ void NavEKF2_core::FuseVelPosNED()
if (fuseVelData || fusePosData || fuseHgtData) { if (fuseVelData || fusePosData || fuseHgtData) {
// calculate additional error in GPS position caused by manoeuvring // 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. // 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 // Use different errors if operating without external aiding using an assumed position or velocity of zero
if (PV_AidingMode == AID_NONE) { if (PV_AidingMode == AID_NONE) {
if (tiltAlignComplete && motorsArmed) { if (tiltAlignComplete && motorsArmed) {
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate // 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 { } else {
// Use a smaller value to give faster initial alignment // Use a smaller value to give faster initial alignment
R_OBS[0] = sq(0.5f); R_OBS[0] = sq(0.5f);
@ -606,33 +606,33 @@ void NavEKF2_core::FuseVelPosNED()
} else { } else {
if (gpsSpdAccuracy > 0.0f) { if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter // 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[0] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f)); R_OBS[2] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
} else if (extNavVelToFuse) { } 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 { } else {
// calculate additional error in GPS velocity caused by manoeuvring // 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[0] = sq(constrain_ftype(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[2] = sq(constrain_ftype(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
} }
R_OBS[1] = R_OBS[0]; R_OBS[1] = R_OBS[0];
// Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter // Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
if (gpsPosAccuracy > 0.0f) { 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) { } 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 { } 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]; 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 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 // 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 // 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) { 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 { } 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; 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. // the accelerometers and we should disable the GPS and barometer innovation consistency checks.
if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) { if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
// calculate innovations for height and vertical GPS vel measurements // calculate innovations for height and vertical GPS vel measurements
float hgtErr = stateStruct.position.z - velPosObs[5]; ftype hgtErr = stateStruct.position.z - velPosObs[5];
float velDErr = stateStruct.velocity.z - velPosObs[2]; ftype velDErr = stateStruct.velocity.z - velPosObs[2];
// check if they are the same sign and both more than 3-sigma out of bounds // 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]))) { 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; badIMUdata = true;
@ -663,7 +663,7 @@ void NavEKF2_core::FuseVelPosNED()
varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3]; varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4]; varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
// apply an innovation consistency threshold test, but don't fail if bad IMU data // 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; posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata); posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// use position data if healthy or timed out // use position data if healthy or timed out
@ -703,8 +703,8 @@ void NavEKF2_core::FuseVelPosNED()
!dal.gps().have_vertical_velocity())) { !dal.gps().have_vertical_velocity())) {
imax = 1; imax = 1;
} }
float innovVelSumSq = 0; // sum of squares of velocity innovations ftype innovVelSumSq = 0; // sum of squares of velocity innovations
float varVelSum = 0; // sum of velocity innovation variances ftype varVelSum = 0; // sum of velocity innovation variances
for (uint8_t i = 0; i<=imax; i++) { for (uint8_t i = 0; i<=imax; i++) {
// velocity states start at index 3 // velocity states start at index 3
stateIndex = i + 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 // apply an innovation consistency threshold test, but don't fail if bad IMU data
// calculate the test ratio // 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 // fail if the ratio is greater than 1
velHealth = ((velTestRatio < 1.0f) || badIMUdata); velHealth = ((velTestRatio < 1.0f) || badIMUdata);
// use velocity data if healthy, timed out, or in constant position mode // 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]; innovVelPos[5] = stateStruct.position.z - velPosObs[5];
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio // 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 // when on ground we accept a larger test ratio to allow
// the filter to handle large switch on IMU bias errors // the filter to handle large switch on IMU bias errors
// without rejecting the height sensor // 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 // fail if the ratio is > maxTestRatio, but don't fail if bad IMU data
hgtHealth = (hgtTestRatio < maxTestRatio) || badIMUdata; hgtHealth = (hgtTestRatio < maxTestRatio) || badIMUdata;
@ -759,9 +759,9 @@ void NavEKF2_core::FuseVelPosNED()
// Calculate a filtered value to be used by pre-flight health checks // 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 // 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) { if (onGround) {
float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f; ftype dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
const float hgtInnovFiltTC = 2.0f; const ftype hgtInnovFiltTC = 2.0f;
float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f); ftype alpha = constrain_ftype(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha; hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
} else { } else {
hgtInnovFiltState = 0.0f; hgtInnovFiltState = 0.0f;
@ -812,8 +812,8 @@ void NavEKF2_core::FuseVelPosNED()
R_OBS[obsIndex] *= sq(gpsNoiseScaler); R_OBS[obsIndex] *= sq(gpsNoiseScaler);
} else if (obsIndex == 5) { } else if (obsIndex == 5) {
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex]; innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
const float gndMaxBaroErr = 4.0f; const ftype gndMaxBaroErr = 4.0f;
const float gndBaroInnovFloor = -0.5f; const ftype gndBaroInnovFloor = -0.5f;
if(dal.get_touchdown_expected() && activeHgtSource == HGT_SOURCE_BARO) { if(dal.get_touchdown_expected() && activeHgtSource == HGT_SOURCE_BARO) {
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor // 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) { if (_rng && rangeDataToFuse) {
const auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx); const auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
if (sensor != nullptr) { if (sensor != nullptr) {
Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset; Vector3F posOffsetBody = sensor->get_pos_offset().toftype() - accelPosOffset;
if (!posOffsetBody.is_zero()) { if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z; rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
} }
} }
@ -976,16 +976,16 @@ void NavEKF2_core::selectHeightForFusion()
activeHgtSource = HGT_SOURCE_RNG; activeHgtSource = HGT_SOURCE_RNG;
} else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) { } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) {
// determine if we are above or below the height switch region // 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 aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * 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 // 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 // used as a height reference in combination with a range finder
// apply a hysteresis to the speed check to prevent rapid switching // 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; 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; 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 // filtered baro data used to provide a reference for takeoff
// it is is reset to last height measurement on disarming in performArmingChecks() // it is is reset to last height measurement on disarming in performArmingChecks()
if (!dal.get_takeoff_expected()) { if (!dal.get_takeoff_expected()) {
const float gndHgtFiltTC = 0.5f; const ftype gndHgtFiltTC = 0.5f;
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f; const ftype dtBaro = frontend->hgtAvg_ms*1.0e-3;
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f); ftype alpha = constrain_ftype(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha; meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
} }
} }
@ -1050,7 +1050,7 @@ void NavEKF2_core::selectHeightForFusion()
// Select the height measurement source // Select the height measurement source
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) { if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
hgtMea = -extNavDataDelayed.pos.z; 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)) { } else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
// using range finder data // using range finder data
// correct for tilt using a flat earth model // correct for tilt using a flat earth model
@ -1063,7 +1063,7 @@ void NavEKF2_core::selectHeightForFusion()
fuseHgtData = true; fuseHgtData = true;
velPosObs[5] = -hgtMea; velPosObs[5] = -hgtMea;
// set the observation noise // 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 // add uncertainty created by terrain gradient and vehicle tilt
posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z))); posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
} else { } else {
@ -1078,9 +1078,9 @@ void NavEKF2_core::selectHeightForFusion()
fuseHgtData = true; fuseHgtData = true;
// set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
if (gpsHgtAccuracy > 0.0f) { 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 { } 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)) { } else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) {
// using Baro data // using Baro data
@ -1089,7 +1089,7 @@ void NavEKF2_core::selectHeightForFusion()
velPosObs[5] = -hgtMea; velPosObs[5] = -hgtMea;
fuseHgtData = true; fuseHgtData = true;
// set the observation noise // 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 // 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()) { if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) {
posDownObsNoise *= frontend->gndEffectBaroScaler; posDownObsNoise *= frontend->gndEffectBaroScaler;

210
libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
View File

@ -26,14 +26,14 @@ void NavEKF2_core::SelectRngBcnFusion()
void NavEKF2_core::FuseRngBcn() void NavEKF2_core::FuseRngBcn()
{ {
// declarations // declarations
float pn; ftype pn;
float pe; ftype pe;
float pd; ftype pd;
float bcn_pn; ftype bcn_pn;
float bcn_pe; ftype bcn_pe;
float bcn_pd; ftype bcn_pd;
const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); const ftype R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
float rngPred; ftype rngPred;
// health is set bad until test passed // health is set bad until test passed
rngBcnHealth = false; rngBcnHealth = false;
@ -54,7 +54,7 @@ void NavEKF2_core::FuseRngBcn()
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffset; bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffset;
// predicted range // predicted range
Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
rngPred = deltaPosNED.length(); rngPred = deltaPosNED.length();
// calculate measurement innovation // calculate measurement innovation
@ -64,37 +64,37 @@ void NavEKF2_core::FuseRngBcn()
if (rngPred > 0.1f) if (rngPred > 0.1f)
{ {
// calculate observation jacobians // calculate observation jacobians
float H_BCN[24] = {}; ftype H_BCN[24] = {};
float t2 = bcn_pd-pd; ftype t2 = bcn_pd-pd;
float t3 = bcn_pe-pe; ftype t3 = bcn_pe-pe;
float t4 = bcn_pn-pn; ftype t4 = bcn_pn-pn;
float t5 = t2*t2; ftype t5 = t2*t2;
float t6 = t3*t3; ftype t6 = t3*t3;
float t7 = t4*t4; ftype t7 = t4*t4;
float t8 = t5+t6+t7; ftype t8 = t5+t6+t7;
float t9 = 1.0f/sqrtf(t8); ftype t9 = 1.0f/sqrtF(t8);
H_BCN[6] = -t4*t9; H_BCN[6] = -t4*t9;
H_BCN[7] = -t3*t9; H_BCN[7] = -t3*t9;
H_BCN[8] = -t2*t9; H_BCN[8] = -t2*t9;
// calculate Kalman gains // calculate Kalman gains
float t10 = P[8][8]*t2*t9; ftype t10 = P[8][8]*t2*t9;
float t11 = P[7][8]*t3*t9; ftype t11 = P[7][8]*t3*t9;
float t12 = P[6][8]*t4*t9; ftype t12 = P[6][8]*t4*t9;
float t13 = t10+t11+t12; ftype t13 = t10+t11+t12;
float t14 = t2*t9*t13; ftype t14 = t2*t9*t13;
float t15 = P[8][7]*t2*t9; ftype t15 = P[8][7]*t2*t9;
float t16 = P[7][7]*t3*t9; ftype t16 = P[7][7]*t3*t9;
float t17 = P[6][7]*t4*t9; ftype t17 = P[6][7]*t4*t9;
float t18 = t15+t16+t17; ftype t18 = t15+t16+t17;
float t19 = t3*t9*t18; ftype t19 = t3*t9*t18;
float t20 = P[8][6]*t2*t9; ftype t20 = P[8][6]*t2*t9;
float t21 = P[7][6]*t3*t9; ftype t21 = P[7][6]*t3*t9;
float t22 = P[6][6]*t4*t9; ftype t22 = P[6][6]*t4*t9;
float t23 = t20+t21+t22; ftype t23 = t20+t21+t22;
float t24 = t4*t9*t23; ftype t24 = t4*t9*t23;
varInnovRngBcn = R_BCN+t14+t19+t24; varInnovRngBcn = R_BCN+t14+t19+t24;
float t26; ftype t26;
if (varInnovRngBcn >= R_BCN) { if (varInnovRngBcn >= R_BCN) {
t26 = 1.0/varInnovRngBcn; t26 = 1.0/varInnovRngBcn;
faultStatus.bad_rngbcn = false; 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[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); Kfusion[21] = -t26*(P[21][6]*t4*t9+P[21][7]*t3*t9+P[21][8]*t2*t9);
} else { } else {
// zero indexes 16 to 21 = 6*4 bytes // zero indexes 16 to 21 = 6
memset(&Kfusion[16], 0, 24); 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[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); 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 // 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; innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
// calculate the innovation consistency test ratio // 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 // fail if the ratio is > 1, but don't fail if bad IMU data
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata); rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
@ -245,7 +245,7 @@ https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon
void NavEKF2_core::FuseRngBcnStatic() void NavEKF2_core::FuseRngBcnStatic()
{ {
// get the estimated range measurement variance // 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 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) { if (numBcnMeas >= 100) {
rngBcnAlignmentStarted = true; 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 // initialise the receiver position to the centre of the beacons and at zero height
receiverPos.x = rngBcnPosSum.x * tempVar; receiverPos.x = rngBcnPosSum.x * tempVar;
receiverPos.y = rngBcnPosSum.y * 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 // 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 // The position estimate should be stable after 100 iterations so we use a simple dual
// hypothesis 1-state EKF to estimate the offset // hypothesis 1-state EKF to estimate the offset
Vector3f refPosNED; Vector3F refPosNED;
refPosNED.x = receiverPos.x; refPosNED.x = receiverPos.x;
refPosNED.y = receiverPos.y; refPosNED.y = receiverPos.y;
refPosNED.z = stateStruct.position.z; refPosNED.z = stateStruct.position.z;
@ -317,14 +317,14 @@ void NavEKF2_core::FuseRngBcnStatic()
// and the main EKF vertical position // and the main EKF vertical position
// Calculate the mid vertical position of all beacons // 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 // 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 // calcuate the two hypothesis for our vertical position
float receverPosDownMax; ftype receverPosDownMax;
float receverPosDownMin; ftype receverPosDownMin;
if (delta >= 0.0f) { if (delta >= 0.0f) {
receverPosDownMax = receiverPos.z; receverPosDownMax = receiverPos.z;
receverPosDownMin = receiverPos.z - 2.0f * delta; receverPosDownMin = receiverPos.z - 2.0f * delta;
@ -347,57 +347,57 @@ void NavEKF2_core::FuseRngBcnStatic()
} }
// calculate the observation jacobian // calculate the observation jacobian
float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset; ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset;
float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y; ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x; ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
float t5 = t2*t2; ftype t5 = t2*t2;
float t6 = t3*t3; ftype t6 = t3*t3;
float t7 = t4*t4; ftype t7 = t4*t4;
float t8 = t5+t6+t7; ftype t8 = t5+t6+t7;
if (t8 < 0.1f) { if (t8 < 0.1f) {
// calculation will be badly conditioned // calculation will be badly conditioned
return; return;
} }
float t9 = 1.0f/sqrtf(t8); ftype t9 = 1.0f/sqrtF(t8);
float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f; ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
float t15 = receiverPos.x*2.0f; ftype t15 = receiverPos.x*2.0f;
float t11 = t10-t15; ftype t11 = t10-t15;
float t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f; ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
float t14 = receiverPos.y*2.0f; ftype t14 = receiverPos.y*2.0f;
float t13 = t12-t14; ftype t13 = t12-t14;
float t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f; ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
float t18 = receiverPos.z*2.0f; ftype t18 = receiverPos.z*2.0f;
float t17 = t16-t18; ftype t17 = t16-t18;
float H_RNG[3]; ftype H_RNG[3];
H_RNG[0] = -t9*t11*0.5f; H_RNG[0] = -t9*t11*0.5f;
H_RNG[1] = -t9*t13*0.5f; H_RNG[1] = -t9*t13*0.5f;
H_RNG[2] = -t9*t17*0.5f; H_RNG[2] = -t9*t17*0.5f;
// calculate the Kalman gains // calculate the Kalman gains
float t19 = receiverPosCov[0][0]*t9*t11*0.5f; ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f;
float t20 = receiverPosCov[1][1]*t9*t13*0.5f; ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f;
float t21 = receiverPosCov[0][1]*t9*t11*0.5f; ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f;
float t22 = receiverPosCov[2][1]*t9*t17*0.5f; ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f;
float t23 = t20+t21+t22; ftype t23 = t20+t21+t22;
float t24 = t9*t13*t23*0.5f; ftype t24 = t9*t13*t23*0.5f;
float t25 = receiverPosCov[1][2]*t9*t13*0.5f; ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f;
float t26 = receiverPosCov[0][2]*t9*t11*0.5f; ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f;
float t27 = receiverPosCov[2][2]*t9*t17*0.5f; ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f;
float t28 = t25+t26+t27; ftype t28 = t25+t26+t27;
float t29 = t9*t17*t28*0.5f; ftype t29 = t9*t17*t28*0.5f;
float t30 = receiverPosCov[1][0]*t9*t13*0.5f; ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f;
float t31 = receiverPosCov[2][0]*t9*t17*0.5f; ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f;
float t32 = t19+t30+t31; ftype t32 = t19+t30+t31;
float t33 = t9*t11*t32*0.5f; ftype t33 = t9*t11*t32*0.5f;
varInnovRngBcn = R_RNG+t24+t29+t33; varInnovRngBcn = R_RNG+t24+t29+t33;
float t35 = 1.0f/varInnovRngBcn; ftype t35 = 1.0f/varInnovRngBcn;
float K_RNG[3]; 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[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[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); K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f);
// calculate range measurement innovation // calculate range measurement innovation
Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED; Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
deltaPosNED.z -= bcnPosOffset; deltaPosNED.z -= bcnPosOffset;
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng; innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
@ -440,7 +440,7 @@ void NavEKF2_core::FuseRngBcnStatic()
// ensure the covariance matrix is symmetric // ensure the covariance matrix is symmetric
for (uint8_t i=1; i<=2; i++) { for (uint8_t i=1; i<=2; i++) {
for (uint8_t j=0; j<=i-1; j++) { 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[i][j] = temp;
receiverPosCov[j][i] = 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 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 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 // 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 // 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 // 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 // The main filter then uses the offset with the smaller innovations
float innov; // range measurement innovation (m) ftype innov; // range measurement innovation (m)
float innovVar; // range measurement innovation variance (m^2) ftype innovVar; // range measurement innovation variance (m^2)
float gain; // Kalman gain ftype gain; // Kalman gain
float obsDeriv; // derivative of observation relative to state ftype obsDeriv; // derivative of observation relative to state
const float stateNoiseVar = 0.1f; // State process noise variance const ftype stateNoiseVar = 0.1f; // State process noise variance
const float filtAlpha = 0.01f; // LPF constant const ftype filtAlpha = 0.01f; // LPF constant
const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std const ftype innovGateWidth = 5.0f; // width of innovation consistency check gate in std
// estimate upper value for offset // estimate upper value for offset
// calculate observation derivative // calculate observation derivative
float t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosOffsetMax; ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosOffsetMax;
float t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y; ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x; ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
float t5 = t2*t2; ftype t5 = t2*t2;
float t6 = t3*t3; ftype t6 = t3*t3;
float t7 = t4*t4; ftype t7 = t4*t4;
float t8 = t5+t6+t7; ftype t8 = t5+t6+t7;
float t9; ftype t9;
if (t8 > 0.1f) { if (t8 > 0.1f) {
t9 = 1.0f/sqrtf(t8); t9 = 1.0f/sqrtF(t8);
obsDeriv = t2*t9; obsDeriv = t2*t9;
// Calculate innovation // 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 // 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 // covariance prediction
bcnPosOffsetMaxVar += stateNoiseVar; bcnPosOffsetMaxVar += stateNoiseVar;
@ -524,14 +524,14 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
t5 = t2*t2; t5 = t2*t2;
t8 = t5+t6+t7; t8 = t5+t6+t7;
if (t8 > 0.1f) { if (t8 > 0.1f) {
t9 = 1.0f/sqrtf(t8); t9 = 1.0f/sqrtF(t8);
obsDeriv = t2*t9; obsDeriv = t2*t9;
// Calculate innovation // 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 // 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 // covariance prediction
bcnPosOffsetMinVar += stateNoiseVar; bcnPosOffsetMinVar += stateNoiseVar;
@ -555,7 +555,7 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
} }
// calculate the mid vertical position of all beacons // 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 // 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); bcnPosOffsetMax = MAX(bcnPosOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);

32
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -28,7 +28,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
} }
// User defined multiplier to be applied to check thresholds // 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) { if (gpsGoodToAlign) {
/* /*
@ -55,9 +55,9 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Check for significant change in GPS position if disarmed which indicates bad GPS // Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary // This check can only be used when the vehicle is stationary
const struct Location &gpsloc = gps.location(); // Current location 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 // 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; lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved // Sum distance moved
gpsDriftNE += gpsloc_prev.get_distance(gpsloc); gpsDriftNE += gpsloc_prev.get_distance(gpsloc);
@ -85,8 +85,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
if (gps.have_vertical_velocity() && onGround) { if (gps.have_vertical_velocity() && onGround) {
// check that the average vertical GPS velocity is close to zero // check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFilt = constrain_ftype(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); gpsVertVelFail = (fabsF(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else { } else {
gpsVertVelFail = false; gpsVertVelFail = false;
} }
@ -95,7 +95,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
if (gpsVertVelFail) { if (gpsVertVelFail) {
dal.snprintf(prearm_fail_string, dal.snprintf(prearm_fail_string,
sizeof(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; gpsCheckStatus.bad_vert_vel = true;
} else { } else {
gpsCheckStatus.bad_vert_vel = false; gpsCheckStatus.bad_vert_vel = false;
@ -106,8 +106,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
bool gpsHorizVelFail; bool gpsHorizVelFail;
if (onGround) { if (onGround) {
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f); gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD); gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
} else { } else {
gpsHorizVelFail = false; gpsHorizVelFail = false;
} }
@ -123,7 +123,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
} }
// fail if horiziontal position accuracy not sufficient // fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f; float hAcc = 0.0;
bool hAccFail; bool hAccFail;
if (gps.horizontal_accuracy(hAcc)) { if (gps.horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); 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 // 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 // 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 ftype 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 tau = 10.0f; // time constant (sec) of peak hold decay
if (lastGpsCheckTime_ms == 0) { if (lastGpsCheckTime_ms == 0) {
lastGpsCheckTime_ms = imuSampleTime_ms; lastGpsCheckTime_ms = imuSampleTime_ms;
} }
float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f; ftype dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
lastGpsCheckTime_ms = imuSampleTime_ms; 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 // get the receivers reported speed accuracy
float gpsSpdAccRaw; float gpsSpdAccRaw;
@ -259,7 +259,7 @@ void NavEKF2_core::calcGpsGoodForFlight(void)
} }
// filter the raw speed accuracy using a LPF // 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 // apply a peak hold filter to the LPF output
sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2)); sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
@ -308,7 +308,7 @@ void NavEKF2_core::detectFlight()
if (assume_zero_sideslip()) { 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 // 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 highGndSpd = false;
bool highAirSpd = false; bool highAirSpd = false;
bool largeHgtChange = false; bool largeHgtChange = false;
@ -327,7 +327,7 @@ void NavEKF2_core::detectFlight()
} }
// trigger if more than 10m away from initial height // trigger if more than 10m away from initial height
if (fabsf(hgtMea) > 10.0f) { if (fabsF(hgtMea) > 10.0f) {
largeHgtChange = true; largeHgtChange = true;
} }

238
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -219,7 +219,7 @@ void NavEKF2_core::InitialiseVariables()
gpsDriftNE = 0.0f; gpsDriftNE = 0.0f;
gpsVertVelFilt = 0.0f; gpsVertVelFilt = 0.0f;
gpsHorizVelFilt = 0.0f; gpsHorizVelFilt = 0.0f;
memset(&statesArray, 0, sizeof(statesArray)); ZERO_FARRAY(statesArray);
memset(&vertCompFiltState, 0, sizeof(vertCompFiltState)); memset(&vertCompFiltState, 0, sizeof(vertCompFiltState));
posVelFusionDelayed = false; posVelFusionDelayed = false;
optFlowFusionDelayed = false; optFlowFusionDelayed = false;
@ -249,7 +249,7 @@ void NavEKF2_core::InitialiseVariables()
ekfGpsRefHgt = 0.0; ekfGpsRefHgt = 0.0;
velOffsetNED.zero(); velOffsetNED.zero();
posOffsetNED.zero(); posOffsetNED.zero();
memset(&velPosObs, 0, sizeof(velPosObs)); ZERO_FARRAY(velPosObs);
// range beacon fusion variables // range beacon fusion variables
memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew)); memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
@ -397,24 +397,24 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
imuDataDelayed = imuDataNew; imuDataDelayed = imuDataNew;
// acceleration vector in XYZ body axes measured by the IMU (m/s^2) // 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 // 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 // read the magnetometer data
readMagData(); readMagData();
// normalise the acceleration vector // normalise the acceleration vector
float pitch=0, roll=0; ftype pitch=0, roll=0;
if (initAccVec.length() > 0.001f) { if (initAccVec.length() > 0.001f) {
initAccVec.normalize(); initAccVec.normalize();
// calculate initial pitch angle // calculate initial pitch angle
pitch = asinf(initAccVec.x); pitch = asinF(initAccVec.x);
// calculate initial roll angle // calculate initial roll angle
roll = atan2f(-initAccVec.y , -initAccVec.z); roll = atan2F(-initAccVec.y , -initAccVec.z);
} }
// calculate initial roll and pitch orientation // 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.x = delAng.x * stateStruct.gyro_scale.x;
delAng.y = delAng.y * stateStruct.gyro_scale.y; 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); 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); 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 // use the nav frame from previous time step as the delta velocities
// have been rotated into that frame // have been rotated into that frame
// * and + operators have been overloaded // * 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 = prevTnb.mul_transpose(delVelCorrected);
delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT; 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 // if we are not aiding, then limit the horizontal magnitude of acceleration
// to prevent large manoeuvre transients disturbing the attitude // to prevent large manoeuvre transients disturbing the attitude
if ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) { if ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) {
float gain = 5.0f/accNavMagHoriz; ftype gain = 5.0f/accNavMagHoriz;
delVelNav.x *= gain; delVelNav.x *= gain;
delVelNav.y *= gain; delVelNav.y *= gain;
} }
// save velocity for use in trapezoidal integration for position calcuation // save velocity for use in trapezoidal integration for position calcuation
Vector3f lastVelocity = stateStruct.velocity; Vector3F lastVelocity = stateStruct.velocity;
// sum delta velocities to get velocity // sum delta velocities to get velocity
stateStruct.velocity += delVelNav; stateStruct.velocity += delVelNav;
@ -717,16 +717,16 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
void NavEKF2_core::calcOutputStates() void NavEKF2_core::calcOutputStates()
{ {
// apply corrections to the IMU data // apply corrections to the IMU data
Vector3f delAngNewCorrected = imuDataNew.delAng; Vector3F delAngNewCorrected = imuDataNew.delAng;
Vector3f delVelNewCorrected = imuDataNew.delVel; Vector3F delVelNewCorrected = imuDataNew.delVel;
correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT, imuDataNew.gyro_index); correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT, imuDataNew.gyro_index);
correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT, imuDataNew.accel_index); correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT, imuDataNew.accel_index);
// apply corections to track EKF solution // apply corections to track EKF solution
Vector3f delAng = delAngNewCorrected + delAngCorrection; Vector3F delAng = delAngNewCorrected + delAngCorrection;
// convert the rotation vector to its equivalent quaternion // convert the rotation vector to its equivalent quaternion
Quaternion deltaQuat; QuaternionF deltaQuat;
deltaQuat.from_axis_angle(delAng); deltaQuat.from_axis_angle(delAng);
// update the quaternion states by rotating from the previous attitude through // update the quaternion states by rotating from the previous attitude through
@ -735,15 +735,15 @@ void NavEKF2_core::calcOutputStates()
outputDataNew.quat.normalize(); outputDataNew.quat.normalize();
// calculate the body to nav cosine matrix // calculate the body to nav cosine matrix
Matrix3f Tbn_temp; Matrix3F Tbn_temp;
outputDataNew.quat.rotation_matrix(Tbn_temp); outputDataNew.quat.rotation_matrix(Tbn_temp);
// transform body delta velocities to delta velocities in the nav frame // 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; delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT;
// save velocity for use in trapezoidal integration for position calcuation // save velocity for use in trapezoidal integration for position calcuation
Vector3f lastVelocity = outputDataNew.velocity; Vector3F lastVelocity = outputDataNew.velocity;
// sum delta velocities to get velocity // sum delta velocities to get velocity
outputDataNew.velocity += delVelNav; outputDataNew.velocity += delVelNav;
@ -756,14 +756,14 @@ void NavEKF2_core::calcOutputStates()
// Perform filter calculation using backwards Euler integration // Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega // 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); const ftype CompFiltOmega = M_2PI * constrain_ftype(frontend->_hrt_filt_freq, 0.1f, 30.0f);
float omega2 = CompFiltOmega * CompFiltOmega; ftype omega2 = CompFiltOmega * CompFiltOmega;
float pos_err = constrain_float(outputDataNew.position.z - vertCompFiltState.pos, -1e5, 1e5); ftype pos_err = constrain_ftype(outputDataNew.position.z - vertCompFiltState.pos, -1e5, 1e5);
float integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT; ftype integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT;
vertCompFiltState.acc += integ1_input; 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; 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; vertCompFiltState.pos += integ3_input;
// apply a trapezoidal integration to velocities to calculate position // apply a trapezoidal integration to velocities to calculate position
@ -776,11 +776,11 @@ void NavEKF2_core::calcOutputStates()
if (!accelPosOffset.is_zero()) { if (!accelPosOffset.is_zero()) {
// calculate the average angular rate across the last IMU update // calculate the average angular rate across the last IMU update
// note delAngDT is prevented from being zero in readIMUData() // 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 // 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 // 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; velOffsetNED = Tbn_temp * velBodyRelIMU;
// calculate the earth frame position of the body frame origin relative to the IMU // 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 // 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 // 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 // Convert to a delta rotation using a small angle approximation
quatErr.normalize(); quatErr.normalize();
Vector3f deltaAngErr; Vector3F deltaAngErr;
float scaler; ftype scaler;
if (quatErr[0] >= 0.0f) { if (quatErr[0] >= 0.0f) {
scaler = 2.0f; scaler = 2.0f;
} else { } else {
@ -818,17 +818,17 @@ void NavEKF2_core::calcOutputStates()
// calculate a gain that provides tight tracking of the estimator states and // 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 // adjust for changes in time delay to maintain consistent damping ratio of ~0.7
float timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms); ftype timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms);
timeDelay = fmaxf(timeDelay, dtIMUavg); timeDelay = fmaxF(timeDelay, dtIMUavg);
float errorGain = 0.5f / timeDelay; ftype errorGain = 0.5f / timeDelay;
// calculate a corrrection to the delta angle // calculate a corrrection to the delta angle
// that will cause the INS to track the EKF quaternions // that will cause the INS to track the EKF quaternions
delAngCorrection = deltaAngErr * errorGain * dtIMUavg; delAngCorrection = deltaAngErr * errorGain * dtIMUavg;
// calculate velocity and position tracking errors // calculate velocity and position tracking errors
Vector3f velErr = (stateStruct.velocity - outputDataDelayed.velocity); Vector3F velErr = (stateStruct.velocity - outputDataDelayed.velocity);
Vector3f posErr = (stateStruct.position - outputDataDelayed.position); Vector3F posErr = (stateStruct.position - outputDataDelayed.position);
// collect magnitude tracking error for diagnostics // collect magnitude tracking error for diagnostics
outputTrackError.x = deltaAngErr.length(); outputTrackError.x = deltaAngErr.length();
@ -836,16 +836,16 @@ void NavEKF2_core::calcOutputStates()
outputTrackError.z = posErr.length(); outputTrackError.z = posErr.length();
// convert user specified time constant from centi-seconds to seconds // 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 // 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 // use a PI feedback to calculate a correction that will be applied to the output state history
posErrintegral += posErr; posErrintegral += posErr;
velErrintegral += velErr; velErrintegral += velErr;
Vector3f velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f; Vector3F velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f;
Vector3f posCorrection = posErr * velPosGain + posErrintegral * 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 // 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 // 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() void NavEKF2_core::CovariancePrediction()
{ {
float windVelSigma; // wind velocity 1-sigma process noise - m/s ftype windVelSigma; // wind velocity 1-sigma process noise - m/s
float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s ftype dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s
float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s ftype dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s
float dAngScaleSigma;// delta angle scale factor 1-Sigma process noise ftype dAngScaleSigma;// delta angle scale factor 1-Sigma process noise
float magEarthSigma;// earth magnetic field 1-sigma process noise ftype magEarthSigma;// earth magnetic field 1-sigma process noise
float magBodySigma; // body magnetic field 1-sigma process noise ftype magBodySigma; // body magnetic field 1-sigma process noise
float daxNoise; // X axis delta angle noise variance rad^2 ftype daxNoise; // X axis delta angle noise variance rad^2
float dayNoise; // Y axis delta angle noise variance rad^2 ftype dayNoise; // Y axis delta angle noise variance rad^2
float dazNoise; // Z axis delta angle noise variance rad^2 ftype dazNoise; // Z axis delta angle noise variance rad^2
float dvxNoise; // X axis delta velocity variance noise (m/s)^2 ftype dvxNoise; // X axis delta velocity variance noise (m/s)^2
float dvyNoise; // Y axis delta velocity variance noise (m/s)^2 ftype dvyNoise; // Y axis delta velocity variance noise (m/s)^2
float dvzNoise; // Z axis delta velocity variance noise (m/s)^2 ftype dvzNoise; // Z axis delta velocity variance noise (m/s)^2
float dvx; // X axis delta velocity (m/s) ftype dvx; // X axis delta velocity (m/s)
float dvy; // Y axis delta velocity (m/s) ftype dvy; // Y axis delta velocity (m/s)
float dvz; // Z axis delta velocity (m/s) ftype dvz; // Z axis delta velocity (m/s)
float dax; // X axis delta angle (rad) ftype dax; // X axis delta angle (rad)
float day; // Y axis delta angle (rad) ftype day; // Y axis delta angle (rad)
float daz; // Z axis delta angle (rad) ftype daz; // Z axis delta angle (rad)
float q0; // attitude quaternion ftype q0; // attitude quaternion
float q1; // attitude quaternion ftype q1; // attitude quaternion
float q2; // attitude quaternion ftype q2; // attitude quaternion
float q3; // attitude quaternion ftype q3; // attitude quaternion
float dax_b; // X axis delta angle measurement bias (rad) ftype dax_b; // X axis delta angle measurement bias (rad)
float day_b; // Y axis delta angle measurement bias (rad) ftype day_b; // Y axis delta angle measurement bias (rad)
float daz_b; // Z axis delta angle measurement bias (rad) ftype daz_b; // Z axis delta angle measurement bias (rad)
float dax_s; // X axis delta angle measurement scale factor ftype dax_s; // X axis delta angle measurement scale factor
float day_s; // Y axis delta angle measurement scale factor ftype day_s; // Y axis delta angle measurement scale factor
float daz_s; // Z axis delta angle measurement scale factor ftype daz_s; // Z axis delta angle measurement scale factor
float dvz_b; // Z axis delta velocity measurement bias (rad) ftype dvz_b; // Z axis delta velocity measurement bias (rad)
Vector25 SF; Vector25 SF;
Vector5 SG; Vector5 SG;
Vector8 SQ; Vector8 SQ;
@ -917,17 +917,17 @@ void NavEKF2_core::CovariancePrediction()
// this allows for wind gradient effects. // this allows for wind gradient effects.
// filter height rate using a 10 second time constant filter // filter height rate using a 10 second time constant filter
dt = imuDataDelayed.delAngDT; dt = imuDataDelayed.delAngDT;
float alpha = 0.1f * dt; ftype alpha = 0.1f * dt;
hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha; hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
// use filtered height rate to increase wind process noise when climbing or descending // use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects. // 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)); 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_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f); dAngBiasSigma = sq(dt) * constrain_ftype(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f);
dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f); dVelBiasSigma = sq(dt) * constrain_ftype(frontend->_accelBiasProcessNoise, 0.0f, 1.0f);
dAngScaleSigma = dt * constrain_float(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f); dAngScaleSigma = dt * constrain_ftype(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f);
magEarthSigma = dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f); magEarthSigma = dt * constrain_ftype(frontend->_magEarthProcessNoise, 0.0f, 1.0f);
magBodySigma = dt * constrain_float(frontend->_magBodyProcessNoise, 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= 0; i<=8; i++) processNoise[i] = 0.0f;
for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma; for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma; 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; day_s = stateStruct.gyro_scale.y;
daz_s = stateStruct.gyro_scale.z; daz_s = stateStruct.gyro_scale.z;
dvz_b = stateStruct.accel_zbias; 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); 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); dvxNoise = dvyNoise = dvzNoise = sq(dt*_accNoise);
// calculate the predicted covariance due to inertial sensor error propagation // 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; uint8_t row;
for (row=first; row<=last; 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; uint8_t row;
for (row=0; row<=23; 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 // 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; outputDataNew.quat = outputDataNew.quat*deltaQuat;
// write current measurement to entire table // write current measurement to entire table
@ -1468,7 +1468,7 @@ void NavEKF2_core::ForceSymmetry()
{ {
for (uint8_t j=0; j<=i-1; j++) 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[i][j] = temp;
P[j][i] = 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 // constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning
void NavEKF2_core::ConstrainVariances() 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=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_float(P[i][i],0.0f,1.0e3f); // velocities 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_float(P[i][i],0.0f,1.0e6f); 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_float(P[8][8],0.0f,1.0e6f); // vertical position 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_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases 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) { 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 { } else {
// we can't reliably estimate scale factors when there is no aiding data due to transient manoeuvre induced innovations // 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 // so inhibit estimation by keeping covariance elements at zero
zeroRows(P,12,14); zeroRows(P,12,14);
zeroCols(P,12,14); zeroCols(P,12,14);
} }
P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias 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_float(P[i][i],0.0f,0.01f); // earth magnetic field for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // earth magnetic field
for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body 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_float(P[i][i],0.0f,1.0e3f); // wind velocity 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 // constrain states using WMM tables and specified limit
void NavEKF2_core::MagTableConstrain(void) void NavEKF2_core::MagTableConstrain(void)
{ {
// constrain to error from table earth field // constrain to error from table earth field
float limit_ga = frontend->_mag_ef_limit * 0.001f; ftype limit_ga = frontend->_mag_ef_limit * 0.001f;
stateStruct.earth_magfield.x = constrain_float(stateStruct.earth_magfield.x, stateStruct.earth_magfield.x = constrain_ftype(stateStruct.earth_magfield.x,
table_earth_field_ga.x-limit_ga, table_earth_field_ga.x-limit_ga,
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,
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,
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() void NavEKF2_core::ConstrainStates()
{ {
// attitude errors are limited between +-1 // 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) // 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); for (uint8_t i=3; i<=5; i++) statesArray[i] = constrain_ftype(statesArray[i],-5.0e2f,5.0e2f);
// position limit 1000 km - TODO apply circular limit // position limit - 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=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 // 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) // 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) // 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) // 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 // earth magnetic field limit
if (frontend->_mag_ef_limit <= 0 || !have_table_earth_field) { if (frontend->_mag_ef_limit <= 0 || !have_table_earth_field) {
// constrain to +/-1Ga // 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 { } else {
// use table constrain // use table constrain
MagTableConstrain(); MagTableConstrain();
} }
// body magnetic field limit // 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 // 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 // constrain the terrain state to be below the vehicle height unless we are using terrain as the height datum
if (!inhibitGndState) { if (!inhibitGndState) {
terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
@ -1563,23 +1563,23 @@ void NavEKF2_core::ConstrainStates()
} }
// calculate the NED earth spin vector in rad/sec // 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); ftype lat_rad = radians(latitude*1.0e-7f);
omega.x = earthRate*cosf(lat_rad); omega.x = earthRate*cosF(lat_rad);
omega.y = 0; 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 // initialise the earth magnetic field states using declination, suppled roll/pitch
// and magnetometer measurements and return initial attitude quaternion // 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 // declare local variables required to calculate initial orientation and magnetic field
float yaw; ftype yaw;
Matrix3f Tbn; Matrix3F Tbn;
Vector3f initMagNED; Vector3F initMagNED;
Quaternion initQuat; QuaternionF initQuat;
if (use_compass()) { if (use_compass()) {
// calculate rotation matrix from body to NED frame // calculate rotation matrix from body to NED frame
@ -1592,17 +1592,17 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
initMagNED = Tbn * magDataDelayed.mag; initMagNED = Tbn * magDataDelayed.mag;
// calculate heading of mag field rel to body heading // 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 // get the magnetic declination
float magDecAng = MagDeclination(); ftype magDecAng = MagDeclination();
// calculate yaw angle rel to true north // calculate yaw angle rel to true north
yaw = magDecAng - magHeading; yaw = magDecAng - magHeading;
// calculate initial filter quaternion states using yaw from magnetometer // 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 // 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); 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 // this check ensures we accumulate the resets that occur within a single iteration of the EKF
if (imuSampleTime_ms != lastYawReset_ms) { 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 // zero the attitude covariances, but preserve the variances
void NavEKF2_core::zeroAttCovOnly() void NavEKF2_core::zeroAttCovOnly()
{ {
float varTemp[3]; ftype varTemp[3];
for (uint8_t index=0; index<=2; index++) { for (uint8_t index=0; index<=2; index++) {
varTemp[index] = P[index][index]; varTemp[index] = P[index][index];
} }

325
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -63,6 +63,13 @@
// number of seconds a request to reset the yaw to the GSF estimate is active before it times out // 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 #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 AP_AHRS;
class NavEKF2_core : public NavEKF_core_common class NavEKF2_core : public NavEKF_core_common
@ -87,7 +94,7 @@ public:
// Return a consolidated error score where higher numbers are less healthy // 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 // 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). // 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 // 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 // broken down as individual elements. Both are equivalent (same
// memory) // memory)
struct state_elements { struct state_elements {
Vector3f angErr; // 0..2 Vector3F angErr; // 0..2
Vector3f velocity; // 3..5 Vector3F velocity; // 3..5
Vector3f position; // 6..8 Vector3F position; // 6..8
Vector3f gyro_bias; // 9..11 Vector3F gyro_bias; // 9..11
Vector3f gyro_scale; // 12..14 Vector3F gyro_scale; // 12..14
float accel_zbias; // 15 ftype accel_zbias; // 15
Vector3f earth_magfield; // 16..18 Vector3F earth_magfield; // 16..18
Vector3f body_magfield; // 19..21 Vector3F body_magfield; // 19..21
Vector2f wind_vel; // 22..23 Vector2F wind_vel; // 22..23
Quaternion quat; // 24..27 QuaternionF quat; // 24..27
}; };
union { union {
@ -401,78 +408,78 @@ private:
}; };
struct output_elements { struct output_elements {
Quaternion quat; // 0..3 QuaternionF quat; // 0..3
Vector3f velocity; // 4..6 Vector3F velocity; // 4..6
Vector3f position; // 7..9 Vector3F position; // 7..9
}; };
struct imu_elements { struct imu_elements {
Vector3f delAng; // 0..2 Vector3F delAng; // 0..2
Vector3f delVel; // 3..5 Vector3F delVel; // 3..5
float delAngDT; // 6 ftype delAngDT; // 6
float delVelDT; // 7 ftype delVelDT; // 7
uint32_t time_ms; // 8 uint32_t time_ms; // 8
uint8_t gyro_index; uint8_t gyro_index;
uint8_t accel_index; uint8_t accel_index;
}; };
struct gps_elements : EKF_obs_element_t { struct gps_elements : EKF_obs_element_t {
Vector2f pos; Vector2F pos;
float hgt; ftype hgt;
Vector3f vel; Vector3F vel;
uint8_t sensor_idx; uint8_t sensor_idx;
}; };
struct mag_elements : EKF_obs_element_t { struct mag_elements : EKF_obs_element_t {
Vector3f mag; Vector3F mag;
}; };
struct baro_elements : EKF_obs_element_t { struct baro_elements : EKF_obs_element_t {
float hgt; ftype hgt;
}; };
struct range_elements : EKF_obs_element_t { struct range_elements : EKF_obs_element_t {
float rng; ftype rng;
uint8_t sensor_idx; uint8_t sensor_idx;
}; };
struct rng_bcn_elements : EKF_obs_element_t { struct rng_bcn_elements : EKF_obs_element_t {
float rng; // range measurement to each beacon (m) ftype rng; // range measurement to each beacon (m)
Vector3f beacon_posNED; // NED position of the beacon (m) Vector3F beacon_posNED; // NED position of the beacon (m)
float rngErr; // range measurement error 1-std (m) ftype rngErr; // range measurement error 1-std (m)
uint8_t beacon_ID; // beacon identification number uint8_t beacon_ID; // beacon identification number
}; };
struct tas_elements : EKF_obs_element_t { struct tas_elements : EKF_obs_element_t {
float tas; ftype tas;
}; };
struct of_elements : EKF_obs_element_t { struct of_elements : EKF_obs_element_t {
Vector2f flowRadXY; Vector2F flowRadXY;
Vector2f flowRadXYcomp; Vector2F flowRadXYcomp;
Vector3f bodyRadXYZ; Vector3F bodyRadXYZ;
Vector3f body_offset; Vector3F body_offset;
}; };
struct ext_nav_elements : EKF_obs_element_t { struct ext_nav_elements : EKF_obs_element_t {
Vector3f pos; // XYZ position measured in a RH navigation frame (m) Vector3F pos; // XYZ position measured in a RH navigation frame (m)
Quaternion quat; // quaternion describing the rotation from navigation to body frame QuaternionF quat; // quaternion describing the rotation from navigation to body frame
float posErr; // spherical poition measurement error 1-std (m) ftype posErr; // spherical poition measurement error 1-std (m)
float angErr; // spherical angular measurement error 1-std (rad) ftype angErr; // spherical angular measurement error 1-std (rad)
bool posReset; // true when the position measurement has been reset bool posReset; // true when the position measurement has been reset
}; };
// bias estimates for the IMUs that are enabled but not being used // bias estimates for the IMUs that are enabled but not being used
// by this core. // by this core.
struct { struct {
Vector3f gyro_bias; Vector3F gyro_bias;
Vector3f gyro_scale; Vector3F gyro_scale;
float accel_zbias; ftype accel_zbias;
} inactiveBias[INS_MAX_INSTANCES]; } inactiveBias[INS_MAX_INSTANCES];
struct ext_nav_vel_elements : EKF_obs_element_t { struct ext_nav_vel_elements : EKF_obs_element_t {
Vector3f vel; // velocity in NED (m) Vector3F vel; // velocity in NED (m)
float err; // velocity measurement error (m/s) ftype err; // velocity measurement error (m/s)
}; };
// update the navigation filter status // update the navigation filter status
@ -509,7 +516,7 @@ private:
void FuseRngBcnStatic(); void FuseRngBcnStatic();
// calculate the offset from EKF vertical position datum to the range beacon system datum // 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 // fuse magnetometer measurements
void FuseMagnetometer(); void FuseMagnetometer();
@ -533,21 +540,21 @@ private:
void StoreQuatReset(void); void StoreQuatReset(void);
// Rotate the stored output quaternion history through a quaternion rotation // 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 // 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 // initialise the covariance matrix
void CovarianceInit(); void CovarianceInit();
// helper functions for readIMUData // helper functions for readIMUData
bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt); bool readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt);
bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt); bool readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt);
// helper functions for correcting IMU data // helper functions for correcting IMU data
void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index); void correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index);
void correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index); void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index);
// update IMU delta angle and delta velocity measurements // update IMU delta angle and delta velocity measurements
void readIMUData(); void readIMUData();
@ -593,7 +600,7 @@ private:
// initialise the earth magnetic field states using declination and current attitude and magnetometer measurements // initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
// and return attitude quaternion // and return attitude quaternion
Quaternion calcQuatAndFieldStates(float roll, float pitch); QuaternionF calcQuatAndFieldStates(ftype roll, ftype pitch);
// zero stored variables // zero stored variables
void InitialiseVariables(); void InitialiseVariables();
@ -604,7 +611,7 @@ private:
void ResetPosition(void); void ResetPosition(void);
// reset the stateStruct's NE position to the specified position // 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 // reset velocity states using the last GPS measurement
void ResetVelocity(void); void ResetVelocity(void);
@ -613,7 +620,7 @@ private:
void ResetHeight(void); void ResetHeight(void);
// reset the stateStruct's D position // reset the stateStruct's D position
void ResetPositionD(float posD); void ResetPositionD(ftype posD);
// return true if we should use the airspeed sensor // return true if we should use the airspeed sensor
bool useAirspeed(void) const; bool useAirspeed(void) const;
@ -631,7 +638,7 @@ private:
void checkDivergence(void); void checkDivergence(void);
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2 // 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 // return true if optical flow data is available
bool optFlowDataPresent(void) const; 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. // 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 // Input is 1-sigma uncertainty in published declination
void FuseDeclination(float declErr); void FuseDeclination(ftype declErr);
// return magnetic declination in radians // 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 // Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer // using a simple observer
@ -734,10 +741,10 @@ private:
void CorrectGPSForAntennaOffset(gps_elements &gps_data) const; void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
// correct external navigation earth-frame position using sensor body-frame offset // 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 // 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 // Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data. // that uses IMU, GPS horizontal velocity and optionally true airspeed data.
@ -753,7 +760,7 @@ private:
// yaw : new yaw angle (rad) // yaw : new yaw angle (rad)
// yaw_variance : variance of new yaw angle (rad^2) // yaw_variance : variance of new yaw angle (rad^2)
// isDeltaYaw : true when the yaw should be added to the existing yaw angle // 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 // attempt to reset the yaw to the EKF-GSF value
// returns false if unsuccessful // 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 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 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 Matrix24 P; // covariance matrix
EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer
EKF_obs_buffer_t<gps_elements> storedGPS; // GPS data buffer EKF_obs_buffer_t<gps_elements> storedGPS; // GPS data buffer
@ -784,10 +791,10 @@ private:
EKF_obs_buffer_t<tas_elements> storedTAS; // TAS data buffer EKF_obs_buffer_t<tas_elements> storedTAS; // TAS data buffer
EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer
EKF_IMU_buffer_t<output_elements> storedOutput;// output state buffer EKF_IMU_buffer_t<output_elements> 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 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) 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 dtIMUavg; // expected time between IMU measurements (sec)
ftype dtEkfAvg; // expected time between EKF updates (sec) ftype dtEkfAvg; // expected time between EKF updates (sec)
ftype dt; // time lapsed since the last covariance prediction (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 fuseVelData; // this boolean causes the velNED measurements to be fused
bool fusePosData; // this boolean causes the posNE 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 bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
Vector3f innovMag; // innovation 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 Vector3F varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
ftype innovVtas; // innovation output from fusion of airspeed measurements ftype innovVtas; // innovation output from fusion of airspeed measurements
ftype varInnovVtas; // innovation variance 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 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 prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
uint32_t lastMagUpdate_us; // last time compass was updated in usec uint32_t lastMagUpdate_us; // last time compass was updated in usec
uint32_t lastMagRead_ms; // last time compass data was successfully read uint32_t lastMagRead_ms; // last time compass data was successfully read
Vector3f velDotNED; // rate of change of velocity in NED frame Vector3F velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED Vector3F velDotNEDfilt; // low pass filtered velDotNED
uint32_t imuSampleTime_ms; // time that the last IMU value was taken uint32_t imuSampleTime_ms; // time that the last IMU value was taken
bool tasDataToFuse; // true when new airspeed data is waiting to be fused bool tasDataToFuse; // true when new airspeed data is waiting to be fused
uint32_t lastBaroReceived_ms; // time last time we received baro height data 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 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 lastYawTime_ms; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec) uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Vector2f lastKnownPositionNE; // last known position Vector2F lastKnownPositionNE; // last known position
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold ftype 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 ftype 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 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 Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold ftype 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. 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 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 inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool lastInhibitMagStates; // previous inhibitMagStates 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 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 struct Location EKF_origin; // LLH origin of the NED axis system
bool validOrigin; // true when the EKF origin is valid bool validOrigin; // true when the EKF origin is valid
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
float gpsHgtAccuracy; // estimated height 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 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 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 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 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 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 Vector3F tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion
float tiltErrFilt; // Filtered tilt error metric ftype tiltErrFilt; // Filtered tilt error metric
bool tiltAlignComplete; // true when tilt alignment is complete bool tiltAlignComplete; // true when tilt alignment is complete
bool yawAlignComplete; // true when yaw alignment is complete bool yawAlignComplete; // true when yaw alignment is complete
bool magStateInitComplete; // true when the magnetic field sttes have been initialised 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 imuDataDelayed; // IMU data at the fusion time horizon
imu_elements imuDataNew; // IMU data at the current 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 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 baroDataNew; // Baro data at the current time horizon
baro_elements baroDataDelayed; // Baro data at the fusion time horizon baro_elements baroDataDelayed; // Baro data at the fusion time horizon
range_elements rangeDataNew; // Range finder data at the current time horizon range_elements rangeDataNew; // Range finder data at the current time horizon
@ -875,10 +882,10 @@ private:
gps_elements gpsDataDelayed; // GPS data at the fusion time horizon gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
output_elements outputDataNew; // output state data at the current time step output_elements outputDataNew; // output state data at the current time step
output_elements outputDataDelayed; // 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 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 velErrintegral; // integral of output predictor NED velocity tracking error (m)
Vector3f posErrintegral; // integral of output predictor NED position tracking error (m.sec) Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec)
float innovYaw; // compass yaw angle innovation (rad) ftype innovYaw; // compass yaw angle innovation (rad)
uint32_t timeTasReceived_ms; // time last TAS data was received (msec) 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 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 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 optFlowFusionDelayed; // true when the optical flow fusion has been delayed
bool airSpdFusionDelayed; // true when the air speed 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 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 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 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 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 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 ftype 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 QuaternionF 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 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 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 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 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 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 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) 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 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) Vector3F delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
bool magFieldLearned; // true when the magnetic field has been learned 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 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 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 bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
bool delAngBiasLearned; // true when the gyro bias has been learned bool delAngBiasLearned; // true when the gyro bias has been learned
nav_filter_status filterStatus; // contains the status of various filter outputs 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) 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 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 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 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 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 // variables used to calculate a vertical velocity that is kinematically consistent with the verical position
struct { struct {
float pos; ftype pos;
float vel; ftype vel;
float acc; ftype acc;
} vertCompFiltState; } vertCompFiltState;
// variables used by the pre-initialisation GPS checks // variables used by the pre-initialisation GPS checks
struct Location gpsloc_prev; // LLH location of previous GPS measurement 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 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 ftype 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 ftype 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 gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
// variable used by the in-flight GPS quality check // variable used by the in-flight GPS quality check
bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
bool ekfInnovationsPass; // true when GPS innovations pass 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 ftype 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 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 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 lastInnovPassTime_ms; // last time in msec the GPS innovations passed
uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed 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 of_elements ofDataDelayed; // OF data at the fusion time horizon
bool flowDataToFuse; // true when optical flow data is ready for fusion bool flowDataToFuse; // true when optical flow data is ready for fusion
bool flowDataValid; // true while optical flow data is still fresh 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 flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range 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 flowMeaTime_ms; // time stamp from latest flow measurement (msec)
uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (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 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec) Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
float Popt; // Optical flow terrain height state covariance (m^2) ftype Popt; // Optical flow terrain height state covariance (m^2)
float terrainState; // terrain position state (m) ftype terrainState; // terrain position state (m)
float prevPosN; // north position at last measurement ftype prevPosN; // north position at last measurement
float prevPosE; // east position at last measurement ftype prevPosE; // east position at last measurement
float varInnovRng; // range finder observation innovation variance (m^2) ftype varInnovRng; // range finder observation innovation variance (m^2)
float innovRng; // range finder observation innovation (m) ftype innovRng; // range finder observation innovation (m)
float hgtMea; // height measurement derived from either baro, gps or range finder data (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 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 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 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 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 ftype 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 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 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 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 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. 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_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 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 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 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 ftype delTimeOF; // time that delAngBodyOF is summed across
Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m) Vector3F accelPosOffset; // position of IMU accelerometer unit in body frame (m)
// Range finder // Range finder
float baroHgtOffset; // offset applied when when switching to use of Baro height ftype baroHgtOffset; // offset applied when when switching to use of Baro height
float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground ftype 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 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 storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors
uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors 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 rngBcnDataNew; // Range beacon data at the current time horizon
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion 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) 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 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 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) ftype varInnovRngBcn; // range beacon observation innovation variance (m^2)
float innovRngBcn; // range beacon observation innovation (m) ftype innovRngBcn; // range beacon observation innovation (m)
uint32_t lastTimeRngBcn_ms[10]; // last time we received a range beacon measurement (msec) 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 bool rngBcnDataToFuse; // true when there is new range beacon data to fuse
Vector3f beaconVehiclePosNED; // NED position estimate from the beacon system (NED) Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
float beaconVehiclePosErr; // estimated position error from the beacon system (m) ftype beaconVehiclePosErr; // estimated position error from the beacon system (m)
uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec) 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 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 uint8_t lastRngBcnChecked; // index of the last range beacon checked for data
Vector3f receiverPos; // receiver NED position (m) - alignment 3 state filter Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
float receiverPosCov[3][3]; // Receiver position covariance (m^2) - 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 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 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 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 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 uint8_t N_beacons; // Number of range beacons in use
float maxBcnPosD; // maximum position of all beacons in the down direction (m) ftype maxBcnPosD; // maximum position of all beacons in the down direction (m)
float minBcnPosD; // minimum position of all beacons in the down direction (m) ftype 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 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) ftype bcnPosOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMaxVar; // Variance of the bcnPosOffsetHigh state (m) ftype bcnPosOffsetMaxVar; // Variance of the bcnPosOffsetHigh state (m)
float OffsetMaxInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetHigh 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) ftype bcnPosOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMinVar; // Variance of the bcnPosoffset state (m) ftype bcnPosOffsetMinVar; // Variance of the bcnPosoffset state (m)
float OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow ftype OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow
// Range Beacon Fusion Debug Reporting // Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
struct rngBcnFusionReport_t { struct rngBcnFusionReport_t {
float rng; // measured range to beacon (m) ftype rng; // measured range to beacon (m)
float innov; // range innovation (m) ftype innov; // range innovation (m)
float innovVar; // innovation variance (m^2) ftype innovVar; // innovation variance (m^2)
float testRatio; // innovation consistency test ratio ftype testRatio; // innovation consistency test ratio
Vector3f beaconPosNED; // beacon NED position Vector3F beaconPosNED; // beacon NED position
} rngBcnFusionReport[10]; } rngBcnFusionReport[10];
// height source selection logic // height source selection logic
@ -1051,11 +1058,11 @@ private:
// Movement detector // Movement detector
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected 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 uint32_t timeAtArming_ms; // time in msec that the vehicle armed
// baro ground effect // 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 // control of post takeoff magnetic field and heading resets
bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed 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 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 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 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) ftype 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) ftype 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 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 uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent
// external navigation fusion // external navigation fusion
@ -1136,8 +1143,8 @@ private:
ftype magXbias; ftype magXbias;
ftype magYbias; ftype magYbias;
ftype magZbias; ftype magZbias;
Matrix3f DCM; Matrix3F DCM;
Vector3f MagPred; Vector3F MagPred;
ftype R_MAG; ftype R_MAG;
Vector9 SH_MAG; Vector9 SH_MAG;
} mag_state; } mag_state;
@ -1147,8 +1154,8 @@ private:
// earth field from WMM tables // earth field from WMM tables
bool have_table_earth_field; // true when we have initialised table_earth_field_ga bool have_table_earth_field; // true when we have initialised table_earth_field_ga
Vector3f table_earth_field_ga; // earth field from WMM tables Vector3F table_earth_field_ga; // earth field from WMM tables
float table_declination; // declination in radians from the tables ftype table_declination; // declination in radians from the tables
// timing statistics // timing statistics
struct ekf_timing timing; struct ekf_timing timing;
@ -1160,7 +1167,7 @@ private:
bool assume_zero_sideslip(void) const; bool assume_zero_sideslip(void) const;
// vehicle specific initial gyro bias uncertainty // 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 // 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) uint32_t EKFGSF_yaw_reset_ms; // timestamp of last emergency yaw reset (uSec)

6
libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp
View File

@ -16,7 +16,7 @@ void NavEKF2_core::resetGyroBias(void)
zeroRows(P,9,11); zeroRows(P,9,11);
zeroCols(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[10][10] = P[9][9];
P[11][11] = 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 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;
} }