38 views

Uploaded by gantebras

- nm-2068
- SENSORLESS VECTOR CONTROL OF BLDC USING EXTENDED KALMAN FILTER
- SEM 1 MCA 11 Mathematics
- Loly Complex Systems 17-1-2
- Pr Final
- Spatio-Temporal video monitoring
- IAS Mains Mathematics 1992 IAS Mains Mathematics 2014 2IN1
- linear system 2 homework
- Prediction Error Methods for Time-domain Blind Identification of Multichannel FIR Filters
- Algebra-Through-Practice-Volume-2-Matrices-and-Vector-Spaces-A-Collection-of-Problems-in-Algebra-with-Solutions.pdf
- eigenv5b
- Linear Mathematics and Vector Calculus Examination
- Eigens by Row(1)
- AFEM.Ch12
- Maxima Book Chapter 7
- 00A Exploring Statistical Arbitrage Opps YE-XIAOXIA
- Kalman
- CH3.PDF
- Root
- FinalC_Fall06

You are on page 1of 20

John A. Christian

A new nonlinear attitude lter 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. The

algorithm can accept either individual unit vector measurements or quaternion measure-

ments. This new algorithm is compared with existing attitude ltering routines, including

the 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 much

attention since the 1960s. The most well known formulation of this problem was given by Grace Wahba in

1965

1

and is commonly referred to as the Wahba Problem. If the Wahba Problem is written in terms of

the attitude quaternion, an analytic solution exists in the form of the solution to an eigenvalue-eigenvector

problem. The optimal attitude is simply given by the eigenvector associated with the largest eigenvalue.

Numerous algorithms have been developed over the years to nd fast and robust solutions to this problem.

2

Some of the most well known algorithms include the QUaternion ESTimator (QUEST),

3

the EStimator of

the 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 in

this 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 the

traditional Wahba Problem into a framework that allows for the creation of an optimal attitude lter.

One of the most common methods of attitude ltering in modern systems is the Multiplicative Extended

Kalman Filter (MEKF). This method uses the four-component attitude quaternion to represent the attitude, but

a three-component representation of the attitude in the lter. The MEKF is structured this way because while

the 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 Kalman

lter difcult. Additionally, a small angle assumption allows the three-component attitude representation to

be cast in a form consistent with the additive nature of the Kalman lter (i.e. x = x + x).

Unlike the MEKF, sequential Wahba Problem lters estimate the full quaternion without requiring solu-

tions to be computed as small angle deviations from a reference attitude. The batch solutions to the Wahba

Problem 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 ltering. Shuster provided the

earliest 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, Shuster

10

demonstrated that although the

Filter 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 fading

memory lters. Shuster demonstrates

7, 8

that under a specic set of conditions, the optimal fading memory

Assistant Instructor, Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, 1 University

Station, 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 an

excellent 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 computed

at the expense of a little more computation time.

An approach for the optimal blending of the new attitude measurements with old attitude estimates directly

in the Wahba Problem solution framework was proposed in 2004 by Choukroun with the Optimal-REQUEST

algorithm.

11

This algorithm, however, requires access to individual unit vector measurements. This may

cause difculties if an instrument provides a measurement in the form of an attitude quaternion, as is the case

with 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 lters discussed

above 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-QUEST

algorithm offers an optimal estimate of the attitude and is capable of also estimating non-attitude states. This

algorithm is formulated as a square-root information lter that contains a propagation phase and a state update

phase. The formulation of Extended-QUEST requires the QR factorization of the propagated information

matrix for use in the lter. It also requires the solution to a more general objective function than required

for Filter QUEST or REQUEST. Despite these increases in complexity and computations, Extended-QUEST

provides an optimal estimate of both attitude and and non-attitude states. It has been shown to be robust with

convergence 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 other

nonlinear attitude ltering approaches exist, they are not discussed here. Thorough discussions of these other

methods may be found in the literature.

15

It is within this context that a new nonlinear attitude ltering algorithm is proposed: the Sequential Optimal

Attitude 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 sequentialization

of 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 ination associated with the propagation

of the state is examined. Treating the covariance in this manner allows for the straightforward inclusion of

process noise and addresses the problem of the suboptimal fading memory factor seen in Filter QUEST and

REQUEST. If the attitude covariance and measurement covariance are known at the time of the update, the

optimal estimate of the attitude may be determined.

The SOAR Filter shares many similarities with some of the previous attitude ltering methods. As subse-

quent discussions will show, the SOAR Filter is most similar to Extended-QUEST. The paper concludes with

a side-by-side comparison of the SOAR Filter with a number of the other attitude ltering 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 great

detail. The objective function commonly used to describe this problem was posed by Grace Wahba in 1965

and is frequently referred to as the Wahba Problem.

1

The Wahba Problem statement, using traditional

nomenclature, is given by:

Min J(A) =

1

2

m

i=1

w

i

w

i

Av

i

2

(1)

where m is the number of observed unit vectors, w

i

is a positive weighting on the i-th observation, w

i

is the

measured unit vector for the i-th observation as expressed in the body frame, v

i

is the known reference unit

vector for the i-th observation in a known inertial frame, and A is the rotation matrix that transforms a vector

from the inertial frame to the body frame.

2

For the purposes of this discussion, however, a slightly different notation will be used. First, the rotation

matrix that transforms a vector from the inertial frame to the body frame will be denoted by T

I

B

. Therefore,

the matrix T

I

B

describes the attitude of the vehicle body frame, B, with respect to the inertial frame, I.

Second, note that the unit vectors w

i

and v

i

in Eq. 1 are actually the same unit vector expressed in two

different frames. Therefore, denote this unit vector as e

i

, such that (e

i

)

I

is the reference unit vector expressed

in the inertial frame, i.e. (e

i

)

I

= v

i

. Unfortunately, measurement errors prevent w

i

from simply being e

i

expressed in the actual body frame. Instead each measurement will exist in some measurement body frame

that would produce the measured unit vector. Therefore, let (e

i

)

B,i

be the i-th unit vector expressed in a

measurement body frame,

B

i

, that yields the measured direction of an observed object, i.e. (e

i

)

B,i

= w

i

.

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 denition. Although

this 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(T

I

B

) =

1

2

