Lab 10

Objective

The goal of this lab was to use the Bayes Filter to implement grid localization on the virtual robot.

Helper functions

Compute control

To characterize the robot's motion we use the odometry model, which splits up robot motion into three steps:

  • Robot rotates by delta_rot_1 and points towards destination
  • Robot translates delta_trans along straight line to destination
  • Robot rotates by delta_rot_2 to correct its orientation.
Given the current and previous poses, the function compute_control reconstructs the motion between two robot poses by calculating the parameters above.



Odometry motion model

Here we calculate the transition probability from the previous pose to the current pose, given the current/previous poses and the control inputs we used. Given the current/previous poses, we can calculate the ideal set of motions the robot would've taken to get from one to the other. We compare this against the actual set of motions the robot took, and determine how likely it was for the robot to end up where it is now. We assume each movement is a Gaussian variable, and that the movements are independent from one another.



Prediction step

The prediction step of the Bayes Filter puts the above two steps togethe by calculating the transition probabilities between every grid cell. These transition probabilities are used to predict the robot's next state, bel_bar. To save computation time, cells that have very low transition probabilities are ignored.



Sensor model

Each measurement is an independent Gaussian variable: given the robot's observations, we find the likelihood of each sensor measurement.



Update step

We incorporate measurement uncertainty into our beliefs and update loc.bel accordingly.


Running the Bayes Filter



Run 1
Run 2

The video shows my Bayes filter running. The raw odometry does a poor job of locating the robot and characterizing its motion. The Bayes filter, however, does a pretty good job of estimating where the robot is as it is not that far off from the ground truth.


Here is some of the data I collected: