March 2008

State Estimation for Linear and NonlinearEquality-Constrained Systems

by

Bruno O. S. Teixeira∗†, Jaganath Chandrasekar†,Leonardo A. B. Torres∗, Luis A. Aguirre∗, and Dennis S. Bernstein†

∗Department of Electronic Engineering, Federal University of Minas GeraisBelo Horizonte, MG, Brazil, 31270-010, brunoot,torres,aguirre@cpdee.ufmg.br

†Department of Aerospace Engineering, University of MichiganAnn Arbor, MI, USA, 48109-2140, jchandra,dsbaero@umich.edu

Abstract

This paper addresses the state-estimation problem for linear and nonlinear systemsfor the case in which prior knowledge is available in the form of an equality constraint.The equality-constrained Kalman filter is derived as the maximum-a-posteriori solution tothe equality-constrained state-estimation problem for linear and Gaussian systems and iscompared to alternative algorithms. Then, four novel algorithms for nonlinear equality-constrained state estimation based on the unscented Kalman filter are presented, namely,the equality-constrained unscented Kalman filter, the projected unscented Kalman filter, themeasurement-augmentation unscented Kalman filter, and the constrained unscented Kalmanfilter. Finally, these methods are compared on linear and nonlinear examples.

Key Words: Equality constraints, state estimation, Kalman filtering, unscented Kalmanfilter

1. Introduction

The classical Kalman filter (KF) for linear systems provides optimal state estimates under

standard noise and model assumptions (Jazwinski 1970). In practice, however, additional

information about the system may be available, and this information may be useful for im-

proving state estimates. A scenario we have in mind is the case in which the dynamics and

the disturbances are such that the states of the system satisfy an equality or inequality con-

straint (Robertson and Lee 2002, Goodwin et al. 2005). For example, in a chemical reaction,

the species concentrations are nonnegative (Massicotte et al. 1995, Chaves and Sontag 2002),

whereas in a compartmental model with zero net inflow (Bernstein and Hyland 1993), mass

is conserved. Likewise, in undamped mechanical systems, such as a system with Hamilto-

nian dynamics, conservation laws hold, while, in the quaternion-based attitude estimation

problem, the attitude vector must have unit norm (Crassidis and Markley 2003, Choukroun

et al. 2006). Additional examples arise in optimal control (Maciejowski 2002, Goodwin

et al. 2005), parameter estimation (Chia et al. 1991, Aguirre et al. 2004, Nepomuceno

et al. 2004, Walker 2006, Aguirre et al. 2007), tracking and navigation (Wen and Durrant-

Whyte 1992, Alouani and Blair 1993, Dissanayake et al. 2001, Shen et al. 2006), and aero-

nautics (Rotea and Lana 2005, Simon and Simon 2006). In such cases, we wish to obtain

state estimates that take advantage of prior knowledge of the states and use this information

to obtain better estimates than those provided by the Kalman filter in the absence of such

information.

Various algorithms have been developed for equality-constrained state estimation. One

of the most popular techniques is the measurement-augmentation Kalman filter (MAKF),

in which a perfect “measurement” of the constrained quantity is appended to the physical

measurements (Porrill 1988, Tahk and Speyer 1990, Wen and Durrant-Whyte 1992, Alouani

and Blair 1993, Chen and Chiang 1993, De Geeter et al. 1997, Walker 2006). In addition,

estimate-projection (Simon and Chia 2002), system-projection (Ko and Bitmead 2007), and

gain-projection (Gupta and Hauser 2007, Teixeira et al. 2008b) methods have been consid-

ered. A two-step projection algorithm for handling nonlinear equality constraints has also

been presented (Julier and LaViola Jr. 2007).

1

For state estimation with inequality constraints, moving horizon estimators (MHE) (Rao

et al. 2001, Rao et al. 2003), unscented filtering algorithms (Vachhani et al. 2006, Teixeira

et al. 2008d, Teixeira 2008), and probabilistic methods (Rotea and Lana 2005) have been

developed. However, inequality constraints are outside the scope of the present paper. Fi-

nally, a general framework for state-estimation with an equality constraint on the estimator

gain (aiming at enforcing special properties on the state estimates) is presented in (Teixeira

et al. 2008b).

The contributions of the present paper are as follows. First, we investigate how a linear

equality state constraint arises in a linear dynamic system and present necessary conditions

on both the dynamics and process noise for the state to be equality constrained. In (Ko

and Bitmead 2007), this problem is stated in the opposite way, that is, given that a system

satisfies an equality constraint, the goal is to characterize the process noise. In these cases,

we show that an equality-constrained linear system is controllable from the process noise only

in the subspace defined by the equality constraint and that additional information regarding

the initial condition provided by the equality constraint is useful for improving the classical

KF estimates.

Second, we derive the equality-constrained Kalman filter (ECKF) as the maximum-a-

posteriori analytical solution to the equality-constrained state-estimation problem for linear

and Gaussian systems. We also prove the equivalence of ECKF and MAKF and discuss

connections with the estimate-projection and system-projection approaches. We compare

these four algorithms by means of a compartmental system example in which the disturbances

are constrained so that mass is conserved.

Next, our main contribution in this paper is to develop and compare four subopti-

mal algorithms for equality-constrained state estimation for nonlinear systems, namely, the

equality-constrained unscented Kalman filter (ECUKF), the projected unscented Kalman

filter (PUKF), the measurement-augmentation unscented Kalman filter (MAUKF), and the

constrained unscented Kalman filter (CUKF). These methods, which extend algorithms for

constrained state estimation developed for linear systems, are based on the unscented Kalman

filter (UKF) (Julier and Uhlmann 2004), which is an example of sigma-point Kalman filters

(SPKF) (van der Merwe et al. 2004). In addition, CUKF is based on MHE with uni-

2

tary horizon. Recent work (Julier et al. 2000, Haykin 2001, Lefebvre et al. 2002, Lefebvre

et al. 2004, van der Merwe et al. 2004, Romanenko and Castro 2004, Hovland et al. 2005, Choi

et al. 2005, Crassidis 2006) illustrates the improved performance of SPKF compared to the

extended Kalman filter (EKF) (Jazwinski 1970), which is prone to numerical problems such

as initialization sensitivity, divergence, and instability for strongly nonlinear systems (Reif

et al. 1999). A quaternion-based attitude estimation problem (Crassidis and Markley 2003)

is used to illustrate UKF, ECUKF, PUKF, MAUKF, and CUKF. Although the state of

the process model satisfies the unit norm constraint, this constraint is violated by the state

estimates obtained from unconstrained Kalman filtering.

Finally, we use equality-constrained Kalman filtering techniques to improve estimation

when an approximate discretized model is used to represent a continuous-time process. The

problem of using UKF with a discrete-time model obtained from black-box identification

to perform state estimation for a continuous-time nonlinear system is treated in (Aguirre

et al. 2005). According to (Rao et al. 2003), constraints can also be used to correct model

error. We illustrate the application of equality-constrained unscented Kalman filter tech-

niques to this problem through an example of a discretized model of an undamped single-

degree-of-freedom pendulum without external disturbances. Although energy is conserved

in the original, continuous-time system, the discretized model is approximate, and the en-

ergy constraint is intended to improve estimates of the discretized states. Additionally, an

application of equality-constrained Kalman filtering to magnetohydrodynamics data assim-

ilation (Chandrasekar et al. 2004, Chandrasekar et al. 2007), in which the zero-divergence

constraint is enforced on the magnetic field using finite-volume discretized models, is dis-

cussed in (Teixeira et al. 2008c). The present paper is based on research in (Teixeira 2008),

while preliminary versions of it appear as (Teixeira et al. 2007, Teixeira et al. 2008a).

2. State Estimation for Linear Systems

For the linear stochastic discrete-time dynamic system

xk = Ak−1xk−1 +Bk−1uk−1 +Gk−1wk−1, (2.1)

yk = Ckxk + vk, (2.2)

3

where Ak−1 ∈ Rn×n, Bk−1 ∈ Rn×p, Gk−1 ∈ Rn×q, and Ck ∈ Rm×n are known matrices, the

state-estimation problem can be described as follows. Assume that, for all k ≥ 1, the known

data are the measurements yk ∈ Rm, the inputs uk−1 ∈ Rp, and the statistical properties of

x0, wk−1 and vk. The initial state vector x0 ∈ Rn is assumed to be Gaussian with mean x0|0

and error-covariance P xx0|0

= E[(x0 − x0|0)(x0 − x0|0)

T]. The process noise wk−1 ∈ Rq, which

represents unknown input disturbances, and the measurement noise vk ∈ Rm, concerning

inaccuracies in the measurements, are assumed white, Gaussian, zero mean, and mutually

independent with known covariance matrices Qk−1 and Rk, respectively. Next, define the

profit function

J(xk)= ρ(xk|(y1, . . . , yk)), (2.3)

which is the conditional probability density function of the state vector xk ∈ Rn given the

past and present measured data y1, . . . , yk. Under the stated assumptions, the maximization

of (2.3) is the state-estimation problem, while the maximizer xk|k of J is the optimal state

estimate.

The optimal state estimate xk|k is given by the Kalman filter (KF) (Jazwinski 1970),

whose forecast step is given by

xk|k−1 = Ak−1xk−1|k−1 +Bk−1uk−1, (2.4)

P xxk|k−1 = Ak−1P

xxk−1|k−1A

T

k−1 +Gk−1Qk−1GT

k−1, (2.5)

yk|k−1 = Ckxk|k−1, (2.6)

P yyk|k−1 = CkP

xxk|k−1C

T

k +Rk, (2.7)

P xyk|k−1 = P xx

k|k−1CT

k , (2.8)

where P xxk|k−1

= E[(xk − xk|k−1)(xk − xk|k−1)

T], P yy

k|k−1

= E[(yk − yk|k−1)(yk − yk|k−1)

T], and

P xyk|k−1

= E[(xk − xk|k−1)(yk − yk|k−1)

T], and whose data-assimilation step is given by

Kk = P xyk|k−1(P

yyk|k−1)

−1, (2.9)

xk|k = xk|k−1 +Kk(yk − yk|k−1), (2.10)

P xxk|k = P xx

k|k−1 −KkPyyk|k−1K

T

k , (2.11)

where P xxk|k

= E[(xk − xk|k)(xk − xk|k)

T] is the error-covariance matrix and Kk is the Kalman

gain matrix. The notation zk|k−1 indicates an estimate of zk at time k based on information

4

available up to and including time k − 1. Likewise, zk|k indicates an estimate of z at time

k using information available up to and including time k. Model information is used during

