Bayes Filter

The Bayes Filter is used to estimate the state of the robot based on odometry and sensor data and prior belief of the state. We used this filter to simulate localization of the robot. The first step of the Bayes filter is prediction. This step uses odometry data and the current state. The next step is the update step which uses the sensor data. The lab10 notebook that was provided to us set up the filter for us using 5 functions. In this lab we wrote those functions and simulated grid localization for the given sample trajectory. The 5 functions are detailed below.

COMPUTE CONTROL

This function uses the current and previous state of the robot to calculate the control input of the Bayes FIlter. The control input consists of rotation and translation components.

ODOMETRY MOTION MODEL

This function uses a gaussian distribution to calculate the probability that the movement in the previous step occurred i.e the probability that the robot is in the current pose give the previous pose and the control input.

PREDICTION STEP

This function was used to predict the position of the robot. We loop through all the previous and current states and calculate the probability that the robot is in each of those states. If the previous belief was greater than 0.0001 the odometry_motion_model function was used to calculate the probability, if it was less than 0.0001 no calculations are done because the state does not need to be considered as it is not likely.

SENSOR MODEL

This function uses a Gaussian to calculate the probability of sensor readings given the robots state. For the Gaussian, the true observation is set as the mean and we have a value sensor_sigma which is the standard deviation. An array is returned containing the likelihoods of the sensor measurements.

UPDATE STEP

In this function we use the previously calculated sensor measurements and control inputs to calculate the belief that the robot is in each state. In each state we calculate the relief by multiplying the prediction belief and the probability based on the sensor data.

Video