Fast Quaternion Integration for Attitude Estimation

  One of the central challenges in creating a flying machine control system is attitude estimation.  The better the estimate, more correctly the control system can make flight adjustments.  The better theses adjustments are, the better the aircraft can go where it's supposed to go or stay where it is wanted.
  I will be writing a series of blog posts that go into detail describing how I have chosen to design my IMU complementary filter for attitude estimation.  Each post will introduce a new sensor and explain how I use it to enhance the output of my system.  To begin with I will start with the gyroscope.

Gyroscope:

This sensor outputs the measured rotation rate around three orthogonal axii.  The integer output of the sensor can be converted to degrees per second or radians per second.  For programming/mathematical reasons, radians per second are almost always used.  The gyroscope is the most important sensor relating to the stability of the aircraft, it is the only sensor required for a manually piloted quadcopter.  

The main strength of the gyroscope (gyro for short), is that it has a high resistance to noise/vibration and it provides very quick response to changes in aircraft movement.  This allows the control system to respond to rotational deviations rapidly, which is the foundation of the quadcopter control stability.  

This ability to respond quickly and resist noise also makes the gyro the foundation for the attitude estimate system.  In addition to stability control (where un-intended rotations are resisted), it may also be desirable to determine how the aircraft is oriented in space (pitch, roll, heading).  By taking the readings from the gyro and multiplying them by the time since the last measurement, the reading can be converted from radians per second to radians.  With some fancy math, we can take each of these little slices of rotation and combine them in such a way that we can track the total amount of angular movement we have made.  It is this math algorithm that is the heart of any attitude estimate system.

The biggest weakness of the gyroscope is that over time error can accumulate in the attitude estimate.  So for short periods of time is can be very accurate, but over "very long" periods of time an observer will tend to see something generally called gyro drift where the accumulated error becomes perceptible and slowly grows larger.  It is common to use other sensors to help counter this long term accumulation of error.  Future posts will go into detail on how I accomplish this.

  In order to take the gyro readings and combine them in a way that allows for tracking the orientation of the aircraft, some type of mathematical tool must be used.  This tool should have the following abilities or characteristics:

1. Be able to combine a very small change in orientation with the previous attitude estimate so as to create a new attitude estimate.
2. Be able to rotate a 3D vector from the aircraft reference frame to the world reference frame.
3. Be able to rotate a 3D vector from the world reference frame to the aircraft reference frame.
4. It must be immune to the phenomenon known as gimbal lock.

  There are two mathematical tools commonly used that meet these requirements.  These are a rotation matrix and a quaternion.  The rotation matrix consists of a 3x3 grid of numbers while the quaternion is composed of one real number and three imaginary numbers.  I'm not going to go into extreme detail on how exactly these things works, as that's beyond the scope of this article, but I will talk about the characteristics that make them more or less attractive for my use.  Both of these tools use a form of multiplication to combine rotations.  So the multiplication of rotation 1 with rotation 2 will result in a rotation which describes a single rotation that includes both 1 and 2.  As is the case with all 3D rotations, the order of multiplication effects the result (A*B != B*A).
  When I started this project I was working with the Arduino platform with the Atmel Atmega328P micro-controller.  This is a very easy unit to work with given the Arduino platform, but is not very powerful.  As you can read about in my early blog posts, I intended to use matrix math to perform my attitude estimate tracking.  The draw backs to this "tool" are the following:

1. Matrix multiplication requires a large number of floating point multiplications, which is time consuming for a low powered processor (27 mult, 18 add/sub). 
2. The matrix is really a set of orthogonal 3D vectors.  Over time these vectors become non-orthogonal, which reduces accuracy and perpetuates more error.  Solving this requires periodic corrections to the orthogonality, these are a waste of processing power and time.
3. As noted in point 2, the matrix is composed of vectors.  For the matrix to function correctly these must be unit vectors.  Over time magnitude error accumulates due to the lack of precision in the 32bit float and these vectors change in magnitude.  These three vectors should be re-normalized periodically, which again is a waste of processing power and time.

  After struggling for a while with trying to handle the above issues elegantly and efficiently on a low power processor, I decided to put some effort into learning to use quaternions.  This was initially a bit of a challenge as quaternions are significantly less intuitive to understand and manipulate than a matrix.  Once I got a grasp of the basics of how to use them, I really liked what I saw.  The following advantages stood out to me:

1. There are no issues with orthogonality error.
2. If a quaternion does need to be normalized, it would only require one normalization operation (unlike the three required for a rotation matrix).
3. Multiplying two unit quaternions to combine rotations required fewer operations (16 mult, 12 add/sub)
4. Storage requirements are lower (4x 32bit floats compared to 9x).

  Once I decided to use quaternions for attitude estimation, I needed to figure out how to convert the rotation rate vector generated by the gyro into a unit quaternion that would represent the very small angular change since my last attitude update.  The most obvious option to me was to convert the gyro vector into angle-axis representation and then convert that into a quaternion.  This was perfectly functional, but it was time consuming for the micro controller to create this incremental update quaternion due to "expensive"  operations such as trig functions, divisions, and square root.  I decided to apply the small angle approximation to this equations to reduce the processing time required to create it.  The results were amazing.  The set of equations simplified to a much greater extent than I had expected.  It went from requiring expensive sin/cos functions, square root function, and division to only needing 8x multiplications and 3x add/sub.  Here is a link to my derivation and analysis of this simplified equation.  For reference, here's the equations:

t_2 = timeDelta * 0.5

Q.x = gyro.x * t_2 

Q.y = gyro.y * t_2

Q.z = gyro.z * t_2

Q.w = 1.0 - 0.5 * (Q.x * Q.x + Q.y * Q.y + Q.z * Q.z)

  The quaternion Q generated by this set of equations can then be multiplied by the previous attitude estimate to create the current attitude estimate:

Q_orientation_current = Q_gyro * Q_orientation_last 

  Since the gyro values are generated in relation to the aircraft's frame of reference, the order of multiplication is as above.  If we were able to observe and measure the aircraft's rotation rates in relation to the earth's frame of reference, then the order would be reversed.
  On an Atmel Atmega  328P (16Mhz) creating the incremental quaternion and multiplying it by the previous attitude quaternion requires 370us.  This is significantly faster than can be done by rotation matrix (588us) allowing this very low power processor to process gyro samples at a rate that can significantly reduce the error caused by rotational non-linearity (acceleration) or the saved time can be used to perform other operations.  As was noted earlier, this solution requires no wasted time correcting orthogonality errors and only requires normalization extremely infrequently.

  The next two posts will cover how my quaternion based complementary filter can use accelerometer and magnetometer data to compensate for gyro drift.  (Edit: Go here to see post with example IMU code)

  Please let me know if you have any questions.

Phillip