Algebraic Quaternion Algorithm#

Roberto Valenti’s Algebraic Quaterion Algorithm (AQUA) [Valenti2015] estimates a quaternion with the algebraic solution of a system from inertial/magnetic observations, solving Wahba’s Problem.

AQUA computes the “tilt” quaternion and the “heading” quaternion separately in two sub-parts. This avoids the impact of the magnetic disturbances on the roll and pitch components of the orientation.

AQUA can be used with a complementary filter to fuse the gyroscope data together with accelerometer and magnetic field readings. The correction part of the filter is based on the independently estimated quaternions and works for both IMU (Inertial Measurement Unit) and MARG (Magnetic, Angular Rate, and Gravity) sensors [Valenti2016].

Quaternion as Orientation#

Any orientation in a three-dimensional euclidean space of a frame \(A\) with respect to a frame \(B\) can be represented by a unit quaternion (a.k.a. versor), \(\mathbf{q}\in\mathbb{H}^4\), in Hamiltonian space defined as:

\[\begin{split}^B_A\mathbf{q} = \begin{bmatrix}q_w\\q_x\\q_y\\q_z\end{bmatrix} = \begin{bmatrix} \cos\frac{\alpha}{2}\\e_x\sin\frac{\alpha}{2}\\e_y\sin\frac{\alpha}{2}\\e_z\sin\frac{\alpha}{2} \end{bmatrix}\end{split}\]

where \(\alpha\) is the rotation angle and \(\mathbf{e}=\begin{bmatrix}e_x&e_y&e_z\end{bmatrix}^T\) is the unit vector representing the rotation axis. The conjugate quaternion is used to represent the orientation of frame \(B\) relative to frame \(A\):

\[\begin{split}^B_A\mathbf{q}^* = \,^A_B\mathbf{q} = \begin{bmatrix}q_w\\-q_x\\-q_y\\-q_z\end{bmatrix}\end{split}\]

The final orientation quaternion can be easily found through quaternion multiplication:

\[^C_A\mathbf{q} = \,^C_B\mathbf{q} \, ^B_A\mathbf{q}\]

where the quaternion multiplication of any given quaternions \(\mathbf{p}\) and \(\mathbf{q}\) is computed as:

\[\begin{split}\mathbf{pq} = \begin{bmatrix} p_w q_w - p_x q_x - p_y q_y - p_z q_z \\ p_w q_x + p_x q_w + p_y q_z - p_z q_y \\ p_w q_y - p_x q_z + p_y q_w + p_z q_x \\ p_w q_z + p_x q_y - p_y q_x + p_z q_w \end{bmatrix}\end{split}\]

Unit quaternions in Hamiltonian spaces are often used to operate rotations of vectors in a 3D euclidean space. A vector \(^A\mathbf{v}=\begin{bmatrix}v_x & v_y & v_z\end{bmatrix}^T\) expressed with respect to frame \(A\) can be represented with respect to frame \(B\) as:

\[^B\mathbf{v}_q = \, ^B_A\mathbf{q} \, ^A\mathbf{v}_q \, ^B_A\mathbf{q}^*\]

where the vector \(^A\mathbf{v}\) is rewritten as the pure quaternion \(^A\mathbf{v}_q=\begin{bmatrix}0 & v_x & v_y & v_z\end{bmatrix}^T\), so that it can multiply with the versor and its conjugate.

The inverse rotation can be achieved by flipping the versors:

\[^A\mathbf{v}_q = \, ^B_A\mathbf{q}^* \, ^B\mathbf{v}_q \, ^B_A\mathbf{q} = \, ^A_B\mathbf{q} \, ^B\mathbf{v}_q \, ^A_B\mathbf{q}^*\]

Quaternion as Rotation Matrix in SO(3)

These rotations can also be expressed within the three-dimensional euclidean space if we express the rotation as a Direction Cosine Matrix \(\mathbf{R}\in SO(3)\):

\[\begin{split}\mathbf{R}(^B_A\mathbf{q}) = \begin{bmatrix} q_w^2+q_x^2-q_y^2-q_z^2 & 2(q_xq_y - q_wq_z) & 2(q_xq_z + q_wq_y) \\ 2(q_xq_y + q_wq_z) & q_w^2-q_x^2+q_y^2-q_z^2 & 2(q_yq_z - q_wq_x) \\ 2(q_xq_z - q_wq_y) & 2(q_wq_x + q_yq_z) & q_w^2-q_x^2-q_y^2+q_z^2 \end{bmatrix}\end{split}\]

And, because the Direction Cosine Matrix belongs to the Special Orthogonal Group \(SO(3)\), the inverse rotation is simply its transpose:

\[^A\mathbf{v} = \mathbf{R}(^A_B\mathbf{q})\,^B\mathbf{v} = \mathbf{R}^T(^B_A\mathbf{q})\,^B\mathbf{v}\]

