AAS 10-211

THE SEQUENTIAL OPTIMAL ATTITUDE RECURSION FILTER

John A. Christian∗ and E. Glenn Lightsey†

A new nonlinear attitude filter called the Sequential Optimal Attitude Recursion (SOAR)Filter is developed. This routine is based on maximum likelihood estimation and a sequen-tialization of the Wahba Problem that has been extended to include non-attitude states. Thealgorithm can accept either individual unit vector measurements or quaternion measure-ments. This new algorithm is compared with existing attitude filtering routines, includingthe Multiplicative Extended Kalman Filter, Filter QUEST, REQUEST, Optimal-REQUEST,and Extended-QUEST.

INTRODUCTION

The problem of estimating spacecraft attitude from a set of unit vector observations has received muchattention since the 1960s. The most well known formulation of this problem was given by Grace Wahba in19651 and is commonly referred to as the “Wahba Problem.” If the Wahba Problem is written in terms ofthe attitude quaternion, an analytic solution exists in the form of the solution to an eigenvalue-eigenvectorproblem. The optimal attitude is simply given by the eigenvector associated with the largest eigenvalue.Numerous algorithms have been developed over the years to find fast and robust solutions to this problem.2

Some of the most well known algorithms include the QUaternion ESTimator (QUEST),3 the EStimator ofthe Optimal Quaternion (ESOQ),4, 5 and Singular Value Decomposition (SVD).6

Traditional solutions to the Wahba Problem are batch estimators that assume all the unit vector observa-tions occur at the same time. The Wahba Problem is a weighted least squares problem. If the weights inthis problem are chosen to be the inverse of the measurement noise variance, the result is a maximum like-lihood (or minimum variance) estimate of the attitude. The objective of the present research is to extend thetraditional Wahba Problem into a framework that allows for the creation of an optimal attitude filter.

One of the most common methods of attitude filtering in modern systems is the Multiplicative ExtendedKalman Filter (MEKF). This method uses the four-component attitude quaternion to represent the attitude, buta three-component representation of the attitude in the filter. The MEKF is structured this way because whilethe attitude quaternion is globally nonsingular, making it a good choice for representing spacecraft attitude,it must obey a unity norm constraint, making direct implementation of the quaternion in a normal Kalmanfilter difficult. Additionally, a small angle assumption allows the three-component attitude representation tobe cast in a form consistent with the additive nature of the Kalman filter (i.e. x = x + δx).

Unlike the MEKF, sequential Wahba Problem filters estimate the full quaternion without requiring solu-tions to be computed as small angle deviations from a reference attitude. The batch solutions to the WahbaProblem have also been shown to be extremely robust. These are some of the advantages that have led re-searchers to investigate sequential solutions to the Wahba Problem for attitude filtering. Shuster provided theearliest known work in this topic with the development of “Filter QUEST” in a number of articles.7, 8 Subse-quently, Bar-Itzhack introduced an alternate sequentialization approach known as the REQUEST algorithm.9

Several years after the introduction of the REQUEST algorithm, Shuster10 demonstrated that although theFilter QUEST and REQUEST algorithms approach the problem from different perspectives, they are math-ematically equivalent. Both Filter QUEST and REQUEST algorithms are examples of suboptimal fadingmemory filters. Shuster demonstrates7, 8 that under a specific set of conditions, the optimal fading memory

∗Assistant Instructor, Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, 1 UniversityStation, C0600, Austin, TX 78712.†Professor, Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, 1 University Station,C0600, Austin, TX 78712.

1

factor, αk, may be analytically computed. The equation for αk is simple, fast to compute, and provides anexcellent approximation of the more general optimal fading memory factor for many practical cases. The as-sumptions required to arrive at this result are rarely met, however, and a better value of αk may be computedat the expense of a little more computation time.

An approach for the optimal blending of the new attitude measurements with old attitude estimates directlyin the Wahba Problem solution framework was proposed in 2004 by Choukroun with the Optimal-REQUESTalgorithm.11 This algorithm, however, requires access to individual unit vector measurements. This maycause difficulties if an instrument provides a measurement in the form of an attitude quaternion, as is the casewith most commercially available star trackers. Additionally, like Filter QUEST and REQUEST, Optimal-REQUEST is only capable of estimating attitude. None of the sequential Wahba Problem filters discussedabove are capable of incorporating estimates of other parameters, such as sensor biases.

Taking a different approach, Psiaki developed the Extended-QUEST algorithm.12 The Extended-QUESTalgorithm offers an optimal estimate of the attitude and is capable of also estimating non-attitude states. Thisalgorithm is formulated as a square-root information filter that contains a propagation phase and a state updatephase. The formulation of Extended-QUEST requires the QR factorization of the propagated informationmatrix for use in the filter. It also requires the solution to a more general objective function than requiredfor Filter QUEST or REQUEST. Despite these increases in complexity and computations, Extended-QUESTprovides an optimal estimate of both attitude and and non-attitude states. It has been shown to be robust withconvergence properties that are substantially better than seen in the MEKF.12

Other algorithms have also been developed that are capable of estimating both attitude states and non-attitude states. Most notable among these is an algorithm introduced by Markley.13, 14 While many othernonlinear attitude filtering approaches exist, they are not discussed here. Thorough discussions of these othermethods may be found in the literature.15

It is within this context that a new nonlinear attitude filtering algorithm is proposed: the Sequential OptimalAttitude Recursion (SOAR) Filter. The derivation of the SOAR Filter begins by creating a maximum likeli-hood estimate (MLE) of the state vector through Bayesian estimation. A fresh look at the sequentializationof the Wahba Problem allows the classic Wahba Problem to be recast in an MLE framework. Additionally,the Bayesian estimation approach allows for the seamless inclusion of non-attitude states in the SOAR Filter.Using ideas from the Extended Kalman Filter (EKF), the covariance inflation associated with the propagationof the state is examined. Treating the covariance in this manner allows for the straightforward inclusion ofprocess noise and addresses the problem of the suboptimal fading memory factor seen in Filter QUEST andREQUEST. If the attitude covariance and measurement covariance are known at the time of the update, theoptimal estimate of the attitude may be determined.

The SOAR Filter shares many similarities with some of the previous attitude filtering methods. As subse-quent discussions will show, the SOAR Filter is most similar to Extended-QUEST. The paper concludes witha side-by-side comparison of the SOAR Filter with a number of the other attitude filtering techniques.

A REVIEW OF THE CLASSICAL WAHBA PROBLEM

The problem of estimating vehicle attitude from a set of unit vector observations has been studied in greatdetail. The objective function commonly used to describe this problem was posed by Grace Wahba in 1965and is frequently referred to as the “Wahba Problem.”1 The Wahba Problem statement, using traditionalnomenclature, is given by:

Min J(A) =12

m∑i=1

wi ‖wi − Avi‖2 (1)

where m is the number of observed unit vectors, wi is a positive weighting on the i-th observation, wi is themeasured unit vector for the i-th observation as expressed in the body frame, vi is the known reference unitvector for the i-th observation in a known inertial frame, and A is the rotation matrix that transforms a vectorfrom the inertial frame to the body frame.

2

For the purposes of this discussion, however, a slightly different notation will be used. First, the rotationmatrix that transforms a vector from the inertial frame to the body frame will be denoted by TIB . Therefore,the matrix TIB describes the attitude of the vehicle body frame, B, with respect to the inertial frame, I .Second, note that the unit vectors wi and vi in Eq. 1 are actually the same unit vector expressed in twodifferent frames. Therefore, denote this unit vector as ei, such that (ei)I is the reference unit vector expressedin the inertial frame, i.e. (ei)I = vi. Unfortunately, measurement errors prevent wi from simply being eiexpressed in the actual body frame. Instead each measurement will exist in some “measurement” body framethat would produce the measured unit vector. Therefore, let (ei)B,i be the i-th unit vector expressed in ameasurement body frame, Bi, that yields the measured direction of an observed object, i.e. (ei)B,i = wi.Because the measurement noise is different for each unit vector observation, there will be i different B frames- one for each of the i observed unit vectors. Hence the use of a subscript i in the frame definition. Althoughthis notation is more cumbersome, it helps emphasize the frames and vectors in a more explicit fashion.Therefore the Wahba Problem may be rewritten in this new notation as

