International Journal of Control

Vol. 82, No. 5, May 2009, 918–936

DOI: 10.1080/00207170802370033
March 2008

State Estimation for Linear and Nonlinear

Equality-Constrained Systems
by

Bruno O. S. Teixeira∗† , Jaganath Chandrasekar† ,

Leonardo A. B. Tôrres∗ , Luis A. Aguirre∗ , and Dennis S. Bernstein†

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

†
Department of Aerospace Engineering, University of Michigan
Ann Arbor, MI, USA, 48109-2140, {jchandra,dsbaero}@umich.edu

Abstract

This paper addresses the state-estimation problem for linear and nonlinear systems
for 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 to
the equality-constrained state-estimation problem for linear and Gaussian systems and is
compared 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, the
measurement-augmentation unscented Kalman filter, and the constrained unscented Kalman
filter. Finally, these methods are compared on linear and nonlinear examples.

Key Words: Equality constraints, state estimation, Kalman filtering, unscented Kalman
filter
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−1 xk−1 + Bk−1 uk−1 + Gk−1 wk−1 , (2.1)

yk = C k xk + 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 x̂0|0
xx � T
and error-covariance P0|0 = E[(x0 − x̂0|0 )(x0 − x̂0|0 ) ]. 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 x̂k|k of J is the optimal state
estimate.
The optimal state estimate x̂k|k is given by the Kalman filter (KF) (Jazwinski 1970),
whose forecast step is given by

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

xx xx T T
Pk|k−1 = Ak−1 Pk−1|k−1 Ak−1 + Gk−1 Qk−1 Gk−1 , (2.5)

ŷk|k−1 = Ck x̂k|k−1 , (2.6)

yy xx T
Pk|k−1 = Ck Pk|k−1 C k + Rk , (2.7)
xy xx T
Pk|k−1 = Pk|k−1 Ck , (2.8)
xx � yy
T � T
where Pk|k−1 = E[(xk − x̂k|k−1 )(xk − x̂k|k−1 ) ], Pk|k−1 = E[(yk − ŷk|k−1 )(yk − ŷk|k−1 ) ], and
xy � T
Pk|k−1 = E[(xk − x̂k|k−1 )(yk − ŷk|k−1 ) ], and whose data-assimilation step is given by
xy yy
Kk = Pk|k−1 (Pk|k−1 )−1 , (2.9)

x̂k|k = x̂k|k−1 + Kk (yk − ŷk|k−1 ), (2.10)

xx xx yy T
Pk|k = Pk|k−1 − Kk Pk|k−1 Kk , (2.11)
xx � T
where Pk|k = E[(xk − x̂k|k )(xk − x̂k|k ) ] is the error-covariance matrix and Kk is the Kalman
gain matrix. The notation ẑk|k−1 indicates an estimate of zk at time k based on information

4
available up to and including time k − 1. Likewise, ẑk|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−1 wk−1 is absent. Therefore, there exists Ek−1 ∈ Rr×n such that
rank(Ek−1 ) = r and
Ek−1 Gk−1 = 0r×q . (3.1)
� �
� T1,k−1
Let T1,k−1 ∈ R(n−r)×n be such that Tk−1 = ∈ Rn×n is invertible. For example, we
Ek−1
� T
can choose T1,k−1 = Gk−1 . Multiplying (2.1) by Tk−1 yields
� � � � � � � �
T1,k−1 T1,k−1 Ak−1 T1,k−1 Bk−1 T1,k−1 Gk−1
xk = xk−1 + uk−1 + wk−1 .
Ek−1 Ek−1 Ak−1 Ek−1 Bk−1 0r×q

Hence, for all k ≥ 1,

Ek−1 xk = ek−1 , (3.2)
�
where ek−1 = Ek−1 (Ak−1 xk−1 + Bk−1 uk−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−1 wk−1 has singular covariance
T
Gk−1 Qk−1 Gk−1 (Ko and Bitmead 2007, Goodwin et al. 2005). � �
� E1,k−1
Let s be an integer satisfying 1 ≤ s ≤ r, and partition Ek−1 = , where
Dk−1
E1,k−1 ∈ R(r−s)×n and Dk−1 ∈ Rs×n . It thus follows from (3.1) that

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

Proposition 3.1. Assume that

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

and

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

5
Then, for all k ≥ 1,
Dk−1 xk = dk−1 , (3.6)

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

Proof. It follows from (3.2) that Dk−1 xk = Dk−1 (Ak−1 xk−1 + Bk−1 uk−1 ) = Dk−1 xk−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−1 d. 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−1 Gk−1 . . . An−1
k−1 Gk−1

by Dk−1 yields

� �
Dk−1 K(Ak−1 , Gk−1 ) = Dk−1 Gk−1 Dk−1 Ak−1 Gk−1 · · · Dk−1 An−1
k−1 Gk−1
� �
= 0s×q Dk−1 Gk−1 · · · Dk−1 An−2
k−1 G

= 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 x̂pk|k denote the solution of the equality-
constrained state-estimation problem.

xx
Lemma 4.1. Assume that Pk|k−1 given by (2.5) and Rk are positive definite. x̂pk|k maxi-
mizes J given by (2.3) subject to (3.6) if and only if x̂pk|k minimizes
� �
� T T
J (xk ) = (xk − x̂k|k−1 ) xx
(Pk|k−1 )−1 (xk − x̂k|k−1 ) + (yk − Ck xk ) Rk−1 (yk − Ck xk ) (4.1)

subject to (3.6), where x̂k|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).

