Crassidis J.L.Predictive filtering for nonlinear systems

PREDICTIVE FILTERING FOR NONLINEAR SYSTEMS

John L. Crassidis

Assistant Professor, Member AIAA

Department of Mechanical Engineering

Catholic University of America

Washington, D.C. 20064

Abstract.

In this paper, a real-time predictive filter is derived for nonlinear systems. This provides a method of determining optimal state estimates in the presence of significant error in the assumed (nominal) model. The new real-time nonlinear filter determines (“predicts”) the optimal model error trajectory so that the measurement-minus-estimate covariance statistically matches the known measurement-minus-truth covariance. The optimal model error is found by using a onetime step ahead control approach. Also, since the continuous model is used to determine state estimates, the filter avoids discrete state jumps, as opposed to the extended Kalman filter. The predictive filter is used to estimate the attitude of a spacecraft with the utilization of attitude sensor measurements and rate integrating gyro measurements. Results using this new algorithm indicate that the real-time predictive filter accurately estimates the attitude of the spacecraft by determining actual gyro bias errors.

Introduction

Conventional filter methods, such as the Kalman filter [1], have proven to be extremely useful in a wide range of applications, including: noise reduction of signals, trajectory tracking of moving objects, and in the control of linear or nonlinear systems. The essential feature of the Kalman filter is the utilization of state-space formulations for the system model. Errors in the dynamic system model are treated as “process noise,” since system models are not usually improved or updated during the estimation process. The process noise is essentially used to “shift” the emphasis from the model to the measurements.

The Kalman filter satisfies an optimality criterion which minimizes the trace of the covariance of the estimate error between the system model responses and actual measurements. Statistical properties of the process noise and measurement error are used to determine an “optimal” filter design. Therefore, model characteristics are combined with sequential measurements in order to obtain state estimates which meliorate both the measurements and model responses.

.Copyright c 1996 by John L. Crassidis. Published by the American Institute of Aeronautics and Astronautics, Inc. with permission.

F. Landis Markley

Staff Engineer, Associate Fellow AIAA

Guidance and Control Branch, Code 712

Goddard Space Flight Center

Greenbelt, MD 20771

In the Kalman filter, the errors in the system model are assumed to be represented by a zero-mean Gaussian noise process with known covariance. However, in actual practice the noise covariance is usually determined by an ad hoc and/or heuristic estimation approach which may result in suboptimal filter designs. Other applications also determine a steady-state gain directly, which may even produce unstable filter designs [2]. Also, in many cases such as nonlinearities in the actual system responses or non-stationary processes, the assumption of a Gaussian model error process can lead to severely degraded state estimates.

In addition to nonlinear model errors, the actual assumed model may be nonlinear (e.g., three-dimensional kinematic and dynamic equations [3]). The filtering problem for nonlinear systems is considerably more difficult and admits a wider variety of solutions than does the linear problem [4]. The extended Kalman filter is a widely used algorithm for nonlinear estimation and filtering [5]. The essential feature of this algorithm is the utilization of a first-order Taylor series expansion of the model and output system equations. The extended Kalman filter retains the linear calculation of the covariance and gain matrices, and it updates the state estimate using a linear function of the measurement residual; however, it uses the original nonlinear equations for state propagation and in the output system equation [5]. But, the model error statistics are still assumed to be represented by a zero-mean Gaussian noise process.

A new approach for performing optimal state estimation in the presence of significant model error has been developed by Mook and Junkins [6]. This algorithm, called the Minimum Model Error (MME) estimator, unlike most filter and smoother algorithms, does not assume that the model error is represented by a Gaussian process. Instead, the model error is determined during the MME estimation process. The algorithm determines the corrections added to the assumed model such that the model and corrections yield an accurate representation of the system behavior. This is accomplished by solving system optimality conditions and an output error covariance constraint. Therefore, accurate state estimates can be determined without the use of precise system representations in the assumed model. Also, the MME estimator can be applied to systems with nonlinear models. The MME estimates are determined from a solution of a two- point-boundary-value-problem ([6-7]). Therefore, the MME

1

American Institute of Aeronautics and Astronautics

estimator is a batch (off-line) estimator which must utilize post-experiment measurements.

The filter algorithm developed in this paper can be implemented in real-time (as can the Kalman filter). However, the algorithm is not limited to Gaussian noise characteristics for the model error. Essentially, this new algorithm combines the good qualities of both the Kalman filter (i.e., a real-time estimator) and the MME estimator (i.e., determines actual model error trajectories). The new algorithm is based on a predictive tracking scheme first introduced by Lu [8]. Although the problem shown in [8] is solved from a control standpoint, the algorithm developed in this paper is reformulated as a filter and estimator with a stochastic measurement process. Therefore, the new algorithm is known as a predictive filter. The advantages of the new algorithm include: (i) the model error is assumed unknown and is estimated as part of the solution, (ii) the model error may take any form (even nonlinear), and (iii) the algorithm can be implemented on-line to both filter noisy measurements and estimate state trajectories.

