Fourati’s nonlinear attitude estimation#

Attitude estimation algorithm as proposed by [Fourati], whose approach combines a quaternion-based nonlinear filter with the Levenberg Marquardt Algorithm (LMA.)

The estimation algorithm has a complementary structure that exploits measurements from an accelerometer, a magnetometer and a gyroscope, combined in a strap-down system, based on the time integral of the angular velocity, using the Earth’s magnetic field and gravity vector to compensate the attitude predicted by the gyroscope.

The rigid body attitude in space is determined when the body’s orientation frame \((X_B, Y_B, Z_B)\) is specified with respect to the navigation frame \((Y_N, Y_N, Z_N)\), where the navigation frame follows the NED convention (North-East-Down.)

The unit quaternion, \(\mathbf{q}\), is defined as a scalar-vector pair of the form:

\[\mathbf{q} = \begin{pmatrix}s & \mathbf{v}\end{pmatrix}^T\]

where \(s\) is the scalar part and \(\mathbf{v}=\begin{pmatrix}v_x & v_y & v_z\end{pmatrix}^T\) is the vector part of the quaternion.

Note

Most literature, and this package’s documentation, use the notation \(\mathbf{q}=\begin{pmatrix}q_w & q_x & q_y & q_z\end{pmatrix}\) to define a quaternion, but this algorithm uses a different one, and it will preserved to keep the coherence with the original document.

The sensor configuration consists of a three-axis gyroscope, a three-axis accelerometer, a three-axis magnetometer. Their outputs can be modelled, respectively, as:

\[\begin{split}\begin{array}{rcl} \omega_G =& \begin{bmatrix}\omega_{GX} & \omega_{GY} & \omega_{GZ}\end{bmatrix}^T &= \omega + b + \delta_G \\&&\\ \mathbf{f} =& \begin{bmatrix}f_x & f_y & f_z\end{bmatrix}^T &= M_N^B(\mathbf{q}) (g+a) + \delta_f \\&&\\ \mathbf{h} =& \begin{bmatrix}h_x & h_y & h_z\end{bmatrix}^T &= M_N^B(\mathbf{q}) m + \delta_h \end{array}\end{split}\]

where \(b\in\mathbb{R}^3\) is the unknown gyro-bias vector and \(\delta_G\), \(\delta_f\) and \(\delta_h\in\mathbb{R}^3\) are assumed white Gaussian noises. \(\omega\) is the real angular velocity, \(g\) is the gravity vector, \(a\) denotes the Dynamic Body Acceleration (DBA), \(m\) describes the direction of the Earth’s magnetic field on the local position, and \(M_N^B(\mathbf{q})\) is the orthogonal matrix describing the attitude of the body frame.

\[\begin{split}\mathbf{M}_N^B(\mathbf{q}) = \begin{bmatrix} 2(s^2 + v_x^2) - 1 & 2(v_xv_y + sv_z) & 2(v_xv_z - sv_y) \\ 2(v_xv_y - sv_z) & 2(s^2 + v_y^2) - 1 & 2(sv_x + v_yv_z) \\ 2(sv_y + v_xv_z) & 2(v_yv_z - sv_x) & 2(s^2 + v_z^2) - 1 \end{bmatrix}\end{split}\]

The kinematic differential equation, in terms of the unit quaternion, that describes the relationship between the rigid body attitude variation and the angular velocity in the body frame is represented by:

\[\begin{split}\begin{array}{rcl} \dot{\mathbf{q}} &=& \frac{1}{2}\mathbf{q}\omega_\mathbf{q} \\ \begin{bmatrix}\dot{s}\\ \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z\end{bmatrix} &=& \frac{1}{2}\begin{bmatrix}-\mathbf{v}^T \\ \mathbf{I}_3s+\lfloor\mathbf{v}\rfloor_\times\end{bmatrix} \begin{bmatrix}\omega_x \\ \omega_y \\ \omega_z\end{bmatrix} \end{array}\end{split}\]

where \(\omega_\mathbf{q}=\begin{bmatrix}0 & \omega^T\end{bmatrix}^T\) is the equivalent to the angular velocity \(\omega\in\mathbb{R}^3\) of the rigid body measured in \(B\) and relative to \(N\), \(\mathbf{I}_3\) is the \(3\times 3\) identity matrix, and \(\lfloor\mathbf{v}\rfloor_\times\) is the Skew symmetric matrix of the vector \(\mathbf{v}\).