Min J(TIB) =12

m∑i=1

wi

∥∥∥(ei)B,i − TIB (ei)I∥∥∥2

(2)

Measurement noise will prevent the rotation matrix TIB from being able to exactly rotate all of the thereference unit vectors to match the corresponding observed unit vectors. Although the error in each line-of-sight unit vector should formally be represented as a rotation, it may also be represented as an additiveerror:

(ei)B,i = TIB (ei)I + εi = (ei)B + εi (3)

where εi is the measurement error in the observed unit vector. For (ei)B,i to remain a unit vector to firstorder, the measurement error must be perpendicular to the true observation unit vector:

(ei)TB εi ≈ 0 (4)

Additionally assume that these measurements are unbiased, such that E [εi] = 0. Further, numerous au-thors16, 17 have shown that the line-of-sight unit vector observations have a covariance that can be describedto first order by

Ri = E[εiε

Ti

]= σ2

φ,i

[I3×3 − (ei)B (ei)

TB

](5)

where σ2φ,i is the angular variance (in radians) of the measurement error.

Wahba’s problem from Eq. 2 may be rewritten as is commonly seen in the literature:2

Min J(TIB) = λ0 −m∑i=1

witr[TIB (ei)I (ei)

TB,i

]= λ0 − tr

[TIBBT

](6)

where λ0 =∑mi=1 wi and B is called the attitude profile matrix and is given by

B =m∑i=1

wi (ei)B,i (ei)TI (7)

Alternatively, if Wahba’s problem is rewritten in terms of the attitude quaternion, q, instead of the rotationmatrix, TIB , using

T (q) =(q24 − qTq

)I3×3 + 2qqT + 2q4 [q×] (8)

then the results of Eq. 2 or Eq. 6 may be expressed in the following well known quadratic form:3, 18

Min J(q) = λ0 − qTKq (9)

3

where K is called the Davenport matrix and is defined as

K =[

S− µI3×3 zzT µ

](10)

andS = B + BT µ = tr [B] [z×] = B− BT (11)

The attitude that minimizes the objective function in Eq. 9 may be found by the straightforward applicationof concepts from optimal control theory.19 Begin by recalling the quaternion unity norm constraint ‖q‖ = 1and adjoin this constraint to the objective function through a Lagrange multiplier, λ. Therefore, from Eq. 9:

Min J (q, λ) = λ0 − qTKq + λ(qT q− 1

)(12)

The optimal attitude is found by setting the first differential of J (q, λ) to zero. The optimal attitude is givenby the solution to3, 18, 20

Kq = λq (13)

Note that Eq. 13 is an eigenvalue-eigenvector problem. It is from this point that various solutions haveemerged. Despite all the discussion circulating about the various attitude determination algorithms, most ofthese are simply different methods for solving the eigenvalue-eigenvector problem in Eq. 13. Davenport’sq-Method, for example, directly solves Eq. 13 as if it were an ordinary eigenvalue-eigenvector problem,2

thus making it one of the more robust algorithms because it relies on relatively robust but slow numericalmethods for solving the eigenvalue-eigenvector problem. Further, by substituting Eq. 13 into Eq. 9, theoriginal objective function becomes:

J(q) = λ0 − qTλq = λ0 − λqT q = λ0 − λ (14)

Therefore, it may be noted that the optimal attitude is achieved with the largest eigenvalue.

The QUEST algorithm,3 developed by Shuster, solves this problem by Newton-Raphson iteration on thecharacteristic equation to find only the largest eigenvalue and associated eigenvector. While faster than Dav-enport’s q-Method, the solution approach introduces a singularity for a 180 degree rotation that must beaddressed in the algorithm implementation (typically through logic checks and sequential rotations).

EStimators of the Optimal Quaternion (ESOQ and ESOQ-2),4, 5 developed by Mortari, are also fundamen-tally based on the eigenvalue problem described in Eq. 13. Recognizing that

(Kq− λI4×4) q = Hq = 0 (15)

it is clear that q must lie in the null space of H. Further if HT = [h1 h2 h3 h4], then q must be perpendicularto all hi. From here, the ESOQ algorithm computes the optimal quaternion through a 4-dimensional cross-product operation (a special case of the n-dimensional cross-product)21 between two vectors in H: hi andhj , i 6= j. In the same year that ESOQ was introduced, Mortari also proposed a follow-on algorithm calledESOQ-2. This algorithm starts with Eq. 13 and computes a symmetric 3 × 3 matrix of rank 2, M, such thatMeθ = 0. Using a similar approach to that seen in the original ESOQ, the attitude may now be determinedby taking the regular 3-dimensional cross product of two rows of M.

ATTITUDE ERROR AND COVARIANCE RELATIONS

The Attitude Error Quaternion

Let the error in the attitude be described by the small error quaternion, δq:

δq = q⊗ ˆq−1 (16)

4

where ⊗ denotes the quaternion product operator. Physically, δq may be interpreted as the quaternion thatrotates the best estimated attitude to the true attitude. If one assumes small angles,

δq =[δqδq4

]≈[δθ/2

1

](17)

where δθ is a three-dimensional parameterization of the attitude given by eδθδθ. Here, eδθ is a unit vectordenoting the axis of rotation and δθ is the magnitude of the rotation about that axis in radians. Therefore,2δq ≈ δθ, which yields

Pθθ = E[δθδθT

]= 4E

[δqδqT

](18)

This choice means that Pθθ is the attitude covariance matrix as expressed in the body frame. Additionally,assume that the quaternion attitude estimate is unbiased:

E [δq] = E

[[δqδq4

]]=[

0 0 0 1]T

(19)

The definition in Eq. 16 also means that the small angle error vector is defined such that

T (θ) = T (δθ) T(θ) (20)

If the three-dimensional parameterization of the attitude given by θ is defined with respect to the attitudeestimate given by ˆq, then θ = 0 and T(θ) = I3×3 by definition. Therefore,

δθ = θ − θ = θ − 0 = θ (21)

Important Attitude Covariance Relations

Next, a few key relations that relate the attitude profile matrix to the covariance matrix are derived. Beginby recalling the Cramer-Rao inequality,22 which states

Pxx = E[(x− x) (x− x)T

]≥ (Fxx)−1 (22)

where Fxx is the Fisher information matrix. The Fisher information matrix is further defined as

Fxx =∂2J(x)∂x∂x

(23)

where the scalar objective function J is a function of the column-vector random variable x. If the objectivefunction J is linear in x and x has a Gaussian distribution then Fxx = P−1

xx . Unfortunately, this is not thecase in the present problem. When J is a nonlinear function of x, Fxx approaches P−1

xx as the number ofmeasurements becomes infinite:

P−1xx = lim

n→∞Fxx = lim

n→∞

∂2J(x)∂x∂x

(24)

where n is the number of observations.

To be consistent with earlier notation, let δθ be the small angle rotation that rotates the estimate of the ve-hicle attitude into the true attitude. Therefore, assuming small angles, the Wahba Problem objective functionof Eq. 6 evaluated at the true attitude may be rewritten as a function of δθ

J (δθ) = λ0 − tr[exp {[−δθ×]}TI

BBT]

(25)