xxp �
= E[(xk − x̂pk|k )(xk − x̂pk|k ) ] + δIn×n , where
T
Define the projected error covariance Pk|k
xx
0 < δ � 1 is a very small number compared to the entries of Pk|k that guarantees that
xxp
Pk|k is positive definite. From our experience, we set 10−15 ≤ δ ≤ 10−9 for computational
implementation.

xx
Proposition 4.1. Let x̂k|k−1 and Pk|k−1 be given by

�
x̂k|k−1 = Ak−1 x̂pk−1|k−1 + Bk−1 uk−1 , (4.2)
xx � xxp T T
Pk|k−1 = Ak−1 Pk−1|k−1 Ak−1 + Gk−1 Qk−1 Gk−1 . (4.3)

Define

�
dˆk−1|k = Dk−1 x̂k|k , (4.4)
dd � xx T
Pk|k = Dk−1 Pk|k Dk−1 , (4.5)
xd � xx T
Pk|k = Pk|k Dk−1 , (4.6)
�
Kkp = Pk|k
xd dd −1
(Pk|k ) , (4.7)

7
 xx
where x̂k|k is given by (2.10) and Pk|k is given by (2.11). Then x̂pk|k and Pk|k
xxp
are given by

x̂pk|k = x̂k|k + Kkp (dk−1 − dˆk−1|k ), (4.8)

T
xxp
Pk|k xx
= Pk|k − Kkp Pk|k
dd
Kkp + δIn×n . (4.9)

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

� T
L = J (xk ) + 2λ (Dk−1 xk − dk−1 ).

The necessary conditions for a minimizer x̂pk|k are given by

∂L T T
xx
= (Pk|k−1 )−1 (x̂pk|k − x̂k|k−1 ) − Ck Rk−1 (yk − Ck x̂pk|k ) + Dk−1 λ = 0n×1 , (4.10)
∂xk
∂L
= Dk−1 x̂pk|k − dk−1 = 0s×1 . (4.11)
∂λ

It follows from (4.10) that

T T T
xx
((Pk|k−1 )−1 + Ck Rk−1 Ck )(x̂pk|k − x̂k|k−1 ) = Ck Rk−1 (yk − Ck x̂k|k−1 ) − Dk−1 λ. (4.12)

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

xx xx yy T
Pk|k = Pk|k−1 − Kk Pk|k−1 Kk
xy yy yy yy xy T T
xx
= Pk|k−1 − Pk|k−1 (Pk|k−1 )−1 Pk|k−1 (Pk|k−1 )− (Pk|k−1 )
T T
xx
= Pk|k−1 xx
− Pk|k−1 xx
Ck (Ck Pk−1|k−1 Ck + Rk )−1 Ck Pk|k−1
xx

T
xx
= ((Pk|k−1 )−1 + Ck Rk−1 Ck ))−1 . (4.13)

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

xy yy
Kk = Pk|k−1 (Pk|k−1 )−1
T T
xx
= Pk|k−1 xx
Ck (Ck Pk|k−1 Ck + Rk )−1
xx −1 xx T T
xx
= Pk|k (Pk|k xx
) Pk|k−1 Ck (Ck Pk|k−1 Ck + Rk )−1
T T T
xx
= Pk|k (Ck Rk−1 Ck + (Pk|k−1
xx
)−1 )Pk|k−1
xx xx
Ck (Ck Pk|k−1 Ck + Rk )−1
T T T T
xx
= Pk|k (Ck Rk−1 Ck Pk|k−1
xx
Ck + Ck Rk−1 Rk )(Ck Pk|k−1
xx
Ck + Rk )−1
T T T
xx
= Pk|k Ck Rk−1 (Ck Pk|k−1
xx xx
Ck + Rk )(Ck Pk|k−1 Ck + Rk )−1
T
xx
= Pk|k Ck Rk−1 . (4.14)

8
 xx
Substituting (4.13) and (4.14) into (4.12) and multiplying by Pk|k yields
T
x̂pk|k = x̂k|k−1 + Kk (yk − Ck x̂k|k−1 ) − Pk|k
xx
Dk−1 λ. (4.15)

Substituting (4.15) into (4.11) yields

T T
xx
dk−1 = Dk−1 x̂k|k−1 + Dk−1 Pk|k Ck Rk−1 (yk − Ck x̂k|k−1 ) − Dk−1 Pk|k
xx
Dk−1 λ,

which implies
T T
xx
λ=(Dk−1 Pk|k Dk−1 )−1 (Dk−1 x̂k|k−1 − dk−1 ) + (Dk−1 Pk|k
xx
Dk−1 )−1 Dk−1 Kk (yk − Ck x̂k|k−1 ).(4.16)

Likewise, substituting (4.16) into (4.15) yields

T T
x̂pk|k = x̂k|k−1 + Kk (yk − Ck x̂k|k−1 ) − Pk|k
xx xx
Dk−1 (Dk−1 Pk|k Dk−1 )−1 (Dk−1 x̂k|k−1 − dk−1 ) −
T T
xx
Pk|k xx
Dk−1 (Dk−1 Pk|k Dk−1 )−1 Dk−1 Kk (yk − Ck x̂k|k−1 )

= x̂k|k−1 + Kk (yk − Ck x̂k|k−1 ) −

xx T xx T
� �
Pk|k Dk−1 (Dk−1 Pk|k Dk−1 )−1 Dk−1 x̂k|k−1 − dk−1 + Dk−1 Kk yk − Dk−1 Kk Ck x̂k|k−1

