Quaternion estimator algorithm: Difference between revisions
fix typo: lambda is the (scalar valued) maximum value of the loss, not the (matrix valued) argument of the maximum |
Rename vector Q to v for consistency (to have all vectors denoted with lowercase letters, and all matrices denoted with uppercase) |
||
Line 19: | Line 19: | ||
where <math>\mathbf{B} = \textstyle \sum_i a_i \mathbf{w}_i \mathbf{v}_i^\top</math> is known as the attitude profile matrix. |
where <math>\mathbf{B} = \textstyle \sum_i a_i \mathbf{w}_i \mathbf{v}_i^\top</math> is known as the attitude profile matrix. |
||
In order to reduce the number of variables, the problem can be reformulated by parametrising the rotation as a unit [[quaternion]] <math>\mathbf{q} = \left( |
In order to reduce the number of variables, the problem can be reformulated by parametrising the rotation as a unit [[quaternion]] <math>\mathbf{q} = \left( v_1, v_2, v_3, q\right)</math> with vector part <math>\mathbf{v} = \left( v_1, v_2, v_3 \right)</math> and scalar part <math>q</math>, representing the rotation of angle <math>\theta = 2 \cos^{-1} q</math> around an axis whose direction is described by the vector <math>\textstyle \frac{1}{\sin \frac{\theta}{2}} \mathbf{v}</math>, subject to the unity constraint <math>\mathbf{q}^\top \mathbf{q} = 1</math>. It is now possible to express <math>\mathbf{A}</math> in terms of the quaternion parametrisation as |
||
:<math> \mathbf{A} = \left( q^2 - \mathbf{ |
:<math> \mathbf{A} = \left( q^2 - \mathbf{v} \cdot \mathbf{v} \right) \mathbf{I} + 2 \mathbf{v}\mathbf{v}^\top + 2 q \mathbf{V}_\times </math> |
||
where <math>\mathbf{ |
where <math>\mathbf{V}_\times</math> is the [[skew-symmetric matrix]] |
||
:<math> \mathbf{ |
:<math> \mathbf{V}_\times = |
||
\begin{pmatrix} |
\begin{pmatrix} |
||
0 & |
0 & v_3 & -v_2 \\ |
||
- |
-v_3 & 0 & v_1 \\ |
||
v_2 & -v_1 & 0 \\ |
|||
\end{pmatrix} |
\end{pmatrix} |
||
</math>. |
</math>. |
||
Line 81: | Line 81: | ||
</math> |
</math> |
||
where <math>\mathbf{y} = \textstyle \frac{1}{q} \mathbf{ |
where <math>\mathbf{y} = \textstyle \frac{1}{q} \mathbf{v}</math> is the [[Rodrigues' rotation formula|Rodrigues vector]]. Substituting <math>\mathbf{y}</math> in the second equation with the first, it is possible to derive an expression of the [[characteristic polynomial|characteristic equation]] |
||
:<math> \lambda = \sigma + \mathbf{z}^\top \left( (\lambda + \sigma) \mathbf{I} - \mathbf{S} \right)^{-1} \mathbf{z} </math>. |
:<math> \lambda = \sigma + \mathbf{z}^\top \left( (\lambda + \sigma) \mathbf{I} - \mathbf{S} \right)^{-1} \mathbf{z} </math>. |
Revision as of 21:34, 28 February 2022
The quaternion estimator algorithm (QUEST) is an algorithm designed to solve Wahba's problem, that consists of finding a rotation matrix between two coordinate systems from two sets of observations sampled in each system respectively. The key idea behind the algorithm is to find an expression of the loss function for the Wahba's problem as a quadratic form, using the Cayley–Hamilton theorem and the Newton–Raphson method to efficiently solve the eigenvalue problem and construct a numerically stable representation of the solution.
The algorithm was introduced by Malcolm D. Shuster in 1981, while working at Computer Sciences Corporation.[1] While being in principle less robust than other methods such as Davenport's q method or singular value decomposition, the algorithm is significantly faster and reliable in practical applications,[2][3] and it is used for attitude determination problem in fields such as robotics and avionics.[4][5][6]
Formulation of the problem
Wahba's problem consists of finding a rotation matrix that minimises the loss function
where are the vector observations in the reference frame, are the vector observations in the body frame, is a rotation matrix between the two frames, and are a set of weights such that . It is possible to rewrite this as a maximisation problem of a gain function
defined in such a way that the loss attains a minimum when is maximised. The gain can in turn be rewritten as
where is known as the attitude profile matrix.
In order to reduce the number of variables, the problem can be reformulated by parametrising the rotation as a unit quaternion with vector part and scalar part , representing the rotation of angle around an axis whose direction is described by the vector , subject to the unity constraint . It is now possible to express in terms of the quaternion parametrisation as
where is the skew-symmetric matrix
- .
Substituting with the quaternion representation and simplifying the resulting expression, the gain function can be written as a quadratic form in
where the matrix
is defined from the quantities
This quadratic form can be optimised under the unity constraint by adding a Lagrange multiplier , obtaining an unconstrained gain function
that attains a maximum when
- .
This implies that the optimal rotation is parametrised by the quaternion that is the eigenvector associated to the largest eigenvalue of .[1][2]
Solution of the characteristic equation
The optimal quaternion can be determined by solving the characteristic equation of and constructing the eigenvector for the largest eigenvalue. From the definition of , it is possible to rewrite
as a system of two equations
where is the Rodrigues vector. Substituting in the second equation with the first, it is possible to derive an expression of the characteristic equation
- .
Since , it follows that and therefore for an optimal solution (when the loss is small). This permits to construct the optimal quaternion by replacing in the Rodrigues vector
- .
The vector is however singular for . An alternative expression of the solution that does not involve the Rodrigues vector can be constructed using the Cayley–Hamilton theorem. The characteristic equation of a matrix is
where
The Cayley–Hamilton theorem states that any square matrix over a commutative ring satisfies its own characteristic equation, therefore
allowing to write
where
and for this provides a new construction of the optimal vector
that gives the quaternion representation of the optimal rotation as
where
- .
The value of can be determined as a numerical solution of the characteristic equation. Replacing inside the previously obtained characteristic equation
- .
gives
where
whose root can be efficiently approximated with the Newton–Raphson method, taking 1 as initial guess of the solution in order to converge to the highest eigenvalue (using the fact, shown above, that when the quaternion is close to the optimal solution).[1][2]
See also
References
Sources
- Crassidis, John L; Markley, F Landis; Cheng, Yang (2007). "Survey of nonlinear attitude estimation methods". Journal of guidance, control, and dynamics. 30 (1): 12–28.
- Markley, F Landis; Mortari, Daniele (2000). "Quaternion attitude estimation using vector observations". The Journal of the Astronautical Sciences. 48 (2). Springer: 359–380.
- Psiaki, Mark L (2000). "Attitude-determination filtering via extended quaternion estimation". Journal of Guidance, Control, and Dynamics. 23 (2): 206–214.
- Shuster, M.D.; Oh, S.D. (1981). "Three-axis attitude determination from vector observations". Journal of guidance and Control. 4 (1): 70–77.
- Wu, Jin; Zhou, Zebo; Gao, Bin; Li, Rui; Cheng, Yuhua; Fourati, Hassen (2017). "Fast linear quaternion attitude estimator using vector observations". IEEE Transactions on Automation Science and Engineering. 15 (1). IEEE: 307–319.
- Yun, Xiaoping; Bachmann, Eric R; McGhee, Robert B (2008). "A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements". IEEE Transactions on instrumentation and measurement. 57 (3). IEEE: 638–650.
External links
- "QUEST — AHRS documentation".
- University of Colorado Boulder. "QUEST". Kinematics: Describing the Motions of Spacecraft. Coursera.