Optimal Linear Estimator of Quaternion#

Considering an attitude determination model from a pair of vector observations:

\[\mathbf{D}^b = \mathbf{CD}^r\]

where \(\mathbf{D}_i^b=\begin{bmatrix}D_{x,i}^b & D_{y,i}^b & D_{z,i}^b\end{bmatrix}^T\) and \(\mathbf{D}_i^r=\begin{bmatrix}D_{x,i}^r & D_{y,i}^r & D_{z,i}^r\end{bmatrix}^T\) are the i-th pair of normalized vector observations from the body frame \(b\) and the reference frame \(r\).

The goal is to find the optimal attitude \(\mathbf{C}\in\mathbb{R}^{3\times 3}\) relating both vectors. The famous Wahba’s problem can help us to find \(\mathbf{C}\) from a set of observations and a least-squares method of the form:

\[L(\mathbf{C}) = \sum_{i=1}^n a_i \|\mathbf{D}_i^b - \mathbf{CD}_i^r \|^2\]

being \(a_i\) the weight of the i-th sensor output. The goal of OLEQ is to find this optimal attitude, but in the form of a quaternion [Zhou2018].

First, notice that the attitude matrix is related to quaternion \(\mathbf{q}=\begin{bmatrix}q_w & q_x & q_y & q_z\end{bmatrix}^T\) via:

\[\mathbf{C} = \begin{bmatrix}\mathbf{P}_1\mathbf{q} & \mathbf{P}_2\mathbf{q} & \mathbf{P}_3\mathbf{q}\end{bmatrix}\]

where the decomposition matrices are:

\[\begin{split}\begin{array}{rcl} \mathbf{P}_1 &=& \begin{bmatrix}q_w & q_x & -q_y & -q_z \\ -q_z & q_y & q_x & -q_w \\ q_y & q_z & q_w & q_x \end{bmatrix} \\ \mathbf{P}_2 &=& \begin{bmatrix}q_z & q_y & q_x & q_w \\ q_w & -q_x & q_y & -q_z \\ -q_x & -q_w & q_z & q_y \end{bmatrix} \\ \mathbf{P}_3 &=& \begin{bmatrix}-q_y & q_z & -q_w & q_x \\ q_x & q_w & q_z & q_y \\ q_w & -q_x & -q_y & q_z \end{bmatrix} \end{array}\end{split}\]

It is accepted that \(\mathbf{P}_1^T=\mathbf{P}_1^\dagger\), \(\mathbf{P}_2^T=\mathbf{P}_2^\dagger\), and \(\mathbf{P}_3^T=\mathbf{P}_3^\dagger\), where the notation \(^\dagger\) stands for the Moore-Penrose pseudo- inverse. So, the reference and observation vectors can be related to the quaternion with a \(4\times 4\) matrix of the form:

\[\begin{split}\begin{array}{rcl} \mathbf{D}^b &=& \mathbf{K}(\mathbf{q}) \mathbf{q} \\ \mathbf{D}^b &=& \big(D_x^r\mathbf{P}_1 + D_y^r\mathbf{P}_2 + D_z^r\mathbf{P}_3\big) \mathbf{q} \end{array}\end{split}\]

Knowing that \(\mathbf{K}^T(\mathbf{q})=\mathbf{K}^\dagger(\mathbf{q})\), the expression can be expanded to:

\[\begin{split}\begin{array}{rcl} \mathbf{K}^T(\mathbf{q})\mathbf{D}^b &=& D_x^r\mathbf{P}_1^T\mathbf{D}^b + D_y^r\mathbf{P}_2^T\mathbf{D}^b + D_z^r\mathbf{P}_3^T\mathbf{D}^b \\ \mathbf{Wq} &=& D_x^r\mathbf{M}_1\mathbf{q} + D_y^r\mathbf{M}_2\mathbf{q} + D_z^r\mathbf{M}_3\mathbf{q} \end{array}\end{split}\]

where \(\mathbf{W}\) is built with:

\[\begin{split}\begin{array}{rcl} \mathbf{W} &=& D_x^r\mathbf{M}_1 + D_y^r\mathbf{M}_2 + D_z^r\mathbf{M}_3 \\ && \\ \mathbf{M}_1 &=& \begin{bmatrix} D_x^b & 0 & D_z^b & -D_y^b \\ 0 & D_x^b & D_y^b & D_z^b \\ D_z^b & D_y^b & -D_x^b & 0 \\ -D_y^b & D_z^b & 0 & -D_x^b \end{bmatrix} \\ \mathbf{M}_2 &=& \begin{bmatrix} D_y^b & -D_z^b & 0 & D_x^b \\ -D_z^b & -D_y^b & D_x^b & 0 \\ 0 & D_x^b & D_y^b & D_z^b \\ D_x^b & 0 & D_z^b & -D_y^b \end{bmatrix} \\ \mathbf{M}_3 &=& \begin{bmatrix} D_z^b & D_y^b & -D_x^b & 0 \\ D_y^b & -D_z^b & 0 & D_x^b \\ -D_x^b & 0 & -D_z^b & D_y^b \\ 0 & D_x^b & D_y^b & D_z^b \end{bmatrix} \end{array}\end{split}\]