Expanding the matrix exponential of [−δθ×] to second order (because the equation for J (δθ) must bedifferentiated twice),

J (δθ) = λ0 − tr[(

I3×3 + [−δθ×] +12

[−δθ×]2)

TIB

BT]

(26)

5

Now, compute the Fisher information matrix by taking the partial derivative of J (δθ) twice with respect toδθ:

Fθθ =∂2

∂δθ ∂δθJ (δθ) = tr

[TIB

BT]

I3×3 − TIB

BT (27)

Therefore,P−1θθ ≈ Fθθ = tr

[TIB

BT]

I3×3 − TIB

BT (28)

Rearranging the above equation for the attitude profile matrix B,

B =[

12

tr [Fθθ] I3×3 − Fθθ]

TIB

(29)

The relationships of Eq. 28 and Eq. 29 were first shown by Shuster in 1989.17 From Eq. 28 and Eq. 29, onemay freely move back and forth between B and Pθθ.

The Wahba Problem Objective Function in Terms of Fisher Information

In preparation for the development of the SOAR algorithm, it is useful to find the value of the WahbaProblem objective function in terms of the Fisher information matrix. Begin by substituting Eq. 29 into Eq.26

J (δθ) = λ0 − tr[(

I3×3 + [−δθ×] +12

[−δθ×]2)

TIB

TBI

[12

tr [Fθθ] I3×3 − FTθθ

]](30)

Recognizing that Fθθ is symmetric, and expanding the terms in the objective function yields

J (δθ) = λ0 −12

tr [Fθθ] +12δθTFθθδθ (31)

Taking closer look at the second term and substituting Eq. 28 for Fθθ,

12

tr [Fθθ] =12

tr[tr[TIB

BT]

I3×3 − TIB

BT]

= tr[TIB

BT]

= ˆqTKˆq (32)

Therefore, substituting this into Eq. 31 means that the Wahba Problem objective function evaluated at thetrue attitude may be written in terms of the Fisher information matrix as

J (δθ) = λ0 − ˆqTKˆq +12δθTFθθδθ (33)

Looking again at Eq. 14, it follows that

J (δθ) = J(ˆq)

+12δθTFθθδθ (34)

The form of Eq. 34 is not surprising given that the solution to the Wahba problem is known to produce amaximum likelihood estimate of the attitude. It is also interesting to note that the difference in the value ofthe objective function at the true attitude and the value of the objective function at the best estimate of theattitude is equivalent to the square of the Mahalanobis distance23, 24 between these two attitudes.

DEVELOPMENT OF SOAR FILTER

Maximum Likelihood State Estimation

The objective is to achieve a maximum likelihood estimate of the state vector at time tk, xk, given an apriori estimate and some set of observations, y. Begin by recalling Bayes Theorem,

p (x|y) =p (y|x) p (x)

p (y)(35)

6

where p (x|y) is the probability density function (PDF) that describes the probability of observing the statevector x conditioned on the knowledge of the observation y.

In the present optimization problem, suppose the objective is to maximize the likelihood of the state esti-mate at time tk conditioned on m new measurements and an a priori estimate of the state (with the a prioriestimate conditioned on r old measurements). Using Bayes Theorem, this may be mathematically expressedas

Max p(xk|y1, y2, . . . , yr+m

)=p(yr+1, . . . , yr+m|xk

)p (xk|y1, y2, . . . , yr)

p(yr+1, . . . , yr+m

) (36)

Proceed by assuming that each of the new observation are independent,

p(yr+1, . . . , yr+m|xk

)=

r+m∏i=r+1

p (yi|xk) (37)

such that the optimization problem in Eq. 36 may be rewritten as∗

Max p(xk|y1, y2, . . . , yr+m

)=p (xk|y1, y2, . . . , yr)

∏r+mi=r+1 p (yi|xk)

p(yr+1, . . . , yr+m

) (38)

This formulation, of course, makes no assumption about the actual distributions of the state vector or themeasurements. Now, if the a priori estimate of the state vector at time tk is assumed to have a Gaussiandistribution, then

p (xk|y1, y2, . . . , yr) = C1,k exp{−1

2

(xk − x−k

)T (P−k)−1

(xk − x−k

)}(39)

where C1,k is a constant that makes the integral of p(·) equal to unity. Similarly, if the measurement errorsare assumed to have a Gaussian distribution

p (yi|xk) = C2,i exp{−1

2

(yi − hi

(x−k))T

R−1i

(yi − hi

(x−k))}

(40)

For the moment, assume the measurements are line-of-sight unit vector observations such that yi = (ei)B,iand hi

(x−k)

= TIB (ei)I . Making these substitutions,

p (yi|xk) = C2,i exp{−1

2

[(ei)B,i − TIB (ei)I

]TR−1i

[(ei)B,i − TIB (ei)I

]}(41)

Unfortunately, the measurement covariance matrix Ri of Eq. 5 is singular and the regular inverse may not becomputed (Ri is a 3 × 3 matrix with a rank of two). Therefore, computing the Moore-Penrose pseudoinverseof Ri yields

“R−1” = R+ =1σ2φ,i

[I3×3 − (ei)B (ei)

TB

](42)

Substituting the pseudoinverse for R−1 in Eq. 41 and recalling the identity from Eq. 4,

p (yi|xk) = C2,i exp

{−1

21σ2φ,i

[(ei)B,i − TIB (ei)I

]T [(ei)B,i − TIB (ei)I

]}(43)

∗To help explicitly connect the PDFs shown in Eq. 38 to the notation used elsewhere in this report, consider the following relations.Assume that the state x is an n× 1 vector given by x = [x1 x2 . . . xn]T

x+k = E

[x−k]

=

∫ ∞−∞

∫ ∞−∞

. . .

∫ ∞−∞

xk p(xk|y1, y2, . . . , yr+m

)dx1 dx2 . . . dxn

x−k = E[

x−k]

=

∫ ∞−∞

∫ ∞−∞

. . .

∫ ∞−∞

xk p (xk|y1, y2, . . . , yr) dx1 dx2 . . . dxn

7

Therefore, substituting the result of Eq. 43 into Eq 37,

p(yr+1, . . . , yr+m|xk

)=

r+m∏i=r+1

p (yi|xk) = C3,k exp

{−1

2

r+m∑i=r+1

1σ2φ,i

∥∥∥(ei)B,i − TIB (ei)I∥∥∥2}

(44)

where C3,k =∏r+mi=r+1 C2,i. Note that the term in the exponent of Eq. 44 is equivalent to the traditional

Wahba Problem objective function to first order if wi = 1/σ2φ,i. This demonstrates that solutions to the

Wahba Problem produce a maximum likelihood estimate of the attitude if wi = 1/σ2φ,i.

Substituting Eq. 39 and Eq. 44 into Eq. 38, a maximum likelihood estimate of the state exists at

Max p(xk|y1, y2, . . . , yr+m

)=

C1,k C3,k

p(yr+1, . . . , yr+m

) exp{−1

2

(xk − x−k

)T (P−k)−1

(xk − x−k

)}(45)

× exp

{−1

2

r+m∑i=r+1

1σ2φ,i

∥∥∥(ei)B,i − TIB (ei)I∥∥∥2}

Recognizing that the maximum of p(xk|y1, y2, . . . , yr+m

)occurs at the same value of the state as the max-

imum of ln[p(xk|y1, y2, . . . , yr+m

)], a maximum likelihood estimate of the state may also be found by

finding

Max ln [C1,k] + ln [C3,k]− ln[p(yr+1, . . . , yr+m

)](46)

− 12

(xk − x−k

)T (P−k)−1

(xk − x−k

)− 1

2

r+m∑i=r+1

1σ2φ,i