the forecast step, while measurement data are injected into the estimates during the data-

assimilation step, specifically, (2.10).

3. State Estimation for Equality-Constrained Linear Systems

In (2.1), assume that rank(Gk−1) = q < n, and define r= n− q, where 1 ≤ r ≤ n. The

case r = n indicates that Gk−1wk−1 is absent. Therefore, there exists Ek−1 ∈ Rr×n such that

rank(Ek−1) = r and

Ek−1Gk−1 = 0r×q. (3.1)

Let T1,k−1 ∈ R(n−r)×n be such that Tk−1=

T1,k−1

Ek−1

∈ Rn×n is invertible. For example, we

can choose T1,k−1= G

T

k−1. Multiplying (2.1) by Tk−1 yields

T1,k−1

Ek−1

xk =

T1,k−1Ak−1

Ek−1Ak−1

xk−1 +

T1,k−1Bk−1

Ek−1Bk−1

uk−1 +

T1,k−1Gk−1

0r×q

wk−1.

Hence, for all k ≥ 1,

Ek−1xk = ek−1, (3.2)

where ek−1= Ek−1(Ak−1xk−1+Bk−1uk−1). Note that ek−1 is not necessarily constant even if

system (2.1)-(2.2) is time invariant. Since rank(Gk−1) < n, Gk−1wk−1 has singular covariance

Gk−1Qk−1GT

k−1 (Ko and Bitmead 2007, Goodwin et al. 2005).

Let s be an integer satisfying 1 ≤ s ≤ r, and partition Ek−1=

E1,k−1

Dk−1

, where

E1,k−1 ∈ R(r−s)×n and Dk−1 ∈ Rs×n. It thus follows from (3.1) that

Dk−1Gk−1 = 0s×q. (3.3)

Proposition 3.1. Assume that

Dk−1Ak−1 = Dk−1 (3.4)

and

Dk−1Bk−1uk−1 = 0s×1 for all k ≥ 1. (3.5)

5

Then, for all k ≥ 1,

Dk−1xk = dk−1, (3.6)

where

dk−1= Dk−1xk−1. (3.7)

Proof. It follows from (3.2) that Dk−1xk = Dk−1(Ak−1xk−1 +Bk−1uk−1) = Dk−1xk−1 =

dk−1.

Corollary 3.1. If (2.1)-(2.2) is time invariant and (3.3)-(3.5) hold, then, for all k ≥ 1,

Dxk = d, (3.8)

where D= Dk−1 and

d= Dx0. (3.9)

Note that, if s = r = n, then xk = D−1d. Hence this case is not of practical interest.

The next result shows that, if (2.1) is equality constrained, then it is not controllable in

Rn from the process noise, but it is rather controllable in the subspace defined by (3.6).

Proposition 3.2. If (3.3)-(3.4) hold, then (Ak−1, Gk−1) is not controllable in Rn.

Proof. Multiplying the controllability matrix

K(Ak−1, Gk−1)=

Gk−1 Ak−1Gk−1 . . . An−1

k−1Gk−1

by Dk−1 yields

Dk−1K(Ak−1, Gk−1) =Dk−1Gk−1 Dk−1Ak−1Gk−1 · · · Dk−1A

n−1k−1Gk−1

=0s×q Dk−1Gk−1 · · · Dk−1A

n−2k−1G

= 0s×nq,

implying that the columns of K lie on the null space of Dk−1 such that rank(K) < n.

Assuming that, for all k ≥ 1, Dk−1 ∈ Rs×n satisfying (3.3)-(3.5) and dk−1 defined by (3.7)

are known, the objective of the equality-constrained state-estimation problem is to maximize

(2.3) subject to (3.6).

6

4. Equality-Constrained Kalman Filter

In this section we solve the equality-constrained state-estimation problem to obtain the

equality-constrained Kalman filter (ECKF). Let xpk|k denote the solution of the equality-

constrained state-estimation problem.

Lemma 4.1. Assume that P xxk|k−1 given by (2.5) and Rk are positive definite. xp

k|k maxi-

mizes J given by (2.3) subject to (3.6) if and only if xpk|k minimizes

J (xk)=

(xk − xk|k−1)

T(P xx

k|k−1)−1(xk − xk|k−1) + (yk − Ckxk)

TR−1

k (yk − Ckxk)(4.1)

subject to (3.6), where xk|k−1 is given by (2.4).

Proof. The proof is similar to the unconstrained counterpart investigated in (Maybeck

1979, pp. 234–235) or (Jazwinski 1970, pp. 207–208) and is omitted for brevity. For

completeness, the proof to the constrained case is presented in (Teixeira 2008, Lemma 5.2.1).

Define the projected error covariance P xxpk|k

= E[(xk − xp

k|k)(xk − xpk|k)

T] + δIn×n, where

0 < δ 1 is a very small number compared to the entries of P xxk|k that guarantees that

P xxpk|k is positive definite. From our experience, we set 10−15 ≤ δ ≤ 10−9 for computational

implementation.

Proposition 4.1. Let xk|k−1 and P xxk|k−1 be given by

xk|k−1= Ak−1x

pk−1|k−1 +Bk−1uk−1, (4.2)

P xxk|k−1

= Ak−1P

xxpk−1|k−1A

T

k−1 +Gk−1Qk−1GT

k−1. (4.3)

Define

dk−1|k= Dk−1xk|k, (4.4)

P ddk|k

= Dk−1P

xxk|kD

T

k−1, (4.5)

P xdk|k

= P xx

k|kDT

k−1, (4.6)

Kpk

= P xd

k|k(Pddk|k)

−1, (4.7)

7

where xk|k is given by (2.10) and P xxk|k is given by (2.11). Then xp

k|k and P xxpk|k are given by

xpk|k = xk|k +Kp

k (dk−1 − dk−1|k), (4.8)

P xxpk|k = P xx

k|k −KpkP

ddk|kK

pk

T

+ δIn×n. (4.9)

Proof. Using Lemma 4.1, let λ ∈ Rs and define the Lagrangian

L= J (xk) + 2λ

T(Dk−1xk − dk−1).

The necessary conditions for a minimizer xpk|k are given by

∂L

∂xk= (P xx

k|k−1)−1(xp

k|k − xk|k−1)− CT

k R−1k (yk − Ckx

pk|k) +D

T

k−1λ = 0n×1, (4.10)

∂L

∂λ= Dk−1x

pk|k − dk−1 = 0s×1. (4.11)

It follows from (4.10) that

((P xxk|k−1)

−1 + CT

k R−1k Ck)(x

pk|k − xk|k−1) = C

T

k R−1k (yk − Ckxk|k−1)−D

T

k−1λ. (4.12)

From (2.11), using (2.7)-(2.9) and the matrix inversion lemma (Bernstein 2005), we have

P xxk|k = P xx

k|k−1 −KkPyyk|k−1K

T

k

= P xxk|k−1 − P xy

k|k−1(Pyyk|k−1)

−1P yyk|k−1(P

yyk|k−1)

−T(P xy

k|k−1)T

= P xxk|k−1 − P xx

k|k−1CT

k (CkPxxk−1|k−1C

T

k +Rk)−1CkP

xxk|k−1

= ((P xxk|k−1)

−1 + CT

k R−1k Ck))

−1. (4.13)

Furthermore, from (2.9), using (2.7)-(2.8), we have

Kk = P xyk|k−1(P

yyk|k−1)

−1

= P xxk|k−1C

T

k (CkPxxk|k−1C

T

k +Rk)−1

= P xxk|k(P

xxk|k)

−1P xxk|k−1C

T

k (CkPxxk|k−1C

T

k +Rk)−1

= P xxk|k(C

T

k R−1k Ck + (P xx

k|k−1)−1)P xx

k|k−1CT

k (CkPxxk|k−1C

T

k +Rk)−1

= P xxk|k(C

T

k R−1k CkP

xxk|k−1C

T

k + CT

k R−1k Rk)(CkP

xxk|k−1C

T

k +Rk)−1

= P xxk|kC

T

k R−1k (CkP

xxk|k−1C

T

k +Rk)(CkPxxk|k−1C

T

k +Rk)−1

= P xxk|kC

T

k R−1k . (4.14)

8

Substituting (4.13) and (4.14) into (4.12) and multiplying by P xxk|k yields

xpk|k = xk|k−1 +Kk(yk − Ckxk|k−1)− P xx

k|kDT

k−1λ. (4.15)

Substituting (4.15) into (4.11) yields

dk−1 = Dk−1xk|k−1 +Dk−1Pxxk|kC

T

k R−1k (yk − Ckxk|k−1)−Dk−1P

xxk|kD

T

k−1λ,

which implies

λ=(Dk−1Pxxk|kD

T

k−1)−1(Dk−1xk|k−1 − dk−1) + (Dk−1P

xxk|kD

T

k−1)−1Dk−1Kk(yk − Ckxk|k−1).(4.16)

Likewise, substituting (4.16) into (4.15) yields

xpk|k = xk|k−1 +Kk(yk − Ckxk|k−1)− P xx

k|kDT

k−1(Dk−1Pxxk|kD

T

k−1)−1(Dk−1xk|k−1 − dk−1)−

P xxk|kD

T

k−1(Dk−1Pxxk|kD

T

k−1)−1Dk−1Kk(yk − Ckxk|k−1)

= xk|k−1 +Kk(yk − Ckxk|k−1)−

P xxk|kD

T

k−1(Dk−1Pxxk|kD

T

k−1)−1

Dk−1xk|k−1 − dk−1 +Dk−1Kkyk −Dk−1KkCkxk|k−1

= xk|k−1 +Kk(yk − Ckxk|k−1) +

P xxk|kD

T

k−1(Dk−1Pxxk|kD

T

k−1)−1

dk−1 −Dk−1(xk|k−1 +Kk(yk − Ckxk|k−1))

.

Now using (4.4)-(4.7), (2.9)-(2.11), we obtain

xpk|k = xk|k−1 +Kk(yk − yk|k−1) +

P xxk|kD

T

k−1(Dk−1Pxxk|kD

T

k−1)−1

dk−1 −Dk−1(xk|k−1 +Kk(yk − yk|k−1))

= xk|k−1 +Kk(yk − yk|k−1) +Kpk (dk−1 − dk−1|k)

= xk|k +Kpk (dk−1 − dk−1|k),

which proves (4.8).

Given the symmetry between (4.8) and (2.10) and recalling that P xxk|k is given by (2.11),

it follows that P xxpk|k is given by (4.9).

Remark 4.1. Note that ECKF is expressed in three steps, namely, the forecast step

