Bayes Filter Implementations
Gaussian filters
• Kalman Filter
• Extended Kalman Filter
Bayes Filter Reminder
•Prediction
bel ( xt )   p( xt | ut , xt 1 ) bel ( xt 1 ) dxt 1
•Correction
bel ( xt )   p( zt | xt ) bel ( xt )
Gaussians
p( x) ~ N (  ,  2 ) :

1 ( x )
p( x) 

1
e 2
2 
2
2
Univariate
-

p(x) ~ Ν (μ,Σ) :
p ( x) 
1
(2 )
d /2
Σ
1/ 2
e
Multivariate
1
 ( x μ ) t Σ 1 ( x μ )
2

Characteristics of Gaussians
X ~ N (  , ) 
T

Y
~
N
(
A


B
,
A

A
)

Y  AX  B 
X 1 ~ N ( 1 ,  12 ) 
2
2

p
(
X

X
)
~
N



,




1
2
1
2
1
2  2  1 2 
2 
X 2 ~ N ( 2 ,  2 ) 
  22
X 1 ~ N ( 1 ,  12 ) 
 12
 12 22 
 p( X 1 )  p( X 2 ) ~ N  2
1  2
2 ,
2
2
2
2 
2 









X 2 ~ N ( 2 ,  2 ) 
 1
2
1
2
1
2 
• We stay in the “Gaussian world” as long as
we start with Gaussians and perform only
linear transformations.
Mixing of two guassians
Variance
reduction
5
Discrete Kalman Filter
Estimates the state x of a discrete-time
controlled process that is governed by the
linear stochastic difference equation
xt  At xt 1  Bt ut   t
Process
noise
with a measurement
zt = Ht xt + d t
Measurement
noise
6
Components of a Kalman Filter
At
Matrix (nxn) that describes how the state
evolves from t to t-1 without controls or
noise.
Bt
Matrix (nxl) that describes how the control ut
changes the state from t to t-1.
Ht
Matrix (kxn) that describes how to map the
state xt to an observation zt.
t
Random variables representing the process
and measurement noise that are assumed to
be independent and normally distributed
with covariance Rt and Qt respectively.
t
7
Kalman Filter
Control at t
ut
xt-1
Pt-1
System
state at t-1
Observation at t
zt = Ht xt + d t
xt  At xt 1  Bt ut   t
Kalman Filter
Pt = At Pt-1 AtT + Rt
xt
Pt
System
state at t
8
Kalman Filter Algorithm
1.
Algorithm Kalman_filter( xt-1, Pt-1, ut, zt):
Prediction:
x t = At xt-1 + Bt ut
Pt = A P A + Rt
T
t t-1 t
State prediction by transition
matrix and control
Predict variance matrix
Correction:
-1
K t = Pt H (Ht Pt H +Qt )
T
t
T
t
xt = x t + K t (zt - Ht x t )
Pt = (I - K t Ht )Pt
Kalman Gain
State update
Innovation provided by zt
Variance update
9
Derivation of KF: Initialization
• Initial belief is normally distributed:
(
bel(x0 ) = N x0 : m0 ,P0
Initial
state
)
Initial
variance
10
Derivation of KF: Prediction
• Dynamics are linear function of state and
control plus additive noise:
xt  At xt 1  Bt ut   t
p(xt |ut ,xt-1 )= N At xt-1 + Bt ut ,Rt
(
)
bel(xt ) = ò p(xt |ut ,xt-1 )
bel(xt-1 )dxt-1
(
ß
~ N At mt-1 + Bt ut ,Rt
)
(
ß
~ N mt-1 ,Pt-1
)
11
Linear Gaussian Systems: Dynamics
bel(xt ) = ò p(xt |ut ,xt-1 )
(
ß
~ N At mt-1 + Bt ut ,Rt
bel(xt -1 )dxt-1
)
ß
(
~ N mt-1 ,Pt-1
)
ß
ì 1
ü
T -1
bel(xt ) = h ò exp í- (xt - At xt-1 - Bt ut ) Rt (xt - At xt-1 - Bt ut )ý
î 2
þ
ì 1
ü
T -1
exp í- (xt-1 - mt-1 ) Pt-1(xt-1 - mt-1 )ý dxt-1
î 2
þ
ì m = A m +B u
ï t
t t-1
t t
bel(xt ) = í
T
P
=
A
P
A
+ Rt
t
ïî
t t-1 t
Convolution
of two
Gaussians
12
Linear Gaussian Systems: Observations
• Observations are linear function of state
plus additive noise:
zt = Ht xt + d t
(
p(zt | xt )= N Ht xt ,Qt
)
bel(xt ) = h p(zt | xt )
(
ß
~ N zt : Ht xt ,Qt
bel(xt )
)
ß
(
~ N xt : m t ,St
)
13
Linear Gaussian Systems: Observations
bel(xt ) = h p(zt | xt )
(
ß
~ N zt ;Ht xt ,Qt
bel(xt )
)
ß
(
~ N xt ; m t ,P t
ß
)
Multiplication
of two
Gaussians
ì 1
ü
ì 1
ü
T -1
T -1
bel(xt ) = h exp í- (zt - Ht xt ) Qt (zt - Ht xt )ý exp í- (xt - mt ) Pt (xt - mt )ý
î 2
þ
î 2
þ
ì m = m + K (z - H m )
ï t
t
t
t
t t
bel(xt ) = í
Pt = (I - K t Ht )P t
ïî
with K t = P t HtT (Ht P t HtT + Qt )-1
14
Kalman Filter Prediction
 t  At t 1  Bt ut
