Heading source & Airspeed sensor-less navigation of Fixed Wing - Yaw estimator (EKF-GSF) and Wind Estimator

junwoo0914 · August 29, 2023, 8:08pm

About

As noted in PID tuning for a small Flying Wing (~47 cm wingspan) - #3 by junwoo0914, I have been working on getting a fixed wing to work without a magnetometer for autonomous navigation.

All of this would be quite straight forward if we don’t consider wind, but when we have significant wind I noticed that there’s a discrepancy between different airspeed / wind velocity / yaw estimation logic inside PX4, and wanted to bring up the discussion.

Estimators in PX4

There are 3 main estimators running in PX4 crucial to the mag-less operation of fixed wing:

EKF-GSF Yaw estimator (as fallback in EKF2 when magnetometer fails)
Wind Estimator (in Airspeed selector, when not using airspeed sensor)
Wind prediction in EKF2 (native state variable for the EKF2)

And the pipeline is like following:

EKF-GSF

Input: based on estimated airspeed, IMU data

Output: ‘heading estimate’, which works as an external source of measurement to EKF2, like a magnetometer would.

Results are published on “yaw_estimator_status” topic

Airspeed selector - Wind Estimator

Input: Inertial velocity and attitude estimate

Ouptut: Wind Estimator provides an estimated ‘wind velocity’ (based on sideslip beta angle fusion)

Results are published on “airspeed_wind” topic

Airspeed selector - Calculating Ground minus Wind

Input: Inertial velocity, wind velocity (from wind estimator)

Output: Estimation of airspeed (when the ASPD_PRIMARY index is ‘Groundspeed minus windspeed’)

Results are published on “airspeed_validated” topic

EKF2 Wind Prediction

It runs on it’s own state prediction algorithm as part of EKF2

Questions

It seems like the Wind Estimator in Airspeed selector is only doing “sideslip beta angle fusion”, and not the “airspeed fusion”, why is that?
We have different wind prediction mechanisms (one in airspeed selector, and one in EKF2), could we not merge these two? Why did we have them separated?

Log link: https://logs.px4.io/plot_app?log=f29f1de2-3097-4ecf-bf03-2fbde6583434

EKF-GSF Yaw Estimator, which is “crucial” for the heading estimate, which in turn also affects the wind estimate (in EKF2 and Airspeed Selector-Wind Estimator), does not really utilize any of the “wind estimate”. Why is that? How are we suppose to really estimate the yaw confidently when we don’t acknowledge the presence of wind (since we are taking in inertial velocity data)?

I am especially wondering how trust-worthy the EKF-GSF output is. Can we trust it enough to then propagate it’s estimate (heading), which then affects wind estimate, then the airspeed estimate? Or, are we just piping the data around in our own algorithms, and can end up in a non-sensical heading / airspeed estimate?

Could we improve the EKF-GSF perhaps by utilizing wind information?
How is EKF2’s wind estimated? How reliable is it? Does it consider the vehicle specific characteristics (e.g. fixed-wing’s coupling between heading and relative airspeed)

Overall, I’m just wondering if the pipeline is ideal, and whether it can be improved for the fixed wing purposes (where the coupling between airspeed / relative airspeed / heading are more relevant than in multicopters case).

More thoughts

The core of the question lies in the following phrase:

Given the inertial velocity (IMU + GPS) and attitude (IMU) estimates, can we accurately guess the heading and airspeed correctly?

And collecting opinions from various sources, it leads me to believe that heading & airspeed estimate probably won’t be accurate, and they can possibly end up in completely non-sensical values, which will lead to uncontrolled aircraft (e.g. like toilet bowling in multirotors when heading estimate is off).

Another way of getting heading estimate?

However, I also think that in a small time horizon (e.g. 10 ~ 20 seconds), by yawing the fixed wing significantly (e.g. 30 degrees), we can probably get a very good heading & airspeed estimate, assuming that wind stays almost constant, by fitting the circle in the inertial velocity estimate XY plot.

But I’m not certain if this logic is considered already in either EKF2’s wind prediction or Wind Estimate (sideslip) in Airspeed selector. I would love to hear some thoughts!

Using a magnetometer?

So far, I have excluded usage of the magnetometer, as I wanted to reduce the complexity / probability of failure of the system as much as possible (as magnetometers are known to be a tricky sensor to calibrate & may not provide good heading at times).

However, I’m wondering if using a magnetometer would in fact enhance the estimate, or make it more error prone. From the industry, I gather that magnetometer should never really be fully trusted as a heading source (as there are so many other disturbances that can affect the reading), but also I’m open for some new suggestions

Other

Possibly related: Sideslip based Wind Estimation · Issue #287 · PX4/PX4-ECL · GitHub

bresch · August 30, 2023, 9:44am