Note

For a more detailed explanation of quaternions and their use in spatial rotation see the documentation of the class Quaternions.

Given two quaternions \(\mathbf{p}\) and \(\mathbf{q}\), the cosine of the angle \(\Omega\) subtended by the arc between them is equal to the dot product of the two quaternions.

\[\cos\Omega = \mathbf{p}\cdot\mathbf{q} = p_wq_w + p_xq_x + p_yq_y + p_zq_z\]

It is easy to see that the dot product of any \(\mathbf{q}=\begin{bmatrix}q_w & q_x & q_y & q_z\end{bmatrix}\) and the identity quaternion \(\mathbf{q}_I=\begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\) is equal to the \(q_w\) component:

\[\mathbf{q}\cdot\mathbf{q}_I = q_w\]

Interpolations

The simple Linear intERPolation (LERP) between two quaternions \(\mathbf{p}\) and \(\mathbf{q}\) is obtained as:

\[\overline{\mathbf{r}} = (1-\alpha)\mathbf{p} + \alpha\mathbf{q}\]

where \(\alpha\in [0, 1]\). But this does not keep the unit norm, so we must normalize the resulting interpolation:

\[\widehat{\mathbf{r}} = \frac{\overline{\mathbf{r}}}{\|\overline{\mathbf{r}}\|}\]

The Spherical Linear intERPolation (SLERP) gives a correct evaluation of the weighted average of two points lying on a curve. In the case of quaternions, the points lie on the surface of the 4D sphere (hypersphere).

\[\widehat{\mathbf{r}} = \frac{\sin([1-\alpha]\Omega)}{\sin\Omega}\mathbf{p} + \frac{\sin(\alpha\Omega)}{\sin\Omega}\mathbf{q}\]

Quaternion from Earth-Field Observations#

The local (sensor) frame is labeled as \(L\), and the global (Earth) frame as \(G\). The measured acceleration, \(^L\mathbf{a}\), and the true Earth gravitational acceleration, \(^G\mathbf{g}\), are defined as unit vectors [1]:

\[\begin{split}\begin{array}{rcl} ^L\mathbf{a} &=& \begin{bmatrix}a_x & a_y & a_z\end{bmatrix}^T \\ && \\ ^G\mathbf{g} &=& \begin{bmatrix}0 & 0 & 1\end{bmatrix}^T \end{array}\end{split}\]

The measured local magnetic field, \(^L\mathbf{m}\), and the true geomagnetic field, \(^G\mathbf{h}\), are also unit vectors:

\[\begin{split}\begin{array}{rcl} ^L\mathbf{m} &=& \begin{bmatrix}m_x & m_y & m_z\end{bmatrix}^T \\ && \\ ^G\mathbf{h} &=& \begin{bmatrix}h_x & h_y & h_z\end{bmatrix}^T \end{array}\end{split}\]

The gyroscopes measure the angular velocity, \(^L\mathbf{\omega}\), around the three sensor frame axes:

\[^L\mathbf{\omega} = \begin{bmatrix}\omega_x & \omega_y & \omega_z\end{bmatrix}^T\]

The measured angular velocities are not normalized, unlike the other sensors, and are assumed to be in radians per second.

A straightforward way to formulate the quaternion, \(^L_G\mathbf{q}\), relating the global frame \(G\) to the local frame \(L\), is through the inverse orientation which rotates the measured quantities \(^L\mathbf{a}\) and \(^L\mathbf{m}\) into the reference quantities \(^G\mathbf{g}\) and \(^G\mathbf{h}\):

\[\begin{split}\begin{array}{rcl} \mathbf{R}^T(^L_G\mathbf{q})\,^L\mathbf{a} &=& \,^G\mathbf{g} \\ && \\ \mathbf{R}^T(^L_G\mathbf{q})\,^L\mathbf{m} &=& \,^G\mathbf{h} \end{array}\end{split}\]

In the case of a disagreement between the gravitational and magnetometer readings, the system will not have a solution. To address this problem a modified equation system is built.

First, the global coordinate frame \(G\) is aligned with the magnetic North. The global frame’s X-axis points to the same direction as the local magnetic field (Z-axis remains vertical.)

Warning

This global frame is only fixed in case the local magnetic field does not change its heading. Thus, no magnetic inference should be present.

Let \(^G\Pi_{zx^+}\) be the half-plane which contains all points that lie in the global XZ-plane such that X is non-negative.

The magnetic reading, when rotated into the global frame, must lie on the half-plane \(^G\Pi_{zx^+}\) to guarantee that the heading will be measured with respect to the geomagnetic North.

\[\begin{split}\begin{array}{lc} \mathbf{R}^T(^L_G\mathbf{q})\,^L\mathbf{a} = \,^G\mathbf{g} &\\ & \\ \mathbf{R}^T(^L_G\mathbf{q})\,^L\mathbf{m} \in \,^G\Pi_{zx^+} & \end{array}\end{split}\]

