From 91af61e118d47c1c34e56e583f7032e3f9b7f0f4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 8 Jun 2023 08:43:07 +1000 Subject: [PATCH 1/3] AP_NavEKF3: When dead reckoning, clip but don't reject airspeed --- libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 38508e44a9a15..81aa5d9489d64 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -134,8 +134,16 @@ void NavEKF3_core::FuseAirspeed() // 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; + // when dead reckoning using airspeed measurements we cannot allow measurements to be rejected because + // this will cause the EKF to stop navigating so instead clip the measurement and accept it + const bool doingAirDataDeadReckoning = posTimeout || (PV_AidingMode != AID_ABSOLUTE); + ftype innovScaleFactor = 1.0f; + if (doingAirDataDeadReckoning && tasTestRatio > 1.0f) { + innovScaleFactor = sqrtF(1.0f / tasTestRatio); + } + + // fail if the 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; @@ -151,7 +159,7 @@ void NavEKF3_core::FuseAirspeed() // correct the state vector for (uint8_t j= 0; j<=stateIndexLim; j++) { - statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas; + statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas * innovScaleFactor; } stateStruct.quat.normalize(); From 393564865050a4d9f9f6b806e0de7f0b4249a2ce Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 8 Jun 2023 08:43:07 +1000 Subject: [PATCH 2/3] AP_NavEKF3: Scale innovation variance instead of clipping aspd innovation --- .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 57 ++++++++++--------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 81aa5d9489d64..8e9e3ad7645bc 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -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 @@ -66,6 +67,29 @@ 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. + tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); + const bool doingAirDataDeadReckoning = posTimeout || (PV_AidingMode != AID_ABSOLUTE); + 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) { @@ -128,29 +152,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); - - // when dead reckoning using airspeed measurements we cannot allow measurements to be rejected because - // this will cause the EKF to stop navigating so instead clip the measurement and accept it - const bool doingAirDataDeadReckoning = posTimeout || (PV_AidingMode != AID_ABSOLUTE); - ftype innovScaleFactor = 1.0f; - if (doingAirDataDeadReckoning && tasTestRatio > 1.0f) { - innovScaleFactor = sqrtF(1.0f / tasTestRatio); - } - - // fail if the 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; - } - // 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))) { @@ -159,7 +160,7 @@ void NavEKF3_core::FuseAirspeed() // correct the state vector for (uint8_t j= 0; j<=stateIndexLim; j++) { - statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas * innovScaleFactor; + statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas; } stateStruct.quat.normalize(); From 031de060998969664a78390930c70f5c2b1b6b7e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 25 Jun 2023 09:16:49 +1000 Subject: [PATCH 3/3] AP_NavEKF3: Use a smaller airspeed observation gate when dead reckoning --- libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 8e9e3ad7645bc..6cd4aff92220e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -72,8 +72,11 @@ void NavEKF3_core::FuseAirspeed() // 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. - tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); 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.