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

## Comments

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

can u provide .c code for direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity?

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?

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.

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?

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

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

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

Next