Lab 10: Grid Localization using Bayes Filter

Overview

Without an absolute position sensor, the robot has to figure out where it is from indirect evidence. Odometry accumulates error quickly, as the previous lab showed. This lab takes a different approach: instead of maintaining a single pose guess, the robot tracks a probability distribution over all possible poses simultaneously. The Bayes Filter updates that distribution at each time step using motion and sensor information, letting confidence build up over time rather than committing prematurely to one answer.

The pose space (x, y, θ) is continuous, so it gets discretized into a 12 by 9 by 18 grid with cells 0.3048 m wide and 20 degrees deep, giving 1944 cells total. Each cell holds the probability that the robot currently occupies that pose. Two operations run every iteration: a prediction step that smears the belief based on how the robot moved, and an update step that tightens it back up using sensor data.

Implementation

compute_control

Any motion between two consecutive poses can be described by three quantities: an initial rotation to face the direction of movement, a straight translation, and a final rotation to reach the new heading. This function extracts those values from a pair of odometry readings. When the translation is nearly zero, the initial rotation is forced to zero rather than computing arctan2 on nearly zero inputs. All angles are wrapped to the interval [−180°, 180°) so the Gaussians in the motion model behave correctly.

def compute_control(cur_pose, prev_pose):
    dx = cur_pose[0] - prev_pose[0]
    dy = cur_pose[1] - prev_pose[1]

    delta_trans = np.hypot(dx, dy)
    if delta_trans < 1e-4:
        delta_rot_1 = 0.0
    else:
        delta_rot_1 = mapper.normalize_angle(np.degrees(np.arctan2(dy, dx)) - 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

odom_motion_model

This function evaluates how likely a given state transition is under the observed odometry. It recomputes the control decomposition for a hypothetical previous and current pose pair, then evaluates a Gaussian for each of the three components against the actual measured control. The three values are multiplied together to produce the overall transition probability. Angle differences are normalized before going into the Gaussian to prevent wraparound issues near the interval boundaries.

def odom_motion_model(cur_pose, prev_pose, u):
    rot1_needed, trans_needed, rot2_needed = compute_control(cur_pose, prev_pose)

    p_rot1  = loc.gaussian(mapper.normalize_angle(u[0] - rot1_needed),  0, loc.odom_rot_sigma)
    p_trans = loc.gaussian(u[1] - trans_needed,                          0, loc.odom_trans_sigma)
    p_rot2  = loc.gaussian(mapper.normalize_angle(u[2] - rot2_needed),  0, loc.odom_rot_sigma)

    return p_rot1 * p_trans * p_rot2

prediction_step

Each call to the prediction step updates bel_bar by iterating over all prior cells and distributing their probability forward through the motion model. The naive version requires 1944 squared evaluations per call, which takes too long. Since the update step concentrates belief onto very few cells each iteration, skipping any prior cell below 0.0001 probability has almost no effect on the final result and brings the runtime down to a few seconds per step.

def prediction_step(cur_odom, prev_odom):
    u = compute_control(cur_odom, prev_odom)
    loc.bel_bar = np.zeros((mapper.MAX_CELLS_X, mapper.MAX_CELLS_Y, mapper.MAX_CELLS_A))

    for px in range(mapper.MAX_CELLS_X):
        for py in range(mapper.MAX_CELLS_Y):
            for pa in range(mapper.MAX_CELLS_A):
                prior = loc.bel[px, py, pa]
                if prior < 0.0001:
                    continue
                prev_world = 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_world = mapper.from_map(cx, cy, ca)
                            loc.bel_bar[cx, cy, ca] += odom_motion_model(cur_world, prev_world, u) * prior

    loc.bel_bar /= np.sum(loc.bel_bar)

update_step

After spinning in place to collect 18 range readings, those measurements are compared against the precomputed true distances stored for every cell. Rather than looping over cells in Python, the Gaussian is broadcast directly over the full obs_views array at once using NumPy. The product of likelihoods across all 18 angles is taken per cell with np.prod(axis=3), multiplied against bel_bar, and renormalized. This single step does most of the work in terms of localization accuracy.

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

Results

The filter ran for 13 steps over the default trajectory. After each update, the belief converged to a single dominant cell with probability at or very near 1.0. Position error stayed below one cell width (0.3048 m) for every step and was under 0.2 m for most of them. Step 12 near the center of the room was the hardest, where a few neighboring cells had similar sensor signatures and the probability dropped to 0.998.

Steps 5 through 9 along the wall on the right side were the most reliable. Close range readings from nearby surfaces are distinctive, so the sensor model easily rules out most of the grid. Even when the prediction step spread the prior very broadly, the update step pulled the belief back to the correct cell.

Collaboration Statement