Now the attitude estimation is shifted to \(\mathbf{Wq}=\mathbf{q}\). If treated as an iterative dynamical system, the quatenion at the n-th itreation is calculated as:

\[\mathbf{q}(n) = \mathbf{Wq}(n-1)\]

It is possible to list all rotation equations as:

\[\begin{split}\begin{bmatrix} \sqrt{a_1}\mathbf{I}_4 \\ \vdots \\ \sqrt{a_n}\mathbf{I}_4 \end{bmatrix} \mathbf{q} = \begin{bmatrix} \sqrt{a_1}\mathbf{W}_1 \\ \vdots \\ \sqrt{a_n}\mathbf{W}_n \end{bmatrix} \mathbf{q}\end{split}\]

Leading to a pre-multiplication of the form:

\[\mathbf{q} = \Big(\sum_{i=1}^na_i\mathbf{W}_i\Big)\mathbf{q}\]

A stable and continuous solution to each equation is done by pre-multiplying \(\frac{1}{2}(\mathbf{W}_i+\mathbf{I}_4)\).

\[\begin{split}\begin{bmatrix} \sqrt{a_1}\mathbf{I}_4 \\ \vdots \\ \sqrt{a_n}\mathbf{I}_4 \end{bmatrix} \mathbf{q} = \begin{bmatrix} \frac{1}{2}\sqrt{a_1}(\mathbf{W}_1+\mathbf{I}_4) \\ \vdots \\ \frac{1}{2}\sqrt{a_n}(\mathbf{W}_n+\mathbf{I}_4) \end{bmatrix} \mathbf{q}\end{split}\]

Based on Brouwer’s fixed-point theorem, it is possible to recursively obtain the normalized optimal quaternion by rotating a randomly given initial quaternion, \(\mathbf{q}_\mathrm{rand}\), over and over again indefinitely.

\[\mathbf{q} = \frac{\mathbf{W} + \mathbf{I}}{2} \mathbf{q}_\mathrm{rand}\]

This equals the least-square of the set of pre-computed single rotated quaternions.

References

[Zhou2018]

Zhou, Z.; Wu, J.; Wang, J.; Fourati, H. Optimal, Recursive and Sub-Optimal Linear Solutions to Attitude Determination from Vector Observations for GNSS/Accelerometer/Magnetometer Orientation Measurement. Remote Sens. 2018, 10, 377. (https://www.mdpi.com/2072-4292/10/3/377)

class ahrs.filters.oleq.OLEQ(acc: ndarray | None = None, mag: ndarray | None = None, weights: ndarray | None = None, magnetic_ref: ndarray | None = None, frame: str = 'NED')#

Optimal Linear Estimator of Quaternion

Parameters:

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

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

  • weights (numpy.ndarray, default: [1., 1.]) – Array with weights for each sensor measurement. The first item weights the observed acceleration, while second item weights the observed magnetic field.

  • magnetic_ref (float or numpy.ndarray) – Local magnetic reference.

  • frame (str, default: 'NED') – Local tangent plane coordinate frame. Valid options are right-handed 'NED' for North-East-Down and 'ENU' for East-North-Up.

Raises:

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

Examples

>>> acc_data.shape, mag_data.shape      # NumPy arrays with sensor data
((1000, 3), (1000, 3))
>>> from ahrs.filters import OLEQ
>>> orientation = OLEQ(acc=acc_data, mag=mag_data)
>>> orientation.Q.shape                 # Estimated attitude
(1000, 4)
WW(Db: ndarray, Dr: ndarray) → ndarray#

W Matrix

\[\mathbf{W} = D_x^r\mathbf{M}_1 + D_y^r\mathbf{M}_2 + D_z^r\mathbf{M}_3\]
Parameters:

  • Db (numpy.ndarray) – Normalized tri-axial observations vector.

  • Dr (numpy.ndarray) – Normalized tri-axial reference vector.

Returns:

W_matrix – W Matrix.

Return type:

numpy.ndarray

estimate(acc: ndarray, mag: ndarray) → 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