Tracking Orientation with Quaternions

I spent the flight back from china and the weekend adding orientation tracking to AltOS. I'd done a bit of research over the last year or so working out the technique, but there's always a big step between reading about something and actually doing it. I know there are a pile of quaternion articles on the net, but I wanted to write down precisely what I did, mostly as a reminder to myself in the future when I need to go fix the code...

Quaternion Basics

Quaternions were invented by Sir William Rowan Hamilton around 1843. It seems to have started off as a purely theoretical piece of math, extending complex numbers from two dimensions to four by introducing two more roots of -1 and defining them to follow:

i² = j² = k² = ijk = -1

Use these new roots to create numbers with four real components, three of which are multiplied by our three roots:

r + ix + jy + kz

With a bit of algebra, you can figure out how to add and multiply these composite values, using the above definition to reduce and combine terms so that you end up with a set which is closed under the usual operations.

Then we add a few more definitions, like the conjugate:

q = (r + ix + jy + kz)
q* = (r - ix - jy - kz)

The norm:

| q | = ✓(qq*) = ✓(r² + x² + y² + z²)

'u' is a unit quaternion if its norm is one:

| u | = 1

Quaternions and Rotation

Ok, so we've got a cute little 4-dimensional algebra. How does this help with our rotation problem? Let's figure out how to rotate a point in space by an arbitrary rotation, defined by an axis of rotation and an amount in radians.

First, take a vector, 'v', and construct a quaternion, 'q' as follows:

q = 0 + ivx + jvy + kvz

Now, take a unit quaternion 'u', which represents a vector in the above form along the axis of rotation, and a rotation amount, ω, and construct a quaternion 'r' as follows:

r = cos ω/2 + u sin ω/2

With a pile of algebra, you can show that the rotation of 'q' by 'r' is:

q° = r q r*

In addition, if you have two rotations, 's' and 'r', then the composite rotation, 't', a rotation by 'r' followed by 's' can be computed with:

q°° = s (r q r*) s*

    = (sr) q (r*s*)

    = (sr) q (sr)*

t   = s r

q°° = t q t*

That's a whole lot simpler than carrying around a 3x3 matrix to do the rotation, which makes sense as a matrix representation of a rotation has a bunch of redundant information, and it avoids a pile of problems if you try to represent the motion as three separate axial rotations performed in sequence.

Computing an initial rotation

Ok, so the rocket is sitting on the pad, and it's tilted slightly. I need to compute the initial rotation quaternion based on the accelerometer readings which provide a vector, 'g' pointing up. Essentially, I want to compute the rotation that would take 'g' and make it point straight down. Construct a vector 'v', which does point straight up:

g = (0, ax, ay, az) / norm(0, ax, ay, az)
v = (0, 0, 0, 1)

G is 'normalized' so that it is also a unit vector. The cross product between g and v will be a vector normal to both, which is the axis of rotation. As both g and v are unit vectors, the length of their cross product will be sin ω

a = g × v

  = u sin ω

The angle between g and v is the dot product of the two vectors, divided by the length of both. As both g and v are unit vectors, the product of their lengths is one, so we have

cos ω = g · v

For our quaternion, we need cos ω/2 and sin ω/2 which we can get from the half-angle formulae:

cos ω/2 = ✓((1 + cos ω)/2)
sin ω/2 = ✓((1 - cos ω)/2)

Now we construct our quaternion by factoring out sin ω from the 'a' and:

q = cos ω/2 + u sin ω sin ω/2 / sin ω

Updating the rotation based on gyro readings

The gyro sensor reports the rate of rotation along all three axes, to compute the change in rotation, we take the instantaneous sensor value and multiply it by the time since the last reading and divide by two (because we want half angles for our quaternions). With the three half angles, (x,y,z), we can compute a composite rotation quaternion:

   cos x cos y cos z + sin x sin y sin z +
i (sin x cos y cos z - cos x sin y sin z) +
j (cos x sin y cos z + sin x cos y sin z) +
k (cos x cos y sin z - sin x sin y cos z)

Now we combine this with the previous rotation to construct our current rotation.

Doing this faster

If we read our sensor fast enough that the angles were a small fraction of a radian, then we could take advantage of this approximation:

sin x ≃ x
cos x ≃ 1

that simplifies the above computation considerably:

1 + xyz + i (x - yz) + j (y + xz) + k (z - xy)

And, as x, y, z « 1, we can further simplify by dropping the quadratic and cubic elements as insignificant:

1 + ix + jy + kz

This works at our 100Hz sampling rate when the rotation rates are modest, but quick motions will introduce a bunch of error. Given that we've got plenty of CPU for this task, there's no reason to use this simpler model. If we did crank up the sensor rate a bunch, we might reconsider.

Computing the Current Orientation

We have a rotation quaternion which maps the flight frame back to the ground frame. To compute the angle from vertical, we simply take a vector in flight frame along the path of flight (0, 0, 0, 1) and rotate that back to the ground frame:

g = r (0 0 0 1) r*

That will be a unit vector in ground frame pointing along the axis of the rocket. The arc-cosine of the Z element will be the angle from vertical.

Results

All of the above code is checked into the AltOS git repository

I added a test mode to the firmware that just dumps out the current orientation over the USB link which lets you play with rotating the board to see how well the system tracks the current orientation. There's a bit of gyro drift, as you'd expect, but overall, the system tracks the current orientation within less than a tenth of a degree per second.

Even with all of this computation added, the whole flight software is consuming less than 7% of the STM32L CPU time.