ArduPilot / ArduPilot/ardupilot

Synthetic airspeed is fused for planes without airpseed sensor when dead-reckoning

Open
#33,720 2 comments 1 reaction 0 assignees View on GitHub
Dominant language
C++
Stars
15.9k
Forks
21.4k
Avg merge
3d 17h
Merged PRs (30d)
119

Description

## Bug Report

Synthetic airspeed is fused for planes without airpseed sensor when dead-reckoning.\
Instead, when navigating with GPS, only side-slip fusion is performed.\
I know synthetic airspeed was added exactly for this reason, but it seems illogical. And seems like it can be replaced with dead-reckoning with sideslip fusion only.

Also, with GPS, wind process noise is scaled by x10 before 1st dead-reckoning. And after turning GPS back, wind process noise is not scaled.\
Related issue for copters #33451 was fixed in `master`.

### Issue details

AP 4.6 introduced new feature to use last estimated airspeed when dead-reckoning for planes.
This feature also takes action with planes without airspeed sensor, here https://github.com/ArduPilot/ardupilot/blob/6d578b4ff48b589ef009aecf13da8a5f45dd0b3e/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp#L899
This synthetic airspeed is then fused here:
https://github.com/ArduPilot/ardupilot/blob/6d578b4ff48b589ef009aecf13da8a5f45dd0b3e/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp#L162
Also, before 1st dead-reckoning wind process noise is scaled by x10 here:
https://github.com/ArduPilot/ardupilot/blob/6d578b4ff48b589ef009aecf13da8a5f45dd0b3e/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp#L1127
After 1st dead-reckoning, wind process noise is NOT scaled by x10, bcz `tasDataDelayed.allowFusion` is already true.

This results in different EKF3 air data fusion behaviour:
* With GPS only side-slip fusion is performed
* Before 1st dead-reckoning, wind process noise is scaled by x10
* After 1st dead-reckoning, wind process noise is NOT scaled by x10
* Without GPS both side-slip and synthetic airspeed fusion is performed

To reproduce, take off, turn GPS off and wait, then turn GPS on.

The issue with x10 wind process noise scale can fixed by adding
```
} else {
tasDataToFuse = false;
tasDataDelayed.allowFusion = false;
```
here
https://github.com/ArduPilot/ardupilot/blob/6d578b4ff48b589ef009aecf13da8a5f45dd0b3e/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp#L913

Regarding synthetic airspeed fusion, seems like it can be replaced with sideslip fusion only and keeping absolute position when dead-reckoning. For this a new timeout condition can be added to `setAidingMode` here
https://github.com/ArduPilot/ardupilot/blob/6d578b4ff48b589ef009aecf13da8a5f45dd0b3e/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp#L326
Smth like
```
// Check if sideslip data is being used
bool sideSlipUsed = (imuSampleTime_ms - prevBetaDragStep_ms <= minTestTime_ms);
...
// Check if attitude drift has been constrained by a measurement source
bool attAiding = posUsed || gpsVelUsed || optFlowUsed || sideSlipUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;

// Check if velocity drift has been constrained by a measurement source
// Currently these are all the same source as will stabilise attitude because we do not currently have
// a sensor that only observes attitude
velAiding = posUsed || gpsVelUsed || optFlowUsed || sideSlipUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;

// Store the last valid airspeed estimate
windStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed);
if (windStateIsObservable && (airSpdUsed || dragUsed)) {
lastAirspeedEstimate = (stateStruct.velocity - Vector3F(stateStruct.wind_vel.x, stateStruct.wind_vel.y, 0.0F)).length();
lastAspdEstIsValid = true;
}
```
Then, synthetic airspeed will be fused only in case of airspeed sensor failure.\
While only sideslip fusion will be fused in case of dead-reckoning without airspeed sensor. Quick testing shows that it behaves very similar to original.

**Version**
4.6.3

**Platform**
[ ] All
[ ] AntennaTracker
[ ] Copter
[*] Plane
[ ] Rover
[ ] Submarine

**Airframe type**
Plane

**Hardware type**
SITL

Contributor guide

Open the contributing guide

Research direction

Start with the cited logic in libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp, AP_NavEKF3_AirDataFusion.cpp, AP_NavEKF3/AP_NavEKF3_core.cpp, and AP_NavEKF3/AP_NavEKF3_Control.cpp. Reproduce the Plane SITL scenario by disabling GPS, waiting, and restoring it, then inspect synthetic airspeed and wind process-noise behavior. Done means the reported dead-reckoning paths have consistent air-data fusion behavior without affecting the airspeed-sensor-failure case.

Written by the indexing model from the issue text.

Assessment

Tech stack
cpp
Domain
embedded-iot, robotics
Issue type
Bug
Difficulty
4/5
Estimated time
3-5 days
Activity status
Quiet
Clarity
Mostly clear
Newbie friendliness
48/100

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.