A simple dead-reckoning algorithm

Team,

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,

Bill

Team,

I received some questions about some of the details of this "dead-reckoning" algorithm (which I really should call loose IMU/GPS integration). Here are the answers.

RMAX is equal to 2**14. Is is the binary equivalent of when a matrix coefficient is equal to 1.

MAX16 is equal to 2**16.

The implementation of the method that I am using has evolved with time. The latest one is very simple, and is contained in the file dead-reckoning.c in the MatrixPilot repository. It was last changed in r918.

One of the parameters that is adjustable is the dead reckoning filter time constant. In the end, I settled on 2.5 seconds, it gives pretty good performance. Using a shorter time constant rejects drift better, but produces more high frequency error.

If you happen to look at the code, you will see the "dead reckoning period" set to 40/GPS_rate + 4. This is a "dead man" timer period to determine if the GPS is still alive or not. If the GPS stops communicating, the algorithm stops using GPS information. The reason for the +4 is to give a little margin, because the GPS communications are asynchronous.

For good performance, it is necessary to account for GPS latency. I have tried various ways of doing that. The technique that I am presently using is a nonlinear predictive filter that looks at differences in GPS position, course, and velocity, to compensate for latency errors.

Someone asked about GPS latency in HILSIM, I am sorry, I don't know, I have not been following HILSIM.

Finally, there were some questions about the method itself.

I tried several methods, none of them performed as well as this very simple method.

One method that I tried that seemed very promising but did not work very well in practice was to simply use the IMU to interpolate between GPS readings. The problem with that is that accuracy is poor if you do not account for GPS latency, and the location estimates are noisy if you do. In any case, you wind up with a large "jump" each time new GPS information comes in, which makes the controls "twitchy".

Whatever method you use, it is very important that you correctly account for acceleration in the calculations, especially centrifugal effects. The key to that is to use airspeed in the calculations, not ground speed. For a long time, MatrixPilot was mistakenly using ground speed in those calculations. It did not make any difference when there was no wind, but under windy conditions, in waypoint mode the controls tended to swerve when flying into the wind. That was because the centrifugal effect was being undercompensated when flying into the wind.

As soon as I figured out my mistake and fixed it, performance of MatrixPilot in the wind improved quite a bit.

Best regards,

Bill

You need to be a member of diydrones to add comments!

Join diydrones

Comments

T3

William Premerlani October 30, 2013 at 5:30pm

Osama,

Conceptually its rather simple to transform accelerometer measurements in body frame into acceleration in earth frame. Assuming that the sign convention you are using for accelerometer to be acceleration minus gravity in body frame, you do this:

