Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_NavEKF3 Prevent rejection of airspeed when dead reckoning #23999

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+31 −19
Open

AP_NavEKF3 Prevent rejection of airspeed when dead reckoning #23999

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
91af61e
AP_NavEKF3: When dead reckoning, clip but don't reject airspeed
priseborough Jun 7, 2023
3935648
AP_NavEKF3: Scale innovation variance instead of clipping aspd innova…
priseborough Jun 7, 2023
031de06
AP_NavEKF3: Use a smaller airspeed observation gate when dead reckoning
priseborough Jun 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 31 additions & 19 deletions libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,11 @@ void NavEKF3_core::FuseAirspeed()
H_TAS[6] = vd*SH_TAS[0];
H_TAS[22] = -SH_TAS[2];
H_TAS[23] = -SH_TAS[1];
// calculate Kalman gains
ftype temp = (tasDataDelayed.tasVariance + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
if (temp >= tasDataDelayed.tasVariance) {
SK_TAS[0] = 1.0f / temp;

// calculate innovation variance
varInnovVtas = (tasDataDelayed.tasVariance + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
if (varInnovVtas >= tasDataDelayed.tasVariance) {
SK_TAS[0] = 1.0f / varInnovVtas;
faultStatus.bad_airspeed = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
Expand All @@ -66,6 +67,32 @@ void NavEKF3_core::FuseAirspeed()
faultStatus.bad_airspeed = true;
return;
}

// Calculate the innovation consistency test ratio.
// When dead reckoning, scale up innovation variance if innovations are larger than the gate
// and do not reject the observation to reduce the impact of bad airspeed data or sudden
// wind speed changes on the ability to continue dead reckoning navigation.
const bool doingAirDataDeadReckoning = posTimeout || (PV_AidingMode != AID_ABSOLUTE);
// Use a smaller gate when dead reckoning to prevent rapid changes to velocity states
// due to gusts or airspeed errors.
const ftype gateStdDev = doingAirDataDeadReckoning ? 1.0f : MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f);
tasTestRatio = sq(innovVtas) / (sq(gateStdDev) * varInnovVtas);
if (doingAirDataDeadReckoning && tasTestRatio > 1.0f) {
// Adjust the Kalman gains equivalent to increasing the observation noise variance to the
// smallest value that would result in the observation being accepted.
SK_TAS[0] /= tasTestRatio;
}

// Reject the observation if the test ratio is > 1, but don't fail if bad IMU data or doing air data dead reckoning
const bool isConsistent = (tasTestRatio < 1.0f) || badIMUdata || doingAirDataDeadReckoning;
tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms;
if (!isConsistent) {
lastTasFailTime_ms = imuSampleTime_ms;
} else {
lastTasFailTime_ms = 0;
}

// Calculate Kalman gains
SK_TAS[1] = SH_TAS[1];

if (tasDataDelayed.allowFusion && !airDataFusionWindOnly) {
Expand Down Expand Up @@ -128,21 +155,6 @@ void NavEKF3_core::FuseAirspeed()
zero_range(&Kfusion[0], 22, 23);
}

// calculate measurement innovation variance
varInnovVtas = 1.0f/SK_TAS[0];

// calculate the innovation consistency test ratio
tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);

// fail if the ratio is > 1, but don't fail if bad IMU data
const bool isConsistent = (tasTestRatio < 1.0f) || badIMUdata;
tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms;
if (!isConsistent) {
lastTasFailTime_ms = imuSampleTime_ms;
} else {
lastTasFailTime_ms = 0;
}

// test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
if (tasDataDelayed.allowFusion && (isConsistent || (tasTimeout && posTimeout))) {

Expand Down