= x̂k|k−1 + Kk (yk − Ck x̂k|k−1 ) +

xx T xx T
� �
Pk|k Dk−1 (Dk−1 Pk|k Dk−1 )−1 dk−1 − Dk−1 (x̂k|k−1 + Kk (yk − Ck x̂k|k−1 )) .

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

x̂pk|k = x̂k|k−1 + Kk (yk − ŷk|k−1 ) +

xx T xx T
� �
Pk|k Dk−1 (Dk−1 Pk|k Dk−1 )−1 dk−1 − Dk−1 (x̂k|k−1 + Kk (yk − ŷk|k−1 ))

= x̂k|k−1 + Kk (yk − ŷk|k−1 ) + Kkp (dk−1 − dˆk−1|k )

= x̂k|k + Kkp (dk−1 − dˆk−1|k ),

which proves (4.8).

xx
Given the symmetry between (4.8) and (2.10) and recalling that Pk|k is given by (2.11),
xxp
it follows that Pk|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

� T T
PN (Dk−1 ) = In×n − W Dk−1 (Dk−1 W Dk−1 )−1 Dk−1 . (4.17)

Then PN (Dk−1 ) is an oblique projector whose range is N (Dk−1 ), that is, PN2 (Dk−1 ) = PN (Dk−1 )
but it is not necessarily symmetric.

xx
For the following two results, let x̂k|k given by (2.10) and Pk|k given by (2.11) denote the
updated estimate and updated error covariance provided by ECKF. Also, let x̂pk|k given by
xxp
(4.8) and Pk|k given by (4.9) denote the projected estimate and projected error covariance
of ECKF.

xx
Proposition 4.2. Set W = Pk|k in (4.17). Then, the projection step (4.4)-(4.9) is
equivalent to

x̂pk|k = PN (Dk−1 ) x̂k|k + dk−1 , (4.18)

xxp xx
Pk|k = PN (Dk−1 ) Pk|k + δIn×n , (4.19)

� T T
xx
where dk−1 = Pk|k xx
Dk−1 (Dk−1 Pk|k Dk−1 )−1 dk−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, Dx̂pk−1|k−1 = d
xxp
and DPk−1|k−1 xx
= 0s×n and set δ = 0. Then Dx̂k|k = d, DPk|k = 0s×n , x̂pk|k = x̂k|k , and
xxp xx
Pk|k = Pk|k .

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

Dx̂k|k−1 = DAx̂pk−1|k−1 + DBuk−1 = Dx̂pk−1|k−1 + 0s×1 = d, (4.20)

xx xxp T xxp T T T T
DPk|k−1 = DAPk−1|k−1 A + DGQk−1 G = DPk−1|k−1 A + 0s×q Qk−1 G = 0s×n A = 0s×n(4.21)
.

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

xy yy yy T yy T
DKk = DPk|k−1 (Pk|k−1 )−1 = DPk|k−1
xx
C (Pk|k−1 )−1 = 0s×n C (Pk|k−1 )−1 = 0s×m . (4.22)

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

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

xx xx yy T yy T
DPk|k = DPk|k−1 − DKk Pk|k−1 Kk = 0s×n + 0s×m Pk|k−1 Kk = 0s×n . (4.24)

Given (4.23)-(4.24) and δ = 0, from (4.18)-(4.19), we have x̂pk|k = x̂k|k and Pk|k
xxp xx
= Pk|k .

Corollary 4.1. Assume that Dx̂p1|1 = d and DP1|1

xxp
= 0s×n . Then, for all k ≥ 2,
xx
Dx̂k|k = d and DPk|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 x̂k|k given by (2.10) satisfies Dx̂k|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 x̂k|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−1 xk + vkd , (5.1)

where vkd ∈ Rs is a white, Gaussian, zero-mean noise with covariance Rkd , and
� it is treated
�
d � Rk 0m×s
as an extra noisy measurement by MAKF (see Appendix I) but with R̃k =
0s×m Rkd
replacing R̃k .

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 ŷ ∈ 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 ŷ and the actual covariance P yy
if h = h1 + h2 , where h1 is linear and h2 is quadratic (Julier et al. 2000). Otherwise, ŷk 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 x̂ak−1|k−1 of the augmented prior state vector
� �
a � xk−1
xk−1 = , (6.4)
wk−1
�
where xak−1 ∈ Rna and na = n + q, as well as the augmented prior error covariance
� xx �
xxa � Pk−1|k−1 0n×q
Pk−1|k−1 = , (6.5)
0q×n Qk−1

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

� � � �1/2 � �1/2 �
Xk−1|k−1 = x̂ak−1|k−1 11×(2na +1) + (na + λ) 0na ×1 xxa
Pk−1|k−1 − xxa
Pk−1|k−1 ,(6.6)

with weights

 (m) � λ

 γ = ,


0
na + λ
 λ
(c) �
γ0 = + 1 − α2 + β, (6.7)

 n a + λ

 (m) � (c) � � � 1

 γi