bel ( xt )  
T


A

A
t
t t 1 t  Rt

Variance
increasing
15
Kalman Filter Update
t  t  Kt ( zt  Ct t )
bel ( xt )  
 t  ( I  Kt Ct )t
with
Kt  t CtT (Ct t CtT  Qt ) 1
Variance
decreasing
16
Kalman Filter Iteration
17
The Prediction-Correction-Cycle
Prediction
 t  At t 1  Bt ut
bel ( xt )  
T
 t  At  t 1 At  Rt
18
The Prediction-Correction-Cycle
Correction

 t  t  K t ( zt  Ct t )
bel ( xt )  
,

 t  ( I  K t Ct ) t
K t   t CtT (Ct  t CtT  Qt ) 1
19
The Prediction-Correction-Cycle
Prediction

 t  t  K t ( zt  Ct t )
bel ( xt )  
,

 t  ( I  K t Ct ) t
K t   t CtT (Ct  t CtT  Qt ) 1
   At t 1  Bt ut
bel ( xt )   t
T
t  At  t 1 At  Rt
Correction
20
Example
Estimating Position of an Aircraft using
Kalman Filter
State
transition
model
Observation
model
aircraftPositionEstimateExampleApp of matlab
21
Implementation in Matlab
22
23
24
Kalman Filter Summary
• Highly efficient: Polynomial in
measurement dimensionality k and
state dimensionality n:
O(k2.376 + n2)
• Optimal for linear Gaussian
systems!
• Most realistic systems are nonlinear!
25
Nonlinear Dynamic Systems
• Most realistic problems involve
nonlinear functions
xt  g (ut , xt 1 )
zt  h( xt )
26
Linearity Assumption Revisited
Linear
transformation
maps a Gaussian
to another
Gaussian
27
Non-linear Function
Nonlinear
transformation
28
EKF Linearization (1)
Linearization by
Talyor
approximation
29
EKF Linearization (2)
When the
Guassian is
“flat”, i.e has
large variance
30
EKF Linearization (3)
When the
Guassian has
small variance
31
EKF Linearization: First Order
Taylor Series Expansion
• State transition:
Taylor
Linearization
g (ut , t 1 )
xt  g (ut , xt 1 )  g (ut , t 1 ) 
( xt 1  t 1 )
xt 1
xt  g (ut , xt 1 )  g (ut , t 1 )  Gt ( xt 1  t 1 )
• Measurement:
Jaccobi
Matrix
Taylor
Linearization
h( t )
h( xt )  h( t ) 
( xt  t )
xt
h( xt )  h( t )  H t ( xt  t )
Jaccobi
Matrix
32
Example of EKF
• State model
𝑟1,𝑡
Radar1
𝑟3,𝑡
𝑟2,𝑡
Radar3
Radar2
• Observation Model
𝑟1,𝑡 =
𝑥1 − 𝑥𝑡
2
+ 𝑦1 − 𝑦𝑡
2
+ 𝑛1
𝐻(𝑥𝑡 , 𝑦𝑡 ) = 𝑟2,𝑡 =
𝑥2 − 𝑥𝑡
2
+ 𝑦2 − 𝑦𝑡
2
+ 𝑛2
𝑟3,𝑡 =
𝑥3 − 𝑥𝑡
2
+ 𝑦3 − 𝑦𝑡
2
+ 𝑛3
33
Example of EKF
• State matrix
• Observation matrix
1,1,0,0 
0,1,0,0

A
0,0,1,1 


0,0,0,1

x1
y1
 r1,t r1,t r1,t r1,t  
,0,
,0 
,
,
,

 
2
2
2
2

( x1  xt )  ( y1  yt )
 xt xt yt yt   ( x1  xt )  ( y1  yt )

 r2,t r2,t r2,t r2,t  
x2
y2
H 
,
,
,
,0,
,0

( x2  xt )2  ( y2  yt )2 
 xt xt yt yt   ( x2  xt )2  ( y2  yt )2

 r r r r  

x3
y3
 3,t , 3,t , 3,t , 3,t  
,0,
,0 
2
2
 xt xt yt yt   ( x  x )2  ( y  y )2
( x3  xt )  ( y3  yt )

3
t
3
t

34
EKF Algorithm
1. Extended_Kalman_filter( t-1, Pt-1, ut, zt):
Prediction:
t  g (ut , t 1 )
 t  At t 1  Bt ut
P t  At Pt 1 AtT  Rt
P t  At Pt -1 AtT  Rt
Correction:
K t  P t H tT ( H t P t H tT  Qt ) 1
t  t  Kt ( zt  h(t ))
Pt  ( I  K t H t ) P t
Pt  ( I  K t H t ) P t
Return t, Pt
h( t )
Ht 
xt
At 
K t  P t H tT ( H t t H tT  Qt ) 1
t   t  K t ( zt  H t  t )
g (ut , t 1 )
xt 1
35
EKF Summary
• Highly efficient: Polynomial in
measurement dimensionality k and
state dimensionality n:
O(k2.376 + n2)
• Not optimal!
• Can diverge if nonlinearities are large!
• Works surprisingly well even when all
assumptions are violated!
36
Home Work
1. Design a EKF-based aircraft tracking simulation
program.
1. Investigate the impacts of the number of RADARs.
2. How many RADARs are needed for well tracking the
aircraft?
3. Investigate the impacts of the topology of the RADARs.
4. What is the proper RADAR network topology for aircraft
tracking?
37