From 324d5da81129baa896d35dd9f8478d2ab8dedd9f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 6 Mar 2023 08:39:40 +1100 Subject: [PATCH] AP_NavEKF3: Use last observed wind states to enable dead reckoning These changes enable the EKF to use the last observable wind velocity vector estimate to synthesise an airspeed measurement if operating without an airspeed sensor and when all other measurement types that can constrain velocoty drift are lost. This enables the EKF to use dead reckoning to continue after loss of GPS when there is no air speed sensor fitted and without the need to set a default airspeed value. The logic used to fuse a default airspeed value has also been cleaned up and the call to FuseAirSpeed() from inside SelectBetaDragFusion() has been removed. AP_NavEKF3: Fix error in default airspeed observation variance AP_NavEKF3: Enable shadow fusion of airspeed when sensor is disabled --- .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 4 --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 12 ++++++- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 35 ++++++++++++------- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 3 ++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 4 ++- 5 files changed, 40 insertions(+), 18 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 38508e44a9..79e18027ac 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -256,10 +256,6 @@ void NavEKF3_core::SelectBetaDragFusion() // we are required to correct only wind states airDataFusionWindOnly = true; } - // Fuse estimated airspeed to aid wind estimation - if (usingDefaultAirspeed) { - FuseAirspeed(); - } FuseSideslip(); prevBetaDragStep_ms = imuSampleTime_ms; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 0489aee688..c76d650aae 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -64,6 +64,7 @@ void NavEKF3_core::setWindMagStateLearningMode() PV_AidingMode != AID_NONE; if (!inhibitWindStates && !canEstimateWind) { inhibitWindStates = true; + windStateLastObsIsValid = false; updateStateIndexLim(); } else if (inhibitWindStates && canEstimateWind && (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) { @@ -77,7 +78,7 @@ void NavEKF3_core::setWindMagStateLearningMode() Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); ftype trueAirspeedVariance; - const bool haveAirspeedMeasurement = usingDefaultAirspeed || (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed()); + const bool haveAirspeedMeasurement = (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed()); if (haveAirspeedMeasurement) { trueAirspeedVariance = constrain_ftype(tasDataDelayed.tasVariance, WIND_VEL_VARIANCE_MIN, WIND_VEL_VARIANCE_MAX); const ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; @@ -342,6 +343,15 @@ void NavEKF3_core::setAidingMode() // a sensor that only observes attitude velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed; + // Store the last wind states whose errors were constrained by measurements + // This will be used to synthesise an airspeed measurement to enable dead reckoning + bool newWindStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed); + if (!inhibitWindStates && !newWindStateIsObservable && windStateIsObservable) { + windStateLastObs = stateStruct.wind_vel; + windStateLastObsIsValid = true; + } + windStateIsObservable = newWindStateIsObservable; + // check if position drift has been constrained by a measurement source bool posAiding = posUsed || rngBcnUsed; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 1b25e84e65..8533490443 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -861,18 +861,29 @@ 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 enabled - if (!useAirspeed() && - imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200 && - is_positive(defaultAirSpeed)) { - tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; - tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar)); - tasDataDelayed.allowFusion = true; - tasDataDelayed.time_ms = 0; - usingDefaultAirspeed = true; - } else { - usingDefaultAirspeed = false; + // Allow use of a default value or a value synthesised from a stored wind velocity vector + if (!useAirspeed()) { + if (is_positive(defaultAirSpeed)) { + if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) { + tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; + tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f))); + tasDataToFuse = true; + tasDataDelayed.allowFusion = true; + } else { + tasDataToFuse = false; + } + } else if (windStateLastObsIsValid && !windStateIsObservable) { + if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) { + // use stored wind state to synthesise an airspeed measurement + const Vector3F windRelVel = stateStruct.velocity - Vector3F(windStateLastObs.x, windStateLastObs.y, 0.0F); + tasDataDelayed.tas = windRelVel.length(); + tasDataDelayed.tasVariance = sq(MAX(frontend->_easNoise, 0.5f)); + tasDataToFuse = true; + tasDataDelayed.allowFusion = true; + } else { + tasDataToFuse = false; + } + } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index d1b4ba86c8..f608f6d4d2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -268,6 +268,9 @@ void NavEKF3_core::InitialiseVariables() prevInFlight = false; manoeuvring = false; inhibitWindStates = true; + windStateLastObs.zero(); + windStateIsObservable = false; + windStateLastObsIsValid = false; windStatesAligned = false; inhibitDelVelBiasStates = true; inhibitDelAngBiasStates = true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 8f3fd2d057..69cbfb5acf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1110,6 +1110,9 @@ private: 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 bool inhibitWindStates; // true when wind states and covariances are to remain constant + Vector2F windStateLastObs; // wind states from the last time wind states were constrained using observations (m/s) + bool windStateLastObsIsValid; + bool windStateIsObservable; // true when wind states are observable from measurements. bool windStatesAligned; // true when wind states have been aligned bool inhibitMagStates; // true when magnetic field states are inactive bool lastInhibitMagStates; // previous inhibitMagStates @@ -1145,7 +1148,6 @@ 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 - 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