Professional Documents
Culture Documents
Prediction Update
Bel ( xt −1 ) step Bel ( xt ) step Bel ( xt )
-
Univariate
p ( x) ~ N ( , 2 ) :
p ( x) =
1
e
−
1 ( x− )2
2 2 Multivariate
2
2
Kalman filter (Thrun Chp 3)
• A parametric Bayes Filter that
represents probability density
functions with Gaussians
• Bel(xt) is updated by exploiting the
properties of Gaussians with respect
to the mean, μ, and variance, σ2
• We will be considering discrete-time
Kalman Filters (KFs)
3
General 1-D Kalman Filter
Prediction step: p(xt | xt-1 , ut ) xt = axt-1 + but + εt
Update step: p(zt | xt) zt = cxt + δt
Belief: Bel(xt) = p(xt | z1:t , u1:t) ~ N ( t , t2 )
Bel ( xt ) ~ N ( , 2 )
t t
Update timestep
Bel(xt)
Bel ( xt )
4
Kalman Filter Example 1
5
6
Example: 1-D KF with ct = 1
t = kt zt + (1 − kt ) t Remember:
only for ct = 1
7
Example: 1-D KF with ct = 1
t = t + k t ( zt − t )
8
9
1-D KF
X ~ N ( , 2 )
Linear Y ~ N ( a + b , a )
2 2
Y = aX + b
Additive N ( , 2
)
d Y ~ N ( a + b, a 2 2 )
YaX + b
12
13
14
Kalman Filter algorithm in 1-D
15
Recall Kalman Filter Example 1
16
Example: 1-D KF with ct = 1
Start with an update step
N ( zt , qt2 )
bel ( xt ) ~ N ( t , t2 )
bel ( xt ) t = kt zt + (1 − kt ) t
bel ( xt ) =
t
2
= (1 − k t ) t
2
t2
with kt = 2
t + qt2
Here: ct = 1 17
Example: 1-D KF with ct = 1
Next timestep
bel ( xt )
bel ( xt −1 )
bel ( xt )
N ( zt , qt2 )
18
Multivariate 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
with a measurement
zt = Ct xt + t
State vector, xt, has n elements
Control vector, ut, has l elements
Measurement vector, zt, has k elements
19
Components of a Kalman Filter
21
Multivariate Gaussians
p (x) ~ Ν (μ,Σ) :
1
1 − ( x −μ ) t Σ −1 ( x −μ )
p ( x) = e 2
(2 )
1/ 2
1/ 2
Σ
x x x2 xy2
x = y , μ = y , Σ = yx2 y2
1 n
x = ( xi − x ) 2
2
n i =1
1 n
2
xy = ( xi − x )( yi − y ) = yx2
n i =1
Σ = ΣT [Covariance matrix] 22
Multivariate Kalman Filter algorithm
• Prediction (motion) step:
t = at t −1 + bt ut
bel ( xt ) = 2
t = a t
2 2
t −1 + rt
2
t = At t −1 + Bt ut
bel ( xt ) = [Compare to 1-D]
t = At t −1 t + Rt
AT
Really uncertain
det(Ct t CtT ) K t Ct → I t = zt
about prediction
det(Qt ) Trust only
measurement
Really uncertain
about det(Ct t CtT ) Kt → 0 t = t
measurement det(Qt ) Trust only
prediction
24
Multivariate Gaussian Systems:
Initialization
bel ( x0 ) = N (x0 ; 0 , 0 )
25
Kalman Filter Algorithm
1. Algorithm Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:
3. t = At t −1 + Bt ut
4. t = At t −1 AtT + Rt
5. Update:
6. K t = t CtT (Ct t CtT + Qt ) −1
7. t = t + K t ( z t − Ct t )
8. t = ( I − K t Ct ) t
9. Return t, t
26
General 1-D Kalman Filter
Prediction step: p(xt | xt-1 , ut ) xt = axt-1 + but + εt
Update step: p(zt | xt) zt = cxt + δt
Belief: Bel(xt) = p(xt | z1:t , u1:t) ~ N ( t , t2 )
Bel ( xt ) ~ N ( , 2 )
t t
Update timestep
Bel(xt)
Bel ( xt )
27
Kalman Filter Summary
28