From 3634a942a1269d0e68996087f1b31331017471b3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 6 Jan 2021 11:42:34 +1100 Subject: [PATCH] AP_NavEKF3: Fix bug causing unwanted use of default airspeed --- .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 17 ++++------------- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 9 ++------- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 14 +++++++++++++- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 2 ++ 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index f5c8c58ae5..2fef831ffe 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -25,7 +25,6 @@ void NavEKF3_core::FuseAirspeed() float vd; float vwn; float vwe; - float EAS2TAS = dal.get_EAS2TAS(); float SH_TAS[3]; float SK_TAS[2]; Vector24 H_TAS = {}; @@ -44,14 +43,7 @@ void NavEKF3_core::FuseAirspeed() if (VtasPred > 1.0f) { // calculate observation innovation and variance - float specifiedVariance = sq(MAX(frontend->_easNoise, 0.5f)); - if (is_positive(defaultAirSpeed)) { - innovVtas = VtasPred - defaultAirSpeed; - specifiedVariance = MAX(specifiedVariance, defaultAirSpeedVariance); - } else { - innovVtas = VtasPred - tasDataDelayed.tas; - } - const float R_TAS = specifiedVariance * sq(constrain_float(EAS2TAS, 0.9f, 10.0f)); + innovVtas = VtasPred - tasDataDelayed.tas; // calculate observation jacobians SH_TAS[0] = 1.0f/VtasPred; @@ -63,8 +55,8 @@ void NavEKF3_core::FuseAirspeed() H_TAS[22] = -SH_TAS[2]; H_TAS[23] = -SH_TAS[1]; // calculate Kalman gains - float temp = (R_TAS + 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 >= R_TAS) { + float temp = (tasErrVar + 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 >= tasErrVar) { SK_TAS[0] = 1.0f / temp; faultStatus.bad_airspeed = false; } else { @@ -256,8 +248,7 @@ void NavEKF3_core::SelectBetaDragFusion() airDataFusionWindOnly = true; } // Fuse estimated airspeed to aid wind estimation - if (is_positive(defaultAirSpeed)) { - frontend->_easNoise = 0.33f * defaultAirSpeed; + if (usingDefaultAirspeed) { FuseAirspeed(); } FuseSideslip(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 15f1c79f6a..7d0978c2c2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -700,16 +700,11 @@ void NavEKF3_core::runYawEstimatorPrediction() } float trueAirspeed; - if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) { - if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) { - trueAirspeed = tasDataDelayed.tas; - } else { - trueAirspeed = defaultAirSpeed * dal.get_EAS2TAS(); - } + if (assume_zero_sideslip()) { + trueAirspeed = MAX(tasDataDelayed.tas, 0.0f); } else { trueAirspeed = 0.0f; } - yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 9b5ed3ec84..10046e7816 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -815,15 +815,17 @@ void NavEKF3_core::correctEkfOriginHeight() // check for new airspeed data and update stored measurements if available void NavEKF3_core::readAirSpdData() { + const float EAS2TAS = dal.get_EAS2TAS(); // if airspeed reading is valid and is set by the user to be used and has been updated then // we take a new reading, convert from EAS to TAS and set the flag letting other functions // know a new measurement is available + const auto *airspeed = dal.airspeed(); if (airspeed && airspeed->use(selected_airspeed) && airspeed->healthy(selected_airspeed) && (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) { - tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * dal.get_EAS2TAS(); + tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS; timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed); tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; @@ -835,6 +837,16 @@ void NavEKF3_core::readAirSpdData() } // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms); + float easErrVar = sq(MAX(frontend->_easNoise, 0.5f)); + // Allow use of a default value if the measurement times out + if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 5000 && is_positive(defaultAirSpeed)) { + tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; + easErrVar = MAX(defaultAirSpeedVariance, easErrVar); + usingDefaultAirspeed = true; + } else { + usingDefaultAirspeed = false; + } + tasErrVar = easErrVar * sq(EAS2TAS); } /******************************************************** diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 7604501338..9273156858 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1056,6 +1056,8 @@ private: range_elements rangeDataDelayed;// Range finder data at the fusion time horizon tas_elements tasDataNew; // TAS data at the current time horizon tas_elements tasDataDelayed; // TAS data at the fusion time horizon + float tasErrVar; // TAS error variance (m/s)**2 + bool usingDefaultAirspeed; // true when a default airspeed is being used instead of a measured value mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon gps_elements gpsDataNew; // GPS data at the current time horizon gps_elements gpsDataDelayed; // GPS data at the fusion time horizon