(4.2)-(4.3), (2.6)-(2.8), the data-assimilation step (2.9)-(2.11), and the projection step (4.4)-

(4.9), where the updated estimates are projected onto the hyperplane defined by the equality

constraint (3.6).

9

Lemma 4.2. Let N (Dk−1) denote the null space of Dk−1, let W ∈ Rn×n be positive

definite, and define

PN (Dk−1)= In×n −WD

T

k−1(Dk−1WDT

k−1)−1Dk−1. (4.17)

Then PN (Dk−1) is an oblique projector whose range is N (Dk−1), that is, P2N (Dk−1)

= PN (Dk−1)

but it is not necessarily symmetric.

For the following two results, let xk|k given by (2.10) and P xxk|k given by (2.11) denote the

updated estimate and updated error covariance provided by ECKF. Also, let xpk|k given by

(4.8) and P xxpk|k given by (4.9) denote the projected estimate and projected error covariance

of ECKF.

Proposition 4.2. Set W = P xxk|k in (4.17). Then, the projection step (4.4)-(4.9) is

equivalent to

xpk|k = PN (Dk−1)xk|k + dk−1, (4.18)

P xxpk|k = PN (Dk−1)P

xxk|k + δIn×n, (4.19)

where dk−1= P xx

k|kDT

k−1(Dk−1P xxk|kD

T

k−1)−1dk−1.

Proof. Using Lemma 4.2 and substituting (4.4)-(4.7) into (4.8) and (4.9) yields (4.18)-

(4.19).

Proposition 4.3. Assume that (2.1)-(2.2) is time invariant. Also, assume that D in

(3.8) satisfies (3.3)-(3.5). Furthermore, assume that, for a given k − 1, Dxpk−1|k−1 = d

and DP xxpk−1|k−1 = 0s×n and set δ = 0. Then Dxk|k = d, DP xx

k|k = 0s×n, xpk|k = xk|k, and

P xxpk|k = P xx

k|k.

Proof. Multiplying (4.2)-(4.3) by D yields

Dxk|k−1 =DAxpk−1|k−1 +DBuk−1 = Dxp

k−1|k−1 + 0s×1 = d, (4.20)

DP xxk|k−1 =DAP xxp

k−1|k−1AT+DGQk−1G

T= DP xxp

k−1|k−1AT+ 0s×qQk−1G

T= 0s×nA

T= 0s×n.(4.21)

With (2.8) and (4.21), multiplying (2.9) by D yields

DKk = DP xyk|k−1(P

yyk|k−1)

−1 = DP xxk|k−1C

T(P yy

k|k−1)−1 = 0s×nC

T(P yy

k|k−1)−1 = 0s×m. (4.22)

10

With (4.20) and (4.22), multiplying (2.10)-(2.11) by D yields

Dxk|k = Dxk|k−1 +DKk(yk − yk|k−1) = d+ 0s×m(yk − yk|k−1) = d, (4.23)

DP xxk|k = DP xx

k|k−1 −DKkPyyk|k−1K

T

k = 0s×n + 0s×mPyyk|k−1K

T

k = 0s×n. (4.24)

Given (4.23)-(4.24) and δ = 0, from (4.18)-(4.19), we have xpk|k = xk|k and P xxp

k|k = P xxk|k.

Corollary 4.1. Assume that Dxp1|1 = d and DP xxp

1|1 = 0s×n. Then, for all k ≥ 2,

Dxk|k = d and DP xxk|k = 0s×n.

Remark 4.2. Therefore, for time-invariant systems, whenever (3.3)-(3.5) hold, the pro-

jection step of ECKF given by (4.4)-(4.9) is required only at k = 1, so that, for all k ≥ 2,

the updated estimate xk|k given by (2.10) satisfies Dxk|k = d.

5. Connections between ECKF and Alternative Approaches

We now compare ECKF to three Kalman filtering algorithms that yield state estimates

satisfying an equality constraint.

First we consider the measurement–augmentation Kalman filter (MAKF) (Porrill 1988,

Tahk and Speyer 1990, Wen and Durrant-Whyte 1992), which treats (3.6) as perfect mea-

surements. In Appendix I, we present the MAKF equations and prove that the MAKF and

ECKF estimates are equal.

In Appendix II, we show that the projected Kalman filter by system projection (PKF-SP)

(Ko and Bitmead 2007), which, assuming that (3.3)-(3.5) hold, incorporates the information

provided by (3.6) only in filter initialization, that is, k = 0, is a special case of ECKF for

time-invariant systems.

In Appendix III, we briefly review the projected Kalman filter by estimate projection

(PKF-EP) (Simon and Chia 2002, Simon 2006), which projects xk|k onto the hyperplane

(3.6) for all k ≥ 1. Unlike ECKF, the projected estimate of PKF-EP is not recursively fed

back in the next iteration.

Figure 1 illustrates how the forecast, data-assimilation, and projection steps are con-

nected for ECKF, PKF-SP, PKF-EP, and MAKF.

11

[Figure 1 about here.]

Also, for the special case of unitary horizon, ECKF and the moving horizon estimator

(MHE) (Rao et al. 2001) minimize the same cost function, specifically, (4.1). However,

ECKF provides the analytical solution to the equality-constrained optimization problem.

Moreover, unlike MHE, ECKF enforces the constraint information on the error covariance

in addition to the state estimate.

We also remark that, if m = 0, then the problem solved by ECKF in Proposition 4.1 is

similar to the case in which KF is used as an iterative solver for systems of linear algebraic

equations such as Dx = d, where D ∈ Rs×n, d ∈ Rs, and rank(D) = s (Pinto and Rios

Neto 1990, Theorem 2).

Note that, if dk−1 is uncertain, then (3.6) can be replaced by the noisy equality constraint

(De Geeter et al. 1997, Ko and Bitmead 2007, Walker 2006)

dk−1 = Dk−1xk + vdk , (5.1)

where vdk ∈ Rs is a white, Gaussian, zero-mean noise with covariance Rdk, and it is treated

as an extra noisy measurement by MAKF (see Appendix I) but with Rdk

=

Rk 0m×s

0s×m Rdk

replacing Rk.

6. State Estimation for Equality-Constrained Nonlinear Systems

For the nonlinear stochastic discrete-time dynamic system

xk = f (xk−1, uk−1, wk−1, k − 1) , (6.1)

yk = h (xk, k) + vk, (6.2)

where f : Rn × Rp × Rq × N → Rn and h : Rn × N → Rm are, respectively, the process and

observation models, and whose state vector xk is known to satisfy the equality constraint

g (xk, k − 1) = dk−1, (6.3)

where g : Rn × N → Rs and dk−1 is known, the objective of the equality-constrained state-

estimation problem is, for all k ≥ 1, to maximize (2.3) subject to (6.3). However, the

12

solution to this problem is complicated (Daum 2005) by the fact that, for nonlinear systems,

ρ(xk|(y1, . . . , yk)) is not completely characterized by its first-order and second-order moments.

We thus use an approximation based on the classical Kalman filter to provide a suboptimal

solution to the nonlinear case.

6.1. Unscented Kalman Filter

First, to address the unconstrained case, we consider the unscented Kalman filter (UKF)

(Julier and Uhlmann 2004) to provide a suboptimal solution to the state-estimation problem.

Instead of analytically or numerically linearizing (6.1)-(6.2) and using (2.4)-(2.11), UKF

employs the unscented transform (UT) (Julier et al. 2000), which approximates the posterior

mean y ∈ Rm and covariance P yy ∈ Rm×m of a random vector y obtained from the nonlinear

transformation y = h(x), where x is a prior random vector whose mean x ∈ Rn and covariance

P xx ∈ Rn×n are assumed known. UT yields the actual mean y and the actual covariance P yy

if h = h1 + h2, where h1 is linear and h2 is quadratic (Julier et al. 2000). Otherwise, yk is a

pseudo mean and P yy is a pseudo covariance.

UT is based on a set of deterministically chosen vectors known as sigma points. To

capture the mean xak−1|k−1 of the augmented prior state vector

xak−1

=

xk−1

wk−1

, (6.4)

where xak−1 ∈ Rna and na

= n+ q, as well as the augmented prior error covariance

P xxak−1|k−1

=

P xxk−1|k−1 0n×q

0q×n Qk−1

, (6.5)

the sigma-point matrix Xk−1 ∈ Rna×(2na+1) is chosen as

Xk−1|k−1 = xak−1|k−111×(2na+1) +

(na + λ)

0na×1

P xxak−1|k−1

1/2 −P xxak−1|k−1

1/2,(6.6)

with weights

γ(m)0

=

λ

na + λ,

γ(c)0

=

λ

na + λ+ 1− α2 + β,

γ(m)i

= γ(c)

i= γ(m)

i+na

= γ(c)

i+na

=

1

2(na + λ), i = 1, . . . , na,

(6.7)

13

where (·)1/2 is the Cholesky square root, 0 < α ≤ 1, β ≥ 0, κ ≥ 0, and λ= α2(κ+na)−na >

−na. We set α = 1 and κ = 0 (Haykin 2001) such that λ = 0 (Julier and Uhlmann 2004)

and set β = 2 (Haykin 2001). Alternative schemes for choosing sigma points are given in

(Julier and Uhlmann 2004) and references therein.

The UKF forecast equations are given by (6.6)-(6.7) together with

coli(X xk|k−1) = f(coli(X x

k−1|k−1), uk−1, coli(Xwk−1|k−1), k − 1), i = 0, . . . , 2na, (6.8)

xk|k−1 =2na

i=0

γ(m)i coli(X x

k|k−1), (6.9)

P xxk|k−1 =

2na

i=0

γ(c)i [coli(X x

k|k−1)− xk|k−1][coli(X xk|k−1)− xk|k−1]

T, (6.10)

coli(Yk|k−1) = h(coli(X xk|k−1), k), i = 0, . . . , 2na, (6.11)

yk|k−1 =2na

i=0

γ(m)i coli(Yk|k−1), (6.12)

P yyk|k−1 =

2na

i=0

γ(c)i [coli(Yk|k−1)− yk|k−1][coli(Yk|k−1)− yk|k−1]

T+Rk, (6.13)

P xyk|k−1 =

2na

i=0

γ(c)i [coli(X x

k|k−1)− xk|k−1][coli(Yk|k−1)− yk|k−1]T, (6.14)

where

X x

k−1|k−1

Xwk−1|k−1

= Xk−1|k−1, X x

k−1|k−1 ∈ Rn×(2na+1), and Xwk−1|k−1 ∈ Rq×(2na+1). The UKF

