module 2:Mapping for Planning
module 2:不废话了直接上代码,之后再详细介绍代码
%%capture
# Intitialize figures.
map_fig = plt.figure()
map_ax = map_fig.add_subplot(111)
map_ax.set_xlim(0, N)
map_ax.set_ylim(0, M)
invmod_fig = plt.figure()
invmod_ax = invmod_fig.add_subplot(111)
invmod_ax.set_xlim(0, N)
invmod_ax.set_ylim(0, M)
belief_fig = plt.figure()
belief_ax = belief_fig.add_subplot(111)
belief_ax.set_xlim(0, N)
belief_ax.set_ylim(0, M)
meas_rs = []
meas_r = get_ranges(true_map, x[:, 0], meas_phi, rmax)
meas_rs.append(meas_r)
invmods = []
invmod = inverse_scanner(M, N, x[0, 0], x[1, 0], x[2, 0], meas_phi, meas_r, \
rmax, alpha, beta)
invmods.append(invmod)
ms = []
ms.append(m)
# Main simulation loop.
for t in range(1, len(time_steps)):
# Perform robot motion.
move = np.add(x[0:2, t-1], u[:, u_i])
# If we hit the map boundaries, or a collision would occur, remain still.
if (move[0] >= M - 1) or (move[1] >= N - 1) or (move[0] <= 0) or (move[1] <= 0) \
or true_map[int(round(move[0])), int(round(move[1]))] == 1:
x[:, t] = x[:, t-1]
u_i = (u_i + 1) % 4
else:
x[0:2, t] = move
x[2, t] = (x[2, t-1] + w[t]) % (2 * math.pi)
# TODO Gather the measurement range data, which we will convert to occupancy probabilities
# using our inverse measurement model.
#计算距离
meas_r = get_ranges(true_map, x[:, t], meas_phi, rmax)
meas_rs.append(meas_r)
# TODO Given our range measurements and our robot location, apply our inverse scanner model
# to get our measure probabilities of occupancy.
#计算雷达扫描角度模型inverse measurement model
invmod = inverse_scanner(M, N, x[0, t], x[1, t], x[2, t], meas_phi, meas_r, rmax, alpha, beta)
invmods.append(invmod)
# TODO Calculate and update the log odds of our occupancy grid, given our measured
# occupancy probabilities from the inverse model.
L = np.log(invmod / (1 - invmod)) + L - L0#更新迭代贝叶斯函数
# TODO Calculate a grid of probabilities from the log odds.
m = np.exp(L)/(np.exp(L) + 1)#将logit函数转换为概率问题,这个在PPT中有提到
ms.append(m)