Grid Localization

Bayes Filtering

In the context of grid localization, Bayes Filter is used for estimating the robot's position on a grid during a simulation. Bayes filter takes in control inputs, sensor data, and the robot's prior belief to estimate the robot's current position.

The robot's space is broken into a predefined grid, and in each step, the Bayes Filter is used to determine the likelihood of the robot being in each cell using the control inputs, sensor data, and prior belief.

Then, the Bayes Filter predicts where the robot will be next using the prior position and expected results of the control inputs.

Finally, the Bayes Filter updates the robot's position using the results from the sensor readings, reducing the gaussian spread of the robot's estimate of its own position.

Helper Functions

compute_control

compute_control() takes in the current and previous positions of the robot in cartesian coordinates, and returns the difference in position from the reference frame of the robot's orientation and travel.

odom_motion_model

odom_motion_model() takes in the current position, previous position, and control input to the robot to compute the probability that the robot moved from prev_pose to cur_pose. This is used to figure out the likelihood that the robot will be in some position in the future.

Is does this by using compute_control() to find the robot's motion. Then, it relates the actual movements to the expected movements based on the control input to find how well they line up. The better the values line up, the higher the probability that the robot is in that location.

prediction_step

prediction_step is the most compute-expensive part of the the Bayes Filter because it iterates through every point in the robot's state space and calculates the odom_motion_model from every point to every other point. It then uses these probabilities to update the robot's belief model.

sensor_model

sensor_model simulates the sensor readings by just taking the calculated readings between the robot and the wall along the robot's line of sight and adding a bit of gaussian noise to the reading. This simulates an imperfect sensor.

update_step

update_step iterates through all the points in the robot's state space, and uses the sensor readings from sensor_model to update its belief of how likely the robot is at any particular position. This method is the feedback step from the sensors. Without it, the robot's position estimate would just spread out over time.

Bayes Filtering Demo

Here's a video of the helper functions I wrote stepping through the room's space. You can see that the cyan line, the robot's estimate of its location, follows the green line quite well, even though the odometer estimate is running far away from the robot's ground truth.