The organization of this paper proceeds as follows. First, the basic equations and concepts used for the filter development are reviewed. Then, a predictive filter is derived for nonlinear systems. This approach determines optimal state estimates in real-time by minimizing a quadratic cost function consisting of a measurement residual term and a model error term. Then, the concept of the covariance constraint is introduced for determining the optimal model error weighting matrix. Finally, an example involving the estimation of a spacecraft from attitude sensors and gyros is presented.

Nonlinear Predictive Filter

Preliminaries

In this section, the nonlinear predictive filter algorithm is derived. This development is based upon the duality which exists between the predictive controller for nonlinear systems by Lu [8] and a general estimation problem. In the nonlinear predictive filter it is assumed that the state and output estimates are given by a preliminary model and a to-be- determined model error vector, given by

ˆ

(t ))+G (xˆ (t ))d (t )

(1a)

x (t )= f (xˆ

yˆ (t )= c (xˆ (t ))

(1b)

where

f

n

ˆ

n

R

is sufficiently differentiable,

R

is the

x(t )

state

estimate

vector, d (t ) Rq represents

the

model

error

vector,

G (xˆ (t )):Rn → Rn×q is

the model-error

distribution

matrix,

c (xˆ (t )) Rm is

the

measurement

vector,

and

ˆ

R

m

is the estimated output vector.

State-observable

y (t )

discrete measurements are assumed for Equation (1b) in the following form

					y	k = c (x (tk ))+vk			(2)
where	y	k	Rm	is the measurement vector at time tk , x (tk )
is the		true state vector, and v					k	Rm	represents the

measurement noise vector which is assumed to be a zeromean, Gaussian white-noise distributed process with

E{vk }= 0	(3a)
E{vk vTl }= Rδkl	(3b)
where R Rm×m is a positive-definite	measurement
covariance matrix.

A Taylor series expansion of the output estimate in Equation (1b) is given by

	yˆ (t + ∆t )≈ yˆ (t )+ z (xˆ (t ), ∆t )+ Λ(∆t )S (xˆ (t ))d (t )									(4)
where the ith row of z (xˆ (t ), ∆t )								is given by
				ˆ	∆	t )	= pi	∆tk k		(5)
		zi (x (t ),					∑	k !	Lf (ci )
							k =1

where pi , i =1, 2,…, m , is the lowest order of the derivative

ˆ

in which any component of d (t ) first appears

of ci (x(t ))

ˆ

(t )

on

due to successive differentiation and substitution for xi

the right side.

Lk (c )is a

k th

order Lie derivative, defined

f

i

by

Lk

(c

)= c

for k = 0

f

i

i

k −1

(ci )

(6)

Lk

(c

)

=

∂ Lf

f

for k ≥1

f

i

∂ xˆ

Λ(∆t ) Rm×m is a diagonal matrix with elements given by

λ

= ∆t pi

,

i =1,2,…, m

(7)

ii

pi !

S (xˆ (t )) Rm×q is a matrix with each ith row given by

si

={Lg1 Lpfi −1 (ci ) ,…, Lgq

Lpfi −1 (ci ) },

(8)

i =1, 2,…, m

where the Lie derivative with respect to Lg j in Equation (8) is defined by

2

American Institute of Aeronautics and Astronautics

∂ Lpi −1

(c

)

L

Lpi −1

(c

)

≡

f

i

g

j

,

j =1, 2,…, q

(9)

g j

f

i

∂ xˆ

Equation (8) is in essence a generalized sensitivity matrix for nonlinear systems.

Nonlinear Filtering

A cost functional consisting of the weighted sum square of the measurement-minus-estimate residuals plus the weighted sum square of the model correction term is minimized, given by

(

)

2 {

}

{

}

J d (t )

=

1

y

(t

+ ∆

t )

− ˆ

+ ∆

t )

T

R

−1

y (t

+ ∆

t )

− ˆ

+ ∆

t )

