QUEST#

QUaternion ESTimator as described by Shuster in [SO81] and [Shu78].

We start to define the goal of finding an orthogonal matrix \(\mathbf{A}\) that minimizes the loss function:

\[L(\mathbf{A}) = \frac{1}{2}\sum_{i=1}^n a_i |\hat{\mathbf{W}}_i - \mathbf{A}\hat{\mathbf{V}}_i|^2\]

where \(a_i\) are a set of non-negative weights such that \(\sum_{i=1}^na_i=1\), \(\hat{\mathbf{V}}_i\) are nonparallel reference vectors, and \(\hat{\mathbf{W}}_i\) are the corresponding observation vectors.

The gain function \(g(\mathbf{A})\) is defined by

\[g(\mathbf{A}) = 1 - L(\mathbf{A}) = \sum_{i=1}^na_i\,\hat{\mathbf{W}}_i^T\mathbf{A}\hat{\mathbf{V}}_i\]

The loss function \(L(\mathbf{A})\) is at its minimum when the gain function \(g(\mathbf{A})\) is at its maximum. The gain function can be reformulated as:

\[g(\mathbf{A}) = \sum_{i=1}^na_i\mathrm{tr}\big(\hat{\mathbf{W}}_i^T\mathbf{A}\hat{\mathbf{V}}_i\big) = \mathrm{tr}(\mathbf{AB}^T)\]

where \(\mathrm{tr}\) is the trace of a matrix, and \(\mathbf{B}\) is the attitude profile matrix:

\[\mathbf{B} = \sum_{i=1}^na_i\hat{\mathbf{W}}_i\hat{\mathbf{V}}_i^T\]

The quaternion \(\bar{\mathbf{q}}\) representing a rotation is defined by Shuster as:

\[\begin{split}\bar{\mathbf{q}} = \begin{bmatrix}\mathbf{Q} \\ q\end{bmatrix} = \begin{bmatrix}\hat{\mathbf{X}}\sin\frac{\theta}{2} \\ \cos\frac{\theta}{2}\end{bmatrix}\end{split}\]

where \(\hat{\mathbf{X}}\) is the axis of rotation, and \(\theta\) is the angle of rotation about \(\hat{\mathbf{X}}\).

Warning

The definition of a quaternion used by Shuster sets the vector part \(\mathbf{Q}\) followed by the scalar part \(q\). This module, however, will return the estimated quaternion with the scalar part first and followed by the vector part: \(\bar{\mathbf{q}} = \begin{bmatrix}q & \mathbf{Q}\end{bmatrix}\)

Because the quaternion works as a versor, it must satisfy:

\[\bar{\mathbf{q}}^T\bar{\mathbf{q}} = |\mathbf{Q}|^2 + q^2 = 1\]

The attitude matrix \(\mathbf{A}\) is related to the quaternion by:

\[\mathbf{A}(\bar{\mathbf{q}}) = (q^2-\mathbf{Q}\cdot\mathbf{Q})\mathbf{I} + 2\mathbf{QQ}^T + 2q\lfloor\mathbf{Q}\rfloor_\times\]

where \(\mathbf{I}\) is the identity matrix, and \(\lfloor\mathbf{Q}\rfloor_\times\) is the antisymmetric matrix of \(\mathbf{Q}\), a.k.a. the skew-symmetric matrix:

\[\begin{split}\lfloor\mathbf{Q}\rfloor_\times = \begin{bmatrix}0 & Q_3 & -Q_2 \\ -Q_3 & 0 & Q_1 \\ Q_2 & -Q_1 & 0\end{bmatrix}\end{split}\]

Now the gain function can be rewritten again, but in terms of quaternions:

\[g(\bar{\mathbf{q}}) = (q^2-\mathbf{Q}\cdot\mathbf{Q})\mathrm{tr}\mathbf{B}^T + 2\mathrm{tr}\big(\mathbf{QQ}^T\mathbf{B}^T\big) + 2q\mathrm{tr}\big(\lfloor\mathbf{Q}\rfloor_\times\mathbf{B}^T\big)\]

A further simplification gives:

\[g(\bar{\mathbf{q}}) = \bar{\mathbf{q}}^T\mathbf{K}\bar{\mathbf{q}}\]

where the \(4\times 4\) matrix \(\mathbf{K}\) is given by:

\[\begin{split}\mathbf{K} = \begin{bmatrix} \mathbf{S} - \sigma\mathbf{I} & \mathbf{Z} \\ \mathbf{Z}^T & \sigma \end{bmatrix}\end{split}\]

using the helper values:

\[\begin{split}\begin{array}{rcl} \sigma &=& \mathrm{tr}\mathbf{B} \\ && \\ \mathbf{S} &=& \mathbf{B} + \mathbf{B}^T \\ && \\ \mathbf{Z} &=& \sum_{i=1}^na_i\big(\hat{\mathbf{W}}_i\times\hat{\mathbf{V}}_i\big) \end{array}\end{split}\]

Note

\(\mathbf{Z}\) can be also defined from \(\lfloor\mathbf{Z}\rfloor_\times = \mathbf{B} - \mathbf{B}^T\)

