Law

 of  Total  Probability,  Marginals  

Discrete  case   Con-nuous  case  

∑ P( x) = 1
x
∫ p( x) dx =1
P( x) = ∑ P( x, y ) p( x) = ∫ p( x, y) dy
y

P( x) = ∑ P( x | y ) P( y ) p( x) = ∫ p( x | y) p( y) dy
y

1  
 Bayes  Formula  

P ( x, y ) = P ( x | y ) P ( y ) = P ( y | x ) P ( x )
⇒
P( y | x) P( x) likelihood ⋅ prior
P( x y ) = =
P( y ) evidence
 Normaliza=on  
P( y | x) P( x)
P( x y ) = = η P( y | x) P( x)
P( y )
−1 1
η = P( y ) =
∑ P( y | x)P( x)
x

Algorithm:  
∀x : aux x| y = P( y | x) P( x)

1
η=
∑ aux x| y
x

∀x : P( x | y ) = η aux x| y
3  
 Observa=ons  
•  Given  by  noise  sensors.  
•  Represent  evidence  about  the  current  state.  
•  More  evidence  -­‐>  more  informa=on  -­‐>  less  
uncertainty  
Observa=ons  

0.2   0.2   0.2   0.2   0.2  

Observa=ons  

0.2   0.2   0.2   0.2   0.2  

?   ?   ?   ?   ?  
Observa=ons  

0.2   0.2   0.2   0.2   0.2  

?   ?   ?   ?   ?  

?   ?   ?   ?   ?  
Observa=ons  

0.2   0.2   0.2   0.2   0.2  

0.12   0.12   0.08   0.12   0.08  

0.23   0.23   0.15   0.23   0.15  

Observa=ons  

0.2   0.2   0.2   0.2   0.2  

0.23   0.23   0.15   0.23   0.15  

0.28   0.28   0.08   0.28   0.08  

?   ?   ?   ?   ?  
Observa=ons  

0.8   0.2   0   0   0  

?   ?   ?   ?   ?  

?   ?   ?   ?   ?  
 Ac=ons  
•  OJen  the  world  is  dynamic  since  
–  ac-ons  carried  out  by  the  robot,  
–  ac-ons  carried  out  by  other  agents,  
–  or  just  the  -me  passing  by  
 change  the  world.  
 
•  How  can  we  incorporate  such  ac-ons?  

11  
 Typical  Ac=ons  
•  The  robot  turns  its  wheels  to  move  
•  The  robot  uses  its  manipulator  to  grasp  an  object  
•  Plants  grow  over  -me…  

•  Ac=ons  are  never  carried  out  with  absolute  

certainty.  
•  In  contrast  to  measurements,  ac-ons  generally  
increase  the  uncertainty.    

12  
 Modeling  Ac=ons  
•  To  incorporate  the  outcome  of  an  ac=on  u  
into  the  current  “belief”,  we  use  the  
condi=onal  pdf    

P(x|u,x’)  
 
•  This  term  specifies  the  pdf  that  execu-ng  u  
changes  the  state  from  x’  to  x.  

13  
 Integra=ng  the  Outcome  of  Ac=ons  

Con=nuous  case:  
 
 
 
P( x | u ) = ∫ P( x | u , x' ) P( x' ) dx '
 
Discrete  case:  

P( x | u ) = ∑ P( x | u , x ' ) P( x ' )

14  
Ac=ons  

0.8   0.2   0   0   0  

?   ?   ?   ?   ?  
Ac=ons  

0.8   0.2   0   0   0  

0.16   0.68   0.16   0   0  

Ac=ons  

0.8   0.2   0   0   0  

0.16   0.68   0.16   0   0  

0.032   0.264   0.576   0.128   0  

 Bayes  Filters:  Framework  
•  Given:  
–  Stream  of  observa=ons  z  and  ac=on  data  u:  
dt = {u1 , z1 …, ut , zt }
–  Sensor  model  P(z|x).  
–  Ac=on  model  P(x|u,x’).  
–  Prior  probability  of  the  system  state  P(x).  
•  Wanted:    
–  Es=mate  of  the  state  X  of  a  dynamical  system.  
–  The  posterior  of  the  state  is  also  called  Belief:  
Bel( xt ) = P( xt | u1 , z1 …, ut , zt )
18  
 Markov  Assump=on  

p( zt | x0:t , z1:t , u1:t ) = p( zt | xt )

p( xt | x1:t −1 , z1:t , u1:t ) = p( xt | xt −1 , ut )
Underlying  Assump=ons  
•  Sta=c  world  
•  Independent  noise  
•  Perfect  model,  no  approxima=on  errors  
19  
 z    =  observa=on  
u    =  ac=on  
Bayes  Filters   x    =  state  