This way we don’t need a-priori knowledge of the direction of Earth’s magnetic field \(^G\mathbf{h}\).

The orientation \(^L_G\mathbf{q}\) is decomposed into two auxiliary quaternions, such that:

\[\begin{split}\begin{array}{rcl} ^L_G\mathbf{q} &=& \mathbf{q}_\mathrm{acc} \mathbf{q}_\mathrm{mag} \\ && \\ \mathbf{R}(^L_G\mathbf{q}) &=& \mathbf{R}(\mathbf{q}_\mathrm{acc}) \mathbf{R}(\mathbf{q}_{\mathrm{mag}}) \end{array}\end{split}\]

The quaternion \(\mathbf{q}_{\mathrm{mag}}\) represents a rotation around the Z-axis to point North only:

\[\mathbf{q}_{\mathrm{mag}} = \begin{bmatrix}q_{w\mathrm{mag}} & 0 & 0 & q_{z\mathrm{mag}}\end{bmatrix}^T\]

Quaternion from Accelerometer#

The auxiliary quaternion \(\mathbf{q}_\mathrm{acc}\) is built as a function of \(^L\mathbf{a}\). The observations of the gravity vector in the two reference frames allows us to find the quaternion that performs the transformation between the two representations.

\[\begin{split}\begin{array}{rcl} \mathbf{R}(^L_G\mathbf{q})\,^G\mathbf{g} &=& \, ^L\mathbf{a} \\ \mathbf{R}(\mathbf{q}_\mathrm{acc})\mathbf{R}(\mathbf{q}_\mathrm{mag}) \begin{bmatrix}0 \\ 0 \\ 1\end{bmatrix} &=& \begin{bmatrix}a_x \\ a_y \\ a_z\end{bmatrix} \end{array}\end{split}\]

The representation of the gravity vector in the global frame \(G\) only has a component on the Z-axis. Thus, any rotation about this axis does not produce any change on it.

\[\begin{split}\mathbf{R}(\mathbf{q}_\mathrm{acc}) \begin{bmatrix}0 \\ 0 \\ 1\end{bmatrix} = \begin{bmatrix}a_x \\ a_y \\ a_z\end{bmatrix}\end{split}\]

The alignment of the gravity vector from global frame into local frame can be achieved by infinite rotations with definite roll and pitch angles and arbitrary yaw. To restrict the solutions to a finite number \(q_{z\mathrm{acc}}=0\) is chosen.

This gives four solutions of \(\mathbf{q}_\mathrm{acc}\). Two are discarded for having a negative norm, and from the remaining two the one with a positive \(q_w\) is taken.

\[\mathbf{q}_\mathrm{acc} = \begin{bmatrix}\sqrt{\frac{a_z+1}{2}} & -\frac{a_y}{\sqrt{2(a_z+1)}} & \frac{a_x}{\sqrt{2(a_z+1)}} & 0\end{bmatrix}^T\]

But it has a singularity at \(a_z=-1\). Therefore, the final solution has to be defined as:

