Lab 10 Bayes Filter Simulation
The goal of this lab was to use Bayes Filter and odometry to implement grid localization..
About the Bayes Filter
The Bayes filter takes in control input, sensor measurements, and "belief" of where the robot is to determines the robot position on a pre-determined grid. For any given location, there is a probability that the robot is actually there based on it's previous location as well as the ideal control action it would take to get to said new location. Similar to the Kalman filter (which, if anything the KF is a subset of Bayes), the Bayes filter has both a prediction step and an update step once data has come in (prior vs posterior).
Compute_Control
The robots motions/actions are predicted using odometry. This essentially splits any movement into the sum of three smaller movements. First, a rotation of the robot in the direction of its final destination. Next, a translation to that new location. And lastly, a second rotation to correct the robots orientation. We calculate this with the function compute_control, which takes in the robots current and previous poses in as an input. Each pose is actually a tuplet of (x,y,theta).
| 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_cur = cur_pose[0] | |
| y_cur = cur_pose[1] | |
| ang_cur = cur_pose[2] * (math.pi / 180) | |
| x_prev = prev_pose[0] | |
| y_prev = prev_pose[1] | |
| ang_prev = prev_pose[2] * (math.pi / 180) | |
| r = math.sqrt((x_cur-x_prev)**2 + (y_cur-y_prev)**2) | |
| theta = math.atan2((x_cur-x_prev), (y_cur-y_prev)) | |
| delta_rot_1 = (theta - ang_prev) * (180/math.pi) | |
| delta_trans = r | |
| delta_rot_2 = (ang_cur - ang_prev - delta_rot_1) * (180/math.pi) | |
| return delta_rot_1, delta_trans, delta_rot_2 |
Odom_Motion_Model
The above function just calculated the three relative motions needed to go between two points. For the actual odometry motion model, two possible paths must be compared: the "actual" action the robot took, based on odometry readings, versus the "ideal" motions the robot would take based on it's previous states. For this code/model, we treat the ideal state as a Gaussian distribution, and compare the actual actions taken to determine a probability p(x'|x, u). Essentially, the ideal location of the robot x, given control u, is the mean or center of the gaussian distribution. We can then check the probability that the robot is actually at location x', and do this for each movement (rotation 1, translation, rotation 2). We assume each movement is an independent event.
| 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) | |
| """ | |
| #odometry readings | |
| delta_rot_1, delta_trans, delta_rot_2 = compute_control(cur_pose, prev_pose) | |
| delta_rot_1 = mapper.normalize_angle(delta_rot_1) | |
| delta_rot_2 = mapper.normalize_angle(delta_rot_2) | |
| #ideal | |
| delta_rot_1_hat, delta_trans_hat, delta_rot_2_hat = u | |
| delta_rot_1_hat = mapper.normalize_angle(delta_rot_1_hat) | |
| delta_rot_2_hat = mapper.normalize_angle(delta_rot_2_hat) | |
| #compute probs | |
| p1 = loc.gaussian(delta_rot_1, delta_rot_1_hat, loc.odom_rot_sigma) | |
| p2 = loc.gaussian(delta_trans, delta_trans_hat, loc.odom_trans_sigma) | |
| p3 = loc.gaussian(delta_rot_2, delta_rot_2_hat, loc.odom_rot_sigma) | |
| prob = p1 * p2 * p3 | |
| return prob |
Prediction_Step
The prediction step goes through every possible previous robot state, x_(t-1), and figures out the probability that the robot will be in some current state x_t, given the belief the robot was in the state x_(t-1) and a control action. We must check all 12 x grid locations, all 9 y locations, and all 18 discretized angles. The result of the prediction step is a new belief, bel_bar, which represents the predicted uncertainty that the robot is in the new location x_t. This function get's computationally expensive because it has to loop through every single location, so I added an if statement to ignore any previous location x_(t-1) with a belief of less than 0.0001. I increased this up to 0.001 and I think it did make my filter slightly faster.
| 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) | |
| cells_X, cells_Y, cells_A = mapper.MAX_CELLS_X, mapper.MAX_CELLS_Y, mapper.MAX_CELLS_A | |
| temp = np.zeros((cells_X, cells_Y, cells_A)) | |
| for i in range(cells_X): | |
| for j in range(cells_Y): | |
| for k in range(cells_A): | |
| if(loc.bel[i,j,k] > .0001): | |
| for a in range(cells_X): | |
| for b in range(cells_Y): | |
| for c in range(cells_A): | |
| cur_pose = mapper.from_map(a,b,c) | |
| prev_pose = mapper.from_map (i,j,k) | |
| prob = odom_motion_model(cur_pose, prev_pose, u) | |
| belief = loc.bel[i,j,k] | |
| temp += prob * belief | |
| sum_val = np.sum(temp) | |
| loc.bel_bar = np.true_divide(temp, sum_val) ##normalized bel_bar |
Sensor_Model
Next came the sensor model, which defines the probability p(z|x). This function defines a gaussian distribution centered around the true observation and a predefined variance, loc.sensor_sigma. For 18 measurements (robot spins in a circle) it determines the likelihood of a specific measurement given the current state.
| 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 = [] | |
| for i in range(18): | |
| #create a gaussian distribuion for each meas | |
| gauss = loc.gaussian(loc.obs_range_data[i], obs[i], loc.sensor_sigma) | |
| prob_array.append(gauss) | |
| return prob_array |
Update_Step
Finally came the update step. It takes the predicted belief, bel_bar, and updates it based on sensor values. To do this calculation you need to loop through all possible locations and run the sensor model function to determine the probability of getting the current measurement, and multiply it by the belief that you are at that grid location. After this, the final belief can be normalized and used to estimate position.
| def update_step(): | |
| """ Update step of the Bayes Filter. | |
| Update the probabilities in loc.bel based on loc.bel_bar and the sensor model. | |
| """ | |
| cells_X, cells_Y, cells_A = mapper.MAX_CELLS_X, mapper.MAX_CELLS_Y, mapper.MAX_CELLS_A | |
| for i in range(cells_X): | |
| for j in range(cells_Y): | |
| for k in range(cells_A): | |
| p = sensor_model(mapper.get_views(i,j,k)) | |
| prob = np.prod(p) | |
| loc.bel[i,j,k] = loc.bel_bar[i,j,k] * prob | |
| loc.bel = loc.bel / np.sum(loc.bel) |
Simulation
Results are shown below.
The odometry model alone (red) is pretty terrible/inaccurate. However comparing the belief (blue) with the true location (green) shows that despite how bad the odometry is, the ultimate prediction from the Bayes filter converges pretty quickly and is quite accurate. It was most inaccurate atthe top right corner of the map, but was still able to correct itself and converge back very fast. It is also slightly less accurate at the center/origin of the grid, maybe due to symmetry. Overall, though, I think it performed very well.
Discussion
The Bayes filter is very powerful. It has a similar structure to the Kalman filter, but is more forgiving with the dynamics of the system since it does not require linear gaussian systems. I was able to simulate/estimate robot precision with high accuracy even with a fairly inaccurate odometry model, which shows how good the Bayes filter can be!