J of Astronaut Sci (2013) 60:303–312 DOI 10.1007/s40295-015-0049-x

Equivalence of Two Solutions of Wahba’s Problem F. Landis Markley1

Published online: 28 July 2015 © American Astronautical Society (Outside the U.S.) 2015

Abstract Many attitude estimation methods are based on an optimization problem posed in 1965 by Grace Wahba. All these methods yield the same optimal estimate, except for inevitable computer roundoff errors. This note shows shows that Shuster’s Quaternion Estimator (QUEST) and Mortari’s Estimator of the Optimal Quaternion (ESOQ) are essentially identical even in the presence of roundoff errors. It also shows some connections between two other algorithms for solving Wahba’s problem: Davenport’s q method and the Singular Value Decomposition (SVD) method. Keywords Attitude estimation · Wahba’s problem

Introduction In many spacecraft attitude systems, the observations used for attitude determination are naturally represented as unit vectors. Typical examples are the unit vectors giving the direction to the sun or a star and the unit vector in the direction of the Earth’s magnetic field. Most attitude determination methods using simultaneous vector measurements are based on a problem posed in 1965 by Grace Wahba [1]. Many of the algorithms for solving Wahba’s problem have been surveyed in Ref. [2]. All solutions of this problem will yield equivalent results, if iterative processes are carried out to full computer precision, except for inevitable computer roundoff errors. The main

 F. Landis Markley

[email protected] 1

Aerospace Engineer Emeritus, Attitude Control Systems Engineering Branch, NASA Goddard Space Flight Center, Greenbelt MD, USA

304

J of Astronaut Sci (2013) 60:303–312

purpose of this note is to show that two of these algorithms, Shuster’s Quaternion Estimator (QUEST) [3] and Mortari’s Estimator of the Optimal Quaternion (ESOQ) [4], are essentially identical irrespective of computer roundoff. We also present some interesting connections between Davenport’s q method [5] and the Singular Value Decomposition (SVD) [6] solution of Wahba’s problem.

Wahba’s Problem Wahba’s problem is to find the orthogonal matrix A with determinant +1 that minimizes the loss function n n n   1 L(A) ≡ ai |bi − Ari |2 = ai − ai bTi Ari , (1) 2 i=1

i=1

i=1

where {bi } is a set of n unit vectors measured in a spacecraft’s body frame, {ri } are the corresponding unit vectors in a reference frame, and {ai } are non-negative weights. We use the cyclic invariance of the trace to write the loss function in the very convenient form L(A) = λ0 − tr(AB T ), (2) with λ0 ≡

n 

ai

(3)

ai bi rTi .

(4)

i=1

and the “attitude profile matrix” B defined by B≡

n  i=1

Now it is clear that Wahba’s loss function is minimized when tr(AB T ) is maximized. Wahba’s problem is closely related to the Orthogonal Procrustes Problem, which has a long history in statistics and the social sciences.

Davenport’s q Method Paul Davenport provided the first solution of Wahba’s problem used to support NASA missions, the three High Energy Astronomy Observatories,1 by using the quaternion representation of the attitude [5, 7, 8]     q1:3 e sin(θ/2) q(e, θ) = , (5) = q4 cos(θ/2)   A(q) = q42 − q1:3 2 I3 − 2q4 [q1:3 ×] + 2 q1:3 qT1:3 , (6)

1 These

satellites were designated HEAO-1,2,3; HEAO-1 was launched in 1978.

J of Astronaut Sci (2013) 60:303–312

305

where In denotes the n × n identity matrix and [x×] is the cross-product matrix, ⎤ ⎡ 0 −x3 x2 (7) [x×] ≡ ⎣ x3 0 −x1 ⎦ . −x2 x1 0 Substituting the quaternion representation into Eq. 2 gives L(A(q)) = λ0 − qT K(B)q,

(8)

where K(B) is the symmetric traceless 4 × 4 matrix   B + B T − (trB)I3 z , K(B) ≡ zT trB with z≡

n 

(9)

ai bi × ri ,

(10)

[z×] = B T − B.

(11)

i=1

which is equivalent to It is known that a real symmetric n × n matrix has n real eigenvalues and n real eigenvectors that can be chosen to form an orthonormal basis [9, 10]. The eigenvalue/eigenvector decomposition of K can be written as K(B) =

4 

λμ qμ qTμ ,

(12)

μ=1

where qμ is the eigenvector corresponding to eigenvalue λμ , and the eigenvalues are labeled so that λmax ≡ λ1 ≥ λ2 ≥ λ3 ≥ λ4 . The eigenvalue/eigenvector decomposition and the cyclic invariance of the trace give L(A(q)) = λ0 −