\[\begin{split}\lfloor\mathbf{v}\rfloor_\times = \begin{bmatrix}0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{bmatrix}\end{split}\]

Note

Any vector \(\mathbf{x}=\begin{bmatrix}x_1 & x_2 & x_3\end{bmatrix}^T\in\mathbb{R}^3\) that multiplies with a quaternion must be considered a pure quaternion, \(\mathbf{x_q}=\begin{bmatrix}0 & x_1 & x_2 & x_3\end{bmatrix}^T\in\mathbb{R}^4\), so that they operate with a Hamilton product.

To achieve an optimal attitude estimation, a nonlinear system is developed, whose output is the stack of the accelerometer and magnetometer measurements:

\[\mathbf{y} = \begin{bmatrix}f_x & f_y & f_z & h_x & h_y & h_z\end{bmatrix}^T\]

The World Magnetic Model considers a magnetic vector \(\mathbf{m}=\begin{bmatrix}m_x & m_y & m_z \end{bmatrix}\in\mathbb{R}^3\) at any location on Earth to describe the geomagnetic field. For practical purposes, the vector is simplified to \(\mathbf{m}=\begin{bmatrix}m\cos\theta & 0 & m\sin\theta\end{bmatrix}\), with a dip angle \(\theta\) and a magnetic intensity \(m\), which varies between 23000 and 67000 nT, depending on the region on Earth. This simplified vector discards the Easterly magnetic field (\(m_y\)), although for an accurate reference, it is preferred to use it.

Similar to \(\mathbf{y}\), the estimated values \(\hat{\mathbf{y}}\) are given by:

\[\hat{\mathbf{y}} = \begin{bmatrix}\hat{f}_x & \hat{f}_y & \hat{f}_z & \hat{h}_x & \hat{h}_y & \hat{h}_z\end{bmatrix}^T\]

whose components are calculated as:

\[\begin{split}\begin{array}{rcl} \hat{\mathbf{f}} &=& \begin{bmatrix}\hat{f}_x & \hat{f}_y & \hat{f}_z \end{bmatrix}^T = \hat{\mathbf{q}}^{-1}\mathbf{g_q}\hat{\mathbf{q}} \\ && \\ \hat{\mathbf{h}} &=& \begin{bmatrix}\hat{h}_x & \hat{h}_y & \hat{h}_z \end{bmatrix}^T = \hat{\mathbf{q}}^{-1}\mathbf{m_q}\hat{\mathbf{q}} \end{array}\end{split}\]

where \(\mathbf{g_q}=\begin{bmatrix}0 & 0 & 0 & 9.8\end{bmatrix}^T\) is the reference gravity vector as a pure quaternion, and \(\mathbf{m_q}=\begin{bmatrix}0 & m\cos\theta & 0 & m\sin\theta\end{bmatrix}^T\) is the local reference geomagnetic field also represented as a pure quaternion.

The modeling error, \(\delta(\hat{\mathbf{q}})=\mathbf{y}-\hat{\mathbf{y}}\), represents the difference between the real measurements \(\mathbf{y}\) and the estimated values \(\hat{\mathbf{y}}\).

The nonlinear filter of this model takes the form:

\[\begin{split}\dot{\mathbf{q}} = \begin{bmatrix}\dot{s}\\ \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z\end{bmatrix} = \frac{1}{2}\hat{\mathbf{q}}\omega_\mathbf{q} \begin{bmatrix}1 \\ \mathbf{K}\end{bmatrix}\end{split}\]

where \(\hat{\mathbf{q}}=\begin{bmatrix}\hat{s}& \hat{v}_x & \hat{v}_y & \hat{v}_z\end{bmatrix}^T\in\mathbb{R}^4\) is the estimated state, and \(\mathbf{K}\in\mathbb{R}^{3\times 6}\) is the observer gain.

This gain \(\mathbf{K}\) is used to correct the modeling error \(\delta(\hat{\mathbf{q}})\), which can be done if we locate the minimum of the squared error function \(\xi(\hat{\mathbf{q}})=\delta(\hat{\mathbf{q}})^T\delta(\hat{\mathbf{q}})\).

For this attitude estimator the Levenberg-Marquardt Algorithm (LMA) is used to minimize the nonlinear function \(\xi(\hat{\mathbf{q}})\). So, the unique minimum can be computed with:

