Okay here is some simple routines for doing the Quaternion Math
First the representation of a vector can be written as:
Vector = Bi + Cj + Dk
where i,j,k are mutually perpendicular imaginary numbers (right hand rule).
note: i*i = -1 (the usual imaginary number def) where i*j = k and j * i = -k etc.
A quaternion is a vector with an associated scalar so that
Quaternion = A + Bi + Cj + Dk
with the added twist that the length of the vector + scalar == 1
sqrt(A*A + B*B + C*C + D*D) = length = 1.0
for software in the C programming language we can easily define a quaternian as a 4 element vector
with values of v,..., v corresponding to A,...,D
to multiply two quaternions we just multiply like any other equation.
(A+Bi+Cj+Dk)*(E+Fi+Gj+Hk) = AE + EFi + AGj + AHk + BiE + BiFi + BiGj + ...
where we can simplify terms like +BiGj into +BGk etc.
Due to rounding errors, sometimes quaternions need to be 'trimmed' to make sure they are still of unit length.
Then there is the complex conjugate. like in imaginary math, reverse the signs of the complex parts.
Now, to transform a vector, V, (say a fix on a star) in the vehicles body frame, to the equivalent vector in the reference frame, with the rotation between the two defined by the quaternion Q we simple do the math
Vref = Q Vbody Conj(Q)
remembering that the Vector is represented with a zero scalar part and can be any length.
finally we want to take some (small) rotations, as measured in our vehicles body frame, and update the quaternion to reflect these motions. First we need to make these rotations into a quaternion, then fold them into (multiply) the existing quaternian. If the angles (in radians) are small, the vector portion will be the normalized vector of the component rotations. The scalar portion is the Cosine of 1/2 the resultant rotation about the quaternion. to keep the total magnitude at one, each vector component is multiplied by the Sine. This looks like:
Cos(t/2) + sin(t/2)Wxi/t + sin(t/2)Wyi/t + sin(t/s)Wzk/t where t = the magnitude of Wx,W,Wz
As an addendum, sometimes the vectors in this math need to be crossed. Since we can represent them as a not necissarily normalized quaternion with a scalar part of zero, we might as well include the code
This is all great except for one or two problems. This is all done in floating point math. Micro-controllers are better at MIPS than FLOPS. So the software needs to be converted to fixed point Math and the Sin and Cos (and sqrt) functions need to be converted into fixed point approximations for speed. (there is a few other ways to speed up part of the math in there also :)
I suppose here is where the discussion showing that all the quaternion components are less than one so we will represent them as binary fractions. (bit16,bit15,bit14,bit13... = 1,1/2,1/4,1/8 ...etc)
back to Inertial Navigation