16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 1 of 9
Lecture 25
Last time:
()
1
2
TT
KPHHPHR
?
??
=+
If
kk
zxv=+,
()
1
2
HI
KPPR
?
??
=
=+
Then if PR
?
,
1
2
KPR
??
→
Alternatively, if R P
?
,
2
KI→
The effect is that
()
?? ?x xKzHx
+? ?
=+ ?
The quantity
2
K is known as the Kalman gain. It is the optimum gain in the
mean squared error sense. Substitute it into the expression for P
+
.
()()
()()
()
() ()
() ()()
()
()
1
T
T
TT T
TT TT T
TT T T
TT T T T T
TT TT
P I KH P I KH KRK
I KHP I KHPHK KRK
IKHP PHK KHPHK KRK
IKHP PHK KHPH RK
I KH P P H K P H HP H R HP H R K
IKHP PHK PHK
IKHP
+?
??
?? ?
?? ?
?
?? ? ? ?
?? ?
?
=? ? +
=? ?? +
=? ? + +
=? ? + +
=? ? + + +
=? ? +
=?
The form at the top is true for any choice of K . The last form is true only for the
Kalman gain.
The first form is better behaved numerically if you process a measurement which
is very accurate relative to your prior information.
So the measurement update step is:
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 2 of 9
()
()
()
1
?? ?
TT
KPHHPH R
xxKzHx
PIKHP
?
??
+? ?
=+
=+ ?
=?
The filter operates by alternating time propagation and measurement update
steps.
The results were derived here on the basis of preserving zero mean errors and
minimizing the error variances. If all errors and noises are assumed normally
distributed, so the probability density functions can be manipulated, one can
derive the same results using the conditional mean approach: define x at every
stage to be the mean of the distribution of x conditioned on all the
measurements available up to that stage.
Example: Conversion of continuous dynamics to discrete time form
N
12
2
10
2
010 0
00 2
B
A
xx
xn
x xn
=
=
????
=+
????
????
We could do time propagation by integration ( )1N = :
??
TT
xAx
PAPPA BB
=
=+ +
()
2
2
1
...
2
At
eIAtAt
?
Φ= = + ?+ ? +
2
010010 00
0000 00
A
??????
==
??????
??????
If this does not work, expand Aφ φ=
, (0) Iφ = ( )2t? = :
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 3 of 9
[]
110
01
120
01
10
k
k
t
IAt
H
?
??
Φ=+?=
??
??
??
=
??
??
=
() ()
() ( )
()
2
0
2, 2,
2, 2
120101102
0101
T
T
k
QBNBd
IA
τττ
ττ
ττ
=Φ Φ
Φ=+?
????
??
==
????
????
∫
[]
2
2
0
40 20
40 20 2
2
1600 1600 400 80 40
80 40 4
3200
80
2
80 8
4
TT
TT
k
k
BNB
BNB d Q
R
τ
τ
τ ττ
τ
τ
?
??
ΦΦ= ?
??
??
?+ ?
=
?
??
??
ΦΦ= =
??
=
∫
The initial conditions are given to be 0, so
0
? (0)
0
00
(0)
00
x
P
?
?
??
=
??
??
??
=
??
??
Using the discrete formulation:
Time propagation:
1
1
??
kk
T
kk
xx
PPQ
?+
+
?+
+
=Φ
=Φ Φ +
Measurement update:
()
()
()
1
?? ?
TT
KPHHPH R
xxKzHx
PIKHP
?
??
+? ?
=+
=+ ?
=?
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 4 of 9
Continuous time filter
As noted in the beginning of this section, we rarely process measurements
continuously in a real system. However, there may be cases in which the
measurements are processed so rapidly that one can approximate the process as
integration of a continuous time differential equation.
Also, since integration of a single differential equation for the estimate and one
for the covariance matrix is logically simpler than alternating time propagation
and measurement update steps, we sometimes approximate the hybrid
continuous-discrete filter with a nearly equivalent continuous filter for analysis
purposes.
The relationship between continuous and discrete measurement processing can
be seen in the following. Suppose we had the continuous measurement
() () () ()zt Htxt vt=+
where ()vt is an unbiased white noise process. Define a nearly equivalent
discrete measurement to be the average of the continuous measurement over an
interval.
() ( ) () ( )
T
Evtv Rt tτ δτ??=?
??
[]
1
1
1
1
1
()
1
() () ()
11
()() ()
1
() ()
k
k
k
k
k
k
k
k
t
k
t
t
t
t
kk
t
t
kk
t
zztd
t
Htxt vt dt
t
Ht xt t vtdt
tt
Hxt vtdt
t
τ
?
?
?
?
=
?
=+
?
≈?+
??
=+
?
∫
∫
∫
∫
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 5 of 9
for small t? . The equivalent discrete measurement noise is
1
1
()
k
k
t
k
t
vvtd
t
?
=
?
∫
with 0
k
v = since () 0vt= , and its covariance is
()
11
11
1
12122
121212
112
2
1
()()
1
()
1
()
1
()
()
kk
kk
kk
kk
k
k
T
kkk
tt
T
tt
tt
tt
t
t
k
k
REvv
dt dt v t v t
t
dt dt R t t t
t
dt R t
t
Rt t
t
Rt
t
δ
??
??
?
??=
??
=
?
=?
?
=
?
≈?
?
=
?
∫∫
∫∫
∫
for small t? .
So when we approximate a discrete measurement by a continuous one, the
power density to assign to the continuous measurement noise is
()
k
R tRt=?
This makes sense because for larger t? (the discrete update process uses fewer
measurements) the intensity of the continuous measurement noise is larger (the
measurements are not as good).
The discrete measurement gain is
()
1
TT
kk
KPHHPHR
?
??
=+
Using the relation between
k
R and ()R t we have
1
()
TT
k
Rt
KPHHPH
t
?
??
??
=+
??
?
??
Note the difference in units:
Discrete:
()
??...
kk
x Kz Hx
+?
=?
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 6 of 9
Continuous: ()?? ?
k
x Ax Gu K z Hx=++ ?
Then the continuous Kalman gain is
()
0
1
0
1
0
1
1
1
( ) lim
1()
lim
lim ( )
k
t
TT
t
TT
t
T
T
Kt K
t
Rt
PH HPH
tt
PH tHPH Rt
PHR
PH R
?→
?
??
?→
?
??
?→
??
?
=
?
??
=+
??
??
??
=?+
=
=
The distinction between P
?
and P
+
disappears as we go to the limit of
continuous measurement processing.
The continuous-discrete update relations for ?x and P can be analyzed in a
similar manner, expanding everything to first order in t? and taking the limit as
0t?→ . This is done in sufficient detail in the text. The result is the continuous
time form of the Kalman filter.
()
1
?? ?
TTT
xAxGuKzHx
P AP PA BNB PH R HP
?
=++ ?
=+ + ?
where
1T
KPHR
?
= .
If this is an approximation to a filter that processes measurements at discrete
points in time, with measurement noise variance
k
R , take
()
k
R tRt=?
If the approximation is good, the discrete and continuous variances will be
related as
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 7 of 9
Example: Discrete time system (typical servo structure)
The input ()ut is any known command signal.
The noise ()nt is a wideband noise approximated as white with power
density N .
The measurements are of y and are processed at intervals t? with discrete
measurement variance
k
R .
Form the approximating continuous Kalman filter:
k
R Rt=?
()()
12
212
01 0 0
FR
FR F R
xx
xKuxKxn
x xu n
KK K K
Ax Gu Bn
=
=?? +
??????
=++
??????
?? ?
??????
=++
The continuous approximation measurement z is
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 8 of 9
[ ]
10zxv
Hx v
=+
=+
The filter is:
()
1
?? ?
TTT
xAxGuKzHx
PAPPA BNB PHRHP
?
=++ ?
=+ + ?
where
1T
KPHR
?
= .
Steady state properties
If the system is time invariant and the random processes are stationary
( ,, , ,ABHNR all constants), then the differential equation for P may approach a
constant steady state value as time increases.
Some characteristics of the steady state solution:
1) If (, )AH is detectable and (,)AB is stabilizable, the solution of the Ricatti
differential equation approaches a steady state value P
∞
which is
independent of the initial condition
0
P .
2) Under the conditions of (1), this P
∞
is the unique positive semidefinite
solution of the algebraic Ricatti equation
1
0
TTT
AP PA BNB PH R HP
?
++ ? =
3) The steady state filter
()
??x AKHxGuKz=? ++
is asymptotically stable if and only if ( ),AH is detectable at least and (),AB is
stabilizable at least with
1T
KPHR
?
∞
= .
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Page 9 of 9
4) Under the conditions of (3), the steady state filter minimizes
lim () ()
T
t
Eet Met
→∞
??
??
for every 0M > . This value is
[ ]
tr PM
∞
5) P
∞
is positive definite if and only if ( ),AB is complete controllable.
6) If (),A H is observable, steady state P is positive definite.
