Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Rework method of synthesising airspeed for dead reckoning

Browse Source 
The previous method resulted in data incest and fusion of predicted airspeed on every EKF internal time step.
This was not apparent during flight where the vehicle was turning, but during long straight legs did not constrain along track drift.
This commit is contained in:
Paul Riseborough 2023-09-28 07:57:34 +10:00 committed by Andrew Tridgell
parent 87a867ee2e
commit 61874da020
5 changed files with 43 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -212,8 +212,10 @@ void NavEKF3_core::SelectTasFusion()
readAirSpdData();
// if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion
if (tasDataToFuse && statesInitialised && !inhibitWindStates) {
FuseAirspeed();
tasDataToFuse = false;
prevTasStep_ms = imuSampleTime_ms;
}
}

14
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -64,7 +64,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
PV_AidingMode != AID_NONE;
if (!inhibitWindStates && !canEstimateWind) {
inhibitWindStates = true;
windStateLastObsIsValid = false;
lastAspdEstIsValid = false;
updateStateIndexLim();
} else if (inhibitWindStates && canEstimateWind &&
(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) {
@ -343,14 +343,12 @@ 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;
// Store the last valid airspeed estimate
windStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed);
if (windStateIsObservable) {
lastAirspeedEstimate = (stateStruct.velocity - Vector3F(stateStruct.wind_vel.x, stateStruct.wind_vel.y, 0.0F)).length();
lastAspdEstIsValid = true;
}
windStateIsObservable = newWindStateIsObservable;
// check if position drift has been constrained by a measurement source
bool posAiding = posUsed || rngBcnUsed;

60
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -850,46 +850,50 @@ void NavEKF3_core::readAirSpdData()
// 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->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS;
timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f));
tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed);
// Correct for the average intersampling delay due to the filter update rate
tasDataNew.time_ms -= localFilterTimeStep_ms/2;
// Save data into the buffer to be fused when the fusion time horizon catches up with it
storedTAS.push(tasDataNew);
}
// 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);
// Allow use of a default value or a value synthesised from a stored wind velocity vector
if (!useAirspeed()) {
if (useAirspeed()) {
const auto *airspeed = dal.airspeed();
if (airspeed &&
(airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed);
if (tasDataNew.allowFusion) {
tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS;
timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f));
// Correct for the average intersampling delay due to the filter update rate
tasDataNew.time_ms -= localFilterTimeStep_ms/2;
// Save data into the buffer to be fused when the fusion time horizon catches up with it
storedTAS.push(tasDataNew);
}
}
// 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);
} else {
if (is_positive(defaultAirSpeed)) {
if (imuDataDelayed.time_ms - lastTasPassTime_ms > 200) {
// this is the preferred method with the autopilot providing a model based airspeed estimate
if (imuDataDelayed.time_ms - prevTasStep_ms > 200 ) {
tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f)));
tasDataToFuse = true;
tasDataDelayed.allowFusion = true;
tasDataDelayed.time_ms = imuDataDelayed.time_ms;
} else {
tasDataToFuse = false;
tasDataDelayed.allowFusion = false;
}
} else if (windStateLastObsIsValid && !windStateIsObservable) {
if (imuDataDelayed.time_ms - lastTasPassTime_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));
} else if (lastAspdEstIsValid && !windStateIsObservable) {
// this uses the last airspeed estimated before dead reckoning started and
// wind states became unobservable
if (lastAspdEstIsValid && imuDataDelayed.time_ms - prevTasStep_ms > 200) {
tasDataDelayed.tas = lastAirspeedEstimate;
// this airspeed estimate has a lot of uncertainty
tasDataDelayed.tasVariance = sq(MAX(MAX(frontend->_easNoise, 0.5f), 0.5f * lastAirspeedEstimate));
tasDataToFuse = true;
tasDataDelayed.allowFusion = true;
tasDataDelayed.time_ms = imuDataDelayed.time_ms;
} else {
tasDataToFuse = false;
tasDataDelayed.allowFusion = false;
}
}
}

3
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -268,10 +268,9 @@ void NavEKF3_core::InitialiseVariables()
prevInFlight = false;
manoeuvring = false;
inhibitWindStates = true;
windStateLastObs.zero();
windStateIsObservable = false;
treatWindStatesAsTruth = false;
windStateLastObsIsValid = false;
lastAspdEstIsValid = false;
windStatesAligned = false;
inhibitDelVelBiasStates = true;
inhibitDelAngBiasStates = true;

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1110,8 +1110,8 @@ 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 should not be used
Vector2F windStateLastObs; // wind states from the last time wind states were constrained using observations (m/s)
bool windStateLastObsIsValid;
ftype lastAirspeedEstimate; // last true airspeed estimate (m/s)
bool lastAspdEstIsValid; // true when the last true airspeed estimate is valid (m/s)
bool windStateIsObservable; // true when wind states are observable from measurements.
bool treatWindStatesAsTruth; // true when wind states should be used as a truth reference
bool windStatesAligned; // true when wind states have been aligned