Direction Cosine Matrix Estimation from Vector Observations using a Matrix Kalman Filter-LUK.pdf

INTRODUCTION

Direction Cosine Matrix

Estimation from Vector

Observations using a Matrix

Kalman Filter

In the course of spacecraft operation the spacecraft

spatial orientation with respect to some reference

frame has to be continuously determined. A popular

mathematical representation of the spacecraft

orientation is the matrix of the rotation between a

Cartesian coordinate frame, which is rigidly fixed

in the spacecraft body, and a reference Cartesian

coordinate frame. This matrix, which is denoted by

D, is also known as direction cosine matrix (DCM)

[1, p. 411]. Typical observations that are processed

on-board spacecraft in order to determine D are

vector observations, like the Earth’s magnetic field

or measurements of lines of sight from the spacecraft

to various celestial objects. The DCM is a 3£3

matrix, whose nine elements are not independent.

Indeed, like any rotation matrix, D is a proper

orthogonal matrix [1, p. 412], which implies six

constraints connecting the nine elements. In spite

of the redundancy created by the constraints, the

DCM D is a convenient rotation representation. This

is so because the equations that describe the vector

measurement model and the spacecraft kinematical

model are linear in D [1, pp. 411, 512]. The latter

facts enable the development of efficient estimators

of the DCM using vector observations. Notice that

two vector observations are necessary and sufficient to

fully determine D [2].

Optimal DCM estimators which handle more

than two vector observations typically fall into two

categories. The first one has its origin in a constrained

weighted least-squares problem, known as the Wahba

problem [3]. Besides the usual good features of the

solutions to Wahba’s problem 1 a particular highlight

is that they are matrix estimators: they preserve the

matrix formulation of the original DCM parameters;

their development and analysis, which involve

standard matrix decompositions, would have been

cumbersome, if not impossible, without maintaining

a matrix notation. Nevertheless, a drawback of

this family of estimators is the suboptimality with

regard to time propagation noises, when estimating

a time-varying DCM, and a lack of clear probabilistic

meaning. 2 On the other hand, the second approach,

namely the minimum-variance or Kalman filtering

approach, provides a convenient stochastic framework

for developing approximate Kalman filters (KF) of a

time-varying DCM. But in order to implement the KF,

the usual approach [11] consists in transforming the

D. CHOUKROUN, Member, IEEE

Ben-Gurion University

H. WEISS

Rafael Advanced Defense Systems, Ltd.

I. Y. BAR-ITZHACK, Fellow, IEEE

Y. OSHMAN, Fellow, IEEE

Technion—Israel Institute of Technology

This work presents several algorithms that use vector

observations in order to estimate the direction cosine matrix

(DCM) as well as three constant biases and three time-varying

drifts in body-mounted gyro output errors. All the algorithms use

the matrix Kalman filter (MKF) paradigm, which preserves the

natural formulation of the DCM state-space model equations.

Focusing on the DCM estimation problem, the assumption of

white noise in the gyro and in the vector observations errors

yields reduced and efficient filter covariance computations.

The orthogonality constraint on the DCM is handled via the

technique of pseudomeasurement, which is naturally embedded

in the MKF. Two additional known “brute-force” procedures are

implemented for the sake of comparison. Extensive Monte-Carlo

simulations illustrate the performances of the different estimators.

When estimating only the DCM, it is shown that all the

proposed orthogonalization procedures accelerate the estimation

convergence. Nevertheless, the pseudomeasurement technique

shows a smoother and shorter transient than the brute-force

procedures, which on the other hand yield more accurate

steady-states. The reduced covariance computations yield a more

accurate steady-state than the full covariance computations but

show a slower transient. When estimating the DCM as well as

the gyro biases and drifts, enforcing orthogonalization seems

to penalize the DCM estimation as long as the biases are not

correctly identified. For the sake of computation savings during

long duration missions, a mixed estimator, switching between long

periods of DCM-only estimation and short periods of DCM-biases

estimation, appears to be a promising strategy.

Manuscript received December 12, 2006; revised October 15, 2007;

