After experimenting with several dead-reckoning algorithms over the past 2 years, I finally have a simple algorithm that I like. The above pictures show a comparisons of a GPS reported trajectory (without compensation for GPS latency), with the corresponding dead-reckoning (IMU) computed trajectory (which includes compensation for GPS latency). In the first picture, the GPS is an EM406 reporting locations at 1 Hz. The IMU is computing locations at 40 Hz. The IMU track is plotted at 4 Hz. The aircraft is an EasyStar flying at 8 meters/second in gusty, 4 meters/second winds from the northeast. The autopilot is the UAVDevBoard running "MatrixPilot" with both wind estimation and dead reckoning turned on. At the beginning of this flight segment, the plane was flying away from waypoint 0 in stabilized mode, and was commanded to fly to waypoint 0. It made a smooth "U" turn, was buffeted by gusty winds after it crossed over the tree line, and then recovered.

The second picture is taken from one of Ric Kuebler's flights of his FunCub (thank you, Ric), flying at about 20 meters/second in winds with peak gusts of 10 meters per second.

The code for the algorithm is attached, deadReckoning.c. It is trivially simple:

1. Use the direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity. (not shown in the code.)

2. Integrate the acceleration to get velocity in earth frame. Integrate velocity to get location.

3. Compute the difference between GPS and IMU velocity and position. Use simple proportional feedback to acceleration and velocity to close the loop and prevent drift.

Actually, I had tried this algorithm once before, and it did not work very well in heavy winds, so I put it on the shelf and tried some other techniques, which did not work either. I recently realized that the key to making any dead-reckoning algorith work is to have accurate values for roll and pitch and GPS latency effects to start with. So, my recent efforts have been to improve the fundamentals in MatrixPilot. Here are two key recent changes:

1. An improved method for accounting for GPS latency. It is a non-linear "feed-forward" technique, based on geometrical concepts that take into account forward velocity and turning rate.

2. An improvement in accounting for forward and centrifugal acceleration effects. I have known for some time that acceleration adjustments should be based on air speed, not ground speed, but the first version of MatrixPilot was using ground speed because at the time I had not yet figured out how to estimate wind speed. I finally got around to making the necessary revisions, and it has made a huge improvement in performance.

By the way, MatrixPilot does not have a direct measurement of airspeed. It infers it from wind speed and ground speed using a "wind estimation" algorithm. If you have a direct airspeed measurement, the algorithm should perform even better.

Best regards,


Views: 29815

Comment by Gary Mortimer on April 16, 2011 at 7:26am
Ooh, cool

Comment by Ryan Beall on April 16, 2011 at 8:20am
Awesome man! Looks really good! We need to catch up sometime!
Comment by Rana on April 16, 2011 at 9:20am

Billu Bhaiya, its a great achievement ! Hats off to you for your continued efforts in improving the MP firmware.

Did you r866 in your flight testing ?

Comment by William Premerlani on April 16, 2011 at 9:51am

Hi Rana,

I used a slightly older version, but you can use r866 or newer.

By the way, Ric just did a couple of test flights with the new code with his FunCub, in heavy winds. Here is what he had to say:

"you are right, the plane flies like there is no wind !
This seems to me the biggest improvement in the navigation code I've ever seen, it rocks :D"
His telemetry showed the navigation was working rather smoothly.
Best regards,

Comment by William Premerlani on April 16, 2011 at 9:55am
Hi Ryan,
Good to hear from you, man. I saw your posting the other day with the question about how the GY401 heading-hold gyro works. If you ever find out, I would love to know.
Best regards,
Comment by Paul Mather on April 16, 2011 at 9:57am
Bill, can you have the guys add windspeed and direction as calculated to the MatrixPilot UDB Extra output? That would be great into to have on the GCS!
Comment by Riccardo Kuebler on April 16, 2011 at 10:04am

Hi Bill,

yes, this really rocks !

Today during my flights I was impressed how the plane flew.

Thank you very much for this great improvement.

I'm going to publish a blog about my FunCub, but this will take some time.

Time to go for another Funjet round ?

Best regards,


Comment by Morli on April 16, 2011 at 10:47am
This is getting better, now if I can only get my ICD2 to write to UDB :((

Comment by William Premerlani on April 16, 2011 at 1:25pm

Hi Happy,

Wind vector (X, Y, and Z components) in centimeters per second is already in the UDB extra output in the F2 message. It has been there for quite some time. For example:


Hi Morli,

I am sorry to hear you are having trouble with your ICD2. Are you trying to use it to program your UDB? I am not sure what you mean by "write to UDB". If you are having trouble programming your UDB with an Olimex ICD2 that you bought from SparkFun, I suggest you contact them, explain the problem, and see if they will take it in exchange toward a PICkit. We have found the PICkits work rather well, but the ICD2s have a history of problems.

Best regards,



Comment by Riccardo Kuebler on April 16, 2011 at 2:21pm


here is an appetizer, just to watch at my first results with this new improvement.



You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service