Factored Quaternion Algorithm

The factored quaternion algorithm (FQA) produces a quaternion output to represent the orientation, restricting the use of magnetic data to the determination of the rotation about the vertical axis.

The FQA and the TRIAD algorithm produce an equivalent solution to the same problem, with the difference that the former produces a quaternion, and the latter a rotation matrix.

Magnetic variations cause only azimuth errors in FQA attitude estimation. A singularity avoidance method is used, which allows the algorithm to track through all orientations.

Warning

This algorithm is not applicable to situations in which relatively large linear accelerations due to dynamic motion are present, unless it is used in a complementary or optimal filter together with angular rate information.

The Earth-fixed coordinate system \((^ex,\,^ey,\,^ez)\) is defined following the North-East-Down (NED) convention.

A body coordinate system \((^bx,\,^by,\,^bz)\) is attached to the rigid body whose orientation is being measured, and the sensor frame \((^sx,\,^sy,\,^sz)\) corresponds to the sensor module conformed by the accelerometer/magnetometer system.

The body coordinate system differs from the sensor frame by a constant offset, if they are not coinciding. For this method they are assumed to occupy the same place.

From Euler’s theorem of rotations, we can use a unit quaternion \(\mathbf{q}\) as an axis-angle rotation:

\[\begin{split}\mathbf{q} = \begin{pmatrix} \cos\frac{\beta}{2} \\ u_x\sin\frac{\beta}{2} \\ u_y\sin\frac{\beta}{2} \\ u_z\sin\frac{\beta}{2} \end{pmatrix}\end{split}\]

where \(\mathbf{u}=\begin{bmatrix}u_x & u_y & u_z\end{bmatrix}^T\) is the rotation axis, and \(\beta\) is the rotation angle. See the Quaternion reference page for further details of it.

A rigid body can be placed in an arbitrary orientation by first rotating it about its Z-axis by an angle \(\psi\) (azimuth or yaw rotation), then about its Y-axis by angle \(\theta\) (elevation or pitch rotation), and finally about its X-axis by angle \(\phi\) (bank or roll rotation).

Elevation Quaternion

The X-axis accelerometer senses only the component of gravity along the X-axis, and this component, in turn, depends only on the elevation angle.

Starting with the rigid body in its reference orientation, the X-axis accelerometer is perpendicular to gravity and thus registers zero acceleration. The Y-axis accelerometer also reads zero, while the Z-axis accelerometer reads \(-g\). If the body is then rotated in azimuth about its Z-axis, the X-axis accelerometer still reads zero, regardless of the azimuth angle.

If the rigid body is next rotated up through an angle \(\theta\), the X-axis accelerometer will instantaneously read

\[a_x = g\sin\theta\]

and the Z-axis will read

\[a_z = -g\cos\theta\]

where \(9.81 \frac{m}{s^2}\) is the gravitational acceleration and \(\mathbf{a}=\begin{bmatrix}a_x & a_y & a_z\end{bmatrix}^T\) is the measured acceleration vector in the body coordinate system.

For convenience, the accelerometer and magnetometer outputs are normalized to unit vectors, so that:

\[\begin{split}\begin{array}{rcl} \mathbf{a} &=& \frac{\mathbf{a}}{\|\mathbf{a}\|} \\ \mathbf{m} &=& \frac{\mathbf{m}}{\|\mathbf{m}\|} \end{array}\end{split}\]

From the normalized accelerometer measurements, we can get:

\[\sin\theta = a_x\]

In order to obtain an elevation quaternion, a value is needed for \(\sin\frac{\theta}{2}\) and \(\cos\frac{\theta}{2}\). From trigonometric half-angle formulas, half-angle values are given by

\[\begin{split}\begin{array}{rcl} \sin\frac{\theta}{2} &=& \mathrm{sgn}(\sin\theta) \sqrt{\frac{1-\cos\theta}{2}} \\ \cos\frac{\theta}{2} &=& \sqrt{\frac{1+\cos\theta}{2}} \end{array}\end{split}\]

where \(\mathrm{sgn}()\) is the sign function.

Because elevation is a rotation about the Y-axis, the unit quaternion representing it will be expressed as:

\[\begin{split}\mathbf{q}_e = \begin{pmatrix}\cos\frac{\theta}{2} \\ 0 \\ \sin\frac{\theta}{2} \\ 0\end{pmatrix}\end{split}\]

Roll Quaternion

Changing the roll angle alters the measurements, so that the accelerometer readings are:

\[\begin{split}\begin{array}{rcl} a_y &=& -\cos\theta\sin\phi \\ a_z &=& -\cos\theta\cos\phi \end{array}\end{split}\]

Note

In reality the measurements are \(-g\cos\theta\sin\phi\) and \(-g\cos\theta\cos\phi\), with a magnitude equal to \(g\), but when normalized their magnitude is equal to \(1\), and \(g\) is overlooked.

If \(\cos\theta\neq 0\), the values of \(\sin\phi\) and \(\cos\phi\) are determined by:

\[\begin{split}\begin{array}{rcl} \sin\phi &=& -\frac{a_y}{\cos\theta} \\ \cos\phi &=& -\frac{a_z}{\cos\theta} \end{array}\end{split}\]