data-assimilation equations are given by (2.9)-(2.11).

7. Equality-Constrained Unscented Kalman Filters

In this section, by using UT, we extend the algorithms PKF-EP, ECKF, and MAKF to

the nonlinear case. These unscented-based approaches provide suboptimal solutions to the

equality-constrained state-estimation problem for nonlinear systems. Unlike the linear case

(Section 4), these approaches do not guarantee that the nonlinear equality constraint (6.3)

is exactly satisfied, but they provide approximate solutions.

Furthermore, to obtain state estimates satisfying (6.3) at a given tolerance by solving

an optimization problem online, we also present an unscented extension of the constrained

Kalman filter (CKF) for linear systems, which is a special case of MHE with unitary horizon

(Rao et al. 2001).

14

7.1. Projected Unscented Kalman Filter

The projection step of ECKF given by (4.4)-(4.9) is now extended to the nonlinear case

by means of UT. Choosing sigma points and associated weights as indicated in (6.6)-(6.7),

we have

X xk|k = xk|k11×(2n+1) +

(n+ λ)

0n×1

P xxk|k

1/2 −P xxk|k

1/2, (7.1)

where xk|k and P xxk|k are given by (2.10) and (2.11), respectively. Then the sigma points

coli(X xk|k) ∈ Rn, i = 0, . . . , 2n, are propagated through (6.3), yielding

coli(Dk|k) = g(coli(X xk|k), k − 1), i = 0, . . . , 2n, (7.2)

such that dk−1|k, P ddk|k, and P xd

k|k are given by

dk−1|k =2n

i=0

γ(m)i coli(Dk|k), (7.3)

P ddk|k =

2n

i=0

γ(c)i [coli(Dk|k)− dk−1|k][coli(Dk|k)− dk−1|k]

T, (7.4)

P xdk|k =

2n

i=0

γ(c)i [coli(X x

k|k)− xk|k][coli(Dk|k)− dk−1|k]T, (7.5)

and Kpk , x

pk|k , and P xxp

k|k are given by (4.7), (4.8), and (4.9), respectively.

Appending the projection step (7.1)-(7.5), (4.7)-(4.9) to UKF (6.6)-(6.14), (2.9)-(2.11)

without feedback recursion (see Figure 1b) yields the projected unscented Kalman filter

(PUKF), which extends PKF-EP to nonlinear systems.

7.2. Equality-Constrained Unscented Kalman Filter

Define xapk−1

=

xpk−1

wk−1

, and P xxap

k−1|k−1

=

P xxpk−1|k−1 0n×q

0q×n Qk−1

, such that the sigma points

Xk−1|k−1 = xapk−1|k−111×(2na+1) +

(na + λ)

0na×1

P xxapk−1|k−1

1/2−P xxapk−1|k−1

1/2, (7.6)

are chosen based on xpk−1|k−1 given by (4.8). Then, by appending the projection step

(7.1)-(7.5), (4.7)-(4.9) to equations (7.6), (6.7)-(6.14), (2.9)-(2.11), we obtain the equality-

constrained unscented Kalman filter (ECUKF). Note that, unlike PUKF, ECUKF connects

the projection step to UKF by feedback recursion; see Figure 1a.

15

7.3. Measurement-Augmentation Unscented Kalman Filter

To extend the MAKF algorithm to the nonlinear case, we replace (6.2) by the augmented

observation

yk= h(xk, k) + vk, (7.7)

where yk=

ykdk−1

, h(xk, k)

=

h(xk, k)

g(xk, k − 1)

, and vk

=

vk0s×1

. With (7.7), the

measurement-augmentation unscented Kalman filter (MAUKF) combines (6.6)-(6.10) with

the augmented forecast equations

coli(Yk|k−1) = h(coli(X xk|k−1), k), i = 0, . . . , 2na, (7.8)

ˆyk|k−1 =2na

i=0

γ(m)i coli(Yk|k−1), (7.9)

P yyk|k−1 =

2na

i=0

γ(c)i [coli(Yk|k−1)− ˆyk|k−1][coli(Yk|k−1)− ˆyk|k−1]

T+ Rk, (7.10)

P xyk|k−1 =

2na

i=0

γ(c)i [coli(X x

k|k−1)− xk|k−1][coli(Yk|k−1)− ˆyk|k−1]T, (7.11)

where

Rk=

Rk 0m×s

0s×m 0s×s

, (7.12)

and the KF data-assimilation equations

Kk = P xyk|k−1(P

yyk|k−1)

−1, (7.13)

xk|k = xk|k−1 + Kk(yk − ˆyk|k−1), (7.14)

P xxk|k = P xx

k|k−1 − KkPyyk|k−1K

T

k . (7.15)

Recall that, unlike the linear case, the ECUKF and MAUKF estimates are not necessarily

equivalent.

In practice, to circumvent ill-conditioning problems in the inversion of P yyk|k−1 in (7.10),

we replace Rk by Rdk, where we set Rd

k = δIs×s, 10−15 ≤ δ ≤ 10−9.

7.4. Constrained Unscented Kalman Filter

We now extend CKF to the nonlinear case by combining the forecast step of UKF with

the data-assimilation step of CKF. Then we obtain the constrained unscented Kalman filter

16

(CUKF), whose forecast equations are given by (6.6)-(6.14) and whose data-assimilation

equations are given by

xk|k =arg min J1(xk),

xk : g (xk, k − 1) = dk−1 (7.16)

together with (2.9) and (2.11), where J1(xk) is given by

J1(xk)=(xk − xk|k−1)

T(P xx

k|k−1)−1(xk − xk|k−1) + (yk − h(xk, k))

TR−1

k (yk − h(xk, k)), (7.17)

where xk|k−1 is given by (6.9) and P xxk|k−1 is given by (6.10). Various optimization methods

can be used to solve online the constrained optimization problem (7.16) (Fletcher 2000).

Note that (7.17) is the nonlinear counterpart of (4.1).

Note that, unlike ECUKF and MAUKF, the equality-constraint information is not as-

similated by the error-covariance P xxk|k in (2.11). Also, CUKF allows the enforcement of

inequality constraints in addition to (6.3) in (7.16).

8. Numerical Examples

In this section, a linear example is investigated using KF, ECKF, MAKF, PKF-EP, and

PKF-SP, and two nonlinear examples are discussed using UKF, ECUKF, MAUKF, PUKF,

and CUKF.

To compare the performance of these algorithms, we use two metrics over a c-run Monte

Carlo simulation. First, the accuracy of the state estimate xi,k|k,j given by (2.10), i = 1, . . . , n

and j = 1, . . . , c, from time k = k0 to kf is quantified by the root mean square error (RMSE)

index given by

RMSEi =1

c

c

j=1

1

kf − k0 + 1

kf

k=k0

(xi,k − xi,k|k,j)2, i = 1, . . . , n (8.1)

where xi,k is the true value. For ECKF and PKF-EP, as well as for ECUKF and PUKF,

xi,k|k,j is replaced by xpi,k|k,j given by (4.8) and (9.27), respectively. Note that, to calculate

RMSEi, xi,k must be known, and thus this index is restricted to simulation.

Next, we assess how informative (Lefebvre et al. 2004) the state estimate xk|k is by

17

evaluating the mean trace (MT) of P xxk|k given by (2.11) from time k = k0 to kf , that is,

MT =1

c

c

j=1

1

kf − k0 + 1

kf

k=k0

tr(P xxk|k,j)

. (8.2)

MT measures the uncertainty in the estimate xk|k. For ECKF and PKF-EP, as well as for

their nonlinear counterparts, P xxk|k,j is replaced by P xxp

k|k,j given by (4.9) and (4.19), respectively.

8.1. Compartmental System

Consider the linear discrete-time compartmental model (2.1)-(2.2) (Bernstein and Hyland

1993) whose matrices are given by

A =

0.94 0.028 0.0190.038 0.95 0.0010.022 0.022 0.98

, B = 03×1, G =

0.05 −0.03

−0.02 0.01−0.03 0.02

, C =

1 0 00 1 0

, (8.3)

with initial condition x0 = [ 1 1 1 ]T

and process noise and observation noise covariance

matrices Qk−1 = σ2wI3×3 and Rk = σ2

vI2×2. The data-free simulation of this system is shown

in Figure 2 for σw = 1.0. Note that (3.3)-(3.5) hold for (8.3) such that the trajectory of

xk ∈ R3 lies on the plane (3.8), whose parameters are assumed known and are given by

D =1 1 1

, d = 3, (8.4)

that is, conservation of mass is verified.

[Figure 2 about here.]

For state estimation, the KF algorithm is initialized with

x0|0 = [ 2 1 0 ]T

, P xx0|0 = I3×3. (8.5)

Figure 3 shows that the KF estimates do not lie on the plane (3.8). Even if x0|0 = x0 or

σw = 0, KF does not produce estimates satisfying (3.8).

[Figure 3 about here.]

Next, we implement the ECKF algorithm. From a 100-run Monte Carlo simulation for

each one of these process noise levels, namely, σw = 0, 0.1, 0.5, 1.0, and σv = 0.01, Table 1

shows that the ECKF estimates satisfy the equality constraint. In addition, these estimates

are both more accurate and more informative than the KF estimates.

18

[Table 1 about here.]

For MAKF, PKF-EP, and PKF-SP, initialization is defined as in (8.5), except for PKF-SP

(see (9.23) in Appendix II). Table 1 summarizes the results. ECKF, MAKF, PKF-SP, and

PKF-EP guarantee that (3.8) is satisfied at machine precision and yield improved estimates

compared to KF. All equality-constrained methods produce similar results concerning RMSE

and MT for this time-invariant system, which is in accordance with (Ko and Bitmead 2007,

Theorem 2) regarding PKF-SP and PKF-EP. Recall that the numerical differences in Table

1 regarding the RMS constraint error for the equality-constrained algorithms are neglegible.

However, though not shown in Table 1, it is relevant to mention that PKF-EP produces less

accurate and less informative forecast estimates xk|k−1 compared to the other constrained

algorithms. This is expected because PKF-EP do not use xpk−1|k−1 to calculate xk|k−1; see

Figure 1b.

In addition, a land-based vehicle linear example with kinematic constraints (Simon and

Chia 2002, Ko and Bitmead 2007) is investigated using KF, ECKF, MAKF, PKF-SP, and

PKF-EP in (Teixeira 2008, Chapter 5).

8.2. Attitude Estimation

Consider an attitude estimation problem (Crassidis and Markley 2003, Choukroun et al.

2006), whose kinematics are modeled as

e(t) =1

2Ω(t)e(t), (8.6)