\[\begin{split}\mathbf{q}_\mathrm{acc} = \left\{ \begin{array}{ll} \begin{bmatrix}\sqrt{\frac{a_z+1}{2}} & -\frac{a_y}{\sqrt{2(a_z+1)}} & \frac{a_x}{\sqrt{2(a_z+1)}} & 0\end{bmatrix}^T & \mathrm{if}\; a_z \geq 0 \\ \begin{bmatrix}-\frac{a_y}{\sqrt{2(1-a_z)}} & \sqrt{\frac{1-a_z}{2}} & 0 & \frac{a_x}{\sqrt{2(1-a_z)}}\end{bmatrix}^T & \mathrm{if}\; a_z < 0 \end{array} \right.\end{split}\]

Quaternion from Magnetometer#

The auxiliary quaternion \(\mathbf{q}_\mathrm{mag}\) is derived as a function of \(^L\mathbf{m}\) and \(\mathbf{q}_\mathrm{acc}\).

The quaternion \(\mathbf{q}_\mathrm{acc}\) is used to rotate the magnetic field vector \(^L\mathbf{m}\) into an intermediate frame whoe Z-axis is the same as the global coordinate frame with X- and Y-axes pointing to unknown directions due to the unknown yaw of \(\mathbf{q}_\mathrm{acc}\).

\[\mathbf{R}^T(\mathbf{q}_\mathrm{acc})\,^L\mathbf{m} = \mathbf{l}\]

where \(\mathbf{l}\) is the rotated magnetic field vector. Then, we find the quaternion :math:mathbf{q}_mathrm{mag} that rotates the vector \(\mathbf{l}\) into the vector that lies on \(^G\Pi_{zx^+}\):

\[\begin{split}\mathbf{R}^T(\mathbf{q}_\mathrm{mag}) \begin{bmatrix}l_x \\ l_y \\ l_z\end{bmatrix} = \begin{bmatrix}\sqrt{\Gamma} \\ 0 \\ l_z\end{bmatrix}\end{split}\]

where \(\Gamma=l_x^2+l_y^2\). This quaternion performs a rotation only about the global Z-axis to align the X-axis of the intermediate frame with the X-axis of the global frame without affecting the roll and pitch. Therefore, if there is a magnetic interference, it would affect only the headin angle.

The solution to find this quaternion ensuring the shortest rotation is:

\[\mathbf{q}_\mathrm{mag} = \begin{bmatrix}\frac{\sqrt{\Gamma + l_x\sqrt{\Gamma}}}{\sqrt{2\Gamma}} & 0 & 0 &\frac{l_y}{\sqrt{2}\sqrt{\Gamma+l_x\sqrt{\Gamma}}}\end{bmatrix}^T\]

This quaternion has a singularity too, but here it happens when \(l_x<0\) and \(l_y=0\). Eventually, a simliar solution is found restraining the condition of \(l_x\):

\[\begin{split}\mathbf{q}_\mathrm{mag} = \left\{ \begin{array}{ll} \begin{bmatrix}\frac{\sqrt{\Gamma + l_x\sqrt{\Gamma}}}{\sqrt{2\Gamma}} & 0 & 0 & \frac{l_y}{\sqrt{2}\sqrt{\Gamma+l_x\sqrt{\Gamma}}}\end{bmatrix}^T & \mathrm{if}\; l_x \geq 0 \\ \begin{bmatrix}\frac{l_y}{\sqrt{2}\sqrt{\Gamma-l_x\sqrt{\Gamma}}} & 0 & 0 & \frac{\sqrt{\Gamma - l_x\sqrt{\Gamma}}}{\sqrt{2\Gamma}}\end{bmatrix}^T & \mathrm{if}\; l_x < 0 \end{array} \right.\end{split}\]

The generalized quaternion orientation of the global frame relative to the local frame as the mulitplication of two quaternions \(\mathbf{q}_\mathrm{acc}\) and \(\mathbf{q}_\mathrm{mag}\):

\[^L_G\mathbf{q} = \mathbf{q}_\mathrm{acc} \, \mathbf{q}_\mathrm{mag}\]

The quaternion \(^L_G\mathbf{q}\) does not suffer from the discontinuity problem of the yaw angle given by the switching formulation of \(\mathbf{q}_\mathrm{acc}\) thanks to the multiplication with \(\mathbf{q}_\mathrm{mag}\), which performs the alignment of the intermediate frame into the global frame.

Quaternion-Based Complementary Filter#

A complementary filter fuses attitude estimation in quaternion form from gyroscope data with accelerometer and magnetometer data in the form of a delta quaternion.

If only IMU data is provided (gyroscopes and accelerometers), it corrects only roll and pitch of the attitude. If magnetometer data is also provided (MARG) a second step is added to the algorithm where a magnetic delta quaternion is derived to correct the heading of the previous estimation by aligning the current frame with the magnetic field.

In the Prediction step the measured angular velocity is used to compute a first estimation of the orientation in quaternion form.

Most literature estimating the quaternion derivative from an angular rate measurement is usually calculated for the one representing the orientation of the local frame with respect to the global frame.

\[^G_L\dot{\mathbf{q}}_{\omega, t_k}=\frac{1}{2}\,^G_L\mathbf{q}_{t_{k-1}}\,^L\mathbf{\omega}_{q, t_k}\]

However, Valenti uses the inverse orientation, so the quaternion derivative is computed using the inverse unit quaternion, which is simply the conjugate:

\[^L_G\dot{\mathbf{q}}_{\omega, t_k}= \,^G_L\dot{\mathbf{q}}_{\omega, t_k}^*=-\frac{1}{2}\,^L\mathbf{\omega}_{q, t_k}\,^L_G\mathbf{q}_{t_{k-1}}\]

where \(^L\mathbf{\omega}_{q, t_k}=\begin{bmatrix}0 & \omega_x & \omega_y & \omega_z\end{bmatrix}^T\) is the measured angular velocity, in radians per second, arranged as a pure quaternion at time \(t_k\), and \(^L_G\mathbf{q}_{t_{k-1}}\) is the previous estimate of the orientation.

The orientation of the global frame relative to local frame at time \(t_k\) can be finally computed by numerically integrating the quaternion derivative using the sampling period \(\Delta t = t_k - t_{k-1}\).

\[^L_G\mathbf{q}_{\omega, t_k} = \, ^L_G\mathbf{q}_{t_{k-1}} + \,^L_G\dot{\mathbf{q}}_{\omega, t_k}\Delta t\]

The Correction step is based on a multiplicative approach, where the predicted quaternion \(^L_G\mathbf{q}_\omega\) is corrected by means of two delta quaternions:

\[\begin{equation} ^L_G\mathbf{q} = \, ^L_G\mathbf{q}_\omega \, \Delta\mathbf{q}_\mathrm{acc} \, \Delta\mathbf{q}_\mathrm{mag} \end{equation}\]

The delta quaternions are computed and filtered independently by the high-frequency noise. This correction is divided in two steps: correction of roll and pitch of the predicted quaternion, and then the correction of the yaw angle if readings of the magnetic field are provided.

Accelerometer-Based Correction#

The inverse predicted quaternion \(^G_L\mathbf{q}_\omega\) is used to rotate the normalized body frame gravity vector \(^L\mathbf{a}\), measured by the accelerometer, into the global frame:

\[\mathbf{R}(^G_L\mathbf{q}_\omega)\,^L\mathbf{a} = \,^G\mathbf{g}_p\]

where \(^G\mathbf{g}_p=\begin{bmatrix}g_x & g_y & g_z\end{bmatrix}^T\) is the predicted gravity, which always has a small deviation from the real gravity vector \(^G\mathbf{g}=\begin{bmatrix}0 & 0 & 1\end{bmatrix}^T\). We compute the delta quaternion \(\Delta\mathbf{q}_\mathrm{acc}\) to rotate \(^G\mathbf{g}\) into \(^G\mathbf{g}_p\):

\[\mathbf{R}(\Delta\mathbf{q}_\mathrm{acc}) \, ^G\mathbf{g} = \,^G\mathbf{g}_p\]

Similar to the auxiliary quaternions, we find a closed-form solution:

\[\Delta\mathbf{q}_\mathrm{acc} = \begin{bmatrix}\sqrt{\frac{g_z+1}{2}} & - \frac{g_y}{\sqrt{2(g_z+1)}} & \frac{g_x}{\sqrt{2(g_z+1)}} & 0\end{bmatrix}^T\]

This has a singularity at \(g_z=-1\), but it can be ignored, because the value of \(g_z\) will always be closer to 1.

The delta quaternion is affected by the accelerometer’s high frequency noise, so we scale it down by using an interpolation with the identity quaternion \(\mathbf{q}_I\). As demonstrated above, the dot product with \(\mathbf{q}_I\) is equal to the \(\Delta q_{w\mathrm{acc}}\) component of \(\Delta\mathbf{q}_\mathrm{acc}\).

If \(\Delta q_{w\mathrm{acc}}>\epsilon\), where \(\epsilon\) is a threshold value (default is \(\epsilon=0.9\)), a simple LERP is used:

\[\widehat{\Delta\mathbf{q}}_\mathrm{acc} = \frac{\overline{\Delta\mathbf{q}}_\mathrm{acc}}{\|\overline{\Delta\mathbf{q}}_\mathrm{acc}\|}\]

The predicted quaternion from gyroscopes is multiplied with the delta quaternion to correct the roll and pitch components:

\[^L_G\mathbf{q}' = \, ^L_G\mathbf{q}_\omega \, \widehat{\Delta\mathbf{q}}_\mathrm{acc}\]

The heading angle predicted by the gyroscope integration is not corrected in this step.

Magnetometer-Based Correction#

When a magnetic field measurement is provided the second step corrects the heading component. We use the quaternion inverse of \(^L_G\mathbf{q}'\) to rotate the magnetic field vector \(^L\mathbf{m}\) from the body frame into the world frame.

\[\mathbf{R}(^L_G\mathbf{q}')\,^L\mathbf{m} = \mathbf{l}\]

The delta quaternion \(\Delta\mathbf{q}_\mathrm{mag}\) rotates the vector \(\mathbf{l}\) into the vector that lies on the XZ-semiplane:

\[\begin{split}\mathbf{R}^T(\Delta\mathbf{q}_\mathrm{mag}) \begin{bmatrix}l_x \\ l_y \\ l_z\end{bmatrix} = \begin{bmatrix}\sqrt{l_x^2+l_y^2} \\ 0 \\ l_z\end{bmatrix}\end{split}\]

The solution to the above ensures the shortest rotation:

\[\Delta\mathbf{q}_\mathrm{mag} = \begin{bmatrix} \frac{\sqrt{\Gamma+l_x\sqrt{\Gamma}}}{\sqrt{2\Gamma}} & 0 & 0 & \frac{l_y}{\sqrt{2(\Gamma+l_x\sqrt{\Gamma})}} \end{bmatrix}^T\]

This delta quaternion is affected by the noise of the magnetometer, which is also filtered like the \(\Delta\mathbf{q}_\mathrm{acc}\) switching between LERP and SLERP according to the same criterion.

Because each delta quaternion is affected independently with different noises, two different thresholds can be used: \(\alpha\) for the accelerometer and \(\beta\) for the magnetometer to obtain \(\widehat{\Delta\mathbf{q}}_\mathrm{mag}\).

Finally, the delta quaternion is multiplied with \(^L_G\mathbf{q}'\) to obtain the orientation of the global frame with respect to the local frame:

\[^L_G\mathbf{q} = \,^L_G\mathbf{q}' \, \widehat{\Delta\mathbf{q}}_\mathrm{mag}\]

Adaptive Gain#

When the vehicle moves with high acceleration, the magnitude and direction of the total measured acceleration vector are different from gravity, and the attitude is evaluated using a false reference.

However, the gyroscope readings are not affected by linear acceleration, thus they can still be used to compute a relatively accurate orientation estimation.

A constant gain fusion algorithm cannot overcome the aforementioned problem if the optimal gain has been evaluated for static conditions. An adaptive gain can bw used to tackle this problem.

First a magnitude error \(e_m\) is defined:

\[e_m = \frac{|\|\,^L\tilde{a}\|-g|}{g}\]

where \(\|\,^L\hat{a}\|\) is the norm of the measured local frame acceleration vector before normalization and \(g=9.81 \, \frac{m}{s^2}\).

From the LERP and SLERP definitions, we make the filtering gain \(\alpha\) dependent on the magnitude error \(e_m\) through the gain factor \(f\):

\[\alpha = \overline{\alpha}f(e_m)\]

where \(\overline{\alpha}\) is the constant value that gives the best filtering result in static conditions and \(f(e_m)\) is what is called the gain factor, which is a piecewise continuous function of the magnitude error.

This gain factor is equal to \(1\) when the magnitude of the non-gravitational acceleration is not high enough to overcome the acceleration gravity and the value of the error magnitude does not reach the first threshold \(t_1\). If the nongravitational acceleration rises and the error magnitude exceeds that first threshold, the gain factor decreases linearly with the increase of the magnitude error until reaching zero for error magnitude equal to the second threshold \(t_2\) and over.

\[\begin{split}f(e_m) = \left\{ \begin{array}{ll} 1 & \mathrm{if}\; e_m \leq t_1 \\ \frac{t_2-e_m}{t_1} & \mathrm{if}\; t_1 < e_m < t_2 \\ 0 & \mathrm{if}\; e_m \geq t_2 \end{array} \right.\end{split}\]

Empirically, the threshold values giving the best results are \(0.1\) and \(0.2\).

Filter Initialization#

The values of the current body-frame acceleration and magnetic field vectors are used to produce the quaternion representing the initial orientation of the rigid body in any configuration. Therefore, for the initialization, the filter does not need any assumption and it is performed in one single step.

\[^L_G\mathbf{q}_0 = \mathbf{q}_\mathrm{acc} \mathbf{q}_\mathrm{mag}\]

The bias of a gyroscope’s reading is a slow-varying signal considered as low frequency noise. A low-pass filtering is applied to separate the bias from the actual angular velocity, but only when the sensor is in a steady-state condition to avoid filtering useful information.

If the sensor is in the steady-state condition, the bias is updated, otherwise it is assumed to be equal to the previous step value. The estimated bias is then subtracted from the gyroscope reading obtaining a bias-free angular velocity measurement.

Footnotes#

References

[Valenti2015]

Valenti, R.G.; Dryanovski, I.; Xiao, J. Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. Sensors 2015, 15, 19302-19330. (https://res.mdpi.com/sensors/sensors-15-19302/article_deploy/sensors-15-19302.pdf)

[Valenti2016]

R. G. Valenti, I. Dryanovski and J. Xiao, “A Linear Kalman Filter for MARG Orientation Estimation Using the Algebraic Quaternion Algorithm,” in IEEE Transactions on Instrumentation and Measurement, vol. 65, no. 2, pp. 467-481, 2016. (https://ieeexplore.ieee.org/document/7345567)

class ahrs.filters.aqua.AQUA(acc: ndarray | None = None, mag: ndarray | None = None, gyr: ndarray | None = None, **kw)#

Algebraic Quaternion Algorithm

Parameters:

gyr (numpy.ndarray, default: None) – N-by-3 array with measurements of angular velocity in rad/s
acc (numpy.ndarray, default: None) – N-by-3 array with measurements of acceleration in m/s^2
mag (numpy.ndarray, default: None) – N-by-3 array with measurements of magnetic field in nT
frequency (float, default: 100.0) – Sampling frequency in Herz
Dt (float, default: 0.01) – Sampling step in seconds. Inverse of sampling frequency. Not required if frequency value is given.
alpha (float, default: 0.01) – Gain characterizing cut-off frequency for accelerometer quaternion
beta (float, default: 0.01) – Gain characterizing cut-off frequency for magnetometer quaternion
threshold (float, default: 0.9) – Threshold to discriminate between LERP and SLERP interpolation
adaptive (bool, default: False) – Whether to use an adaptive gain for each sample
q0 (numpy.ndarray, default: None) – Initial orientation, as a versor (normalized quaternion).

Variables:

gyr (numpy.ndarray) – N-by-3 array with N gyroscope samples.
acc (numpy.ndarray) – N-by-3 array with N accelerometer samples.
mag (numpy.ndarray) – N-by-3 array with N magnetometer samples.
frequency (float) – Sampling frequency in Herz
Dt (float) – Sampling step in seconds. Inverse of sampling frequency.
alpha (float) – Gain characterizing cut-off frequency for accelerometer quaternion.
beta (float) – Gain characterizing cut-off frequency for magnetometer quaternion.
threshold (float) – Threshold to discern between LERP and SLERP interpolation.
adaptive (bool) – Flag indicating use of adaptive gain.
q0 (numpy.ndarray) – Initial orientation, as a versor (normalized quaternion).

Omega(x: ndarray) → ndarray#

Omega operator.

Given a vector \(\mathbf{x}\in\mathbb{R}^3\), return a \(4\times 4\) matrix of the form:

\[\begin{split}\boldsymbol\Omega(\mathbf{x}) = \begin{bmatrix} 0 & -\mathbf{x}^T \\ \mathbf{x} & \lfloor\mathbf{x}\rfloor_\times \end{bmatrix} = \begin{bmatrix} 0 & x_1 & x_2 & x_3 \\ -x_1 & 0 & x_3 & -x_2 \\ -x_2 & -x_3 & 0 & x_1 \\ -x_3 & x_2 & -x_1 & 0 \end{bmatrix}\end{split}\]

This operator is a simplification to create a 4-by-4 matrix used for the product between the angular rate and a quaternion, such that:

\[^L_G\dot{\mathbf{q}}_{\omega, t_k} = \frac{1}{2}\boldsymbol\Omega(\,^L\mathbf{\omega}_{q, t_k})\;^L_G\mathbf{q}_{t_{k-1}}\]

Note

The original definition in the article (eq. 39) has an errata missing the multiplication with \(\frac{1}{2}\).

Parameters:: x (numpy.ndarray) – Three-dimensional vector representing the angular rate around the three axes of the local frame.
Returns:: Omega – Omega matrix.
Return type:: numpy.ndarray

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

Quaternion from Earth-Field Observations

Algebraic estimation of a quaternion as a function of an observation of the Earth’s gravitational force and magnetic fields.

It decomposes the quaternion \(\mathbf{q}\) into two auxiliary quaternions \(\mathbf{q}_{\mathrm{acc}}\) and \(\mathbf{q}_{\mathrm{mag}}\), such that:

\[\mathbf{q} = \mathbf{q}_{\mathrm{acc}}\mathbf{q}_{\mathrm{mag}}\]

Parameters:

acc (numpy.ndarray) – Sample of tri-axial Accelerometer in m/s^2
mag (numpy.ndarray, default: None) – Sample of tri-axial Magnetometer in mT

Returns:

q – Estimated quaternion.

Return type:

numpy.ndarray

init_q(acc: ndarray, mag: ndarray | None = None) → ndarray#: Synonym of method estimate.

updateIMU(q: ndarray, gyr: ndarray, acc: ndarray, dt: float | None = None) → ndarray#

Quaternion Estimation with a IMU architecture.

The estimation is made in two steps: a prediction is done with the angular rate (gyroscope) to integrate and estimate the current orientation; then a correction step uses the measured accelerometer to infer the expected gravity vector and use it to correct the predicted quaternion.

If the gyroscope data is invalid, it returns the given a-priori quaternion. Secondly, if the accelerometer data is invalid the predicted quaternion (using gyroscopes) is returned.

Parameters:

q (numpy.ndarray) – A-priori quaternion.
gyr (numpy.ndarray) – Sample of tri-axial Gyroscope in rad/s.
acc (numpy.ndarray) – Sample of tri-axial Accelerometer in m/s^2
dt (float, default: None) – Time step, in seconds, between consecutive Quaternions.

Returns:

q – Estimated quaternion.

Return type:

numpy.ndarray

updateMARG(q: ndarray, gyr: ndarray, acc: ndarray, mag: ndarray, dt: float | None = None) → ndarray#

Quaternion Estimation with a MARG architecture.

The estimation is made in two steps: a prediction is done with the angular rate (gyroscope) to integrate and estimate the current orientation; then a correction step uses the measured accelerometer and magnetic field to infer the expected geodetic values. Its divergence is used to correct the predicted quaternion.

If the gyroscope data is invalid, it returns the given a-priori quaternion. Secondly, if the accelerometer data is invalid the predicted quaternion (using gyroscopes) is returned. Finally, if the magnetometer measurements are invalid, returns a quaternion corrected by the accelerometer only.

Parameters:

q (numpy.ndarray) – A-priori quaternion.
gyr (numpy.ndarray) – Sample of tri-axial Gyroscope in rad/s.
acc (numpy.ndarray) – Sample of tri-axial Accelerometer in m/s^2
mag (numpy.ndarray) – Sample of tri-axial Magnetometer in mT
dt (float, default: None) – Time step, in seconds, between consecutive Quaternions.

Returns:

q – Estimated quaternion.

Return type:

numpy.ndarray

ahrs.filters.aqua.adaptive_gain(gain: float, a_local: ndarray, t1: float = 0.1, t2: float = 0.2, g: float = 9.809030668031474) → float#

Adaptive filter gain factor

The estimated gain \(\alpha\) is dependent on the gain factor \(f(e_m)\):

\[\alpha = a f(e_m)\]

where the magnitude error is defined by the measured acceleration \(\mathbf{a}\) before normalization and the reference gravity \(g\approx 9.809196 \, \frac{m}{s^2}\):

\[e_m = \frac{|\|\mathbf{a}\|-g|}{g}\]

The gain factor is constant and equal to 1 when the magnitude of the nongravitational acceleration is not high enough to overcome gravity.

If nongravitational acceleration rises and \(e_m\) exceeds the first threshold, the gain factor \(f\) decreases linearly with the increase of the magnitude until reaching zero at the second threshold and above it.

Empirically, both thresholds have been defined at 0.1 and 0.2, respectively. They can be, however, changed by setting the values of input parameters t1 and t2.

Parameters:

gain (float) – Gain yielding best results in static conditions.
a_local (numpy.ndarray) – Measured local acceleration vector.
t1 (float, default: 0.1) – First threshold.
t2 (float, default: 0.2) – Second threshold.
g (float, default: 9.809196) – Reference gravitational acceleration in m/s^2. The estimated gravity in Munich, Germany (9.809196) is used as default reference value.

Returns:

alpha – Gain factor.

Return type:

float

Examples

>>> from ahrs.filters.aqua import adaptive_gain
>>> alpha = 0.01    # Best gain in static conditions
>>> acc = np.array([0.0699, 9.7688, -0.2589])   # Measured acceleration. Quasi-static state.
>>> adaptive_gain(alpha, acc)
0.01
>>> acc = np.array([0.8868, 10.8803, -0.4562])  # New measured acceleration. Slightly above first threshold.
>>> adaptive_gain(alpha, acc)
0.008615664547367627
>>> acc = np.array([4.0892, 12.7667, -2.6047])  # New measured acceleration. Above second threshold.
>>> adaptive_gain(alpha, acc)
0.0
>>> adaptive_gain(alpha, acc, t1=0.2, t2=0.5)   # Same acceleration. New thresholds.
0.005390131074499384
>>> adaptive_gain(alpha, acc, t1=0.2, t2=0.5, g=9.82)   # Same acceleration and thresholds. New reference gravity.
0.005466716107480152

ahrs.filters.aqua.slerp_I(q: ndarray, ratio: float, t: float) → ndarray#

Interpolation with identity quaternion

Interpolate a given quaternion with the identity quaternion \(\mathbf{q}_I=\begin{pmatrix}1 & 0 & 0 & 0\end{pmatrix}\) to scale it to closest versor.

The interpolation can be with either LERP (Linear) or SLERP (Spherical Linear) methods, decided by a threshold value \(t\), which lies between 0.0 and 1.0.

\[\begin{split}\mathrm{method} = \left\{ \begin{array}{ll} \mathrm{LERP} & \: q_w > t \\ \mathrm{SLERP} & \: \mathrm{otherwise} \end{array} \right.\end{split}\]

For LERP, a simple equation is implemented:

\[\hat{\mathbf{q}} = (1-\alpha)\mathbf{q}_I + \alpha\Delta \mathbf{q}\]

where \(\alpha\in [0, 1]\) is the gain characterizing the cut-off frequency of the filter. It basically decides how “close” to the given quaternion or to the identity quaternion the interpolation is.

If the scalar part \(q_w\) of the given quaternion is below the threshold \(t\), SLERP is used:

\[\hat{\mathbf{q}} = \frac{\sin([1-\alpha]\Omega)}{\sin\Omega} \mathbf{q}_I + \frac{\sin(\alpha\Omega)}{\sin\Omega} \mathbf{q}\]

where \(\Omega=\arccos(q_w)\) is the subtended arc between the quaternions.

Parameters:

q (numpy.array) – Quaternion to inerpolate with.
ratio (float) – Gain characterizing the cut-off frequency of the filter.
t (float) – Threshold deciding interpolation method. LERP when qw>t, otherwise SLERP.

Returns:

q – Interpolated quaternion

Return type:

numpy.array