Cornell University: ECE 4960

Lab 8: Grid Localization using Bayes Filter

Objective

The purpose of this lab is to implement grid localization using Bayes filter.

Even though the entire lab is in simulation, it requires you to have a good understanding of the bayes filter algorithm and the class documentations presented in the Jupyter notebook. Running the bayes filter for the entire trajectory takes around 5-10 mins based on how efficient your code is, so please start early. Make sure you refer to the Implementation Tips section and the feedback from Lab 7 before you begin to code.

Background

Refer Lab7.

Instructions

Workflow Tips

To close the simulator gracefully, use the lab-manager terminal window. Refrain from quitting the simulator using other methods.
You can close the lab-manager along with all the open tools by pressing <CTRL + C>.
You can double click left of a cell in a Jupyter notebook to collapse it to a single line. Double clicking again will expand the cell.
If the plotter becomes sluggish over long operating times, restart it.
There is a small “A” button on the bottom left corner of the plotter tool that zooms the plot to fit in the window.
Try Visual Studio Code. It has all the necessary plugins to help you edit markdown (save with the extension .md) and python files.

Setup the base code

Download the lab8 base code and follow the setup instructions as per the previous labs.

Simulator and Plotter

Use the lab8-manager to start the simulator and the plotter.

Jupyter Lab

Start the Jupyter server in your VM from the work directory:
```
cd /home/artemis/catkin_ws/src/lab8/scripts
```
Open the notebook “lab8.ipynb” from the left file browser pane.
SAVE your notebook after making changes, before closing the notebook or stopping the Jupyter server.

Implementation Tips

Implementation in robotics can be a daunting task with multiple sub-objectives. It is always a good idea to list out the sub-objectives for your task and modularize your code accordingly. The Jupyter notebook provides you with a skeleton code and some helper functions. Test out some of these functions (to_map, from_map, normalize_angle) individually to get a hang of the larger code base.

Computation Time

Notice that in each iteration of the bayes filter, you will need to go through all possible previous and current states in the grid to estimate the belief. Think about how many loops would be required to perform this.

Given that your grid has 20×20×18 = 7200 possible states, there is lot of computation involved in each iteration of the bayes filter. Hence, you need to write efficient code, especially in Python, if you want your entire estimation process to run within a couple of minutes. Try to get the running time of the prediction step and the update step functions to be within a couple of seconds; shorter running times may prove beneficial for testing and debugging. Here are some ways to reduce processing time:

If the probability of a state (i.e grid cell) is 0, then we can skip that grid cell in the inner loops of the prediction step of the bayes filter. (i.e only in multiplicative terms, since multiplying any value with a 0 results in a 0). In fact, if a state has a probability less than $0.0001$, we can skip those states as they dont contribute a lot to the belief and thus can reduce the computation time. Note that since you are skipping some states, the sum of the probabilities across the grid may no longer sum to 1. So you will need to make sure you normalize at the end of your prediction step and update step (which you already do as per the algorithm).
Reduce unnecessary interim variables. This can lead to slow processing times, especially in Python.
Use Numpy for faster matrix operations instead of element wise operations. Numpy is faster if you can use matrix like operations because the backend processing happens in C.
- HINT: The gaussian function can handle Numpy variables so think about how to perform Numpy operations in the update step. You may not even need the sensor_model function if you perform grid indexing to directly compute the sensor noise model in the update_step function.

Sensor Model with multiple individual measurements

Each measurement $z_{t}$ consists of 18 different individual measurements $z^{1}_{t}, z^{2}_{t}, ...., z^{18}_{t}$ recorded at equidistant angular positions during the robot’s (anti-clockwise) rotation behavior. The 18 true measurements values are recorded at the same equidistant angular positions for each grid cell (state) and is available through the Mapper class. Therefore, each index of the member variable array obs_views (of class Mapper) is the true individual measurement of the corresponding index of the member variable array obs_range_data (of class BaseLocalization). You do not need to use obs_bearing_data in your bayes filter implementation.

You will need to find the likelihood of the 18 measurements given a state i.e.

$p(z_{t}|x_{t},m) = \prod_{k=1}^{18} p(z^{k}_{t}|x_{t}, m)$

In the above equation, we assume that individual measurements are independent given the robot state.

Arithmetic Underflow

When you multiply probabilities with each other (especially in the update step), the resulting value may suffer from floating point underflow. To prevent this from happening normalize the belief grid when necessary using the following code:

loc.bel = loc.bel / np.sum(loc.bel)

Think about how often you need to do this; every operation takes time.

Gaussian Function

The gaussian function of class BaseLocalization can be used to model noise. It is advised to use Gaussians in the continious world as it is might be more involved to use Gaussians in a discretized world. Use the member functions from_map and to_map of class Mapper accordingly.

Normalizing your angles

The third dimension of the grid represents the orientation (yaw) in the range [-180,+180) degrees. When dealing with angles in your bayes filter (for example in calculating rotation1 and rotation2 in the odom motion model), you need to make sure the final angles are in the above range.

Think about what happens when you use a Gaussian to model a rotation of 350 degrees where the true value is -10 degrees and standard deviation is 20 degrees. $\mathcal{N}(350|\mu=-10,\sigma=20)$ is highly unlikely though 350 degrees is equivalent to -10 degrees.

Use the function normalize_angle from class Mapper to normalize your angles when necessary.

The units of the noise parameters: odom_rot_sigma, odom_trans_sigma and sensor_sigma are degrees, meters and meters, respectively.

Useful Python Functions

Use the python module math to import various functions such as degrees, radians, cos, sin, tan2, hypot, etc. [Refer math module functions]

Use the python module numpy for Numpy operations as basic slicing and indexing, functions such as sum(), cos, sin, tan2, arctan2 and operators such as + and * . [Refer numpy math functions]

NOTE: Most of these functions use radians as the default unit.

Write-up

To demonstrate that you’ve successfully completed the lab, please upload a brief lab report (<1.000 words), with code (not included in the word count), photos, and videos documenting that everything worked and what you did to make it happen. Include the most probable state after each iteration of the bayes filter along with its probability and compare it with the ground truth pose. Write down your inference of when it works and when it doesn’t.

You may use screen-recording tools from within the VM or on your host machine to record any necessary media.