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]
A*
l\left(A\right)=
| 1 | |
| 2 |
| n | |
| \sum | |
| i=1 |
ai\left\|wi-Avi\right\|2
where
wi
vi
A
ai
style\sumiai=1
g
g\left(A\right)=1-l\left(A\right)=\sumiai
| \top | |
| w | |
| i |
Avi
defined in such a way that the loss
l
g
g
g\left(A\right)=\operatorname{tr}\left(AB\top\right)
where
B=style\sumiaiwi
| \top | |
| v | |
| i |
q=\left(v1,v2,v3,q\right)
v=\left(v1,v2,v3\right)
q
\theta=2\cos-1q
| style | 1 | ||||
|
v
q\topq=1
A
A=\left(q2-v ⋅ v\right)I+2vv\top+2qV x
where
V x
V x = \begin{pmatrix} 0&v3&-v2\\ -v3&0&v1\\ v2&-v1&0\\ \end{pmatrix}
Substituting
A
q
g(q)=q\topKq
where the
4 x 4
K= \begin{pmatrix} S-\sigmaI&z\\ z\top&\sigma \end{pmatrix}
is defined from the quantities
\begin{align} S&=B+B\top\\ z&=\sumiai\left(wi x vi\right)\\ \sigma&=\operatorname{tr}B. \end{align}
-λq\topq
\hat{g}\left(q\right)=q\topKq-λq\topq
that attains a maximum when
Kq=λq
This implies that the optimal rotation is parametrised by the quaternion
q*
λmax
K
The optimal quaternion can be determined by solving the characteristic equation of
K
K
Kq=λq
as a system of two equations
\begin{align} y&=\left((λ+\sigma)I-S\right)-1z\\ λ&=\sigma+zy \end{align}
where
y=style
| 1 | |
| q |
v
y
λ=\sigma+z\top\left((λ+\sigma)I-S\right)-1z
Since
λmax=maxg\left(A\right)
λmax=1-minl\left(A\right)
λmax ≈ 1
l
q*
λmax
y
q*=
| 1 | ||||||
|
The
y
\theta=\pi
3 x 3
S
\det\left[S-\xiI\right]=-\xi3+2\sigma\xi2-k\xi+\Delta=0
where
\begin{align} \sigma&=
| 1 | |
| 2 |
\operatorname{tr}{S
The Cayley–Hamilton theorem states that any square matrix over a commutative ring satisfies its own characteristic equation, therefore
-S3+2\sigmaS2-kS+\Delta=0
allowing to write
\left((\omega+\sigma)I-S\right)-1=
| \alphaI+\betaS+S2 | |
| \gamma |
where
\begin{align} \alpha&=\omega2-\sigma2+k\\ \beta&=\omega-\sigma\\ \gamma&=(\omega+\sigma)\alpha-\Delta \end{align}
and for
\omega=λmax
\begin{align} y*&=\left((λ+\sigma)I-S\right)-1z\\ &=
| \alphaI+\betaS+S2 | |
| \gamma |
z \end{align}
that gives the conjugate quaternion representation of the optimal rotation as
q*=
| 1 | |
| \sqrt{\gamma2+\left|x\right|2 |
where
x=\left(\alphaI+\betaS+S2\right)z
The value of
λmax
\left((\omega+\sigma)I-S\right)-1
λ=\sigma+z\top\left((λ+\sigma)I-S\right)-1z
gives
λ4-(a+b)λ2-cλ+(ab+c\sigma-d)=0
where
\begin{align} a&=\sigma2-k\\ b&=\sigma2+z\topz\\ c&=\Delta+z\topSz\\ d&=z\topS2z \end{align}
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
λmax ≈ 1