Lab 10

The robot’s 3D state space (x, y, θ) spanning [-1.6764, 1.9812) m × [-1.3716, 1.3716) m × [-180, 180) is discretized into a 12 × 9 × 18 grid (1944 cells). Each cell stores the probability that the robot occupies that pose. The Bayes Filter alternates between a prediction step (which spreads belief forward using the odometry motion model) and an update step (which decreased uncertainty inthe belief using ToF sensor measurements).

Instead of relying on noisy odometry alone, the Bayes filter maintains a probabilistic belief over the robot’s pose that is updated iteratively with both motion and sensor data.

The pseudocode for the algorithm is as follows (from Lecture 17, slide 41):

Implementation

Compute Control

compute_control decomposes the movement between two odometry poses into (δ_rot1, δ_trans, δ_rot2). Both rotations are passed through mapper.normalize_angle to keep them in [-180°, 180°].

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_prev, y_prev, theta_prev = prev_pose
  x_cur, y_cur, theta_cur = cur_pose

  dx = x_cur - x_prev
  dy = y_cur - y_prev

  delta_rot_1 = mapper.normalize_angle(np.degrees(np.arctan2(dy, dx)) - theta_prev)
  delta_trans  = np.sqrt(dx**2 + dy**2)
  delta_rot_2  = mapper.normalize_angle(theta_cur - theta_prev - delta_rot_1)

  return delta_rot_1, delta_trans, delta_rot_2

Odometry Motion Model

odom_motion_model uses compute_control to find the implied motion between a candidate previous and current pose, then evaluates how likely that motion is given the actual odometry reading u via three independent Gaussians (one per motion component). The product of the three gives the transition probability $p(x_t | u_t, x_{t-1})$.

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

prediction_step implements the prediction equation by iterating over all previous cells and accumulating their contributions to each possible current cell weighted by the transition probability. Cells with prior belief below 0.0001 are skipped. The resulting bel_bar is normalized.

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
  """

  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):
          bel_prev = loc.bel[px, py, pa]
          if bel_prev < 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)
                prob = odom_motion_model(cur_pose, prev_pose, u)
                loc.bel_bar[cx, cy, ca] += prob * bel_prev

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

Sensor Model

sensor_model compares the 18 true range observations for a given cell against the robot’s actual ToF readings using a Gaussian noise model, returning an array of 18 per-reading likelihoods.

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 = loc.gaussian(obs, loc.obs_range_data.flatten(), loc.sensor_sigma)

  return prob_array

Update Step

update_step loops over all current cells, computes the product of the 18 sensor likelihoods via np.prod(sensor_model(...)), multiplies by bel_bar, and normalizes to produce the updated belief bel.

def update_step():
  """ Update step of the Bayes Filter.
  Update the probabilities in loc.bel based on loc.bel_bar and the sensor model.
  """
  range_data = loc.obs_range_data.flatten()
  for (x, y, a), bel_bar in np.ndenumerate(loc.bel_bar):
    true_obs = mapper.get_views(x, y, a)
    prob_array = loc.gaussian(true_obs, range_data, loc.sensor_sigma)
    loc.bel[x, y, a] = np.prod(prob_array) * bel_bar

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

Running the Bayes Filter in Simulation

The blue Bayes-filtered trajectory closely tracks the green ground truth throughout the run, while the red odometry path drifts a lot. The inaccuracy of the odometry model is highlighted in the video as it has a non-reliable trajectory. Belief probabilities near 1.0 indicate high-confidence localization at most steps.

The Bayes filter perform sbetter when the robot is near walls. This is because the sensor data being more trustworthy and consistent when it is closer to the walls, and less trustworthy in larger open spaces, which cause larger errors (refer to .txt files). In open spaces, like the center of the map, multiple grid cells share similar expected sensor profiles, causing the belief to spread and confidence to drop slightly.

Even with independently noisy motion and sensor models, the robot’s combined belief converges reliably to a good estimate of true position.

The raw data for both runs can be downloaded here: Run 1 CSV | Run 2 CSV Note that the ground truth θ is cumulative/unwrapped from the simulator. The belief θ is normalized to [-180°, 180°).

Run 1 Results

Run 2 Results

Acknowledgments

I referred to Lucca Correial and Adian McNay’s lab reports from Spring 2025.