4 

λμ (qT qμ )2 = λ0 − λ1

μ=1

= λ0 − λ1 +

4 

(qT qμ )2 +

μ=1 4 

(λ1 − λμ )(qT qμ )2 ,

4 

(λ1 − λμ )(qT qμ )2

μ=1

(13)

μ=2

where the final equality holds because the four eigenvectors constitute an orthonormal basis and because the μ = 1 term in the second sum vanishes identically. The loss function is minimized if the quaternion is orthogonal to q2 , q3 , and q4 ; i.e. the optimal estimate, indicated by a caret, is the normalized eigenvector of K corresponding to the largest eigenvalue qˆ = q1 . (14) This result can also be derived by using a Lagrange multiplier to append the quaternion norm constraint to the loss function. Davenport’s algorithm does not have a unique solution if the two largest eigenvalues of K(B) are equal. This is not a failure of the q method; it means that the data

306

J of Astronaut Sci (2013) 60:303–312

are not sufficient to determine the attitude uniquely. Very robust algorithms exist to solve the symmetric eigenvalue problem, and Davenport’s method remains the best method for solving Wahba’s problem if one has access to one of these eigenvalue decomposition algorithms.

Quaternion Estimator (QUEST) The computers of the time were not powerful enough to use Davenport’s q method to provide the more frequent attitude computations required by the MAGSAT spacecraft, launched two years after HEAO-1. The QUEST algorithm was devised to answer this need, and has become the most widely used algorithm for solving Wahba’s problem [2, 3, 8]. We can express Davenport’s eigenvalue condition as H (λmax ) qˆ = 0, where

 H (λ) ≡ λI4 − K(B) =

(15)

(λ + trB)I3 − S −z −zT λ − trB

 (16)

and S ≡ B + BT .

(17)

Equation 15 is equivalent to the two equations (ρI3 − S)qˆ 1:3 = qˆ4 z (λmax − trB)qˆ4 − zT qˆ 1:3 = 0,

(18a) (18b)

ρ ≡ λmax + trB.

(19)

where If we knew λmax , Eq. 18a would give the optimal quaternion as   (ρI3 − S)−1 z , qˆ = α 1

(20)

where α is determined by quaternion normalization. This is mathematically equivalent to   adj(ρI3 − S)z , (21) qˆ = α  det(ρI3 − S) where adj denotes the classical adjoint [9]. Substituting Eq. 21 into Eq. 18b gives a quartic equation for the maximum eigenvalue 0 = ψ(λ) ≡ (λ − trB) det(ρI3 − S) − zT adj(ρI3 − S)z =

4 

(λ − λμ ),

(22)

μ=1

which is just the characteristic equation of the matrix K. We have written λ instead of λmax in Eq. 22 because this equation has four roots. The QUEST algorithm finds

J of Astronaut Sci (2013) 60:303–312

307

the largest root by Newton-Raphson iteration of Eq. 22 with the starting value λ0 , and then solves Eq. 20 to find the optimal quaternion [2, 3]. The classical adjoint of the matrix H (λ) evaluated at λ = λmax = λ1 , is adj H (λmax ) = (λmax − λ2 )(λmax − λ3 )(λmax − λ4 )qˆ qˆ T = γ qˆ qˆ T ,

(23)

where γ is positive if λmax  = λ2 , which we have seen to be the condition for uniqueness of the attitude solution. In fact, γ is just dψ/dλ evaluated at λ = λmax . The 4 − 4 component of Eq. 23 is [adj H (λmax )]44 = det(ρI3 − S) = γ qˆ42

(24)

and the other three elements of the fourth column are [adj H (λmax )]k4 = [adj(ρI3 − S)z]k = γ qˆ4 qˆk

for k = 1, 2, 3.

(25)

Comparison with Eq. 21 establishes that the QUEST quaternion estimate is just the normalized fourth column of adj H (λmax ) and that α  = ±(γ qˆ4 )−1 = ±[γ det(ρI3 − S)]−1/2 .

(26)

Equation 24 clearly shows that q4 is zero if ρI3 − S is singular, which means that the estimate in this case is a 180◦ rotation. We can also see that α is infinite in this limit. Quaternion normalization requires that adj(ρI3 − S)z tends to [γ det(ρI3 − S)]1/2 in the singular limit, so it is clear that the QUEST computation of the quaternion loses all numerical significance.