\[\begin{split}\begin{array}{rcl} \eta(\hat{\mathbf{q}}) &=& \mathbf{K}\delta(\hat{\mathbf{q}}) \\ &=& k[\mathbf{X}^T\mathbf{X} + \lambda\mathbf{I}_3]^{-1}\mathbf{X}^T\delta(\hat{\mathbf{q}}) \end{array}\end{split}\]

where the tiny value \(\lambda\) guarantees the inversion of the matrix, the gain factor \(k\) tunes the balance between measurement noise supression and the filter’s response time, and \(\mathbf{X}\in\mathbb{R}^{6\times 3}\) is the Jacobian matrix:

\[\begin{split}\begin{array}{rcl} \mathbf{X} &=& -2\begin{bmatrix}\lfloor\hat{\mathbf{f}}\rfloor_\times & \lfloor\hat{\mathbf{h}}\rfloor_\times\end{bmatrix} \\ &=& -2\begin{bmatrix} 0 & -\hat{f}_z & \hat{f}_y & 0 & -\hat{h}_z & \hat{h}_y \\ \hat{f}_z & 0 & -\hat{f}_x & \hat{h}_z & 0 & -\hat{h}_x \\ -\hat{f}_y & \hat{f}_x & 0 & -\hat{h}_y & \hat{h}_x & 0 \end{bmatrix} \end{array}\end{split}\]

The resulting structure of the nonlinear filter is complementary: it blends the low-frequency region (low bandwidth) of the accelerometer and magnetometer data, where the attitude is typically more accurate, with the high-frequency region (high bandwidth) of the gyroscope data, where the integration of the angular velocity yields better attitude estimates.

By filtering the high-frequency components of the signals from the accelerometer (DBA) and the low-frequency components of the gyroscope signal (slow-moving drift), the nonlinear filter produces an accurate estimate of the attitude.

The correction term, \(\Delta\in\mathbb{R}^{4\times 7}\), is computed using the gain \(K\) such as:

\[\begin{split}\Delta = \begin{bmatrix}1 & \mathbf{0} \\ \mathbf{0} & \mathbf{K}\end{bmatrix} \begin{bmatrix}1 \\ \delta(\hat{\mathbf{q}})\end{bmatrix}\end{split}\]

It is used to correct the estimated angular velocity, \(\dot{\hat{\mathbf{q}}}\), as:

\[\dot{\hat{\mathbf{q}}} = \big(\frac{1}{2}\hat{\mathbf{q}}\omega_\mathbf{q}\big)\Delta\]

With the corrected angular velocity, we integrate it using the sampling step \(\Delta_t\) and add it to the previous quaternion \(\mathbf{q}_{t-1}\) to obtain the new attitude \(\mathbf{q}_t\):

\[\mathbf{q}_t = \mathbf{q}_{t-1} + \dot{\hat{\mathbf{q}}}\Delta_t\]

Warning

Do not confuse the correction term \(\Delta\) with the sampling step \(\Delta_t\), which is actually the inverse of the sampling frequency \(f=\frac{1}{\Delta_t}\).

References

[Fourati]

Hassen Fourati, Noureddine Manamanni, Lissan Afilal, Yves Handrich. A Nonlinear Filtering Approach for the Attitude and Dynamic Body Acceleration Estimation Based on Inertial and Magnetic Sensors: Bio-Logging Application. IEEE Sensors Journal, Institute of Electrical and Electronics Engineers, 2011, 11 (1), pp. 233-244. 10.1109/JSEN.2010.2053353. (https://hal.archives-ouvertes.fr/hal-00624142/file/Papier_IEEE_Sensors_Journal.pdf)

class ahrs.filters.fourati.Fourati(gyr: ndarray | None = None, acc: ndarray | None = None, mag: ndarray | None = None, **kwargs)#

Fourati’s attitude estimation

Parameters:

  • acc (numpy.ndarray, default: None) – N-by-3 array with measurements of acceleration in in m/s^2

  • gyr (numpy.ndarray, default: None) – N-by-3 array with measurements of angular velocity in rad/s

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

  • 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.

  • gain (float, default: 0.1) – Filter gain factor.

  • q0 (numpy.ndarray, default: None) – Initial orientation, as a versor (normalized quaternion).

  • magnetic_dip (float, array-like, default: None) – Magnetic Inclination angle, in degrees, or as a local geomagnetic vector in a pure quaternion of the form [0, mx, my, mz], where mx, my, and mz are the three-dimensional components of the local geomagnetic vector.

Raises:

ValueError – When dimension of input array(s) acc, gyr, or mag are not equal.

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

Quaternion Estimation with a MARG architecture.

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