Fast Robotics Spring 2026

Overview

The objective of this lab was to implement grid localization using a Bayes Filter in simulation. Rather than trusting odometry alone, the robot maintains a probabilistic belief over its pose and continuously refines that belief using both motion updates and depth observations. The robot state is represented as (x, y, θ), and the continuous map is discretized into a 12 × 9 × 18 grid over position and orientation. Each grid cell stores the probability that the robot is in that state, and the full belief distribution is updated at each iteration of the filter.

This lab was mainly about putting the Bayes Filter pipeline together cleanly and understanding what each step was doing. The prediction step spreads belief forward using the odometry motion model, while the update step sharpens that belief again using the 18 depth observations collected during a full scan. In practice, this meant balancing correctness with computation time, since even a modest grid still leads to a large number of possible state transitions.

Bayes Filter Structure

The full Bayes Filter alternates between prediction and update. Prediction uses the previous belief and the measured control input to estimate where the robot could have moved. Update then compares the robot’s actual observation against the expected observations for every grid cell and boosts the states that best match the sensor data.

In this lab, the filter worked over a fully discretized belief grid. That means the most likely robot pose at any moment is just the cell with the highest probability after normalization. The entire implementation was built around five core functions: compute_control(), odom_motion_model(), prediction_step(), sensor_model(), and update_step().

Compute Control

The first step was extracting the odometry control input between two poses in the form of rotation, translation, and final rotation. I normalized the angular components so that all comparisons stayed within the expected heading range.

def compute_control(cur_pose, prev_pose):
    """ Given the current and previous odometry poses, this function extracts
    the control information based on the odometry motion model.

    Args:
        cur_pose  ([Pose]): Current Pose
        prev_pose ([Pose]): Previous Pose 

    Returns:
        [delta_rot_1]: Rotation 1  (degrees)
        [delta_trans]: Translation (meters)
        [delta_rot_2]: Rotation 2  (degrees)
    """

    x_diff = cur_pose[0] - prev_pose[0]
    y_diff = cur_pose[1] - prev_pose[1]

    delta_rot_1 = np.degrees(math.atan2(y_diff,x_diff)) - prev_pose[2]
    delta_trans = math.sqrt(x_diff**2 + y_diff**2)
    delta_rot_2 = cur_pose[2] - prev_pose[2] - delta_rot_1
    
    return mapper.normalize_angle(delta_rot_1), delta_trans, mapper.normalize_angle(delta_rot_2)

Odometry Motion Model

Once the control tuple was available, I used Gaussian models to score how likely a state transition was under that motion. This function compares the motion needed to move from one pose to another against the actual control input measured from odometry.

def odom_motion_model(cur_pose, prev_pose, u):
    """ Odometry Motion Model

    Args:
        cur_pose  ([Pose]): Current Pose
        prev_pose ([Pose]): Previous Pose
        (rot1, trans, rot2) (float, float, float): A tuple with control data in the format 
                                                   format (rot1, trans, rot2) with units (degrees, meters, degrees)

    Returns:
        prob [float]: Probability p(x'|x, u)
    """

    delta_rot_1, delta_trans, delta_rot_2 = compute_control(cur_pose, prev_pose)

    prob_rot_1 = loc.gaussian(delta_rot_1, u[0], loc.odom_rot_sigma)
    prob_trans = loc.gaussian(delta_trans, u[1], loc.odom_trans_sigma)
    prob_rot_2 = loc.gaussian(delta_rot_2, u[2], loc.odom_rot_sigma)

    prob = prob_rot_1 * prob_trans * prob_rot_2
    return prob

Prediction Step

The prediction step builds the prior belief bel_bar. This is where the computational cost shows up, because the code has to consider many previous-to-current state transitions. To reduce unnecessary work, I ignored prior states whose probability was below 0.0001. That threshold helped a lot with runtime while still keeping the dominant probability mass intact after normalization.