Method of Sequential Rotations Shuster introduced the method of sequential rotations to deal with the indeterminacy of the QUEST solution when ρI3 − S is singular [3, 8, 11]. The idea behind this method is to solve for a quaternion qk ≡ qBRk representing the attitude with respect to a rotated frame Rk . The attitude quaternion q ≡ qBR with respect to the original frame R is the product of qk and the quaternion qRk R representing the rotation between reference frames R and Rk . For simplicity, the reference frames are related by a 180◦ rotation about one of the coordinate axes. With the quaternion multiplication convention of Refs. [12] and [13], this means that q ≡ qBR = qBRk ⊗ qRk R = qk ⊗ q(ek , π)  k     k  q4 ek + ek × qk1:3 q1:3 ek = = ⊗ = Ek qk , 0 q4k −ek · qk1:3 where ek is the unit vector along the kth coordinate axis and where   [ek ×] ek . Ek ≡ −eTk 0

(27)

(28)

The matrix Ek is skew-symmetric and orthogonal. It has exactly one element with the value ±1 in each column and exactly one element with the value ±1 in each row; all

308

J of Astronaut Sci (2013) 60:303–312

its other elements are zero. Thus Ek is like a permutation matrix except for the minus signs. This means that the transformation from qk to q requires no multiplications; it merely permutes the components of the quaternion with some sign changes. The inverse transformation is qk = q ⊗ q(ek , −π ) = EkT q.

(29)

We see that the kth component of q ends up as the fourth component of qk . Because q is a unit quaternion, at least one of its components must have magnitude greater than or equal to 1/2, so it is always possible to find a reference frame rotation that results in q4k having magnitude of at least 1/2. Since QUEST prefers to keep the magnitude of this component away from zero, it follows that the optimal reference frame rotation is a rotation about the axis corresponding to the component of q having the largest magnitude. If the fourth component of q has the largest magnitude, no reference frame rotation is required. These rotations are easy to implement on the input data, since a 180◦ rotation about the kth coordinate axis simply changes the signs of the ith and j th components of each reference vector, where {i, j, k} is a permutation of {1, 2, 3}. This is equivalent to changing the signs of the ith and j th columns of the B matrix. The reference system rotation is easily undone by Eq. 27 after the optimal quaternion in the rotated frame has been computed. The original QUEST implementation performed sequential rotations one axis at a time, until an acceptable reference coordinate system was found. It is not necessary to find a rotation resulting in q4k ≥ 1/2, it is only necessary for q4 to be larger than some chosen value. It is clearly preferable to save computations by choosing a single desirable rotation as early in the computation as possible. This can be accomplished by considering the components of an a priori quaternion if one is available. An a priori quaternion is generally available before computing the final attitude estimate in a star tracker application since an approximate attitude estimate is needed to identify the stars in the tracker’s field of view. This is either available from a previous estimate or is produced by a “lost-in-space” algorithm using fewer (generally three or four) stars than are employed for the final attitude estimate. We now want to show the effect of reference frame rotations on Davenport’s K matrix. This is not part of the implementation of QUEST, but serves to show the relation to the ESOQ algorithm. Putting A(q) instead of B into Davenport’s definition of K gives K(A(q)) = 4qqT − I4 . (30) The orthonormality of the eigenvectors and the cyclic invariance of the trace mean that 4  0 = tr(K(B)) = λμ . (31) μ=1

These allow us to write Eq. 12 as K(B) =

4 4 1 1 λμ (4qμ qTμ − I4 ) = λμ K(A(qμ )). 4 4 μ=1

μ=1

(32)

J of Astronaut Sci (2013) 60:303–312

309

It follows from this that that B=

4 1 λμ A(qμ ). 4

(33)

μ=1

In the rotated reference frame, we have Bk = BA(ek , −π ) =

4 1 λμ A(qμ )A(ek , −π ) 4 μ=1

=

1 4

4 

λμ A(qμ ⊗ q(ek , −π )) =

μ=1

4 1 λμ A(EkT qμ ), 4

(34)

μ=1

but then we have K(Bk ) =

4 4 1 1 λμ K(A(EkT qμ )) = λμ EkT (4qμ qTμ − I4 )Ek = EkT K(B)Ek 4 4 μ=1

μ=1

(35) This result would be achieved much more directly if we could simply assert that all the eigenvectors of K transform according to Eq. 29 under reference frame rotations, but this is only guaranteed to be true if no two eigenvectors are equal. We have shown that the effect of a reference frame rotation on K(B) is to permute its rows and columns, with some sign changes. The form of Ek shows that the rows and columns labeled 4 and k are interchanged, as are the rows and columns labeled i and j . From this viewpoint, the purpose of reference frame rotations is to move [K(B)]kk to the lower-right corner of K(Bk ).