∥∥∥(ei)B,i − TIB (ei)I∥∥∥2

Because the first three terms do not depend on the estimate of the state, they may be dropped from theobjective function in determining the maximum likelihood estimate. Dropping the these terms and rewritingas a minimization problem yields the following form of the objective function

Min L =12

(xk − x−k

)T (P−k)−1

(xk − x−k

)+

12

r+m∑i=r+1

1σ2φ,i

∥∥∥(ei)B,i − TIB (ei)I∥∥∥2

(47)

Discussion of SOAR Filter State Vector

The objective of the present filter is to estimate the attitude of a vehicle along with a number of othervehicle parameters. To express the attitude, it is desired to use a minimal degree representation (a number ofdifferent three-dimensional parameterizations would work here) instead of the constrained four-dimensionalquaternion. Here, the attitude will be represented by the vector θ:

θ = eθθ (48)

Where θ is expressed in the body frame and represents the rotation from the current best estimate of theattitude to the true attitude. As was observed earlier, this means that θ

−≡ 0. The implications of this

selection are critical to the proper treatment of attitude in the filter and will be discussed in detail in thefollowing sections.

In addition to estimating the attitude, the SOAR Filter also estimates other vehicle parameters that aredefined by the parameter vector β. Therefore, the state vector for the SOAR Filter is given by

x =[θβ

](49)

8

Reformulation of SOAR Objective Function

Recalling the relation between the covariance matrix and the Fisher information matrix from Eq. 24, theobjective function from Eq. 47 may be expanded as

Min L =12

[δθTk F−θθ,kδθk + δθTk F−θβ,kδβk + δβTk F−βθ,kδθk + δβTk F−ββ,kδβk

](50)

+12

r+m∑i=r+1

1σ2φ,i

∥∥∥(ei)B,i − TIB (ei)I∥∥∥2

The subscript k is temporarily dropped in the following derivation to abbreviate notation. Additionally, thepartitioned Fisher information matrix in Eq. 50 is given by

P−1 ≈ F =[

Fθθ FθβFβθ Fββ

](51)

Care must be taken here not to confuse the Fθθ generated from the inverse of the 3 × 3 attitude covariancematrix in Eq. 28 with the Fθθ described in Eq. 50 and Eq. 51. The Fθθ in Eq. 50 and Eq. 51 is the upper 3×3matrix of the inverse of the entire covariance matrix, which contains both attitude and non-attitude states.

Further, recognizing that Fθβ = FTβθ and using the form of the Wahba Problem cost function from Eq. 9,

Min L =12δθTF−θθδθ + δθTF−θβδβ +

12δβTF−ββδβ + λm0 − qTKmq (52)

where Km is the measurement Davenport matrix constructed using only the new measurements. If the mea-surements are unit vectors, then Km is computed as in the regular Wahba Problem. If the measurementsare quaternions (as is sometimes the case for commercially available star trackers that provide a quaternionestimate instead of the raw measurements), then the measured attitude and measurement covariance matrixare used to create Bm from Eq. 29. Now, Bm may be used to compute Km from Eq. 10 and Eq. 11.

Now, recalling the following relation for the inverse of a partitioned matrix

[A11 A12

A21 A22

]−1

=

[A11 − A12A−1

22 A21

]−1 −[A11 − A12A−1

22 A21

]−1 A12A−122

−A−111 A12

[A22 − A21A−1

11 A12

]−1 [A22 − A21A−1

11 A12

]−1

(53)

it may be shown through a little algebra that

Fθθ = P−1θθ + FθβF−1

ββFβθ (54)

Therefore, the first term in Eq. 52 may be rewritten as

12δθTF−θθδθ =

12δθT

(P−θθ)−1

δθ +12δθTF−θβ

(F−ββ

)−1

F−βθδθ (55)

Combining Eq. 9 and Eq. 33, and inserting the result in Eq. 55, it may be shown that

12δθTF−θθδθ = λ−0 − qTK−q− J(ˆq−) +

12δθTF−θβ

(F−ββ

)−1

F−βθδθ (56)

The a-priori Davenport matrix, K−, may be found by using Eq. 10 and Eq. 11 and the a-priori attitudeprofile matrix, B−. The a-priori attitude and covariance may be used to find B− from Eq. 29,

B− =[

12

tr[(

P−θθ)−1]

I3×3 −(P−θθ)−1]

TIB

(57)

9

where TIB

is found through Eq. 8 using ˆq− as the attitude. Substituting this result into the above objectivefunction from Eq. 52 and combining like terms,

Min L =(λm0 + λ−0

)− J

(ˆq−)− qT

(Km + K−

)q +

12δθTF−θβ

(F−ββ

)−1

F−βθδθ (58)

+ δθTF−θβδβ +12δβTF−ββδβ

To simplify, note the following relations associated with rotations and quaternion multiplication[δθ/2

1

]≈ δq = q⊗

(ˆq−)−1

=[

qq4

]⊗[−q−

q−4

](59)

Using the definition of the quaternion product, proceed by showing that

[qq4

]⊗[−q−

q−4

]=

[q−4 q− q4q− + q× q−

q4q−4 + qT q−

]=

q−4 I3×3 −[q−×

]−q−(

q−)T

q−4

q (60)

Therefore, define the 3× 4 matrix Ψ(ˆq−k)

as

Ψ(ˆq−)

=[q−4 I3×3 −

[q−×

]−q−

](61)

such thatδθ = 2Ψ

(ˆq−)

q (62)

Now, substituting this identity into Eq. 58 allows the objective function to be written as

Min L =(λm0 + λ−0

)− J

(ˆq−)− qT

(Km + K−

)q + 2qTΨ

(ˆq−)T F−θβ

(F−ββ

)−1

F−βθΨ(ˆq−)

q (63)

+ 2qTΨ(ˆq−)T F−θβδβ +

12δβTF−ββδβ

Finding the Optimal State Update in SOAR

Using a similar approach as for the optimal solution for the regular Wahba problem, adjoin the attitudequaternion unity norm constraint to the objective function using a Lagrange multiplier, λ, such that

Min L (q,β, λ) =(λm0 + λ−0

)− J

(ˆq−)− qT

(Km + K−

)q (64)

+ 2qTΨ(ˆq−)T F−θβ

(F−ββ

)−1

F−βθΨ(ˆq−)

q

+ 2qTΨ(ˆq−)T F−θβδβ +

12δβTF−ββδβ + λ

(qT q− 1

)Taking the differential of L (q,β, λ),

dL (q,β, λ) ={−2qT

(Km + K−

)+ 4qTΨ

(ˆq−)T F−θβ

(F−ββ

)−1

F−βθΨ(ˆq−)

(65)

+2δβTFβθΨ(ˆq−)

+ 2λqT}dq

+{

2qTΨ(ˆq−)T F−θβ + δβTF−ββ

}dβ +

{qT q− 1

}dλ = 0

where the first three components of dq are independent differentials and the fourth component is a dependentdifferential (this distinction is required because of the quaternion unity norm constraint). The differentialshave been combined back into dq for brevity. Setting the coefficient of dλ to zero produces

qT q− 1 = 0 (66)

10

which simply states that the quaternion unity norm constraint must be satisfied. Assuming no constraintson the non-attitude states, dβ is an independent differential. Therefore, if dJ (q,β, λ) is to be zero for anarbitrary dβ, the coefficient of dβ must be zero,

2qTΨ(ˆq−)T F−θβ + δβTF−ββ = 0 (67)

Rearranging and solving for δβ (again recall that Fββ is symmetric),

δβ = −2(

F−ββ)−1

F−βθΨ(ˆq−)

q (68)

