Mobile Robotics II: Simultaneous localization and mapping

Transcription

1 Mobile Robotics II: Simultaneous localization and mapping Introduction: probability theory, estimation Miroslav Kulich Intelligent and Mobile Robotics Group Gerstner Laboratory for Intelligent Decision Making and Control Czech Technical University in Prague Monday 23/07/2012 laboratory Gerstner

2 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

3 Something to wake up... Suppose a road between two pubs and a bus stop exactly at a half way. 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 at regular intervals (the same) from each pub. After one year, the man noticed that he drinks Pilsen twice frequently than Budweiser. How is it possible? Budweiser bus stop Pilsen house

4 Gentle introduction to probability theory Key idea: explicit representation of uncertainty using the calculus 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 number lower than five?

5 Discrete and continuous random variable Discrete: X is finite, i.e. X = x 1, x 2,..., x n p is called probability mass function Continuous: X takes on values in the continuum p is called probability density function Several distributions Mostly known: Normal distribution (Gaussian) p(x) = 1 2πσ e (x µ)2 2σ 2

6 Multivariete normal distribution p(x) = 1 2π Σ e 1 2 (x µ)t Σ 1 (x µ) Eigenvectors and eigenvalues of covariance matrix determine elipses.

7 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)

8 Law of Total probability, Marginals Discrete case p(x) = 1 x p(x) = p(x, y) y p(x) = p(x y)p(y) y Continuous case p(x)dx = 1 x p(x) = y y p(x, y)dy p(x) = p(x y)p(y)dy

9 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 prior evidence p(x y) = p(y x)p(x) = ηp(y x)p(x) p(y) η = p(y) 1 1 = x p(y x)p(x)

10 Exercise A robot uses a range sensor that can measure ranges from 0m to 3m. For simplicity, assume that actual ranges are distributed uniformly 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 is p = Suppose the robot queried its sensor N times, and every single tim the measurement value is bellow 1m. What is the posterior probability of a sensor fault, for N = 1, 2,.... Formulate the corresponding probabilistic model.

11 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 (counting frequencies) Bayes rule allows us to use causal: p(open z) = p(z open)p(open) p(z)

12 Example - open doors p(z open) = 0.6 p(z open) = 0.3 p(open) = p( open) = 0.5 p(open z) = p(open z) =?? p(z open)p(open) p(z open)p(open) + p(z open)p( open)

13 Example - open doors p(z open) = 0.6 p(z open) = 0.3 p(open) = p( open) = 0.5 p(open z) = p(open z) = p(z open)p(open) p(z open)p(open) + p(z open)p( open) = 2 3 = 0.67 z raises probability that the door is open.

14 Example - second measurement p(z 2 open) = 0.5 p(z 2 open) = 0.6 p(open z 1 ) = 2 3 p(z 2 open)p(open z 1 ) p(open z 2 z 1 ) = p(z 2 open)p(open z 1 ) + p(z 1 open)p( open z 1 ) = = 5 8 = z 2 lowers the probability that the door is open.

15 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 the uncertainty. 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 state from x to x.

16 Continuing the example - closing the door p(x u, x ) for u = close door OPEN CLOSED 1 0 p(x, u) = x p(x u, x )p(x ) If the door is open, the action close door succeeds in 90% of all cases.

17 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) =??

18 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) = = p(open u) = x p(open u, x )p(x ) = p(open u, open)p(open) + p(open u, closed)p(closed) = = 1 16 = 1 p(closed u)

19 Exercise Suppose we live at a place where days are either sunny, cloudy, or rainy. The weather transition function is a Markov chain with the following transition table: today it s... tomorrow will be... sunny cloudy rainy sunny cloudy rainy Draw state transition diagram.

20 Exercise Suppose we live at a place where days are either sunny, cloudy, or rainy. The weather transition function is a Markov chain with the following transition table: today it s... Draw state transition diagram. tomorrow will be... sunny cloudy rainy sunny cloudy rainy Suppose Day 1 is a sunny day. What is the probability if the following sequence of days: Day 2=cloudy, Day 3=cloudy, Day 4=rainy?

21 Exercise Suppose we live at a place where days are either sunny, cloudy, or rainy. The weather transition function is a Markov chain with the following transition table: today it s... Draw state transition diagram. tomorrow will be... sunny cloudy rainy sunny cloudy rainy Suppose Day 1 is a sunny day. What is the probability if the following sequence of days: Day 2=cloudy, Day 3=cloudy, Day 4=rainy? What is the probability that a random day will be sunny, cloudy, or rainy?

22 Exercise Suppose we live at a place where days are either sunny, cloudy, or rainy. The weather transition function is a Markov chain with the following transition table: today it s... Draw state transition diagram. tomorrow will be... sunny cloudy rainy sunny cloudy rainy Suppose Day 1 is a sunny day. What is the probability if the following sequence of days: Day 2=cloudy, Day 3=cloudy, Day 4=rainy? What is the probability that a random day will be sunny, cloudy, or rainy? Compute the probability table of yesterday s weather given today s weather.

23 Exercise Suppose that we cannot observe the weather directly, but instead rely on sensor. The problem is that our sensor is noisy. Its measurements are governed by the following measurement model: the actual weather is... our sensor tells us... sunny cloudy rainy sunny cloudy rainy Suppose Day 1 is sunny, and the subsequent four days our sensor observes cloudy, cloudy, rainy, sunny. What is the probability that Day 5 is indeed sunny as predicted by our sensor?