(m)
= γi = γi+na = γi+na =
(c)
, i = 1, . . . , na ,
2(na + λ)

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
x x w
coli (Xk|k−1 ) = f (coli (Xk−1|k−1 ), uk−1 , coli (Xk−1|k−1 ), k − 1), i = 0, . . . , 2na , (6.8)
2na
� (m) x
x̂k|k−1 = γi coli (Xk|k−1 ), (6.9)
i=0
2na
�
xx (c) x x T
Pk|k−1 = γi [coli (Xk|k−1 ) − x̂k|k−1 ][coli (Xk|k−1 ) − x̂k|k−1 ] , (6.10)
i=0
x
coli (Yk|k−1 ) = h(coli (Xk|k−1 ), k), i = 0, . . . , 2na , (6.11)
2na
� (m)
ŷk|k−1 = γi coli (Yk|k−1 ), (6.12)
i=0
2na
�
yy (c) T
Pk|k−1 = γi [coli (Yk|k−1 ) − ŷk|k−1 ][coli (Yk|k−1 ) − ŷk|k−1 ] + Rk , (6.13)
i=0
2na
�
xy (c) x T
Pk|k−1 = γi [coli (Xk|k−1 ) − x̂k|k−1 ][coli (Yk|k−1 ) − ŷk|k−1 ] , (6.14)
i=0
� x �
Xk−1|k−1 � x
where w = Xk−1|k−1 , Xk−1|k−1 ∈ Rn×(2na +1) , and Xk−1|k−1
w
∈ Rq×(2na +1) . The UKF
Xk−1|k−1
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 xx 1/2 xx 1/2
Xk|k = x̂k|k 11×(2n+1) + (n + λ) 0n×1 Pk|k − Pk|k , (7.1)

xx
where x̂k|k and Pk|k are given by (2.10) and (2.11), respectively. Then the sigma points
x
coli (Xk|k ) ∈ Rn , i = 0, . . . , 2n, are propagated through (6.3), yielding

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

such that dˆk−1|k , Pk|k