Now move to the term involving dq = [dqT dq4]T in Eq. 65. Because dq is an independent differential, itscoefficients must be zero. Additionally, the Lagrange multiplier λ is picked such that the coefficient of thedependent differential dq4 is also zero. Therefore, the coefficient of dq in Eq. 65 must be zero,

− 2qT(Km + K−

)+ 4qTΨ

(ˆq−)T F−θβ

(F−ββ

)−1

F−βθΨ(ˆq−)

+ 2δβTF−βθΨ(ˆq−)

+ 2λqT = 0 (69)

Substituting the optimal value of δβ from Eq. 68,

−qT(Km + K−

)+ 2qTΨ

(ˆq−)T F−θβ

(F−ββ

)−1

F−βθΨ(ˆq−)

(70)

− 2qTΨ(ˆq−)T F−θβ

(F−ββ

)−1

F−βθΨ(ˆq−)

+ λqT = 0

The middle two terms cancel, leaving

−qT(Km + K−

)+ λqT = 0 (71)

Therefore, if K+ is given byK+ = Km + K− (72)

then Eq. 71 may be rewritten as− qTK+ + λqT = 0 (73)

Recognizing that K+ is symmetric and that the solution to Eq. 73 is the a-posteriori attitude estimate, whichallows q to be replaced by ˆq+,

K+ ˆq+ = λˆq+ (74)

The optimal ˆq+ may be found using any solution method to the normal Wahba Problem. Once ˆq+ is known,δβ may be found using Eq. 68. The updated parameter vector, therefore, is simply given by

β+ = β− + δβ = β− − 2(

F−ββ)−1

F−βθΨ(ˆq−)

ˆq+ (75)

Finding the Optimal Covariance Update in SOAR

Now that the optimal state update has been derived, it is necessary to compute the corresponding update tothe state covariance matrix. Begin by rewriting the objective function from Eq. 58 as

Min L =(λm0 + λ−0

)− J

(ˆq−)− tr

[TIB(Bm + B−

)T ]+

12δθTF−θβ

(F−ββ

)−1

F−βθδθ (76)

+ δθTF−θβδβ +12δβTF−ββδβ

As was done in finding the covariance for the regular Wahba Problem in Eq. 26, rewrite the attitude term interms of δθ, where δθ is a small angle rotation from the best estimate of the body frame,

Min L =(λm0 + λ−0

)− J

(ˆq−)− tr

[(I3×3 + [−δθ×] +

12

[−δθ×]2)

TIB

(Bm + B−

)T](77)

+12δθTF−θβ

(F−ββ

)−1

F−βθδθ + δθTFθβδβ +12δβTFββδβ

11

If a new variable B+ is defined asB+ = Bm + B− (78)

then straightforward differentiation of Eq. 77 will yield the following results

F+θθ =

∂2

∂δθ ∂δθL (δθ) = tr

[TIB

(B+)T ] I3×3 − TI

B

(B+)T

+ F−θβ(

F−ββ)−1

F−βθ (79)

F+θβ =

(F+βθ

)T= F−θβ (80)

F+ββ = F−ββ (81)

This makes sense in that the only new information available is in the attitude estimate. The present imple-mentation of the SOAR Filter only allows line-of-sight unit vector measurements and attitude measurements,therefore improved estimates of the parameter vector β is only possible through knowledge of the dynamicsand correlation between the estimates of the attitude and parameters. Further, it is interesting to note therelationship between Eq. 79 and Eq. 28. The first two terms in Eq. 79 are what would be expected from Eq.28 to go from B+ to F+

θθ in a system where only attitude states are estimated. The third term, which is new,follows from the inclusion of non-attitude states.

To compute the a-posteriori covariance matrix, recall the relation for the inverse of a segmented matrixgiven in Eq. 53. From this relation, it is straightforward to see that

P+θθ =

[F+θθ − F+

θβ

(F+ββ

)−1

F+βθ

]−1

(82)

P+ββ =

[F+ββ − F+

βθ

(F+θθ

)−1 F+θβ

]−1

(83)

P+βθ =

(F−ββ

)−1

F−βθP+θθ (84)

The same results may also be found using the standard definition of the covariance matrix (i.e. a co-variance/uncertainty approach rather than an information approach). The details of this equivalence are notpresented here for brevity.

Propagation of State and Covariance for SOAR

The only remaining step is to propagate the a-posteriori state estimate and covariance at time tk forward intime to create the a-priori state estimate and covariance at time tk+1. Consider a system with the followingnonlinear dynamic model:

x = f (x(t),u(t), t) + G(t)w(t) (85)

˙x = f (x(t),u(t), t) (86)

Proceed by taking the Taylor series expansion of f (x,u, t) about the estimated trajectory,

x =

[f (x(t),u(t), t) +

(∂f∂x

∣∣∣∣x=x(t)

)(x(t)− x(t)) +H.O.T.

]+ G(t)w(t) (87)

Defining the matrix F (x(t), t) as

F (x(t), t) =∂f∂x

∣∣∣∣x=x(t)

(88)

and establishing the following state error vectors,

δx = x− x δx = x− ˙x (89)

12

allows for Eq. 87 to be rewritten as the following linearized model:

δx = F (x, t) δx + G(t)w(t) (90)

This is a standard result that seen in the implementation of the EKF; it is well-known and discussed thoroughlyin the literature.25–27 Such a linear model is known to admit a solution of the form

δx(t) = Φ (t, t0) δx(t0) +∫ t

t0

Φ (t, τ) G(τ)w(τ) dτ (91)

where Φ (t, t0) is the state transition matrix from t0 to t. Therefore, assuming that the state error and theprocess noise are uncorrelated, i.e. E

[δx(t)w(t)T

]= 0, it may be shown that

P = E[δxδxT

]= Φ (t, t0) P0Φ (t, t0)T +

∫ t

t0

Φ (t, τ) G(τ)Q(τ)GTΦ (t, τ)T dτ (92)

whereQ(τ) = E

[w(τ)w(τ)T

](93)

Taking the time derivative of Eq. 92 (which requires the application of Leibniz’s Rule) and recognizing thatΦ = FΦ, yields the well known equation for the propagation of the covariance matrix typically seen in thediscrete-continuous EKF:

P (t) = F (x, t) P (t) + P (t) F (x, t)T + G (t) Q (t) G (t)T (94)

This expression is used to propagate the covariance between measurement updates in SOAR.

CONSTRUCTION OF SOAR ALGORITHM

All the theoretical components required to create the SOAR Filter are now in place. A brief summary ofthe SOAR Filter is provided in the following paragraphs.

Given an a-priori attitude estimate and covariance matrix, compute the a-priori attitude profile matrix fromEq. 29. Use this to create the a-priori Davenport matrix using Eq. 10 and Eq. 11.

Next, compute the measurement attitude profile matrix. If the measurements are from unit vectors, createthe measurement attitude profile matrix using Eq. 7. If, on the other hand, a measured attitude is available,compute the measurement attitude profile matrix using Eq. 29. Because the Davenport matrix K is a linearhomogeneous function of B, the measurement attitude profile matrices for all measurement sources (whetherthey come from unit vector measurements or quaternion measurements) may be added before using Eq. 10and Eq. 11 to get the measurement Davenport matrix at any given epoch.

The SOAR Davenport matrix may now be computed through Eq. 72. Use any solution method to theWahba Problem to solve for the optimal attitude. With the optimal attitude found, use Eq. 68 to compute theupdate to the parameter vector β. Finally compute the updated covariance matrix by taking the inverse of theFisher information matrix found using Eq. 79 through Eq. 81.

To state vector may be propagated to the next measurement epoch using Eq. 86, and the covariance matrixmay be propagated using Eq. 94. Once at the next epoch, the procedure is repeated.

COMPARISON OF SOAR WITH OTHER ATTITUDE FILTERS

