RollPitchDriftCompensation

William Premerlani, December 1, 2011

Problem

We wish to use an accelerometer and GPS to detect gyro roll-pitch drift in an IMU,

without any information about orientation of the aircraft with respect to air speed vector.

Background

During the development of the theoretical foundations of MatrixPilot, the conventional wisdom of the DIY UAV community was that the best way to account for acceleration in roll-pitch drift compensation is to compute the acceleration in the body frame, using GPS acceleration to determine forward acceleration, and the cross product of the aircraft velocity vector with gyro rotation to compute cross acceleration. In order for this

technique to be accurate, the angles of attack and side slip must be known, estimated, or assumed. In the case of fixed wing aircraft, this introduces errors during high acceleration. In the case multicopters, this introduces very large errors, because a

multicopter does not naturally align itself with the direction of flight. So, a method that does not require attack and side slip angles would be more accurate.

Solution

The solution is trivial if you use gravity minus acceleration as the reference vector, rather than gravity. In the body (aircraft) frame of reference, the vectors reported by the

accelerometer are the differences between the gravity and acceleration vectors, as seen in the body frame of reference:

()()()()()()ter

accelerome of output frame

body in seen as on,accelerati frame

body in seen as gravity, ===-=t t t t t t b b b b b b A a g a g A Equation 1

Our estimate of the rotation matrix provides a connection between body and earth frame of reference: ()()()()()matrix rotation the of estimate our ˆframe

earth in seen as on,accelerati frame earth in seen as gravity, ˆ===-=×t t t t t R a g a g A R e e e

e b Equation 2

If our estimate of the rotation matrix were correct, equation 2 would be an identity.

However, because of gyro drift, equation 2 will not be exactly satisfied. In particular, the

cross product of the vectors on the left and right hand side of Equation 2 is an indication of the drift error. To improve accuracy, it is a good idea to integrate both sides of the equation between GPS reports. That way we can use velocity information instead of acceleration:

()()()()()()1212ˆ21t t t t d t t e

e e b V V g A R --×-=××òt t t Equation 3

Use equation 3 on a regular basis (such as synchronized with GPS messages) to estimate drift. The left side is computed as the sum of multiplications of the rotation matrix with the accelerometer output. The first term on the right hand side is the time interval times the known, constant gravity vector. The second term is the difference in velocity vectors reported by the GPS.

Take the cross product of the left hand side of equation 3 with the right hand side. This is an indication of the drift rotation. Note that this method is capable of detecting yaw drift as well as roll-pitch, if the aircraft is accelerating laterally. So, it can be integrated with magnetometer yaw drift compensation. Also note that no assumptions have been made about the orientation of the aircraft.

So, the error rotation vector is given by:

()()()()()()()()1212ˆ221t t t t d t t t earth e e e b V V g A R error --×-´÷÷ø

öççèæ××=òt t t Equation 4

Finally, transform the error in the body frame of reference. It can then be used directly as an input to the drift PI feedback controller:

()()22ˆt t earth

body error R error T ×= Equation 5