where the state vector is the quaternion vector e(t) = [e0(t) e1(t) e2(t) e3(t)]T

, the matrix

Ω(t) is given by

Ω(t) =

0 r(t) −q(t) p(t)−r(t) 0 p(t) q(t)q(t) −p(t) 0 r(t)−p(t) −q(t) −r(t) 0

, (8.7)

and the angular velocity vector u(t) = [p(t) q(t) r(t)]T

is a known input. Since ||e(0)||2 = 1

and Ω(t) is skew symmetric, it follows that, for all t > 0,

||e(t)||2 = 1. (8.8)

19

We set e(0) = [0.9603 0.1387 0.1981 0.1387]T

and

u(t) =0.03 sin

2π600t

0.03 sin

2π600t− 300

0.03 sin

2π600t− 600

T. (8.9)

To perform attitude estimation, we assume that

uk−1 = u((k − 1)T ) + βk−1 + wuk−1 (8.10)

is measured by rate gyros, where T is the discretization step, wuk−1 ∈ R3 is zero-mean,

Gaussian noise, and βk−1 ∈ R3 is drift error. The discrete-time equivalent of (8.6) augmented

by the gyro drift random-walk model (Crassidis and Markley 2003) is given byekβk

=

Ak−1 04×3

03×4 I3×3

ek−1

βk−1

+

04×1

wβk−1

, (8.11)

where ek= e(kT ), wβ

k−1 ∈ R3 is process noise associated with the drift-error model, xk=

ekβk

∈ R7 is the state vector, wk−1

=

wu

k−1

wβk−1

∈ R6 is the process noise, and

Ak−1= cos(sk−1)I4×4 +

1

2

T sin(sk−1)

sk−1Ωk−1, (8.12)

Ωk−1= Ω((k − 1)T ), (8.13)

sk−1=

T

2

uk−1 − βk−1 − wuk−1

2. (8.14)

The constraint (8.8) also holds for (8.11), that is,

x21,k + x2

2,k + x23,k + x2

4,k = 1. (8.15)

We set T = 0.1 s, βk−1 = [ 0.001 −0.001 0.0005 ]Trad/s, andQk−1 = diag (10−5I3×3, 10−10I3×3).

Attitude observations y[i]k ∈ R3 for a direction sensor are given by (Crassidis and Markley

2003)

y[i]k = Ckr[i] + v[i]k , (8.16)

where r[i] ∈ R3 is a reference direction vector to a known point, and Ck is the rotation matrix

from the reference to the body-fixed frame

Ck =

x21,k − x2

2,k − x23,k + x2

4,k 2(x1,kx2,k + x3,kx4,k) 2(x1,kx3,k − x2,kx4,k)2(x1,kx2,k − x3,kx4,k) −x2

1,k + x22,k − x2

3,k + x24,k 2(x1,kx4,k + x2,kx3,k)

2(x1,kx3,k + x2,kx4,k) 2(−x1,kx4,k + x2,kx3,k) −x21,k − x2

2,k + x23,k + x2

4,k

.

(8.17)

20

We assume that two directions are available (Crassidis and Markley 2003, Lee et al. 2007),

which can be obtained using either a star tracker or a combined three-axis accelerometer/three-

axis magnetometer. We set r[1] = [ 1 0 0 ]T, r[2] = [ 0 1 0 ]

T, and Rk = 10−4I6×6. These

direction measurements are assumed to be provided at a lower rate, specifically, at 1 Hz,

which corresponds to a sample interval of 10T s.

We implement Kalman filtering using UKF, ECUKF, PUKF, MAUKF, and CUKF with

(8.11), (8.16), and constraint (8.15). We initialize these algorithms with x0|0 = [ 1 0 0 0 0 0 0 ]T

and P xx0|0 = diag (0.5I4×4 0.01I3×3). We use the fmincon algorithm of Matlab in the CUKF

implementation.

Table 2 shows the results obtained from a 100-run Monte Carlo simulation. Note that,

for the nonlinear case, MT given by (8.2) is obtained from the pseudo-error covariance, and

thus, smaller values of MT do not guarantee a smaller spread about the mean. Nevertheless,

with the usage of prior knowledge by ECUKF and MAUKF, the values of MT show that

more informative estimates are produced compared to the unconstrained estimates given by

UKF. However, a slight increase in the RMS error is observed for algorithms ECUKF and

MAUKF implying loss of accuracy around 5% compared to UKF. On the other hand, PUKF

and CUKF yield estimates as accurate as UKF does. The equality constraint is more closely

tracked whenever a constrained filter is employed; see Figure 4. Percent errors around 0.0007

are obtained for PUKF, MAUKF, and ECUKF, which exhibits the smallest constraint error

among these three algorithms. Compared to PUKF, MAUKF, and ECUKF, note that our

implementation of CUKF using the fmincon algorithm provides a 11 times smaller constraint

error for the quaternion norm, but at a 2 times larger processing time.

[Table 2 about here.]

[Figure 4 about here.]

Also, for comparison, we implement the EKF-counterparts of ECUKF, PUKF, and

MAUKF, namely, ECEKF, PEKF (Simon and Chia 2002), and MAEKF (Alouani and

Blair 1993). The results (not shown) indicate that the unscented approaches yield improved

results.

21

8.3. Simple Pendulum

We consider the continuous-time undamped and unforced pendulum

θ(t) +g

Lsin θ(t) = 0, (8.18)

where θ(t) is the angular position such that θ = 0 rad corresponds to the stable equilibrium

position, g is the gravity acceleration, and L is the length. Given noisy measurements of

the angular velocity of the pendulum, we want to obtain states that satisfy the energy

conservation property.

Using Euler discretization with time step T , such that t = kT , and defining x1,k = θ(kT )

and x2,k = θ(kT ), we obtain the approximate discretized modelx1,k

x2,k

=

x1,k−1 + Tx2,k−1

x2,k−1 − T gL sin (x1,k−1)

+

w1,k

w2,k

, (8.19)

with noisy measurements of the true (continuous-time model) values of angular velocity given

by

yk = x2(kT ) + vk. (8.20)

By conservation of energy in (8.18) we have

−mgL cos(x1(t)) +mL2

2x22(t) = E(0), (8.21)

where E(0) is the total mechanical energy and m is the pendulum mass. Next, we define

the approximate energy Ek of the discrete-time model by

Ek= −mgL cos(x1,k) +

mL2

2x22,k. (8.22)

We use the 4-th order Runge-Kutta integration scheme to obtain x(kT ) for L = 1 m, T = 10

ms, and initial conditions θ(0) = 3π4 , θ(0) = π

50 . We assume that E(0) is known and we

implement equality-constrained state estimation by constraining Ek = E(0) for all k ≥ 1.

The state estimation is initialized with Qk−1 = σ2wI2×2, Rk = σ2

v , x0|0 = [ 1 1 ]T, and P xx

0|0 =

I2×2, where three values of observation noise are tested, namely, σv = 0.1, 0.25, and 0.5, and

process noise with σw = 0.007 is set to help convergence of estimates (Xiong et al. 2007). A

100–run Monte Carlo simulation is performed for each σv.

22

Table 3 shows the percent RMS errors related to the equality constraint (8.22). Figure

5 compares the accuracy of the algorithms with relation to (8.21). It can be noticed, in

this example, that the data-free simulation of the discretized model results in an unrealistic

increasing energy Ek. Note that UKF is not able to closely track the constraint. For

higher observation noise levels, that is, σv = 0.5, RMS constraint errors between 4% and

7% are observed. Nonetheless, whenever PUKF, MAUKF, and ECUKF are employed, these

indices are reduced by 2 orders of magnitude, while, by using CUKF, we observe a reduction

of 7 orders of magnitude. In addition to the improvement in the accuracy of constraint

satisfaction, the usage of prior knowledge also results in more accurate and more informative

estimates.

[Table 3 about here.]

[Figure 5 about here.]

According to the indices in italics in Table 3, the same comparative analysis is applicable

when the true continuous-time model (8.18) is used replacing (8.19). The use of a 4-th

Runge-Kutta integration with UKF yields 30% smaller RMSE indices and 2 times smaller

MT index compared to Euler discretization with ECUKF, but with approximately 7 times

larger RMS constraint error and with approximately 4 times larger processing time. Similar

to Section 8.2, note that the use of a process model whose state vector satisfies an equality

constraint does not guarantee that the state estimates satisfy this constraint because such

information is not taken into account during data assimilation.

For this numerical example, ECUKF and MAUKF yield more accurate and more informa-

tive estimates than PUKF and CUKF. Moreover, the performance of ECUKF and MAUKF

almost coincide for this nonlinear example. When it comes to constraint satisfaction, CUKF

yields the most accurate results.

In addition, we implement the EKF-counterparts of ECUKF, PUKF, and MAUKF. The

results (not shown) indicate that the unscented approaches yield competitive results com-

pared to the extended approaches.

23

9. Concluding Remarks

We have shown that the problem of equality-constrained state estimation for linear and

Gaussian systems arises from the definition of both process noise and dynamic equations

with special properties, specifically, (3.3)-(3.5), such that the system is not controllable in

Rn from the process noise. In this case, the classical Kalman filter does not guarantee that

its estimates satisfy the equality constraint.

Then we have solved the equality-constrained state-estimation problem for linear and

Gaussian systems using the maximum-a-posteriori approach, yielding the equality-constrained

Kalman filter (ECKF). Moreover, we have proved the equivalence of ECKF to the measurement-

augmentation Kalman filter (MAKF) and have presented its connections to the projection

Kalman filter by system-projection (PKF-SP) and the projection Kalman filter by estimate-

projection (PKF-EP). We have compared these four methods by means of a compartmental

system example with mass conservation.

For the nonlinear case, where is the main contribution of the present paper, four subop-

timal algorithms based on the unscented Kalman filter (UKF) were presented, namely, the

equality-constrained unscented Kalman filter (ECUKF), the projected unscented Kalman

filter (PUKF), the measurement-augmentation unscented Kalman filter (MAUKF), and the

constrained unscented Kalman filter (CUKF). CUKF, which is an optimization-based ap-

proach, allows the enforcement of both equality and inequality constraints at a given toler-

ance. These methods were compared on two examples, including a quaternion-based attitude

estimation problem, as well as a mechanical system with conserved energy.

Numerical results suggest that, in addition to exactly, that is, at machine precision, (in

the linear case and for CUKF in the nonlinear case) or very closely (for ECUKF, MAUKF,

and PUKF in the nonlinear case) satisfying a known equality constraint of the system, the