Comparison of the SOAR Filter with the Multiplicative Extended Kalman Filter

The central idea behind the MEKF is to use the unconstrained three-parameter representation δθ instead ofthe constrained four-parameter attitude quaternion in the state vector of a regular EKF.15, 28 This means thatthe MEKF estimates δθ and then updates the attitude quaternion by

q+ ≈[δθ/2

1

]⊗ q− (95)

13

This update only maintains the quaternion unity norm constraint to first order, making it common practice tonormalize the a-posteriori quaternion, q+ = q+/ ‖q+‖.

An important similarity between the MEKF and the SOAR Filter is the objective function used to findthe optimal update. Both the MEKF and SOAR Filter explicitly find the minimum variance state updateand therefore minimize J = tr

[P+]. Therefore, the MEKF and the SOAR Filter are expected to behave in

a similar fashion when the errors are small and the MEKF linearization assumptions are good. Simulationconfirms that this is the case. A second important similarity is that both the MEKF ans SOAR can estimatenon-attitude states.

Unlike the MEKF, the SOAR Filter deals directly with the nonlinear nature of attitude representations andthe Wahba Problem. Rather than linearizing the update equation as is required in the MEKF, the SOAR Filterexplicitly solves the constrained quadratic cost function. This allows the SOAR Filter to directly computethe attitude quaternion. Additionally, because the SOAR Filter approaches the problem from an informationperspective, it is capable of dealing with scenarios where no a-priori attitude estimate is available. In theMEKF, an a-priori estimate is required to perform the linearization.

Comparison of SOAR with Filter QUEST and REQUEST

Filter QUEST7, 8 and REQUEST9 are both based on sequentialization of the Wahba Problem. Performingthis sequentialization produces the following recursive filter equation (using the same nomenclature as forthe SOAR Filter) that is called “Filter QUEST” in the literature,7

B+k+1 = Bmk+1 + αk+1Φ (tk+1, tk) B+

k (96)

where αk+1 ∈ [0 1] and represents reduction in knowledge associated with the propagation. This is a classicfading memory filter25 where αk+1 = 0 causes the filter to ignore the old measurements and αk+1 = 1makes the filter think there is no process noise and the measurements are combined as if they were all takensimultaneously. Therefore, heuristically speaking, the fading memory factor αk+1 should look somethinglike

∑σ(tk)2φ,i/σ(tk+1)2φ,i. Note, however, that the Filter QUEST algorithm picks a suboptimal value for

αk+1 to reduce complexity and increase algorithm speed.

Because the Davenport matrix K is a linear homogeneous function of attitude profile matrix B, Eq. 96 maybe written in terms of K instead of B. This represents the fundamental difference between the Filter QUESTand REQUEST algorithm: Filter QUEST is a sequentialization on B and REQUEST is a sequentializationon K. Shuster provides an excellent discussion of the relation between these two algorithms,10 which areshown to be equivalent when the same selection is made for αk+1. Therefore, the conclusions made in thecomparison with Filter QUEST also hold for REQUEST.

Although not generally true, it is possible to analytically select the forgetfulness factor (αk+1 in Eq. 96)under specific scenarios. Consider a system with the following assumptions:

1. Only three unit vector measurements are available2. Each unit vector measurement lies along one of the coordinate axes with a measurement standard

deviation of σφ3. The process noise is a zero mean white noise process with Q = qI3×3

4. Steady state filter covariances5. The attitude covariance matrix is diagonal, Pθθ = pI3×3

Shuster demonstrates7, 8 that under these conditions the optimal αk+1 may be analytically computed as

αk+1 =σ2φ/q + 1−

√1 + 2σ2

φ/q

σ2φ/q

(97)

This simple equation for αk+1 provides an excellent approximation of the more general optimal fading mem-ory factor. Note, however, that a typical star tracker may track many more than three stars and all the unit

14

vector observations will be separated by an angle less than the camera FOV (i.e. it is not possible to obtainmeasurements along each of the coordinate axes). This means that the αk+1 given in Eq. 97 is suboptimal inthe general sense.

The SOAR Filter removes all five of the assumptions made in computing αk+1. As discussed above, theSOAR Filter provides the optimal state update under the assumption that state vector error and measurementerrors are normally distributed. The SOAR Filter is also capable of estimating other states in addition toattitude (recall that one of the major drawbacks to Filter QUEST and REQUEST is their inability to alsoestimate non-attitude states). These advantages, however, come at the expense of additional computations.

Comparison of SOAR with Optimal-REQUEST

The Optimal-REQUEST algorithm11 finds the attitude that minimizes the objective function

Min J = tr[E[∆K (∆K)T

]](98)

where K = K + ∆K. Recognizing that ∆K is symmetric, it is straightforward to show that

λ2max (∆K) = λmax

(∆K (∆K)T

)= ‖∆K‖22 ≤ ‖∆K‖2F = tr

[∆K (∆K)T

](99)

Recalling an important result from eigenvalue-eigenvector stability,29 the estimate of the attitude quaternionmay be written as,

ˆq = q +4∑j=2

[uTj ∆Kqλ1 − λj

]uj +H.O.T. (100)

where uj are the eigenvectors and λ1 ≥ λ2 ≥ λ3 ≥ λ4 are the eigenvalues of the true Davenport ma-trix, K. Also recall that the true quaternion is associated with the largest eigenvalue of K (i.e. u1 ≡ q).Because all uj are assumed to have unity norm and ∆K must be a symmetric matrix, it is straightfor-ward to show (Jordan decomposition) that uTj ∆Kq ≤ λmax (∆K). Because it can also be shown that

tr [Pθθ] = 4tr[E[(

ˆq− q) (

ˆq− q)T ]]

to first order, then

tr [Pθθ] ≤ 4E[λ2max (∆K)

] 4∑j=2

4∑l=2

[1

(λ1 − λj) (λ1 − λl)

]tr[ujuTl

](101)

Note that the double summation is only a function of the true attitude and, therefore, may be moved outsideof the expected value operator

tr [Pθθ] ≤ 4E[‖∆K‖22

]ζ(K) ≤ 4E

[‖∆K‖2F

]ζ(K) = 4tr

[E[∆K (∆K)T

]]ζ(K) (102)

where

ζ (Kk) =4∑j=2

4∑l=2

[1

(λ1 − λj) (λ1 − λl)

]tr[ujuTl

](103)

The SOAR Filter, because it provides a maximum likelihood estimate and assumes normally distributed ran-dom variables, provides a minimum variance estimate of the attitude (i.e. the SOAR Filter minimizes tr [Pθθ]).Therefore, Optimal-REQUEST and the SOAR Filter provide the minimum to two different objective func-tions. The objective functions of the SOAR Filter and Optimal-REQUEST are related by Eq. 102. The SOARFilter has the advantage that it explicitly addresses the objective of providing a maximum likelihood (or min-imum variance) estimate of the spacecraft attitude. Finally, because Optimal-REQUEST is an extension ofthe REQUEST algorithm (with optimal αk+1 chosen to minimize Eq. 98), the Optimal-REQUEST algorithmis also unable to estimate non-attitude states.

15

Comparison of the SOAR Filter with Extended-QUEST

The Extended-QUEST algorithm is the closest of the existing attitude filtering methods to the SOAR Filter.The Extended-QUEST algorithm is a solution to the following optimization problem,12

JExt−QUEST =12

qTKmq +12[Rqq

(q− q−

)]T [Rqq (q− q−)]

(104)