dd xd
, and Pk|k are given by
2n
�
dˆk−1|k =
(m)
γi coli (Dk|k ), (7.3)
i=0
�2n
T
γi [coli (Dk|k ) − dˆk−1|k ][coli (Dk|k ) − dˆk−1|k ] ,
dd (c)
Pk|k = (7.4)
i=0
�2n
T
) − x̂k|k ][coli (Dk|k ) − dˆk−1|k ] ,
xd (c) x
Pk|k = γi [coli (Xk|k (7.5)
i=0

and Kkp , x̂pk|k , and Pk|k

xxp
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

� p � � xxp �
ap � xk−1 xxap � Pk−1|k−1 0n×q
Define xk−1 = , and Pk−1|k−1 = , such that the sigma points
wk−1 0q×n Qk−1
� � �1/2 � �1/2 �
ap
� xxap xxap
Xk−1|k−1 = x̂k−1|k−1 11×(2na +1) + (na + λ) 0na ×1 Pk−1|k−1 − Pk−1|k−1 , (7.6)

are chosen based on x̂pk−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
�
ỹk = h̃(xk , k) + ṽk , (7.7)
� � � � � �
� yk � h(xk , k) � vk
where ỹk = , h̃(xk , k) = , and ṽk = . With (7.7), the
dk−1 g(xk , k − 1) 0s×1
measurement-augmentation unscented Kalman filter (MAUKF) combines (6.6)-(6.10) with
the augmented forecast equations
x
coli (Ỹk|k−1 ) = h̃(coli (Xk|k−1 ), k), i = 0, . . . , 2na , (7.8)
2na
� (m)
ỹˆk|k−1 = γi coli (Ỹk|k−1 ), (7.9)
i=0
2na
� (c) T
ỹ ỹ
P̃k|k−1 = γi [coli (Ỹk|k−1 ) − ỹˆk|k−1 ][coli (Ỹk|k−1 ) − ỹˆk|k−1 ] + R̃k , (7.10)
i=0
2na
� (c) T
xỹ
P̃k|k−1 = x
γi [coli (Xk|k−1 ) − x̂k|k−1 ][coli (Ỹk|k−1 ) − ỹˆk|k−1 ] , (7.11)
i=0

where
� �
� Rk 0m×s
R̃k = , (7.12)
0s×m 0s×s
and the KF data-assimilation equations
xỹ ỹ ỹ
K̃k = P̃k|k−1 (P̃k|k−1 )−1 , (7.13)

x̂k|k = x̂k|k−1 + K̃k (ỹk − ỹˆk|k−1 ), (7.14)

xx xx ỹ ỹ T
Pk|k = Pk|k−1 − K̃k P̃k|k−1 K̃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̃k|k−1 in (7.10),
we replace R̃k by R̃kd , where we set Rkd = δ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

arg min J1 (xk ),

x̂k|k = {x : g (x , k − 1) = d } (7.16)
k k k−1

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

� �
� T T
J1 (xk ) = (xk − x̂k|k−1 ) xx
(Pk|k−1 )−1 (xk − x̂k|k−1 ) + (yk − h(xk , k)) Rk−1 (yk − h(xk , k)) , (7.17)

xx
where x̂k|k−1 is given by (6.9) and Pk|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-
xx
similated by the error-covariance Pk|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 x̂i,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
�
�c �
� �kf
1 � 1
RMSEi = (xi,k − x̂i,k|k,j )2 , i = 1, . . . , n (8.1)
c j=1 kf − k0 + 1 k=k
0

where xi,k is the true value. For ECKF and PKF-EP, as well as for ECUKF and PUKF,
x̂i,k|k,j is replaced by x̂pi,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 x̂k|k is by

17
 xx
evaluating the mean trace (MT) of Pk|k given by (2.11) from time k = k0 to kf , that is,
c
� kf
�
1� 1 �
xx
MT = tr(Pk|k,j ) . (8.2)
c j=1 kf − k0 + 1 k=k
0

MT measures the uncertainty in the estimate x̂k|k . For ECKF and PKF-EP, as well as for
xx xxp
their nonlinear counterparts, Pk|k,j is replaced by Pk|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
   
0.94 0.028 0.019 0.05 −0.03 � �
1 0 0
A =  0.038 0.95 0.001  , B = 03×1 , G =  −0.02 0.01  , C = , (8.3)
0 1 0
0.022 0.022 0.98 −0.03 0.02
T
with initial condition x0 = [ 1 1 1 ] and process noise and observation noise covariance
matrices Qk−1 = σw2 I3×3 and Rk = σv2 I2×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

T
xx
x̂0|0 = [ 2 1 0 ] , P0|0 = I3×3 . (8.5)

Figure 3 shows that the KF estimates do not lie on the plane (3.8). Even if x̂0|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 x̂k|k−1 compared to the other constrained
algorithms. This is expected because PKF-EP do not use x̂pk−1|k−1 to calculate x̂k|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
1
ė(t) = Ω(t)e(t), (8.6)
2
T
where the state vector is the quaternion vector e(t) = [e0 (t) e1 (t) e2 (t) e3 (t)] , the matrix
Ω(t) is given by
 
0 r(t) −q(t) p(t)
 −r(t) 0 p(t) q(t) 
Ω(t) = 
 q(t) −p(t)
, (8.7)
0 r(t) 
−p(t) −q(t) −r(t) 0
T
and the angular velocity vector u(t) = [p(t) q(t) r(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
 T
We set e(0) = [0.9603 0.1387 0.1981 0.1387] and
� � 2π � � 2π � � 2π ��T
u(t) = 0.03 sin 600
t 0.03 sin 600
t − 300 0.03 sin 600
t − 600 . (8.9)

To perform attitude estimation, we assume that

u
ŭk−1 = u((k − 1)T ) + βk−1 + wk−1 (8.10)

u
is measured by rate gyros, where T is the discretization step, wk−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 by
� � � �� � � �
ek Ak−1 04×3 ek−1 04×1
= + β , (8.11)
βk 03×4 I3×3 βk−1 wk−1
� β �
where ek = e(kT ), wk−1 ∈ R3 is process noise associated with the drift-error model, xk =
� � � u �
ek 7 � wk−1
∈ R is the state vector, wk−1 = β ∈ R6 is the process noise, and
βk wk−1

� 1 T sin(sk−1 )
Ak−1 = cos(sk−1 )I4×4 + Ωk−1 , (8.12)
2 sk−1
�
Ωk−1 = Ω((k − 1)T ), (8.13)
� T �� ��
sk−1 = ��ŭk−1 − βk−1 − wk−1
u ��
. (8.14)
2 2

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

x21,k + x22,k + x23,k + x24,k = 1. (8.15)

T
We set T = 0.1 s, βk−1 = [ 0.001 −0.001 0.0005 ] rad/s, and Qk−1 = diag (10−5 I3×3 , 10−10 I3×3 ).
[i]
Attitude observations yk ∈ R3 for a direction sensor are given by (Crassidis and Markley
2003)
[i] [i]
yk = Ck r[i] + vk , (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
 2 
x1,k − x22,k − x23,k + x24,k 2(x1,k x2,k + x3,k x4,k ) 2(x1,k x3,k − x2,k x4,k )
Ck =  2(x1,k x2,k − x3,k x4,k ) −x21,k + x22,k − x23,k + x24,k 2(x1,k x4,k + x2,k x3,k ) .
2(x1,k x3,k + x2,k x4,k ) 2(−x1,k x4,k + x2,k x3,k ) −x21,k − x22,k + x23,k + x24,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-
T T
axis magnetometer. We set r[1] = [ 1 0 0 ] , r[2] = [ 0 1 0 ] , and Rk = 10−4 I6×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
T
(8.11), (8.16), and constraint (8.15). We initialize these algorithms with x̂0|0 = [ 1 0 0 0 0 0 0 ]
xx
and P0|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

g
θ̈(t) + sin θ(t) = 0, (8.18)
L

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 model
� � � � � �
x1,k x1,k−1 + T x2,k−1 w1,k
= + , (8.19)
x2,k x2,k−1 − T Lg sin (x1,k−1 ) w2,k

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

mL2 2
−mgL cos(x1 (t)) + x (t) = E(0), (8.21)
2 2

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

� mL2 2
Ek = −mgL cos(x1,k ) + x . (8.22)
2 2,k

We use the 4-th order Runge-Kutta integration scheme to obtain x(kT ) for L = 1 m, T = 10
3π π
ms, and initial conditions θ(0) = 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.
T
The state estimation is initialized with Qk−1 = σw2 I2×2 , Rk = σv2 , x̂0|0 = [ 1 1 ] , and P0|0
xx
=
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 vk
ỹk = = C̃k xk + , (9.1)
dk−1 0s×1

25
where � �
� Ck
C̃k = . (9.2)
Dk−1
With (9.1), MAKF uses (2.4)-(2.5) together with the augmented forecast equations

ỹˆk|k−1 = C̃k x̂k|k−1 , (9.3)

ỹ ỹ xx T
P̃k|k−1 = C̃k Pk|k−1 C̃k + R̃k , (9.4)
xỹ xx T
P̃k|k−1 = Pk|k−1 C̃k , (9.5)

where R̃k is given by (7.12), and the augmented data-assimilation equations given by (7.13)-
(7.15).
�
For convenience, let x̃k|k−1 = x̂k|k−1 (2.4) denote the forecast estimate provided by MAKF.
xx � xx
Furthermore, let P̃k|k−1 = Pk|k−1 (2.5) be the associated forecast error covariance of MAKF.
xx
Also let x̂k|k−1 (4.2) and Pk|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.

xx xx
Proposition 9.1. Assume that x̃k|k−1 = x̂k|k−1 and P̃k|k−1 = Pk|k−1 . Then x̃k+1|k = x̂k+1|k
xx xx
and P̃k+1|k = Pk+1|k .

ỹ ỹ
Proof. P̃k|k−1 (9.4) is equivalent to
� yy xx T �
ỹ ỹ Pk|k−1 Ck Pk|k−1 Dk−1
P̃k|k−1 = xx .
Dk−1 Pk|k−1 CkT dd
Pk|k−1
ỹ ỹ −1
It follows from (Bernstein 2005) that P̃k|k−1 has entries
 
−1 −1
(P̃k|k−1 )1 (P̃k|k−1 )12
ỹ ỹ −1
P̃k|k−1 =  � −1 �T
−1
,
(P̃k|k−1 )12 (P̃k|k−1 )2

where
� �−1
−1 � yy
(P̃k|k−1 )1 = Pk|k−1 xx
− Ck Pk|k−1 T
Dk−1 dd
(Pk|k−1 )−1 Dk−1 Pk|k−1
xx
CkT ,
� �−1
−1 � yy
(P̃k|k−1 )12 = − Pk|k−1 xx
− Ck Pk|k−1 T
Dk−1 dd
(Pk|k−1 )−1 Dk−1 Pk|k−1
xx
CkT
xx
×Ck Pk|k−1 T
Dk−1 dd
(Pk|k−1 )−1,
� �−1
−1 � yy
(P̃k|k−1 dd
)2 = Pk|k−1 xx
− Dk−1 Pk|k−1 CkT (Pk|k−1 )−1 Ck Pk|k−1
xx T
Dk−1 . (9.6)

26
Furthermore, it can be shown that

−1 yy yy −1 yy
(P̃k|k−1 )1 = (Pk|k−1 )−1 + (Pk|k−1 )−1 Ck Pk|k−1
xx T
Dk−1 (P̃k|k−1 xx
)2 Dk−1 Pk|k−1 CkT (Pk|k−1 )−1 , (9.7)
−1 yy −1
(P̃k|k−1 )12 = −(Pk|k−1 )−1 Ck Pk|k−1
xx T
Dk−1 (P̃k|k−1 )2 . (9.8)

It follows from (2.9) that

yy
xx
Kk = Pk|k−1 CkT (Pk|k−1 )−1 , (9.9)

Furthermore substituting (9.9) into (2.11) yields

yy
xx
Pk|k xx
= Pk|k−1 xx
− Pk|k−1 CkT (Pk|k−1 )−1 Ck Pk|k−1
xx
.

Hence,
� �−1
yy
xx T
(Dk−1 Pk|k Dk−1 )−1 = xx
Dk−1 Pk|k−1 T
Dk−1 xx
− Dk−1 Pk|k−1 CkT (Pk|k−1 )−1 Ck Pk|k−1
xx T
Dk−1
−1
= (P̃k|k−1 )2 . (9.10)

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

� �� �
x̂pk|k = x̂k|k−1 + Kk − Kkp Dk−1 Kk Kkp ỹk − C̃k x̂k|k−1 . (9.11)

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

� �
−1
p xx ( P̃ k|k−1 ) 12
Kk = Pk|k−1 C̃kT −1 . (9.12)
(P̃k|k−1 )2

Substituting (9.8) into (9.12) and substituting the resulting expression into Kk − Kkp Dk−1 Kk
yields
�
Kk − Kkp Dk−1 Kk = Pk|k−1
xx yy
CkT (Pk|k−1 yy
)−1 + (Pk|k−1 )−1 Ck Pk|k−1
xx T
Dk−1 −1
(P̃k|k−1 )2 Dk−1
�
yy −1 yy
xx
Pk|k−1 CkT (Pk|k−1 )−1 − Pk|k−1
xx T
Dk−1 (P̃k|k−1 xx
)2 Dk−1 Pk|k−1 CkT (Pk|k−1 )−1
.

Hence, (9.7) and (9.8) imply that

� �
−1
(P̃k|k−1 )1
Kk − Kkp Dk−1 Kk = Pk|k−1
xx
C̃kT −1 T . (9.13)
(P̃k|k−1 )12

27
Therefore, it follows from (9.12) and (9.13) that
� � ỹ ỹ −1
Kk − Kkp Dk−1 Kk Kkp xx
= Pk|k−1 C̃kT P̃k|k−1 . (9.14)

Since the estimate x̃k|k of MAKF is given by

� �
x̃k|k = x̃k|k−1 + K̃k ỹk − C̃k x̃k|k−1 , (9.15)

where
� �−1
xx
K̃k = P̃k|k−1 C̃kT C̃k P̃k|k−1
xx
C̃kT + R̃k ,

it follows from (9.14) that

� �
K̃k = Kk − Kkp Dk−1 Kk Kkp . (9.16)

Therefore (9.11) and (9.15) imply that x̃k|k = x̂pk|k and (2.4) and (4.2) imply that x̃k+1|k =
x̂k+1|k .
Note that (2.11) and (4.9) can be expressed as

xx xx
Pk|k = (In×n − Kk Ck )Pk|k−1 (In×n − Kk Ck )T + Kk Rk KkT , (9.17)
xxp
Pk|k = (In×n − Kkp Dk−1 )Pk|k
xx
(In×n − Kkp Dk−1 )T . (9.18)

Substituting (9.17) into (9.18) yields

xxp
Pk|k = (In×n − Kkp Dk−1 )(In×n − Kk Ck )Pk|k−1
xx
(In×n − Kk Ck )T (In×n − Kkp Dk−1 )T

+ (Kk − Kkp Dk−1 Kk )Rk (Kk − Kkp Dk−1 Kk )T . (9.19)

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

xxp xx
Pk|k = (In×n − K̃ C̃k )Pk|k−1 (In×n − K̃ C̃k )T + K̃ R̃k K̃ T . (9.20)

Since, (2.11) implies that

xx xx
P̃k|k = (In×n − K̃ C̃k )P̃k|k−1 (In×n − K̃ C̃k )T + K̃ R̃k K̃ T , (9.21)

xx xxp
it follows from (9.20) and (9.21) that P̃k|k = Pk|k . Hence, (2.4) and (4.3) imply that
xx xx
P̃k+1|k = Pk+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 x̂p0|0
satisfying

Dx̂p0|0 = d, (9.22)

and the singular initial error covariance

xxp xx
P0|0 = PN (D) P0|0 , (9.23)

where the projector PN (D) ∈ Rn×n is obtained by the singular value decomposition
� �� T �
T � � Ss V1
D = U1 U2 T , (9.24)
0(n−s)×s V2
where U2 ∈ Rn×(n−s) such that
T
PN (D) = U2 U2 . (9.25)
T
Also, note that, since (3.3) holds, Gwk−1 is constrained in PN (D) and GQk−1 G is 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 x̂k|k (2.10) onto the hyperplane defined by (3.6)
by minimizing the cost function
� � �T � �
J(xk ) = xk − x̂k|k W −1 xk − x̂k|k (9.26)

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

x̂pk|k = x̂k|k + Kkp (dk−1 − Dk−1 x̂k|k ), (9.27)

29
where

� T T
Kkp = W Dk−1 (Dk−1 W Dk−1 )−1 . (9.28)

xxp
The projected error covariance Pk|k associated with x̂pk|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.
xx
We set W = Pk|k xx
in (9.28), where Pk|k is given by (2.11), such that x̂pk|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 x̂pk|k (9.27) and the error covariance Pk|k
xxp

given by (4.19) back in forecast (2.4)-(2.5); see Figure 1b. Therefore, the PKF-EP forecast
estimate x̂k|k−1 (2.4) is different from its ECKF counterpart (4.2).

30
 References
Aguirre, L. A., Alves, G. B., and Corrêa, M. V. 2007. Steady-state performance
constraints for dynamical models based on RBF networks. Engineering Applications of
Artificial 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 by
means of constrained parameter estimation. IEE Proceedings in Control Theory and
Applications, 151(2), 174–179.
Aguirre, L. A., Teixeira, B. O. S., and Tôrres, L. A. B. 2005. Using data-driven
discrete-time models and the unscented Kalman filter to estimate unobserved variables
of 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 the
extended and unscented Kalman filters for discrete-time systems with nondifferentiable
dynamics. Proceedings of the 2007 American Control Conference. New York City, USA.
Chaves, M. and Sontag, E. D. 2002. State-estimators for chemical reaction networks of
Feinberg-Horn-Jackson zero deficiency type. European Journal of Control, 8, 343–359.
Chen, Y. H. and Chiang, C. T. 1993. Adaptive Beamforming using the Constrained
Kalman Filter. IEEE Transactions on Antennas and Propagation, 41(11), 1576–1580.
Chia, T. L., Chow, P., and Chizeck, H. J. 1991. Recursive parameter identification of
constrained 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 using
Recurrent Multilayer Perceptrons with Unscented Kalman Filter. Neural Processing
Letters, 22(1), 69–84.
Choukroun, D., Bar-Itzhack, I. Y., and Oshman, Y. 2006. Novel quaternion Kalman
filter. 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 Attitude
Estimation. 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 Doná, J. A. 2005. Constrained Control and
Estimation: 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 tracking
in 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 equality
constraints. 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 for
the nonlinear transformation of means and covariances in filters and estimators. IEEE
Transactions on Automatic Control, 45(3), 477–482.
Ko, S. and Bitmead, R. R. 2007. State estimation for linear systems with state equality
constraints. 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 Control
Conference. New York City, USA.
Lefebvre, T., Bruyninckx, H., and De Schutter, J. 2002. Comment on ”A new
method 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 for
nonlinear systems: A comparison of performance. International Journal of Control,
77(7), 639–653.
Maciejowski, J. M. 2002. Predictive Control with Constraints. (Essex, England: Pearson
Education 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 spectrometric
data. 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., and
Mendes, E. M. A. M. 2004. Multiobjective nonlinear system identification: a case
study with thyristor series capacitor (TCSC). International Journal of Systems Science,
35(9), 537–546.
Nikoukhah, R., Campbell, S. L., and Delebecque., F. 1999. Kalman Filtering
for 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 solve
systems 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 for
nonlinear discrete-time systems: Stability and moving horizon approximations. IEEE
Transactions on Automatic Control, 48(2), 246–258.
Reif, K., Günther, S., Yaz, E., and Unbehauen, R. 1999. Stochastic stability of
the 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 squares
estimation and control. Automatica, 38, 1113–1123.
Romanenko, A. and Castro, J. A. A. M. 2004. The unscented filter as an alternative
to the EKF for nonlinear state estimation: a simulation case study. Computers and
Chemical Engineering, 28, 347–355.
Rotea, M. and Lana, C. 2005. State Estimation with Probability Constraints. 44th IEEE
Conference 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 with
constrained 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. IEEE
Transactions 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 Dynamic
Systems. Ph.D. Thesis. Graduate Program in Electrical Engineering, Federal University
of Minas Gerais. Belo Horizonte – MG, Brazil.
Teixeira, B. O. S., Chandrasekar, J., Tôrres, L. A. B., Aguirre, L. A., and
Bernstein, 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., Tôrres, L. A. B., Aguirre, L. A., and
Bernstein, 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.,
Tôrres, L. A. B., Aguirre, L. A., & Bernstein, D. S. 2008b. Gain-constrained
Kalman filtering for linear and nonlinear systems. IEEE Transactions on Signal Pro-
cessing, to appear.
Teixeira, B. O. S., Ridley, A., Tôrres, 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., Tôrres, L. A. B., Aguirre, L. A., and Bernstein, D. S. 2008d.
Unscented filtering for interval-constrained nonlinear systems. Proceedings of the 47th
IEEE 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 of
Process Control, 16, 1075–1086.
van der Merwe, R., Wan, E. A., and Julier, S. J. 2004. Sigma-point Kalman filters
for 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. . . . . . . . . . . . . 35
2 Simulation of compartmental model. . . . . . . . . . . . . . . . . . . . . . . 36
3 Estimation of total mass for compartmental model. . . . . . . . . . . . . . . 37
4 Estimation error of quaternion norm using UKF, ECUKF, PUKF, MAUKF,
and CUKF. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5 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 by
system 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 associated
error covariance carry the information provided by the equality constraint. Note that, since
T
(3.3) holds such that GQk−1 G is a “constrained” covariance, Qk−1 need not to be modified
T T
in the PKF-SP implementation. Otherwise, we can replace GQk−1 G by PN (D) GQk−1 G in
(2.5), where PN (D) is given by (9.25). In MAKF, the equality constraint is enforced at the
data-assimilation step.

36
 2

2
1.5

x3,k
1.5
xk

1.5
0.5 1
0.4 1
0.6 0.5
0 0.8
0 200 400 600 800 1000 1200 1400 1600 1800 2000 1 0
x2,k x
k 1,k

(a) (b)

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

37
 3.05

Dxk
3

2.95
0 500 1000 1500 2000
k

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

38
 0
10

−5
log10(1 − ||ek||2)

10

−10
10 UKF
MAUKF
PUKF
ECUKF
CUKF
−15
10
0 20 40 60 80 100 120 140 160 180 200
kT (s)

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

39
 2
10

0
10

−2
10
log (E(0) − E )
k

−4
10
discretized model
UKF
−6
10 MAUKF
10

PUKF
−8 ECUKF
10 CUKF

−10
10

−12
10
0 1 2 3 4 5 6 7 8 9 10
kT (s)

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 —-), and
CUKF (thin —-) with σv = 0.25. For comparison, the thin dot-dashed line, which is above
the remaining lines, refers to the energy Ek calculated from data-free simulation of the
discretized model (8.19). ECUKF and MAUKF estimates almost coincide, while CUKF
estimates satisfy the equality constraint at machine precision at most times.

40
List of Tables
1 Comparative analysis for compartmental model. . . . . . . . . . . . . . . . . 41
2 Comparative analysis for attitude estimation. . . . . . . . . . . . . . . . . . 42
3 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 to
2000, for 100-run Monte Carlo simulation for compartmental system, concerning process
noise levels σw = 0, 0.1, 0.5, and 1.0, and KF, ECKF, MAKF, PKF-EP, and PKF-SP
algorithms.
σw KF ECKF MAKF PKF-EP PKF-SP
Percent RMS constraint error
0 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.21
0.1 6.26, 2.60, 7.34 6.25, 2.54, 4.19
0.5 9.01, 4.58, 13.2 9.01, 4.55, 6.75
1.0 9.35, 5.58, 19.7 9.35, 5.56, 8.07
MT (×10−4 )
0 0.0996 0.0012
0.1 1.0515 0.6352
0.5 2.8057 1.4722
1.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

−3
RMSEi for e0 , e1 , e2 , e3 , β1 , β2 , β3 (×10 )
1.331 1.398 1.334 1.398 1.332
1.366 1.434 1.365 1.433 1.365
1.320 1.377 1.320 1.376 1.318
1.319 1.374 1.315 1.374 1.313
0.124 0.130 0.124 0.130 0.124
0.119 0.125 0.119 0.125 0.119
0.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 to
4000, for 100-run Monte Carlo simulation for the discretized pendulum (8.19), concerning
different 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 the
true 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.000151
0.25 459.40 3.50 9.11 3.51 0.000050
25.68 0.14 1.09 0.14 0.000134
0.5 594.61 5.98 15.93 5.97 0.000040
29.80 0.32 3.16 0.31 0.000119
−2
RMSEi , i = 1, 2 (×10 )
0.1 2.95, 2.88 0.91, 1.92 1.15, 2.12 0.91, 1.92 1.20, 2.07
0.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.75
0.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.03
1.60, 3.55 0.62, 1.35 1.15, 2.54 0.60, 1.30 0.98, 2.14
MT (×10−4 )
0.1 26.79 8.09 9.08 8.09 26.89
2.18 0.37 1.09 0.37 0.37
0.25 61.66 20.67 26.63 20.66 61.54
8.31 0.94 4.18 0.94 0.94
0.5 139.94 42.13 66.86 42.11 138.60
23.17 2.10 11.73 2.10 2.10

44