proposed methods can yield more accurate and more informative estimates than KF (in the

linear case) or UKF (in nonlinear case). For the linear scenario, ECKF, MAKF, PKF-SP,

and PKF-EP have produced similar results. However, for the nonlinear case, considering

the examples investigated, we recommend the user to test ECUKF, MAUKF, and PUKF

in this order for a given equality-constrained state-estimation application. If the constraint

24

satisfaction accuracy has priority over processing time and ease of implementation, we sug-

gest CUKF. Recall that, since these nonlinear methods are approximate, their performance

depends on the application. Moreover, except for CUKF, all equality-constrained approaches

have required similar processing time, which was competitive to KF (for linear algorithms)

and UKF (for nonlinear cases) processing time. In this case, CUKF has a larger process-

ing time because it solves online a constrained optimization problem. The performance of

CUKF depends on the optimization algorithm and problem. For the two nonlinear examples

investigated, the CUKF processing time was 2 to 15 times larger than the UKF processing

time.

Finally, we have also addressed the case where an approximate discretized model is used

to represent a continuous-time process in state estimation. Improved estimates were obtained

when equality-constrained Kalman filtering algorithms were employed to enforce a conserved

quantity of the original continuous-time model, but without the higher computational burden

required by more accurate integration schemes.

We believe that comparisons of MHE for nonlinear systems (Rao et al. 2003) against

ECUKF and MAUKF would be valuable in a deeper analysis of the algorithms and that this

should be pursued in the near future.

Acknowledgments

We wish to thank Prof. Harris McClamroch for assistance in the attitude estimation

example.

This research was supported by the National Council for Scientific and Technological De-

velopment (CNPq), Brazil, and by the National Science Foundation Information Technology

Research Initiative, through Grants ATM-0325332 and CNS-0539053 to the University of

Michigan, Ann Arbor, USA.

Appendix I: Equivalence between ECKF and MAKF

Define the augmented observation

yk=

ykdk−1

= Ckxk +

vk0s×1

, (9.1)

25

where

Ck=

Ck

Dk−1

. (9.2)

With (9.1), MAKF uses (2.4)-(2.5) together with the augmented forecast equations

ˆyk|k−1 = Ckxk|k−1, (9.3)

P yyk|k−1 = CkP

xxk|k−1C

T

k + Rk, (9.4)

P xyk|k−1 = P xx

k|k−1CT

k , (9.5)

where Rk is given by (7.12), and the augmented data-assimilation equations given by (7.13)-

(7.15).

For convenience, let xk|k−1= xk|k−1 (2.4) denote the forecast estimate provided by MAKF.

Furthermore, let P xxk|k−1

= P xx

k|k−1 (2.5) be the associated forecast error covariance of MAKF.

Also let xk|k−1 (4.2) and P xxk|k−1 (4.3) denote the forecast estimate and the associated error

covariance of ECKF. Assume that δ in (4.9) is sufficiently small and can be neglected.

Proposition 9.1. Assume that xk|k−1 = xk|k−1 and P xxk|k−1 = P xx

k|k−1. Then xk+1|k = xk+1|k

and P xxk+1|k = P xx

k+1|k.

Proof. P yyk|k−1 (9.4) is equivalent to

P yyk|k−1 =

P yyk|k−1 CkP xx

k|k−1DTk−1

Dk−1P xxk|k−1C

Tk P dd

k|k−1

.

It follows from (Bernstein 2005) that P yy −1k|k−1 has entries

P yy −1k|k−1 =

(P−1

k|k−1)1 (P−1k|k−1)12

(P−1k|k−1)12

T(P−1

k|k−1)2

,

where

(P−1k|k−1)1

=P yyk|k−1 − CkP

xxk|k−1D

Tk−1(P

ddk|k−1)

−1Dk−1Pxxk|k−1C

Tk

−1,

(P−1k|k−1)12

=−

P yyk|k−1 − CkP

xxk|k−1D

Tk−1(P

ddk|k−1)

−1Dk−1Pxxk|k−1C

Tk

−1

×CkPxxk|k−1D

Tk−1(P

ddk|k−1)

−1,

(P−1k|k−1)2

=P ddk|k−1 −Dk−1P

xxk|k−1C

Tk (P

yyk|k−1)

−1CkPxxk|k−1D

Tk−1

−1. (9.6)

26

Furthermore, it can be shown that

(P−1k|k−1)1 = (P yy

k|k−1)−1 + (P yy

k|k−1)−1CkP

xxk|k−1D

Tk−1(P

−1k|k−1)2Dk−1P

xxk|k−1C

Tk (P

yyk|k−1)

−1, (9.7)

(P−1k|k−1)12 =−(P yy

k|k−1)−1CkP

xxk|k−1D

Tk−1(P

−1k|k−1)2. (9.8)

It follows from (2.9) that

Kk = P xxk|k−1C

Tk (P

yyk|k−1)

−1, (9.9)

Furthermore substituting (9.9) into (2.11) yields

P xxk|k = P xx

k|k−1 − P xxk|k−1C

Tk (P

yyk|k−1)

−1CkPxxk|k−1.

Hence,

(Dk−1Pxxk|kD

Tk−1)

−1 =Dk−1P

xxk|k−1D

Tk−1 −Dk−1P

xxk|k−1C

Tk (P

yyk|k−1)

−1CkPxxk|k−1D

Tk−1

−1

= (P−1k|k−1)2. (9.10)

Substituting (9.9) into (2.10) yields (4.8). Substituting (4.4) into (4.8) yields

xpk|k = xk|k−1 +

Kk −Kp

kDk−1Kk Kpk

yk − Ckxk|k−1

. (9.11)

It follows from (4.5), (9.6), (9.8) and (9.10) that

Kpk = P xx

k|k−1CTk

(P−1

k|k−1)12(P−1

k|k−1)2

. (9.12)

Substituting (9.8) into (9.12) and substituting the resulting expression into Kk−KpkDk−1Kk

yields

Kk −KpkDk−1Kk = P xx

k|k−1CTk

(P yy

k|k−1)−1 + (P yy

k|k−1)−1CkP

xxk|k−1D

Tk−1(P

−1k|k−1)2Dk−1

P xxk|k−1C

Tk (P

yyk|k−1)

−1− P xx

k|k−1DTk−1(P

−1k|k−1)2Dk−1P

xxk|k−1C

Tk (P

yyk|k−1)

−1.

Hence, (9.7) and (9.8) imply that

Kk −KpkDk−1Kk = P xx

k|k−1CTk

(P−1

k|k−1)1(P−1

k|k−1)T

12

. (9.13)

27

Therefore, it follows from (9.12) and (9.13) that

Kk −Kp

kDk−1Kk Kpk

= P xx

k|k−1CTk P

yy −1k|k−1 . (9.14)

Since the estimate xk|k of MAKF is given by

xk|k = xk|k−1 + Kk

yk − Ckxk|k−1

, (9.15)

where

Kk = P xxk|k−1C

Tk

CkP

xxk|k−1C

Tk + Rk

−1,

it follows from (9.14) that

Kk =Kk −Kp

kDk−1Kk Kpk

. (9.16)

Therefore (9.11) and (9.15) imply that xk|k = xpk|k and (2.4) and (4.2) imply that xk+1|k =

xk+1|k.

Note that (2.11) and (4.9) can be expressed as

P xxk|k = (In×n −KkCk)P

xxk|k−1(In×n −KkCk)

T +KkRkKTk , (9.17)

P xxpk|k = (In×n −Kp

kDk−1)Pxxk|k(In×n −Kp

kDk−1)T. (9.18)

Substituting (9.17) into (9.18) yields

P xxpk|k = (In×n −Kp

kDk−1)(In×n −KkCk)Pxxk|k−1(In×n −KkCk)

T(In×n −KpkDk−1)

T

+ (Kk −KpkDk−1Kk)Rk(Kk −Kp

kDk−1Kk)T. (9.19)

Substituting (9.2) and (9.16) into (9.19) yields

P xxpk|k = (In×n − KCk)P

xxk|k−1(In×n − KCk)

T + KRkKT. (9.20)

Since, (2.11) implies that

P xxk|k = (In×n − KCk)P

xxk|k−1(In×n − KCk)

T + KRkKT, (9.21)

it follows from (9.20) and (9.21) that P xxk|k = P xxp

k|k . Hence, (2.4) and (4.3) imply that

P xxk+1|k = P xx

k+1|k.

28

Appendix II: Connection between ECKF and PKF-SP

Assume that system (2.1)-(2.2), (3.8) is time-invariant. Also, assume that (3.3)-(3.5)

hold for D in (3.8). In (Ko and Bitmead 2007), using a descriptor system representation

(Nikoukhah et al. 1999), the system (2.1)-(2.2), (3.8) is written in a projected representa-

tion. Then, consider PKF-SP which uses KF equations (2.4)-(2.11), but initialized with xp0|0

satisfying

Dxp0|0 = d, (9.22)

and the singular initial error covariance

P xxp0|0 = PN (D)P

xx0|0, (9.23)

where the projector PN (D) ∈ Rn×n is obtained by the singular value decomposition

DT=

U1 U2

Ss

0(n−s)×s

V

T

1

VT

2

, (9.24)

where U2 ∈ Rn×(n−s) such that

PN (D) = U2UT

2 . (9.25)

Also, note that, since (3.3) holds, Gwk−1 is constrained in PN (D) and GQk−1GTis a “con-

strained” covariance as used in (Ko and Bitmead 2007).

With Corollary 4.1 and comparing (9.22)-(9.23) to (4.18)-(4.19), we see that, similar to

ECKF, which performs projection only at k = 1 to guarantee constraint satisfaction for all

k ≥ 1, PKF-SP performs projection in initialization, that is, only at k = 0, providing that

(3.3)-(3.5) hold; see Figure 1c.

Appendix III: Connection between ECKF and PKF-EP

PKF-EP projects the updated estimate xk|k (2.10) onto the hyperplane defined by (3.6)

by minimizing the cost function

J(xk)=

xk − xk|k

T

W−1xk − xk|k

(9.26)

subject to (3.6), where W ∈ Rn×n is positive definite. The solution xpk|k to (9.26) is given by

xpk|k = xk|k +Kp

k (dk−1 −Dk−1xk|k), (9.27)

29

where

Kpk

= WD

T

k−1(Dk−1WDT

k−1)−1. (9.28)

The projected error covariance P xxpk|k associated with xp

k|k is given by (4.19).

PKF-EP is formed by forecast (2.4)-(2.8), data-assimilation (2.9)-(2.11), and projection

(9.27)-(9.28), (4.17), (4.19) steps.

