mobile robotics ii: simultaneous localization and...
TRANSCRIPT
![Page 1: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/1.jpg)
laboratory
Gerstner
Probability State estimation
Mobile Robotics II: Simultaneous localization andmapping
Introduction: probability theory, estimation
Miroslav Kulich
Intelligent and Mobile Robotics GroupGerstner Laboratory for Intelligent Decision Making and Control
Czech Technical University in Prague
Monday 23/07/2012
![Page 2: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/2.jpg)
Probability State estimation
Course outline
• Introduction to probability and estimation
• Bayes filter and its implementations
• Bayes filter based localization
• Probabilistic approach to mapping
• Simultaneous localization and mapping
• EKF-SLAM
• Fast-SLAM
• Labs
![Page 3: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/3.jpg)
Probability State estimation
Something to wake up...
Suppose a road between two pubs and a bus stop exactly at a halfway. A man goes every day at a random time to the bus stop,waits for the first bus and goes to the pub. The buses start atregular intervals (the same) from each pub. After one year, theman noticed that he drinks Pilsen twice frequently than Budweiser.How is it possible?
Budweiser bus stop Pilsen
house
![Page 4: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/4.jpg)
Probability State estimation
Gentle introduction to probability theory
• Key idea: explicit representation of uncertainty using thecalculus of probability theory
• p(X=x) probability that the random variable X has the value x
• 0 ≤ p(x) ≤ 1
• p(true) = 1, p(false) = 0
• p(A ∨ B) = p(A) + p(B)− p(A ∧ B)
What is the probability of throwing an even number or a numberlower than five?
![Page 5: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/5.jpg)
Probability State estimation
Discrete and continuous random variable
• Discrete: X is finite, i.e.X = x1, x2, . . . , xn
• p is called probability mass function
• Continuous: X takes on values in thecontinuum
• p is called probability density function
• Several distributions
• Mostly known: Normal distribution(Gaussian)
• p(x) = 1√2πσ
e−(x−µ)2
2σ2
![Page 6: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/6.jpg)
Probability State estimation
Multivariete normal distribution
p(x) =1√
2π|Σ|e−
12
(x−µ)T Σ−1(x−µ)
• Eigenvectors and eigenvalues of covariance matrix determineelipses.
![Page 7: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/7.jpg)
Probability State estimation
Joint and conditional probability
• p(X = x and Y = y) = p(x , y)
• If X and Y are independent then
p(x , y) = p(x)p(y)
• p(x |y) is the probability of x given y
p(x |y) = p(x , y)/p(y)
p(x , y) = p(x |y)/p(y)
• If X and Y are independent then
p(x |y) = p(x)
![Page 8: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/8.jpg)
Probability State estimation
Law of Total probability, Marginals
Discrete case∑x
p(x) = 1
p(x) =∑y
p(x , y)
p(x) =∑y
p(x |y)p(y)
Continuous case∫xp(x)dx = 1
p(x) =
∫yp(x , y)dy
p(x) =
∫yp(x |y)p(y)dy
![Page 9: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/9.jpg)
Probability State estimation
Bayes formula
p(x , y) = p(x |y)p(y) = p(y |x)p(x)
⇒
p(x |y) =p(y |x)p(x)
p(y)=
likelihood · priorevidence
p(x |y) =p(y |x)p(x)
p(y)= ηp(y |x)p(x)
η = p(y)−1 =1∑
x p(y |x)p(x)
![Page 10: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/10.jpg)
Probability State estimation
Exercise
A robot uses a range sensor that can measure ranges from 0m to3m. For simplicity, assume that actual ranges are distributeduniformly in this interval. Unfortunatelly, the sensor can be faulty.When the sensor is faulty, it constantly outputs a range bellow 1m,regardless of the actual range in the sensor’s measurements cone.We know that the prior probability for a sensor to be faulty isp = 0.01.Suppose the robot queried its sensor N times, and every single timthe measurement value is bellow 1m. What is the posteriorprobability of a sensor fault, for N = 1, 2, . . . . Formulate thecorresponding probabilistic model.
![Page 11: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/11.jpg)
Probability State estimation
Simple example of state estimation
• Suppose a robot obtains measurement z
• What is p(open|z)?
• p(open|z) is diagnostic
• p(z |open) is causal
• Often causal knowledge is easier to obtain (countingfrequencies)
• Bayes rule allows us to use causal:
p(open|z) =p(z |open)p(open)
p(z)
![Page 12: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/12.jpg)
Probability State estimation
Example - open doors
• p(z |open) = 0.6 p(z |¬open) = 0.3
• p(open) = p(¬open) = 0.5
p(open|z) =p(z |open)p(open)
p(z |open)p(open) + p(z |¬open)p(¬open)
p(open|z) = ??
![Page 13: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/13.jpg)
Probability State estimation
Example - open doors
• p(z |open) = 0.6 p(z |¬open) = 0.3
• p(open) = p(¬open) = 0.5
p(open|z) =p(z |open)p(open)
p(z |open)p(open) + p(z |¬open)p(¬open)
p(open|z) =0.6 · 0.5
0.6 · 0.5 + 0.3 · 0.5=
2
3= 0.67
• z raises probability that the door is open.
![Page 14: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/14.jpg)
Probability State estimation
Example - second measurement
• p(z2|open) = 0.5 p(z2|¬open) = 0.6
• p(open|z1) = 23
p(open|z2z1) =p(z2|open)p(open|z1)
p(z2|open)p(open|z1) + p(z1|¬open)p(¬open|z1)
=12 ·
23
12 ·
23 + 3
5 ·13
=5
8= 0.625
• z2 lowers the probability that the door is open.
![Page 15: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/15.jpg)
Probability State estimation
Actions
• Often the world is dynamic since• actions carried out by the robot,• actions carried out by other agents,• or just the time passing by change the world (plants grow).
• Actions are never carried out with absolute certainty.
• In contrast to measurements, actions generally decrease theuncertainty.
• To incorporate the outcome of an action u into the current“belief”, we use the conditional pdf
p(x |u, x ′)
• This term specifies the pdf that executing u changes the statefrom x ′ to x .
![Page 16: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/16.jpg)
Probability State estimation
Continuing the example - closing the door
p(x |u, x ′) for u = ”close door”
CLOSEDOPEN
0.9
0
10.1
p(x , u) =∑x ′
p(x |u, x ′)p(x ′)
If the door is open, the action ”close door” succeeds in 90% of allcases.
![Page 17: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/17.jpg)
Probability State estimation
Continuing the example - closing the door
p(closed |u) =∑x ′
p(closed |u, x ′)p(x ′)
= p(closed |u, open)p(open)
+ p(closed |u, closed)p(closed)
= ??
![Page 18: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/18.jpg)
Probability State estimation
Continuing the example - closing the door
p(closed |u) =∑x ′
p(closed |u, x ′)p(x ′)
= p(closed |u, open)p(open)
+ p(closed |u, closed)p(closed)
=9
10· 5
8+
1
1· 3
8=
15
16
p(open|u) =∑x ′
p(open|u, x ′)p(x ′)
= p(open|u, open)p(open)
+ p(open|u, closed)p(closed)
=1
10· 5
8+
0
1· 3
8=
1
16= 1− p(closed |u)
![Page 19: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/19.jpg)
Probability State estimation
ExerciseSuppose we live at a place where days are either sunny, cloudy, or rainy.The weather transition function is a Markov chain with the followingtransition table:
tomorrow will be . . .sunny cloudy rainy
today it’s . . .sunny 0.8 0.2 0cloudy 0.4 0.4 0.2rainy 0.2 0.6 0.2
• Draw state transition diagram.
• Suppose Day 1 is a sunny day. What is the probability if thefollowing sequence of days: Day 2=cloudy , Day 3=cloudy , Day4=rainy?
• What is the probability that a random day will be sunny, cloudy, orrainy?
• Compute the probability table of yesterday’s weather given today’sweather.
![Page 20: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/20.jpg)
Probability State estimation
ExerciseSuppose we live at a place where days are either sunny, cloudy, or rainy.The weather transition function is a Markov chain with the followingtransition table:
tomorrow will be . . .sunny cloudy rainy
today it’s . . .sunny 0.8 0.2 0cloudy 0.4 0.4 0.2rainy 0.2 0.6 0.2
• Draw state transition diagram.
• Suppose Day 1 is a sunny day. What is the probability if thefollowing sequence of days: Day 2=cloudy , Day 3=cloudy , Day4=rainy?
• What is the probability that a random day will be sunny, cloudy, orrainy?
• Compute the probability table of yesterday’s weather given today’sweather.
![Page 21: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/21.jpg)
Probability State estimation
ExerciseSuppose we live at a place where days are either sunny, cloudy, or rainy.The weather transition function is a Markov chain with the followingtransition table:
tomorrow will be . . .sunny cloudy rainy
today it’s . . .sunny 0.8 0.2 0cloudy 0.4 0.4 0.2rainy 0.2 0.6 0.2
• Draw state transition diagram.
• Suppose Day 1 is a sunny day. What is the probability if thefollowing sequence of days: Day 2=cloudy , Day 3=cloudy , Day4=rainy?
• What is the probability that a random day will be sunny, cloudy, orrainy?
• Compute the probability table of yesterday’s weather given today’sweather.
![Page 22: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/22.jpg)
Probability State estimation
ExerciseSuppose we live at a place where days are either sunny, cloudy, or rainy.The weather transition function is a Markov chain with the followingtransition table:
tomorrow will be . . .sunny cloudy rainy
today it’s . . .sunny 0.8 0.2 0cloudy 0.4 0.4 0.2rainy 0.2 0.6 0.2
• Draw state transition diagram.
• Suppose Day 1 is a sunny day. What is the probability if thefollowing sequence of days: Day 2=cloudy , Day 3=cloudy , Day4=rainy?
• What is the probability that a random day will be sunny, cloudy, orrainy?
• Compute the probability table of yesterday’s weather given today’sweather.
![Page 23: Mobile Robotics II: Simultaneous localization and mappinglabe.felk.cvut.cz/~kulich/ECI_2012/files/intro.pdfDraw state transition diagram. Suppose Day 1 is a sunny day. What is the](https://reader034.vdocuments.us/reader034/viewer/2022042101/5e7dd13955d3a02a6f5b8e62/html5/thumbnails/23.jpg)
Probability State estimation
Exercise
Suppose that we cannot observe the weather directly, but insteadrely on sensor. The problem is that our sensor is noisy. Itsmeasurements are governed by the following measurement model:
our sensor tells us . . .sunny cloudy rainy
the actual weather is . . .sunny 0.6 0.4 0cloudy 0.3 0.7 0rainy 0 0 1
• Suppose Day 1 is sunny, and the subsequent four days oursensor observes cloudy , cloudy , rainy , sunny . What is theprobability that Day 5 is indeed sunny as predicted by oursensor?