Estimator of the Optimal Quaternion (ESOQ) The significance of the matrix adjH (λmax ) was first recognized by Daniele Mortari, who observed that the optimal quaternion can be computed by normalizing any column of that matrix [4].2 His ESOQ algorithm uses this insight to avoid the need for explicit reference frame rotations by treating the four components of the quaternion more symmetrically than QUEST [2, 4]. The two algorithms are similar in finding λmax from the characteristic equation of Davenport’s K matrix and in locating the component of q with the maximum magnitude. The implementation of ESOQ is straightforward. Let Hk denote the symmetric 3 × 3 matrix obtained by deleting the kth row and kth column from H (λmax ), where k can be any index between 1 and 4, and let hk denote the three-component column vector obtained by deleting the kth element from the kth column of H (λmax ). Then the kth component of the optimal quaternion is given by qˆk = −αk det Hk 2 Mortari

(36)

originally presented ESOQ’s computation of the optimal quaternion as the 4-dimensional cross product of three rows of H (λmax ), which is mathematically equivalent to the method presented here.

310

J of Astronaut Sci (2013) 60:303–312

and the other three components are given by3   qˆ 1:k−1 = αk (adj Hk )hk , qˆ k+1:4

(37)

where αk can be determined by quaternion normalization. If an a priori quaternion estimate is available, the index k is is chosen to be the index of the component of the a priori quaternion having the largest magnitude. Otherwise k is chosen to be the index of the diagonal element [adjH (λmax )]kk = det Hk of the adjoint matrix having the largest magnitude. Assume that QUEST and ESOQ use the same values of λmax and k. If k = 4, then H4 = ρI3 −S and h4 = −z, so it is obvious that the QUEST and ESOQ equations for the optimal quaternion differ only by an irrelevant overall minus sign. The previous section showed that sequential rotations in QUEST are equivalent to relabeling the rows and columns of H , with possible sign changes, from which it follows that the only difference in the QUEST and ESOQ algorithms for other values of k is in some intermediate multiplications, for which any computer gives (−a)b = a(−b) and (−a)(−b) = ab exactly. Variations in the detailed implementation of Eqs. 21, 36, and 37 may give rise to some differences, of course.

Relation to the SVD Method The singular value decomposition of the attitude profile matrix can be written as [6, 9, 10] (38) B = U+ V+T = U+ diag([s1 s2 s3 ])V+T , where U+ and V+ are proper orthogonal matrices and s1 ≥ s2 ≥ |s3 |. It was shown in Ref. [6] that the optimal attitude matrix is ˆ = A(q1 ). Aopt = U+ V+T = A(q)

(39)

The orthogonality of the four eigenvectors of Davenport’s K matrix means that they specify attitudes that differ by 180◦ rotations, because the fourth component of the product     (qν )4 (qμ )1:3 − (qμ )4 (qν )1:3 + (qμ )1:3 × (qν )1:3 eμν sin(θμν /2) qμ ⊗q−1 = = ν cos(θμν /2) qμ · q ν (40) is zero for μ = ν. Letting the axis of the body frame rotation relating A(qμ+1 ) to A(q1 ) be U+ nμ , where n1 , n2 , n3 form an orthogonal triad of unit vectors, gives A(qμ+1 ) = R(U+ nμ , π)A(q1 ) = U+ R(nμ , π)V+T = U+ (2nμ nTμ − I3 )V+T for i = 1, 2, 3, (41)

3 We

employ the convention that x1:0 or x5:4 is an empty vector, with no components.

J of Astronaut Sci (2013) 60:303–312

311

where R(e, θ) denotes a rotation about axis e by an angle θ and we have employed the identity R(U nμ , θ) = U R(nμ , θ)U T for proper orthogonal U . Substituting this and Eqs. 38 and 39 into Eq. 33 gives 4 = λ1 I3 +

3 

λμ+1 (2nμ nTμ − I3 ) = 2

μ=1

3 

(λ1 + λμ+1 )nμ nTμ ,

(42)

μ=1

using Eq. 31 and 3μ=1 nμ nTμ = I3 , which holds because the vectors nμ constitute a complete orthonormal basis. It is easy to see that the eigenvalues of are 1 (43) (λ1 + λμ+1 ) for μ = 1, 2, 3, 2 where the index assignments are determined by the ordering of both the singular values of B and the eigenvalues of K in order of descending magnitude. Substituting Eq. 43 into Eq. 42 gives sμ =

=

3 

sμ nμ nTμ .

(44)

μ=1

