T&C LAB-AI
Robotics
Probabilistic Robotics
From KF to PF
양정연
2020/12/10
1
T&C LAB-AI
Robotics
Now, Localization with Kalman Filter
Where am I?( What is Xk=?)
• We Don’t Know Xk.
• We Only know Observation, Zk.
• But, Xk is noisy by Slip and so on.
• But, Zk is noisy by Matching error and sensor
performance.
• What we do all is using Kalman Filter.
2
Legend
Robot
Distance
observation
Xk
X
Zk
Uk
T&C LAB-AI
Robotics
Now, Localization with Kalman Filter
Where am I?( What is Xk=?)
• Current Estimate is
• Control Xdesired by Uk
• Zk changed then KF will estimate Xk by minimizing
P (covariance of state variable estimate)
3
Legend
Distance
observation
X
Zk
New
Xdesired
Uk
Initial
Estimates
1|
1
ˆ
k
k
x
T&C LAB-AI
Robotics
Remind: Kalman
Filter’s Simple Concept
4
X1
X2
X3
Z1
Z2
Z3
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
x
f x
w
z
h x v
Actual Model
1
ˆ
ˆ
(
)
ˆ
ˆ
( )
k
k
k
k
k
k
x
f x
z
h x
Estimation
We don’t know noise
,
k
k
w v
2
2
1
ˆ
ˆ
( )
x
f x
2
w
3
w
3
3
2
ˆ
ˆ
( )
x
f x
2
v
3
v
X2
X3
X1
X2
X3
Z1
Z2
Z3
2
w
3
w
2
v
3
v
X2
X3
T&C LAB-AI
Robotics
Probabilistic Notation for KF concept
• What we want to know is,
• Reversely, we want to estimate from Z to X, “ZX”
5
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
x
f x
w
z
h x v
Causal relationship from Eqs,
k
x
k
z
1
k
x
k
w
k
x
k
v
1:
1
2
{ ,
,...., }
k
k
k
Estimaing x from z
z z
z
Probabilistic
sense
1:
(
| z )
k
k
P x
T&C LAB-AI
Robotics
Concept of X estimation in Probability
6
X1
X2
Xk-1
Xk
Z1
Z2
Zk-1
Zk
x
x
T&C LAB-AI
Robotics
Basic of Probability
7
T&C LAB-AI
Robotics
Definition of Engineering Concept
Conditional Probability
• Prediction
• Filtering or Updating
• Smoothing in DSP
8
1
1:
(
|
)
k
k
P x
z
1:
(
|
)
k
k
P x
z
1
1:
(
|
)
k
k
P x
z
T&C LAB-AI
Robotics
Graph Expression in Probability
• Conditional Probability
9
A
B
A
B
B
(A, B)
(
)
( | B)
( )
( )
P
P A
B
P A
P B
P B
A given B
T&C LAB-AI
Robotics
Joint Probability
10
( )
( , )
( | ) ( )
( | ) ( )
b B
b B
B
P A
P A b
P A b P b
P A b P b db
( , )
( , | ) ( )
( , | ) ( )
c C
C
P A B
P A B c P c
P A B c P c dc
( ) 1
x X
P x
Sum of Probabilities =1
( , ) 1
y Y x X
P x y
Joint Probability P(x,y)
( , )
( )
y Y
P x y
P x
( , )
( )
x X
P x y
P y
P(x,y)
x
y
P(y)
P(x)
T&C LAB-AI
Robotics
Gaussian Distribution
11
2
2
2
2
1
( )
~
(0,1)
2
1
( )
1
2
z
z
p z
e
Z
N
p z dz
e
dz
Gaussian Distribution
Normal Distribution
2
2
2
2
2
1
1
2
2
2
1
2
2
1
2
2
,
1
1
1
( )
1
( )
2
2
2
1
( )
~
(0,
)
2
,
1
( )
~
( ,
)
2
x
x
z
x
x
x
z dx
dz
dx
p z dz
e
dz
e
e
dx
p x dx
p x
e
X
N
x
z
dx
dz
p x
e
X
N
Tip: Matlab
z=randn
x= sigma*randn
2
1
2
1
( | , )
( )
2
x
p x
e
p x
( ) : probabilistic density function
p x
T&C LAB-AI
Robotics
Strict Expression
• Probability Mass function, P (Upper case)
• Probability Density function, p(Lower case)
•
p(x,w) = p(x|w)P(w) = P(w|x)p(x)
•
p(x|w)P(w) p(x|w)P(w)
•
P(w|x) = ---------------- = -------------------------
•
p(x)
12
mass
density
mass
density
density
( )
( |
) ( )
W
P
p x
p
W
W
x
Tip:
p(x,y|a,b) or p(x,y) :
if x,y requires sum or integraton, then p(x,y|a,b) or p(x,y) is a density function
T&C LAB-AI
Robotics
Independence and Dependence
• A and B are Independent
• A and B are NOT Independent
• A and B are Conditionally Independent Given C
13
( , )
( ) ( )
P A B
P A P B
( , )
( | ) ( )
( | ) ( )
P A B
P A B P B
P B A P A
( , | )
( | ) ( | )
P A B C
P A C P B C
T&C LAB-AI
Robotics
Bayesian Networks:
Directed Acyclic Graph
14
A
B
C
Prob. C given A and B
Prob. C given A, B
( , , )
( | , )
( , )
( , , )
( | , ) ( , )
P A B C
P C A B
P A B
P A B C
P C A B P A B
( , , )
( | , ) ( ) ( )
P A B C
P C A B P A P B
A
B
C
( , , )
( , | ) ( )
( , | )
?
P A B C
P A B C P C
P A B C
However, in this case
Prob. A, B given C
T&C LAB-AI
Robotics
Conditional Independence
• If A is Independent of B given C, ( A B | C)
15
( , , )
( , , )
( , )
( , | )
( | , ) ( | )
( )
( , )
( )
P A B C
P A B C P B C
P A B C
P A B C P B C
P C
P B C
P C
A
B
C
Then,
( ,
| )
?
P A B C
A
B
C
?
( ,
| )
( | , ) ( | )
( | ) ( | )
P A B C
P A B C P B C
P A C P B C
Conditional Independence
( , | )
( | ) ( | )
P A B C
P A C P B C
( | , )
( | )
or P A B C
P A C
T&C LAB-AI
Robotics
Bayesian Networks
with Conditional Independence
16
B
A
D
E
C
G
F
( , , , , , , )
( ) ( ) ( ) (
| , , ) ( | , ) ( |
) ( |
, )
P A B C D E F G
P A P B P C P D A B C P E A C P F D P G D E
( , , , , , , )
( ,
| , , , , ) ( , , , , )
( ,
|
, ) ( ,
| , , ) ( , , )
( , , , )
( , , , , )
( , , )
( , )
( , , )
( | , , ) P(F, D, E)
(
| , , , ) P(E, A, B, C)
( , , )
( , )
( , , )
( |
, ) P(F, D, E
P A B C D E F G
P F G A B C D E P A B C D E
P F G D E P D E A B C P A B C
P F G D E P D E A B C
P A B C
P D E
P A B C
P G F D E
P D E A B C
P A B C
P D E
P A B C
P G D E
)
(
| , , ) P(E | A, B, C) P(A, B, C)
( , )
( |
, ) P(F | D, E) P(D, E)
(
| , , ) P(E | A, C) P(A, B, C)
( , )
( |
, ) P(F | D) (
| , , ) P(E | A, C) P(A) P(B) P(C)
P D A B C
P D E
P G D E
P D A B C
P D E
P G D E
P D A B C
T&C LAB-AI
Robotics
General Factorization of Bayesian Net
• Probability of Network is factorized by
• Remind that
• Why Factorization is useful?
– Use Logarithm
17
( , , , , , , )
( ) ( ) ( ) (
| , , ) ( | , ) ( |
) ( |
, )
P A B C D E F G
P A P B P C P D A B C P E A C P F D P G D E
1
( )
(
|
)
K
k
k
k
P x
P x a
( , , , , , , )
( ) Log ( ) Log ( ) Log (
| , , )
Log ( | , ) Log ( |
) Log ( |
, )
LogP A B C D E F G
LogP A
P B
P C
P D A B C
P E A C
P F D
P G D E
T&C LAB-AI
Robotics
Markov Chain
18
A
B
C
( , , )
( | ) ( | ) ( )
( | , )
( | )
( , )
( | ) ( )
P C A B
P C B P B A P A
P C A B
P C B
P A B
P B A P A
X1
X2
X3
….
Xk-1
Xk
1
2
1
1:
1
1
1:
1
1
(
| ,
,..,
)
(
|
)
(
|
)
(
|
)
(
|
) : Markov Process
k
k
k
k
k
k
k
k
k
k
P x
x x
x
P x
x
P x
x
P x
x
P x
x
History
Current
state
Previous
state
Current
state
T&C LAB-AI
Robotics
Probability in Markov Process
1. Markov Chain Transitional Probability
2. Joint Probability
19
1
2
1
1:
1
1
1:
1
1
(
|
,
,..,
)
(
|
)
(
|
)
(
|
)
(
|
) : Markov Process
k
k
k
k
k
k
k
k
k
k
P x
x x
x
P x
x
P x
x
P x
x
P x
x
( , )
( | ) ( | ) ( )
( | ) ( | )
c C
C
P A B
P A C P B C P C
P A C P B C dc
1
1
1
1
1
( )
( ,
)
(
|
) (
)
k
k
k
k
k
k
k
k
x
P x
P x x
P x
x
P x
dx
Remind
T&C LAB-AI
Robotics
2. K.F. with Probabilistic Approaches
20
T&C LAB-AI
Robotics
K.F. Two steps in Probability
• Our goal is,
– From the history of Observation, Z1:k, we want to estimate Xk
• Prediction
• Update (or Filter)
21
1:
(
|
)
k
k
P x
z
1:
1
(
|
)
k
k
P x
z
1:
(
|
)
k
k
P x
z
T&C LAB-AI
Robotics
Prediction
22
1:
1
(
|
)
k
k
P x
z
1
1:
1
1
1:
1
1
(
|
)
( ,
|
)
k
k
k
k
k
k
k
x
P x
z
P x x
z
dx
Remind Joint probability Definition
1
1
1
1
1
1:
1
1:
1
1
1:
1
1
1
1:
1
1
1:
1
1
1:
1
1
1
1:
1
1:
1
1
1:
1
1
1:
1
1
( ,
,
)
(
|
)
( ,
|
)
(
)
( ,
,
)
(
,
)
(
,
)
(
)
(
|
,
) (
|
)
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
x
x
k
k
k
k
k
k
k
k
k
x
k
k
k
k
k
k
x
P x x
z
P x
z
P x x
z
dx
dx
P z
P x x
z
P x
z
dx
P x
z
P z
P x
x
z
P x
z
dx
Xk-1
Xk
Zk-1
Zk
Causal
Update
1
1:
1
1
1
1:
1
1
(
|
)
(
|
) (
|
)
k
k
k
k
k
k
k
k
x
P x
z
P x
x
P x
z
dx
Tip:(Zk-1 Xk-1 Xk) = (Xk-1 Xk)
T&C LAB-AI
Robotics
Update(Filter)
23
1:
(
|
)
k
k
P x
z
1:
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
1:
1
(
|
)
(
|
,
)
( ,
,
)
( ,
,
)
( ,
)
( ,
)
( ,
)
( ,
)
( ,
) / (
)
(
|
,
)
( ,
) / (
)
(
|
)
(
|
,
)
(
|
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
P x
z
P x
z z
P x z z
P z x z
P x z
P z z
P x z
P z z
P x z
P z
P z
x z
P z z
P z
P x
z
P z
x z
P z
z
1:
1
1:
1
1:
1
)
(
|
,
) (
|
)
k
k
k
k
k
k
P z
x z
P x
z
Xk-1
Xk
Zk-1
Zk
Causal
Update
1:
1:
1
(
|
)
(
|
) (
|
)
k
k
k
k
k
k
P x
z
P z
x P x
z
T&C LAB-AI
Robotics
Prediction and Update with Probability
• Prediction
• Update
• Easily, with Belief function
• What is the PROBLEM?
24
1
1:
1
1
1
1:
1
1
(
|
)
(
|
) (
|
)
k
k
k
k
k
k
k
k
x
P x
z
P x
x
P x
z
dx
1:
1:
1
(
|
)
(
|
) (
|
)
k
k
k
k
k
k
P x
z
P z
x P x
z
1:
Bel( )
(
|
)
k
k
k
x
P x
z
Prediction k=0
Bel’(x0) = P(x0)
Update k=0
Bel(x0) = P(x0|z0)
0
0
0
0
0
0
0
(
|
) (
|
)
(
|
) ( )
P z
x P x
z
P z
x P x
Prediction k=1
Bel’(x1) = P(x1|z0)
1
0
0
0
0
(
|
) (
|
)
P x x P x
z dx
1:k
z
1:k
z
Update k=1
Bel(x1) = P(x1|Z0:1)
1
1
1
1
0:1
( |
) ( |
)
P z x P x z
How to Solve Integration?
T&C LAB-AI
Robotics
3. Particle Filter
Estimation instead of Solving Integration
25
T&C LAB-AI
Robotics
Monte Carlo Method
• Monte Carlo Casino
– Named by Stanislaw Ulam,
– Developed by John Von Neumann
in Eniac.
• We Already know it.
26
Probability of Blue = P(Blue) = ¼
Prob. Can be estimated by Area calculation
Prob. Can be estimated by Particles
T&C LAB-AI
Robotics
Probability Can be Estimated
by Monte Carlo Method
• Probability of 1 from Dice Throwing
– P(C1)
= 1/6
– Test5.m
• After over 5000 trials, P(c1) converges 0.167.
• Prob. Estimation requires over than 5000~10,000 trials
27
0
500
1000
1500
2000
2500
3000
3500
4000
4500
5000
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Trial
P
ro
b
(c
1
)
T&C LAB-AI
Robotics
Probabilistic Approach for State Estimation
• Remind that our goal is,
– From the history of observation, Z1:k, We want Xk estimation!
• Probability is estimated by many Trials as in Monte
Carlo.
• However, we want more and more Trials.
How to reduce the number of trials?
Particle Filter with Importance Sampling
28
1:
(
|
)
k
k
P x
z
T&C LAB-AI
Robotics
X with State Model
Z with Observation Model
• X is a State from System Dynamics.
• Z is the measured value( from sensor)
29
X=0
Z=10
Z=7
We know,
X=3
U=3
T&C LAB-AI
Robotics
X with State Model
Z with Observation Model
30
X1
X2
Z1
Z2
1
(
)
( )
k
k
k
k
k
k
x
f x
z
h x
State Model
X
K-1
Xk
Zk
Z measured by Sensor
We can know X by z=h(x)
1
(
)
k
k
k
x
f x
( )
k
k
k
z
h x
If there are noises, How the system changes?
T&C LAB-AI
Robotics
In a NOISY World,
We
DON’T Know X, but DO Know Z
• In most cases, We DO NOT Know X
• We estimate X by measurement Z
• Also, there are noise. We model only Noise Variance.
31
X=0
Z=10
Z=7
1. We cannot believe Z=7 because there is NOISE
2. U=3 in a rainy day causes sliding.
3. So, how to estimate X=?
U=3
x
Noise
T&C LAB-AI
Robotics
We DON’T Know X,
but DO Know Z
32
X1
X2
Z1
Z2
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
f
z
h
x
x
w
x v
Actual State Model
1
ˆ
ˆ
(
)
ˆ
ˆ
( )
k
k
k
k
k
k
x
f x
z
h x
Estimation
We don’t know noise
,
k
k
w v
2
w
1
v
2
v
Xk-1
Xk
k
w
Zk
k
v
X =?
Z measured by Sensor
Red : Unknowns
Black: Knowns
T&C LAB-AI
Robotics
We DON’T Know X,
but DO Know Z
33
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
f
z
h
x
x
w
x v
Actual State Model
1
ˆ
ˆ
(
)
ˆ
ˆ
( )
k
k
k
k
k
k
x
f x
z
h x
Estimation
We don’t know noise
,
k
k
w v
Red : Unknowns
Black: Knowns
1
(
,
)
k
m
m
k
k
k
x
f x
w
1
(
,
)
k
m
m
k
k
k
x
f x
w
1
(
,
)
k
k
k
k
f
x
x
w
Xk is a distribution.
Our model Wk does NOT tell where Xk is.
k
x
is a particle with Random Noise Wk.
k
m
x
( ,
)
k
k
f
w
Wk for a particle Generate Distribution.
T&C LAB-AI
Robotics
Step 1. PF State Transition
34
X1
X2
Z1
Z2
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
f
z
h
x
x
w
x v
Actual State Model
1
ˆ
ˆ
(
)
ˆ
ˆ
( )
k
k
k
k
k
k
x
f x
z
h x
Estimation
We don’t know noise
,
k
k
w v
2
w
1
v
2
v
X1
Xk
k
w
Zk
k
v
Red : Unknowns
Black: Knowns
Particle
(
)
k
m
m
k
k
z
h x
1
(
,
)
k
m
m
k
k
k
x
f x
w
1
(
,
)
k
m
m
k
k
k
x
f x
w
T&C LAB-AI
Robotics
Step 2. PF Sampling Weight
35
X1
X2
Z1
Z2
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
f
z
h
x
x
w
x v
Actual State Model
1
ˆ
ˆ
(
)
ˆ
ˆ
( )
k
k
k
k
k
k
x
f x
z
h x
Estimation
We don’t know noise
,
k
k
w v
2
w
1
v
2
v
Xk-1
Xk
k
w
Zk
k
v
Red : Unknowns
Black: Knowns
Particle
(
)
k
m
m
k
k
z
h x
wm = P(Z|Xm)
2
2
( (Xm) Z)
1
( | X )
2
h
m
m
w
p Z
e
1
(
,
)
k
m
m
k
k
k
x
f x
w
1
(
,
)
k
m
m
k
k
k
x
f x
w
T&C LAB-AI
Robotics
Step 3. PF Resampling
36
X1
X2
Z1
Z2
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
f
z
h
x
x
w
x v
Actual State Model
1
ˆ
ˆ
(
)
ˆ
ˆ
( )
k
k
k
k
k
k
x
f x
z
h x
Estimation
We don’t know noise
,
k
k
w v
2
w
1
v
2
v
Xk
k
w
Zk
k
v
Red : Unknowns
Black: Knowns
Particle
(
)
k
m
m
k
k
z
h x
wm = P(Z|Xm)
2
2
( (Xm) Z)
1
( | X )
2
h
m
m
w
p Z
e
Threshold
For
resampling
Xk-1
1
(
,
)
k
m
m
k
k
k
x
f x
w
1
(
,
)
k
m
m
k
k
k
x
f x
w
T&C LAB-AI
Robotics
Step 3. PF Resampling
37
X1
X2
Z1
Z2
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
f
z
h
x
x
w
x v
Actual State Model
1
ˆ
ˆ
(
)
ˆ
ˆ
( )
k
k
k
k
k
k
x
f x
z
h x
Estimation
We don’t know noise
,
k
k
w v
2
w
1
v
2
v
Xk
k
w
Zk
k
v
Red : Unknowns
Black: Knowns
Particle
(
)
k
m
m
k
k
z
h x
Xk-1
1
ˆ
k
x
1
(
,
)
k
m
m
k
k
k
x
f x
w
1
(
,
)
k
m
m
k
k
k
x
f x
w
Wk model
T&C LAB-AI
Robotics
Resampling Question???
38
X1
X2
Z1
Z2
2
w
1
v
2
v
Xk
k
w
Zk
k
v
Particle
(
)
k
m
m
k
k
z
h x
Xk-1
• After all, are same?
– In most cases, they are NOT same. Why?
k
m
x
1
(
,
)
k
m
m
k
k
k
x
f x
w
T&C LAB-AI
Robotics
Resampling : A Magical Way
39
• Resampling is the Essential Point of Particle Filter
– After system dynamics, the variance of particles distribution
changes Particles variance is not constant.
1
m
k
x
Possibly, Particles can be Scattered
m
k
x
System Noise wk possibly enables Particles to be scattered.
Resampling
1
(
,
)
k
m
m
k
k
k
x
f x
w
1
(
,
)
k
m
m
k
k
k
x
f x
w
1
1
1
(
,
)
m
m
k
k
k
k
x
f
x w
1
k
w
T&C LAB-AI
Robotics
PF Summary
Random Particle Generation + Resampling
40
1
(
,
)
( ,
)
k
k
k
k
k
k
k
k
f
z
h
x
x
w
x v
Actual State Model
Particle Filter
1
(
,
)
k
m
m
k
k
k
x
f x
w
Randomly
Particle
Distribution
(Through
System State
Transition)
2
2
( (Xm) Z)
2
1
Importance Sampling Weight=
( | X )
2
h
m
m
w
p Z
e
(
)
m
m
k
k
k
z
h x
k
z
(
|
)
(
|
)
m
m
m
k
k
k
k
k
w
p z x
p z z
high
m
k
Find
w
(Resampling)
m
k
Good x
Then, we should understand the theoretical features of Importance Sampling Weight
Next step
Observation
Model
ˆ
(
)
k
m
k
x
E x
T&C LAB-AI
Robotics
PF Example (TestPF.m)
• System dynamics
41
20
sin(
)
x
x
px
w
( , )
(0, 10)
w
N
N
0
20
40
60
80
100
120
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
0
20
40
60
80
100
120
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
xk : blue graph
xk : blue graph
Q: why these differ?
A: w is a noise
T&C LAB-AI
Robotics
PF Example (TestPF.m)
• System dynamics and Measurement
42
20
sin(
)
x
x
px
w
z
x v
2
( , )
(0, 10 )
w
N
N
( , )
(0,1)
v
N
N
0
20
40
60
80
100
120
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
0
10
20
30
40
50
60
70
80
90
100
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
x:blue z:red
xk : blue graph
zk : red graph
T&C LAB-AI
Robotics
Simulation for TestPF.m
• Discretization
• W,V simulation( Normal distribution=“randn” in Matlab)
43
1
1
20
sin(
)
20
sin(
)
k
k
k
k
k
k
k
k
k
k
k
k
k
x
x
x
x
px
w
t
x
x
tx
t
px
tw
z
x
v
2
2
1
( )
~
(0,1)
2
z
p z
e
Z
N
randn
2
~
(0,
)
x
z
X
N
randn
k
k
w
W randn
v
V randn
T&C LAB-AI
Robotics
Simulation Code for TestPF.m
44
1
(1 20
)
sin(
)
k
k
k
k
k
k
k
x
t x
t
px
tw
z
x
v
0
20
40
60
80
100
120
-3
-2
-1
0
1
2
3
x:blue z:red
T&C LAB-AI
Robotics
0. Particle Generation at an Initial State, x0
45
-4
-2
0
2
4
6
8
10
12
14
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
x:blue z:red
x0
Particle
~
(0, )
m
X
N
randn
T&C LAB-AI
Robotics
46
0. Particle Generation at an Initial State, x0
(testPF2.m)
x: actual state
X: particle
X generation with sigma=0.01
x0
x1
x2
Particle X in Matlab Xm in Eq.
1
(1 20
)
sin(
)
k
k
k
k
k
k
k
x
t x
t
px
tw
z
x
v
T&C LAB-AI
Robotics
47
1. Particle Virtually Moves by System Dynamics
1
(1 20
)
sin(
)
m
m
m
m
k
k
k
k
m
m
k
k
x
t x
t
px
tw
z
x
x0
x1
x2
Particle X in Matlab Xm in Eq.
x
X
wp: particle has noisy system dynamics
vp=0: Particle state is a virtual one.
We measure it WITHOUT noise.
T&C LAB-AI
Robotics
2. Weight Calculation
• P(z|xm)=P(z|zm)
48
zk
zk
Particle is WELL
distributed
around zk.
Particle is poorly
distributed
around zk.
Good:
High w
Bad:
Low w
2
2
( m Z)
2
Importance Sampling Weight=
1
( | X )
2
Z
m
m
w
p Z
e
T&C LAB-AI
Robotics
3. Resampling
• Choose Good Particle( with high weight)
49
zk
zk
Particle is WELL
distributed
around zk.
Particle is poorly
distributed around zk.
Good:
High w
Bad:
Low w
No need for Resampling
zk
Particle is well
Resampled around zk.
1. Resampling with Constant Number, N
2. Choose High Weight.
3. Most Particles converges into One
particle, probably.
4. Goal : every has same or similar weight.
T&C LAB-AI
Robotics
3. Resampling in Matlab
• Randomly choose
higher Weight
50
xm1
xm2
xmN
Cumulative
sum
xm1
xm2
xm
Weight,
w
T&C LAB-AI
Robotics
Result: Without Resampling
51
0
10
20
30
40
50
60
70
80
90
100
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
# of Particle = 10
Most particles are
far away
from an actual
value
T&C LAB-AI
Robotics
Remind the Result of Measurement
52
0
20
40
60
80
100
120
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
0
10
20
30
40
50
60
70
80
90
100
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
x:blue z:red
Much Noisy..
T&C LAB-AI
Robotics
TestPF2 with N=10
53
0
10
20
30
40
50
60
70
80
90
100
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0
10
20
30
40
50
60
70
80
90
100
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0
10
20
30
40
50
60
70
80
90
100
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
Blue: Actual
Red: Estimation
T&C LAB-AI
Robotics
TestPF2 with N=50
54
Blue: Actual
Red: Estimation
T&C LAB-AI
Robotics
TestPF2 with N=200
55
Blue: Actual
Red: Estimation
T&C LAB-AI
Robotics
Question:
When we Choose the Best Weight,
How PF works?
56
zk
Good:
High w
Bad:
Low w
zk
Best W
TestPF3.m
Q: Why the results show Poor
Performance?
Best Weight can be biased.
Best Weight May be NOT
the best Distribution.
T&C LAB-AI
Robotics
Extra Concept:
Weight Measurement of Importance
Sampling
• Variance of Weight
57
zk
zk
Good:
High w
Bad:
Low w
Good
Not Good
Weight(based Resampling) is needed
W=[0.1, 0.3, 0.01 ]
W=[0.1, 0.1, 0.1 ]
2
1
(Neff<N ) Resampling
otherwise, No resampling
eff
m
m
th
N
w
if
TestPF4.m
1
2
3
4
5
6
7
8
9
10
0.075
0.08
0.085
0.09
0.095
0.1
0.105
0.11
0.115
0.12
Particle
wm
Weight Changes after PF
Weight
variance
becomes
small.
T&C LAB-AI
Robotics
Why PF becomes More Dominant than KF
• KF requires O(N2) operation ( 10 Legends 22x22 matrix)
• Particle Filter O(MLogN) operation
– Most SLAM methods are based on PF concept.
– All cleaning robot uses PF-based SLAM.
• Low Dimension : KF>PF
• Higher Dimension : PF>>>KF
58
