Monte Carlo Localization

Shawn Farsai
Aug 31, 2018 · 5 min read

Monty what?

Localization is the practice of “becoming located,” and more specifically in the realm of AI and Robotics, the practice of locating a robot. But since it’s impossible to determine the location with absolute certainty, it’s more precise to say that we want to determine the probability distribution as to where we think the robot is.


In this first article, we look at the simplest form of localization, Monte Carlo, where we have a cyclic histogram(i.e. discrete locations) model of the world. By cyclic we mean that when you reach the end of the world, you cycle back to the beginning.

Welcome to our world

Our world — Let’s assume our world is only 5 tiles: p=[0,0,0,0,0]. Now let’s replace those zeroes with something more meaningful, like the likelihood of the robot being in the tile. That gives us this uniform distribution: p=[0.25, 0.25, 0.25, 0.25, 0.25 (recall that probabilities must add up to 1).

Meet our robot

Our robot contains a sensor that can detect the colors green and red up to a certain accuracy. It can move U tiles per motion, and it will always move before measuring.

Motion

Now every time we move, our robot will somehow affect our probability distribution. Say we have p=[0, 0.25, 0.5, 0.25, 0.25] and assume that our robot has the ability of perfect motion. This distribution says in english: the robot is definitely not in tile 1, maybe it’s in tile 2, 4, 5, but it’s most likely in tile 3. If our robot makes a motion U=2 that will give us a new distribution p=[0.25, 0.25, 0, 0.25. 0.5], shifting our distribution to the right by two.

Measurement

Now there’s something I didn’t tell you, our world is colored and we a have a map! Let’s says our world is, world=['green', 'red', 'red', 'green', 'green']. Remember we already made a motion, so our distribution is currently p=[0.25, 0.25, 0, 0.25. 0.5]. Now the robot turns on its sensor and detects the color 'green'. Well we know green is in tiles 1, 4, 5. So we expect our distribution to have increased probabilities in those tiles and decreased in the others. Now our sensor isn’t perfectly accurate, let’s assign a multiplier pHit=0.6 for matches and pMiss=0.2 for misses. What we’ll do now is multiply each tile by either pHit or pMiss accordingly, giving us p=[0.15, .05, 0, 0.15, .3]. Awesome! Wait, it doesn’t add up to 1! Yup… we need to re-normalize our data. So what we do is sum up our current values and divide each tile by that sum, this should take us back to a proper distribution adding up to 1. The sum is 0.65, dividing each tile gives us p=[.23, .08, 0, .23, .46], back to 1!

Now back to motion…

Okay, so it turns out our robot motion is not perfect, it can make mistakes. Let’s now assume our robot will move correctly with a probability of pExact=0.8, incorrectly overshoot by 1 tile with a probability of pOvershoot=0.1, and incorrectly undershoot by 1 tile with a probability of pUndershoot=0.1. Let’s now try it out and attempt to move over 2 tiles with the distribution q=[0, 1, 0, 0, 0]. This gives us q=[0, 0, 0.1, 0.8, 0.1], because there a 0.1 chance we only move 1 tile, 0.1 chance we accidentally move 3 tiles, and 0.8 chance we move exactly 2 tiles.

Let’s try to move 2 tiles on the distribution p=[0.25, 0.25, 0, 0.25. 0.5]. We have to look at each tile, and add up the probabilities of all possible ways to get there. What does this mean? Let’s try it and it will make more sense. How can we get to tile 1? First we initialize a new value for this tile s=0. One way is correctly moving 2 spaces from tile 4 (remember our world is cyclic), so we multiply our probability from tile 4 to pHit and add it to our accumulator s = s + 0.25 * pHit = 0.15. We can also reach it by accidentally undershooting from tile 5, s = s + 0.5 * pMiss = 0.25. Or we can reach it by overshooting from tile 3, s = s + 0 * pMiss = .25. Nice! Now we know tile 1 has a probability of 0.25, rinse repeat for each tile.


Math…

I promise it’s not much and just for intuition. If you want, you can ignore the math and just use the algorithm, but I’m personally not a religious person…

Bayes’ Theorem is used for measurement updates. It helps us answer the question, “what’s the probability we are in tile X given we just measured green?” You can think of the top part of the equation as multiplying each tile by pHit or pMiss , and you can think of the bottom part as our normalization step.

P(A|B) := “probability of A given B occurred”

2. Law of Total Probability is used for motion updates. In terms our project, you can think of this as, the probability we are in tile X is the sum of all the possible ways we could get to tile X

Okay, Okay… can we code already?!

Let’s start with the following. A distribution p, a world map world, a set of measurements, a set of motions, and their respective constants.

For our sense function, we initialize our new distribution q, loop through p, and append either p[i] * pHit or p[i] * pMiss. Finally, we loop through one more time and divide each term by the sum to normalize our distribution and bring it back to one that sums to 1.

For our move function, we a again initialize our new distribution q, loop through p and add to s all the possible ways of hitting it, intentionally or accidentally.

Finally, we execute! It’s practice that we always move first then measure.


Practice this by adjusting the code to make it for a 2d world!

Shout out to Sebastian Thrun and Udacity for the great course that this article is based off!

Welcome to a place where words matter. On Medium, smart voices and original ideas take center stage - with no ads in sight. Watch
Follow all the topics you care about, and we’ll deliver the best stories for you to your homepage and inbox. Explore
Get unlimited access to the best stories on Medium — and support writers while you’re at it. Just $5/month. Upgrade