Virtual compass for ardupilot = even faster and pinpoint navigation ???

I was wondering if it was possible to create some sort of a virtual compass ( software ) for ardupilot.
The point would be to have instantaneous and unlimited access to the heading information in order to have a much faster Heading PID loop and be able to steer the plane directly to the next waypoint without any loitering in a relatively hight bank turn.
This brings us back to the thermopile characteristic. If the bank can accurately be determined, then the rate of turn is also known ( RoT=G*tan(Bank)/Airspeed hope I got it right ;-) ). By integrating this value over time we can then extrapolate "heading + error" . Erro can be computed with the gps heading and gps delays can also be taken into account.
It's actually a Kalman filter, from two observed parameters RoT (from Bank & Airspeed) and GPS heading, we combine the two and we have a value that benefits from the quick response of the first parameter and the long term accuracy of the GPS.

RoT = Rate of Turn

I think that this (,if it works, ) would be a substantial step forward. No need for a fast GPS. Even 1 Hz should be enough. As far as the CPU power is concerned, it only adds one tan() one integration at each cycle...

Thank you for letting me know if you share my enthusiasm.


Views: 458

Reply to This

Replies to This Discussion

I'm not quite sure I understand the question, but I'll try to answer.

First, we obviously do have a "virtual compass" in both the hardware and software ArduStations on the ground using the basic heading output from the GPS. And of course as we migrate to the uBlox5 the heading loop in the air becomes 4x faster, which is fast enough for virtually all applications we can think of.

I think you're asking if we can interpolate the heading information at faster than 1Hz with the EM406. Perhaps, but I'm not quite sure how, and as I mentioned, we don't see a need with the uBlox5.

I think that Dave_fr has a very valid observation that can improve the instantaneous flight path bearing solution in software without having to invest in a new GPS. Being able to improve the quality and speed of a solution in existing hardware is always a plus. Besides, I have already invested in three EM406A GPSs and will now have to invest in three uBlox5 GPSs to obtain in hardware what I might be able to obtain in a software modification is worth investigating.

What do you think?


I agree that would be a good thing. If someone can construct an algorithm we can try we'll check it out. As I said, it's not a priority for us, because the EM406 works well enough for most applications and we're migrating to the Ublox which work great for everything else, but this is an open source project so we'd be delighted to have other come up with other solutions, too.
Virtual Compass for ardupilot

I) Introduction

The point of this is to have a variable that contains the ground heading that is as accurate as it can be at any time constantly updated.
The hardware studied will be a thermopile, an airspeed sensor and a GPS.
We will start with the general principle of how we can mix the readings of these sensors in order to have the best heading information possible then we will go more into depth with the technical details.

II)How to combine the sensor readings to make the most of them.

Based on the knowledge of a system behavior it is sometimes possible to anticipate its behavior. Let’s say that based on your experience your girlfriend is always late by 30 minutes. As an extremely busy man you don’t have any time to waste, obviously you want to have the best estimate of when she comes so you don’t have to wait for too long. In this case, even if you’ve agreed on 20:00 you will come at around 20:30. 20:30 is the best estimate you can have based on your knowledge of the system.
Now, if you know that when the weather is bad she takes an extra 10mins, you will work out your estimate using this new parameter (the weather).

It’s the same thing with your plane equipped with a thermopile, airspeed sensor and GPS.
Let say that we have a very poor GPS that can only give us a new heading value every 10 seconds (a little bit extreme lol). We are in a straight and level flight heading north, we’ve just received the last GPS heading (360°) update and we have to wait almost 10 seconds before we can have a new one. At that same moment the navigation system switches to way point n°2 which is west of the present position. A left turn is initiated. The system needs to know when it has to level off the wings in order to make a heading of 270° (west). If you are only relying on the GPS, you still have 9 seconds to wait before knowing where about you are in the turn. Not very convenient.
Now let’s introduce a new parameter, the RoT (Rate of Turn) which corresponds to the rate at which the plane is turning round the vertical axis. If you were sitting in the plane, it would correspond to the speed at which a perfect compass needle pointing north is moving.
In our example we have a rate of turn of 360° in 30 seconds. It’s now much easier for the system to turn the waypoint 2. We have turn left 90° which corresponds to (90/360)*30= 7.5 seconds. The system knows that after 7.5 seconds in the turn it has to level off(easily manageable). If it had to wait for an extra GPS update it would have overshot.

III)Thermopile attitude and RoT(Rate of Turn)

