Lab 8: Grid Localization using Bayes Filter
Objective
The purpose of this lab is to implement grid localization using Bayes Filter on the simulator.
Procedure
For localization, I implemented using the Bayesian filter code below and attached here. The bayesian filter mainly consists of the predict and update step. The predict step requires the likelihood that the "robot" is in a certain place given the previous location and the controls, which can be calculated in odom_motion_model and compute_control. The update step is updating the new position given the sensor measurements and the prediction, which is calculated in sensor_model and update_step.
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)
"""
# calculating translation
delta_x = cur_pose[0] - prev_pose[0] #change in position in the x direction
delta_y = cur_pose[1] - prev_pose[1] #change in position in the y direction
delta_trans = math.sqrt(delta_x**2 + delta_y**2) #distance between current pose and previous pose
# calculating rotation 1 and 2 and convert to degrees and normalizing the angle
delta_rot_1 = loc.mapper.normalize_angle((math.atan2(delta_y,delta_x)*180/np.pi) - cur_pose[2])
delta_rot_2 = loc.mapper.normalize_angle(cur_pose[2] - prev_pose[2] - delta_rot_1)
return delta_rot_1, delta_trans, delta_rot_2
# In world coordinates
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)
"""
#get controls given the previous and current pose
odom_motion = compute_control(cur_pose,prev_pose)
#comput the difference between the calculated controls and actual controls
x1 = u[0] - odom_motion[0] #error in rot1
x2 = u[1] - odom_motion[1] #error in trans
x3 = u[2] - odom_motion[2] #error in rot2
rot1_prob = loc.gaussian(x1,0,loc.odom_rot_sigma) #finding the probability that the measurement is accurate
trans_prob = loc.gaussian(x2,0,loc.odom_trans_sigma)
rot2_prob = loc.gaussian(x3,0,loc.odom_rot_sigma)
prob = rot1_prob*trans_prob*rot2_prob #multiplying all the probabilities assuming they are independent
return prob
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) #calculating controls given the final and previous positions
for prev_x in range(mapper.MAX_CELLS_X): #iterate through previous x
for prev_y in range(mapper.MAX_CELLS_Y): #iterate through previous y
for prev_theta in range(mapper.MAX_CELLS_A): #iterate through previous theta
if (loc.bel[prev_x][prev_y][prev_theta]>0.0001):
for cur_x in range(mapper.MAX_CELLS_X): #iterate through current x
for cur_y in range(mapper.MAX_CELLS_Y): #iterate through current y
for cur_theta in range(mapper.MAX_CELLS_A): #iterate through current theta
curr_pose = mapper.from_map(cur_x,cur_y,cur_theta) #getting the current pose from mapper
prev_pose = mapper.from_map(prev_x,prev_y,prev_theta) #getting the previous pose from mapper
#use the given grid pose make prediction for each grid
loc.bel_bar[cur_x][cur_y][cur_theta] += odom_motion_model(curr_pose,prev_pose, u) * loc.bel[prev_x][prev_y][prev_theta]
loc.bel_bar = loc.bel_bar/np.sum(loc.bel_bar)
def sensor_model(zt,xt):
""" This is the equivalent of p(z|x).
Args:
zt ([ndarray]): A 1D array consisting of the measurements made in rotation loop
xt are cell numbers (x,y,theta)
Returns:
[ndarray]: Returns a 1D array of size 18 (=loc.OBS_PER_CELL) with the likelihood of each individual measurements
"""
#initializing prob_array
(a,b,c) = xt
prob_array = np.zeros(mapper.OBS_PER_CELL)
#iterate through all the measurements
for i in range(mapper.OBS_PER_CELL):
#find the probability of measurement given data
prob_array[i] = loc.gaussian(zt[i],loc.mapper.get_views(a,b,c)[i],loc.sensor_sigma)
return prob_array
def update_step():
""" Update step of the Bayes Filter.
Update the probabilities in loc.bel based on loc.bel_bar and the sensor model.
Args:
zt ([ndarray]): A 1D array consisting of the measurements made in rotation loop
"""
for x in range(mapper.MAX_CELLS_X): #iterate through current x
for y in range(mapper.MAX_CELLS_Y): #iterate through current y
for a in range(mapper.MAX_CELLS_A):
loc.bel[x][y][a] = np.prod(sensor_model(loc.obs_range_data,(x,y,a)))*loc.bel_bar[x][y][a]
loc.bel = loc.bel / np.sum(loc.bel)
I started up the simulator and plotter to test the localization. First, I tested the trajectory using only odometry. In the graph below, green line indicates the global truth pose and the blue indicated odometry pose. The odometry can sometimes capture the overall trajectory, but is inaccurate in terms of the robot's location. In the video, you can see that the location is completely off the boundary limits.
After, I tested the same trajectory with the Bayes filter and got the trajectory below. The green is truth pose, blue is odometry and yellow is belief from Bayesian filter. As seen in the image, the trajectory is much more accurate than odometry because it takes in to consideration sensor measurements that helps estimate its location. The process can be seen in the video. The filter takes a while to process all the probabilities and sensor measurements, so it took about 11 mins total to make its trip around the map.