m

i=1

w

i

_

_

_(e

i

)

B,i

T

I

B

(e

i

)

I

_

_

_

2

(2)

Measurement noise will prevent the rotation matrix T

I

B

from being able to exactly rotate all of the the

reference 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 additive

error:

(e

i

)

B,i

= T

I

B

(e

i

)

I

+

i

= (e

i

)

B

+

i

(3)

where

i

is the measurement error in the observed unit vector. For (e

i

)

B,i

to remain a unit vector to rst

order, the measurement error must be perpendicular to the true observation unit vector:

(e

i

)

T

B

i

0 (4)

Additionally assume that these measurements are unbiased, such that E [

i

] = 0. Further, numerous au-

thors

16, 17

have shown that the line-of-sight unit vector observations have a covariance that can be described

to rst order by

R

i

= E

_

T

i

=

2

,i

_

I

33

(e

i

)

B

(e

i

)

T

B

_

(5)

where

2

,i

is the angular variance (in radians) of the measurement error.

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

2

Min J(T

I

B

) =

0

i=1

w

i

tr

_

T

I

B

(e

i

)

I

(e

i

)

T

B,i

_

=

0

tr

_

T

I

B

B

T

(6)

where

0

=

m

i=1

w

i

and B is called the attitude prole matrix and is given by

B =

m

i=1

w

i

(e

i

)

B,i

(e

i

)

T

I

(7)

Alternatively, if Wahbas problem is rewritten in terms of the attitude quaternion, q, instead of the rotation

matrix, T

I

B

, using

T( q) =

_

q

2

4

q

T

q

_

I

33

+ 2qq

T

+ 2q

4

