Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: allow for double EKF build

This commit is contained in:
Andrew Tridgell 2021-05-04 21:12:23 +10:00 committed by Randy Mackay
parent 78e87b32d6
commit 601eb12efb
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()
{
// declarations
float vn;
float ve;
float vd;
float vwn;
float vwe;
float EAS2TAS = dal.get_EAS2TAS();
const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
ftype vn;
ftype ve;
ftype vd;
ftype vwn;
ftype vwe;
ftype EAS2TAS = dal.get_EAS2TAS();
const ftype R_TAS = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(EAS2TAS, 0.9f, 10.0f));
Vector3 SH_TAS;
float SK_TAS;
ftype SK_TAS;
Vector24 H_TAS;
float VtasPred;
ftype VtasPred;
// copy required states to local variable names
vn = stateStruct.velocity.x;
@ -46,7 +46,7 @@ void NavEKF2_core::FuseAirspeed()
if (VtasPred > 1.0f)
{
// calculate observation jacobians
SH_TAS[0] = 1.0f/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
SH_TAS[0] = 1.0f/(sqrtF(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
for (uint8_t i=0; i<=2; i++) H_TAS[i] = 0.0f;
@ -57,7 +57,7 @@ void NavEKF2_core::FuseAirspeed()
H_TAS[23] = -SH_TAS[1];
for (uint8_t i=6; i<=21; i++) H_TAS[i] = 0.0f;
// calculate Kalman gains
float temp = (R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0]));
ftype temp = (R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0]));
if (temp >= R_TAS) {
SK_TAS = 1.0f / temp;
faultStatus.bad_airspeed = false;
@ -107,7 +107,7 @@ void NavEKF2_core::FuseAirspeed()
innovVtas = VtasPred - tasDataDelayed.tas;
// calculate the innovation consistency test ratio
tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
// fail if the ratio is > 1, but don't fail if bad IMU data
bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
@ -238,21 +238,21 @@ void NavEKF2_core::SelectBetaFusion()
void NavEKF2_core::FuseSideslip()
{
// declarations
float q0;
float q1;
float q2;
float q3;
float vn;
float ve;
float vd;
float vwn;
float vwe;
const float R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg
ftype q0;
ftype q1;
ftype q2;
ftype q3;
ftype vn;
ftype ve;
ftype vd;
ftype vwn;
ftype vwe;
const ftype R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg
Vector10 SH_BETA;
Vector5 SK_BETA;
Vector3f vel_rel_wind;
Vector3F vel_rel_wind;
Vector24 H_BETA;
float innovBeta;
ftype innovBeta;
// copy required states to local variable names
q0 = stateStruct.quat[0];
@ -278,7 +278,7 @@ void NavEKF2_core::FuseSideslip()
{
// Calculate observation jacobians
SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
if (fabsf(SH_BETA[0]) <= 1e-9f) {
if (fabsF(SH_BETA[0]) <= 1e-9f) {
faultStatus.bad_sideslip = true;
return;
} else {
@ -307,7 +307,7 @@ void NavEKF2_core::FuseSideslip()
H_BETA[23] = SH_BETA[1]*SH_BETA[3]*SH_BETA[8] - SH_BETA[4];
// Calculate Kalman gains
float temp = (R_BETA + (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][4]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[6] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][23]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[6] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][3]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[6] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][22]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[6] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (sq(SH_BETA[1])*SH_BETA[3] + 1)*(P[22][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][2]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[6] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3))*(P[22][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][5]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[6] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[2]*SH_BETA[6]*(P[22][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][0]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[6] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[3]*(P[22][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][1]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[6] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]));
ftype temp = (R_BETA + (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][4]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[6] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][23]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[6] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][3]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[6] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][22]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[6] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (sq(SH_BETA[1])*SH_BETA[3] + 1)*(P[22][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][2]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[6] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3))*(P[22][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][5]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[6] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[2]*SH_BETA[6]*(P[22][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][0]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[6] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[3]*(P[22][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][1]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[6] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]));
if (temp >= R_BETA) {
SK_BETA[0] = 1.0f / temp;
faultStatus.bad_sideslip = false;

32
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

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

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_gps_status gpsStatus {};
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
ftype tempVar = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
getFilterFaults(_faultStatus);
getFilterStatus(solutionStatus);
getFilterGpsStatus(gpsStatus);
@ -142,7 +142,7 @@ void NavEKF2_core::Log_Write_NKF4(uint64_t time_us) const
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : tiltErrFilt, // tilt error convergence metric
tiltErr : float(tiltErrFilt), // tilt error convergence metric
offsetNorth : offset.x,
offsetEast : offset.y,
faults : _faultStatus,
@ -174,10 +174,10 @@ void NavEKF2_core::Log_Write_NKF5(uint64_t time_us) const
offset : (int16_t)(100*terrainState), // // estimated vertical position of the terrain relative to the nav filter zero datum
RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
errHAGL : (uint16_t)(100*sqrtf(Popt)), // filter ground offset state error
angErr : outputTrackError.x,
velErr : outputTrackError.y,
posErr : outputTrackError.z
errHAGL : (uint16_t)(100*sqrtF(Popt)), // filter ground offset state error
angErr : float(outputTrackError.x),
velErr : float(outputTrackError.y),
posErr : float(outputTrackError.z)
};
AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
}
@ -235,7 +235,7 @@ void NavEKF2_core::Log_Write_Beacon(uint64_t time_us)
rng : (int16_t)(100*report.rng),
innov : (int16_t)(100*report.innov),
sqrtInnovVar : (uint16_t)(100*safe_sqrt(report.innovVar)),
testRatio : (uint16_t)(100*constrain_float(report.testRatio,0.0f,650.0f)),
testRatio : (uint16_t)(100*constrain_ftype(report.testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*report.beaconPosNED.x),
beaconPosE : (int16_t)(100*report.beaconPosNED.y),
beaconPosD : (int16_t)(100*report.beaconPosNED.z),

228
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

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

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

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()
{
// horizontal velocity squared
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
ftype velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
// don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
// don't fuse flow data if it exceeds validity limits
@ -90,34 +90,34 @@ void NavEKF2_core::EstimateTerrainOffset()
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
ftype distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
prevPosN = stateStruct.position[0];
prevPosE = stateStruct.position[1];
// in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity
float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5];
ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5];
Popt += Pincrement;
timeAtLastAuxEKF_ms = imuSampleTime_ms;
// fuse range finder data
if (rangeDataToFuse) {
// predict range
float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
ftype predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
// Copy required states to local variable names
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
float R_RNG = frontend->_rngNoise;
ftype R_RNG = frontend->_rngNoise;
// calculate Kalman gain
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
// Calculate the innovation variance for data logging
varInnovRng = (R_RNG + Popt/sq(SK_RNG));
@ -129,7 +129,7 @@ void NavEKF2_core::EstimateTerrainOffset()
innovRng = predRngMeas - rangeDataDelayed.rng;
// calculate the innovation consistency test ratio
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
// Check the innovation test ratio and don't fuse if too large
if (auxRngTestRatio < 1.0f) {
@ -150,18 +150,18 @@ void NavEKF2_core::EstimateTerrainOffset()
if (!cantFuseFlowData) {
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
Vector2f losPred; // predicted optical flow angular rate measurement
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
float K_OPT;
float H_OPT;
Vector2f auxFlowObsInnovVar;
Vector3F relVelSensor; // velocity of sensor relative to ground in sensor axes
Vector2F losPred; // predicted optical flow angular rate measurement
ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
ftype K_OPT;
ftype H_OPT;
Vector2F auxFlowObsInnovVar;
// predict range to centre of image
float flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z;
ftype flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z;
// constrain terrain height to be below the vehicle
terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
@ -178,20 +178,20 @@ void NavEKF2_core::EstimateTerrainOffset()
auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp;
// calculate observation jacobians
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = stateStruct.position.z - terrainState;
float t7 = 1.0f / (t6*t6);
float t8 = q0*q3*2.0f;
float t9 = t2-t3-t4+t5;
ftype t2 = q0*q0;
ftype t3 = q1*q1;
ftype t4 = q2*q2;
ftype t5 = q3*q3;
ftype t6 = stateStruct.position.z - terrainState;
ftype t7 = 1.0f / (t6*t6);
ftype t8 = q0*q3*2.0f;
ftype t9 = t2-t3-t4+t5;
// prevent the state variances from becoming badly conditioned
Popt = MAX(Popt,1E-6f);
// calculate observation noise variance from parameter
float flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f));
ftype flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f));
// Fuse Y axis data
@ -205,7 +205,7 @@ void NavEKF2_core::EstimateTerrainOffset()
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y;
// calculate the innovation consistency test ratio
auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y);
auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y);
// don't fuse if optical flow data is outside valid range
if (auxFlowTestRatio.y < 1.0f) {
@ -237,7 +237,7 @@ void NavEKF2_core::EstimateTerrainOffset()
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x;
// calculate the innovation consistency test ratio
auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x);
auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x);
// don't fuse if optical flow data is outside valid range
if (auxFlowTestRatio.x < 1.0f) {
@ -267,23 +267,23 @@ void NavEKF2_core::EstimateTerrainOffset()
void NavEKF2_core::FuseOptFlow()
{
Vector24 H_LOS;
Vector3f relVelSensor;
Vector3F relVelSensor;
Vector14 SH_LOS;
Vector2 losPred;
// Copy required states to local variable names
float q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3];
float vn = stateStruct.velocity.x;
float ve = stateStruct.velocity.y;
float vd = stateStruct.velocity.z;
float pd = stateStruct.position.z;
ftype q0 = stateStruct.quat[0];
ftype q1 = stateStruct.quat[1];
ftype q2 = stateStruct.quat[2];
ftype q3 = stateStruct.quat[3];
ftype vn = stateStruct.velocity.x;
ftype ve = stateStruct.velocity.y;
ftype vd = stateStruct.velocity.z;
ftype pd = stateStruct.position.z;
// constrain height above ground to be above range measured on ground
float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
float ptd = pd + heightAboveGndEst;
ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
ftype ptd = pd + heightAboveGndEst;
// Calculate common expressions for observation jacobians
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
@ -304,14 +304,14 @@ void NavEKF2_core::FuseOptFlow()
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
// calculate range from ground plain to centre of sensor fov assuming flat earth
float range = constrain_float((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
ftype range = constrain_ftype((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
// correct range for flow sensor offset body frame position offset
// the corrected value is the predicted range from the sensor focal point to the
// centre of the image on the ground assuming flat terrain
Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
Vector3F posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
range -= posOffsetEarth.z / prevTnb.c.z;
}
@ -333,97 +333,97 @@ void NavEKF2_core::FuseOptFlow()
H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[6];
H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13];
float t2 = SH_LOS[3];
float t3 = SH_LOS[0];
float t4 = SH_LOS[2];
float t5 = SH_LOS[6];
float t100 = t2 * t3 * t5;
float t6 = SH_LOS[4];
float t7 = t2*t3*t6;
float t9 = t2*t4*t5;
float t8 = t7-t9;
float t10 = q0*q3*2.0f;
float t21 = q1*q2*2.0f;
float t11 = t10-t21;
float t101 = t2 * t3 * t11;
float t12 = pd-ptd;
float t13 = 1.0f/(t12*t12);
float t104 = t3 * t4 * t13;
float t14 = SH_LOS[5];
float t102 = t2 * t4 * t14;
float t15 = SH_LOS[1];
float t103 = t2 * t3 * t15;
float t16 = q0*q0;
float t17 = q1*q1;
float t18 = q2*q2;
float t19 = q3*q3;
float t20 = t16-t17+t18-t19;
float t105 = t2 * t3 * t20;
float t22 = P[1][1]*t102;
float t23 = P[3][0]*t101;
float t24 = P[8][0]*t104;
float t25 = P[1][0]*t102;
float t26 = P[2][0]*t103;
float t63 = P[0][0]*t8;
float t64 = P[5][0]*t100;
float t65 = P[4][0]*t105;
float t27 = t23+t24+t25+t26-t63-t64-t65;
float t28 = P[3][3]*t101;
float t29 = P[8][3]*t104;
float t30 = P[1][3]*t102;
float t31 = P[2][3]*t103;
float t67 = P[0][3]*t8;
float t68 = P[5][3]*t100;
float t69 = P[4][3]*t105;
float t32 = t28+t29+t30+t31-t67-t68-t69;
float t33 = t101*t32;
float t34 = P[3][8]*t101;
float t35 = P[8][8]*t104;
float t36 = P[1][8]*t102;
float t37 = P[2][8]*t103;
float t70 = P[0][8]*t8;
float t71 = P[5][8]*t100;
float t72 = P[4][8]*t105;
float t38 = t34+t35+t36+t37-t70-t71-t72;
float t39 = t104*t38;
float t40 = P[3][1]*t101;
float t41 = P[8][1]*t104;
float t42 = P[2][1]*t103;
float t73 = P[0][1]*t8;
float t74 = P[5][1]*t100;
float t75 = P[4][1]*t105;
float t43 = t22+t40+t41+t42-t73-t74-t75;
float t44 = t102*t43;
float t45 = P[3][2]*t101;
float t46 = P[8][2]*t104;
float t47 = P[1][2]*t102;
float t48 = P[2][2]*t103;
float t76 = P[0][2]*t8;
float t77 = P[5][2]*t100;
float t78 = P[4][2]*t105;
float t49 = t45+t46+t47+t48-t76-t77-t78;
float t50 = t103*t49;
float t51 = P[3][5]*t101;
float t52 = P[8][5]*t104;
float t53 = P[1][5]*t102;
float t54 = P[2][5]*t103;
float t79 = P[0][5]*t8;
float t80 = P[5][5]*t100;
float t81 = P[4][5]*t105;
float t55 = t51+t52+t53+t54-t79-t80-t81;
float t56 = P[3][4]*t101;
float t57 = P[8][4]*t104;
float t58 = P[1][4]*t102;
float t59 = P[2][4]*t103;
float t83 = P[0][4]*t8;
float t84 = P[5][4]*t100;
float t85 = P[4][4]*t105;
float t60 = t56+t57+t58+t59-t83-t84-t85;
float t66 = t8*t27;
float t82 = t100*t55;
float t86 = t105*t60;
float t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86;
float t62 = 1.0f/t61;
ftype t2 = SH_LOS[3];
ftype t3 = SH_LOS[0];
ftype t4 = SH_LOS[2];
ftype t5 = SH_LOS[6];
ftype t100 = t2 * t3 * t5;
ftype t6 = SH_LOS[4];
ftype t7 = t2*t3*t6;
ftype t9 = t2*t4*t5;
ftype t8 = t7-t9;
ftype t10 = q0*q3*2.0f;
ftype t21 = q1*q2*2.0f;
ftype t11 = t10-t21;
ftype t101 = t2 * t3 * t11;
ftype t12 = pd-ptd;
ftype t13 = 1.0f/(t12*t12);
ftype t104 = t3 * t4 * t13;
ftype t14 = SH_LOS[5];
ftype t102 = t2 * t4 * t14;
ftype t15 = SH_LOS[1];
ftype t103 = t2 * t3 * t15;
ftype t16 = q0*q0;
ftype t17 = q1*q1;
ftype t18 = q2*q2;
ftype t19 = q3*q3;
ftype t20 = t16-t17+t18-t19;
ftype t105 = t2 * t3 * t20;
ftype t22 = P[1][1]*t102;
ftype t23 = P[3][0]*t101;
ftype t24 = P[8][0]*t104;
ftype t25 = P[1][0]*t102;
ftype t26 = P[2][0]*t103;
ftype t63 = P[0][0]*t8;
ftype t64 = P[5][0]*t100;
ftype t65 = P[4][0]*t105;
ftype t27 = t23+t24+t25+t26-t63-t64-t65;
ftype t28 = P[3][3]*t101;
ftype t29 = P[8][3]*t104;
ftype t30 = P[1][3]*t102;
ftype t31 = P[2][3]*t103;
ftype t67 = P[0][3]*t8;
ftype t68 = P[5][3]*t100;
ftype t69 = P[4][3]*t105;
ftype t32 = t28+t29+t30+t31-t67-t68-t69;
ftype t33 = t101*t32;
ftype t34 = P[3][8]*t101;
ftype t35 = P[8][8]*t104;
ftype t36 = P[1][8]*t102;
ftype t37 = P[2][8]*t103;
ftype t70 = P[0][8]*t8;
ftype t71 = P[5][8]*t100;
ftype t72 = P[4][8]*t105;
ftype t38 = t34+t35+t36+t37-t70-t71-t72;
ftype t39 = t104*t38;
ftype t40 = P[3][1]*t101;
ftype t41 = P[8][1]*t104;
ftype t42 = P[2][1]*t103;
ftype t73 = P[0][1]*t8;
ftype t74 = P[5][1]*t100;
ftype t75 = P[4][1]*t105;
ftype t43 = t22+t40+t41+t42-t73-t74-t75;
ftype t44 = t102*t43;
ftype t45 = P[3][2]*t101;
ftype t46 = P[8][2]*t104;
ftype t47 = P[1][2]*t102;
ftype t48 = P[2][2]*t103;
ftype t76 = P[0][2]*t8;
ftype t77 = P[5][2]*t100;
ftype t78 = P[4][2]*t105;
ftype t49 = t45+t46+t47+t48-t76-t77-t78;
ftype t50 = t103*t49;
ftype t51 = P[3][5]*t101;
ftype t52 = P[8][5]*t104;
ftype t53 = P[1][5]*t102;
ftype t54 = P[2][5]*t103;
ftype t79 = P[0][5]*t8;
ftype t80 = P[5][5]*t100;
ftype t81 = P[4][5]*t105;
ftype t55 = t51+t52+t53+t54-t79-t80-t81;
ftype t56 = P[3][4]*t101;
ftype t57 = P[8][4]*t104;
ftype t58 = P[1][4]*t102;
ftype t59 = P[2][4]*t103;
ftype t83 = P[0][4]*t8;
ftype t84 = P[5][4]*t100;
ftype t85 = P[4][4]*t105;
ftype t60 = t56+t57+t58+t59-t83-t84-t85;
ftype t66 = t8*t27;
ftype t82 = t100*t55;
ftype t86 = t105*t60;
ftype t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86;
ftype t62 = 1.0f/t61;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t61 > R_LOS) {
@ -487,97 +487,97 @@ void NavEKF2_core::FuseOptFlow()
H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[5];
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13];
float t2 = SH_LOS[3];
float t3 = SH_LOS[0];
float t4 = SH_LOS[1];
float t5 = SH_LOS[5];
float t100 = t2 * t3 * t5;
float t6 = SH_LOS[4];
float t7 = t2*t3*t6;
float t8 = t2*t4*t5;
float t9 = t7+t8;
float t10 = q0*q3*2.0f;
float t11 = q1*q2*2.0f;
float t12 = t10+t11;
float t101 = t2 * t3 * t12;
float t13 = pd-ptd;
float t14 = 1.0f/(t13*t13);
float t104 = t3 * t4 * t14;
float t15 = SH_LOS[6];
float t105 = t2 * t4 * t15;
float t16 = SH_LOS[2];
float t102 = t2 * t3 * t16;
float t17 = q0*q0;
float t18 = q1*q1;
float t19 = q2*q2;
float t20 = q3*q3;
float t21 = t17+t18-t19-t20;
float t103 = t2 * t3 * t21;
float t22 = P[0][0]*t105;
float t23 = P[1][1]*t9;
float t24 = P[8][1]*t104;
float t25 = P[0][1]*t105;
float t26 = P[5][1]*t100;
float t64 = P[4][1]*t101;
float t65 = P[2][1]*t102;
float t66 = P[3][1]*t103;
float t27 = t23+t24+t25+t26-t64-t65-t66;
float t28 = t9*t27;
float t29 = P[1][4]*t9;
float t30 = P[8][4]*t104;
float t31 = P[0][4]*t105;
float t32 = P[5][4]*t100;
float t67 = P[4][4]*t101;
float t68 = P[2][4]*t102;
float t69 = P[3][4]*t103;
float t33 = t29+t30+t31+t32-t67-t68-t69;
float t34 = P[1][8]*t9;
float t35 = P[8][8]*t104;
float t36 = P[0][8]*t105;
float t37 = P[5][8]*t100;
float t71 = P[4][8]*t101;
float t72 = P[2][8]*t102;
float t73 = P[3][8]*t103;
float t38 = t34+t35+t36+t37-t71-t72-t73;
float t39 = t104*t38;
float t40 = P[1][0]*t9;
float t41 = P[8][0]*t104;
float t42 = P[5][0]*t100;
float t74 = P[4][0]*t101;
float t75 = P[2][0]*t102;
float t76 = P[3][0]*t103;
float t43 = t22+t40+t41+t42-t74-t75-t76;
float t44 = t105*t43;
float t45 = P[1][2]*t9;
float t46 = P[8][2]*t104;
float t47 = P[0][2]*t105;
float t48 = P[5][2]*t100;
float t63 = P[2][2]*t102;
float t77 = P[4][2]*t101;
float t78 = P[3][2]*t103;
float t49 = t45+t46+t47+t48-t63-t77-t78;
float t50 = P[1][5]*t9;
float t51 = P[8][5]*t104;
float t52 = P[0][5]*t105;
float t53 = P[5][5]*t100;
float t80 = P[4][5]*t101;
float t81 = P[2][5]*t102;
float t82 = P[3][5]*t103;
float t54 = t50+t51+t52+t53-t80-t81-t82;
float t55 = t100*t54;
float t56 = P[1][3]*t9;
float t57 = P[8][3]*t104;
float t58 = P[0][3]*t105;
float t59 = P[5][3]*t100;
float t83 = P[4][3]*t101;
float t84 = P[2][3]*t102;
float t85 = P[3][3]*t103;
float t60 = t56+t57+t58+t59-t83-t84-t85;
float t70 = t101*t33;
float t79 = t102*t49;
float t86 = t103*t60;
float t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86;
float t62 = 1.0f/t61;
ftype t2 = SH_LOS[3];
ftype t3 = SH_LOS[0];
ftype t4 = SH_LOS[1];
ftype t5 = SH_LOS[5];
ftype t100 = t2 * t3 * t5;
ftype t6 = SH_LOS[4];
ftype t7 = t2*t3*t6;
ftype t8 = t2*t4*t5;
ftype t9 = t7+t8;
ftype t10 = q0*q3*2.0f;
ftype t11 = q1*q2*2.0f;
ftype t12 = t10+t11;
ftype t101 = t2 * t3 * t12;
ftype t13 = pd-ptd;
ftype t14 = 1.0f/(t13*t13);
ftype t104 = t3 * t4 * t14;
ftype t15 = SH_LOS[6];
ftype t105 = t2 * t4 * t15;
ftype t16 = SH_LOS[2];
ftype t102 = t2 * t3 * t16;
ftype t17 = q0*q0;
ftype t18 = q1*q1;
ftype t19 = q2*q2;
ftype t20 = q3*q3;
ftype t21 = t17+t18-t19-t20;
ftype t103 = t2 * t3 * t21;
ftype t22 = P[0][0]*t105;
ftype t23 = P[1][1]*t9;
ftype t24 = P[8][1]*t104;
ftype t25 = P[0][1]*t105;
ftype t26 = P[5][1]*t100;
ftype t64 = P[4][1]*t101;
ftype t65 = P[2][1]*t102;
ftype t66 = P[3][1]*t103;
ftype t27 = t23+t24+t25+t26-t64-t65-t66;
ftype t28 = t9*t27;
ftype t29 = P[1][4]*t9;
ftype t30 = P[8][4]*t104;
ftype t31 = P[0][4]*t105;
ftype t32 = P[5][4]*t100;
ftype t67 = P[4][4]*t101;
ftype t68 = P[2][4]*t102;
ftype t69 = P[3][4]*t103;
ftype t33 = t29+t30+t31+t32-t67-t68-t69;
ftype t34 = P[1][8]*t9;
ftype t35 = P[8][8]*t104;
ftype t36 = P[0][8]*t105;
ftype t37 = P[5][8]*t100;
ftype t71 = P[4][8]*t101;
ftype t72 = P[2][8]*t102;
ftype t73 = P[3][8]*t103;
ftype t38 = t34+t35+t36+t37-t71-t72-t73;
ftype t39 = t104*t38;
ftype t40 = P[1][0]*t9;
ftype t41 = P[8][0]*t104;
ftype t42 = P[5][0]*t100;
ftype t74 = P[4][0]*t101;
ftype t75 = P[2][0]*t102;
ftype t76 = P[3][0]*t103;
ftype t43 = t22+t40+t41+t42-t74-t75-t76;
ftype t44 = t105*t43;
ftype t45 = P[1][2]*t9;
ftype t46 = P[8][2]*t104;
ftype t47 = P[0][2]*t105;
ftype t48 = P[5][2]*t100;
ftype t63 = P[2][2]*t102;
ftype t77 = P[4][2]*t101;
ftype t78 = P[3][2]*t103;
ftype t49 = t45+t46+t47+t48-t63-t77-t78;
ftype t50 = P[1][5]*t9;
ftype t51 = P[8][5]*t104;
ftype t52 = P[0][5]*t105;
ftype t53 = P[5][5]*t100;
ftype t80 = P[4][5]*t101;
ftype t81 = P[2][5]*t102;
ftype t82 = P[3][5]*t103;
ftype t54 = t50+t51+t52+t53-t80-t81-t82;
ftype t55 = t100*t54;
ftype t56 = P[1][3]*t9;
ftype t57 = P[8][3]*t104;
ftype t58 = P[0][3]*t105;
ftype t59 = P[5][3]*t100;
ftype t83 = P[4][3]*t101;
ftype t84 = P[2][3]*t102;
ftype t85 = P[3][3]*t103;
ftype t60 = t56+t57+t58+t59-t83-t84-t85;
ftype t70 = t101*t33;
ftype t79 = t102*t49;
ftype t86 = t103*t60;
ftype t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86;
ftype t62 = 1.0f/t61;
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if (t61 > R_LOS) {
@ -633,7 +633,7 @@ void NavEKF2_core::FuseOptFlow()
}
// calculate the innovation consistency test ratio
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {

52
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -25,8 +25,8 @@ bool NavEKF2_core::healthy(void) const
return false;
}
// position and height innovations must be within limits when on-ground and in a static mode of operation
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) {
ftype horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
return false;
}
@ -36,16 +36,16 @@ bool NavEKF2_core::healthy(void) const
// Return a consolidated error score where higher numbers represent larger errors
// Intended to be used by the front-end to determine which is the primary EKF
float NavEKF2_core::errorScore() const
ftype NavEKF2_core::errorScore() const
{
float score = 0.0f;
ftype score = 0.0f;
if (tiltAlignComplete && yawAlignComplete) {
// Check GPS fusion performance
score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
// Check altimeter fusion performance
score = MAX(score, hgtTestRatio);
// Check attitude corrections
const float tiltErrThreshold = 0.05f;
const ftype tiltErrThreshold = 0.05f;
score = MAX(score, tiltErrFilt / tiltErrThreshold);
}
return score;
@ -90,7 +90,7 @@ void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
gyroBias.zero();
return;
}
gyroBias = stateStruct.gyro_bias / dtEkfAvg;
gyroBias = stateStruct.gyro_bias.tofloat() / dtEkfAvg;
}
// return body axis gyro scale factor error as a percentage
@ -115,7 +115,7 @@ void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const
// return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF2_core::getQuaternion(Quaternion& ret) const
{
ret = outputDataNew.quat;
ret = outputDataNew.quat.tofloat();
}
// return the amount of yaw angle change due to the last yaw angle reset in radians
@ -130,7 +130,7 @@ uint32_t NavEKF2_core::getLastYawResetAngle(float &yawAng) const
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF2_core::getLastPosNorthEastReset(Vector2f &pos) const
{
pos = posResetNE;
pos = posResetNE.tofloat();
return lastPosReset_ms;
}
@ -146,7 +146,7 @@ uint32_t NavEKF2_core::getLastPosDownReset(float &posD) const
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF2_core::getLastVelNorthEastReset(Vector2f &vel) const
{
vel = velResetNE;
vel = velResetNE.tofloat();
return lastVelReset_ms;
}
@ -164,7 +164,7 @@ void NavEKF2_core::getWind(Vector3f &wind) const
void NavEKF2_core::getVelNED(Vector3f &vel) const
{
// correct for the IMU position offset (EKF calculations are at the IMU)
vel = outputDataNew.velocity + velOffsetNED;
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
}
// return estimate of true airspeed vector in body frame in m/s
@ -174,7 +174,7 @@ bool NavEKF2_core::getAirSpdVec(Vector3f &vel) const
if (inhibitWindStates || PV_AidingMode == AID_NONE) {
return false;
}
vel = outputDataNew.velocity + velOffsetNED;
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
vel.x -= stateStruct.wind_vel.x;
vel.y -= stateStruct.wind_vel.y;
Matrix3f Tnb; // rotation from nav to body frame
@ -218,7 +218,7 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const
if ((dal.gps().status(dal.gps().primary_sensor()) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
const struct Location &gpsloc = dal.gps().location();
const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc);
posNE.x = tempPosNE.x;
posNE.y = tempPosNE.y;
return false;
@ -364,13 +364,13 @@ bool NavEKF2_core::getOriginLLH(struct Location &loc) const
// return earth magnetic field estimates in measurement units / 1000
void NavEKF2_core::getMagNED(Vector3f &magNED) const
{
magNED = stateStruct.earth_magfield * 1000.0f;
magNED = (stateStruct.earth_magfield * 1000.0).tofloat();
}
// return body magnetic field estimates in measurement units / 1000
void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
{
magXYZ = stateStruct.body_magfield*1000.0f;
magXYZ = (stateStruct.body_magfield*1000.0).tofloat();
}
// return magnetometer offsets
@ -383,14 +383,14 @@ bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
// compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
// primary compass is valid and state variances have converged
const float maxMagVar = 5E-6f;
const ftype maxMagVar = 5E-6f;
bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
if ((mag_idx == magSelectIndex) &&
finalInflightMagInit &&
!inhibitMagStates &&
dal.get_compass()->healthy(magSelectIndex) &&
variancesConverged) {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - (stateStruct.body_magfield*1000.0).tofloat();
return true;
} else {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex);
@ -419,15 +419,15 @@ void NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vecto
// also return the delta in position due to the last position reset
void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
velVar = sqrtf(velTestRatio);
posVar = sqrtf(posTestRatio);
hgtVar = sqrtf(hgtTestRatio);
velVar = sqrtF(velTestRatio);
posVar = sqrtF(posTestRatio);
hgtVar = sqrtF(hgtTestRatio);
// If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtf(tasTestRatio);
offset = posResetNE;
magVar.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtF(tasTestRatio);
offset = posResetNE.tofloat();
}
@ -537,14 +537,14 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) const
Vector2f offset;
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
const float mag_max = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
// Only report range finder normalised innovation levels if the EKF needs the data for primary
// height estimation or optical flow operation. This prevents false alarms at the GCS if a
// range finder is fitted for other applications
float temp;
if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
temp = sqrtf(auxRngTestRatio);
temp = sqrtF(auxRngTestRatio);
} else {
temp = 0.0f;
}

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
// storedOutput, outputDataNew and outputDataDelayed are updated with the change in position
// lastPosReset_ms is updated with the time of the reset
void NavEKF2_core::ResetPositionNE(float posN, float posE)
void NavEKF2_core::ResetPositionNE(ftype posN, ftype posE)
{
// Store the position before the reset so that we can record the reset delta
const Vector3f posOrig = stateStruct.position;
const Vector3F posOrig = stateStruct.position;
// Set the position states to the new position
stateStruct.position.x = posN;
@ -245,10 +245,10 @@ void NavEKF2_core::ResetHeight(void)
// posResetD is updated to hold the change in position
// storedOutput, outputDataNew and outputDataDelayed are updated with the change in position
// lastPosResetD_ms is updated with the time of the reset
void NavEKF2_core::ResetPositionD(float posD)
void NavEKF2_core::ResetPositionD(ftype posD)
{
// Store the position before the reset so that we can record the reset delta
const float posDOrig = stateStruct.position.z;
const ftype posDOrig = stateStruct.position.z;
// write to the state vector
stateStruct.position.z = posD;
@ -279,7 +279,7 @@ bool NavEKF2_core::resetHeightDatum(void)
return false;
}
// record the old height estimate
float oldHgt = -stateStruct.position.z;
ftype oldHgt = -stateStruct.position.z;
// reset the barometer so that it reads zero at the current height
dal.baro().update_calibration();
// reset the height state
@ -313,7 +313,7 @@ bool NavEKF2_core::resetHeightDatum(void)
*/
void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
{
const Vector3f &posOffsetBody = dal.gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
const Vector3F posOffsetBody = dal.gps().get_antenna_offset(gpsDataDelayed.sensor_idx).toftype() - accelPosOffset;
if (posOffsetBody.is_zero()) {
return;
}
@ -321,33 +321,33 @@ void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
// Don't fuse velocity data if GPS doesn't support it
if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
Vector3f velOffsetBody = angRate % posOffsetBody;
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
Vector3F velOffsetBody = angRate % posOffsetBody;
Vector3F velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
gps_data.vel.x -= velOffsetEarth.x;
gps_data.vel.y -= velOffsetEarth.y;
gps_data.vel.z -= velOffsetEarth.z;
}
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
gps_data.pos.x -= posOffsetEarth.x;
gps_data.pos.y -= posOffsetEarth.y;
gps_data.hgt += posOffsetEarth.z;
}
// correct external navigation earth-frame position using sensor body-frame offset
void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const
void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3F &ext_position) const
{
#if HAL_VISUALODOM_ENABLED
const auto *visual_odom = dal.visualodom();
if (visual_odom == nullptr) {
return;
}
const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset;
const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
if (posOffsetBody.is_zero()) {
return;
}
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
ext_position.x -= posOffsetEarth.x;
ext_position.y -= posOffsetEarth.y;
ext_position.z -= posOffsetEarth.z;
@ -355,19 +355,19 @@ void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const
}
// correct external navigation earth-frame velocity using sensor body-frame offset
void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3F &ext_velocity) const
{
#if HAL_VISUALODOM_ENABLED
const auto *visual_odom = dal.visualodom();
if (visual_odom == nullptr) {
return;
}
const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset;
const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
if (posOffsetBody.is_zero()) {
return;
}
// TODO use a filtered angular rate with a group delay that matches the sensor delay
const Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
const Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
ext_velocity += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
#endif
}
@ -565,7 +565,7 @@ void NavEKF2_core::FuseVelPosNED()
bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check
// declare variables used to check measurement errors
Vector3f velInnov;
Vector3F velInnov;
// declare variables used to control access to arrays
bool fuseData[6] = {false,false,false,false,false,false};
@ -575,7 +575,7 @@ void NavEKF2_core::FuseVelPosNED()
// declare variables used by state and covariance update calculations
Vector6 R_OBS; // Measurement variances used for fusion
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
float SK;
ftype SK;
// perform sequential fusion of GPS measurements. This assumes that the
// errors in the different velocity and position components are
@ -586,14 +586,14 @@ void NavEKF2_core::FuseVelPosNED()
if (fuseVelData || fusePosData || fuseHgtData) {
// calculate additional error in GPS position caused by manoeuvring
float posErr = frontend->gpsPosVarAccScale * accNavMag;
ftype posErr = frontend->gpsPosVarAccScale * accNavMag;
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
// Use different errors if operating without external aiding using an assumed position or velocity of zero
if (PV_AidingMode == AID_NONE) {
if (tiltAlignComplete && motorsArmed) {
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f));
R_OBS[0] = sq(constrain_ftype(frontend->_noaidHorizNoise, 0.5f, 50.0f));
} else {
// Use a smaller value to give faster initial alignment
R_OBS[0] = sq(0.5f);
@ -606,33 +606,33 @@ void NavEKF2_core::FuseVelPosNED()
} else {
if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
R_OBS[0] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
} else if (extNavVelToFuse) {
R_OBS[2] = R_OBS[0] = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f));
R_OBS[2] = R_OBS[0] = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f));
} else {
// calculate additional error in GPS velocity caused by manoeuvring
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
R_OBS[0] = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
R_OBS[2] = sq(constrain_ftype(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
}
R_OBS[1] = R_OBS[0];
// Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
if (gpsPosAccuracy > 0.0f) {
R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
R_OBS[3] = sq(constrain_ftype(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
} else if (extNavUsedForPos) {
R_OBS[3] = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f));
R_OBS[3] = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f));
} else {
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
R_OBS[3] = sq(constrain_ftype(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
}
R_OBS[4] = R_OBS[3];
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
// For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS perfomrance
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
float obs_data_chk;
ftype obs_data_chk;
if (extNavVelToFuse) {
obs_data_chk = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
obs_data_chk = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
} else {
obs_data_chk = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
obs_data_chk = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
}
R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk;
}
@ -644,8 +644,8 @@ void NavEKF2_core::FuseVelPosNED()
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
// calculate innovations for height and vertical GPS vel measurements
float hgtErr = stateStruct.position.z - velPosObs[5];
float velDErr = stateStruct.velocity.z - velPosObs[2];
ftype hgtErr = stateStruct.position.z - velPosObs[5];
ftype velDErr = stateStruct.velocity.z - velPosObs[2];
// check if they are the same sign and both more than 3-sigma out of bounds
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) {
badIMUdata = true;
@ -663,7 +663,7 @@ void NavEKF2_core::FuseVelPosNED()
varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
// apply an innovation consistency threshold test, but don't fail if bad IMU data
float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
ftype maxPosInnov2 = sq(MAX(0.01f * (ftype)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// use position data if healthy or timed out
@ -703,8 +703,8 @@ void NavEKF2_core::FuseVelPosNED()
!dal.gps().have_vertical_velocity())) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations
float varVelSum = 0; // sum of velocity innovation variances
ftype innovVelSumSq = 0; // sum of squares of velocity innovations
ftype varVelSum = 0; // sum of velocity innovation variances
for (uint8_t i = 0; i<=imax; i++) {
// velocity states start at index 3
stateIndex = i + 3;
@ -718,7 +718,7 @@ void NavEKF2_core::FuseVelPosNED()
}
// apply an innovation consistency threshold test, but don't fail if bad IMU data
// calculate the test ratio
velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (ftype)frontend->_gpsVelInnovGate, 1.0f)));
// fail if the ratio is greater than 1
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
// use velocity data if healthy, timed out, or in constant position mode
@ -744,12 +744,12 @@ void NavEKF2_core::FuseVelPosNED()
innovVelPos[5] = stateStruct.position.z - velPosObs[5];
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio
hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (ftype)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
// when on ground we accept a larger test ratio to allow
// the filter to handle large switch on IMU bias errors
// without rejecting the height sensor
const float maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0 : 1.0;
const ftype maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0 : 1.0;
// fail if the ratio is > maxTestRatio, but don't fail if bad IMU data
hgtHealth = (hgtTestRatio < maxTestRatio) || badIMUdata;
@ -759,9 +759,9 @@ void NavEKF2_core::FuseVelPosNED()
// Calculate a filtered value to be used by pre-flight health checks
// We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
if (onGround) {
float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
const float hgtInnovFiltTC = 2.0f;
float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
ftype dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
const ftype hgtInnovFiltTC = 2.0f;
ftype alpha = constrain_ftype(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
} else {
hgtInnovFiltState = 0.0f;
@ -812,8 +812,8 @@ void NavEKF2_core::FuseVelPosNED()
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
} else if (obsIndex == 5) {
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
const float gndMaxBaroErr = 4.0f;
const float gndBaroInnovFloor = -0.5f;
const ftype gndMaxBaroErr = 4.0f;
const ftype gndBaroInnovFloor = -0.5f;
if(dal.get_touchdown_expected() && activeHgtSource == HGT_SOURCE_BARO) {
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
@ -824,7 +824,7 @@ void NavEKF2_core::FuseVelPosNED()
// ____/|
// / |
// / |
innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
innovVelPos[5] += constrain_ftype(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
}
}
@ -954,9 +954,9 @@ void NavEKF2_core::selectHeightForFusion()
if (_rng && rangeDataToFuse) {
const auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
if (sensor != nullptr) {
Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
Vector3F posOffsetBody = sensor->get_pos_offset().toftype() - accelPosOffset;
if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
}
}
@ -976,16 +976,16 @@ void NavEKF2_core::selectHeightForFusion()
activeHgtSource = HGT_SOURCE_RNG;
} else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) {
// determine if we are above or below the height switch region
float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
ftype rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt;
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
// If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder
// apply a hysteresis to the speed check to prevent rapid switching
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
ftype horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
ftype trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
/*
@ -1030,9 +1030,9 @@ void NavEKF2_core::selectHeightForFusion()
// filtered baro data used to provide a reference for takeoff
// it is is reset to last height measurement on disarming in performArmingChecks()
if (!dal.get_takeoff_expected()) {
const float gndHgtFiltTC = 0.5f;
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
const ftype gndHgtFiltTC = 0.5f;
const ftype dtBaro = frontend->hgtAvg_ms*1.0e-3;
ftype alpha = constrain_ftype(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
}
}
@ -1050,7 +1050,7 @@ void NavEKF2_core::selectHeightForFusion()
// Select the height measurement source
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
hgtMea = -extNavDataDelayed.pos.z;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f));
posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f));
} else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
// using range finder data
// correct for tilt using a flat earth model
@ -1063,7 +1063,7 @@ void NavEKF2_core::selectHeightForFusion()
fuseHgtData = true;
velPosObs[5] = -hgtMea;
// set the observation noise
posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
posDownObsNoise = sq(constrain_ftype(frontend->_rngNoise, 0.1f, 10.0f));
// add uncertainty created by terrain gradient and vehicle tilt
posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
} else {
@ -1078,9 +1078,9 @@ void NavEKF2_core::selectHeightForFusion()
fuseHgtData = true;
// set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
if (gpsHgtAccuracy > 0.0f) {
posDownObsNoise = sq(constrain_float(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
posDownObsNoise = sq(constrain_ftype(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
} else {
posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
posDownObsNoise = sq(constrain_ftype(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
}
} else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) {
// using Baro data
@ -1089,7 +1089,7 @@ void NavEKF2_core::selectHeightForFusion()
velPosObs[5] = -hgtMea;
fuseHgtData = true;
// set the observation noise
posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
posDownObsNoise = sq(constrain_ftype(frontend->_baroAltNoise, 0.1f, 10.0f));
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) {
posDownObsNoise *= frontend->gndEffectBaroScaler;

210
libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
View File

@ -26,14 +26,14 @@ void NavEKF2_core::SelectRngBcnFusion()
void NavEKF2_core::FuseRngBcn()
{
// declarations
float pn;
float pe;
float pd;
float bcn_pn;
float bcn_pe;
float bcn_pd;
const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
float rngPred;
ftype pn;
ftype pe;
ftype pd;
ftype bcn_pn;
ftype bcn_pe;
ftype bcn_pd;
const ftype R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
ftype rngPred;
// health is set bad until test passed
rngBcnHealth = false;
@ -54,7 +54,7 @@ void NavEKF2_core::FuseRngBcn()
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffset;
// predicted range
Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
rngPred = deltaPosNED.length();
// calculate measurement innovation
@ -64,37 +64,37 @@ void NavEKF2_core::FuseRngBcn()
if (rngPred > 0.1f)
{
// calculate observation jacobians
float H_BCN[24] = {};
float t2 = bcn_pd-pd;
float t3 = bcn_pe-pe;
float t4 = bcn_pn-pn;
float t5 = t2*t2;
float t6 = t3*t3;
float t7 = t4*t4;
float t8 = t5+t6+t7;
float t9 = 1.0f/sqrtf(t8);
ftype H_BCN[24] = {};
ftype t2 = bcn_pd-pd;
ftype t3 = bcn_pe-pe;
ftype t4 = bcn_pn-pn;
ftype t5 = t2*t2;
ftype t6 = t3*t3;
ftype t7 = t4*t4;
ftype t8 = t5+t6+t7;
ftype t9 = 1.0f/sqrtF(t8);
H_BCN[6] = -t4*t9;
H_BCN[7] = -t3*t9;
H_BCN[8] = -t2*t9;
// calculate Kalman gains
float t10 = P[8][8]*t2*t9;
float t11 = P[7][8]*t3*t9;
float t12 = P[6][8]*t4*t9;
float t13 = t10+t11+t12;
float t14 = t2*t9*t13;
float t15 = P[8][7]*t2*t9;
float t16 = P[7][7]*t3*t9;
float t17 = P[6][7]*t4*t9;
float t18 = t15+t16+t17;
float t19 = t3*t9*t18;
float t20 = P[8][6]*t2*t9;
float t21 = P[7][6]*t3*t9;
float t22 = P[6][6]*t4*t9;
float t23 = t20+t21+t22;
float t24 = t4*t9*t23;
ftype t10 = P[8][8]*t2*t9;
ftype t11 = P[7][8]*t3*t9;
ftype t12 = P[6][8]*t4*t9;
ftype t13 = t10+t11+t12;
ftype t14 = t2*t9*t13;
ftype t15 = P[8][7]*t2*t9;
ftype t16 = P[7][7]*t3*t9;
ftype t17 = P[6][7]*t4*t9;
ftype t18 = t15+t16+t17;
ftype t19 = t3*t9*t18;
ftype t20 = P[8][6]*t2*t9;
ftype t21 = P[7][6]*t3*t9;
ftype t22 = P[6][6]*t4*t9;
ftype t23 = t20+t21+t22;
ftype t24 = t4*t9*t23;
varInnovRngBcn = R_BCN+t14+t19+t24;
float t26;
ftype t26;
if (varInnovRngBcn >= R_BCN) {
t26 = 1.0/varInnovRngBcn;
faultStatus.bad_rngbcn = false;
@ -141,18 +141,18 @@ void NavEKF2_core::FuseRngBcn()
Kfusion[20] = -t26*(P[20][6]*t4*t9+P[20][7]*t3*t9+P[20][8]*t2*t9);
Kfusion[21] = -t26*(P[21][6]*t4*t9+P[21][7]*t3*t9+P[21][8]*t2*t9);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
// zero indexes 16 to 21 = 6
zero_range(&Kfusion[0], 16, 21);
}
Kfusion[22] = -t26*(P[22][6]*t4*t9+P[22][7]*t3*t9+P[22][8]*t2*t9);
Kfusion[23] = -t26*(P[23][6]*t4*t9+P[23][7]*t3*t9+P[23][8]*t2*t9);
// Calculate innovation using the selected offset value
Vector3f delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
Vector3F delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
// calculate the innovation consistency test ratio
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
// fail if the ratio is > 1, but don't fail if bad IMU data
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
@ -245,7 +245,7 @@ https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon
void NavEKF2_core::FuseRngBcnStatic()
{
// get the estimated range measurement variance
const float R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
const ftype R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
/*
The first thing to do is to check if we have started the alignment and if not, initialise the
@ -269,7 +269,7 @@ void NavEKF2_core::FuseRngBcnStatic()
}
if (numBcnMeas >= 100) {
rngBcnAlignmentStarted = true;
float tempVar = 1.0f / (float)numBcnMeas;
ftype tempVar = 1.0f / (float)numBcnMeas;
// initialise the receiver position to the centre of the beacons and at zero height
receiverPos.x = rngBcnPosSum.x * tempVar;
receiverPos.y = rngBcnPosSum.y * tempVar;
@ -298,7 +298,7 @@ void NavEKF2_core::FuseRngBcnStatic()
// position offset to be applied to the beacon system that minimises the range innovations
// The position estimate should be stable after 100 iterations so we use a simple dual
// hypothesis 1-state EKF to estimate the offset
Vector3f refPosNED;
Vector3F refPosNED;
refPosNED.x = receiverPos.x;
refPosNED.y = receiverPos.y;
refPosNED.z = stateStruct.position.z;
@ -317,14 +317,14 @@ void NavEKF2_core::FuseRngBcnStatic()
// and the main EKF vertical position
// Calculate the mid vertical position of all beacons
float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
// calculate the delta to the estimated receiver position
float delta = receiverPos.z - bcnMidPosD;
ftype delta = receiverPos.z - bcnMidPosD;
// calcuate the two hypothesis for our vertical position
float receverPosDownMax;
float receverPosDownMin;
ftype receverPosDownMax;
ftype receverPosDownMin;
if (delta >= 0.0f) {
receverPosDownMax = receiverPos.z;
receverPosDownMin = receiverPos.z - 2.0f * delta;
@ -347,57 +347,57 @@ void NavEKF2_core::FuseRngBcnStatic()
}
// calculate the observation jacobian
float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset;
float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
float t5 = t2*t2;
float t6 = t3*t3;
float t7 = t4*t4;
float t8 = t5+t6+t7;
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset;
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
ftype t5 = t2*t2;
ftype t6 = t3*t3;
ftype t7 = t4*t4;
ftype t8 = t5+t6+t7;
if (t8 < 0.1f) {
// calculation will be badly conditioned
return;
}
float t9 = 1.0f/sqrtf(t8);
float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
float t15 = receiverPos.x*2.0f;
float t11 = t10-t15;
float t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
float t14 = receiverPos.y*2.0f;
float t13 = t12-t14;
float t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
float t18 = receiverPos.z*2.0f;
float t17 = t16-t18;
float H_RNG[3];
ftype t9 = 1.0f/sqrtF(t8);
ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
ftype t15 = receiverPos.x*2.0f;
ftype t11 = t10-t15;
ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
ftype t14 = receiverPos.y*2.0f;
ftype t13 = t12-t14;
ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
ftype t18 = receiverPos.z*2.0f;
ftype t17 = t16-t18;
ftype H_RNG[3];
H_RNG[0] = -t9*t11*0.5f;
H_RNG[1] = -t9*t13*0.5f;
H_RNG[2] = -t9*t17*0.5f;
// calculate the Kalman gains
float t19 = receiverPosCov[0][0]*t9*t11*0.5f;
float t20 = receiverPosCov[1][1]*t9*t13*0.5f;
float t21 = receiverPosCov[0][1]*t9*t11*0.5f;
float t22 = receiverPosCov[2][1]*t9*t17*0.5f;
float t23 = t20+t21+t22;
float t24 = t9*t13*t23*0.5f;
float t25 = receiverPosCov[1][2]*t9*t13*0.5f;
float t26 = receiverPosCov[0][2]*t9*t11*0.5f;
float t27 = receiverPosCov[2][2]*t9*t17*0.5f;
float t28 = t25+t26+t27;
float t29 = t9*t17*t28*0.5f;
float t30 = receiverPosCov[1][0]*t9*t13*0.5f;
float t31 = receiverPosCov[2][0]*t9*t17*0.5f;
float t32 = t19+t30+t31;
float t33 = t9*t11*t32*0.5f;
ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f;
ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f;
ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f;
ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f;
ftype t23 = t20+t21+t22;
ftype t24 = t9*t13*t23*0.5f;
ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f;
ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f;
ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f;
ftype t28 = t25+t26+t27;
ftype t29 = t9*t17*t28*0.5f;
ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f;
ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f;
ftype t32 = t19+t30+t31;
ftype t33 = t9*t11*t32*0.5f;
varInnovRngBcn = R_RNG+t24+t29+t33;
float t35 = 1.0f/varInnovRngBcn;
float K_RNG[3];
ftype t35 = 1.0f/varInnovRngBcn;
ftype K_RNG[3];
K_RNG[0] = -t35*(t19+receiverPosCov[0][1]*t9*t13*0.5f+receiverPosCov[0][2]*t9*t17*0.5f);
K_RNG[1] = -t35*(t20+receiverPosCov[1][0]*t9*t11*0.5f+receiverPosCov[1][2]*t9*t17*0.5f);
K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f);
// calculate range measurement innovation
Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
deltaPosNED.z -= bcnPosOffset;
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
@ -440,7 +440,7 @@ void NavEKF2_core::FuseRngBcnStatic()
// ensure the covariance matrix is symmetric
for (uint8_t i=1; i<=2; i++) {
for (uint8_t j=0; j<=i-1; j++) {
float temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
ftype temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
receiverPosCov[i][j] = temp;
receiverPosCov[j][i] = temp;
}
@ -458,7 +458,7 @@ void NavEKF2_core::FuseRngBcnStatic()
Run a single state Kalman filter to estimate the vertical position offset of the range beacon constellation
Calculate using a high and low hypothesis and select the hypothesis with the lowest innovation sequence
*/
void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning)
void NavEKF2_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning)
{
// Handle height offsets between the primary height source and the range beacons by estimating
// the beacon systems global vertical position offset using a single state Kalman filter
@ -466,35 +466,35 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// A high and low estimate is calculated to handle the ambiguity in height associated with beacon positions that are co-planar
// The main filter then uses the offset with the smaller innovations
float innov; // range measurement innovation (m)
float innovVar; // range measurement innovation variance (m^2)
float gain; // Kalman gain
float obsDeriv; // derivative of observation relative to state
ftype innov; // range measurement innovation (m)
ftype innovVar; // range measurement innovation variance (m^2)
ftype gain; // Kalman gain
ftype obsDeriv; // derivative of observation relative to state
const float stateNoiseVar = 0.1f; // State process noise variance
const float filtAlpha = 0.01f; // LPF constant
const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std
const ftype stateNoiseVar = 0.1f; // State process noise variance
const ftype filtAlpha = 0.01f; // LPF constant
const ftype innovGateWidth = 5.0f; // width of innovation consistency check gate in std
// estimate upper value for offset
// calculate observation derivative
float t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosOffsetMax;
float t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
float t5 = t2*t2;
float t6 = t3*t3;
float t7 = t4*t4;
float t8 = t5+t6+t7;
float t9;
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosOffsetMax;
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
ftype t5 = t2*t2;
ftype t6 = t3*t3;
ftype t7 = t4*t4;
ftype t8 = t5+t6+t7;
ftype t9;
if (t8 > 0.1f) {
t9 = 1.0f/sqrtf(t8);
t9 = 1.0f/sqrtF(t8);
obsDeriv = t2*t9;
// Calculate innovation
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
innov = sqrtF(t8) - rngBcnDataDelayed.rng;
// calculate a filtered innovation magnitude to be used to select between the high or low offset
OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsf(innov);
OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsF(innov);
// covariance prediction
bcnPosOffsetMaxVar += stateNoiseVar;
@ -524,14 +524,14 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
t5 = t2*t2;
t8 = t5+t6+t7;
if (t8 > 0.1f) {
t9 = 1.0f/sqrtf(t8);
t9 = 1.0f/sqrtF(t8);
obsDeriv = t2*t9;
// Calculate innovation
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
innov = sqrtF(t8) - rngBcnDataDelayed.rng;
// calculate a filtered innovation magnitude to be used to select between the high or low offset
OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsf(innov);
OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsF(innov);
// covariance prediction
bcnPosOffsetMinVar += stateNoiseVar;
@ -555,7 +555,7 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
}
// calculate the mid vertical position of all beacons
float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
// ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle
bcnPosOffsetMax = MAX(bcnPosOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);

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
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
ftype checkScaler = 0.01f*(ftype)frontend->_gpsCheckScaler;
if (gpsGoodToAlign) {
/*
@ -55,9 +55,9 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary
const struct Location &gpsloc = gps.location(); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
const ftype posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsed since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
ftype deltaTime = constrain_ftype(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved
gpsDriftNE += gpsloc_prev.get_distance(gpsloc);
@ -85,8 +85,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
if (gps.have_vertical_velocity() && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
gpsVertVelFilt = constrain_ftype(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsF(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else {
gpsVertVelFail = false;
}
@ -95,7 +95,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
if (gpsVertVelFail) {
dal.snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsF(gpsVertVelFilt), (double)(0.3f*checkScaler));
gpsCheckStatus.bad_vert_vel = true;
} else {
gpsCheckStatus.bad_vert_vel = false;
@ -106,8 +106,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
bool gpsHorizVelFail;
if (onGround) {
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
} else {
gpsHorizVelFail = false;
}
@ -123,7 +123,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
}
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
float hAcc = 0.0;
bool hAccFail;
if (gps.horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
@ -243,14 +243,14 @@ void NavEKF2_core::calcGpsGoodForFlight(void)
// use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks
// set up varaibles and constants used by filter that is applied to GPS speed accuracy
const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
const float tau = 10.0f; // time constant (sec) of peak hold decay
const ftype alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
const ftype tau = 10.0f; // time constant (sec) of peak hold decay
if (lastGpsCheckTime_ms == 0) {
lastGpsCheckTime_ms = imuSampleTime_ms;
}
float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
ftype dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
lastGpsCheckTime_ms = imuSampleTime_ms;
float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f);
ftype alpha2 = constrain_ftype(dtLPF/tau,0.0f,1.0f);
// get the receivers reported speed accuracy
float gpsSpdAccRaw;
@ -259,7 +259,7 @@ void NavEKF2_core::calcGpsGoodForFlight(void)
}
// filter the raw speed accuracy using a LPF
sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
sAccFilterState1 = constrain_ftype((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
// apply a peak hold filter to the LPF output
sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
@ -308,7 +308,7 @@ void NavEKF2_core::detectFlight()
if (assume_zero_sideslip()) {
// To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
ftype gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
bool highGndSpd = false;
bool highAirSpd = false;
bool largeHgtChange = false;
@ -327,7 +327,7 @@ void NavEKF2_core::detectFlight()
}
// trigger if more than 10m away from initial height
if (fabsf(hgtMea) > 10.0f) {
if (fabsF(hgtMea) > 10.0f) {
largeHgtChange = true;
}

238
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

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

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
#define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
// limit on horizontal position states
#if HAL_WITH_EKF_DOUBLE
#define EK2_POSXY_STATE_LIMIT 50.0e6
#else
#define EK2_POSXY_STATE_LIMIT 1.0e6
#endif
class AP_AHRS;
class NavEKF2_core : public NavEKF_core_common
@ -87,7 +94,7 @@ public:
// Return a consolidated error score where higher numbers are less healthy
// Intended to be used by the front-end to determine which is the primary EKF
float errorScore(void) const;
ftype errorScore(void) const;
// Write the last calculated NE position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false
@ -383,16 +390,16 @@ private:
// broken down as individual elements. Both are equivalent (same
// memory)
struct state_elements {
Vector3f angErr; // 0..2
Vector3f velocity; // 3..5
Vector3f position; // 6..8
Vector3f gyro_bias; // 9..11
Vector3f gyro_scale; // 12..14
float accel_zbias; // 15
Vector3f earth_magfield; // 16..18
Vector3f body_magfield; // 19..21
Vector2f wind_vel; // 22..23
Quaternion quat; // 24..27
Vector3F angErr; // 0..2
Vector3F velocity; // 3..5
Vector3F position; // 6..8
Vector3F gyro_bias; // 9..11
Vector3F gyro_scale; // 12..14
ftype accel_zbias; // 15
Vector3F earth_magfield; // 16..18
Vector3F body_magfield; // 19..21
Vector2F wind_vel; // 22..23
QuaternionF quat; // 24..27
};
union {
@ -401,78 +408,78 @@ private:
};
struct output_elements {
Quaternion quat; // 0..3
Vector3f velocity; // 4..6
Vector3f position; // 7..9
QuaternionF quat; // 0..3
Vector3F velocity; // 4..6
Vector3F position; // 7..9
};
struct imu_elements {
Vector3f delAng; // 0..2
Vector3f delVel; // 3..5
float delAngDT; // 6
float delVelDT; // 7
Vector3F delAng; // 0..2
Vector3F delVel; // 3..5
ftype delAngDT; // 6
ftype delVelDT; // 7
uint32_t time_ms; // 8
uint8_t gyro_index;
uint8_t accel_index;
};
struct gps_elements : EKF_obs_element_t {
Vector2f pos;
float hgt;
Vector3f vel;
Vector2F pos;
ftype hgt;
Vector3F vel;
uint8_t sensor_idx;
};
struct mag_elements : EKF_obs_element_t {
Vector3f mag;
Vector3F mag;
};
struct baro_elements : EKF_obs_element_t {
float hgt;
ftype hgt;
};
struct range_elements : EKF_obs_element_t {
float rng;
ftype rng;
uint8_t sensor_idx;
};
struct rng_bcn_elements : EKF_obs_element_t {
float rng; // range measurement to each beacon (m)
Vector3f beacon_posNED; // NED position of the beacon (m)
float rngErr; // range measurement error 1-std (m)
ftype rng; // range measurement to each beacon (m)
Vector3F beacon_posNED; // NED position of the beacon (m)
ftype rngErr; // range measurement error 1-std (m)
uint8_t beacon_ID; // beacon identification number
};
struct tas_elements : EKF_obs_element_t {
float tas;
ftype tas;
};
struct of_elements : EKF_obs_element_t {
Vector2f flowRadXY;
Vector2f flowRadXYcomp;
Vector3f bodyRadXYZ;
Vector3f body_offset;
Vector2F flowRadXY;
Vector2F flowRadXYcomp;
Vector3F bodyRadXYZ;
Vector3F body_offset;
};
struct ext_nav_elements : EKF_obs_element_t {
Vector3f pos; // XYZ position measured in a RH navigation frame (m)
Quaternion quat; // quaternion describing the rotation from navigation to body frame
float posErr; // spherical poition measurement error 1-std (m)
float angErr; // spherical angular measurement error 1-std (rad)
Vector3F pos; // XYZ position measured in a RH navigation frame (m)
QuaternionF quat; // quaternion describing the rotation from navigation to body frame
ftype posErr; // spherical poition measurement error 1-std (m)
ftype angErr; // spherical angular measurement error 1-std (rad)
bool posReset; // true when the position measurement has been reset
};
// bias estimates for the IMUs that are enabled but not being used
// by this core.
struct {
Vector3f gyro_bias;
Vector3f gyro_scale;
float accel_zbias;
Vector3F gyro_bias;
Vector3F gyro_scale;
ftype accel_zbias;
} inactiveBias[INS_MAX_INSTANCES];
struct ext_nav_vel_elements : EKF_obs_element_t {
Vector3f vel; // velocity in NED (m)
float err; // velocity measurement error (m/s)
Vector3F vel; // velocity in NED (m)
ftype err; // velocity measurement error (m/s)
};
// update the navigation filter status
@ -509,7 +516,7 @@ private:
void FuseRngBcnStatic();
// calculate the offset from EKF vertical position datum to the range beacon system datum
void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning);
void CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning);
// fuse magnetometer measurements
void FuseMagnetometer();
@ -533,21 +540,21 @@ private:
void StoreQuatReset(void);
// Rotate the stored output quaternion history through a quaternion rotation
void StoreQuatRotate(const Quaternion &deltaQuat);
void StoreQuatRotate(const QuaternionF &deltaQuat);
// calculate the NED earth spin vector in rad/sec
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
void calcEarthRateNED(Vector3F &omega, int32_t latitude) const;
// initialise the covariance matrix
void CovarianceInit();
// helper functions for readIMUData
bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt);
bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt);
bool readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt);
bool readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt);
// helper functions for correcting IMU data
void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index);
void correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index);
void correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index);
void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index);
// update IMU delta angle and delta velocity measurements
void readIMUData();
@ -593,7 +600,7 @@ private:
// initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
// and return attitude quaternion
Quaternion calcQuatAndFieldStates(float roll, float pitch);
QuaternionF calcQuatAndFieldStates(ftype roll, ftype pitch);
// zero stored variables
void InitialiseVariables();
@ -604,7 +611,7 @@ private:
void ResetPosition(void);
// reset the stateStruct's NE position to the specified position
void ResetPositionNE(float posN, float posE);
void ResetPositionNE(ftype posN, ftype posE);
// reset velocity states using the last GPS measurement
void ResetVelocity(void);
@ -613,7 +620,7 @@ private:
void ResetHeight(void);
// reset the stateStruct's D position
void ResetPositionD(float posD);
void ResetPositionD(ftype posD);
// return true if we should use the airspeed sensor
bool useAirspeed(void) const;
@ -631,7 +638,7 @@ private:
void checkDivergence(void);
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
void calcIMU_Weighting(float K1, float K2);
void calcIMU_Weighting(ftype K1, ftype K2);
// return true if optical flow data is available
bool optFlowDataPresent(void) const;
@ -697,10 +704,10 @@ private:
// Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
// Input is 1-sigma uncertainty in published declination
void FuseDeclination(float declErr);
void FuseDeclination(ftype declErr);
// return magnetic declination in radians
float MagDeclination(void) const;
ftype MagDeclination(void) const;
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer
@ -734,10 +741,10 @@ private:
void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
// correct external navigation earth-frame position using sensor body-frame offset
void CorrectExtNavForSensorOffset(Vector3f &ext_position) const;
void CorrectExtNavForSensorOffset(Vector3F &ext_position) const;
// correct external navigation earth-frame velocity using sensor body-frame offset
void CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const;
void CorrectExtNavVelForSensorOffset(Vector3F &ext_velocity) const;
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
@ -753,7 +760,7 @@ private:
// yaw : new yaw angle (rad)
// yaw_variance : variance of new yaw angle (rad^2)
// isDeltaYaw : true when the yaw should be added to the existing yaw angle
void resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw);
void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, bool isDeltaYaw);
// attempt to reset the yaw to the EKF-GSF value
// returns false if unsuccessful
@ -775,7 +782,7 @@ private:
bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
bool badIMUdata; // boolean true if the bad IMU data is detected
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
ftype gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Matrix24 P; // covariance matrix
EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU 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<range_elements> storedRange; // Range finder data 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 accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3F earthRateNED; // earths angular rate vector in NED (rad/s)
ftype dtIMUavg; // expected time between IMU measurements (sec)
ftype dtEkfAvg; // expected time between EKF updates (sec)
ftype dt; // time lapsed since the last covariance prediction (sec)
@ -804,8 +811,8 @@ private:
bool fuseVelData; // this boolean causes the velNED measurements to be fused
bool fusePosData; // this boolean causes the posNE measurements to be fused
bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
Vector3F innovMag; // innovation output from fusion of X,Y,Z compass measurements
Vector3F varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
ftype innovVtas; // innovation output from fusion of airspeed measurements
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
bool magFusePerformed; // boolean set to true when magnetometer fusion has been performed in that time step
@ -813,8 +820,8 @@ private:
uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
uint32_t lastMagUpdate_us; // last time compass was updated in usec
uint32_t lastMagRead_ms; // last time compass data was successfully read
Vector3f velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED
Vector3F velDotNED; // rate of change of velocity in NED frame
Vector3F velDotNEDfilt; // low pass filtered velDotNED
uint32_t imuSampleTime_ms; // time that the last IMU value was taken
bool tasDataToFuse; // true when new airspeed data is waiting to be fused
uint32_t lastBaroReceived_ms; // time last time we received baro height data
@ -829,13 +836,13 @@ private:
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t lastYawTime_ms; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Vector2f lastKnownPositionNE; // last known position
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
Vector2F lastKnownPositionNE; // last known position
ftype velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
ftype posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
ftype defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool lastInhibitMagStates; // previous inhibitMagStates
@ -844,18 +851,18 @@ private:
uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
struct Location EKF_origin; // LLH origin of the NED axis system
bool validOrigin; // true when the EKF origin is valid
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
float gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
ftype gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
uint32_t lastGpsVelPass_ms; // time of last GPS vertical velocity consistency check pass
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
float posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
ftype posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
bool useGpsVertVel; // true if GPS vertical velocity should be used
float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
ftype yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
Vector3f tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion
float tiltErrFilt; // Filtered tilt error metric
Vector3F tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion
ftype tiltErrFilt; // Filtered tilt error metric
bool tiltAlignComplete; // true when tilt alignment is complete
bool yawAlignComplete; // true when yaw alignment is complete
bool magStateInitComplete; // true when the magnetic field sttes have been initialised
@ -863,7 +870,7 @@ private:
imu_elements imuDataDelayed; // IMU data at the fusion time horizon
imu_elements imuDataNew; // IMU data at the current time horizon
imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate
Quaternion imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
QuaternionF imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
baro_elements baroDataNew; // Baro data at the current time horizon
baro_elements baroDataDelayed; // Baro data at the fusion time horizon
range_elements rangeDataNew; // Range finder data at the current time horizon
@ -875,10 +882,10 @@ private:
gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
output_elements outputDataNew; // output state data at the current time step
output_elements outputDataDelayed; // output state data at the current time step
Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
Vector3f velErrintegral; // integral of output predictor NED velocity tracking error (m)
Vector3f posErrintegral; // integral of output predictor NED position tracking error (m.sec)
float innovYaw; // compass yaw angle innovation (rad)
Vector3F delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
Vector3F velErrintegral; // integral of output predictor NED velocity tracking error (m)
Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec)
ftype innovYaw; // compass yaw angle innovation (rad)
uint32_t timeTasReceived_ms; // time last TAS data was received (msec)
bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system
uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
@ -889,57 +896,57 @@ private:
bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed
bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
Vector3F lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized
Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
Vector2F posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
uint32_t lastPosReset_ms; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
Vector2f velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
Vector2F velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
float posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
ftype posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
uint32_t lastPosResetD_ms; // System time at which the last position reset occurred. Returned by getLastPosDownReset
float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
Quaternion prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
ftype yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
QuaternionF prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
ftype hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF
bool runUpdates; // boolean true when the EKF updates can be run
uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele
uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
ftype posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3F delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3F delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
bool magFieldLearned; // true when the magnetic field has been learned
uint32_t wasLearningCompass_ms; // time when we were last waiting for compass learn to complete
Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
Vector3F earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
Vector3F bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
bool delAngBiasLearned; // true when the gyro bias has been learned
nav_filter_status filterStatus; // contains the status of various filter outputs
float ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
ftype ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
double ekfGpsRefHgt; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m)
uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected
Vector3f outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
Vector3F outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
Vector3F velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3F posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
// variables used to calculate a vertical velocity that is kinematically consistent with the verical position
struct {
float pos;
float vel;
float acc;
ftype pos;
ftype vel;
ftype acc;
} vertCompFiltState;
// variables used by the pre-initialisation GPS checks
struct Location gpsloc_prev; // LLH location of previous GPS measurement
uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks
float gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
float gpsVertVelFilt; // amount of filterred vertical GPS velocity detected durng pre-flight GPS checks
float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
ftype gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
ftype gpsVertVelFilt; // amount of filterred vertical GPS velocity detected durng pre-flight GPS checks
ftype gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
// variable used by the in-flight GPS quality check
bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks
float sAccFilterState1; // state variable for LPF applid to reported GPS speed accuracy
float sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
ftype sAccFilterState1; // state variable for LPF applid to reported GPS speed accuracy
ftype sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked
uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed
uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed
@ -951,28 +958,28 @@ private:
of_elements ofDataDelayed; // OF data at the fusion time horizon
bool flowDataToFuse; // true when optical flow data is ready for fusion
bool flowDataValid; // true while optical flow data is still fresh
Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
Vector2F auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
Matrix3F Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
float Popt; // Optical flow terrain height state covariance (m^2)
float terrainState; // terrain position state (m)
float prevPosN; // north position at last measurement
float prevPosE; // east position at last measurement
float varInnovRng; // range finder observation innovation variance (m^2)
float innovRng; // range finder observation innovation (m)
float hgtMea; // height measurement derived from either baro, gps or range finder data (m)
ftype Popt; // Optical flow terrain height state covariance (m^2)
ftype terrainState; // terrain position state (m)
ftype prevPosN; // north position at last measurement
ftype prevPosE; // east position at last measurement
ftype varInnovRng; // range finder observation innovation variance (m^2)
ftype innovRng; // range finder observation innovation (m)
ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m)
bool inhibitGndState; // true when the terrain position state is to remain constant
uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2f auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2f flowGyroBias; // bias error of optical flow sensor gyro output
Vector2F auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
ftype R_LOS; // variance of optical flow rate measurements (rad/sec)^2
ftype auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2F flowGyroBias; // bias error of optical flow sensor gyro output
bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon.
bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon.
bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon.
@ -984,15 +991,15 @@ private:
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions
bool gndOffsetValid; // true when the ground offset state can still be considered valid
Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
float delTimeOF; // time that delAngBodyOF is summed across
Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m)
Vector3F delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
ftype delTimeOF; // time that delAngBodyOF is summed across
Vector3F accelPosOffset; // position of IMU accelerometer unit in body frame (m)
// Range finder
float baroHgtOffset; // offset applied when when switching to use of Baro height
float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
float storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors
ftype baroHgtOffset; // offset applied when when switching to use of Baro height
ftype rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
ftype storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors
uint32_t storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors
uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors
@ -1003,47 +1010,47 @@ private:
rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innvovation consistency checks (msec)
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
ftype rngBcnTestRatio; // Innovation test ratio for range beacon measurements
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
bool rngBcnTimeout; // boolean true if range beacon measurements have faled innovation consistency checks for too long
float varInnovRngBcn; // range beacon observation innovation variance (m^2)
float innovRngBcn; // range beacon observation innovation (m)
ftype varInnovRngBcn; // range beacon observation innovation variance (m^2)
ftype innovRngBcn; // range beacon observation innovation (m)
uint32_t lastTimeRngBcn_ms[10]; // last time we received a range beacon measurement (msec)
bool rngBcnDataToFuse; // true when there is new range beacon data to fuse
Vector3f beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
float beaconVehiclePosErr; // estimated position error from the beacon system (m)
Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
ftype beaconVehiclePosErr; // estimated position error from the beacon system (m)
uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec)
bool rngBcnGoodToAlign; // true when the range beacon systems 3D fix can be used to align the filter
uint8_t lastRngBcnChecked; // index of the last range beacon checked for data
Vector3f receiverPos; // receiver NED position (m) - alignment 3 state filter
float receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
bool rngBcnAlignmentStarted; // True when the initial position alignment using range measurements has started
bool rngBcnAlignmentCompleted; // True when the initial position alignment using range measurements has finished
uint8_t lastBeaconIndex; // Range beacon index last read - used during initialisation of the 3-state filter
Vector3f rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
Vector3F rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
uint8_t numBcnMeas; // Number of beacon measurements - used during initialisation of the 3-state filter
float rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
ftype rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
uint8_t N_beacons; // Number of range beacons in use
float maxBcnPosD; // maximum position of all beacons in the down direction (m)
float minBcnPosD; // minimum position of all beacons in the down direction (m)
float bcnPosOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype maxBcnPosD; // maximum position of all beacons in the down direction (m)
ftype minBcnPosD; // minimum position of all beacons in the down direction (m)
ftype bcnPosOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMaxVar; // Variance of the bcnPosOffsetHigh state (m)
float OffsetMaxInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetHigh
ftype bcnPosOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMaxVar; // Variance of the bcnPosOffsetHigh state (m)
ftype OffsetMaxInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetHigh
float bcnPosOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMinVar; // Variance of the bcnPosoffset state (m)
float OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow
ftype bcnPosOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMinVar; // Variance of the bcnPosoffset state (m)
ftype OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow
// Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
struct rngBcnFusionReport_t {
float rng; // measured range to beacon (m)
float innov; // range innovation (m)
float innovVar; // innovation variance (m^2)
float testRatio; // innovation consistency test ratio
Vector3f beaconPosNED; // beacon NED position
ftype rng; // measured range to beacon (m)
ftype innov; // range innovation (m)
ftype innovVar; // innovation variance (m^2)
ftype testRatio; // innovation consistency test ratio
Vector3F beaconPosNED; // beacon NED position
} rngBcnFusionReport[10];
// height source selection logic
@ -1051,11 +1058,11 @@ private:
// Movement detector
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected
float rngAtStartOfFlight; // range finder measurement at start of flight
ftype rngAtStartOfFlight; // range finder measurement at start of flight
uint32_t timeAtArming_ms; // time in msec that the vehicle armed
// baro ground effect
float meaHgtAtTakeOff; // height measured at commencement of takeoff
ftype meaHgtAtTakeOff; // height measured at commencement of takeoff
// control of post takeoff magnetic field and heading resets
bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed
@ -1063,9 +1070,9 @@ private:
bool magStateResetRequest; // true if magnetic field states need to be reset using the magneteomter measurements
bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
float posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
float yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
Quaternion quatAtLastMagReset; // quaternion states last time the mag states were reset
ftype posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
ftype yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
QuaternionF quatAtLastMagReset; // quaternion states last time the mag states were reset
uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent
// external navigation fusion
@ -1136,8 +1143,8 @@ private:
ftype magXbias;
ftype magYbias;
ftype magZbias;
Matrix3f DCM;
Vector3f MagPred;
Matrix3F DCM;
Vector3F MagPred;
ftype R_MAG;
Vector9 SH_MAG;
} mag_state;
@ -1147,8 +1154,8 @@ private:
// earth field from WMM tables
bool have_table_earth_field; // true when we have initialised table_earth_field_ga
Vector3f table_earth_field_ga; // earth field from WMM tables
float table_declination; // declination in radians from the tables
Vector3F table_earth_field_ga; // earth field from WMM tables
ftype table_declination; // declination in radians from the tables
// timing statistics
struct ekf_timing timing;
@ -1160,7 +1167,7 @@ private:
bool assume_zero_sideslip(void) const;
// vehicle specific initial gyro bias uncertainty
float InitialGyroBiasUncertainty(void) const;
ftype InitialGyroBiasUncertainty(void) const;
// The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF
uint32_t EKFGSF_yaw_reset_ms; // timestamp of last emergency yaw reset (uSec)

6
libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp
View File

@ -16,7 +16,7 @@ void NavEKF2_core::resetGyroBias(void)
zeroRows(P,9,11);
zeroCols(P,9,11);
P[9][9] = sq(radians(0.5f * dtIMUavg));
P[9][9] = sq(radians(0.5 * dtIMUavg));
P[10][10] = P[9][9];
P[11][11] = P[9][9];
}
@ -24,8 +24,8 @@ void NavEKF2_core::resetGyroBias(void)
/*
vehicle specific initial gyro bias uncertainty in deg/sec
*/
float NavEKF2_core::InitialGyroBiasUncertainty(void) const
ftype NavEKF2_core::InitialGyroBiasUncertainty(void) const
{
return 2.5f;
return 2.5;
}