Post on 01-Feb-2016
description
Probabilistic Robotics
Robot Localization
2
Localization
• Given • Map of the environment.• Sequence of sensor measurements.
• Wanted• Estimate of the robot’s position.
• Problem classes• Position tracking• Global localization• Kidnapped robot problem (recovery)
“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]
3
Localization
4
Localization
Position tracking
5
Localization
Global localization
6
Landmark-based Localization
7
Linearity Assumption Revisited
8
Non-linear Function
9
EKF Linearization (1)
10
EKF Linearization (2)
11
EKF Linearization (3)
12
•Prediction:
•Correction:
EKF Linearization: First Order Taylor Series Expansion
)(),(),(
)(),(
),(),(
1111
111
111
ttttttt
ttt
tttttt
xGugxug
xx
ugugxug
)()()(
)()(
)()(
ttttt
ttt
ttt
xHhxh
xx
hhxh
13
EKF Algorithm
1. Extended_Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return t, t
),( 1 ttt ug
tTtttt RGG 1
1)( tTttt
Tttt QHHHK
))(( ttttt hzK
tttt HKI )(
1
1),(
t
ttt x
ugG
t
tt x
hH
)(
ttttt uBA 1
tTtttt RAA 1
1)( tTttt
Tttt QCCCK
)( tttttt CzK
tttt CKI )(
14
1. EKF_localization ( t-1, t-1, ut, zt, m):
Prediction:
2.
3.
4.
5.
6.
),( 1 ttt ug Tttt
Ttttt VMVGG 1
,1,1,1
,1,1,1
,1,1,1
1
1
'''
'''
'''
),(
tytxt
tytxt
tytxt
t
ttt
yyy
xxx
x
ugG
tt
tt
tt
t
ttt
v
y
v
y
x
v
x
u
ugV
''
''
''
),( 1
2
42
3
22
21
0
0
tt
ttt
v
vM
Motion noise
Jacobian of g w.r.t location
Predicted mean
Predicted covariance
Jacobian of g w.r.t control
15
1. EKF_localization ( t-1, t-1, ut, zt, m):
Correction:
2.
3.
4.
5.
6.
7.
8.
)ˆ( ttttt zzK
tttt HKI
,
,
,
,
,
,),(
t
t
t
t
yt
t
yt
t
xt
t
xt
t
t
tt
rrr
x
mhH
,,,
2,
2,
,2atanˆ
txtxyty
ytyxtxt
mm
mmz
tTtttt QHHS
1 tTttt SHK
2
2
0
0
r
rtQ
Predicted measurement mean
Pred. measurement covariance
Kalman gain
Updated mean
Updated covariance
Jacobian of h w.r.t location
16
EKF Prediction Step (known correspondences)
17
EKF Correction Step (known correspondences)
18
EKF Prediction Step (unknown correspondences)
19
EKF Correction Step (unknown correspondences)
20
EKF Prediction Step
21
EKF Observation Prediction Step
22
EKF Correction Step
23
Estimation Sequence (1)
24
Estimation Sequence (2)
25
Comparison to GroundTruth
26
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!
27
Linearization via Unscented Transform
EKF UKF
28
UKF Sigma-Point Estimate (2)
EKF UKF
29
UKF Sigma-Point Estimate (3)
EKF UKF
30
Unscented Transform
média a relação em espalhados estão points sigma os distantes
quão determinam que parâmetros a iguais sendok e com ,)(
2,...,1for )(2
1 )(
)1(
2
2000
nkn
nin
wwn
nw
nw
ic
imi
i
cm
Sigma points Weights
)( ii g
n
i
Tiiic
n
i
iim
w
w
2
0
2
0
))(('
'
Pass sigma points through nonlinear function
Recover mean and covariance
31
UKF_localization ( t-1, t-1, ut, zt, m):
Prediction:
2
42
3
22
21
0
0
tt
ttt
v
vM
2
2
0
0
r
rtQ
TTTt
at 000011
t
t
tat
Q
M
00
00
001
1
at
at
at
at
at
at 111111
xt
utt
xt ug 1,
L
i
T
txtit
xti
ict w
2
0,,
L
i
xti
imt w
2
0,
Motion noise
Measurement noise
Augmented state mean
Augmented covariance
Sigma points
Prediction of sigma points
Predicted mean
Predicted covariance
32
UKF_localization ( t-1, t-1, ut, zt, m):
Correction:
zt
xtt h
L
iti
imt wz
2
0,ˆ
Measurement sigma points
Predicted measurement mean
Pred. measurement covariance
Cross-covariance
Kalman gain
Updated mean
Updated covariance
Ttti
L
itti
ict zzwS ˆˆ ,
2
0,
Ttti
L
it
xti
ic
zxt zw ˆ,
2
0,
,
1, tzx
tt SK
)ˆ( ttttt zzK
Tttttt KSK
33
UKF Prediction Step
34
UKF Observation Prediction Step
35
UKF Correction Step
36
EKF Correction Step
37
Estimation Sequence
EKF PF UKF
38
Estimation Sequence
EKF UKF
39
Prediction Quality
EKF UKF
40
UKF Summary
•Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications
•Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term)
•Derivative-free: No Jacobians needed
•Still not optimal!
41
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
Kalman Filter-based System
[Courtesy of Kai Arras]
42
Map-based Localization
43
Monte Carlo (Particle Filter) Localization
44
Resampling Algorithm
45
Monte Carlo (Particle Filter) Localization
46
Monte Carlo (Particle Filter) Localization
1. Algorithm sample_normal_distribution(b):
2. return
1. Algorithm sample_triangular_distribution(b):
2. return
47
Monte Carlo (Particle Filter) Localization
48
Monte Carlo (Particle Filter) Localization
49
Monte Carlo (Particle Filter) Localization
50
Monte Carlo (Particle Filter) Localization
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
Monte Carlo (Particle Filter) Localization
67
Monte Carlo (Particle Filter) Localization
68
Multi-hypothesisTracking
69
• Belief is represented by multiple hypotheses
• Each hypothesis is tracked by a Kalman filter
• Additional problems:
• Data association: Which observation
corresponds to which hypothesis?
• Hypothesis management: When to add / delete
hypotheses?
• Huge body of literature on target tracking, motion
correspondence etc.
Localization With MHT
70
MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
71
MHT: Implemented System (3)Example run
Map and trajectory
# hypotheses
#hypotheses vs. time
P(Hbest)
Courtesy of P. Jensfelt and S. Kristensen