# Grid and Particle Filter Localization

For PS5, submit material you wish to deliver electronically via:

``` submit fredm ps5 zip-or-tar-file
```

For PS6, submit material you wish to deliver electronically via:

``` submit fredm ps6 zip-or-tar-file
```

## Introduction

Your robot lives in a hallway with three doors as shown above. Your robot doesn't know where it is. Your job is to move it around and sense walls or doors, and have the robot learn where it is. You will do this by implementing (for PS5) the histogram/grid localization method (Table 8.1 on page 238) and (for PS6) the particle filter/Monte Carlo localization method (Table 8.2 on page 252) from Probabilistic Robotics.

Note: It is recommended that you set aside at least two significant work sessions to complete this assignment. This is not a one-night assignment. I recommend that you have completed the grid/histogram implementation before the next class meeting.

## Map

The map, as shown above, represents a hallway that is approximately 60 m in length. Each pixel in the source image represents 10 cm. The doorways are 2 m wide.

## Motion Model

You can make the robot move by publishing to the ROS topic `/robot/cmd_vel`.

You are only to move horizontally, in the positive or negative x direction. Attempts to move vertically or to rotate will be ignored.

The robot's actual velocity will follow a normal distribution of your commanded velocity. The standard deviation of the normal curve is one-third of the commanded velocity. By the definition of a normal distribution, approx. 68% of the time, the actual velocity will be within one standard deviation of the commanded velocity.

See http://en.wikipedia.org/wiki/Normal_distribution for more.

Note: if you attempt to drive beyond the left or right edges of the world, your robot will get stuck, but you won't receive an error message. This will confuse the algorithm, so don't do this.

## Sensor Model

Your robot has one sensor. It outputs a string, saying `Wall` or `Door`. If the robot is next to a wall (no door in sight), then 90% of the time the sensor will report `Wall`. If the robot is directly next to a door, 90% of the time, the sensor will report `Door`. As the robot crosses over the edge of a door, the probabilities are somewhere between these two endpoints.

The sensor publishes to the ROS topic `/robot/wall_door_sensor`. The images below visually illustrate its performance.

 P(Door)=0.1 P(Door)=0.12 P(Door)=0.21 P(Door)=0.37 P(Door)=0.47 P(Door)=0.65 P(Door)=0.75 P(Door)=0.85 P(Door)=0.9

Your program code will subscribe to the `/robot/wall_door_sensor` topic to receive the sensor readings.

You can also display the output of this sensor in a terminal window with the command:

``` \$ rostopic echo /robot/wall_door_sensor
```

### sensing-model-and-map.py

The file `sensing-model-and-map.py` (linked at the top) contains Python code which models the door sensor and the world map.

The code contains four Python functions:

• `gauss (x, mu, sigma)`: This calculates the value of the normal curve given its center point `mu`, its standard deviation `sigma`, and where along the curve you are sampling (`x`). Note that the height of the curve at its peak varies depending upon the standard deviation, because the area under the curve must sum to one. Therefore, if there is a small standard deviation, and the curve is highly peaked, the maximum value will be greater.
• `door(mu, x)`: This calculates the probability of seeing a doorway, given the center point of the door `mu` (in meters) and your `x` position. The result is scaled to a maximum likelihood of 0.8. So if you are standing right in middle of a door—e.g., `door(10, 10)`—the output will be 0.8.
• `p_door(x)`: This calculates the probability of seeing a door given your position in the world `x`. It incorporates the world model by locating doors at 11 m, 18.5 m, and 41 m. It also adds in the error baseline of 0.1, so probabilities will be in the range of 0.1 to 0.9.
Example output:
`p_door(11) = 0.9` because this is the exact middle of a doorway
`p_door(5) = 0.1` because there is a wall there
`p_door(10) = p_door(12) ~= 0.43` because this is the edge of a doorway.
• `p_wall(x)`: This is 1 - `p_door(x)`.

## Before You Write Any Code!!!

Before you write any code, write down your answer and explanations for items 1 through 3 below, under “To Turn In.”

## What Your Code Should Do