released for publication August 20, 2008.

IEEE Log No. T-AES/46/1/935928.

Refereeing of this contribution was handled by P. Willett.

This work was presented as Paper 2003-5482 at the 43rd AIAA

Guidance, Navigation, and Control Conference, Austin, TX, Aug.

2003.

This paper is dedicated to the memory of Itzhack Y. Bar-Itzhack,

Professor Emeritus of Aerospace Engineering at the Technion—Israel

Institute of Technology, who died 9 May 2007 in Haifa, Israel.

Authors’ addresses: D. Choukroun, Dept. of Mechanical

Engineering, Ben-Gurion University of the Negev, Israel, E-mail:

(danielch@bgu.ac.il); H. Weiss, Rafael Advanced Defense Systems,

Ltd., PO Box 2250, Dept. 35, Haifa 31021, Israel; Y. Oshman,

Dept. of Aerospace Engineering, Technion—Israel Institute of

Technology, Israel.

1 They are closed-form solutions, and thus, they need no a priori

estimate of D [4—10]. Moreover the orthogonality constraint on D is

inherent to the formulation of the problem.

2 In general, the weights in Wahba’s cost function are chosen as

scalars equal to the inverse of the variance of the associated vector

measurement. Through this choice, the solution to Wahba’s problem

acquires some statistical ground. Nevertheless, this choice is at most

heuristic.

0018-9251/10/$26.00 ° 2010 IEEE

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 46, NO. 1 JANUARY 2010

AUTHORIZED LICENSED USE LIMITED TO: IEEE XPLORE. DOWNLOADED ON MAY 13,2010 AT 11:53:31 UTC FROM IEEE XPLORE. RESTRICTIONS APPLY.

DCM into its vectorized representation, by stacking

its columns one under the other. This is done in

order to comply with the conventional state-vector

formulation of model equations. Following that

approach all system equations are vectorized. As

a result, the physical insight in the plant equations

is lost, rendering the analysis of the related KF

cumbersome.

The issue of developing optimal algorithms for

estimation and control that operate on matrix systems,

while preserving the matrix formulation of the original

systems has been given much attention [12—14].

The various solutions to Wahba’s problem are

such examples. When modeling structural systems,

physical parameters are represented as matrices

like the stiffness matrix, the inertia matrix or the

damping matrix. Reference [12] features a matrix

estimator of the stiffness matrix. Matrix calculus tools,

such as the gradient matrix [13], were developed

in order to design optimal control algorithms that

operate on matrix processes. Recently, a generic

matrix Kalman filter (MKF) was introduced [14] that

operates on plants which are naturally described by

matrix equations. Notice that, beyond the notational

advantage, using an MKF instead of the associated

vectorized KF allows savings in computations (see

[14, Table V, Cases A and C for DCM estimation]).

This work is concerned with the development,

via the MKF approach, of estimators of the DCM

and of additional parameters such as constant biases

and time-varying drifts in body-mounted gyro output

errors. The matrix formalism adopted throughout

this work allows straightforward developments of

expressions for the filter noise covariance matrices.

This in turn is helpful in designing simplified filters

based on specific stochastic assumptions. Focusing

on DCM estimation and assuming gyro output, white

noise leads to filter covariance computations that can

be expressed by reduced 3£3 equations instead of

the full 9£9 equations. The more realistic case of

constant biases and time-varying drifts in the gyro

errors is handled via a state augmentation and the

development of the associated matrix state-space

model. The traditional issue of orthogonalization of

the DCM estimate, which is crucial when it is needed

for axis rotation, e.g. in inertial navigation systems,

is addressed via the technique of orthogonality

pseudomeasurement (OPM). The orthogonality

constraint being a quadratic matrix equation is,

upon some modeling transformations, recast as a

virtual matrix measurement equation of the DCM.

Then, this additional matrix measurement yields

an additional measurement update stage naturally

embedded in the formalism of the MKF. As opposed

to other orthogonalization techniques [11, 17],

the OPM technique provides the designer with a

covariance computation that is associated with the