As we have just discussed, RoT becomes crucial information. I worked out that RoT is determined by the Bank angle and the airspeed as follows: RoT = G*sin(Bank)/Airspeed.
The bank angle can be computed using the thermopile. Airspeed is obtained through the pressure sensor. You see that RoT is relatively easy to compute provided that we have a good enough estimate of the bank angle.
IV)Put it all into equations.
We are going to use the rapid response of the RoT. Rapid because it can almost be instantaneously determined with the thermopile and the airspeed sensor. RoT is obviously not good enough because it only describes the evolution of the turn and using this parameter only would eventually lead to an error that would accumulate over time in the same manner as a gyroscope drifts.
This is when we need to use the GPS to compensate for that drift, in other words, to use the GPS heading (when it is available) to work out this error.

Let’s define a few variables :
dt : The small length of time between each system iteration, between state N and N-1
Hdg : Heading
RoT : Rate of Turn (angular speed)
HdgGPS : Heading given by the GPS
K : A coefficient
Dlay : GPS delay

We are not going to bother defining units, this being a general description.

RoT = ( Hdg(N)-Hdg(N-1) )/dt

We now have :
Hdg(N)=dt*RoT+ Hdg(N-1)+error1

Now let’s deal with the error1. This will be done each time a GPS heading is available.
We could do, for example :

error1=K*(Hdg(N)-HdgGPS) +error1

Now because the GPS has a certain Dlay (Delay)

error1=K*(Hdg(N-ND)-HdgGPS) +error1


What the system has to do is
1-Thermopile + Airspeed sensor =>( RoT = G*sin(Bank)/Airspeed.)
2-RoT intergration => Heading +error (Hdg(N)=dt*RoT+ Hdg(N-1)+error1)
3-When GPS heading is available we calculate the error1=K*(Hdg(N-ND)-HdgGPS) +error1

This method tries to take advantage of the anticipation provided by the Bank (thermopile) and the airspeed and the accuracy of the GPS.

Thank you for reading.
The Ublox seems to be an amazing piece of kit, though I don't see how it can give five reliable and independent ground headings per second. Let's say that we have a plane moving at a ground speed of 30kph which is about 10 meters per second, every fifth of a second, it only moves 2 meters. Considering the accuracy of the GPS system; two points spaced by two meters is not enough to give an accurate bearing. I think what the Ublox must do is an interpolation (averages) of all the points which is great for the precision but still introduces delays. My method also interpolates (averages) but it doesn't introduce any delay by anticipating what the GPS will see in the future, given the instantaneous attitude and speed of the airplane. I don't know what exactly the current setup with the Ublox is able to perform, but this method would, for instance, allow a very rapid change in direction from one waypoint to another with relatively high bank turns. I've just posted a more detailed description of this method.
Best regards,

Nice analysis of your software modification proposal. Also, nice observation about the capabilities of the uBlox5 GPS and the effectiveness of its ~5 Hz update rate.

Is there any part of the software proposed here to account for slip or uncoordinated turns where the plane banks but doesen't necessarily make a two minute 360 degree turn at 30 degrees.
Not really,

Well firstly, this method is based on a constant update of the Rate of Turn (RoT), 360°/30 seconds is just an example. It would work the best for coordinated turns. In case of flat turns (ardu steered by rudder stabilized by aileron) the formula RoT=G*sin(Bank)/Airspeed is no longer valid and a coefficient will have to be applied and the formula will look like :

Anyway, as I said, it fully works if the turns aren't too dissymetrical (aileron going one way, rudder steering the other way). The current setup with the 2 axis easytar no aileron wouldn't be a problem.
Thank you very much Thomas,

The point would be to collect as many people's feedback as possible. I really beleive in this method and i know that ardupilot is working very well enough and I'm always amazed when I look at the code and people's flight recollection and all the work that has been accomplished by Chris, Jordi and others. This is just a proposition for a step forward in the project. I see all the improvements that it would bring without no penalty in terms of cost, hardware modification and very little CPU power used.
Maybe the implementation will not be easy but it could be really worth it.
Imagine your plane on a triangular track. Just after waypoint hit it would start a sharp turn the the next point and levels off just at the right time with very little overshoot.

In essence, DIYDroners if you want to see this implemented on your ardupilot support this project !
Actually, the Ublox doesn't interpolate its measurements - when you ask for the maximum of 4 updates per seconds, it is actually solving at 4Hz. The way it gets accurate heading is actually from the velocity, which is calculated using from the doppler shift of the GPS signals, as opposed to successive position estimations.
I understand, well I guess that what I said above is wrong Ubloxs are really amazing pieces of kit !
Thank you very much Daniel. Please excuse my ignorance on this one ;-)

As it looks like we are migrating to the Ublox GPS, would anyone be interested in selling me one of his "old" EM406 for a good price so that I can start experimenting ?

Reply to Discussion


© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service