Bel( xt ) = P( xt | u1, z1 …, ut , zt )
Bayes = η P( zt | xt , u1, z1, …, ut ) P( xt | u1, z1, …, ut )
Markov = η P( zt | xt ) P( xt | u1, z1, …, ut )
Total prob. = η P( zt | xt ) ∫ P( xt | u1 , z1 , …, ut , xt −1 )
P( xt −1 | u1 , z1 , …, ut ) dxt −1
Markov = η P( zt | xt ) ∫ P( xt | ut , xt −1 ) P( xt −1 | u1, z1, …, ut ) dxt −1
Markov = η P( zt | xt ) ∫ P( xt | ut , xt −1 ) P( xt −1 | u1 , z1 , …, zt −1 ) dxt −1

= η P( zt | xt ) ∫ P( xt | ut , xt −1 ) Bel( xt −1 ) dxt −1
20  
 Bel( xt ) = η Bayes  
P( zt | xt ) ∫FPilter  
( xt | utA
, xlgorithm     t−1
t −1 ) Bel ( xt −1 ) dx

1.   Algorithm  Bayes_filter(  Bel(x),d  ):  

2.   η=0

3.   If  d  is  a  perceptual  data  item  z  then  
4.           For  all  x  do  
5.      Bel ' ( x) = P( z | x) Bel ( x)
6.      η = η + Bel ' ( x)
7.           For  all  x  do  
8.      Bel' ( x) = η −1Bel' ( x)
9.   Else  if  d  is  an  ac=on  data  item  u  then  
10.           For  all  x  do  
11.      Bel' ( x) = ∫ P( x | u, x' ) Bel( x' ) dx'
12.   Return  Bel’(x)              

21  
 Bel( xt ) = η Bayes  
P( zt | xt ) ∫FPilter  
( xt | utA
, xlgorithm     t−1
t −1 ) Bel ( xt −1 ) dx

1.   Algorithm  Bayes_filter(  Bel(x),d  ):  

2.   η=0

3.   If  d  is  a  observa=on  data  item  z  then  
4.           For  all  x  do  
5.      Bel ' ( x) = P( z | x) Bel ( x)
6.      η = η + Bel ' ( x)
7.           For  all  x  do  
8.      Bel' ( x) = η −1Bel' ( x)
9.   Else  if  d  is  an  ac=on  data  item  u  then  
10.           For  all  x  do  
11.      Bel' ( x) = ∫ P( x | u, x' ) Bel( x' ) dx'
12.   Return  Bel’(x)              

22  
 Bayes  Filter  

•  Predic=on  
bel( xt ) = ∫ p( xt | ut , xt −1 ) bel( xt −1 ) dxt −1

•  Correc=on  
bel( xt ) = η p( zt | xt ) bel( xt )
 Gaussians  
p( x) ~ N ( µ , σ 2 ) :
µ  
1 ( x−µ )2
1 −
2 σ2
p( x) = e
2π σ
Univariate   -­‐σ   σ  

p(x) ~ Ν (µ,Σ) :

1
1 − ( x −µ ) t Σ −1 ( x −µ ) µ  
p ( x) = d /2 1/ 2
e 2

(2π ) Σ

Mul=variate  
 Gaussians  
1D  

3D  

2D  

Video  
 Proper=es  of  Gaussians  
X ~ N ( µ , σ 2 )⎫ 2 2
⎬ ⇒ Y ~ N ( aµ + b, a σ )
Y = aX + b ⎭

2
X 1 ~ N ( µ1 , σ 1 ) ⎫ ⎛ σ 2 2 σ 1
2
1 ⎞
2 ⎬
⇒ p( X 1 ) ⋅ p( X 2 ) ~ N ⎜ 2
⎜ µ + 2
2 1 2
µ2 , −2
⎟
− 2 ⎟
X 2 ~ N ( µ2 , σ 2 )⎭ ⎝ σ 1 + σ 2 σ1 + σ 2 σ 1 + σ 2 ⎠
 Mul=variate  Gaussians  
X ~ N ( µ , Σ) ⎫ T
⎬ ⇒ Y ~ N ( Aµ + B , AΣA )
Y = AX + B ⎭

X 1 ~ N ( µ1 , Σ1 ) ⎫ ⎛ Σ 2 Σ1 1 ⎞
⎬ ⇒ p( X 1 ) ⋅ p( X 2 ) ~ N ⎜⎜ µ1 + µ2 , −1
⎟
−1 ⎟
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  transforma=ons.  
Observa=ons  
 Kalman  Filter  Updates  in  1D  
⎧µt = µt + K t ( zt − µt ) σ t2
bel ( xt ) = ⎨ 2 2
with K t = 2 2
⎩ tσ = (1 − K t )σ t σ t + σ obs ,t

⎧µt = µt + K t ( zt − Ct µt )
bel ( xt ) = ⎨ with Kt = Σt CtT (Ct Σt CtT + Qt ) −1
⎩ Σt = ( I − Kt Ct )Σt

29  
Ac=ons  
⎧ µt = at µt −1 + bt ut
bel ( xt ) = ⎨ 2 2 2 2
σ
⎩ t = a σ
t t + σ act ,t

⎧ µt = At µt −1 + Bt ut
bel ( xt ) = ⎨ T
⎩Σt = At Σt −1 At + Rt
Kalman  Filter  

31  
 Discrete  Kalman  Filter  
Es=mates  the  state  x  of  a  discrete-­‐=me  controlled  process  that  
is  governed  by  the  linear  stochas=c  difference  equa=on  

xt = At xt −1 + Bt ut + ε t

with  a  measurement    

zt = Ct xt + δ t

32  
 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.  

Ct Matrix  (kxn)  that  describes  how  to  map  the  state  xt  to  
an  observa=on  zt.  

εt Random  variables  represen=ng  the  process  and  

measurement  noise  that  are  assumed  to  be  
δt independent  and  normally  distributed  with  covariance  
Rt  and  Qt  respec=vely.  
33  
 Linear  Gaussian  Systems:  Ini=aliza=on  

•  Ini=al  belief  is  normally  distributed:  

bel( x0 ) = N (x0 ; µ0 , Σ0 )

34  
 Linear  Gaussian  Systems:  Dynamics  
•  Dynamics  are  linear  func=on  of  state  and  control  plus  
addi=ve  noise:  

xt = At xt −1 + Bt ut + ε t
p( xt | ut , xt −1 ) = N (xt ; At xt −1 + Bt ut , Rt )

bel ( xt ) = ∫ p( xt | ut , xt −1 ) bel ( xt −1 ) dxt −1

⇓ ⇓
~ N (xt ; At xt −1 + Bt ut , Rt ) ~ N (xt −1 ; µt −1 , Σt −1 )
35  
 Linear  Gaussian  Systems:  Dynamics  
bel ( xt ) = ∫ p ( xt | ut , xt −1 ) bel ( xt −1 ) dxt −1
⇓ ⇓
~ N (xt ; At xt −1 + Bt ut , Rt ) ~ N (xt −1 ; µt −1 , Σ t −1 )
⇓
⎧ 1 T −1 ⎫
bel ( xt ) = η ∫ exp⎨− ( xt − At xt −1 − Bt ut ) Rt ( xt − At xt −1 − Bt ut )⎬
⎩ 2 ⎭
⎧ 1 ⎫
exp⎨− ( xt −1 − µt −1 )T Σ t−−11 ( xt −1 − µt −1 )⎬ dxt −1
⎩ 2 ⎭
⎧ µt = At µt −1 + Bt ut
bel ( xt ) = ⎨ T
Σ
⎩ t = A t t −1 t + Rt
Σ A
36  
 Linear  Gaussian  Systems:  Observa=ons  

•  Observa=ons  are  linear  func=on  of  state  plus  addi=ve  

noise:  

zt = Ct xt + δ t
p( zt | xt ) = N (zt ; Ct xt , Qt )

bel ( xt ) = η p( zt | xt ) bel ( xt )
⇓ ⇓
~ N (zt ; Ct xt , Qt ) (
~ N xt ; µ t , Σ t )
37  
 Linear  Gaussian  Systems:  Observa=ons  

bel ( xt ) = η p ( zt | xt ) bel ( xt )
⇓ ⇓
~ N (zt ; Ct xt , Qt ) (
~ N xt ; µ t , Σ t )
⇓
⎧ 1 ⎫ ⎧ 1 ⎫
bel ( xt ) = η exp⎨− ( zt − Ct xt )T Qt−1 ( zt − Ct xt )⎬ exp⎨− ( xt − µt )T Σt−1 ( xt − µt )⎬
⎩ 2 ⎭ ⎩ 2 ⎭

⎧µt = µt + K t ( zt − Ct µt )
bel ( xt ) = ⎨ with K t = Σ t CtT (Ct Σ t CtT + Qt ) −1
⎩ Σ t = ( I − K t Ct )Σ t

38  
 Kalman  Filter  Algorithm    
1.   Algorithm  Kalman_filter(  µt-­‐1,  Σt-­‐1,  ut,  zt):  

2.   Predic=on:  
3.         µ      t = At µt −1 + Bt ut
4.       Σt = At Σt −1 AtT + Rt
 
5.   Correc=on:  
6.       K      t   = Σt CtT (Ct Σt CtT + Qt ) −1
7.      µt = µ t + Kt ( zt − Ct µ t )
8.      Σt = ( I − Kt Ct )Σt
9.   Return  µt,  Σt              

39  
The  Predic=on-­‐Correc=on-­‐Cycle  
Predic=on  

⎧ µt = at µt −1 + bt ut
bel ( xt ) = ⎨ 2 2 2 2
⎩σ t = at σ t + σ act ,t
⎧ µ = At µt −1 + Bt ut
bel ( xt ) = ⎨ t T
⎩Σ t = At Σ t −1 At + Rt

40  
 The  Predic=on-­‐Correc=on-­‐Cycle  

⎧µ = µ + K t ( zt − µt ) σ t2
bel ( xt ) = ⎨ t 2 t 2
, K t =
⎩ σ t = (1 − K t )σ t σ t2 + σ obs
2
,t

⎧µ = µt + K t ( zt − Ct µt )
bel ( xt ) = ⎨ t , K t = Σt CtT (Ct Σt CtT + Qt ) −1
⎩ Σ t = ( I − K t Ct ) Σ t

Correc=on  
41  
 The  Predic=on-­‐Correc=on-­‐Cycle  

Predic=on  

⎧µ = µ + K t ( zt − µt ) σ t2 ⎧ µt = at µt −1 + bt ut
bel ( xt ) = ⎨ t 2 t 2
, K t = bel ( xt ) = ⎨ 2
⎩ σ t = (1 − K t )σ t σ t2 + σ obs
2 2 2 2
,t
⎩σ t = at σ t + σ act ,t
⎧µ = µt + K t ( zt − Ct µt ) ⎧ µ = At µt −1 + Bt ut
bel ( xt ) = ⎨ t , K t = Σt CtT (Ct Σt CtT + Qt ) −1 bel ( xt ) = ⎨ t T
⎩ Σt = ( I − K t Ct )Σt ⎩Σ t = At Σ t −1 At + Rt

Correc=on  

42  
 Kalman  Filter  Summary  
•  Highly  efficient:  Polynomial  in  measurement  
dimensionality  k  and  state  dimensionality  n:    
                         O(k2.376  +  n2)    

•  Op=mal  for  linear  Gaussian  systems!  

•  Most  robo=cs  systems  are  nonlinear!  

43  
Sampling  
 Sensor Information: Importance Sampling
Bel( x) ← α p( z | x) Bel − ( x)
α p( z | x) Bel − ( x)
w ← = α p ( z | x)
Bel ( x)
−
 Robot Motion
Bel − ( x) ← ∫ p( x | u x' ) Bel( x' ) d x' ,
The image cannot be displayed. Your computer may not have enough memory to open the image, or the image may have been corrupted. Restart your computer, and then open the file again. If the red x still appears, you may have to delete the image and then
insert it again.
 Sensor Information: Importance Sampling
Bel( x) ← α p( z | x) Bel − ( x)
α p( z | x) Bel − ( x)
w ← = α p ( z | x)
Bel ( x)
−
 Robot Motion
Bel − ( x) ← ∫ p( x | u x' ) Bel( x' ) d x'
,
 Par=cle  Filter  Algorithm  

1.   Algorithm  par-cle_filter(  St-­‐1,  ut-­‐1  zt):  

2.      St = ∅, η =0
3.   For i = 1… n Generate new samples
4.  Sample index j(i) from the discrete distribution given by wt-1
5.  Sample xti
from p( xt | xt −1 , uusing
t −1 ) xand
j (i )
t −1
ut −1
6.  wti = p ( zt | xti ) Compute importance weight
7.  η = η + wti Update normalization factor
8.  S t = S t ∪ {< xti , wti >} Insert
9.  For i = 1… n
10.  wti = wti / η Normalize weights
 
 Particle Filter Algorithm
Bel ( xt ) = η p( zt | xt ) ∫ p( xt | xt −1, ut −1 ) Bel ( xt −1 ) dxt −1

draw xit-1 from Bel(xt-1)

draw xit from p(xt | xit-1,ut-1)

Importance factor for xit:

i target distribution
w =
t
proposal distribution
η p( zt | xt ) p( xt | xt −1 , ut −1 ) Bel ( xt −1 )
=
p( xt | xt −1 , ut −1 ) Bel ( xt −1 )
∝ p( zt | xt )
51  
52  
53  
54  
55  
56  
57  
58  
59  
60  
61  
62  
63  
64  
65  
66  
 Summary  
•  Par=cle  filters  are  an  implementa=on  of  recursive  
Bayesian  filtering  
•  They  represent  the  posterior  by  a  set  of  weighted  
samples.  
•  In  the  context  of  localiza=on,  the  par=cles  are  
propagated  according  to  the  mo=on  model.  
•  They  are  then  weighted  according  to  the  likelihood  
of  the  observa=ons.  
•  In  a  re-­‐sampling  step,  new  par=cles  are  drawn  with  a  
probability  propor=onal  to  the  likelihood  of  the  
observa=on.    

67