orthogonalization procedure and, furthermore, allows

him to tune that constraint. Extensive Monte-Carlo

simulations were performed in order to illustrate the

performances of the various filters. In particular, the

relative advantages of four various orthogonalization

schemes are studied, the consequence of using

reduced covariance computations is investigated, and

the performance of the augmented filter for DCM bias

and drift estimation are shown. Furthermore, for long

span simulations, a special augmented filter is tested,

where the bias and drift estimation processes are

turned off from time to time, for filter computational

savings, while the overall DCM estimation accuracy

stays at an acceptable level.

The remainder of the paper is organized as

follows. The next section presents the mathematical

model of the DCM system in the case of white noise

in the gyro output error. This model is already known

in the literature, except for the explicit expression

describing the process noise covariance matrix,

which is presented here. Two MKFs of the DCM

are developed in the following section. The first

filter includes a full (9£9) covariance computation

algorithm. The second one includes a reduced (3£3)

covariance computation. Both filters do not include

any orthogonalization procedure. The subsequent

section presents the development of the augmented

state mathematical model, including constant biases

and time-varying drifts in the gyro output error.

Four orthogonalization procedures are proposed in

the following section. Then, the performance of the

various estimators are demonstrated and compared

via extensive Monte-Carlo simulations. Finally,

conclusions are drawn in the last section.

DCM STATE-SPACE MODEL

Process Model

Let the spacecraft body frame and the reference

frame be denoted, respectively, by B and R. Assuming

that B is rotating with respect to R,wedenoteby

! o the angular velocity vector of this rotation as

expressed in B. It is well known that the dynamics

of the DCM, D, is governed by the following matrix

differential equation [1, p. 512]

dt D =¡[! o £]D:

(1)

The matrix [! o £]in(1)isa3£3skew-symmetric

matrix and is defined according to the identity

[! o £] x =! o £ x ,where£ denotes the cross-product

and x is any 3£1 column-matrix. The discrete-time

version of (1) is the difference equation given by

D k+1 =© k D k

(2)

where © k is the transition matrix from time t k to time

t k+1 . Taking the time increment ¢t=t k+1 ¡t k small

enough, we assume that ! o is piecewise constant in

the intervals of time length ¢t,sothat© k in (2) can

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 46, NO. 1 JANUARY 2010

AUTHORIZED LICENSED USE LIMITED TO: IEEE XPLORE. DOWNLOADED ON MAY 13,2010 AT 11:53:31 UTC FROM IEEE XPLORE. RESTRICTIONS APPLY.

be approximated as

generality, we assume that the observed physical

vector is of unit length. In general, r k is accurately

known from tables or almanacs while b k is a noisy

measurement acquired on-board by sensing devices.

Modeling the measurement noise as an additive

error term v k yields the classical vector measurement

equation

(3)

The true angular velocity ! k being unknown, we

assume here that a triad of body-mounted gyroscopes

measures ! k and that the output of the gyros ! k is

corrupted by an additive error ² k , thus

! k =! k +² k :

(4)

b k =D k r k + v k

(9)

where v k is assumed to be a zero-mean white

noise sequence with known covariance matrix

R k . Furthermore, we assume that ² k and v k are

uncorrelated with one another and with the

initial attitude matrix D 0 . Equation (9) is a linear

measurement equation with respect to the state matrix

D k . Combining (6)—(9) yields a matrix state-space

model for D k , on which the MKF can be applied. For

the sake of completeness, the generic MKF [14] is

reviewed in Appendix A.

Then we substitute ! k for ! k in (3) and use the

measured transition matrix, denoted by © k ,in(2).

Considering the difference equation (2) and using a

first-order approximation in the time increment ¢t of

the Taylor series expansion of © k [1, p. 512], yields

the following process equation:

D k+1 =© k D k

=(I 3 ¡[! k £]¢t)D k +HOT

=(I 3 ¡[! k £]¢t+[² k £]¢t)D k +HOT

=(I 3 ¡[! k £]¢t)D k +([² k £]¢t)D k +HOT