def prediction_step(cur_odom, prev_odom):
    """ Prediction step of the Bayes Filter.
    Update the probabilities in loc.bel_bar based on loc.bel from the previous time step and the odometry motion model.

    Args:
        cur_odom  ([Pose]): Current Pose
        prev_odom ([Pose]): Previous Pose
    """

    loc.bel_bar = np.zeros((mapper.MAX_CELLS_X, mapper.MAX_CELLS_Y, mapper.MAX_CELLS_A))
    
    # Get motion model
    u = compute_control(cur_odom, prev_odom)

    # Loop over each possible form cell to to cell combination within probaility threshold
    for x_cell in range(mapper.MAX_CELLS_X):
        for y_cell in range(mapper.MAX_CELLS_Y):
            for t_cell in range(mapper.MAX_CELLS_A):
                prev_bel = loc.bel[x_cell,y_cell,t_cell]
                # Threshold previous probaility
                if prev_bel >= 0.0001:
                    cell_pose = mapper.from_map(x_cell, y_cell, t_cell)
                    for to_x in range(mapper.MAX_CELLS_X):
                        for to_y in range(mapper.MAX_CELLS_Y):
                            for to_t in range(mapper.MAX_CELLS_A):
                                to_pose = mapper.from_map(to_x, to_y, to_t)
                                # Get probability and update belief
                                prob = odom_motion_model(to_pose, cell_pose, u)
                                loc.bel_bar[to_x, to_y, to_t] += prob * prev_bel
    
    # Normalize
    loc.bel_bar = loc.bel_bar / np.sum(loc.bel_bar)

Sensor Model

The sensor model compares the expected observations for a grid cell against the actual range data measured by the robot. Since each scan contains 18 individual observations, I computed the per-measurement Gaussian likelihoods and returned them as an array.

def sensor_model(obs):
    """ This is the equivalent of p(z|x).

    Args:
        obs ([ndarray]): A 1D array consisting of the true observations for a specific robot pose in the map 

    Returns:
        [ndarray]: Returns a 1D array of size 18 (=loc.OBS_PER_CELL) with the likelihoods of each individual sensor measurement
    """

    prob_array = np.zeros(mapper.OBS_PER_CELL)

    for i in range(mapper.OBS_PER_CELL):
        prob_array[i] = loc.gaussian(obs[i], loc.obs_range_data[i,0], loc.sensor_sigma)

    return prob_array

Update Step

The update step multiplies the prior belief by the full observation likelihood for each state. Since the scan contains 18 depth values, I took the product of the 18 individual sensor probabilities and used that to weight bel_bar. After that, I normalized the final belief again.

def update_step():
    """ Update step of the Bayes Filter.
    Update the probabilities in loc.bel based on loc.bel_bar and the sensor model.
    """

    for x_cell in range(mapper.MAX_CELLS_X):
        for y_cell in range(mapper.MAX_CELLS_Y):
            for t_cell in range(mapper.MAX_CELLS_A):
                prob = np.prod(sensor_model(mapper.get_views(x_cell,y_cell,t_cell)))
                loc.bel[x_cell,y_cell,t_cell] = prob * loc.bel_bar[x_cell,y_cell,t_cell]

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

Results and Analysis

From my simulation results, the Bayes Filter clearly outperformed odometry alone. The prediction step spread the belief in a way that made sense, but it still carried noticeable uncertainty since it only depended on noisy motion estimates. After the update step, the belief often collapsed much more tightly around the correct region of the map. That was the main takeaway of the lab: the motion model gives a reasonable prior, but the sensor update is what actually keeps the estimate anchored.

I also noticed that the localization worked best near walls and corners, where the depth observations were more unique. In those parts of the map, the update step had a much easier time separating one state from another. In more open regions, several grid cells could produce similar observations, so the filter had less information to confidently choose between them. That made the estimate slightly less sharp in the center of the environment.

Another practical takeaway was runtime. Even though the grid is not huge, the full prediction step still becomes expensive quickly when all state combinations are considered. The probability threshold in my prediction loop was important for making the implementation usable without changing the basic logic of the Bayes Filter.

Simulation Video

The following video demonstrates the simulation trajectory, depth observations, and the Bayes Filter running live throughout the localization process.

Conclusion

Overall, this lab was a good demonstration of why probabilistic localization is necessary. Odometry alone was not reliable enough, but once the robot combined motion information with range observations using a Bayes Filter, the state estimate became much more stable and meaningful. The final implementation showed that even with a relatively coarse grid, the robot could localize well enough to stay close to ground truth, especially when it had strong sensor information from surrounding walls.

Acknowledgements

I would like to thank the teaching staff for persistent help and oversight. I would also like to credit ChatGPT for helping answer Bayes Filter implementation questions and for converting my written lab report into a functional html page.

• Back to Top
• Back to Lab Reports