[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

q

T

K q (9)

3

where K is called the Davenport matrix and is dened as

K =

_

S I

33

z

z

T

_

(10)

and

S = B + B

T

= tr [B] [z] = B B

T

(11)

The attitude that minimizes the objective function in Eq. 9 may be found by the straightforward application

of concepts from optimal control theory.

19

Begin by recalling the quaternion unity norm constraint q = 1

and adjoin this constraint to the objective function through a Lagrange multiplier, . Therefore, from Eq. 9:

Min J( q, ) =

0

q

T

K q +

_

q

T

q 1

_

(12)

The optimal attitude is found by setting the rst differential of J( q, ) to zero. The optimal attitude is given

by the solution to

3, 18, 20

K q = q (13)

Note that Eq. 13 is an eigenvalue-eigenvector problem. It is from this point that various solutions have

emerged. Despite all the discussion circulating about the various attitude determination algorithms, most of

these are simply different methods for solving the eigenvalue-eigenvector problem in Eq. 13. Davenports

q-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 numerical

methods for solving the eigenvalue-eigenvector problem. Further, by substituting Eq. 13 into Eq. 9, the

original objective function becomes:

J( q) =

0

q

T

q =

0

q

T

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 the

characteristic equation to nd only the largest eigenvalue and associated eigenvector. While faster than Dav-

enports q-Method, the solution approach introduces a singularity for a 180 degree rotation that must be

addressed 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

(K q I

44

) q = H q = 0 (15)

it is clear that q must lie in the null space of H. Further if H

T

= [h

1

h

2

h

3

h

4

], then q must be perpendicular

to all h

i

. 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: h

i

and

h

j

, i = j. In the same year that ESOQ was introduced, Mortari also proposed a follow-on algorithm called

ESOQ-2. This algorithm starts with Eq. 13 and computes a symmetric 3 3 matrix of rank 2, M, such that

Me

= 0. Using a similar approach to that seen in the original ESOQ, the attitude may now be determined

by 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 that

rotates the best estimated attitude to the true attitude. If one assumes small angles,

q =

_

q

q

4

_

_

/2

1

_

(17)

where is a three-dimensional parameterization of the attitude given by e

. Here, e

is a unit vector

denoting the axis of rotation and is the magnitude of the rotation about that axis in radians. Therefore,

2q , which yields

P

= E

_

T

_

= 4E

_

qq

T

(18)

This choice means that P

assume that the quaternion attitude estimate is unbiased:

E [ q] = E

__

q

q

4

__

=

_

0 0 0 1

T

(19)

The denition in Eq. 16 also means that the small angle error vector is dened such that

T() = T() T(

) (20)

If the three-dimensional parameterization of the attitude given by is dened with respect to the attitude

estimate given by

q, then

= 0 and T(

) = I

33

by denition. Therefore,

=

= 0 = (21)

Important Attitude Covariance Relations

Next, a few key relations that relate the attitude prole matrix to the covariance matrix are derived. Begin

by recalling the Cram er-Rao inequality,

22

which states

P

xx

= E

_

(x x) (x x)

T

_

(F

xx

)

1

(22)

where F

xx

is the Fisher information matrix. The Fisher information matrix is further dened as

F

xx

=

2

J(x)

xx

(23)

where the scalar objective function J is a function of the column-vector random variable x. If the objective

function J is linear in x and x has a Gaussian distribution then F

xx

= P

1

xx

. Unfortunately, this is not the

case in the present problem. When J is a nonlinear function of x, F

xx

approaches P

1

xx

as the number of

measurements becomes innite:

P

1

xx

= lim

n

F

xx

= lim

n

2

J(x)

xx

(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 function

of Eq. 6 evaluated at the true attitude may be rewritten as a function of

J () =

0

tr

_

exp {[]} T

I

B

B

T

(25)

Expanding the matrix exponential of [] to second order (because the equation for J () must be

differentiated twice),

J () =

0

tr

__

I

33

+ [] +

1

2

[]

2

_

T

I

B

B

T

_

(26)

5

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

:

F

=

2

J () = tr

_

T

I

B

B

T

I

33

T

I

B

B

T

(27)

Therefore,

P

1

= tr

_

T

I

B

B

T

I

33

T

I

B

B

T

(28)

Rearranging the above equation for the attitude prole matrix B,

B =

_

1

2

tr [F

] I

33

F

_

T

I

B

(29)

The relationships of Eq. 28 and Eq. 29 were rst shown by Shuster in 1989.

17

From Eq. 28 and Eq. 29, one

may 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 nd the value of the Wahba

Problem objective function in terms of the Fisher information matrix. Begin by substituting Eq. 29 into Eq.

26

J () =

0

tr

__

I

33

+ [] +

1

2

[]

2

_

T

I

B

T

B

I

_

1

2

tr [F

] I

33

F

T

__

(30)

Recognizing that F

J () =

0

1

2

tr [F

] +

1

2

T

F

(31)

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

,

1

2

tr [F

] =

1

2

tr

_

tr

_

T

I

B

B

T

I

33

T

I

B

B

T

= tr

_

T

I

B

B

T

=

q

T

K

q (32)

Therefore, substituting this into Eq. 31 means that the Wahba Problem objective function evaluated at the

true attitude may be written in terms of the Fisher information matrix as

J () =

0

q

T

K

q +

1

2

T

F

(33)

Looking again at Eq. 14, it follows that

J () = J

_

q

_

+

1

2

T

F

(34)

The form of Eq. 34 is not surprising given that the solution to the Wahba problem is known to produce a

maximum likelihood estimate of the attitude. It is also interesting to note that the difference in the value of

the objective function at the true attitude and the value of the objective function at the best estimate of the

attitude is equivalent to the square of the Mahalanobis distance

23, 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 t

k

, x

k

, given an a

priori 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 state

vector 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 t

k

conditioned on m new measurements and an a priori estimate of the state (with the a priori

estimate conditioned on r old measurements). Using Bayes Theorem, this may be mathematically expressed

as

Max p

_

x

k

|y

1

, y

2

, . . . , y

r+m

_

=

p

_

y

r+1

, . . . , y

r+m

|x

k

_

p (x

k

|y

1

, y

2

, . . . , y

r

)

p

_

y

r+1

, . . . , y

r+m

_ (36)

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

p

_

y

r+1

, . . . , y

r+m

|x

k

_

=

r+m

i=r+1

p (y

i

|x

k

) (37)

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

Max p

_

x

k

|y

1

, y

2

, . . . , y

r+m

_

=

p (x

k

|y

1

, y

2

, . . . , y

r

)

r+m

i=r+1

p (y

i

|x

k

)

p

_

y

r+1

, . . . , y

r+m

_ (38)

This formulation, of course, makes no assumption about the actual distributions of the state vector or the

measurements. Now, if the a priori estimate of the state vector at time t

k

is assumed to have a Gaussian

distribution, then

p (x

k

|y

1

, y

2

, . . . , y

r

) = C

1,k

exp

_

1

2

_

x

k

x

k

_

T _

P

k

_

1

_

x

k

x

k

_

_

(39)

where C

1,k

is a constant that makes the integral of p() equal to unity. Similarly, if the measurement errors

are assumed to have a Gaussian distribution

p (y

i

|x

k

) = C

2,i

exp

_

1

2

_

y

i

h

i

_

x

k

__

T

R

1

i

_

y

i

h

i

_

x

k

__

_

(40)

For the moment, assume the measurements are line-of-sight unit vector observations such that y

i

= (e

i

)

B,i

and h

i

_

x

k

_

= T

I

B

(e

i

)

I

. Making these substitutions,

p (y

i

|x

k

) = C

2,i

exp

_

1

2

_

(e

i

)

B,i

T

I

B

(e

i

)

I

_

T

R

1

i

_

(e

i

)

B,i

T

I

B

(e

i

)

I

_

_

(41)

Unfortunately, the measurement covariance matrix R

i

of Eq. 5 is singular and the regular inverse may not be

computed (R

i

is a 3 3 matrix with a rank of two). Therefore, computing the Moore-Penrose pseudoinverse

of R

i

yields

R

1

= R

+

=

1

2

,i

_

I

33

(e

i

)

B

(e

i

)

T

B

_

(42)

Substituting the pseudoinverse for R

1

in Eq. 41 and recalling the identity from Eq. 4,

p (y

i

|x

k

) = C

2,i

exp

_

1

2

1

2

,i

_

(e

i

)

B,i

T

I

B

(e

i

)

I

_

T

_

(e

i

)

B,i

T

I

B

(e

i

)

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 = [x

1

x

2

. . . xn]

T

x

+

k

= E

. . .

x

k

p

x

k

|y

1

, y

2

, . . . , y

r+m

dx

1

dx

2

. . . dxn

x

k

= E

. . .

x

k

p (x

k

|y

1

, y

2

, . . . , y

r

) dx

1

dx

2

. . . dxn

7

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

p

_

y

r+1

, . . . , y

r+m

|x

k

_

=

r+m

i=r+1

p (y

i

|x

k

) = C

3,k

exp

_

1

2

r+m

i=r+1

1

2

,i

_

_

_(e

i

)

B,i

T

I

B

(e

i

)

I

_

_

_

2

_

(44)

where C

3,k

=

r+m

i=r+1

C

2,i

. Note that the term in the exponent of Eq. 44 is equivalent to the traditional

Wahba Problem objective function to rst order if w

i

= 1/

2

,i

. This demonstrates that solutions to the

Wahba Problem produce a maximum likelihood estimate of the attitude if w

i

= 1/

2

,i

.

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

Max p

_

x

k

|y

1

, y

2

, . . . , y

r+m

_

=

C

1,k

C

3,k

p

_

y

r+1

, . . . , y

r+m

_ exp

_

1

2

_

x

k

x

k

_

T _

P

k

_

1

_

x

k

x

k

_

_

(45)

exp

_

1

2

r+m

i=r+1

1

2

,i

_

_

_(e

i

)

B,i

T

I

B

(e

i

)

I

_

_

_

2

_

Recognizing that the maximum of p

_

x

k

|y

1

, y

2

, . . . , y

r+m

_

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

imum of ln

_

p

_

x

k

|y

1

, y

2

, . . . , y

r+m

_

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

nding

Max ln [C

1,k

] + ln [C

3,k

] ln

_

p

_

y

r+1

, . . . , y

r+m

_

(46)

1

2

_

x

k

x

k

_

T _

P

k

_

1

_

x

k

x

k

_

1

2

r+m

i=r+1

1

2

,i

_

_

_(e

i

)

B,i

T

I

B

(e

i

)

I

_

_

_

2

Because the rst three terms do not depend on the estimate of the state, they may be dropped from the

objective function in determining the maximum likelihood estimate. Dropping the these terms and rewriting

as a minimization problem yields the following form of the objective function

Min L =

1

2

_

x

k

x

k

_

T _

P

k

_

1

_

x

k

x

k

_

+

1

2

r+m

i=r+1

1

2

,i

_

_

_(e

i

)

B,i

T

I

B

(e

i

)

I

_

_

_

2

(47)

Discussion of SOAR Filter State Vector

The objective of the present lter is to estimate the attitude of a vehicle along with a number of other

vehicle parameters. To express the attitude, it is desired to use a minimal degree representation (a number of

different three-dimensional parameterizations would work here) instead of the constrained four-dimensional

quaternion. 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 the

attitude to the true attitude. As was observed earlier, this means that

selection are critical to the proper treatment of attitude in the lter and will be discussed in detail in the

following sections.

In addition to estimating the attitude, the SOAR Filter also estimates other vehicle parameters that are

dened 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, the

objective function from Eq. 47 may be expanded as

Min L =

1

2

_

T

k

F

,k

k

+

T

k

F

,k

k

+

T

k

F

,k

k

+

T

k

F

,k

k

_

(50)

+

1

2

r+m

i=r+1

1

2

,i

_

_

_(e

i

)

B,i

T

I

B

(e

i

)

I

_

_

_

2

The subscript k is temporarily dropped in the following derivation to abbreviate notation. Additionally, the

partitioned Fisher information matrix in Eq. 50 is given by

P

1

F =

_

F

_

(51)

Care must be taken here not to confuse the F

matrix in Eq. 28 with the F

matrix of the inverse of the entire covariance matrix, which contains both attitude and non-attitude states.

Further, recognizing that F

= F

T

and using the form of the Wahba Problem cost function from Eq. 9,

Min L =

1

2

T

F

+

T

F

+

1

2

T

F

+

m

0

q

T

K

m

q (52)

where K

m

is the measurement Davenport matrix constructed using only the new measurements. If the mea-

surements are unit vectors, then K

m

is computed as in the regular Wahba Problem. If the measurements

are quaternions (as is sometimes the case for commercially available star trackers that provide a quaternion

estimate instead of the raw measurements), then the measured attitude and measurement covariance matrix

are used to create B

m

from Eq. 29. Now, B

m

may be used to compute K

m

from Eq. 10 and Eq. 11.

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

_

A

11

A

12

A

21

A

22

_

1

=

_

_

_

A

11

A

12

A

1

22

A

21

_

A

11

A

12

A

1

22

A

21

1

A

12

A

1

22

A

1

11

A

12

_

A

22

A

21

A

1

11

A

12

1

_

A

22

A

21

A

1

11

A

12

1

_

_

(53)

it may be shown through a little algebra that

F

= P

1

+ F

F

1

(54)

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

1

2

T

F

=

1

2

T

_

P

_

1

+

1

2

T

F

_

F

_

1

F

(55)

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

1

2

T

F

0

q

T

K

q J(

) +

1

2

T

F

_

F

_

1

F

(56)

The a-priori Davenport matrix, K

, may be found by using Eq. 10 and Eq. 11 and the a-priori attitude

prole matrix, B

B

=

_

1

2

tr

_

_

P

_

1

_

I

33

_

P

_

1

_

T

I

B

(57)

9

where T

I

B

is found through Eq. 8 using

q

function from Eq. 52 and combining like terms,

Min L =

_

m

0

+

0

_

J

_

_

q

T

_

K

m

+ K

_

q +

1

2

T

F

_

F

_

1

F

(58)

+

T

F

+

1

2

T

F

To simplify, note the following relations associated with rotations and quaternion multiplication

_

/2

1

_

q = q

_

_

1

=

_

q

q

4

_

_

q

4

_

(59)

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

_

q

q

4

_

_

q

4

_

=

_

q

4

q q

4

q

+ q q

q

4

q

4

+ q

T

q

_

=

_

_

q

4

I

33

_

q

_

q

_

q

_

T

q

4

_

_

q (60)

Therefore, dene the 3 4 matrix

_

k

_

as

_

=

_

q

4

I

33

_

q

_

q

_

(61)

such that

= 2

_

_

q (62)

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

Min L =

_

m

0

+

0

_

J

_

_

q

T

_

K

m

+ K

_

q + 2 q

T

_

T

F

_

F

_

1

F

_

q (63)

+ 2 q

T

_

T

F

+

1

2

T

F

Using a similar approach as for the optimal solution for the regular Wahba problem, adjoin the attitude

quaternion unity norm constraint to the objective function using a Lagrange multiplier, , such that

Min L( q, , ) =

_

m

0

+

0

_

J

_

_

q

T

_

K

m

+ K

_

q (64)

+ 2 q

T

_

T

F

_

F

_

1

F

_

q

+ 2 q

T

_

T

F

+

1

2

T

F

+

_

q

T

q 1

_

Taking the differential of L( q, , ),

dL( q, , ) =

_

2 q

T

_

K

m

+ K

_

+ 4 q

T

_

T

F

_

F

_

1

F

_

(65)

+2

T

F

_

+ 2 q

T

_

d q

+

_

2 q

T

_

T

F

+

T

F

_

d +

_

q

T

q 1

_

d = 0

where the rst three components of d q are independent differentials and the fourth component is a dependent

differential (this distinction is required because of the quaternion unity norm constraint). The differentials

have been combined back into d q for brevity. Setting the coefcient of d to zero produces

q

T

q 1 = 0 (66)

10

which simply states that the quaternion unity norm constraint must be satised. Assuming no constraints

on the non-attitude states, d is an independent differential. Therefore, if dJ ( q, , ) is to be zero for an

arbitrary d, the coefcient of d must be zero,

2 q

T

_

T

F

+

T

F

= 0 (67)

Rearranging and solving for (again recall that F

is symmetric),

= 2

_

F

_

1

F

_

q (68)

Now move to the term involving d q = [dq

T

dq

4

]

T

in Eq. 65. Because dq is an independent differential, its

coefcients must be zero. Additionally, the Lagrange multiplier is picked such that the coefcient of the

dependent differential dq

4

is also zero. Therefore, the coefcient of d q in Eq. 65 must be zero,

2 q

T

_

K

m

+ K

_

+ 4 q

T

_

T

F

_

F

_

1

F

_

+ 2

T

F

_

+ 2 q

T

= 0 (69)

Substituting the optimal value of from Eq. 68,

q

T

_

K

m

+ K

_

+ 2 q

T

_

T

F

_

F

_

1

F

_

(70)

2 q

T

_

T

F

_

F

_

1

F

_

+ q

T

= 0

The middle two terms cancel, leaving

q

T

_

K

m

+ K

_

+ q

T

= 0 (71)

Therefore, if K

+

is given by

K

+

= K

m

+ K

(72)

then Eq. 71 may be rewritten as

q

T

K

+

+ q

T

= 0 (73)

Recognizing that K

+

is symmetric and that the solution to Eq. 73 is the a-posteriori attitude estimate, which

allows 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

+

(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 to

the state covariance matrix. Begin by rewriting the objective function from Eq. 58 as

Min L =

_

m

0

+

0

_

J

_

_

tr

_

T

I

B

_

B

m

+ B

_

T

_

+

1

2

T

F

_

F

_

1

F

(76)

+

T

F

+

1

2

T

F

As was done in nding the covariance for the regular Wahba Problem in Eq. 26, rewrite the attitude term in

terms of , where is a small angle rotation from the best estimate of the body frame,

Min L =

_

m

0

+

0

_

J

_

_

tr

__

I

33

+ [] +

1

2

[]

2

_

T

I

B

_

B

m

+ B

_

T

_

(77)

+

1

2

T

F

_

F

_

1

F

+

T

F

+

1

2

T

F

11

If a new variable B

+

is dened as

B

+

= B

m

+ B

(78)

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

F

+

=

2

L() = tr

_

T

I

B

_

B

+

_

T

_

I

33

T

I

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 dynamics

and correlation between the estimates of the attitude and parameters. Further, it is interesting to note the

relationship between Eq. 79 and Eq. 28. The rst 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 matrix

given 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 denition of the covariance matrix (i.e. a co-

variance/uncertainty approach rather than an information approach). The details of this equivalence are not

presented 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 t

k

forward in

time to create the a-priori state estimate and covariance at time t

k+1

. Consider a system with the following

nonlinear dynamic model:

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

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)

Dening 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 thoroughly

in the literature.

2527

Such a linear model is known to admit a solution of the form

x(t) = (t, t

0

) x(t

0

) +

_

t

t0

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

where (t, t

0

) is the state transition matrix from t

0

to t. Therefore, assuming that the state error and the

process noise are uncorrelated, i.e. E

_

x(t)w(t)

T

P = E

_

xx

T

= (t, t

0

) P

0

(t, t

0

)

T

+

_

t

t0

(t, ) G()Q()G

T

(t, )

T

d (92)

where

Q() = E

_

w()w()

T

(93)

Taking the time derivative of Eq. 92 (which requires the application of Leibnizs Rule) and recognizing that

= F, yields the well known equation for the propagation of the covariance matrix typically seen in the

discrete-continuous EKF:

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 of

the SOAR Filter is provided in the following paragraphs.

Given an a-priori attitude estimate and covariance matrix, compute the a-priori attitude prole matrix from

Eq. 29. Use this to create the a-priori Davenport matrix using Eq. 10 and Eq. 11.

Next, compute the measurement attitude prole matrix. If the measurements are from unit vectors, create

the measurement attitude prole matrix using Eq. 7. If, on the other hand, a measured attitude is available,

compute the measurement attitude prole matrix using Eq. 29. Because the Davenport matrix K is a linear

homogeneous function of B, the measurement attitude prole matrices for all measurement sources (whether

they come from unit vector measurements or quaternion measurements) may be added before using Eq. 10

and 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 the

Wahba Problem to solve for the optimal attitude. With the optimal attitude found, use Eq. 68 to compute the

update to the parameter vector . Finally compute the updated covariance matrix by taking the inverse of the

Fisher 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 matrix

may 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 of

the constrained four-parameter attitude quaternion in the state vector of a regular EKF.

15, 28

This means that

the MEKF estimates

q

+

/2

1

_

q

(95)

13

This update only maintains the quaternion unity norm constraint to rst order, making it common practice to

normalize the a-posteriori quaternion, q

+

= q

+

/ q

+

.

An important similarity between the MEKF and the SOAR Filter is the objective function used to nd

the optimal update. Both the MEKF and SOAR Filter explicitly nd the minimum variance state update

and 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. Simulation

conrms that this is the case. A second important similarity is that both the MEKF ans SOAR can estimate

non-attitude states.

Unlike the MEKF, the SOAR Filter deals directly with the nonlinear nature of attitude representations and

the Wahba Problem. Rather than linearizing the update equation as is required in the MEKF, the SOAR Filter

explicitly solves the constrained quadratic cost function. This allows the SOAR Filter to directly compute

the attitude quaternion. Additionally, because the SOAR Filter approaches the problem from an information

perspective, it is capable of dealing with scenarios where no a-priori attitude estimate is available. In the

MEKF, an a-priori estimate is required to perform the linearization.

Comparison of SOAR with Filter QUEST and REQUEST

Filter QUEST

7, 8

and REQUEST

9

are both based on sequentialization of the Wahba Problem. Performing

this sequentialization produces the following recursive lter equation (using the same nomenclature as for

the SOAR Filter) that is called Filter QUEST in the literature,

7

B

+

k+1

= B

m

k+1

+

k+1

(t

k+1

, t

k

) B

+

k

(96)

where

k+1

[0 1] and represents reduction in knowledge associated with the propagation. This is a classic

fading memory lter

25

where

k+1

= 0 causes the lter to ignore the old measurements and

k+1

= 1

makes the lter think there is no process noise and the measurements are combined as if they were all taken

simultaneously. Therefore, heuristically speaking, the fading memory factor

k+1

should look something

like

(t

k

)

2

,i

/(t

k+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 prole matrix B, Eq. 96 may

be written in terms of K instead of B. This represents the fundamental difference between the Filter QUEST

and REQUEST algorithm: Filter QUEST is a sequentialization on B and REQUEST is a sequentialization

on K. Shuster provides an excellent discussion of the relation between these two algorithms,

10

which are

shown to be equivalent when the same selection is made for

k+1

. Therefore, the conclusions made in the

comparison 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 specic scenarios. Consider a system with the following assumptions:

1. Only three unit vector measurements are available

2. Each unit vector measurement lies along one of the coordinate axes with a measurement standard

deviation of

33

4. Steady state lter covariances

5. The attitude covariance matrix is diagonal, P

= pI

33

Shuster demonstrates

7, 8

that under these conditions the optimal

k+1

may be analytically computed as

k+1

=

/q + 1

_

1 + 2

2

/q

/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 obtain

measurements along each of the coordinate axes). This means that the

k+1

given in Eq. 97 is suboptimal in

the general sense.

The SOAR Filter removes all ve of the assumptions made in computing

k+1

. As discussed above, the

SOAR Filter provides the optimal state update under the assumption that state vector error and measurement

errors are normally distributed. The SOAR Filter is also capable of estimating other states in addition to

attitude (recall that one of the major drawbacks to Filter QUEST and REQUEST is their inability to also

estimate non-attitude states). These advantages, however, come at the expense of additional computations.

Comparison of SOAR with Optimal-REQUEST

The Optimal-REQUEST algorithm

11

nds 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

2

max

(K) =

max

_

K(K)

T

_

= K

2

2

K

2

F

= tr

_

K(K)

T

_

(99)

Recalling an important result from eigenvalue-eigenvector stability,

29

the estimate of the attitude quaternion

may be written as,

q = q +

4

j=2

_

u

T

j

K q

j

_

u

j

+ H.O.T. (100)

where u

j

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. u

1

q).

Because all u

j

are assumed to have unity norm and K must be a symmetric matrix, it is straightfor-

ward to show (Jordan decomposition) that u

T

j

K q

max

(K). Because it can also be shown that

tr [P

] = 4tr

_

E

_

_

q q

_ _

q q

_

T

__

to rst order, then

tr [P

] 4E

_

2

max

(K)

j=2

4

l=2

_

1

(

1

j

) (

1

l

)

_

tr

_

u

j

u

T

l

(101)

Note that the double summation is only a function of the true attitude and, therefore, may be moved outside

of the expected value operator

tr [P

] 4E

_

K

2

2

_

(K) 4E

_

K

2

F

_

(K) = 4tr

_

E

_

K(K)

T

__

(K) (102)

where

(K

k

) =

4

j=2

4

l=2

_

1

(

1

j

) (

1

l

)

_

tr

_

u

j

u

T

l

(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 SOARFilter 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 SOAR

Filter 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 of

the REQUEST algorithm (with optimal

k+1

chosen to minimize Eq. 98), the Optimal-REQUEST algorithm

is 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 ltering methods to the SOAR Filter.

The Extended-QUEST algorithm is a solution to the following optimization problem,

12

J

ExtQUEST

=

1

2

q

T

K

m

q +

1

2

_

R

qq

_

q q

_

T

_

R

qq

_

q q

_

(104)

+

1

2

_

R

q

_

q q

_

+ R

_

T

_

R

q

_

q q

_

+ R

_

where R

xx

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 same

nomenclature as in the SOAR Filter to facilitate easy comparison. Strong parallels between the SOAR Filter

and Extended-QUEST are immediately evident by comparing the SOAR Filter objective function in Eq. 52

with the Extended-QUEST objective function in Eq. 104. These similarities produce similar results for the

state vector update. The Extended-QUEST parameter vector update is given by

+

=

R

1

R

q

_

q

(105)

which is clearly similar to the SOAR Filter parameter vector update from Eq. 75. The attitude update,

however, is signicantly different than in the SOAR Filter. Extended-QUEST nds the optimal a-posteriori

attitude through the solution to

_

K

m

+ R

T

qq

R

qq

q R

T

qq

R

qq

q

= q (106)

The introduction of the constant vector R

T

qq

R

qq

q

the simple eigenvector-eigenvalue problem in Eq. 74 for the SOAR Filter.

Despite strong similarities, there are numerous noteworthy differences between Extended-QUEST and the

SOAR Filter. The rst signicant difference is that Extended-QUEST is a square-root information lter 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 this

reduces computations in the SOAR Filter, it places the SOAR Filter at a disadvantage from the standpoint of

numerical precision relative to Extended-QUEST.

The second signicant difference between the SOAR Filter and Extended-QUEST is how they express

the attitude error. Extended-QUEST treats the attitude quaternion error from an additive standpoint, rather

than a multiplicative standpoint. Although the term additive is used here, it is important to note that

the Extended-QUEST algorithm explicitly enforces the quaternion unity norm constraint in generating the

quaternion update. Therefore, the update here should not be confused with additive updates that do not

preserve the unity norm constraint.

The difference between using an additive or multiplicative quaternion update has two signicant impli-

cations. The rst is that the covariance associated with R

is related to q

A

= q

q

multiplicative attitude quaternion error described in Eq. 16: q = q

q

signicance of the resulting P

in Extended-QUEST.

The second major implication of using the additive quaternion error is related to the optimal update. The

additive representation of the attitude quaternion error requires the solution of a more general objective func-

tion. When the rst differential of Eq. 104 is set to zero, Extended-QUEST requires the solution to Eq. 106,

which is of the following form

(KI

44

) q + g = 0 (107)

Psiaki demonstrates a robust method for nding the desired solution to this problem.

12

Solving Eq. 107,

however, requires more computation than simply nding the largest eigenvalue/eigenvector of K using a

method such as QUEST or ESOQ. This means that solving for the optimal attitude in the SOAR Filter (as

described in Eq. 74) will be faster than nding the optimal attitude in Extended-QUEST.

16

EXAMPLE: ESTIMATION OF ATTITUDE AND GYRO BIAS

Consider a scenario where a lter is required to estimate the attitude of a spacecraft and the gyro bias, b

g

.

Rather than have the lter 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 spacecraft

attitude lters - sometimes referred to as ying the gyros. Therefore, assume a gyro measurement model

given by

30

= [I

33

+ S

g

+

g

] ( + b

g

+ w

) (108)

where S

g

is the scale-factor error matrix,

g

is the misalignment error matrix, is the measured angular

velocity, b

g

is the gyro bias, and w

that there is no scale-factor error, S

g

= 0

33

, and that there is no misalignment error,

g

= 0

33

. Under

these conditions, the gyro measurement model becomes

= + b

g

+ w

(109)

Further assume that the gyro bias behaves as a rst-order Gauss-Markov process:

25, 26

b

g

=

1

b

g

+ w

b

(110)

where is the correlation time and w

b

is a zero mean white noise process. It may further be shown that the

variance of w

b

is 2

2

/.

25

Therefore,

= E [] =

b

g

(111)

b

g

= E

_

b

g

=

1

b

g

(112)

Two test cases are considered. The rst is a scenario with typical sensor errors and noise. In the rst

scenario, only SOAR Filter errors and covariances are shown. The second scenario is a stressing case that

contains very large a-priori attitude errors and bias errors. The measurement noise is also increased. Results

for the second scenario are presented for the SOAR Filter, the MEKF, and Filter-QUEST.

The rst test case has performance specications as described in Table 1. The results for this test case are

shown in Fig. 1. Star tracker measurements were made available to the lter at a frequency of 1 Hz. The fact

that almost all the results for the bias error lie within the 1 bounds indicates that the process noise could

be 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

T

a-priori bias error b

g

b

g

= [0.25 0.5 0.375]

T

deg/hr

Star observation unit vector error

,i

= 10 arcsec

Gyro bias Gauss-Markov error = 0.05 deg/hr with correlation time of = 1 hr

Gyro measurement error

= 0.05 deg/hr

The second test case has performance specications as described in Table 2. The results for this test case

are shown in Fig. 2. The exact same noise and observations are applied to each lter technique in order to

facilitate direct comparison. Under these extreme conditions, Filter-QUEST is unable to converge because it

is not estimating the gyro bias and because of its suboptimal fading memory factor. Both the MEKF and the

SOAR Filter converge for this test case. Despite being given equivalent a-priori information and processing

the exact same measurements, signicant differences are observed prior to lter convergence. In all cases, the

SOAR 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

r

o

l

l

e

r

r

o

r

,

a

r

c

s

e

c

SOAR attitude estimate error

1-sigma bounds on estimated attitude

-2

0

2

4

p

i

t

c

h

e

r

r

o

r

,

a

r

c

s

e

c

0 100 200 300 400 500

-1

0

1

r

o

l

l

b

i

a

s

,

d

e

g

/

h

r

SOAR bias estimate error

1-sigma bounds on estimated bias

0

1

p

i

t

c

h

b

i

a

s

,

d

e

g

/

h

r

0 100 200 300 400 500

-4

-2

p

i

t

c

h

e

r

r

o

r

,

a

r

c

s

e

c

0 100 200 300 400 500

-4

-2

0

2

4

time, sec

y

a

w

e

r

r

o

r

,

a

r

c

s

e

c

0 100 200 300 400 500

-1 p

i

t

c

h

b

i

a

s

,

d

e

g

/

h

r

0 100 200 300 400 500

-1

0

1

time, sec

y

a

w

b

i

a

s

,

d

e

g

/

h

r

a) 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 objective

function and are expected to exhibit similar behavior once the lter converges. The results shown in Fig. 2

support this observation. As an example, a zoom-in of the roll-axis attitude error (see Fig. 3) shows that the

SOAR Filter and the MEKF exhibit identical performance after the lter converges. Similar results may be

seen 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

T

a-priori bias error b

g

b

g

= [90 90 67.5]

T

deg/hr

Star observation unit vector error

,i

= 1 deg

Gyro bias Gauss-Markov error = 0.5 deg/hr with correlation time of = 1 hr

Gyro measurement error

= 0.5 deg/hr

CONCLUSIONS

A new nonlinear attitude lter called the Sequential Optimal Attitude Recursion (SOAR) Filter has been

developed. This lter is based on maximum likelihood estimation and is capable of dealing with both attitude

and non-attitude states. In its current formulation, however, the SOAR Filter is only able to accept two types

of measurements: unit vector measurements and complete attitude estimates. The SOAR Filter algorithm was

compared to numerous other attitude ltering techniques from a theoretical standpoint. Then, an example

simulation was performed where the lter was required to estimate attitude and gyro bias. The performance

of the SOAR Filter was shown to be advantageous relative to previously existing lters in some scenarios.

18

0 100 200 300 400 500

-1

-0.5

0

0.5

1

r

o

l

l

e

r

r

o

r

,

d

e

g

0

0.5

1

p

i

t

c

h

e

r

r

o

r

,

d

e

g

MEKF

Filter-QUEST

SOAR

0 100 200 300 400 500

-0.05

0

0.05

0.1

r

o

l

l

b

i

a

s

e

r

r

o

r

,

d

e

g

/

s

e

c

0.05

0.1

p

i

t

c

h

b

i

a

s

e

r

r

o

r

,

d

e

g

/

s

e

c

MEKF

Filter-QUEST

SOAR

0 100 200 300 400 500

-1

-0.5

p

i

t

c

h

e

r

r

o

r

,

d

e

g

0 100 200 300 400 500

-1

-0.5

0

0.5

1

time, sec

y

a

w

e

r

r

o

r

,

d

e

g

0 100 200 300 400 500

-0.05

0

p

i

t

c

h

b

i

a

s

e

r

r

o

r

,

d

e

g

/

s

e

c

0 100 200 300 400 500

-0.05

0

0.05

0.1

time, sec

y

a

w

b

i

a

s

e

r

r

o

r

,

d

e

g

/

s

e

c

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 SOARFilter

in of the SOAR Filter in scenario 2 (stressing case with large errors).

0

0.1

0.2

0.3

0.4

0.5

r

o

l

l

e

r

r

o

r

,

d

e

g

MEKF

Filter-QUEST

SOAR

0 50 100 150 200 250 300 350 400 450 500

-0.5

-0.4

-0.3

-0.2

-0.1

r

o

l

l

e

r

r

o

r

,

d

e

g

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 Mu noz of the University of Texas at Austin; John Crassidis of

the 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 Journal

of the Astronautical Sciences, Vol. 48, April-September 2000, pp. 359380.

[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. 7077.

[4] D. Mortari, ESOQ: A Closed-Form Solution to the Wahba Problem, The Journal of the Astronautical

Sciences, Vol. 45, April-June 1997, pp. 195204.

[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. 817826.

[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. 245258.

[7] M. Shuster, A Simple Kalman Filter and Smoother for Spacecraft Attitude, Journal of the Astronauti-

cal Sciences, Vol. 37, January-March 1989, pp. 89106.

[8] M. Shuster, NewQuests for Better Attitudes, Flight Mechanics/Estimation Theory Symposium, NASA

Goddard 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. 10341038.

[10] M. Shuster, Filter QUEST or REQUEST, Journal of Guidance, Control, and Dynamics, Vol. 32,

March-April 2009, pp. 643645.

[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. 418425.

[12] M. Psiaki, Attitude-Determination Filtering via Extended Quaterion Estimation, Journal of Guidance,

Control, and Dynamics, Vol. 23, March-April 2000, pp. 206214.

[13] F. Markley, AttitudeDetermination and Parameter Estimation Using Vector Observations: Theory, The

Journal of the Astronautical Sciences, Vol. 37, January-March 1989, pp. 4158.

[14] F. Markley, AttitudeDetermination and Parameter Estimation Using Vector Observations: Applica-

tion, The Journal of the Astronautical Sciences, Vol. 39, July-September 1991, pp. 367381.

[15] J. Crassidis, F. Markley, and Y. Cheng, Survey of Nonlinear Attitude Estimation Methods, Journal of

Guidance, Control, and Dynamics, Vol. 30, No. 1, 2007, pp. 1228.

[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 Astronautical

Sciences, Vol. 37, Jan-March 1989, pp. 7988.

[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. 509515.

[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. 311317.

[29] K. Atkinson, An Introduction to Numerical Analysis, Second Edition. John Wiley & Sons, 1989.

[30] S. Mu noz, 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

- nm-2068Uploaded byRoshan Paudel
- SENSORLESS VECTOR CONTROL OF BLDC USING EXTENDED KALMAN FILTERUploaded byCS & IT
- SEM 1 MCA 11 MathematicsUploaded byMalatesh Havanagi
- Loly Complex Systems 17-1-2Uploaded byjuan perez arrikitaun
- Pr FinalUploaded byHakan Kaya
- Spatio-Temporal video monitoringUploaded byAnita Veeramani
- IAS Mains Mathematics 1992 IAS Mains Mathematics 2014 2IN1Uploaded byMohit Grewal
- linear system 2 homeworkUploaded bylarasmoyo
- Prediction Error Methods for Time-domain Blind Identification of Multichannel FIR FiltersUploaded byKiran Kumar
- Algebra-Through-Practice-Volume-2-Matrices-and-Vector-Spaces-A-Collection-of-Problems-in-Algebra-with-Solutions.pdfUploaded bysohamdey
- eigenv5bUploaded byXiaoqin Zhou
- Linear Mathematics and Vector Calculus ExaminationUploaded byaucyaucy123
- Eigens by Row(1)Uploaded byJeeva Jp
- AFEM.Ch12Uploaded byPanchoMiyamoto
- Maxima Book Chapter 7Uploaded byprakush_prakush
- 00A Exploring Statistical Arbitrage Opps YE-XIAOXIAUploaded byjahbreeze
- KalmanUploaded byjohnnyz88
- CH3.PDFUploaded byteknikpembakaran2013
- RootUploaded byMuhammad Fahad
- FinalC_Fall06Uploaded byloser mommy
- ls4Uploaded byshankyforu
- v2_8Uploaded byamahesh13
- uon_HG_2010_HG1M11E1-10Uploaded byZakariya Mohamed
- Dynamic Hedge Ratio Between ETF Pairs Using the Kalman Filter _ QuantStart.pdfUploaded bycicciomessere
- pr_l3Uploaded byBirkan Göçerler
- t4v1MAT1341 Linear Algebra Midterm 4Uploaded byWeideSha
- Survey 3 de las matematicasUploaded byChecoz
- determinantsUploaded byshashin17
- An Improved Face Detection and Recognition Method Based On Information of Skin Colour for Security SystemUploaded byseventhsensegroup
- SanUploaded byseelan9

- Barbara Wendelberger - Calculus_of_VariationsUploaded byFacepaz Dar
- Effects Zero Pole on system responseUploaded byfancy_mask
- Mazya. Sovolev SpaceUploaded byRicardo
- lec2Uploaded byVanitha Shah
- The Riemann Hypothesis- Arithmetic and Geometry - Jeffrey C. LagariasUploaded byantonio.mm.neves5467
- NACA Report on Conformal Transformations(Aerodynamics)Uploaded byHariharan Ramanathan
- Manuel de La SenUploaded byWseas Friends
- WiproUploaded bysuganya004
- 9th Maths Number System Test Paper WorksheetUploaded byChirag Garg
- Continuous Time Fourier TransformUploaded byaparajit5054
- 1001 Algebra ProblemsUploaded byasokan80
- MTE119 - Solution Problem 5.3Uploaded byJoe Huanca Rivera
- FunMath4Uploaded byCijara
- PROVA IIUploaded bySergio Carvalho
- v55i09Uploaded byluthfi2011
- RatiosUploaded byOmer Ali
- math 10Uploaded byRomeo Jr Pacheco Opena
- teqn 1.pdfUploaded byadityabaid4
- General Formula of ReliabilityUploaded byMuhammad Reza Pradecta
- plot curve.pdfUploaded byShafi Ullah
- StressUploaded byNauman Suria
- MATH15 Project JavaScript Source CodeUploaded byIvan Perez
- Calculus - First CourseUploaded byrambori
- Sokolnikoff Theory of ElasticityUploaded byAdam Taylor
- Nurturance for Differential EquationUploaded byMark
- g4 techcorrelationchartUploaded byapi-281256997
- DE23_solUploaded byPrathyush KanchiReddy
- Contribution of AryabhattaUploaded byIshpreet Singh Bagga
- IIT JEE Mathematics 1997Uploaded bymohit24031986
- Linear Multistep MethodUploaded byShankar Prakash G