=e (¡[! k £]¢t) D k +[² k £]D k ¢t+HOT

=© k D k +[² k £]D k ¢t+HOT

=© k D k +W k

DCM FILTER DESIGN

(5)

In this section we present two MKFs for the

estimation of the DCM. These filters differ by

the computational complexity of the covariance

computations. The first algorithm performs a full

covariance computation, where the estimation error

covariance matrix is of size 9£9 while the second

algorithm performs a reduced covariance computation,

where only a 3£3matrixisusedinordertorepresent

the whole estimation error covariance. For the sake

of clarity all the matrices that are involved in the

full covariance computation are denoted by capital

script letters, like P. We use normal fonts, like P,to

denote the matrices involved in the reduced covariance

computation.

where HOT denotes higher order terms (of order ¢t 2

and higher). In (5), the third equality results from (4).

The definition of © k and W k is obvious. To summarize,

D k+1 =© k D k +W k

(6)

where

(7)

W k =[² k £]D k ¢t+HOT:

(8)

The gyro error ² k is here assumed to be a zero-mean

white sequence with known covariance matrix

Q k =¢t, where the factor 1=¢t is consistent with the

assumption that ² k approximates a continuous-time

white noise process. The more realistic case that

includes constant biases and time-varying drifts in

the gyro output error will be handled in a subsequent

section. Notice from (8) that the process noise matrix

W k is state dependent, and that the first-order term is

linear in the components of ² k . The latter fact will

help at developing an expression for the process

noise covariance matrix. The process model for the

DCM is described by (6)—(8). It can be seen that these

equations feature, in their natural form, a state matrix

D k and a process noise matrix W k .

Full Covariance Filter

In order to obtain an approximate expression for

the 9£9covariancematrixofW k , which is denoted

by Q k , we neglect the HOT and substitute the best

available estimate, D k=k ,forD k in (8). This yields

Q k =covfW k g

=covfvecW k g

=covf(D k=k

−I 3 )(vec[² k £])¢tg

=covf(D k=k

−I 3 )(L² k )¢tg

=(D k=k

−I 3 )Lcovf² k gL T (D k=k

−I 3 )¢t 2

(10)

Measurement Model

where − denotes the Kronecker product [15, p. 227],

I 3 is the three-dimensional identity matrix, L is a 9£3

matrix defined as

L T =[[ e 1 £] e 2 £] e 3 £]] (11)

Assume that a physical vector quantity is observed

at each epoch time t k ; namely, we simultaneously

know r k , its decomposition in R,and b k ,itsmeasured

decomposition in B. Moreover, without loss of

CHOUKROUN ET AL.: DCM ESTIMATION FROM VECTOR OBSERVATIONS USING A MATRIX KALMAN FILTER

AUTHORIZED LICENSED USE LIMITED TO: IEEE XPLORE. DOWNLOADED ON MAY 13,2010 AT 11:53:31 UTC FROM IEEE XPLORE. RESTRICTIONS APPLY.

and e j , j =1,2,3,isthejth column of I 3 .Thethird

equality in (10) is obtained using [16, Lemma 4.3.1,

p. 254], and the fourth equality stems from the

definition of the cross-product matrix [²£]. Recalling

that the covariance matrix of ² k is given by Q k =¢t

yields

have the same covariance ¹ k ,thatis,

R k+1 =¹ k+1 I 3 :

(24)

Similarly, we assume that P 0=0 is given as follows:

P 0=0 =P 0=0

−I 3

(25)

−I 3 )¢t: (12)

The full covariance MKF is summarized as follows.

Given the matrices D 0=0 and P 0=0 , compute:

1) Time Update Equations:

Q k =(D k=k

−I 3 )LQ k L T (D k=k

where P 0=0 is known. Then, using basic properties

of the Kronecker product, one can show that the

nine-dimensional covariance equations can be reduced

to three-dimensional equations (see Appendix B).

In spite of these strong model simplifications, the

reduced estimator performs well as demonstrated

through extensive Monte-Carlo simulations. The

reduced covariance MKF is summarized as follows.

Given the initial estimate D 0=0 ,andthe3£3matrix

P 0=0 , compute

1) Time Update Equations:

