# 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.

Dave

Views: 471

### Replies to This Discussion

Amendement RoT = G*tan(Bank)/Airspeed
I guess I see more what you are talking about.. Your approach will most likely not help the problem you are trying to solve. You will have more bandwidth on your heading controller that is true. However the reason the aircraft skews way past the waypoint and takes forever to get back on is because that's the dynamics you are accecpting in the waypoint switching. Each aircraft has a min radius for a turn and you want to set up the waypoint switch early such that it leads the turns, turns, and lags the turn to get back on...ie: for a left turn you must turn right first then make a min radius left turn and then another right turn to get back on track. All three turns utilizing min radius turns...this is the most dynamicly sound way to make turns that don't skew way off track like is currently programmed in the nav code.

This is some code I wrote and tested in matlab. The circles represent the min radius I was talking about...they are tangent to eachother they just don't map well in non spherical 2d space. I hope this makes sense. the skewed line with an x is the aircraft and its heading

http://diydrones.com/photo/track_smoothing-1?context=user
Another assumption that would kill this implemenation is the over simplification of the ROT equation. This would work for very small AOA's but nothing outside of 15 deg probably even less than 10 deg. Here is why

ROT = g*sqrt(n^2-1)/Vinf

I guess you could back out n or assume its 1 or...I don't know but in a 60 deg AOB turn its 2 for a steady level turn. You can see how quickly this will walk your equation away. The main thing is....Its a simplification and I think the truth data recieved from it would only be good for steady level at low bandwidth. So you would probably be in the same boat. I just don't think assuming n =1 in turns is a simplification you can overlook.

a better assumption would be to assume the turn is steady level and it would look likewise

n = 1/(cos(phi)); phi = roll angle
ROT = same as above

Hope this helps. Great idea and very easy to implement!!
Do you do full size flying Ryan ?
If yes have you ever flown a trajectory only using a slow gps as heading reference?
I'm just introducing a very reactive way to get a good real time estimate of the heading over ground. If that was useless we wouldn't have gyro stabilised direction indicator on most of full size aeroplane.
Believe me, when flying IFR it helps a little...
I just think that you're confused with where I want to go.
I was just talking about high bank turns as a positive consequence. Having a real time heading allows it. Having 1 heading a second only allows a very slow establishment to the new desired heading.

Best regards,
Okay thank for the clarification G not g( local ) . I see how this could be useful . but a few questions , are you assuming a level(no alt loss) coordinated turn?what would happen if the plane was banked w/o turning ?
Interesting question.

It should work as long as the ailerons are not fighting the rudder in other words, if there is a fma copilot always keeping the wings leveled and the rudder kicking the nose one way to the next waypoint, yes it would be a problem. The reason being that because in this precise case it is the fuselage and the engine thrust that turns the plane, no direct relation between the bank and RoT can be made. However a coefficient could possibly be applied but I don't think that it would work as such.
Now with aileron control I can't see any problem neither with the 2 axis (rudder only). Even if with the 2 axis (rudder only) planes, the turn is uncoordinated only momentaneously it is still the wing Lift that changes the direction of the plane the dissymetrical airflow pushes left and right as rudder is applied but averages out in the end.

Now, if there is altitude loss it means that Lift hasn't been increased as it should ( pull on the stick ). It is actually the assumption that I made when I said that the RoT=G*sin(Bank)/Airspeed. The difference betwen the formula is just a tan() instead of a sin() where the difference become only noticeable after 30° bank.
Anyway the lift is always somehow compensated for (increase in speed) otherwise it would mean that the plane keeps going faster and faster (down force not compensated for = constant downward acceleration).

Best regards,
Yeah, I fly for the Navy. As far as the heading controller at 1Hz not being adequate. I have built my own fully capable imu auto pilot from the ground up and have used both COG from GPS as well as the IMU estimated (smoothed) psi for heading control. Surpirsingly the COG from GPS will hold the heading just fine(moderate wind).
When i'm flying i can't read the RMI at 1Hz down to the 6th decimal .....just saying I'm not that good but my auto pilot is and its perfectly adequate from what I've seen in the feild.....It is the nav logic setting up the turns that helps a ton. However I don't know how the code is written for the track holding....You may just have to increase the cross track error gain... Speaking in the terminology that I use on my auto pilot?? Do you know the equivalent in the code?

I compared your equation to mine and they are the same. I stand corrected....They both assume steady level coor turns.

http://diydrones.com/photo/straightlinehold-1?context=user
This is a matlab plot I used to check my code. chaning the crosstrack gain causes the heading controller to converge quicker on the track....just a thought?
Well, it looks like we are talking about the same thing now.
As far as the code is concerned I'm just taking Chris and Jordi's. No cross track for the moment just permanent gotos (as far as I've seen). As far as the coordinated turn assumption is concerned I've just answered Wayne.

Best regards,
I am assuming they use some cross track just from some of the google earth plots I have seen. Doesn't seem like they are doing direct to navigation. I could be wrong....I'm intrigued...I'll see if I can find out
-Ryan
Yeah just glanced at the Ardupilot code for the first time.. I'm impressed with how organized it is. Yeah there is no cross track vectoring. So yeah much improvement in the logic here would greatly improve your ground nav!! It is very simple if you have any questions Very simple!

I bet if you just simply added this (which is what I'm running) you would be completely satisfied with its heading performance!

Never the less I like your filter idea!

1

2

3

4

5

6

7

8

9

10

## Groups

1472 members

45 members

282 members

470 members

• ### 3D Models

57 members

Season Two of the Trust Time Trial (T3) Contest
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here