ece4160

Lab 10: Grid Localization using Bayes Filter

In this lab, I implemented grid localization using a Bayes Filter.

The Bayes Filter is an algorithm that uses measurements taken and control data to generate the belief distribution.

To perform grid localization for the sample trajectory, I had to write the necessary code for the functions compute_control, odom_motion_model, prediction_step, sensor_model, and update_step given in the Lab 10 notebook.

For the first function, compute_control, I was able to calculate initial rotation, translation, and final rotation using the equations given in lecture, as seen below. This function extracts the control information based on the odometry motion model, given the current and previous odometry poses.

I implemented this in Python using the code below.

Next, I implemented the function for the odometry motion model, odom_motion_model, as seen below. This was based on the algorithm given in lecture.

I then implemented the function for the prediction step of the Bayes filter, prediction_step. This function took the current and previous poses as arguments and updated the probabilities in loc.bel_bar based on loc.bel from the previous time step and the odometry motion model.

Next, I implemented the function sensor_model, which calculated p(z|x). This function took a 1D array consisting of the true observations for a specific robot pose in the map as an argument and returned a 1D array, with a size equal to the number of observations per cell, with the likelihoods of each individual sensor measurement.

Finally, I implemented the function update_step, which performed the update step of the Bayes Filter by updating the probabilities in loc.bel based on loc.bel_bar and the sensor model.

I then used these functions to perform grid localization for the given sample trajectory, as seen in the video below, where red is the odometry measurements, blue is the belief of the robot, and green is the truth pose.

The Bayes Filter works very well for the most part, although at times its path was perpendicular to the robots true path, although it may have just needed a couple of update steps to correct itself.

References

I referenced Anya’s code extensively to better understand what was required.