(13)

D k+1=k =© k D k=k

(14)

ª k =I 3

−© k

(15)

P k+1=k =ª k P k=k ª k +Q k :

(16)

2) Measurement Update Equations:

(26)

H k+1 = r k+1

−I 3

(17)

D k+1=k =© k D k=k

(27)

S k+1 =H k+1 P k+1=k H k+1 +R k+1

(18)

P k+1=k =P k=k +Q k :

(28)

K k+1 =P k+1=k H k+1 S ¡1

k+1

(19)

2) Measurement Update Equations:

D k+1=k+1 = D k+1=k +[K k+1

K k+1

K k+1 ]

s k+1 = r k+1 P k+1=k r k+1 +¹ k+1

(29)

£[I 3

−( b k+1 ¡ D k+1=k r k+1 )] (20)

g k+1 =P k+1 r k+1 =s k+1

(30)

D k+1=k+1 = D k+1=k +( b k+1 ¡ D k+1=k r k+1 ) g k+1

P k+1=k+1 =(I 9 ¡K k+1 H k+1 )P k+1=k

£(I 9 ¡K k+1 H k+1 ) T +K k+1 R k+1 K k+1

(31)

P k+1=k+1 =(I 3 ¡ g k+1 r k+1 )P k+1=k (I 3 ¡ g k+1 r k+1 ) T

+¹ k+1 g k+1 g k+1 :

(21)

where K k+1 in (20), j =1,2,3,are3£3 submatrices

of the 9£3Kalmangainmatrix,K k+1 , such that

K T =[(K 1 ) T (K 2 ) T (K 3 ) T ]. Note from (14) and (20) that

the full covariance filter, described by (14) to (21),

produces a Kalman filter estimate of the state DCM

D k=k using the original matrices of the given plant.

(32)

the covariance time propagation (28). That is a direct

result of the assumed statistical independence among

Appendix B). The consequence of the assumptions

on the noises is that the three rows of the state matrix

are estimated based on identical covariance equations,

which are of dimension 3. Indeed, all the necessary

information is conveniently represented and processed

by 3£3 equations. If the full-scale matrices are

needed, they are readily computed from the reduced

associated matrices as follows (see Appendix B).

Reduced Covariance Filter

Our purpose is here two-fold: it is, first, to achieve

a reduction in the computational burden of the filter

and, second, to illustrate how the MKF formulation

facilitates mathematical manipulations. We assume

here that the rows of the process noise matrix W k

are uncorrelated and that they have the same 3£3

covariance matrix Q k where

Q k =Q k ¢t:

P k+1=k =P k+1=k

−I 3

(33)

P k+1=k+1 =P k+1=k+1

−I 3

(34)

(22)

S k+1 =s k+1 I 3

(35)

Therefore, the following expression for Q k is

assumed:

K k+1 = g k+1

−I 3 :

(36)

One realizes that the filter computational burden

is thus reduced by a factor of 27 (3 3 ). Recall

that the computational burden of a conventional

n-dimensional Kalman filter is dictated by its

Q k =Q k

−I 3 :

(23)

We also assume that the elements of the measurement

noise column-matrix v k are uncorrelated and that they

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 46, NO. 1 JANUARY 2010

AUTHORIZED LICENSED USE LIMITED TO: IEEE XPLORE. DOWNLOADED ON MAY 13,2010 AT 11:53:31 UTC FROM IEEE XPLORE. RESTRICTIONS APPLY.

covariance time-propagation stage, which is in O(n 3 ).

In addition and independently of the latter, let us

recall here that the implementation of the general

MKF is computationally less intensive than that of the

corresponding vectorized Kalman filter. As shown in

[14], the computational advantage of an MKF resides

in the time-propagation stage and was evaluated to

be of factor 37 for DCM estimation using vector

observations and orthogonalization, where that factor

was defined as the ratio between the numbers of

FLOP (floating point operation) per cycle.

To complete the state-space model we recall the vector