There is one WindEstimator instance using sideslip only plus one instance per airspeed sensor (encapsulated it AirspeedValidator. The instance with sideslip only is there to detect a failure of the airspeed sensor.

They both have pro and cons. In EKF2, having the airspeed estimate as a state allows us to extend out inertial dead-reckoning in fixed-wing through the airspeed fusion and allows multirotors to estimate wind using a drag model. The standalone wind estimator on its side is not directly coupled with other states so it’s much faster to run and we can create an instance per airspeed sensor which allows us to monitor the health of each of them. It can also estimate the scale factor, which is really useful as most drones have a large scale factor error (adding this to EKF2 would require to add a state, which comes with a high computational cost).

There is no need to know the wind strength not its direction to determine the heading of an object. The yaw estimator is not estimating the course over ground (i.e.: the direction of motion) but the actual heading of the vehicle. It finds it by matching the inertial data with the GNSS velocity measurements, all of this works regardless of the current wind.

The heading provided by the yaw estimator is usually really good as long as the heading is observable (accelerations are required to be observable). The accuracy can also be decreased while flying really long coordinated turns, but this can be counteracted by using an airspeed sensor. The estimate doesn’t depend on the wind, so it won’t be affected by a bad wind estimate.

Not at all, as said above, the wind doesn’t enter in the equation at all.

Same as for the standalone wind estimator: airspeed fusion and zero sideslip assumption.

junwoo0914 · August 30, 2023, 4:53pm

Answer

Thanks for the info! I definitely wasn’t aware that EKF-GSF Yaw Estimator is using mainly the detected acceleration and interpolate that to match the inertial velocity changes (aligned via estimating the yaw), so it is indeed not affected by wind easily!

Calculations workflow

Then, also considering absence of airspeed sensor, it seems like the magnetometer & airspeed sensor-less fixed wing’s guidance will happen like following:

EKF-GSF Yaw Estimator provides yaw estimate from IMU acceleration & inertial velocity measurements (as we don’t have magnetometer)
Airspeed selector’s Wind Estimator calculates the wind velocity estimate
Airspeed selector calculates Airspeed from inertial velocity - wind velocity (as we don’t have airspeed sensor)
EKF-GSF Yaw Estimator then uses that updated Airspeed estimate to come up with better yaw estimate (and the cycle continues). However, as noted below, this is actually not the case as it gets reset to EKFGSF_tas_default parameter value.

And on the side, EKF2 utilizes that estimated airspeed (but only when real airspeed sensor is present, check comment below) to calculate it’s own wind velocity estimate (and has it’s own airspeed estimate, which isn’t published).

Further discussions

Wind estimator sideslip

It seems like the wind estimator in airspeed selector seems to be only related to providing the wind velocity estimate, which gets used in estimating airspeed by using inertial velocity. Hence, I couldn’t find part where it “detects the failure of airspeed sensor”, which part would correspond to that?

EKF2 not considering ground speed - wind speed valid

It seems that EKF2 will actually not push the “airspeed_validated” data into airspeed sample queue when the ‘selected_airspeed_index’ is 0 (Airspeed deduced from Ground speed - Wind speed), leading to “estimator_status_flags/cs_fuse_aspd” being set to false, as there’s no valid airspeed data to fuse.

github.com

PX4/PX4-Autopilot/blob/23b31cc5fdc33fe1f43dcf12bb6fe4a5afbbc24d/src/modules/ekf2/EKF2.cpp#L1932


      
          			if (_msg_missed_airspeed_validated_perf == nullptr) {
          				_msg_missed_airspeed_validated_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed validated messages missed");
          
          			} else if (_airspeed_validated_sub.get_last_generation() != last_generation + 1) {
          				perf_count(_msg_missed_airspeed_validated_perf);
          			}
          
          			if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
          			    && PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
          			    && (airspeed_validated.calibrated_airspeed_m_s > 0.f)
          			    && (airspeed_validated.selected_airspeed_index > 0)
          			   ) {
          				airspeedSample airspeed_sample {
          					.time_us = airspeed_validated.timestamp,
          					.true_airspeed = airspeed_validated.true_airspeed_m_s,
          					.eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s,
          				};
          				_ekf.setAirspeedData(airspeed_sample);
          			}
          
          			_airspeed_validated_timestamp_last = airspeed_validated.timestamp;

And as you stated in the PR ekf2: use validated airspeed from airspeed sensor only by bresch · Pull Request #20293 · PX4/PX4-Autopilot · GitHub, it is to remove the circular dependency (I guess what you meant was this causing changes in EKF2’s wind estimate, thus indirectly affecting the other EKF2 states, which gets used for airspeed estimate via ground speed - wind speed?)

EKF-GSF Yaw Estimator not getting real time airspeed estimate

As noted above, currently the “estimator_status_flags/cs_fuse_aspd” is set to false (don’t fuse airspeed), which then triggers condition to not set the estimated airspeed to EKF-GSF yaw estimator:

github.com

PX4/PX4-Autopilot/blob/23b31cc5fdc33fe1f43dcf12bb6fe4a5afbbc24d/src/modules/ekf2/EKF/airspeed_fusion.cpp#L67-L69


      
          			if (!_control_status.flags.fuse_aspd) {
          				_yawEstimator.setTrueAirspeed(_params.EKFGSF_tas_default);
          			}

This means that only having an actual airspeed sensor would allow EKF-GSF yaw estimator to have estimated airspeed piped through properly, which I believe would lead to sub optimal behavior, as we can have varying airspeed in fixed wing.

Questions

What’s the risk of reverting that circular dependency prevention commit (and thus enabling EKF2 airspeed fusion, allowing yaw Estimator to use ground speed - wind speed based airspeed estimate)? Could the airspeed and wind estimate reach non-sensical value?

sfuhrer · September 18, 2023, 4:04pm

The module that contains it all is called “airspeed selector”. It contains one “airspeed validator” per airspeed sensor, which in turn each contain one “wind estimator”. The checks are then run for each airspeed validator instance, see https://github.com/PX4/PX4-Autopilot/blob/6bd13c551498d9701ffc92f88e9dbf1425820220/src/modules/airspeed_selector/AirspeedValidator.cpp#L214

Fusing the airspeed_validated in case that the source is not an airspeed sensor wouldn’t give the EKF any new data, as in that case the airspeed_validated was just constructed using position and heading (which are already fused into the EKF).

Hope that helps already, for all the GSF stuff @bresch is the better person to answer.