When implemented properly, the both the grid/histogram and particle filter/MCL algorithms will allow the robot to localize regardless of its initial starting position on the map.

### Grid Algorithm Details

This portion of the assignment is for PS5 only.

You should implement two versions of your code: one in which the robot starts at the left of the world and moves to the right, and one in which the robot starts at the right of the world and moves to the left. The only difference between these two versions should be the x velocity command given to your robot; nothing else should need to change. See the Notes below for information on how to position your robot at the right edge of the world before running your code that moves to the left.

Besides implementing the core algorithm of Table 8.1, you must create some kind of function that outputs your belief histogram each time through the loop. You may print the histogram out as ASCII art to the terminal console, write your own Python graphics display tool, or use a package like matplotlib to display the histogram.

It is recommended that you use a histogram of 600 bins—i.e., one bin per 10 cm of the world (one bin per pixel of the source map).

As discussed on page 241 of Probabilistic Robotics, for good performance, it is necessary that the robot's movement update causes probabilities to shift across bins in each time step. Since the default ROS time unit is 100 ms (10 Hz), it is suggested to use a command velocity of 4 m/s, or approx. 4 bins per time step. It might be necessary to perform a delayed motion update (as described on page 244) for good performance; feel free to experiment.

Remember that you should initialize your histogram with maximum uncertainty as to the robot's initial position (i.e., equal probability in every bin).

### Particle Filter/MCL Algorithm Details

This portion of the assignment is for PS6 only.

For the particle filter implementation, create the algorithm using the template shown in Table 8.2/page 252 of the text, and using the details provided in lecture, and from the AI class lectures/YouTube videos.

In a similar fashion to the histogram display described above for the grid algorithm, you must implement some kind of visual display of your particle clustering, which will be shown between each iteration of the particle sampling loop.

You can try different numbers of particles to determine how many are needed for good performance.

## Notes

Extract the `uml_hmm` folder and add it to your `ROS_PACKAGE_PATH`.

Launch the world with the command:

``` \$ roslaunch uml_hmm hmm.launch
```

In the terminal that ran the launch command, you will see a continuous display of the output of the wall/door sensor.

You can cause the robot to move around from the terminal with the following command:

``` \$ rostopic pub /robot/cmd_vel geometry_msgs/Twist --once -- [5,0,0] [0,0,0]
```

This will cause the robot to move 5m to the right for one or two time steps. This goes through the probabilistic function to smear the actual velocity.

You can use this to initialize your robot to different locations before running your code.

## To Turn In

You should zip or tar up all of the below when submitting your assignment:

### For PS5

1. If your robot happens to start at the left edge of the world (per the given startup code), and moves to the right, at what point in the world will the robot be when the histogram first reports high confidence in the robot's position? Explain.
2. If your robot happens to start at the right edge of the world, and moves to the left, at what point in the world will the robot be when the histogram first reports high confidence in the robot's position? Explain.
3. If your robot happens to start in the center of the world, and moves to the right, at what point in the world will the robot be when the histogram first reports high confidence in the robot's position? Explain.
4. Turn in your implementation control program for the grid/histogram localization.
5. Turn in some visual evidence (e.g., screen snaps) showing that your localization code works properly when your robot moves from left to right. This should include a simultaneous view of both the robot-in-its-world (i.e., Stage) and your histogram display. Please include three to five screen snaps, with numerically increasing filenames indicating temporal progress. If you have the software to record a continuous movie of what's on your screen, that would be fine too.
6. Using grid/histogram localization, turn in visual evidence of your code working with the robot moving starting at the left edge of the world (the default world initialization) and moving to the right.
7. Indicate your choice of robot velocity, and how frequently you performed motion model updates (e.g., every time step, or every n time steps?).

### For PS6

1. Turn in your implementation control program for the particle filter/MCL localization.
2. Using the particle filter/Monte Carlo localization, turn in visual evidence of your code working with the robot starting immediately to the right of the middle door and moving to the right.
3. In 100-200 words, specify how many particles you used in your MCL implementation, and why this seemed to be a good number.

This assignment was jointly developed by Fred Martin, James Dalphond, and Nat Tuck.