Lab 10: Grid Localization using Bayes Filter

1. Objective

The goal of this lab was to implement grid localization using a Bayes Filter on a virtual robot navigating a known map. The robot's 3D state space (x, y, θ) was discretized into a 12×9×18 grid (1944 cells), and the filter was used to estimate the robot's most probable pose at each of 16 time steps along a pre-planned trajectory.

2. Implementation

Four functions were implemented to complete the Bayes Filter:

2.1 compute_control(cur_pose, prev_pose)

Computes the relative transformation between two consecutive poses and decomposes it into the odometry motion parameters (δrot1, δtrans, δrot2). delta_trans is the Euclidean distance between the two positions (hypot), and delta_rot_1 is the heading toward the new position relative to prev_pose[2] (via atan2). Critically, delta_rot_2 is computed as cur_pose[2] − prev_pose[2] − delta_rot_1 and then normalized via normalize_angle() to ensure the "shortest-path" angle is used — preventing the Gaussian from being evaluated at a numerically distant but geometrically equivalent angle.

2.2 odom_motion_model(cur_pose, prev_pose, u)

Returns the transition probability p(x_t | u_t, x_{t-1}) by computing what control would be needed to go from prev_pose to cur_pose, then comparing that to the actual control u using three independent Gaussians — one each for δrot1, δtrans, and δrot2 — parameterized by odom_rot_sigma and odom_trans_sigma.

2.3 prediction_step(cur_odom, prev_odom)

This implements the Motion Update. It loops over all possible previous and current grid cells. To optimize performance, cells with a previous belief <0.0001 are skipped. The resulting prior belief, loc.bel_bar, is accumulated and normalized to maintain a valid probability distribution.

2.4 sensor_model(obs) + update_step()

The update step is fully vectorized using NumPy. The sensor model computes Gaussian likelihoods for all 18 readings across all 1944 cells simultaneously by broadcasting loc.obs_range_data against mapper.obs_views (shape 12×9×18×18). The product of 18 likelihoods per cell weights bel_bar to produce the final bel, which is then normalized.

def compute_control(cur_pose, prev_pose):
    delta_trans = math.hypot(cur_pose[1] - prev_pose[1], cur_pose[0] - prev_pose[0])
    delta_rot_1 = mapper.normalize_angle(
        math.degrees(math.atan2(cur_pose[1] - prev_pose[1], cur_pose[0] - prev_pose[0]))
        - prev_pose[2])
    delta_rot_2 = mapper.normalize_angle(cur_pose[2] - prev_pose[2] - delta_rot_1)
    return delta_rot_1, delta_trans, delta_rot_2

def odom_motion_model(cur_pose, prev_pose, u):
    actual_rot_1, actual_trans, actual_rot_2 = compute_control(cur_pose, prev_pose)
    p_rot_1  = loc.gaussian(mapper.normalize_angle(actual_rot_1 - u[0]), 0, loc.odom_rot_sigma)
    p_trans  = loc.gaussian(actual_trans - u[1], 0, loc.odom_trans_sigma)
    p_rot_2  = loc.gaussian(mapper.normalize_angle(actual_rot_2 - u[2]), 0, loc.odom_rot_sigma)
    return p_rot_1 * p_trans * p_rot_2

def prediction_step(cur_odom, prev_odom):
    u = compute_control(cur_odom, prev_odom)
    loc.bel_bar = np.zeros(loc.bel_bar.shape)
    for px in range(mapper.MAX_CELLS_X):
        for py in range(mapper.MAX_CELLS_Y):
            for pa in range(mapper.MAX_CELLS_A):
                if loc.bel[px, py, pa] < 0.0001:
                    continue
                prev_pose = mapper.from_map(px, py, pa)
                for cx in range(mapper.MAX_CELLS_X):
                    for cy in range(mapper.MAX_CELLS_Y):
                        for ca in range(mapper.MAX_CELLS_A):
                            cur_pose = mapper.from_map(cx, cy, ca)
                            p = odom_motion_model(cur_pose, prev_pose, u)
                            loc.bel_bar[cx, cy, ca] += p * loc.bel[px, py, pa]
    loc.bel_bar /= np.sum(loc.bel_bar)

def sensor_model(obs):
    return loc.gaussian(loc.obs_range_data.reshape(-1), obs, loc.sensor_sigma)

def update_step():
    obs_flat = loc.obs_range_data.reshape(-1)
    likelihoods = loc.gaussian(mapper.obs_views, obs_flat, loc.sensor_sigma)
    loc.bel = np.prod(likelihoods, axis=3) * loc.bel_bar
    loc.bel /= np.sum(loc.bel)

3. Results

The plotter visualizes odometry (red), ground truth (green), and Bayes Filter belief (blue). The belief tracks the ground truth closely for the majority of the trajectory, while odometry drifts significantly — especially in angle — due to accumulated error.

Figure 1 — The given pre-planned trajectory the virtual robot follows through the map.

Figure 2 — Full 16-step trajectory result. Odometry (red) drifts badly; ground truth (green) and Bayes Filter belief (blue) remain tightly coupled.

4. Per-Step Summary

The table below compares the ground truth pose, the Bayes Filter's most probable belief pose, and the resulting position/angle error at each of the 16 time steps.

5. Analysis: When It Works and When It Doesn't

5.1 Success Conditions

The filter excels in geometrically unique regions (corners/alcoves). In steps 3–5 and 14–15, position errors remained <0.15 m with belief probabilities of ~1.0. The 18-ray sensor model provides a highly discriminative "signature" that allows the update step to recover even from a diffuse or inaccurate prior — for instance, in Step 3, the update step successfully corrected the belief despite bel_bar at the winning cell being only 0.00055.

5.2 Failure Modes

Challenges arose during rapid rotations (Steps 0–2) and when odometry drift was at its peak. Because the ground truth yaw accumulates unboundedly (reaching 5946° by Step 15) while grid cells are constrained to [−180°, 180°), the prediction step occasionally placed prior mass in a non-winning cell. Step 2 exhibited the highest translational error (0.223 m) because the motion was primarily rotational, making the translation component of the motion model less discriminative.

Most strikingly, Steps 5 and 10 saw bel_bar values of 6.5×10⁻¹⁵ and 3.3×10⁻⁶ at the correct cell — effectively a small-scale Kidnapped Robot scenario where the prediction step considered the true pose nearly impossible. In both cases, the 18-ray sensor model was strong enough to "rescue" the filter, assigning ~1.0 probability to the correct cell after the update. This robustness is a direct result of the richness of the full 360° scan.

AI usage: AI was used to build this site and to help format and structure the lab writeup.