+12[Rβq

(q− q−

)+ Rββ

(β − β−

)]T [Rβq (q− q−)

+ Rββ(β − β−

)]where Rxx are the square root information matrices that come from the left QR factorization of the informa-tion matrix at each epoch. The Extended-QUEST algorithm in Eq. 104 has been rewritten using the samenomenclature as in the SOAR Filter to facilitate easy comparison. Strong parallels between the SOAR Filterand Extended-QUEST are immediately evident by comparing the SOAR Filter objective function in Eq. 52with the Extended-QUEST objective function in Eq. 104. These similarities produce similar results for thestate vector update. The Extended-QUEST parameter vector update is given by

β+

= β−− R−1

ββRβq[q− ˆq−

](105)

which is clearly similar to the SOAR Filter parameter vector update from Eq. 75. The attitude update,however, is significantly different than in the SOAR Filter. Extended-QUEST finds the optimal a-posterioriattitude through the solution to [

Km + RTqqRqq]

q− RTqqRqqq− = λq (106)

The introduction of the constant vector −RTqqRqqq− makes the solution to this problem more difficult thanthe simple eigenvector-eigenvalue problem in Eq. 74 for the SOAR Filter.

Despite strong similarities, there are numerous noteworthy differences between Extended-QUEST and theSOAR Filter. The first significant difference is that Extended-QUEST is a square-root information filter and,therefore, requires the use of the square-root information matrices that are obtained through QR factorization.Because the SOAR Filter uses the information matrix directly, QR factorization is not required. Although thisreduces computations in the SOAR Filter, it places the SOAR Filter at a disadvantage from the standpoint ofnumerical precision relative to Extended-QUEST.

The second significant difference between the SOAR Filter and Extended-QUEST is how they expressthe attitude error. Extended-QUEST treats the attitude quaternion error from an additive standpoint, ratherthan a multiplicative standpoint. Although the term “additive” is used here, it is important to note thatthe Extended-QUEST algorithm explicitly enforces the quaternion unity norm constraint in generating thequaternion update. Therefore, the update here should not be confused with additive updates that do notpreserve the unity norm constraint.

The difference between using an additive or multiplicative quaternion update has two significant impli-cations. The first is that the covariance associated with Rqq is related to δqA = q − ˆq−, rather than themultiplicative attitude quaternion error described in Eq. 16: δq = q ⊗ ˆq−. This changes the physicalsignificance of the resulting Pqq in Extended-QUEST.

The second major implication of using the additive quaternion error is related to the optimal update. Theadditive representation of the attitude quaternion error requires the solution of a more general objective func-tion. When the first differential of Eq. 104 is set to zero, Extended-QUEST requires the solution to Eq. 106,which is of the following form

(K− λI4×4) q + g = 0 (107)

Psiaki demonstrates a robust method for finding the desired solution to this problem.12 Solving Eq. 107,however, requires more computation than simply finding the largest eigenvalue/eigenvector of K using amethod such as QUEST or ESOQ. This means that solving for the optimal attitude in the SOAR Filter (asdescribed in Eq. 74) will be faster than finding the optimal attitude in Extended-QUEST.

16

EXAMPLE: ESTIMATION OF ATTITUDE AND GYRO BIAS

Consider a scenario where a filter is required to estimate the attitude of a spacecraft and the gyro bias, bg .Rather than have the filter estimate the angular velocity, which would require models for external torques,the gyro measurements of angular velocity are taken to be truth. This is a common practice for spacecraftattitude filters - sometimes referred to as “flying the gyros.” Therefore, assume a gyro measurement modelgiven by30

ω = [I3×3 + Sg + Γg] (ω + bg + wω) (108)

where Sg is the scale-factor error matrix, Γg is the misalignment error matrix, ω is the measured angularvelocity, bg is the gyro bias, and wω is zero mean white noise. This general model is simplified by assumingthat there is no scale-factor error, Sg = 03×3, and that there is no misalignment error, Γg = 03×3. Underthese conditions, the gyro measurement model becomes

ω = ω + bg + wω (109)

Further assume that the gyro bias behaves as a first-order Gauss-Markov process:25, 26

bg = −1τ

bg + wb (110)

where τ is the correlation time and wb is a zero mean white noise process. It may further be shown that thevariance of wb is 2σ2/τ .25 Therefore,

ω = E [ω] = ω − bg (111)

˙bg = E[bg]

= −1τ

bg (112)

Two test cases are considered. The first is a scenario with typical sensor errors and noise. In the firstscenario, only SOAR Filter errors and covariances are shown. The second scenario is a stressing case thatcontains very large a-priori attitude errors and bias errors. The measurement noise is also increased. Resultsfor the second scenario are presented for the SOAR Filter, the MEKF, and Filter-QUEST.

The first test case has performance specifications as described in Table 1. The results for this test case areshown in Fig. 1. Star tracker measurements were made available to the filter at a frequency of 1 Hz. The factthat almost all the results for the bias error lie within the ±1 σ bounds indicates that the process noise couldbe reduced from the theoretically predicted value.

Table 1. Summary of noise levels for SOAR Filter validations (scenario 1).

Description Initial Conditions or Standard Deviation

a-priori attitude error Rotation of 1 deg about eθ = [0.577 0.577 0.577]T

a-priori bias error bg − bg = [0.25 − 0.5 0.375]T deg/hrStar observation unit vector error σφ,i = 10 arcsecGyro bias Gauss-Markov error σ = 0.05 deg/hr with correlation time of τ = 1 hrGyro measurement error σω = 0.05 deg/hr

The second test case has performance specifications as described in Table 2. The results for this test caseare shown in Fig. 2. The exact same noise and observations are applied to each filter technique in order tofacilitate direct comparison. Under these extreme conditions, Filter-QUEST is unable to converge because itis not estimating the gyro bias and because of its suboptimal fading memory factor. Both the MEKF and theSOAR Filter converge for this test case. Despite being given equivalent a-priori information and processingthe exact same measurements, significant differences are observed prior to filter convergence. In all cases, theSOAR Filter performs as well or better than the MEKF. In both the attitude estimation and the bias estimation,the SOAR Filter has lower overshoot (the overshoot is induced by the very poor a-priori guess) and faster

17

0 100 200 300 400 500-4

-2

0

2

4

roll

err

or,

arc

sec

SOAR attitude estimate error

1-sigma bounds on estimated attitude

-2

0

2

4

pitch

err

or,

arc

sec

0 100 200 300 400 500-1

0

1

roll

bia

s,

deg/h

r

SOAR bias estimate error

1-sigma bounds on estimated bias

0

1

pitch

bia

s,

deg/h

r

0 100 200 300 400 500-4

-2

pitch

err

or,

arc

sec

0 100 200 300 400 500-4

-2

0

2

4

time, sec

yaw

err

or,

arc

sec

0 100 200 300 400 500-1p

itch

bia

s,

deg/h

r

0 100 200 300 400 500-1

0

1

time, sec

yaw

bia

s,

deg/h

ra) attitude estimate error a) bias estimate error

Figure 1. Performance of the SOAR Filter in scenario.

convergence. As was noted before, however, the SOAR Filter and the MEKF minimize the same objectivefunction and are expected to exhibit similar behavior once the filter converges. The results shown in Fig. 2support this observation. As an example, a zoom-in of the roll-axis attitude error (see Fig. 3) shows that theSOAR Filter and the MEKF exhibit identical performance after the filter converges. Similar results may beseen for the other attitude errors and all of the bias errors.

Table 2. Summary of noise levels for SOAR Filter validations (scenario 2).

Description Initial Conditions or Standard Deviation

a-priori attitude error Rotation of 179 deg about eθ = [0.577 0.577 0.577]T

a-priori bias error bg − bg = [90 − 90 67.5]T deg/hrStar observation unit vector error σφ,i = 1 degGyro bias Gauss-Markov error σ = 0.5 deg/hr with correlation time of τ = 1 hrGyro measurement error σω = 0.5 deg/hr

CONCLUSIONS

A new nonlinear attitude filter called the Sequential Optimal Attitude Recursion (SOAR) Filter has beendeveloped. This filter is based on maximum likelihood estimation and is capable of dealing with both attitudeand non-attitude states. In it’s current formulation, however, the SOAR Filter is only able to accept two typesof measurements: unit vector measurements and complete attitude estimates. The SOAR Filter algorithm wascompared to numerous other attitude filtering techniques from a theoretical standpoint. Then, an examplesimulation was performed where the filter was required to estimate attitude and gyro bias. The performanceof the SOAR Filter was shown to be advantageous relative to previously existing filters in some scenarios.

18

0 100 200 300 400 500-1

-0.5

0

0.5

1ro

llerr

or,

deg

0

0.5

1

pitch

err

or,

deg

MEKF

Filter-QUEST

SOAR

0 100 200 300 400 500-0.05

0

0.05

0.1

roll

bia

serr

or,

deg/s

ec

0.05

0.1

pitch

bia

serr

or,

deg/s

ec

MEKF

Filter-QUEST

SOAR

0 100 200 300 400 500-1

-0.5

pitch

err

or,

deg

0 100 200 300 400 500-1

-0.5

0

0.5

1

time, sec

yaw

err

or,

deg

0 100 200 300 400 500-0.05

0

pitch

bia

serr

or,

deg/s

ec

0 100 200 300 400 500-0.05

0

0.05

0.1

time, sec

yaw

bia

serr

or,

deg/s

ec

a) attitude estimate error for different filters a) bias estimate error for different filters

Figure 2 Performance comparison of the MEKF, Filter QUEST, and the SOAR Filterin of the SOAR Filter in scenario 2 (stressing case with large errors).

0

0.1

0.2

0.3

0.4

0.5

roll

err

or,

deg

MEKF

Filter-QUEST

SOAR

0 50 100 150 200 250 300 350 400 450 500-0.5

-0.4

-0.3

-0.2

-0.1roll

err

or,

deg

time, sec

Figure 3. Zoom-in of roll-axis attitude error for scenario 2 (stressing case with large errors).

ACKNOWLEDGMENT

The authors thank the following individuals for reviewing this manuscript and for providing valuable com-ments, critiques, and suggestions: Sebastian Munoz of the University of Texas at Austin; John Crassidis ofthe University at Buffalo, State University of New York; and Mark Psiaki of Cornell University.

REFERENCES

[1] G. Wahba, “A Least Squares Estimate of Satellite Attitude,” SIAM Review, Vol. 7, July 1965, p. 409.

19

[2] F. Markley and D. Mortari, “Quaternion Attitude Estimation Using Vector Observations,” The Journalof the Astronautical Sciences, Vol. 48, April-September 2000, pp. 359–380.

[3] M. Shuster and S. Oh, “Three-Axis Attitude Determination from Vector Observations,” Journal of Guid-ance and Control, Vol. 4, January-February 1981, pp. 70–77.

[4] D. Mortari, “ESOQ: A Closed-Form Solution to the Wahba Problem,” The Journal of the AstronauticalSciences, Vol. 45, April-June 1997, pp. 195–204.

[5] D. Mortari, “ESOQ-2 Single-Point Algorithm for Fast Optimal Spacecraft Attitude Determination,”Advances in the Astronautical Sciences, Vol. 95, No. 2, 1997, pp. 817–826.

[6] F. Markley, “Attitude Determinatio Using Vector Observations and the Singular Value Decomposition,”The Journal of the Astronautical Sciences, Vol. 36, No. 3, 1988, pp. 245–258.

[7] M. Shuster, “A Simple Kalman Filter and Smoother for Spacecraft Attitude,” Journal of the Astronauti-cal Sciences, Vol. 37, January-March 1989, pp. 89–106.

[8] M. Shuster, “New Quests for Better Attitudes,” Flight Mechanics/Estimation Theory Symposium, NASAGoddard Space Flight Center, 21-23 May 1991.

[9] I. Bar-Itzhack, “REQUEST: A Recursive QUEST Algorithm for Sequential Attitude Determination,”Journal of Guidance, Control, and Dynamics, Vol. 19, Sept-Oct 1996, pp. 1034–1038.

[10] M. Shuster, “Filter QUEST or REQUEST,” Journal of Guidance, Control, and Dynamics, Vol. 32,March-April 2009, pp. 643–645.

[11] D. Choukroun, I. Bar-Itzhack, and Y. Oshman, “Optimal-REQUEST Algorithm for Attitude Determi-nation,” Journal of Guidance, Control, and Dynamics, Vol. 27, May-June 2004, pp. 418–425.

[12] M. Psiaki, “Attitude-Determination Filtering via Extended Quaterion Estimation,” Journal of Guidance,Control, and Dynamics, Vol. 23, March-April 2000, pp. 206–214.

[13] F. Markley, “AttitudeDetermination and Parameter Estimation Using Vector Observations: Theory,” TheJournal of the Astronautical Sciences, Vol. 37, January-March 1989, pp. 41–58.

[14] F. Markley, “AttitudeDetermination and Parameter Estimation Using Vector Observations: Applica-tion,” The Journal of the Astronautical Sciences, Vol. 39, July-September 1991, pp. 367–381.

[15] J. Crassidis, F. Markley, and Y. Cheng, “Survey of Nonlinear Attitude Estimation Methods,” Journal ofGuidance, Control, and Dynamics, Vol. 30, No. 1, 2007, pp. 12–28.

[16] J. Christian and E. Lightsey, “High-Fidelity Measurement Models for Optical Spacecraft Navigation,”Institute of Navigation GNSS 2009 Conference, Savannah, GA, 22-25 September 2009.

[17] M. Shuster, “Maximum Likelihood Estimation of Spacecraft Attitude,” The Journal of the AstronauticalSciences, Vol. 37, Jan-March 1989, pp. 79–88.

[18] J. Wertz, ed., Spacecraft Attitude Determination and Control. D. Reidel Publishing Company, 1985.[19] D. Hull, Optimal Control Theory for Applications. Springer, 2003.[20] J. Keat, “Analysis of Least-Squares Attitude Determination Routine DOAOP,” Tech. Rep. CSC/TM-

77/6034, Computer Sciences Corp, Feb 1977.[21] D. Mortari, “n-Dimensional Cross Product and its Application to the Matrix Eigenanalysis,” Journal of

Guidance, Control, and Dynamics, Vol. 20, No. 3, 1997, pp. 509–515.[22] H. Sorenson, Parameter Estimation: Principles and Problems. Marcel Dekker, Inc, 1980.[23] A. Rencher, Multivariate Statistical Inference and Applications. John Wiley and Sons, Inc., 1998.[24] D. Morrison, Multivariate Statistical Methods, Fourth Edition. Brooks/Cole, 2005.[25] A. Gelb, Applied Optimal Estimation. The MIT Press, 1974.[26] R. Brown and P. Hwang, Introduction to Random Signals and Applied Kalman Filtering: With MATLAB

Exercised and Solutions, Third Edition. John Wiley and Sons, 1997.[27] J. Crassidis and J. Junkins, Optimal Estimation of Dynamical Systems. CRC Press LLC, 2004.[28] F. Markley, “Attitude Error Representations for Kalman Filtering,” Journal of Guidance, Control, and

Dynamics, Vol. 26, March-April 2003, pp. 311–317.[29] K. Atkinson, An Introduction to Numerical Analysis, Second Edition. John Wiley & Sons, 1989.[30] S. Munoz, J. Christian, and E. Lightsey, “Development of an End to End Simulation Tool for Au-

tonomous Cislunar Navigation,” AIAA Guidance, Navigation, and Control Conference, Chicago, IL,10-13 August 2009.

20