textbook

Thanks for stopping by! Please note that this page will not be updated anymore and all content has been moved into an open-source textbook. The books is available open-source on github, compiled as PDF, and in print on Amazon.com.

Robots employ sensors and actuators that are subject to uncertainty. We learned last week how to quantify this uncertainty using probability density functions that associate a probability with each possible outcome of a random process, such as the reading of a sensor or the actual physical change of an actuator. One of the most common probability density functions is the Gaussian distribution. It has the shape of a bell and can entirely be described by its mean – the center of the bell curve – and its variance – the width of its bell curve. We are often interested in extracting high-level features, such as the distance to a wall or the position in space, from a number of different sensors. As the underlying measurements are uncertain, also these features will be subject to uncertainty. How to calculate the uncertainty of a feature from the uncertainty of the sensors that detect this feature, is covered by the error propagation law. Here, the key insight is that the variance of a feature is the weighted sum of all contributing sensors’ variances, weighed by their impact on the feature of interest. This impact can be approximated by the derivative of the function mapping sensor input to feature output. Unfortunately, uncertainty keeps increasing without the ability to correct measurements.The goals of this lecture are to present mathematical tools and algorithms that will enable you to actually shrink the uncertainty of a measurement by combining it with additional observations.  In particular, we will study

  • Using landmarks to decrease the estimate of a position update (Markov Localization)
  •  Moving from continuous to discrete belief representations (Particle Filter)

Markov Localization

Imagine a floor with three doors, two of which are closer together, and the third farther down the corridor. Imagine know that your robot is able to detect doors, i.e., is able to tell whether it is in front of a wall or in front of a door. Such a feature can serve the robot as a landmark. Given a map of this simple environment and no information whatsoever where our robot is located, we can use such a landmark to drastically reduce the space of possible locations once the robot has passed one of the doors. One way of representing this belief is to describe the robot’s position with three Gaussian distributions, each centered in front of  a door and its variance a function of the uncertainty with which the robot can detect a door’s center. (This approach is known as a multi-hypothesis belief.) What happens if the robot continues to move? From last lecture’s error propagation law we know:
  1. The Gaussians describing the robot’s 3 possible locations will move with the robot.
  2. The variance of each Gaussian will keep increasing with the distance the robot moves.
What happens if the robot arrives at another door? Given a map of the environment, we can now map the three Gaussian distributions to the location of the three doors. As all three Gaussians will have moved, but the doors are not equally spaced, only some of the peaks will coincide with the location of  a door. Assuming we fully trust our door detector, we can now remove all beliefs that do not coincide with a door.
Exercise: Make a series of drawings depicting this situation. Assume the corridor to be a 1D environment with your robot traveling from left to right.
Things are just slightly more complicated if our door detector is also subject to uncertainty: there is a chance that we are in front of a door, but haven’t noticed it. Then, it would be a mistake to remove this belief. Instead, we just weight all beliefs with the probability that there could be a door. Say our door detector detects false-positives with a 10% chance. Then, there is a 10% chance to be at any location that is not in front of door, even if our detector tells us we are in front of a door. Similarly, our detector might detect false-negatives with 20% chance, i.e., tell us there is no door even though the robot is just in front of it. Thus, we would need to weigh all locations in front of a door with 20% chance and all locations not in front of a door with 80% likelihood if our robot tells us there is no door, even if we are indeed in front of one. Fortunately, there is a formal way to describe such situations: Bayes Rule.

Bayes Rule

Bayes’ Rule relates a conditional probability to its inverse. In other words, if we know the probability of event A happen given event B happening, we can calculate the probability of B given A happening. Bayes’ rule can be derived from the simple observation that the probability of A and B happen together (P(A \cap B)) is given by P(A)P(B|A) or the probability of A to happen and the probability of B to happen given that A happens. Similarly, we can write

P(A \cap B)=P(A)P(B|A)=P(B)P(A|B)

From this, deriving Bayes’ rule is straightforward:

P(A|B)=\frac{P(A)P(B|A)}{P(B)}

In words, if we know the probability that B happens given that A happens, we can calculate that A happens given that B happens. How does this map into the Markov Localization framework (or to anything useful)? Lets assume, event A is to be at a specific location loc. Lets also assume event B corresponds to the event to see a particular feature feat. We can now rewrite Bayes’ rule to

P(loc|feat)=\frac{P(loc)P(feat|loc)}{P(feat)}