We set W = P xxk|k in (9.28), where P xx

k|k is given by (2.11), such that xpk|k (9.27) is optimal

according to the maximum-a-posteriori and minimum-variance criteria (Simon and Chia

2002). In this case, note that the projection equations (9.27)-(9.28), (4.17), (4.19) of PKF-EP

are equal to the projection equations (4.4)-(4.9) of ECKF. However, unlike ECKF, PKF-EP

does not recursively feed the projected estimate xpk|k (9.27) and the error covariance P xxp

k|k

given by (4.19) back in forecast (2.4)-(2.5); see Figure 1b. Therefore, the PKF-EP forecast

estimate xk|k−1 (2.4) is different from its ECKF counterpart (4.2).

30

References

Aguirre, L. A., Alves, G. B., and Correa, M. V. 2007. Steady-state performanceconstraints for dynamical models based on RBF networks. Engineering Applications ofArtificial Intelligence, 20, 924–935.

Aguirre, L. A., Barroso, M. F. S., Saldanha, R. R., and Mendes, E. M. A. M.2004. Imposing steady-state performance on identified nonlinear polynomial models bymeans of constrained parameter estimation. IEE Proceedings in Control Theory andApplications, 151(2), 174–179.

Aguirre, L. A., Teixeira, B. O. S., and Torres, L. A. B. 2005. Using data-drivendiscrete-time models and the unscented Kalman filter to estimate unobserved variablesof nonlinear systems. Physical Review E, 72(2), 026226.

Alouani, A. T. and Blair, W. D. 1993. Use of a Kinematic Constraint in Target Track-ing Constant Speed, Maneuvering Targets. IEEE Transactions on Automatic Control,38, 1107–1111.

Bernstein, D. S. 2005.Matrix Mathematics. (Princeton, USA: Princeton University Press).

Bernstein, D. S. and Hyland, D. C. 1993. Compartmental modelling and second-moment analysis of state space systems. SIAM Journal on Matrix Analysis and Ap-plications, 14(3), 880–901.

Chandrasekar, J., Barrero, O., Ridley, A. J., Bernstein, D. S., and De Moor,D. 2004. State estimation for linearized MHD flow. Proceedings of the 43th IEEE Con-ference on Decision and Control. Paradise Island, The Bahamas. pp. 2584-2589.

Chandrasekar, J., Ridley, A. J., and Bernstein, D. S. 2007. A comparison of theextended and unscented Kalman filters for discrete-time systems with nondifferentiabledynamics. Proceedings of the 2007 American Control Conference. New York City, USA.

Chaves, M. and Sontag, E. D. 2002. State-estimators for chemical reaction networks ofFeinberg-Horn-Jackson zero deficiency type. European Journal of Control, 8, 343–359.

Chen, Y. H. and Chiang, C. T. 1993. Adaptive Beamforming using the ConstrainedKalman Filter. IEEE Transactions on Antennas and Propagation, 41(11), 1576–1580.

Chia, T. L., Chow, P., and Chizeck, H. J. 1991. Recursive parameter identification ofconstrained systems: An application to eletrically stimulated muscle. IEEE Transac-tions on Biomedical Engineering, 38(5), 429–441.

Choi, J., Yeap, T. H., and Bouchard, M. 2005. Online State-Space Modeling usingRecurrent Multilayer Perceptrons with Unscented Kalman Filter. Neural ProcessingLetters, 22(1), 69–84.

Choukroun, D., Bar-Itzhack, I. Y., and Oshman, Y. 2006. Novel quaternion Kalmanfilter. IEEE Transactions on Aerospace and Electronic Systems, 42(1), 174–190.

Crassidis, J. L. 2006. Sigma-point Kalman filtering for integrated GPS and inertial navi-gation. IEEE Transactions on Aerospace and Electronic Systems, 42(2), 750–756.

Crassidis, J. L. and Markley, F. L. 2003. Unscented Filtering for Spacecraft AttitudeEstimation. AIAA Journal of Guidance, Control, and Dynamics, 26(4), 536–542.

Daum, F. E. 2005. Nonlinear filters: Beyond the Kalman filter. IEEE Aerospace and Elec-tronics Systems Magazine, 20(8), 57–69.

De Geeter, J., van Brussel, H., and De Schutter, J. 1997. A smoothly con-strained Kalman filter. IEEE Transactions on Pattern Analysis and Machine Intelli-gence, 19(10), 1171–1177.

Dissanayake, G., Sukkarieh, S., and Nebot, E. 2001. The aiding of a low-cost strap-down inertial measurement unit using vehicle model constraints for land vehicle appli-cations. IEEE Transactions on Robotics and Automation, 17(5), 731–747.

Fletcher, R. 2000. Practical Methods of Optimization. John Wiley & Sons.

Friedberg, J. P. 1987. Ideal Magnetohydrodynamics. Plenum Press.

Goodwin, G. C., Seron, M. M., and de Dona, J. A. 2005. Constrained Control andEstimation: An Optimisation Approach. Springer).

Gupta, N. and Hauser, R. 2007. Kalman filtering with equality and inequality state con-straints. Technical Report 07/18. Numerical Analysis Group, Oxford University Com-puting Laboratory, University of Oxford. http://arxiv.org/abs/0709.2791.

Haykin, S. (Ed.) 2001. Kalman Filtering and Neural Networks. (New York City, USA:Wiley Publishing).

Hirsch, C. 1990. Numerical Computation of Internal and External Flows. John Wiley &Sons).

Hovland, G. E., von Hoff, T. P., Gallestey, E. A., Antoine, M., Farruggio,D., and Paice, A. D. B. 2005. Nonlinear estimation methods for parameter trackingin power plants. Control Engineering Practice, 13, 1341–1355.

Jazwinski, A. H. 1970. Stochastic Processes and Filtering Theory. (New York City, USA:Academic Press, Inc.).

Julier, S. J. and LaViola Jr., J. J. 2007. On Kalman filtering with nonlinear equalityconstraints. IEEE Transactions on Signal Processing, 55(6), 2774–2784.

Julier, S. J. and Uhlmann, J. K. 2004. Unscented filtering and nonlinear estimation.Proceedings of the IEEE, 92, 401–422.

Julier, S. J., Uhlmann, J. K., and Durrant-Whyte, H. F. 2000. A new method forthe nonlinear transformation of means and covariances in filters and estimators. IEEETransactions on Automatic Control, 45(3), 477–482.

Ko, S. and Bitmead, R. R. 2007. State estimation for linear systems with state equalityconstraints. Automatica, 43(8), 1363–1368.

Lee, T., Leok, M., McClamroch, N. H., and Sanyal, A. 2007. Global attitude esti-mation using single direction measurements. Proceedings of the 2007 American ControlConference. New York City, USA.

Lefebvre, T., Bruyninckx, H., and De Schutter, J. 2002. Comment on ”A newmethod for the nonlinear transformation of means and covariances in filters and esti-mators”. IEEE Transactions on Automatic Control, 47(8), 1406–1408.

Lefebvre, T., Bruyninckx, H., and De Schutter, J. 2004. Kalman filters fornonlinear systems: A comparison of performance. International Journal of Control,77(7), 639–653.

Maciejowski, J. M. 2002. Predictive Control with Constraints. (Essex, England: PearsonEducation Ltd.).

Massicotte, D., Morawski, R. Z., and Barwicz., A. 1995. Incorporation of a pos-itivity constraint into a Kalman-filter-based algorithm for correction of spectrometricdata. IEEE Transactions on Instrumentation and Measurement, 44(1), 2–7.

Maybeck, P. 1979. Sthocastic Models, Estimation and Control, v. 1. (San Diego, USA:Academic Press).

Nepomuceno, E. G., Takahashi, R. H. C., Aguirre, L. A., Neto, O. M., andMendes, E. M. A. M. 2004. Multiobjective nonlinear system identification: a casestudy with thyristor series capacitor (TCSC). International Journal of Systems Science,35(9), 537–546.

Nikoukhah, R., Campbell, S. L., and Delebecque., F. 1999. Kalman Filteringfor General Discrete-Time Linear Systems. IEEE Transactions on Automatic Control,44(10), 1829–1839.

Pinto, R. L. F. and Rios Neto, A. 1990. An optimal linear estimation approach to solvesystems of linear algebraic equations. Journal of Computational and Applied Mathemat-ics, 33, 261–268.

Porrill, J. 1988. Optimal combination and constraints for geometrical sensor data. Inter-national Journal of Robotics Research, 7(7), 66–77.

Rao, C. V., Rawlings, J. B., and Lee, J. H. 2001. Constrained Linear State Estimation- A Moving Horizon Approach. Automatica, 37(10), 1619–1628.

Rao, C. V., Rawlings, J. B., and Mayne, D. Q. 2003. Constrained state estimation fornonlinear discrete-time systems: Stability and moving horizon approximations. IEEETransactions on Automatic Control, 48(2), 246–258.

Reif, K., Gunther, S., Yaz, E., and Unbehauen, R. 1999. Stochastic stability ofthe discrete-time extended Kalman filter. IEEE Transactions on Automatic Control,44(4), 714–728.

Robertson, D. G. and Lee, J. H. 2002. On the use of constraints in least squaresestimation and control. Automatica, 38, 1113–1123.

Romanenko, A. and Castro, J. A. A. M. 2004. The unscented filter as an alternativeto the EKF for nonlinear state estimation: a simulation case study. Computers andChemical Engineering, 28, 347–355.

Rotea, M. and Lana, C. 2005. State Estimation with Probability Constraints. 44th IEEEConference on Decision and Control and 2005 European Control Conference. Seville,Spain. pp. 380–385.

Shen, S., Honga, L., and Cong, S. 2006. Reliable road vehicle collision prediction withconstrained filtering. Signal Processing, 86(11), 3339–3356.

Simon, D. 2006. Optimal State Estimation: Kalman, H∞ and Nonlinear Approaches. Wiley-Interscience).

Simon, D. and Chia, T. L. 2002. Kalman filtering with state equality constraints. IEEETransactions on Aerospace and Electronic Systems, 38(1), 128–136.

Simon, D. and Simon, D. L. 2006. Kalman filtering with inequality constraints for tur-bofan engine health estimation. IEE Proceedings – Control Theory and Applications,153(3), 371–378.

Tahk, M. and Speyer, J. L. 1990. Target tracking problems subject to kinematic con-straints. IEEE Transactions on Automatic Control, 35(3), 324–326.

Teixeira, B. O. S. 2008. Constrained State Estimation for Linear and Nonlinear DynamicSystems. Ph.D. Thesis. Graduate Program in Electrical Engineering, Federal Universityof Minas Gerais. Belo Horizonte – MG, Brazil.

