Lab 11

Grid Localization using Bayes Filter

Objective

The purpose of this lab is to implement grid localization using Bayes filter.

Prelab

Downloaded the lab11 notebook and copied it into the notebooks directory as shown below.

compute_control

This function basically computes the translation from A to B, both before and after the action, every translation takes three arguments, x and y coordinates, and orientation info. Below is the theory in graph.

My implementation is based on the following formulas, using the python math library.

1
2
3
4
5
6
def compute_control(cur_pose, prev_pose):
    delta_rot_1 = math.degrees(math.atan2((cur_pose[1] - prev_pose[1]), (cur_pose[0] - prev_pose[0]))) - prev_pose[2]
    delta_trans = math.hypot((cur_pose[0] - prev_pose[0]), (cur_pose[1] - prev_pose[1]))
    delta_rot_2 = cur_pose[2] - prev_pose[2] - delta_rot_1
    
    return delta_rot_1, delta_trans, delta_rot_2

odom_motion_model

This function is to calculate the probability between the current position and the previous position. There are three parameters, cur_pose, prev_pose and u. The goal is to calculate a u_bar from set of cur_pose and prev_pose. The reason of why need to normalize angle is because the state space for orientation is from -180 deg to +180 deg.

The implementation is based on formulas below.

1
2
3
4
5
6
7
8
9
def odom_motion_model(cur_pose, prev_pose, u):
    p_u = compute_control(cur_pose, prev_pose)   
    p_rot1 = loc.gaussian(mapper.normalize_angle(p_u[0]-u[0]), 0, loc.odom_rot_sigma)
    p_trans = loc.gaussian(mapper.normalize_angle(p_u[1]-u[1]), 0, loc.odom_trans_sigma)
    p_rot2 = loc.gaussian(mapper.normalize_angle(p_u[2]-u[2]), 0, loc.odom_rot_sigma)

    prob = p_rot1 * p_trans * p_rot2
    
    return prob

prediction_step

Here need six nested loops, 3 of them for sum all current poses, and rest of 3 for previous poses. In theory, the worst case time complexity is O(n^6), which is severe. To make a purge optimization, set the belief threshold to 0.0001 for the bel, then the best case time complexity will be O(n^3) ,but the same time complexity for the worst case. At the end, normalize the table by using the division.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
def prediction_step(cur_odom, prev_odom):
    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 prev_x in range(0, mapper.MAX_CELLS_X):
        for prev_y in range(0, mapper.MAX_CELLS_Y):
            for prev_a in range(0, mapper.MAX_CELLS_A):
                if (loc.bel[prev_x, prev_y, prev_a] > 0.0001):
                    for curr_x in range(0, mapper.MAX_CELLS_X):
                        for curr_y in range(0, mapper.MAX_CELLS_Y):
                            for curr_a in range(0, mapper.MAX_CELLS_A):
                                prev_pose = mapper.from_map(prev_x, prev_y, prev_a)
                                curr_pose = mapper.from_map(curr_x, curr_y, curr_a)
                                loc.bel_bar[curr_x, curr_y, curr_a] += (odom_motion_model(curr_pose, prev_pose, u) * loc.bel[prev_x, prev_y, prev_a])
                                
    loc.bel_bar /= np.sum(loc.bel_bar)

sensor_model

This function calculates the probability of ToF data correctness. Compare the ideal and the actual result, and use the gaussian distribution to correct poses.

Implementation would be pretty straightforward, declare a variable and initialize it as a string object, then loop through 18 positions and append the result.

1
2
3
4
5
6
7
def sensor_model(obs):
    prob = []
    
    for i in range(0, mapper.OBS_PER_CELL):
        prob.append(loc.gaussian(loc.obs_range_data[i], obs[i], loc.sensor_sigma))

    return prob

update_step

The update step is 3 nested loops, walk through all possible current poses and get the belief bar data, use division to normalize it at the end.

1
2
3
4
5
6
7
def update_step():
    for curr_x in range(mapper.MAX_CELLS_X):
        for curr_y in range(mapper.MAX_CELLS_Y):
            for curr_a in range(mapper.MAX_CELLS_A):
                loc.bel[curr_x, curr_y, curr_a] = np.prod(sensor_model(mapper.get_views(curr_x, curr_y, curr_a))) * loc.bel_bar[curr_x, curr_y, curr_a]
                
    loc.bel /= np.sum(loc.bel)

Without bel_bar (Click here for Video Demo)

With bel_bar (Click here for Video Demo)

Credits

Collaborate with Tongqing Zhang (TZ422) and Lu Vincent (YL929).