Comparison with Eq. 38 shows that if the singular values are distinct, nμ must be equal to eμ , the unit vector along the μth coordinate axis. If the singular values are not distinct, then U+ and V+ are not uniquely defined. In this case, at least two of the eigenvalues λμ are equal, and the corresponding eigenvectors can be chosen to be any orthogonal set in the subspace corresponding to the equal eigenvalues. The result is that there is always a singular value decomposition and an eigenvalue/eigenvector decomposition such that A(qμ+1 ) = U+ R(eμ , π)V+T = Aopt R(V+ eμ , π) = R(U+ eμ , π)Aopt for μ = 1, 2, 3, (45) or equivalently     U+ eμ V+ eμ ± qμ+1 = qˆ ⊗ = ⊗ qˆ for μ = 1, 2, 3, (46) 0 0 but these relations are guaranteed to be true only if the singular values are distinct. There is no sign ambiguity between the last two terms of Eq. 46 because this equality is equivalent to         ˆ + eμ V + eμ U + eμ A(q)V U+ V+T V+ eμ −1 , (47) = qˆ ⊗ ⊗ qˆ = = 0 0 0 0 which is an obvious identity. Note that U+ eμ and V+ eμ are just the μth columns of U+ and V+ , respectively.

Conclusions All algorithms that minimize Wahba’s loss function would provide the same attitude estimate if the minimization could be performed with infinite precision. The

312

J of Astronaut Sci (2013) 60:303–312

QUEST and ESOQ algorithms are known to provide efficient and accurate solutions to Wahba’s problem. We have shown that these two algorithms actually provide identical solutions without requiring the minimization procedure to converge completely, if implemented with the same reference frame and the same solution of the characteristic equation of Davenport’s K matrix. Between these two algorithms, ESOQ may be preferred for replacing some computations with simple indexing operations. However, the computational savings are not large, and implementations of QUEST incorporating extensive error checks have a long history of successful application. Note that the solution provided by Mortari’s Second Estimator of the Optimal Quaternion (ESOQ2) [14] is not precisely identical to any other solution in the same sense. This paper has also exhibited relations between the singular value decomposition of the attitude profile matrix B and the eigenvalues and eigenvectors of Davenport’s K matrix. We have not found any practical application for these relations, however. Acknowledgments I would like to acknowledge many valuable discussions of Wahba’s problem with Malcolm D. Shuster and Daniele Mortari. In particular, I want to thank Malcolm for suggesting the topic of this note. He intuited the equivalence of QUEST and ESOQ exactly as shown in this paper. I also want to acknowledge Yang Cheng’s critical contribution to the discussion of the singularity of QUEST for 180◦ rotations.

References 1. Wahba, G.: A least-squares estimate of satellite attitude. SIAM Review 7(3), 409 (1965) 2. Markley, F.L., Mortari, D.: Quaternion attitude estimation using vector observations. J. Astronaut. Sci. 48(2/3), 359–380 (2000) 3. Shuster, M.D., Oh, S.D.: Attitude determination from vector observations. J. Guid. Control 4(1), 70– 77 (1981) 4. Mortari, D.: ESOQ: A closed-form solution of the Wahba problem. J. Astronaut. Sci. 45(2), 195–204 (1997) 5. Lerner, G.M.: Three-axis attitude determination. In: Wertz, J.R. (ed.) Spacecraft attitude determination and control. chap. 12. Kluwer Academic Publishers, The Netherlands (1978) 6. Markley, F.L.: Attitude determination using vector observations and the singular value decomposition. J. Astronaut. Sci. 36(3), 245–258 (1988) 7. Fallon III, L., Harrop, I.H., Sturch, C.R.: Ground attitude determination and gyro calibration procedures for the HEAO missions. In: 17th Aerospace Sciences Meeting, No. AIAA 79-0397, New Orleans, LA (1979) 8. Shuster, M.D.: The quest for better attitudes. J. Astronaut. Sci. 54(3/4), 657–683 (2006) 9. Horn, R.A., Johnson, C.R.: Matrix Analysis. Cambridge University Press, Cambridge (1985) 10. Golub, G.H., Van Loan, C.F. Matrix Computations, 3rd ed. The Johns Hopkins University Press, Baltimore (1996) 11. Shuster, M.D., Natanson, G.A.: Quaternion computation from a geometric point of view. J. Astronaut. Sci. 41(4), 545–556 (1993) 12. Lefferts, E.J., Markley, F.L., Shuster, M.D.: Kalman filtering for spacecraft attitude estimation. J. Guid. Control. Dyn. 5(5), 417–429 (1982) 13. Shuster, M.D.: A survey of attitude representations. J. Astronaut. Sci. 41(4), 439–517 (1993) 14. Mortari, D.: Second estimator of the optimal quaternion. J. Guid. Control. Dyn. 23(5), 885–888 (2000)