Download - Probabilistic Robotics
![Page 1: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/1.jpg)
Probabilistic Robotics
Bayes Filter Implementations
Gaussian filters
![Page 2: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/2.jpg)
•Prediction (Action)
•Correction (Measurement)
Bayes Filter Reminder
111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
![Page 3: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/3.jpg)
Gaussians
2
2)(
2
1
2
2
1)(
:),(~)(
x
exp
Nxp
-
Univariate
)()(2
1
2/12/
1
)2(
1)(
:)(~)(
μxΣμx
Σx
Σμx
t
ep
,Νp
d
Multivariate
![Page 4: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/4.jpg)
),(~),(~ 22
2
abaNYbaXY
NX
Properties of Gaussians
22
21
222
21
21
122
21
22
212222
2111 1
,~)()(),(~
),(~
NXpXpNX
NX
![Page 5: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/5.jpg)
• We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.
),(~),(~ TAABANY
BAXY
NX
Multivariate Gaussians
12
11
221
11
21
221
222
111 1,~)()(
),(~
),(~
NXpXpNX
NX
![Page 6: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/6.jpg)
6
Discrete Kalman Filter
tttttt uBxAx 1
tttt xCz
Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation
with a measurement
![Page 7: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/7.jpg)
7
Components of a Kalman Filter
t
Matrix (nxn) that describes how the state evolves from t-1 to t without controls or noise.
tA
Matrix (nxl) that describes how the control ut changes the state from t-1 to t.tB
Matrix (kxn) that describes how to map the state xt to an observation zt.tC
t
Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.
![Page 8: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/8.jpg)
8
Kalman Filter Updates in 1D
Measurement
Initial belief Measurement with associated uncertainty
Integrating measurement into belief via KF
![Page 9: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/9.jpg)
9
Kalman Filter Updates in 1D
1)(with )(
)()(
tTttt
Tttt
tttt
ttttttt QCCCK
CKI
CzKxbel
2,
2
2
22 with )1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
Kalman gain
![Page 10: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/10.jpg)
10
Kalman Filter Updates in 1D
tTtttt
tttttt RAA
uBAxbel
1
1)(
2
,2221)(
tactttt
tttttt a
ubaxbel
Motion to the right, == Introduces uncertainty
![Page 11: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/11.jpg)
11
Kalman Filter Updates
New measurement Resulting belief
Belief after motion to right
![Page 12: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/12.jpg)
12
0000 ,;)( xNxbel
Linear Gaussian Systems: Initialization
• Initial belief is normally distributed:
![Page 13: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/13.jpg)
13
• Dynamics are linear function of state and control plus additive noise:
tttttt uBxAx 1
Linear Gaussian Systems: Dynamics
ttttttttt RuBxAxNxuxp ,;),|( 11
1111
111
,;~,;~
)(),|()(
ttttttttt
tttttt
xNRuBxAxN
dxxbelxuxpxbel
State transition probability
![Page 14: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/14.jpg)
14
Linear Gaussian Systems: Dynamics
tTtttt
tttttt
ttttT
tt
ttttttT
tttttt
ttttttttt
tttttt
RAA
uBAxbel
dxxx
uBxAxRuBxAxxbel
xNRuBxAxN
dxxbelxuxpxbel
1
1
1111111
11
1
1111
111
)(
)()(2
1exp
)()(2
1exp)(
,;~,;~
)(),|()(
![Page 15: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/15.jpg)
15
• Observations are linear function of state plus additive noise:
tttt xCz
Linear Gaussian Systems: Observations
tttttt QxCzNxzp ,;)|(
ttttttt
tttt
xNQxCzN
xbelxzpxbel
,;~,;~
)()|()(
Measurement probability
![Page 16: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/16.jpg)
16
Linear Gaussian Systems: Observations
1
11
)(with )(
)()(
)()(2
1exp)()(
2
1exp)(
,;~,;~
)()|()(
tTttt
Tttt
tttt
ttttttt
tttT
ttttttT
tttt
ttttttt
tttt
QCCCKCKI
CzKxbel
xxxCzQxCzxbel
xNQxCzN
xbelxzpxbel
![Page 17: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/17.jpg)
17
Kalman Filter Algorithm
1. Algorithm Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return t, t
ttttt uBA 1
tTtttt RAA 1
1)( tTttt
Tttt QCCCK
)( tttttt CzK
tttt CKI )(
![Page 18: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/18.jpg)
18
The Prediction-Correction-Cycle
tTtttt
tttttt RAA
uBAxbel
1
1)(
2
,2221)(
tactttt
tttttt a
ubaxbel
Prediction
![Page 19: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/19.jpg)
19
The Prediction-Correction-Cycle
1)(,)(
)()(
tTttt
Tttt
tttt
ttttttt QCCCK
CKI
CzKxbel
2,
2
2
22 ,)1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
Correction
![Page 20: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/20.jpg)
20
The Prediction-Correction-Cycle
1)(,)(
)()(
tTttt
Tttt
tttt
ttttttt QCCCK
CKI
CzKxbel
2,
2
2
22 ,)1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
tTtttt
tttttt RAA
uBAxbel
1
1)(
2
,2221)(
tactttt
tttttt a
ubaxbel
Correction
Prediction
![Page 21: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/21.jpg)
21
Kalman Filter Summary
•Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)
•Optimal for linear Gaussian systems!
•Most robotics systems are nonlinear!
![Page 22: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/22.jpg)
22
Nonlinear Dynamic Systems
•Most realistic robotic problems involve nonlinear functions
),( 1 ttt xugx
)( tt xhz
![Page 23: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/23.jpg)
23
Linearity Assumption Revisited
![Page 24: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/24.jpg)
24
Non-linear Function
![Page 25: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/25.jpg)
25
EKF Linearization (1)
![Page 26: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/26.jpg)
26
EKF Linearization (2)
![Page 27: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/27.jpg)
27
EKF Linearization (3)
![Page 28: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/28.jpg)
28
•Prediction:
•Correction:
EKF Linearization: First Order Taylor Series Expansion
)(),(),(
)(),(
),(),(
1111
111
111
ttttttt
ttt
tttttt
xGugxug
xx
ugugxug
)()()(
)()(
)()(
ttttt
ttt
ttt
xHhxh
xx
hhxh
![Page 29: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/29.jpg)
29
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 )(
![Page 30: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/30.jpg)
30
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!
![Page 31: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/31.jpg)
31
Linearization via Unscented Transform
EKF UKF
![Page 32: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/32.jpg)
32
UKF Sigma-Point Estimate (2)
EKF UKF
![Page 33: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/33.jpg)
33
UKF Sigma-Point Estimate (3)
EKF UKF
![Page 34: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/34.jpg)
34
Unscented Transform
nin
wwn
nw
nw
ic
imi
i
cm
2,...,1for )(2
1 )(
)1( 2000
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 covarianceFor n-dimensional Gaussianλ is scaling parameter that determine how far the sigma points are spread from the meanIf the distribution is an exact Gaussian, β=2 is the optimal choice.
![Page 35: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/35.jpg)
35
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!
![Page 36: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/36.jpg)
36
Thank You
![Page 37: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/37.jpg)
37
Localization
• Given • Map of the environment.• Sequence of sensor measurements.
• Wanted• Estimate of the robot’s position.
• Problem classes• Position tracking (initial robot pose is known)• Global localization (initial robot pose is unknown)• 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]
![Page 38: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/38.jpg)
38
Markov Localization
Markov Localization: The straightforward application of Bayes filters to the localization problem
![Page 39: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/39.jpg)
39
1. EKF_localization ( t-1, t-1, ut, zt, m):
Prediction:
2.
3.
4.
5.
6.
),( 1 ttt ug T
tttTtttt 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
43
221
||||0
0||||
tt
ttt
v
vM
Motion noise covariance
Matrix from the control
Jacobian of g w.r.t location
Predicted mean
Predicted covariance
Jacobian of g w.r.t control
![Page 40: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/40.jpg)
40
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
![Page 41: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/41.jpg)
41
EKF Prediction Step
![Page 42: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/42.jpg)
42
EKF Observation Prediction Step
![Page 43: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/43.jpg)
43
EKF Correction Step
![Page 44: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/44.jpg)
44
Estimation Sequence (1)
![Page 45: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/45.jpg)
45
Estimation Sequence (2)
![Page 46: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/46.jpg)
46
Comparison to GroundTruth
![Page 47: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/47.jpg)
47
UKF_localization ( t-1, t-1, ut, zt, m):
Prediction:
2
43
221
||||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
![Page 48: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/48.jpg)
48
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
![Page 49: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/49.jpg)
49
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
![Page 50: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/50.jpg)
50
UKF Prediction Step
![Page 51: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/51.jpg)
51
UKF Observation Prediction Step
![Page 52: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/52.jpg)
52
UKF Correction Step
![Page 53: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/53.jpg)
53
EKF Correction Step
![Page 54: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/54.jpg)
54
Estimation Sequence
EKF PF UKF
![Page 55: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/55.jpg)
55
Estimation Sequence
EKF UKF
![Page 56: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/56.jpg)
56
Prediction Quality
EKF UKF
![Page 57: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/57.jpg)
57
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
Kalman Filter-based System
[Courtesy of Kai Arras]
![Page 58: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/58.jpg)
58
Multi-hypothesisTracking
![Page 59: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/59.jpg)
59
• 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
![Page 60: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/60.jpg)
60
• Hypotheses are extracted from LRF scans
• Each hypothesis has probability of being the correct one:
• Hypothesis probability is computed using Bayes’ rule
• Hypotheses with low probability are deleted.
• New candidates are extracted from LRF scans.
MHT: Implemented System (1)
)}(,,ˆ{ iiii HPxH
},{ jjj RzC
)(
)()|()|(
sP
HPHsPsHP ii
i
[Jensfelt et al. ’00]
![Page 61: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/61.jpg)
61
MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
![Page 62: Probabilistic Robotics](https://reader030.vdocuments.site/reader030/viewer/2022013101/568158c7550346895dc61025/html5/thumbnails/62.jpg)
62
MHT: Implemented System (3)Example run
Map and trajectory
# hypotheses
#hypotheses vs. time
P(Hbest)
Courtesy of P. Jensfelt and S. Kristensen