acceleration_earth = ( rmat * accelerometer_body ) + tranpose ( { 0 , 0 , G )

where rmat is the direction cosine matrix for transformation from body to earth.

In the case of MatrixPilot, we use the opposite sign convention, accelerometer is gravity minus acceleration, and we use dot products to do the transformation:

accelEarth[0] = VectorDotProduct( 3 , &rmat[0] , gplane )<<1;
accelEarth[1] = - VectorDotProduct( 3 , &rmat[3] , gplane )<<1;
accelEarth[2] = -((int16_t)GRAVITY) + (VectorDotProduct( 3 , &rmat[6] , gplane )<<1);

Best regards,

Bill
OSAMA SARAFARZ October 18, 2013 at 12:35am

can u provide .c code for direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity?
Wojciech Batog October 6, 2011 at 5:49am

I know that's this implementation is unfortunately for a different platform. But since both use basically the same input data it should be possible to implement it on a MEGA.
Or is it far to much load for that kind of processor?
Tibor Arpas October 6, 2011 at 5:35am

Hi Wojciech,

this article is about an algorithm implemented in "MatrixPilot" on the UAV Dev Board hardware. That is a different project then ArduPilot with different (incompatible) processor. If you wan't to go on with the ArduPilot you should ask the ArduPilot community.
Wojciech Batog October 6, 2011 at 5:00am

Hi guys,

I'm a glider pilot and actually bought ArduPilot Mega to use as a base for my project which is to create my own flight instrument- mostly used as a variometer.
As I can see I'm not the first one:)
At first the idea was to create a sort of inertial variometer based on measurements of INS and GPS data, after doing some research i'm leaning towards combining inertial and pressure data for more accurate measurements.

The main idea behind this was to create a sort of 'electronic behind' (since you can feel the lift with your butt before you see it on the on-board vario) providing instant, precise measurements,

This dead reckong algorithm seems like a good way to go .
Do you think the measurements will be precise enough?
Alex June 18, 2011 at 12:30am

Hi Bill,

Many thanks indeed for the update. It is really instrumental to me and mates here who are interested in your algorithm. I will let the whole team know the implementation result very soon.

Cheers,

Alex
T3

William Premerlani June 16, 2011 at 5:35pm

Team,

I received some questions about some of the details of this "dead-reckoning" algorithm (which I really should call loose IMU/GPS integration). Here are the answers.

RMAX is equal to 2**14. Is is the binary equivalent of when a matrix coefficient is equal to 1.

MAX16 is equal to 2**16.

The implementation of the method that I am using has evolved with time. The latest one is very simple, and is contained in the file dead-reckoning.c in the MatrixPilot repository. It was last changed in r918.

One of the parameters that is adjustable is the dead reckoning filter time constant. In the end, I settled on 2.5 seconds, it gives pretty good performance. Using a shorter time constant rejects drift better, but produces more high frequency error.

If you happen to look at the code, you will see the "dead reckoning period" set to 40/GPS_rate + 4. This is a "dead man" timer period to determine if the GPS is still alive or not. If the GPS stops communicating, the algorithm stops using GPS information. The reason for the +4 is to give a little margin, because the GPS communications are asynchronous.

For good performance, it is necessary to account for GPS latency. I have tried various ways of doing that. The technique that I am presently using is a nonlinear predictive filter that looks at differences in GPS position, course, and velocity, to compensate for latency errors.

Someone asked about GPS latency in HILSIM, I am sorry, I don't know, I have not been following HILSIM.

Finally, there were some questions about the method itself.

I tried several methods, none of them performed as well as this very simple method.

One method that I tried that seemed very promising but did not work very well in practice was to simply use the IMU to interpolate between GPS readings. The problem with that is that accuracy is poor if you do not account for GPS latency, and the location estimates are noisy if you do. In any case, you wind up with a large "jump" each time new GPS information comes in, which makes the controls "twitchy".

Whatever method you use, it is very important that you correctly account for acceleration in the calculations, especially centrifugal effects. The key to that is to use airspeed in the calculations, not ground speed. For a long time, MatrixPilot was mistakenly using ground speed in those calculations. It did not make any difference when there was no wind, but under windy conditions, in waypoint mode the controls tended to swerve when flying into the wind. That was because the centrifugal effect was being undercompensated when flying into the wind.

As soon as I figured out my mistake and fixed it, performance of MatrixPilot in the wind improved quite a bit.

Best regards,

Bill
T3

William Premerlani May 26, 2011 at 6:10pm

Hi Tibor,

Welcome aboard. I think it would be very interesting and productive for you do use either an ArduPilotMega or a UAVDevboard (UDB) to estimate the wind.

The accuracy of the wind estimation method that you refer to is limited mainly by the accuracy of the GPS, that comes out to be around 1 or 2 meters/second, so you should be able to get that sort of accuracy in the wind estimation.

There are a few real pilots who have used one or both of these boards for flying model airplanes, but I am not aware of anyone who has used them for anything in a glider.

For the fun of it, last summer I took the UDB up in a 2 seater glider. It computed a wind speed and direction that seemed in pretty good agreement with what you could feel on the ground.

Most UDB users fly with the wind estimation option turned on. Overall experience with it is very favorable.

If you decide to go with the UDB, we have our own separate website and users group, where you can get help with what you are trying to do. If you decide to go with the APM, I am sure you would get help from that group.

Best regards,

Bill Premerlani
Tibor Arpas May 24, 2011 at 4:22am

Hi guys,

I don't do any model flying, DIY hardware or microcontroller programming (yet). But I'm a glider pilot and I'm looking into options to improve my basic instrument panel in some cost effective way. One of the problems I'm trying to solve is to get reliable and relatively precise wind calculation into my cockpit.

The gliding instruments with reliable wind calculation take into account airspeed and GPS and cost $1500 and more. The hardware around here is of course a fraction of that cost. The software is nothing short of amazing.

My question is if you know anybody from your community who's also a glider pilot and maybe investigated this before.

Which one of the projects (ArduPilotMega, UAVDevboard) should I dive into?

How precise is this wind estimation method: http://diydrones.com/forum/topics/wind-estimation-without-an ?

Sorry for hijacking this discussion.I'm a newbie who want's something else than all the other newbies and I didn't find any better place for my questions :-).

Tibor
T3

William Premerlani May 13, 2011 at 7:09am

Hi Gerry,
My preference in developing algorithms for navigation and control is analysis and flight testing, with telemetry capturing plenty of flight information.
There is a technique that I find very effective: set up the controls to fly a given waypoint pattern several times, with some feature that is being tested turned on for one circuit, and then turned off for another circuit. Then, you can compare the two tracks using Google Earth and decide whether the feature improves performance or not.
Best regards,
Bill

of 4

This reply was deleted.