y (t

y (t

+

1

dT (t )W d

(t )

2

(10)

where W Rq×q

is positive semidefinite.

Also, a constant

sampling

rate

is

assumed

so

that

y

(t + ∆t )≡

y

k +1 .

Substituting Equation (4), and minimizing Equation (10) with respect to d (t ) leads to the following model error

Covariance Constraint

The weighting matrix (W ) in Equation (11) can be

determined on the basis that the measurement-minus-estimate error covariance matrix must match the measurement-minus- truth error covariance matrix (see [6]). This condition is referred to as the “covariance constraint,” shown as

	1		N
			∑{ek −e}{ek −e}T	≈ R	(14)
		N
			k =0
	where e is the sample mean of y − yˆ ,			and N	is a large

number. A test for whiteness can be based upon the autocorrelation function matrix of the measurement residual [5]. The maximum likelihood estimate of the m×m autocorrelation function matrix for N samples is given by

Ck =	1	N	e eT	(15)
	N
		∑ i i−k
		i=k

A 95% confidence interval for whiteness using a finite sample length is given by [5]

−1

ρiik

≤1.96 / N1/ 2

(16)

d (t )

{

(

t )S (x)

T

R

−1

(

t )S (x)

W}

= −

∆

Λ

∆

+

Λ

ˆ

ˆ

(11)

where ρii corresponds to the diagonal elements resulting by

× Λ(∆t )S (xˆ) T R−1 z (xˆ, ∆t )− y (t + ∆t )+ yˆ (t )

By using the matrix inversion lemma [9], the model error in Equation (11) can be re-written as

d (t )

= −

ˆ

∆

t )

−

y (t

+ ∆

t )

+

ˆ

(12)

M (t ) z

(x,

y

(t )

where

M (t )

=

W

−1

− Λ

∆

ˆ

T

I

( t )S

(x)

−1 Λ

T

−1

× Λ ∆

ˆ

(

∆

ˆ

+

R}

(13)

{

(

t )S (x)W

t )S (x)

× Λ

(

∆

ˆ

W

−1

Λ

∆

ˆ

T

R

−1

t )S (x)

)

( t )S (x)

The measurement at time tk +1 is used in Equation (12) to find the appropriate d for the nonlinear propagation of the state estimates from time tk to time tk +1 . By looking ahead to the next measurement to compute d , the predictive filter gives a

continuous state estimate without the jumps exhibited by the Kalman filter. The matrix W serves to weight the amount of model error added to correct the assumed model in Equation

(1). As W decreases, more model error is added to correct the model, so that the estimates more closely follow the measurements. As W increases, less model error is added, so that the estimates more closely follow the propagated model.

normalizing the autocorrelation matrix by the zero-lag elements, given by

ρiik =	cii	(17)
	k
	cii
	0

If the confidence interval in Equation (16) and the covariance constraint in Equation (14) are met, then the weighting matrix is optimal. Therefore, the proper balance between model error and measurement residual has been achieved. If the measurement residual covariance is higher than the known measurement error covariance (R), then W should be

decreased to less penalize the model error. Conversely, if the residual covariance is lower than the known covariance, then W should be increased so that less unmodeled dynamics are added to the assumed system model.

The sample measurement covariance can be determined from a recursive relationship given by [10]

ˆ

ˆ

1

k

T

ˆ

R

+

= R

+

(e

k

+1

−e

k

)(e

k+1

−e

k

)

− R

(18a)

k

1

k

k +1

k

k +1

e k +1 = e k +

1

(ek +1 −e k )

(18b)

k

+1

ˆ

→ R .

The covariance constraint is met when Rk

Even though the model error is determined by Equation (11) or (12), it still involves stochastic processes. Therefore, a

3

American Institute of Aeronautics and Astronautics

covariance of the model error can be derived. First, the	Stability
covariance constraint is re-written as	Filter Stability

(

k

− ˆ

k )(

k

− ˆ

k )

=

T

E

y

y

y

y

R

Substituting Equation (2) into Equation (19), and using

E{yk vTk }= E{vk yTk }= E{yˆk vTk }= E{vk yˆTk }= 0

leads to

E{yk yTk }− yˆk yˆTk = R

If Equation (14) is met, the process is stationary so that

E{yk +1 yTk +1}− yˆk +1 yˆTk +1 = R

(19)

The effect of W on filter stability and bandwidth can be

determined by applying a discrete error analysis. The filter

residual is given by

=

+ ∆

− ˆ

+ ∆

e(t

+∆

t )

y (t

t )

(t

t )

(27)

(20)

y

Substituting Equations (4) and (12) into Equation (27) leads to

e(t

+∆

t )

=

−Λ

(

∆

ˆ

ˆ

(t

+ ∆

t )

(28)

I

t )S (x)M

(t ) e

(21)

where

ˆ

(t

+∆

t )

≡

y (t

+ ∆

t )

−

ˆ

−

ˆ

∆

t )

(29)

e

y (t )