But if \(\cos\theta=0\) the roll angle \(\phi\) is undefined and can be assumed to be equal to zero. We obtain the half-angles similar to the elevation quaternion, and roll quaternion is then computed as:

\[\begin{split}\mathbf{q}_r = \begin{pmatrix}\cos\frac{\phi}{2} \\ \sin\frac{\phi}{2} \\ 0 \\ 0\end{pmatrix}\end{split}\]

Azimuth Quaternion

Since the azimuth rotation has no effect on accelerometer data, the strategy is to use the readings of the magnetometer, but first we have to rotate the normalized magnetic readings \(^b\mathbf{m}\) into an intermediate coordinate system through the elevation and roll quaternions:

\[^e\mathbf{m} = \mathbf{q}_e\mathbf{q}_r \,^b\mathbf{m}\mathbf{q}_r^{-1}\mathbf{q}_e^{-1}\]

where \(^b\mathbf{m}=\begin{pmatrix}0 & ^bm_x & ^bm_y & ^bm_z\end{pmatrix}\) is the magnetic field measured in the body frame, and represented as a pure quaternion.

The rotated magnetic measurements should correspond to the normalized known local geomagnetic field [1] vector \(\mathbf{n}=\begin{bmatrix}n_x & n_y & n_z\end{bmatrix}\), except for the azimuth:

\[\begin{split}\begin{bmatrix}n_x \\ n_y\end{bmatrix}= \begin{bmatrix}\cos\psi & -\sin\psi \\ \sin\psi & \cos\psi\end{bmatrix} \begin{bmatrix}^em_x \\ ^em_y\end{bmatrix}\end{split}\]

where \(\psi\) is the azimuth angle. We normalize both sides to enforce equal length of its vectors:

\[\begin{split}\begin{bmatrix}N_x \\ N_y\end{bmatrix}= \begin{bmatrix}\cos\psi & -\sin\psi \\ \sin\psi & \cos\psi\end{bmatrix} \begin{bmatrix}M_x \\ M_y\end{bmatrix}\end{split}\]

with:

\[\begin{split}\begin{array}{rcl} \begin{bmatrix}N_x \\ N_y\end{bmatrix} &=& \frac{1}{\sqrt{n_x^2+n_y^2}} \begin{bmatrix}n_x \\ n_y\end{bmatrix} \\ \begin{bmatrix}M_x \\ M_y\end{bmatrix} &=& \frac{1}{\sqrt{^em_x^2+^em_y^2}} \begin{bmatrix}^em_x \\ ^em_y\end{bmatrix} \end{array}\end{split}\]

And now we just solve for the azimuth angle with:

\[\begin{split}\begin{bmatrix}\cos\psi \\ \sin\psi \end{bmatrix} = \begin{bmatrix}M_x & M_y \\ -My & M_x \end{bmatrix} \begin{bmatrix}N_x \\ N_y \end{bmatrix}\end{split}\]

In the same manner as with the elevation and roll, we estimate the half-angle values and define the azimuth quaternion as:

\[\begin{split}\mathbf{q}_a = \begin{pmatrix}\cos\frac{\psi}{2} \\ 0 \\ 0 \\ \sin\frac{\psi}{2} \end{pmatrix}\end{split}\]

Final Quaternion

Having computed all three quaternions, the estimation representing the orientation of the rigid body is given by their product:

\[\mathbf{q} = \mathbf{q}_a\mathbf{q}_e\mathbf{q}_r\]

It should be noted that this algorithm does not evaluate any trigonometric function at any step, although a singularity occurs in the FQA if the elevation angle is \(\pm 90°\), making \(\cos\theta=0\), but that is dealt with at the computation of the first quaternion.

Footnotes

[1]

The local geomagnetic field can be obtained with the World Magnetic Model. See the WMM documentation page for further details.

References

[Yun]

Xiaoping Yun et al. (2008) A Simplified Quaternion-Based Algorithm for Orientation Estimation From Earth Gravity and Magnetic Field Measurements. https://ieeexplore.ieee.org/document/4419916

class ahrs.filters.fqa.FQA(acc: ndarray | None = None, mag: ndarray | None = None, mag_ref: ndarray | None = None)

Factored Quaternion Algorithm

Parameters:

• acc (numpy.ndarray, default: None) – N-by-3 array with N measurements of the gravitational acceleration.

• mag (numpy.ndarray, default: None) – N-by-3 array with N measurements of the geomagnetic field.

• mag_ref (numpy.ndarray, default: None) – Reference geomagnetic field. If None is given, defaults to the geomagnetic field of Munich, Germany.

Raises:

ValueError – When dimension of input arrays acc and mag are not equal.

Examples

>>> from ahrs.filters import FQA
>>> g = [0, 0, -9.81]
>>> m = [21_000.0, 1_486.0, 43_882.0]
>>> fqa = FQA(acc=g, mag=m)
>>> fqa.Q
array([ 1.00000000e+00,  0.00000000e+00,  0.00000000e+00, -2.32409382e-05])

estimate(acc: ndarray, mag: ndarray | None = None) → ndarray

Attitude Estimation.

Parameters:

• acc (numpy.ndarray) – Sample of tri-axial Accelerometer.

• mag (numpy.ndarray) – Sample of tri-axial Magnetometer.

Returns:

q – Estimated quaternion.

Return type:

numpy.ndarray