This looks like something interesting: we can calculate the probability to be at location loc, given that we see feature feat. For example, loc could correspond to door 1, 2 or 3, and feat could be the event of sensing a door. What do we need to know?

  1. We need to know the prior probability to be at location loc (P(loc))
  2. We need to know the probability to see the feature at this location (P(feat|loc))
  3. We need the probability to encounter the feature feat (P(feat))

Lets start with (3), which is most confusing. The answer is simple, no matter what P(feat) is, it will cancel out as the probability to be at any of the possible locations has to sum up to 1. (This is easy to see, if you think about a problem with only three possible locations. Each of the equations will contain the same P(feat)).

The prior probability to be at location  loc P(loc) is given by the belief model. In the case of the 3 door example, it is the value of the Gaussian distribution underneath the door corresponding to loc.

Finally, we need to know the probability to see the feature at location loc P(feat|loc). If your sensor were perfect, this probability is simply 1. If it is not, P(feat|loc) corresponds to the likelihood for the sensor to yield a false-negative, i.e. telling us there is no feature even if it is right in front of it.

The final missing piece is how to best represent possible locations. So far, we assumed Gaussian distributions for each possible location (which is perfectly fine). Alternatively, we can also imagine that we discretize the world into a grid and calculate the likelihood of the robot to be in any of its cells. In our 3-door world, it might make sense to choose grid cells that have just the width of a door.

Including motion

So far, we have understood how we can update the probability of a belief based on sensor information. How can odometry come

Markov Localization, from top to down: the robot starts at an unknown location. After the first perception update, the area of possible locations shrinks drastically (middle), and continues to shrink after the next perception updated (bottom). In this example, the map is known.

into play, however? Recall, that odometry input is just another sensor that we assume to have a Gaussian distribution; if our odometer tells us that the robot traveled a meter, it could have traveled a  little less or a little more, with decreasing likelihood. We can therefore calculate the posterior probability of the robot being at a certain location loc given its odometer input odo and its previous location loc’:

P(loc|odo)=P(loc')P(odo|loc'->loc)

Here, we already took out the P(odo) term. The term P(loc') should be clear by now: it is the prior probability that we are at position loc’. The term P(odo|loc'->loc) corresponds to the probability to get odometer reading odo after traveling from position loc’ to loc. If getting reading odo is reasonable for the distance from loc to loc’ this probability is high. If its unreasonable, for example if the distance is much larger than the robot could possibly ever have driven, this probability should be very low. The challenge is now that the robot could have potentially been everywhere. We therefore have to calculate the posterior probability P(loc|odo) for all possible positions $loc’$. This can be easily accomplished by summing over all possible locations:

P(loc|odo)=\sum_{loc'}P(loc')P(odo|loc'->loc)

In other words, the law of total probability requires you to consider all possible locations the robot could have ever been at. In practice you don’t need to calculate this for all possible locations, but only those that are technically feasible given the maximum speed of the robot. Notice also that the sum notation technically corresponds to a convolution with the robots odometry error distribution.

We have now learned two methods to update the belief distribution of where the robot could be in the environment. First, a robot can use external landmarks to update its position. This is known as perception update in Markov Localization and relies on exterioception. Second, can observe its internal sensors. This is known as action update and relies on proprioception. Markov localization relies on the combination of these two processes and performs one after the other. You can think about the action update to increase the uncertainty of the robot’s position and the perception update to shrink it. (You can also think about the action update as a discrete version of the error propagation model. Also here you are using the robotics kinematic model and the noise model of your odometer to calculate P(odo|loc'->loc).

Example 1: Topological Map

This example describes one of the first successful real robot systems that employed Markov Localization in an office environment. The experiment is described in more detail in a 1994 article of AI Magazine. The office environment consisted of two rooms and a corridor that can be modeled by a topological map. In a topological map, areas that the robot can be in are modeled as vertices, and navigable connections between them are modeled as edges of a graph. The location of the robot can now be represented as a probability distribution over the vertices of  this graph.

From left to right: Dervish robot in a mock-up office environment, map of the environment, corresponding topological map.

The robot has the following sensing abilities:

  • It can detect a wall to its left or right
  • It can detect an open door to its left or right
  • It can detect a closed door to its left or right
  • It can detect whether it is an open hallway
Unfortunately, the robot’s sensors are not at all reliable. The researchers have experimentally found the following probabilities to obtain a certain sensor response for specific physical positions
Wall Closed door Open door Open hallway Foyer
Nothing detected 70% 40% 5% 0.1% 30%
Closed door detected 30% 60% 0% 0% 5%
Open door detected 0% 0% 90% 10% 15%
Open hallway detected 0% 0% 0.1% 90% 50%
For example, the success rate to detect a closed door is only 60%, whereas a foyer looks like an open door in 15% of the trials. This data corresponds to the conditional probability to detect a certain feature given a certain location.

Rooms, doors, and corridors for the environment used in the example in the text.

Consider now the following initial belief state distribution: p(1-2)=0.8 and p(2-3)=0.2. You know that the robot faces east with certainty. The robot now drives for a while until it reports “open hallway on its left and open door on its right”. This actually corresponds to location 2, but the robot can in fact be anywhere. For example there is a 10% chance that the open door is in fact an open hallway, i.e. the robot is really at position 4. How can we calculate the new probability distribution of the robot’s location? Here are the possible trajectories that could happen:
  1. The robot could move from 2-3 to 3, 3-4 and 4. The probability to detect an open door on its right is zero for 3 and 3-4, leaves position 4. For this to happen, the following things need to have happened:
    * The robot must have started at 2-3 (20%)
    * Not have seen the open door at the left of 3 (5%) and not have seen the wall at the right (70%)
    * Not have seen the wall to its left (70%) and not have seen the wall to its right (70%) at node 3-4
    * Correctly identify the open hallway to its left (90%) and mistake the open hallway to its right for an open door (10%)
    Together, the likelihood that the robot got from position2-3 to position 4 is therefore given by 0.2*0.05*0.7*0.7*0.7*0.9*0.1=0.03% – much more unlikely than the 10% that a naive estimate based on sensor accuracy would provide!
  2. The robot could also move from 1-2 to 2, 2-3, 3, 3-4 or 4.
    * The chance that it correctly detects the open hallway and door at position 2 is 0.9*09, so the chance to be at position 2 is
    0.8*0.9*0.9=64%.
    * The chance to have seen an open door instead of a wall at 2-3, 3, and 3-4 is zero.
    * Similar calculation as for 1 applies for position 4, but the event to have not seen both open hallway and open door at 2
    needs to be multiplied in. This probability would need to be added to the one obtained in 1.

Example 2: Grid-based Markov Localization

The following example (figure to the right) models the environment as a grid. Each cell is marked with a probability corresponding to the likelihood of the robot being at this exact location. We assume that the robot is able to detect walls with some certainty. The images in the right column show the actual location of the robot. Initially, the robot does not see a wall and therefore could be almost anywhere. The robot now moves northwards. The action update now propagates the probability of the robot being somewhere upwards. As soon as the robot encounters the wall, the perception update bumps up the likelihood to be anywhere near a wall. As there is some uncertainty associated with the wall detector, the robot cannot only be directly at the wall, but anywhere -with decreasing probability – close by. As the action update involved continuous motion to the north, the likelihood to be close to the south wall is almost zero. The robot then performs a right turn and travels along the wall in clockwise direction. As soon as it hits the east wall, it is almost certain about its position, which then again decreases.

Particle Filter

Although grid-based Markov Localization can provide compelling results, it can be computationally very expensive, in particular when the environment is large and the resolution is small. This is in part due to the fact that we need to carry the probability to be at a certain location forward for every cell on the grid, regardless of how small this probability is. An elegant solution to this problem is the particle filter. It works as follows:

  • Represent the robots position by N particles that are randomly distributed around its estimated initial position. Like we expect the robot’s position to be distributed, we can choose a Gaussian distribution.
  • Every time the robot moves, we will move each particle in the exact same way, but add noise to each movement much like we would expect it the real robot to exhibit. Without a perception update, the particles will spread apart farther and farther.
  • Upon a perception event, we evaluate every single particle using our sensor model.What would the likelihood be to have a perception event such as we observed at this location? We can then use Bayes rule to update each particles position.
  • Once in a while, particles that have a too low probability can be deleted, while those with the highest probability can be replicated.

Take-Home Lessons

  • If the robot has no additional sensors and its odometry is noisy, error propagation will lead to ever increasing uncertainty of a robots position.
  • Once the robot is able to sense features with known locations, Bayes’ rule can be used to update the posterior probability of a possible position. The key insight is that the conditional probability to be at a certain position given a certain observation can be inferred from the likelihood to actually make this observation given a certain position.
  • A complete solution that performs this process for all possible locations is known as Markov Localization. It can be approximated by a particle filter.
 

3 Responses to Introduction to Robotics #9: Markov Localization

  1. Dylan White says:

    Under “Example 2: Grid-based Markov Localization” the notes refer to a figure on the right, which isn’t there. Could you include this figure with the notes?

Leave a Reply

Set your Twitter account name in your settings to use the TwitterBar Section.