A new gain function \(g'(\bar{\mathbf{q}})\) with Lagrange multipliers is defined:

\[g'(\bar{\mathbf{q}}) = \bar{\mathbf{q}}^T\mathbf{K}\bar{\mathbf{q}} - \lambda\bar{\mathbf{q}}^T\bar{\mathbf{q}}\]

It is verified that \(\mathbf{K}\bar{\mathbf{q}}=\lambda\bar{\mathbf{q}}\). Thus, \(g(\bar{\mathbf{q}})\) will be maximized if \(\bar{\mathbf{q}}_\mathrm{opt}\) is chosen to be the eigenvector of \(\mathbf{K}\) belonging to the largest eigenvalue of \(\mathbf{K}\):

\[\mathbf{K}\bar{\mathbf{q}}_\mathrm{opt} = \lambda_\mathrm{max}\bar{\mathbf{q}}_\mathrm{opt}\]

which is the desired result. This equation can be rearranged to read, for any eigenvalue \(\lambda\):

\[\lambda = \sigma + \mathbf{Z}\cdot\mathbf{Y}\]

where \(\mathbf{Y}\) is the Gibbs vector, a.k.a. the Rodrigues vector, defined as:

\[\mathbf{Y} = \frac{\mathbf{Q}}{q} = \hat{\mathbf{X}}\tan\frac{\theta}{2}\]

rewriting the quaternion as:

\[\begin{split}\bar{\mathbf{q}} = \frac{1}{\sqrt{1+|\mathbf{Y}|^2}} = \begin{bmatrix}\mathbf{Y}\\ 1 \end{bmatrix}\end{split}\]

\(\mathbf{Y}\) and \(\bar{\mathbf{q}}\) are representations of the optimal attitude solution when \(\lambda\) is equal to \(\lambda_\mathrm{max}\), leading to an equation for the eigenvalues:

\[\lambda = \sigma + \mathbf{Z}^T \frac{1}{(\lambda+\sigma)\mathbf{I}-\mathbf{S}}\mathbf{Z}\]

which is equivalent to the characteristic equation of the eigenvalues of \(\mathbf{K}\)

With the aid of Cayley-Hamilton theorem we can get rid of the Gibbs vector to find a more convenient expression of the characteristic equation:

\[\lambda^4-(a+b)\lambda^2-c\lambda+(ab+c\sigma-d)=0\]

where:

\[\begin{split}\begin{array}{rcl} a &=& \sigma^2-\kappa \\ && \\ b &=& \sigma^2 + \mathbf{Z}^T\mathbf{Z} \\ && \\ c &=& \Delta + \mathbf{Z}^T\mathbf{SZ} \\ && \\ d &=& \mathbf{Z}^T\mathbf{S}^2\mathbf{Z} \\ && \\ \sigma &=& \frac{1}{2}\mathrm{tr}\mathbf{S} \\ && \\ \kappa &=& \mathrm{tr}\big(\mathrm{adj}(\mathbf{S})\big) \\ && \\ \Delta &=& \mathrm{det}(\mathbf{S}) \end{array}\end{split}\]

To find \(\lambda\) we implement the Newton-Raphson method using the sum of the weights \(a_i\) (in the beginning is constrained to be equal to 1) as a starting value.

\[\lambda_{t+1} \gets \lambda_t - \frac{f(\lambda)}{f'(\lambda)} = \lambda_t - \frac{\lambda^4-(a+b)\lambda^2-c\lambda+(ab+c\sigma-d)}{4\lambda^3-2(a+b)\lambda-c}\]

For sensor accuracies better than 1 arc-min (1 degree) the accuracy of a 64-bit word is exhausted after only one iteration.

Finally, the optimal quaternion describing the attitude is found as:

\[\begin{split}\bar{\mathbf{q}}_\mathrm{opt} = \frac{1}{\sqrt{\gamma^2+|\mathbf{X}|^2}} \begin{bmatrix}\mathbf{X}\\ \gamma \end{bmatrix}\end{split}\]

with:

\[\begin{split}\begin{array}{rcl} \mathbf{X} &=& (\alpha\mathbf{I} + \beta\mathbf{S} + \mathbf{S}^2)\mathbf{Z} \\ && \\ \gamma &=& (\lambda + \sigma)\alpha - \Delta \\ && \\ \alpha &=& \lambda^2 - \sigma^2 + \kappa \\ && \\ \beta &=& \lambda - \sigma \end{array}\end{split}\]

This solution can still lead to an indeterminant result if both \(\gamma\) and \(\mathbf{X}\) vanish simultaneously. \(\gamma\) vanishes if and only if the angle of rotation is equal to \(\pi\), even if \(\mathbf{X}\) does not vanish along.

class ahrs.filters.quest.QUEST(acc: ndarray = None, mag: ndarray = None, **kw)#

QUaternion ESTimator

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 (array-like) – Array with two weights. One per sensor measurement.

  • magnetic_dip (float) – Local magnetic inclination angle, in degrees.

  • gravity (float) – Local normal gravity, in m/s^2.

Variables:

  • acc (numpy.ndarray) – N-by-3 array with N accelerometer samples.

  • mag (numpy.ndarray) – N-by-3 array with N magnetometer samples.

  • w (numpy.ndarray) – Weights for each observation.

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 QUEST
>>> orientation = QUEST(acc=acc_data, mag=mag_data)
>>> orientation.Q.shape                 # Estimated attitudes as Quaternions
(1000, 4)
>>> orientation.Q
array([[-0.09867706, -0.33683592, -0.52706394, -0.77395607],
       [-0.10247491, -0.33710813, -0.52117549, -0.77732433],
       [-0.10082646, -0.33658091, -0.52082828, -0.77800078],
       ...,
       [-0.78760687, -0.57789515,  0.2131519,  -0.01669966],
       [-0.78683706, -0.57879487,  0.21313092, -0.02142776],
       [-0.77869223, -0.58616905,  0.22344478, -0.01080235]])
estimate(acc: ndarray, mag: ndarray) → ndarray#

Attitude Estimation.

Parameters:

  • acc (numpy.ndarray) – Sample of tri-axial Accelerometer in m/s^2

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

Returns:

q – Estimated attitude as a quaternion.

Return type:

numpy.ndarray

On this page

This Page