Lab 10 - Grid Localization using Bayes Filter

1. Defining Functions

2. Implementation

The compute_control function calculates the sequence of motions (rotate, translate, rotate) needed to move from the previous pose to the current pose. It uses np.arctan2 to find the direction angle and np.hypot for the Euclidean distance. All angles are normalized to the range [-180, 180) degrees using mapper.normalize_angle to handle angular wrap-around.

The odom_motion_model computes the probability that the robot transitions from prev_pose to cur_pose given the actual control input u. It uses the loc.gaussian function with the noise parameters loc.odom_rot_sigma and loc.odom_trans_sigma to model the uncertainty in rotations and translation. The product of the three Gaussian probabilities gives the overall probability.

The prediction step implements the Bayesian prior update. It first extracts the actual control u from the odometry readings. It loops over all previous states with non‑negligible belief (≥ 0.0001) and, for each, updates the belief bar of every current state using the motion model. Finally, it normalizes loc.bel_bar.

The sensor model computes the likelihood of the 18 actual range measurements loc.obs_range_data given the true distances at a specific grid cell (obs). It assumes independent measurements and uses a Gaussian distribution with standard deviation loc.sensor_sigma.

The update step computes the posterior belief. It precomputes all true observation arrays for every grid cell using mapper.get_views. For each cell, it multiplies the prior belief loc.bel_bar by the measurement likelihood (product of the 18 individual likelihoods) and stores the result in loc.bel. A final normalization ensures the probabilities sum to 1

3. Video

4. Comparison of Poses

Positional error remains small with all steps having an error of less than 0.2 meters. The filter works best in an environment with distinctive features at each step. From the table, only step 11 had a lower probability of 0.8075, which shows that the environment has many distinct features.

Collaborators

For Lab 10, I worked with Sean Zhen, and I also used Angela Voo’s pages for reference.