i.e. Factorized Quaternion Algorithm

Quaternion: what’s this ?

In aeronautic, it is common to use Euler angles to describe the attitude of an object (space position):

When you have all three angles, you can get any angle by just rotating your object from a given reference base in the following order: yaw, pitch and then roll (this order is a choice).

Quaternions are mathematical objects containing four values that represent spatial rotation in a slightly different way: for an angle θ and a rotation vector u, the quaternion is For some practical reasons (like computation efficiency and the use of them in our Kalman filter), it is more efficient to compute directly quaternions, using the FQA algorithm in order to precisely know our attitude.

Procedure

Our Inertial Measurement Unit contains 3*3 sensors: accelerometers, gyroscopes and magnetometers. During slow motions, the orientation can be precisely known by just using accelerometers and magnetometers.

Indeed, accelerometers measure all applied forces excepted the gravity one. As the object is in slow or without motion, the only remaining applied force is the compensation of gravity.

First, the sine of the pitch angle can be directly read into normalized x-axis accelerometer, by a simple projection: Then, the cosine of the pitch angle can be known, if we choose by convention that pitch is contained in [-π/2 : π/2]: As quaternions are defined on half angle sine functions, they need to be computed by using the following formula: The pitch quaternion is constructed: Then, by another projection, the opposite of the sine of the roll can be fetched by dividing normalized y-axis accelerometers by the cosine of the pitch, and the opposite of the cosine of the roll by dividing normalized z-axis by the cosine of the pitch:  A slightly exception may occur if the cosine of the pitch is null, we decide then that roll is null too. Once again, roll is contained in [-π/2 : π/2] and we compute its half angle sine and cosine.

The roll quaternion is constructed: We finally use normalized magnetometer to get yaw orientation. We first apply pitch and roll rotations on it and then get it by solving a small equation system involving exact magnetic north vector (see FQA paper in source for more details). This time, as we know the sine and cosine of the yaw, it is contained in [-π : π].

We construct the yaw quaternion:

The final quaternion is the result of quaternion multiplication: (see Wikipedia to get the exact formula)

Features

This algorithm is really useful because it lets users know exact attitude using only square roots and standard operations. No sine or cosine operations are needed and the quaternion form is used in our Kalman filter.

Sources

A Simplified Quaternion-Based Algorithm for Orientation Estimation From Earth Gravity and Magnetic Field Measurements

Wikipedia article