1 mobile robot localization (ch. 7) mobile robot localization is the problem of determining the pose...
TRANSCRIPT
![Page 1: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/1.jpg)
1
Mobile Robot Localization (ch. 7)
• Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment. Because,
• Unfortunately, the pose of a robot can not be sensed directly, at least for now. The pose has to be inferred from data.
• A single sensor measurement is enough?
• The importance of localization in robotics.
• Mobile robot localization can be seen as a problem of coordinate transformation. One point of view.
![Page 2: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/2.jpg)
2
Mobile Robot Localization
•Localization techniques have been developed for a broad set of map representations. • Feature based maps, location based
maps, occupancy grid maps, etc. (what exactly are they?) (See figure 7.2)
• (You can probably guess What is the mapping problem?)
•Remember, in localization problem, the map is given, known, available.
• Is it hard? Not really, because,
![Page 3: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/3.jpg)
3
Mobile Robot Localization
•Most localization algorithms are variants of Bayes filter algorithm.
•However, different representation of maps, sensor models, motion model, etc lead to different variant.
•Here is the agenda.
![Page 4: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/4.jpg)
4
Mobile Robot Localization
• We want to know different kinds of maps.
• We want to know different kinds of localization problems.
• We want to know how to solve localization problems, during which process, we also want to know how to get sensor model, motion model, etc.
![Page 5: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/5.jpg)
5
Mobile Robot Localization(We want to know different kinds of maps. )
Different kinds of maps.
At a glance, ….
feature-based, location-based, metric, topological map, occupancy grid map, etc.
see figure 7.2• http://www.cs.cmu.edu/afs/cs/project/jair/pub/volume11/fox99a-
html/node23.html
![Page 6: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/6.jpg)
6
Mobile Robot Localization – A Taxonomy(We want to know different kinds of localization problems.)
•Different kinds of Localization problems.
•A taxonomy in 4 dimensions• Local versus Global (initial knowledge)
• Static versus Dynamic (environment)
• Passive versus active (control of robots)
• Single robot or multi-robot
![Page 7: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/7.jpg)
7
Mobile Robot Localization
•Solved already, the Bayes filter algorithm. How?
•The straightforward application of Bayes filters to the localization problem is called Markov localization.
•Here is the algorithm (abstract?)
![Page 8: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/8.jpg)
8
Mobile Robot Localization
• Algorithm Bayes_filter ( )
• for all do
•
• endfor
• return
111^ )(),|()( tttttt dxxbelxuxpxbel
ttt zuxbel ,),( 1
)( txbel
)()|( )( ^tttt xbelxzpxbel
tx
![Page 9: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/9.jpg)
9
Mobile Robot Localization
• Algorithm Markov Locatlization ( )
• for all do
•
• endfor
• return
The Markov Localization algorithm addresses the global localization problem, the position tracking problem, and the kidnapped robot problem in static environment.
111^ )(),,|()( tttttt dxxbelmxuxpxbel
mzuxbel ttt ,,),( 1
)( txbel
)(),|( )( ^tttt xbelmxzpxbel
tx
![Page 10: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/10.jpg)
10
Mobile Robot Localization
• Revisit Figure 7.5 to see how Markov localization algorithm in working.
• The algorithm Markov Localization is still very abstract. To put it in work (eg. your project), we need a lot of more background knowledge to realize motion model, sensor model, etc….
• We start with Guassian Filter (also called Kalman filter)
![Page 11: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/11.jpg)
11SA-1SA-1
Bayes Filter Implementations (1)
Kalman Filter(Gaussian filters)
(back to Ch.3)
![Page 12: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/12.jpg)
12
•Prediction
•Correction
Bayes Filter Reminder
111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
![Page 13: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/13.jpg)
13
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 14: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/14.jpg)
14
),(~),(~ 22
2
abaNYbaXY
NX
Properties of Gaussians
22
21
222
21
21
122
21
22
212222
2111 1
,~)()(),(~
),(~
NXpXpNX
NX
![Page 15: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/15.jpg)
15
• We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.
• Review your probability textbook
),(~),(~ TAABANY
BAXY
NX
Multivariate Gaussians
12
11
221
11
21
221
222
111 1,~)()(
),(~
),(~
NXpXpNX
NX
http://en.wikipedia.org/wiki/Multivariate_normal_distribution
![Page 16: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/16.jpg)
16
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 17: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/17.jpg)
17
Components of a Kalman Filter
t
Matrix (nxn) that describes how the state evolves from t to t-1 without controls or noise.
tA
Matrix (nxl) that describes how the control ut changes the state from t to t-1.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 18: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/18.jpg)
18
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 19: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/19.jpg)
19
Kalman Filter Updates in 1D
![Page 20: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/20.jpg)
20
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
![Page 21: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/21.jpg)
21
Kalman Filter Updates in 1D
tTtttt
tttttt RAA
uBAxbel
1
1)(
2
,2221)(
tactttt
tttttt a
ubaxbel
![Page 22: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/22.jpg)
22
Kalman Filter Updates
![Page 23: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/23.jpg)
23
0000 ,;)( xNxbel
Linear Gaussian Systems: Initialization
• Initial belief is normally distributed:
![Page 24: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/24.jpg)
24
• 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
![Page 25: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/25.jpg)
25
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 26: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/26.jpg)
26
• Observations are linear function of state plus additive noise:
tttt xCz
Linear Gaussian Systems: Observations
tttttt QxCzNxzp ,;)|(
ttttttt
tttt
xNQxCzN
xbelxzpxbel
,;~,;~
)()|()(
![Page 27: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/27.jpg)
27
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
See page 45-54 for mathematical derivation.
![Page 28: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/28.jpg)
28
The Prediction-Correction-Cycle
tTtttt
tttttt RAA
uBAxbel
1
1)(
2
,2221)(
tactttt
tttttt a
ubaxbel
Prediction
![Page 29: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/29.jpg)
29
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 30: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/30.jpg)
30
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 31: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/31.jpg)
31
Kalman Filter Summary
•Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)
•Optimal for linear Gaussian systems!
•However, most robotics systems are nonlinear, unfortunately!
![Page 32: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/32.jpg)
32
Nonlinear Dynamic Systems
•Most realistic robotic problems involve nonlinear functions
),( 1 ttt xugx
)( tt xhz
![Page 33: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/33.jpg)
33
Linearity Assumption Revisited
![Page 34: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/34.jpg)
34
Non-linear Function
![Page 35: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/35.jpg)
35
EKF Linearization (1)
![Page 36: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/36.jpg)
36
EKF Linearization (2)
![Page 37: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/37.jpg)
37
EKF Linearization (3)
![Page 38: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/38.jpg)
38
•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 39: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/39.jpg)
39
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 40: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/40.jpg)
40SA-1SA-1
Bayes Filter Implementations (2)
Particle filters
![Page 41: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/41.jpg)
41
Sample-based Localization (sonar)
![Page 42: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/42.jpg)
42
Represent belief by random samples
Estimation of non-Gaussian, nonlinear processes
Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter
Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]
Computer vision: [Isard and Blake 96, 98] Dynamic Bayesian Networks: [Kanazawa et al., 95]
Particle Filters
![Page 43: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/43.jpg)
43
Particle Filters
![Page 44: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/44.jpg)
44
)|()(
)()|()()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
Sensor Information: Importance Sampling
![Page 45: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/45.jpg)
45
'd)'()'|()( , xxBelxuxpxBel
Robot Motion
![Page 46: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/46.jpg)
46
)|()(
)()|()()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
Sensor Information: Importance Sampling
![Page 47: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/47.jpg)
47
Robot Motion
'd)'()'|()( , xxBelxuxpxBel
![Page 48: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/48.jpg)
48
1. Algorithm particle_filter( St-1, ut-1 zt):
2.
3. For Generate new samples
4. Sample index j(i) from the discrete distribution given by wt-
1
5. Sample from using and
6. Compute importance weight
7. Update normalization factor
8. Insert
9. For
10. Normalize weights
Particle Filter Algorithm
0, tS
ni ...1
},{ it
ittt wxSS
itw
itx ),|( 11 ttt uxxp )(
1ij
tx 1tu
)|( itt
it xzpw
ni ...1
/it
it ww
![Page 49: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/49.jpg)
49
draw xit1 from Bel(xt1)
draw xit from p(xt | xi
t1,ut1)
Importance factor for xit:
)|()(),|(
)(),|()|(ondistributi proposal
ondistributitarget
111
111
tt
tttt
tttttt
it
xzpxBeluxxp
xBeluxxpxzp
w
1111 )(),|()|()( tttttttt dxxBeluxxpxzpxBel
Particle Filter Algorithm
![Page 50: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/50.jpg)
50
Weight samples: w = f / g
Importance Sampling
http://en.wikipedia.org/wiki/Importance_sampling
![Page 51: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/51.jpg)
51
Importance Sampling with Resampling
),...,,(
)()|(),...,,|( :fon distributiTarget
2121
n
kk
n zzzp
xpxzpzzzxp
)(
)()|()|( :gon distributi Sampling
l
ll zp
xpxzpzxp
),...,,(
)|()(
)|(
),...,,|( : w weightsImportance
21
21
n
lkkl
l
n
zzzp
xzpzp
zxp
zzzxp
g
f
![Page 52: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/52.jpg)
52
Importance Sampling with Resampling
Weighted samples After resampling
![Page 53: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/53.jpg)
53
Resampling
• Given: Set S of weighted samples.
• Wanted : Random sample, where the probability of drawing xi is given by wi.
• Typically done n times with replacement to generate new sample set S’.
![Page 54: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/54.jpg)
54
w2
w3
w1wn
Wn-1
Resampling
w2
w3
w1wn
Wn-1
• Roulette wheel
• Binary search, n log n
• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance
![Page 55: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/55.jpg)
55
1. Algorithm systematic_resampling(S,n):
2.
3. For Generate cdf4. 5. Initialize threshold
6. For Draw samples …7. While ( ) Skip until next threshold reached8. 9. Insert10. Increment threshold
11. Return S’
Resampling Algorithm
11,' wcS
ni ...2i
ii wcc 1
1],,0]~ 11 inUu
nj ...1
11
nuu jj
ij cu
1,'' nxSS i
1ii
Also called stochastic universal sampling
![Page 56: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/56.jpg)
56
![Page 57: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/57.jpg)
57
![Page 58: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/58.jpg)
58
![Page 59: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/59.jpg)
60
![Page 60: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/60.jpg)
61
![Page 61: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/61.jpg)
62
![Page 62: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/62.jpg)
63
![Page 63: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/63.jpg)
64
![Page 64: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/64.jpg)
65
![Page 65: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/65.jpg)
66
![Page 66: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/66.jpg)
67
![Page 67: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/67.jpg)
68
![Page 68: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/68.jpg)
69
![Page 69: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/69.jpg)
70
![Page 70: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/70.jpg)
71
![Page 71: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/71.jpg)
72
![Page 72: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/72.jpg)
73
![Page 73: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/73.jpg)
74
![Page 74: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/74.jpg)
75
Sample-based Localization (sonar)
![Page 75: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/75.jpg)
76
Initial Distribution
![Page 76: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/76.jpg)
77
After Incorporating Ten Ultrasound Scans
![Page 77: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/77.jpg)
78
After Incorporating 65 Ultrasound Scans
![Page 78: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/78.jpg)
79
Estimated Path
![Page 79: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/79.jpg)
80
Using Ceiling Maps for Localization
![Page 80: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/80.jpg)
81
Vision-based Localization
P(z|x)
h(x)z
![Page 81: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/81.jpg)
82
Under a Light
Measurement z: P(z|x):
![Page 82: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/82.jpg)
83
Next to a Light
Measurement z: P(z|x):
![Page 83: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/83.jpg)
84
Elsewhere
Measurement z: P(z|x):
![Page 84: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/84.jpg)
85
Global Localization Using Vision
![Page 85: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/85.jpg)
86
Robots in Action: Albert
![Page 86: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/86.jpg)
87
Limitations
•The approach described so far is able to • track the pose of a mobile robot and to• globally localize the robot.
•How can we deal with localization errors (i.e., the kidnapped robot problem)?
![Page 87: 1 Mobile Robot Localization (ch. 7) Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment](https://reader036.vdocument.in/reader036/viewer/2022062518/56649f315503460f94c4cc6a/html5/thumbnails/87.jpg)
88
Approaches
•Randomly insert samples (the robot can be teleported at any point in time).
• Insert random samples proportional to the average likelihood of the particles (the robot has been teleported with higher probability when the likelihood of its observations drops).