I have recently performed flight tests on a major change in the way acceleration is accounted for in performing roll-pitch drift compensation from accelerometer measurements. The theory behind the idea was recently published in a discussion on this website. Since then, I have made a couple of refinements to the idea, the latest version is documented here.

Implementation in MatrixPilot is shown here. There is one slight "tweak" in the implementation that is not covered in the theory paper: although in principle the new method can provide yaw drift information, in the implementation I have used it for roll and pitch only.

I put off posting a blog entry until I had flight test data, which I now have. The above picture is from one of several recent test flights with a UAVDevBoard running MatrixPilot on an EasyStar, that shows that the idea works. What is shown is a comparison of the GPS track and the "dead-reckoning" computation. The yellow arrows indicate the wind direction and the plane icons indicate the attitude of the plane. The fact that the two tracks match rather closely is an indication that the estimated attitude matches the actual attitude, otherwise there would be a large discrepancy.

A major advantage of the new method is that it does not require a model for computing aerodynamic forces, nor does it make any of the approximations that previous methods do. It is based on kinematics only, and does not make any approximations. Therefore, it will provide accurate acceleration compensation for any platform, including quads, cars, motorcycles, baseballs, anything you want to attach an IMU to. (For anything but fixed wing, it does require a magnetometer.)

Now, some more pictures.

The next picture is taken from a portion of the flight in which the EasyStar flew in a tight circle for several minutes. Both GPS and dead-reckoning tracks are plotted. Two out of many revolutions are shown. It is during circling flight that it is particularly critical that acceleration be correctly accounted for. As can be seen, GPS and dead-reckoning match, indicating that attitude estimate is correct.


The final picture is the track of the entire 20 minutes of one of the test flights, in which I did my best to "break" the algorithm.


Best regards,


E-mail me when people leave their comments –

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

Join diydrones


  • T3

    Hi Roy,

    Its great to hear from you, as always. I am glad that you are interested in this topic, and have given it some thought...

    Regarding doing the computations in the earth frame versus doing them in the body frame...

    Because the gyro/accelerometer update rate is much greater than the GPS reporting rate, there is a distinct improvement in accuracy by doing the calculations in the earth frame. That is because, in the earth frame, the integral of the rotation matrix times the accelerometer readings are compared with changes in velocity.

    However, in the body frame, we need the integral of the transpose of the rotation rate times the instantaneous acceleration. Except the GPS does not report instantaneous acceleration, it reports change in velocity over finite time periods. We might be tempted to use the average acceleration computed from GPS change in velocity, but in the body frame, that leads to an approximation that dilutes the accuracy of the calculation.

    So, it is much better to do the computations in the earth frame.

    I revised my report to include an appendix with some math that shows the computations can be done exactly in earth frame, but only approximately in the body frame. So, if you would like to study the math, read the appendix in this revised report.

    Best regards,


  • Bill,

    Great work, as always. I have some suggestions for improving the efficiency of the calculation:

    - Work in the body axes - that is, transform the GPS velocity into the body axes, instead of transforming the accelerometer reading into earth-axes. This saves one transform (the one in eq 6 of the paper).

    - Subtract the GPS velocity from the sensed accelerometer value. This gives the sensed gravity vector (ignoring noise, sensor imperfections, etc.). Then take the cross product of this with the estimated gravity vector - i.e. the bottom row of DCM. This calculation could then be used as the error vector, eliminating the concerns of quadratic data in eq 4 and the need to "normalize" in eq 5.

    Yes, the calculations technically would be done in the body axes instead of the earth axes, but mathematically this approach is equivalent.

    Also, I think the same technique could be performed using (3-D) optical flow sensors (with suitable care to account for coordinate system misalignment).

    - Roy

  • T3

    Hi Ryan,

    Its really great to hear from you!! What have you been up to?..

    Now, in response to your question...

    The end result is more accurate attitude during accelerated motion.

    This is an exact method for compensating for acceleration in using an accelerometer to provide a roll-pitch reference vector. It is not a filter. Unlike previous methods for compensating for acceleration, it makes no assumptions about the dynamic model of the object being tracked, so it will work for anything that you might want to attach an IMU onto, including rockets, helis, quads, baseballs, boats, motorcycles, etc. It is based on kinematics only. It should work especially well for quads and helis.

    Previous methods for providing a roll-pitch reference vector used gravity as the reference vector. They started with the accelerometer vector, which is equal to gravity minus acceleration, and added an estimated acceleration vector. The estimation of the acceleration vector was usually done based on a centrifugal calculation, which really only works for fixed wing, and then was only an approximation.

    This new method uses gravity minus acceleration as the roll-pitch reference vector, so it does not require any adjustment to the accelerometer signal at all. All that it needs is a computation of the exact gravity minus acceleration vector in the earth frame of reference, which can be done very simply from the GPS velocity. As a bonus, the method naturally works with integrals and changes in GPS velocity, so the accuracy improves.

    I am not sure why I did not think of this method before, it really blows the doors off the previous methods for dealing with acceleration. I think the reason is that the initial discussions on this forum focused on doing computations in the body frame. I do not think that anyone, including myself, thought that the computations could be done in the earth frame instead. And basing the computations on centrifugal effects was intuitively appealing...and flawed.

    Best regards,


  • Developer

    Just a quick verification:  This is a fixed gain filter that feeds the GPS velocity/position estimation error into the accelerometers.  There fore the end result is more accurate attitude.  Or is the error pushed into the attitude directly?  

  • T3

    Hi Tridge,

    Thanks. I am optimistic that you will get this algorithm working in APM.

    Best regards,


  • Developer

    Great work Bill!

    I'll have another go at this algorithm in APM soon. It would be a major step forward for multi-copters if this works as well as it should!

    Cheers, Tridge

  • T3


    Just to be clear, the new algorithm is for accounting for acceleration in roll-pitch gyro drift compensation, in a way that does not require any assumptions or approximations. So, it is perfectly suited for quads. Andrew Tridgell has done simulations that show it will improve loitering performance for quads. It remains for someone to implement and test the new algorithm in quad estimation and control.

    The new algorithm cannot compensate location estimation for GPS drift, that is not its purpose. It simply prevents acceleration from inducing roll-pitch attitude errors. Because the algorithm uses changes in GPS reported velocity, it is not itself sensitive to GPS drift.

    One measure that I use to estimate the accuracy of roll-pitch attitude estimation is whether or not the GPS track matches the "dead-reckoning", IMU track. Any roll-pitch attitude estimation error will make the two tracks diverge, because it will generate an error in the dead-reckoning computations.

    Best regards,


  • Looks good.

    I guess it's basically impossible to determine if the new algorithm is essentially "perfect", since the GPS itself has a lot of error.

    I suppose one test would be to take off, fly around, land, and then return the aircraft to the exact spot it started. Then see how far off the algorithm is.

  • Developer

    I flew Bill's test branch this evening, and it seemed to fly well and smoothly. It looked fine in the post flight analysis.

    I'm sure it will take a lot more testing before we are completely sure about the new algorithm.

    Thanks Bil for publishing the concept and the code.

  • Moderator


This reply was deleted.