Lab 10: Grid Localization using Bayes Filter

Overview

The purpose of this lab is to implement grid localization using a Bayes filter in simulation. Pure odometry is insufficient for localization because wheel slip, IMU drift, and other noise sources cause the estimated pose to diverge from the true pose over time. The Bayes filter addresses this by maintaining a probability distribution over all possible robot poses, using a prediction step driven by odometry and an update step driven by sensor measurements to iteratively correct the belief.

The continuous pose space is discretized into a 12x9x18 grid covering [-1.6764, +1.9812) m in x, [-1.3716, +1.3716) m in y, and [-180, +180) degrees in theta, with each cell measuring 0.3048 m x 0.3048 m x 20 degrees. The most probable cell after each update step is taken as the robot's estimated pose. The default sample trajectory was used without modification.

Implementation

The Bayes filter implementation requires five functions: compute_control, odom_motion_model, prediction_step, sensor_model, and update_step, together implementing the full predict-update cycle.

compute_control

Extracts the rotation1, translation, and rotation2 control tuple from two consecutive odometry poses using math.hypot for translation and math.atan2 for heading, with angles normalized to [-180, 180) via loc.mapper.normalize_angle.

odom_motion_model

Computes the transition probability between a hypothetical previous and current pose. It calls compute_control to get the "required" control tuple, then compares each component against the actual odometry control tuple using a Gaussian. The three resulting probabilities (rotation1, translation, and rotation2) are multiplied together. Angles are explicitly normalized before evaluation to prevent wrap-around errors (e.g., incorrectly applying a massive penalty to the physical equivalence of 350° and -10°).

prediction_step

This step propagates the robot's belief state forward by iterating over all possible prior belief cells and applying the odometry motion model to distribute that probability into current cells. Because iterating over the full 3D grid against itself requires a massive number of loop executions, an aggressive optimization is implemented: any prior cell with a probability below 0.0001 is immediately skipped. For the remaining valid cells, loc.mapper.from_map is used to convert grid indices back into continuous physical poses before evaluating the motion model.

sensor_model

This function computes the likelihood of the 18 current ToF sensor readings given a specific hypothetical grid cell. Rather than using an explicit loop, it takes advantage of NumPy's optimized C-backend to perform a vectorized Gaussian evaluation. It compares the live sensor readings stored in loc.obs_range_data.flatten() against the precomputed ground truth distances for the given cell passed in as the obs array, using the predefined sensor standard deviation.

update_step

The update step incorporates the sensor data to refine the prior belief generated by the prediction step. For every cell in the grid, it fetches the ground truth sensor views using loc.mapper.get_views. It then calls sensor_model to get an array of 18 independent likelihoods, uses np.prod to combine them into a single probability scalar, and multiplies this by the cell's prior belief loc.bel_bar. Finally, to prevent arithmetic underflow and ensure a valid probability distribution, the entire 3D belief grid is divided by its sum to normalize the values back to 1.0.

Bayes Filter Results

The video below shows the Bayes filter running in the simulator across the full trajectory for Run 1. The green trajectory represents the ground truth path, the blue trajectory represents the odometry path, and the red trajectory represents the filter's most probable belief state at each timestep. As seen in the video, the belief closely tracks the ground truth throughout the run, demonstrating the filter's ability to correct for odometry drift using sensor updates.

Video showcasing the Bayes Filter.

The tables below show the most probable belief state after the update step at each timestep across three independent runs, compared against ground truth. Position values are in meters and angles are in degrees. While the ground truth state from the simulator accumulates angle without bounds, the belief state and the resulting angular error have been properly normalized to the [-180, 180) range to accurately reflect true heading deviation.

Run 1

Run 2

Run 3

Overall the filter performs well across all three runs using the default sample trajectory, with x and y position errors consistently staying within one grid cell width (0.3048 m). The angular errors after normalization are small throughout, generally staying within roughly 10-17 degrees, which is less than one grid cell in the theta dimension (20 degrees). Results are consistent across all three runs, with the filter converging to the same solution each time.

The update step is clearly the dominant source of accuracy. While the prediction step alone would produce a diffuse belief spread across multiple cells due to noisy odometry, the 18-reading sensor update provides enough information to sharply localize the robot, converging probabilities near 1.0. The filter struggles slightly before initial convergence, and at timestep 15 in run 3 the probability dips to 0.874, likely due to accumulated odometry drift and a noisier sensor sweep. Despite this, the positional estimate remains within one grid cell of ground truth.

Aidan McNay's website was used to inspire sections of this lab.