z (x,

(22)which is the predicted measurement residual at t + ∆t assuming d = 0 .

For constant a sampling interval, Equation (22) is equivalent

If

S is square and full rank, then

ΛS M is also full rank.

to

AsW → 0 ,

then ΛS M → I ,

and (I −ΛS M )→ 0 .

This

E

{

y

(t

+ ∆

t )y

T

(t

+ ∆

t )

=

ˆ

+ ∆

ˆT

(t

+ ∆

t )

+

R

(23)

y (t

t )y

approaches a deadbeat response for

the

filter

dynamics.

}

As long as the process remains stationary, Equation (23) is

AsW → ∞, then M → 0 , and (I −ΛS M )→ I . This yields a

filter response with discrete eigenvalues approaching the unit

valid even if the covariance constraint is not satisfied. Also,

circle. As

long as the covariance matrix is positive, the

since the optimal model error solution in Equation (11) is a

discrete eigenvalues of the filter will lie within the unit circle.

function of the stochastic measurement noise process,

a test

Therefore, the filter remains contractive [11].

for the whiteness of the “determined” model error can be

Robustness

found by using the correlation function in Equations

(15)-

(17), replacing

e

with d .

If the model error is sufficiently

Lu [12] has shown that the dual control problem achieves

white, then the

covariance of

the model

error

can

also be

input/output linearization, and asymptotic tracking of any

determined using a recursive formula shown in Equation (18),

given

trajectory if pi ≤ 4 .

An

analysis of

the

robustness

again replacing

e

with d .

Another form for the model error

covariance can be determined by using Equation (11), and

properties

in

the

face

of

unmodeled

dynamics

for pi

=1 ,

assuming that

W

=

0

,

and

square

and

nonsingular

ˆ

has

also

been

}

{

}

S (x)

{

ˆ

+ ∆

=

+ ∆

ˆT

=

shown

ˆ

m×3

In

this

≤

the

case of pi =1 , W

≠ 0 ,

E

y (t )v

(t

t )

E

v (t

t )y

(t )

0

(24a)

and S (x)

R

,

where

m

3

is

considered.

The

{

}

{

}

T

T

continuous output estimate for

pi =1 is given by [12]

E

z (t )v

(t + ∆t )

=

E

v (t + ∆t )z

(t )

= 0

(24b)

ˆ =

(c)

+

ˆ

(30)

which leads to

y

L f

S (x)d

{

}

{

}

=

ˆ

−

ˆ

+ ∆

+

+

where

E d (t )d

T

(t )

M (t )

(t )

(t

t )

z (t )

R

M

T

(t )(25)

y

y

Lf (c1 )

where

L f

≡ a aT

(c)≡

(31)

a

for any a

(26)

Lf (cm )

Therefore, the relative magnitude of the model error can now be determined. In fact, if the determined model error process is truly white, then the inverse of the weighting matrix (W )

can be shown to be the maximum likelihood estimate of the model error covariance. This can be used to determine an adaptive scheme for determining W to satisfy the covariance constraint (which will be reported at a later time).

Suppose that the unmodeled errors are introduced into the output estimate by

ˆ

=

+ ∆

+

ˆ

+ ∆

ˆ

(32)

y

L f (c)

L f (c)

S (x)

S (x) d

and suppose that L f (c)

and ∆L f

(c)

are bounded by

L f

(c)

≤ n1,

∆L f (c)

≤ n2,

for all x X

(33)

4

American Institute of Aeronautics and Astronautics

Furthermore, assume that ∆S (xˆ) is represented by

∆

ˆ

= δ

ˆ

ˆ

(34)

S (x)

(x)S

(x)

where

δ

ˆ

is a scalar, continuous function with bound given

(x)

−

< δ

ˆ

<

n3 . Assuming

by

1

(x)

that

the

model

errors and

measurement

errors

are

isotropic

leads

toW = w I ,

and R = r I . Then, the matrix inverse in Equation (11) can be written as (suppressing arguments)

{

}

[

T

−1

−1

{(

)

}

−1

ΛS

]

R ΛS +W

=

v −σ

I +C

(35)

where

C ≡ ∆t2

ST S

(36a)

r

σ =

∆t2 tr (ST

S )

(36b)

2 r

v = w +σ

(36c)

By the Cayley-Hamilton theorem, any meromorphic function of C can be expressed as a quadratic in C [13], yielding

{(v −σ )I +C}−1 =

1

(α I + β C +C2 )

(37)

γ

where

α = v2 −σ2 + k

(38a)

β = −(v +σ )

(38b)

γ = (v −σ )α + ∆ = wα + ∆

(38c)

k = tr (adjC )

= ∆t24

tr

adj

(ST S )

(38d)

r

∆ = ∆t36

det (ST S )

(38e)

r

Therefore, the error dynamics become

e = −

∆t

(1+δ )Q e + y − L

f

−∆L

f

r γ

(39)

∆t2

(1

+δ )Q L

+

f

− y

where

r γ

(

S ST

)

+

∆t2

(

S ST

)

2

+

∆t4

(

S ST

)

3

r

β

r2

(40)

Q ≡ α

Now, define a Lyapunov functionV = e 2

2 .

Using

the

norm inequality [14], and the fact that

eT Q e ≥

1

eT e

(41)

leads to

Q−1

V ≤ −

2 ∆t

(1+δ

)

V +

eT

y − L f −∆L f

(42)

r γ

Q−1

∆t2

(1+δ )

T

+

e

Q

L f

− y

r γ

(4 z)

a b ≤ z a2 +b2

Next, using the well known inequality

for any a , b ,

and z > 0 ,

and

defining

ξ ≡ ∆t (1+δ )

(r γ )

yields

2ξ

y

− L f

−∆L f

2

V ≤ −

+ 4 z

V +

Q−1

4 z

2

2

(43)

∆t2ξ2

+

Q

L f − y

4 z

Substituting 4 z = ξ

Q−1

leads to

V ≤ −

ξ

V +b

(44)

Q−1

where

b ≡

Q−1

1

y − L f

−∆L f

2 + ∆t2ξ

Q

2

L f − y

2

(45)

ξ

Therefore, Equation (44) can be solved to yield

b

Q−1

Q

−1

b

Q−1

V ≤

V

−

e−ξ t

+

(46)

0

ξ

ξ

where V ≥ b Q−1

ξ

is required to maintain the inequality.

0

Defining the bounds

θ = inf (1+δ )> 0

(47a)

m

y

∞ = tmax0, t

∑

y

i

(47b)

f i=1

and using the matrix norm inequality again leads to

e

≤

2

r γ

Q−1

µ

2

+

2 ∆t

Q−1

Q

µ2

(48)

∆tθ

1

2

where

5

American Institute of Aeronautics and Astronautics

µ

=

y

−n

−n

(49a)

1

1

2

∞

µ2

= θ

(n1 −

y

∞ )

(49b)

Assuming that S ST ≤ s1 for all S ST leads to the following bound on Q

Q

≤α s + ∆t2

β

s2

+ ∆t4

s3

(50)

1

r

1

r2

1

Also, tr (ST S )≤ 3 s1 and det (ST S )≤ s13 , which leads to

σ ≤		3∆t2			s	(51a)
		2 r		1
k ≤	9 ∆t4				s2	(51b)
		r2		1
∆ ≤		∆t6		s3		(51c)
		r3		1

Substituting Equation (51) into Equations (38) and (50) leads to the following bounds on Q and γ

can also be determined using a Cayley-Hamilton expansion, but becomes increasingly more complicated.

Numerical Stability

If the system is unobservable then ST S is not full rank. However, the filter can compensate for this by adding more model correction. It can be shown that the filter remains stable for bounded model uncertainties as long as

		(v −σ )α + ∆ > 0			(55)
If ST S is	not full	rank, then ∆ = 0 , which			leads to the
following condition
		v >	∆t2	tr (ST S )	(56)
			2 r
Therefore,	the filter	remains		contractive as long as w > 0 .

This condition is always met, but Equation (56) can be used to help determine any numerical difficulties (i.e., large values of σ may produce numerical difficulties). One possible solution is to make r as large as possible. However, then w will be adjusted to meet the covariance constraint, so that the numerical difficulties remain. Another solution to this problem is to use smaller sampling interval, but this may not be possible. A more practical solution is to utilize a “U − D ” factorization of Equation (13) [15].

≤ w2s

4 w∆t

2s2

13

∆t4s3

Q

+

1

+

1

1

r

r2

3

3 w2∆t2s

9 ∆t4s2

∆t6s3

1

1

1

γ ≤ w

+

+

+

r3

r

r2

A bound on Q−1 is found by writing it as

∆t

2

−1

−1

Q

−1

= γ

T

S

T

S

r

S

S + wI

≤ γ

(S ST )

−1

∆t

2

ST S + wI

r

(52a)

Cases

Case 1. Let pi

=1 for the output system.

Equations

(52b)

(5), (7) and (8) reduce to

z

= ∆

t H

ˆ

ˆ

(x) f

(x)

(57a)

ˆ

≡

∂

yˆ

(57b)

H (x)

∂ xˆ

Λ = ∆t I

(57c)

S

=

ˆ

ˆ

(57d)

H (x)G (x)

(53)Therefore, the model error trajectory in Equation (12) is given by

(S ST )

−1

∆t

2

≤ γ

ST S

r

+ w

Assuming that

(S ST )−1

≤ s2

for all S ST

leads to

Q−1

≤ γ s

2

(54)

∆t

s

+ w

2

r

1

Therefore Equations (48), (52), and (54) define the bound for the error dynamics under unmodeled uncertainty. Similar results can be obtained for1 < pi ≤ 4 . The case where q > 3

H GW −1GT HT

+ ∆t−2 R

−1

d = −∆t I −W −1GT HT

H G

(58)

H

R

{y

(t )

y (t

t )

t H f }

W G

T

T

× −1

−1

ˆ

−

+ ∆

+ ∆

Case 2. Consider the following system

ˆ

= f

(xˆ , xˆ

)

(59a)

x

1

1

1

2

ˆ

= f

(xˆ

)+G

(xˆ

)d

(59b)

x

2

2

2

2

2

yˆ = c (xˆ1 )

(59c)

6

American Institute of Aeronautics and Astronautics

with pi = 2 . Equation (59a) usually defines the kinematics,

and Equation (59b) usually defines the dynamics of a system. Equations (5), (7) and (8) now become

+ ∆t

2

∂ L1

∂ L1

z = ∆t L1

f

f

(xˆ

, xˆ

)+

f

f

(xˆ

)

(60a)

f

2

∂ xˆ

1

1

2

∂ xˆ

2

2

2

1

L1

≡

∂ c

f

(xˆ

, xˆ

)+

∂ c

f

(xˆ

)

(60b)

f

1

1

2

2

2

∂ xˆ1

∂ xˆ2

Λ =

∆t2

I

(60c)

2

ω

−[ω×]

(64a)

Ω(ω)≡

−ωT

0

q I

+ q

×

Ξ(

)≡

4

3×3

13

q

(64b)

−

q

T

13

where I3×3 is a 3×3 identity matrix. The 3×3 dimensional matrices [ω×] and q13 × are referred to as cross product

S =

∂ L1f

G2 (xˆ2 )

(60d)

matrices since a ×b = [a ×]b , with

∂ xˆ2

0

−a3

a2

Spacecraft Attitude Estimation

[a ×]

≡

a

0

−a

(65)

3

a

1

−a

0

In this section, the predictive filter is used to determine

2

1

the attitude of a spacecraft using gyros and attitude sensor

Since a three degree-of-freedom attitude system is represented

measurements. First, a brief review of attitude kinematics and

by a four-dimensional vector, the quaternions cannot be

gyro dynamics is shown. Next, a brief review of the Kalman

independent.

This

condition

leads

to

the

following

filter for attitude estimation is shown. Then, a predictive filter

normalization constraint

for attitude estimation is developed. Finally, simulation

qT q = qT

q

+ q42

=1

(66)

results using the predictive filter to estimate the attitude of the

Tropical Rainfall Measurement Mission (TRMM) spacecraft

13

13

The measurement model is assumed to be of the form

[16] are shown.

Attitude Kinematics and Gyro Dynamics

given by

BB = A(

q

)BI

(67)

The attitude is assumed to be represented by the

quaternion, defined as

where BI

is a

3×1 dimensional vector of some reference

q

object (e.g., a vector to the sun or to a star, or the Earth’s

q =

13

(61)

magnetic field vector) in a reference coordinate system,

BB is

q

4

a 3×1 dimensional vector

defining

the

components

of the

with

corresponding reference vector measured in the spacecraft

q1

body frame, and

A(q)

is given by

q

≡

=

ˆ

θ

(62a)

13

q2

nsin (

/ 2)

A

q

=

q2

−qT q

I

+ 2 q

qT

−2 q

q

×

(68)

q3

(

)

( 4

13

13 )

3×3

13

13

4

13

q4 = cos(θ / 2)

(62b)

where A SO (3), which is a Lie group of orthogonal matrices

where nˆ is a unit vector corresponding to the axis of rotation,

with determinant 1 (i.e., AT A = I and det (A)=1).

A simpler

and θ is the angle of rotation.

form for the attitude matrix in Equation (68) is given by

The quaternion kinematic equations of motion are derived

A(q)= −ΞT (q)Ψ(q)

(69)

by using the spacecraft’s angular velocity ( ω ),

where

1

Ω(ω)

1

Ξ(

)ω

q

=

q

=

q

(63)

−q

+ q

×

2

2

I

3×3

where Ω(ω) and Ξ(

)

Ψ(

)

4

13

q

(70)

q

are defined as

≡

q

T

13

7

American Institute of Aeronautics and Astronautics

ω = ωg −ωb −η1

Also, another useful identity is given by

component of d first appears in qˆ is two, so that pi = 2 .

Ψ(q)ω = Γ(ω)q					for any ω3×1	(71)	Therefore, the Λ , z
where
		−[ω×]			−ω	(72)
		Γ(ω)≡
			ωT
					0

The true angular velocity is assumed to be modeled by

(73) where

, and e quantities are given by

Λ =	∆t2					(79a)
	2		I
1	+	∆t2			2	(79b)
z = ∆tL f		2			L f
e = BB − A(qˆ)BI						(79c)

ω is

ωg is

Lk

≡

Lk

(c

)

(80)

where

the

true

angular

velocity,

the

gyro-

f

i

f

i

determined angular velocity, and ωb

is the gyro drift vector,

Using the definitions in Equations (64b), (71), and (72), the

which is modeled by

derivative of Equation (78) with respect to qˆ

is

ωb =

η

2

(74)

∂ c

= −2ΞT

(qˆ)Γ(BI )

(81)

The 3×1 vectors,

η

η

∂ qˆ

1 and

2 , are assumed to be modeled by

Therefore, L1f is given by

Gaussian white-noise processes with

E η

(t )

= 0

i =1, 2

(75a)

1

T

{

i

}

L f

= −Ξ

(qˆ)Γ(BI )Ξ(qˆ)ωˆ

(82)

(t )ηT

(t′)

(t −t′)

(75b)

E η

= Q δ

δ

i, j =

1, 2

The derivative of Equation (82) with respect to qˆ

is given by

{

i

j

}

i

ij

∂ L1f

Predictive Filtering

T

(

)

(

I )

The nonlinear predictive filter minimizes the following

∂ qˆ

= 2

ωˆ

× Ξ

qˆ

Γ

B

(83)

J = 1 {BB − A(qˆ)BI

}

R−1 {BB − A(qˆ)BI }

ˆ

is given

The derivative of Equation (82) with respect to

ωb

T

2

1

t+∆t

t+∆t

(76)

by

1

T

(t )W d (t )

+

d

∂

L f

= ΞT

(qˆ)Γ(BI )Ξ(qˆ)

2

(84)

subject to

∂ωˆb

Therefore, L2f is given by

ˆ

1

Ω(ωˆ )qˆ

1

Ξ(qˆ)ωˆ,

qˆ (t0 )= q

q

=

=

0

(77a)

2

2

f

ΞT

(

qˆ

)

Γ

(

B

I

)

Ξ

(

qˆ

)

ωˆ

(85)

ˆ

ˆ

(t0 )= 03×1

L2 = ωˆ

×

ωb = d,

(77b)

ωb

The S matrix, which is formed using Equation (8) is given by

ωˆ = ωg −ωˆb

(77c)

S = ΞT (

qˆ

)Γ(BI )Ξ(

qˆ

)= − A(

qˆ

)BI ×

(86)

This has the form given by Equation (59), with

xˆ1 = qˆ

A(

)BI

×

and xˆ2 =ωˆb . Since the body measurements (BB )

The 3×3 matrix

qˆ

has

at

most rank

2, which

are used as

the required tracking trajectories, the output

vector

in

reflects the fact that there is no information about rotations

Equation (2) is given by

around the current measurement vector [17].

ˆ

=

A

ˆ

(78)

The extension to multiple measurement sets is achieved

c (x)

(q)BI

by stacking these measurements, e.g.,

Since c depends on qˆ

and not explicitly on ωˆb , the lowest

order

time

derivative

of

Equation

(78)

in

which

any

8

American Institute of Aeronautics and Astronautics

e

field reference is modeled using a 10th order International

1

Geomagnetic

Reference Field

(IGRF) model

[19].

TAM

e =

e2

(87)

sensor noise is modeled by a Gaussian white-noise process

with a mean of zero and a standard deviation of 0.5 mG.

em

The two DSS’s each have a field of view

of

about

sen

where msen

is the total number of vector measurement sets

50°× 50° (see [16] for more details). The two DSS’s combine

to provide sun measurements for about 2/3 of a complete

available at time tk .

orbit.

The DSS sensor noise is also modeled by a Gaussian

An advantage of the nonlinear predictive filter over the

white-noise process

with a

mean

of

zero

and

a

standard

deviation of 0.05°. The gyro “measurements” are simulated

conventional

Kalman filter

for

attitude

estimation

is

that

using

Equations (73)

and (74),

with

a gyro noise

standard

quaternion

normalization

is

always

preserved.

The

deviation of 0.062 deg/hr, a ramp noise standard deviation of

normalization

constraint

leads

to

a

singularity

in

the

0.235 deg/hr/hr, and an initial drift of -0.1 deg/hr.

covariance matrix of the Kalman filter. Several methods are

shown in references [17] and [18] which overcome this

A plot of the roll, pitch, and yaw attitude errors using the

difficulty. However, since Equation (77) is used to determine

propagated gyro model for a typical simulation run is shown

the quaternion estimate, normalization is always maintained in

in Figure 3.

Clearly, the gyro model used in the simulation

the nonlinear

predictive

filter.

Also, the predictive

filter

produces significant errors in pitch.

Also, the roll and yaw

determines the model-error trajectory as part of the solution,

errors eventually drift to large errors.

A plot of the gyro-bias

as opposed to the Kalman filter which assumes that this error

estimates using the predictive filter is shown in Figure 4. The

is modeled by a Gaussian process with known covariance.

predictive filter is able to accurately estimate for the gyro-

Attitude Estimation of TRMM

biases. A plot of the predictive filter covariance is shown in

Figure 5. This shows that the y-axis gyro-bias covariance is

In this section, the predictive filter is used to estimate the

smaller than

the x and z-axes.

This

is most likely due to

attitude of the TRMM spacecraft from vector measurement

roll/yaw quarter-orbit coupling for Earth-pointing spacecraft

observations and gyro measurements. The TRMM spacecraft

(i.e., a yaw rate error becomes a roll angle error as the

(see Figure 1) is due to be launched in 1997 with a nominal

spacecraft rotates in the orbital plane).

A plot of the attitude

mission lifetime of 42 months.

The main objectives of this

errors using the predictive filter is shown in Figure 6.

mission

include: (i) to obtain

multi-year measurements of

A plot of the quaternion normalization for the Kalman

tropical rainfall, (ii) to understand how interactions between

filter and the predictive filter is shown in Figure 7.

For the

the sea,

air,

and land masses

produce

changes

in

global

Kalman filter, the top of Figure

7

shows

the post-update

rainfall and climate, and (iii) to help improve the modeling of

normalization

error,

where

the

quaternion

is

explicitly

rainfall processes and their influence on global circulation.

normalized before being propagated to the next measurement.

The spacecraft is three-axis stabilized in a near circular

The Kalman filter has larger errors at the start of the

(350 km) orbit with an inclination of 35°. Figure 2 shows the

simulation run due to large initial updates.

This shows that

position of the spacecraft for a typical orbit.

The nominal

the linearization assumption in the Kalman filter becomes

Earth-pointing mission mode requires a rotation once per orbit

more invalid as the updates in the state estimates become

about the spacecraft’s y-axis.

The

attitude

determination

larger.

However, from the bottom plot of Figure 7, it is clear

hardware consists of an Earth Sensor Assembly (ESA),

that the predictive filter is able accurately estimate the attitude

Digital Sun Sensors (DSS), Coarse Sun Sensors (CSS), a

of the spacecraft, while preserving quaternion normalization

Three-Axis Magnetometer (TAM), and gyroscopic rate

in the filter (to within numerical accuracy).

sensors.

The attitude control hardware includes three

Magnetic Torquer Bars (MTB) which are used to provide

magnetic momentum unloading capability, and a Reaction

Wheel Assembly (RWA) which consists of four wheels in a

pyramidal arrangement to maximize momentum storage

capability along a preferred axis.

Primary attitude knowledge is determined by the ESA

and gyros. However, in the event of ESA failure, a

contingency mode is used which utilizes the FSS, TAM, and

gyros. This mode is used for attitude estimation in the

simulations

for this paper.

The

simulated

spacecraft

completes an orbit in approximately 90 minutes, giving a

rotation rate of 236 deg/hr about the spacecraft’s y-axis while

Figure 1 TRMM Spacecraft

holding the remaining axis rotations near zero. The magnetic

9

American Institute of Aeronautics and Astronautics

Latitude (Deg)

					Geodetic Position on Earth												Predictive Filter Covariance
	90													10−3
	80												(Deg^2/Hr^2)
	70													10−4
	60
	50												X	10−5
	40
														0	1	2	3	4	5	6	7	8
	30
	20												(Deg^2/Hr^2)Y	10−3
	−20													10−5
	10
	0													10−4
	−10
	−30													0	1	2	3	4	5	6	7	8
	−40
	−50												(Deg^2/Hr^2)Z	10−3
	−90													−5
	−60
	−70													10−4
	−80
	0	30	60	90	120	150	180	150	120	90	60	30	0	10
														0	1	2	3	4	5	6	7	8
						Longitude (Deg)
																		Time (Hr)

Figure 2 TRMM Spacecraft Position

Figure 5 Predictive Filter Covariance

Errors Between Truth and Propagated Gyro Solution

	0.1										0.1
Roll (Deg)
	0									Roll (Deg)	0
	−0.1										−0.1
	0	1	2	3	4	5	6	7	8
											0

	0										0.1
Pitch (Deg)
	−0.5									Pitch (Deg)	0
	−1
											−0.1
	0	1	2	3	4	5	6	7	8
											0

	0.1										0.1
Yaw (Deg)
	0									Yaw (Deg)	0
	−0.1
											−0.1
	0	1	2	3	4	5	6	7	8
											0
					Time (Hr)

Figure 3 Propagated Gyro Attitude Errors

Errors Between Truth and Predictive Filter Solution

1

2

3

4

5

6

7

8

1

2

3

4

5

6

7

8

1	2	3	4	5	6	7	8
			Time (Hr)

Figure 6 Predictive Filter Attitude Errors

Predictive Filter Gyro Drift Estimation

	0
X (Deg/Hr)	−0.1
	−0.2	1	2	3	4	5	6	7	8
	0

	0
Y (Deg/Hr)	−0.1
	−0.2	1	2	3	4	5	6	7	8
	0

	0
Z (Deg/Hr)	−0.1
	−0.2	1	2	3	4	5	6	7	8
	0
					Time (Hr)

Figure 4 Predictive Filter Gyro-Biases Estimates

	x 10	−5	Kalman Filter
	1
(Db)	0.5
	0
Norm
	−0.5
	−1	0.1	0.2	0.3	0.4	0.5
	0
			Time (Hr)

	x 10	−10	Kalman Filter
	5
	4
(Db)	3
	2
Norm
	1
	0
	−1		4	6	8
	2

Time (Hr)

−15	Predictive Filter
x 10

		4
	(Db)	2
		0
	Norm
		−2

−4

0	1	2	3	4	5	6	7	8
				Time (Hr)

Figure 7 Quaternion Normalization for EKF and PF

10

American Institute of Aeronautics and Astronautics