QUEST¶
QUaternion ESTimator as described by Shuster in [Shuster1981] and [Shuster1978].
We start to define the goal of finding an orthogonal matrix \(\mathbf{A}\) that minimizes the loss function:
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
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:
where \(\mathrm{tr}\) is the trace of a matrix, and \(\mathbf{B}\) is the attitude profile matrix:
The quaternion \(\bar{\mathbf{q}}\) representing a rotation is defined by Shuster as:
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:
The attitude matrix \(\mathbf{A}\) is related to the quaternion by:
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:
Now the gain function can be rewritten again, but in terms of quaternions:
A further simplification gives:
where the \(4\times 4\) matrix \(\mathbf{K}\) is given by:
using the helper values:
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:
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}\):
which is the desired result. This equation can be rearranged to read, for any eigenvalue \(\lambda\):
where \(\mathbf{Y}\) is the Gibbs vector, a.k.a. the Rodrigues vector, defined as:
rewriting the quaternion as:
\(\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:
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:
where:
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.
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:
with:
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.
References
[Shuster1981] | Shuster, M.D. and Oh, S.D. “Three-Axis Attitude Determination from Vector Observations,” Journal of Guidance and Control, Vol.4, No.1, Jan.-Feb. 1981, pp. 70-77. |
[Shuster1978] | Shuster, Malcom D. Approximate Algorithms for Fast Optimal Attitude Computation, AIAA Guidance and Control Conference. August 1978. (http://www.malcolmdshuster.com/Pub_1978b_C_PaloAlto_scan.pdf) |
-
class
ahrs.filters.quest.
QUEST
(acc: numpy.ndarray = None, mag: numpy.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 arraysacc
andmag
are not equal.-
estimate
(acc: numpy.ndarray = None, mag: numpy.ndarray = None) → numpy.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