Teixeira, B. O. S., Chandrasekar, J., Torres, L. A. B., Aguirre, L. A., andBernstein, D. S. 2007. State estimation for equality-constrained linear systems. Pro-ceedings of the 46th IEEE Conference on Decision and Control. New Orleans, USA. pp.6220–6225.

Teixeira, B. O. S., Chandrasekar, J., Torres, L. A. B., Aguirre, L. A., andBernstein, D. S. 2008a. Unscented filtering for equality-constrained nonlinear sys-tems. Proceedings of the 2008 American Control Conference. Seattle, USA, to appear.

Teixeira, B. O. S., Chandrasekar, J., Palanthandalam-Madapusi, H. J.,Torres, L. A. B., Aguirre, L. A., & Bernstein, D. S. 2008b. Gain-constrainedKalman filtering for linear and nonlinear systems. IEEE Transactions on Signal Pro-cessing, to appear.

Teixeira, B. O. S., Ridley, A., Torres, L. A. B., Aguirre, L. A., and Bernstein,D. S. 2008c. Data assimilation for magnetohydrodynamics with a zero-divergence con-straint on the magnetic field. Proceedings of the 2008 American Control Conference.Seattle, USA, to appear.

Teixeira, B. O. S., Torres, L. A. B., Aguirre, L. A., and Bernstein, D. S. 2008d.Unscented filtering for interval-constrained nonlinear systems. Proceedings of the 47thIEEE Conference on Decision and Control. Cancun, Mexico, submitted.

Vachhani, P., Narasimhan, S., and Rengaswamy, R. 2006. Robust and reliable es-timation via Unscented Recursive Nonlinear Dynamic Data Reconciliation. Journal ofProcess Control, 16, 1075–1086.

van der Merwe, R., Wan, E. A., and Julier, S. J. 2004. Sigma-point Kalman filtersfor nonlinear estimation and sensor-fusion – applications to integrated navigation. Pro-ceedings of the AIAA Guidance, Navigation & Control Conference. Providence, USA.n. AIAA2004-5120.

Walker, D. M. 2006. Parameter estimation using Kalman filters with constraints. Inter-national Journal of Bifurcation and Chaos, 4(16), 1067–1078.

Wen, W. and Durrant-Whyte, H. F. 1992. Model-based multi-sensor data fusion.Proceedings of the 1992 IEEE International Conference on Robotics and Automation.Nice, France. pp. 1720–1726.

Xiong, K., Zhang, H., and Chan, C. 2007. Author’s reply to “Comments on ‘Perfor-mance evaluation of UKF-based nonlinear filtering”’. Automatica, 43, 569–570.

List of Figures

1 Comparison of ECKF, PKF-EP, PKF-SP, and MAKF. . . . . . . . . . . . . 352 Simulation of compartmental model. . . . . . . . . . . . . . . . . . . . . . . 363 Estimation of total mass for compartmental model. . . . . . . . . . . . . . . 374 Estimation error of quaternion norm using UKF, ECUKF, PUKF, MAUKF,

and CUKF. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 385 Estimation error of pendulum energy using UKF, ECUKF, PUKF, MAUKF,

and CUKF. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

35

(a)

(b)

(c)

(d)

Figure 1: Comparative diagram of the (a) equality-constrained Kalman filter (ECKF), (b)projected Kalman filter by estimate projection (PKF-EP), (c) projected Kalman filter bysystem projection (PKF-SP), and (d) measurement-augmentation Kalman filter (MAKF).Unlike PKF-EP, the projection step of ECKF is connected by feedback recursion. In PKF-SP, in the context of time-invariant dynamics, the initial state estimate and the associatederror covariance carry the information provided by the equality constraint. Note that, since(3.3) holds such that GQk−1G

Tis a “constrained” covariance, Qk−1 need not to be modified

in the PKF-SP implementation. Otherwise, we can replace GQk−1GTby PN (D)GQk−1G

Tin

(2.5), where PN (D) is given by (9.25). In MAKF, the equality constraint is enforced at thedata-assimilation step.

36

0 200 400 600 800 1000 1200 1400 1600 1800 20000

0.5

1

1.5

2

k

xk

0

0.5

1

1.5

0.4

0.6

0.8

1

1

1.5

2

x1,k

x2,k

x3

,k

(a) (b)

Figure 2: Data-free simulation of the compartmental model. In (a), the state componentsare shown evolving with time and, in (b), in state space.

37

0 500 1000 1500 2000

2.95

3

3.05

k

Dx

k

Figure 3: Estimate of the total mass (constraint) Dxk in the conservative compartmentalsystem (8.3) using KF (—–) in comparison with the true value (−−).

38

0 20 40 60 80 100 120 140 160 180 20010

!15

10!10

10!5

100

log

10(1

! ||e

k||2)

kT (s)

UKF

MAUKF

PUKF

ECUKF

CUKF

Figure 4: Estimation error of the quaternion vector norm using UKF (· · ··), ECUKF (thick—–), PUKF (−−−), MAUKF (− ·−), and CUKF (thin —–) algorithms. The ECUKF andMAUKF estimates almost coincide, while the constraint error for PUKF is slightly largerthan the constraint error for ECUKF and MAUKF. CUKF estimates satisfy the equalityconstraint at machine precision at most times.

39

0 1 2 3 4 5 6 7 8 9 1010

!12

10!10

10!8

10!6

10!4

10!2

100

102

log

10(E

(0) !

Ek)

kT (s)

discretized model

UKF

MAUKF

PUKF

ECUKF

CUKF

Figure 5: Estimation error of the total energy E(0) for the discretized pendulum (8.19)-(8.20) for UKF (· · ··), MAUKF (thick − · −), PUKF (− − −), ECUKF (thick —-), andCUKF (thin —-) with σv = 0.25. For comparison, the thin dot-dashed line, which is abovethe remaining lines, refers to the energy Ek calculated from data-free simulation of thediscretized model (8.19). ECUKF and MAUKF estimates almost coincide, while CUKFestimates satisfy the equality constraint at machine precision at most times.

40

List of Tables

1 Comparative analysis for compartmental model. . . . . . . . . . . . . . . . . 412 Comparative analysis for attitude estimation. . . . . . . . . . . . . . . . . . 423 Comparative analysis for pendulum using discretized model. . . . . . . . . . 43

41

Table 1: Percent RMS constraint error, RMSE (8.1), and MT (8.2), from k = 1500 to2000, for 100-run Monte Carlo simulation for compartmental system, concerning processnoise levels σw = 0, 0.1, 0.5, and 1.0, and KF, ECKF, MAKF, PKF-EP, and PKF-SPalgorithms.

σw KF ECKF MAKF PKF-EP PKF-SP

Percent RMS constraint error0 0.12 4.52×10−15 4.24×10−11 4.53×10−15 8.19×10−12

0.1 0.22 4.52×10−15 2.01×10−11 4.52×10−15 4.05×10−12

0.5 0.40 4.50×10−15 0.88×10−11 4.51×10−15 3.92×10−12

1.0 0.62 4.53×10−15 0.50×10−11 4.51×10−15 3.98×10−12

RMSEi, i = 1, 2, 3 (×10−3)0 0.57, 0.36, 2.93 0.10, 0.16, 0.210.1 6.26, 2.60, 7.34 6.25, 2.54, 4.190.5 9.01, 4.58, 13.2 9.01, 4.55, 6.751.0 9.35, 5.58, 19.7 9.35, 5.56, 8.07

MT (×10−4)0 0.0996 0.00120.1 1.0515 0.63520.5 2.8057 1.47221.0 5.4646 1.8387

42

Table 2: Percent RMS constraint error, RMSE (8.1), and MT (8.2), from k = 5000 to 10000,for 100-run Monte Carlo simulation for attitude estimation using UKF, MAUKF, PUKF,ECUKF, and CUKF.

UKF MAUKF PUKF ECUKF CUKF

Percent RMS constraint error (×10−4)254.1 6.50 8.31 6.49 0.58

RMSEi for e0, e1, e2, e3, β1, β2, β3 (×10−3)1.331 1.398 1.334 1.398 1.3321.366 1.434 1.365 1.433 1.3651.320 1.377 1.320 1.376 1.3181.319 1.374 1.315 1.374 1.3130.124 0.130 0.124 0.130 0.1240.119 0.125 0.119 0.125 0.1190.120 0.126 0.120 0.125 0.120

MT for attitude (×10−6)8.18 5.42 8.16 5.42 8.18

MT for drift (×10−7)1.17 1.09 1.17 1.09 1.17

43

Table 3: Percent RMS constraint error, RMSE (8.1), and MT (8.2), from k = 3000 to4000, for 100-run Monte Carlo simulation for the discretized pendulum (8.19), concerningdifferent levels of observation noise σv = 0.1, 0.25, and 0.5, and algorithms, namely, UKF,ECUKF, PUKF, MAUKF, and CUKF. We show in italics results for the case in which thetrue continuous-time model (8.18) is used with σw = 0.0003 to help convergence.

σv UKF MAUKF PUKF ECUKF CUKF

Percent RMS constraint error (×10−2)0.1 356.30 1.95 5.65 1.95 0.000066

21.81 0.06 0.27 0.06 0.0001510.25 459.40 3.50 9.11 3.51 0.000050

25.68 0.14 1.09 0.14 0.0001340.5 594.61 5.98 15.93 5.97 0.000040

29.80 0.32 3.16 0.31 0.000119RMSEi, i = 1, 2 (×10−2)

0.1 2.95, 2.88 0.91, 1.92 1.15, 2.12 0.91, 1.92 1.20, 2.070.59, 1.16 0.17, 0.36 0.39, 0.82 0.17, 0.36 0.32, 0.67

0.25 3.93, 5.59 1.32, 3.05 1.76, 3.84 1.32, 3.04 1.84, 3.750.98, 2.15 0.30, 0.68 0.70, 1.55 0.29, 0.68 0.59, 1.32

0.5 5.56, 9.61 1.80, 4.00 2.76, 5.93 1.80, 3.99 2.86, 6.031.60, 3.55 0.62, 1.35 1.15, 2.54 0.60, 1.30 0.98, 2.14MT (×10−4)

0.1 26.79 8.09 9.08 8.09 26.892.18 0.37 1.09 0.37 0.37

0.25 61.66 20.67 26.63 20.66 61.548.31 0.94 4.18 0.94 0.94

0.5 139.94 42.13 66.86 42.11 138.6023.17 2.10 11.73 2.10 2.10

44