measurement equation (9)

b k+1 =D k+1 r k+1 + v k+1

(45)

where v k+1 is zero-mean white noise sequence

with covariance matrix R k+1 . It is assumed that the

sequences ² k , ¹ k , º k ,and v k are uncorrelated with

one another and with the initial state variables D 0 , c 0 ,

and m 0 . We wish to estimate the DCM, and the gyro

output parameters c k and m k via an MKF. To meet

this end, we generalize the known technique of state

augmentation to the current matrix state-space model

(38)—(45).

AUGMENTED MATRIX KALMAN FILTER

Augmented State Matrix Model : In this section we

show how to handle the case of constant biases and

Markov drifts in the gyro outputs using an MKF. It is

assumed that the gyro output is described by

P ROPOSITION 1 Let X k denote the augmented 3£5

state matrix that is defined as follows:

X k =[D k c k m k ]:

(46)

! k =! k + c k + m k +² k

(37)

The dynamics model equations of D k , c k , and m k ,

(38) , (39) ,and (42) can be written as the following

augmented 3£5 matrix difference equation

where ! k is the true angular velocity vector, c k is the

3£1 vector of constant biases, m k is the 3£1 vector

of the gyro Markov drifts, and ² k is assumed to be

a zero-mean white noise sequence with covariance

matrix Q k =¢t. In order to estimate c k and m k viaaKF

we model their dynamics as follows

X k+1 =

£ k X k ª k +W k

(47)

r=1

where

c k+1 = c k +¹ k

(38)

£ k =e (¡[! k £]¢t) , ª k =E 11 +E 22 +E 33

m k+1 =¤ m k +º k

(39)

(48a)

where

£ k =I 3 ,

ª k =E 44

¤=e (¡T¢t)

(48b)

(40)

and

£ k =¡[ d k,1 £]¢t, ª k =E 41

(48c)

T =diagf1=¿ x ,1=¿ y ,1=¿ z g:

(41)

The scalars 1=¿ x ,1=¿ y ,and1=¿ z are known constants.

In (38) and (39), ¹ k and º k areassumedtobe

zero-mean white noise sequences with covariance

matrices Q k =¢t and Q k =¢t, respectively. The addition

of ¹ k is needed for filter stability reasons: it avoids

computations involving a singular process noise

covariance matrix. As it is common practice when

designing KFs, the values of the process noise

covariance matrices, Q k , Q k and Q k are used as

tuning parameters with the usual trade-off: increasing

the filter process noise covariance speeds up the

estimation convergence but decreases the steady-state

estimation accuracy. In order to develop the DCM

process equation we only have to substitute the

expression (¡! k + c k + m k )for¡! k in (7). This yields

£ k =¡[ d k,2 £]¢t, ª k =E 42

(48d)

£ k =¡[ d k,3 £]¢t, ª k =E 43

(48e)

£ k =¤,

ª k =E 55

(48f)

£ k =¡[ d k,1 £]¢t, ª k =E 51

(48g)

£ k =¡[ d k,2 £]¢t, ª k =E 52

(48h)

£ k =¡[ d k,3 £]¢t, ª k =E 53 :

(48i)

(42)

In (48) , the vectors d k,j , j =1,2,3 , denote the three

columns of the state matrix D k and the matrices E ij

denote 5£5 matrices with 1 at position ij and 0

elsewhere. The augmented noise matrix in (47) , W k ,is

defined as

W k =[E k ¹ k º k ]

(49)

(43)

where, to first order in ¢t , E k is expressed as

E k =[² k £]D k ¢t:

(44)

(50)

CHOUKROUN ET AL.: DCM ESTIMATION FROM VECTOR OBSERVATIONS USING A MATRIX KALMAN FILTER

AUTHORIZED LICENSED USE LIMITED TO: IEEE XPLORE. DOWNLOADED ON MAY 13,2010 AT 11:53:31 UTC FROM IEEE XPLORE. RESTRICTIONS APPLY.

Plik z chomika:

Inne pliki z tego folderu:

Inne foldery tego chomika: