Quaternion math

From OpenLuna
Jump to: navigation, search

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

float q[4];

with values of v[0],..., v[3] 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.

Float Quaternion Multiply

Due to rounding errors, sometimes quaternions need to be 'trimmed' to make sure they are still of unit length.

Float Quaternion Normalise

Then there is the complex conjugate. like in imaginary math, reverse the signs of the complex parts.

Float Quaternion Conj

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.

Float quaternion Transform

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  

Float Quaternion Propagate

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

Float Quaternoinish Cross

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)

Fixed Point Quaternion routines

back to Inertial Navigation

Personal tools
Namespaces

Variants
Actions
Navigation
Toolbox