The Bayes filter estimates the state of the robot based on the prior belief about the state along with odometry and sensor data. This is done in two steps: First, the prediction step calculates the predicted state distribution given the current state and odometry input. Next, in the update step, sensor measurements are incorporated into the system by calculating the likelihood of said measurements given each state. The combination of the prediction and update steps outputs the posterior state distribution and thus the most probable state of the robot. The algorithm is broken down into the five sections below for simplicity of implementation. I referred to Anya Prabowo's website for the Python code used in this lab.

The compute_control() function takes the robot's current and previous positions as input and outputs u, which represents the control input in the Bayes filter algorithm. u is a tuple that consists of the rotation 1, translation, and rotation 2 components which can be calculated using the equations below from class:

The odom_motion_model() function calculates the state transition probability, which is the likelihood that the robot is in the curr_pose given the prev_pose and control input u. The equation for transition probability is given below as the product of three gaussians, each of which takes mu, x, and sigma values and outputs the relative likelihood of x being in a Normal Distribution with mean mu and standard deviation sigma.

This step updates the bel_bar probability based on the cur_odom and prev_odom inputs. First, the control input is updated by calling compute_control(). Next, I loop through the previous and current states and call odom_motion_model() to calculate the probability of being in each state. I then multiplied the probability of each state by the previous belief at that state and normalized the distribution so sum to 1. If the previous belief for any state is under a threshold of 0.0001, the calculations for that state are negligable and thus skipped to increase efficiency.

The sensor_model() function calculates the probability of obtaining certain sensor readings given the state of the robot. It does this by calculating a Gaussian using the true observation (since this is a simulation, we know what the robot should sense) as the mean and sensor_sigma as the standard deviation. The function returns an array of the calculated results for the measurement, which consists of 18 data points.

In the update step, the belief that the robot is in each state is calculated given all past measurements and control inputs. I iterate through each potential current position and multiply each state's belief by the probability of obtaining the current sensor readings given the said position. The belief value for each pose is updated accordingly and normalized so that the total probability is 1.

Here are the simulations of the robot's localization while moving in a predetermined trajectory and the corresponding output. The green line shows the robot's actual ground truth position, the red line is estimation based on odometry/sensor values, and the blue line is the Bayes filter estimation of the position.