Lab 10: Grid Localization using Bayes Filter

In this lab I implemented a Bayes Filter to localize the robot on a discretized grid using odometry and sensor data.

1. Grid Localization and Bayes Filter Overview

The goal of this lab was to estimate the robot’s pose (x, y, θ) over time using a probabilistic method. Instead of assuming the robot is at one exact point, grid localization divides the world into a finite 3D grid, where each cell represents a possible pose and stores a probability value.

In this lab, the grid had dimensions (12, 9, 18), corresponding to the discretized x, y, and orientation axes. The probability values across all cells form the robot’s belief, and the sum of all those probabilities must always be 1.

The Bayes Filter updates this belief in two parts. The prediction step uses odometry data to estimate where the robot moved, which increases uncertainty. The update step then uses sensor data to correct that estimate and reduce uncertainty.

2. Computing the Control Input

To use the odometry motion model, I first needed to calculate the control input in the form (rotation1, translation, rotation2) from the robot’s previous and current odometry poses.

The first rotation points the robot toward the direction of motion, the translation is the straight-line distance traveled, and the second rotation accounts for the remaining orientation change after that motion.

Control Input Computation
def compute_control(cur_pose, prev_pose):
    cur_x, cur_y, cur_theta = cur_pose
    prev_x, prev_y, prev_theta = prev_pose

    dx = cur_x - prev_x
    dy = cur_y - prev_y

    heading = np.degrees(np.arctan2(dy, dx))
    delta_rot_1 = mapper.normalize_angle(heading - prev_theta)

    delta_trans = np.sqrt(dx**2 + dy**2)

    delta_rot_2 = mapper.normalize_angle(cur_theta - prev_theta - delta_rot_1)

    return delta_rot_1, delta_trans, delta_rot_2

3. Odometry Motion Model

Once I had the control input, I implemented the odometry motion model. This function calculates the probability of transitioning from one state to another given the measured control input. To do this, I computed the hypothetical control required for a specific state transition and compared it to the actual odometry-based control input using Gaussian distributions.

Each component of the motion model, rotation 1, translation, and rotation 2, is treated independently and multiplied together to get the total transition probability.

Odometry Motion Model
def odom_motion_model(cur_pose, prev_pose, u):
    d_rot1, d_trans, d_rot2 = compute_control(cur_pose, prev_pose)

    p1 = loc.gaussian(d_rot1, u[0], loc.odom_rot_sigma)
    p2 = loc.gaussian(d_trans, u[1], loc.odom_trans_sigma)
    p3 = loc.gaussian(d_rot2, u[2], loc.odom_rot_sigma)

    return p1 * p2 * p3

4. Prediction Step

The prediction step computes the prior belief bel_bar based on the previous belief and the motion model. This is the most computationally expensive part of the Bayes Filter because it considers transitions from many possible previous states to many possible current states.

To help reduce computation time, I ignored previous states whose belief was lower than 1e-4. Since those states contribute very little to the final result, skipping them made the prediction step much faster without significantly affecting performance.

Prediction Step Logic
def prediction_step(cur_odom, prev_odom):
    u = compute_control(cur_odom, prev_odom)

    loc.bel_bar = np.zeros_like(loc.bel)

    gx, gy, gt = np.indices(loc.bel.shape)
    flat_states = list(zip(gx.ravel(), gy.ravel(), gt.ravel()))

    threshold = 1e-4
    valid_prev = np.argwhere(loc.bel >= threshold)

    for px, py, pt in valid_prev:
        prev_state = mapper.from_map(px, py, pt)
        weight = loc.bel[px, py, pt]

        probs = np.array([
            odom_motion_model(mapper.from_map(cx, cy, ct), prev_state, u)
            for (cx, cy, ct) in flat_states
        ])

        probs = probs.reshape(loc.bel.shape)
        loc.bel_bar += weight * probs

    total = np.sum(loc.bel_bar)
    if total > 0:
        loc.bel_bar /= total

This step spreads the belief based on motion uncertainty, which is why the belief becomes more uncertain before the sensor update is applied.

5. Sensor Model

The sensor model compares the expected observations at a given state with the robot’s actual sensor readings. Each state in the map has 18 expected range measurements, and the likelihood of each of those measurements is modeled using a Gaussian distribution.

Since the individual measurements are assumed to be independent, the final sensor likelihood for a state is the product of the 18 individual likelihoods.

Sensor Model
def sensor_model(obs):
    return [
        loc.gaussian(exp_val, meas_val, loc.sensor_sigma)
        for exp_val, meas_val in zip(obs, loc.obs_range_data)
    ]

6. Update Step

The update step corrects the predicted belief using the sensor model. For every possible state in the grid, I generated the expected sensor readings, compared them to the real measurements, and multiplied those probabilities together to get the likelihood of being in that state.

That likelihood was then multiplied by the prior belief bel_bar, and the result was normalized to get the updated belief bel.

Update Step Logic
def update_step():
    gx, gy, gt = np.indices(loc.bel.shape)
    flat_states = zip(gx.ravel(), gy.ravel(), gt.ravel())

    likelihoods = np.array([
        np.prod(
            sensor_model(
                mapper.get_views(x, y, t)
            )
        )
        for (x, y, t) in flat_states
    ])

    likelihoods = likelihoods.reshape(loc.bel.shape)

    loc.bel = loc.bel_bar * likelihoods

    norm = np.sum(loc.bel)
    if norm > 0:
        loc.bel /= norm

7. Results and Localization Performance

I ran the Bayes Filter in the simulator over the sample trajectory and compared the estimated pose to the ground truth and raw odometry. As expected, the raw odometry drifted away over time because small motion errors continued to accumulate.

The Bayes Filter performed much better because the sensor update kept correcting the pose estimate based on the expected map observations. This let the estimated pose stay much closer to the ground truth during the run.

The media below can be used to compare the raw odometry results against the Bayes Filter localization results.

Raw Odometry
Bayes Filter Localization

8. Discussion

One important part of this lab was seeing how much better localization gets when odometry and sensor data are fused together. If I used odometry alone, the robot’s estimate drifted quickly. But once the update step was included, the filter was able to correct for that drift by matching sensor readings to the map.

Another major part of the lab was computation time. Since the grid contains many possible states, the prediction step can easily become very slow if every state is considered every time. Thresholding low-probability states was one of the most helpful ways to speed up the algorithm.

Overall, this lab showed how a Bayes Filter can produce a much more reliable pose estimate than raw odometry alone, even when the robot motion and sensor readings are both noisy.