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.
Many protocols today allow you to upgrade to TLS from within a cleartext version of the protocol. This often falls under the rubric of "STARTTLS", though different protocols have different ways of doing it. I often forget the exact steps, and when i'm debugging a TLS connection (e.g. with tools like 
This could have been a blog post about toothbrushes for monkeys, which I
seem to have dreamed about this morning. But then I was vaguely listening
to the 
In a comment to yesterday s post on 
A new maintenance release 0.10.6 of
... numpads on laptop keyboards.
Tanglu QA
We further improved the automatic archive QA. There is now 
Please do always keep in mind that Tanglu is a new project, so the first release might still have some teething problems but we re working on it. The Aequorea release will be supported one to two more months after the next release is out.
Tanglu Policy
I started to draft a Tanglu policy, which defines stuff like the procedures to become a Tanglu project member and/or developer, some basic values of the distribution, decision making processes etc. This work is at a very early stage right now, and will need lots of feedback later. But as soon as it is done, joining the project will be easier and what Tanglu is will be well-defined.
The policy will also include a Code-of-Conduct as an additional document from the start.
We need help! 