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