y_range = np.arange(-0.2, 0.2 + 0.05, 0.05)
print("Testing mapCorrelation...")
# INPUT
# MAP.map the map
# x_im,y_im physical x,y positions of the grid map cells
# Y(1:3,:) occupied x,y positions from range sensor (in physical unit)
# x_range,y_range physical x,y,positions you want to evaluate "correlation"
#
# OUTPUT
# c sum of the cell values of all the positions hit by range sensor
#
# mapCorrelation():
# (im: List[List[int8]], x_im: List[float], y_im: List[float], vp: List[List[float]], xs: List[float], ys: List[float]) -> List[List[float]]
c = MU.mapCorrelation(MAP['map'], x_im, y_im, Y[0:2, :], x_range, y_range)
c_ex = np.array([[3, 4, 8, 162, 270, 132, 18, 1, 0],
[25, 1, 8, 201, 307, 109, 5, 1, 3],
[314, 198, 91, 263, 366, 73, 5, 6, 6],
[130, 267, 360, 660, 606, 87, 17, 15, 9],
[17, 28, 95, 618, 668, 370, 271, 136, 30],
[9, 10, 64, 404, 229, 90, 205, 308, 323],
[5, 16, 101, 360, 152, 5, 1, 24, 102],
[7, 30, 131, 309, 105, 8, 4, 4, 2],
[16, 55, 138, 274, 75, 11, 6, 6, 3]])
if np.sum(c == c_ex) == np.size(c_ex):
print("...Test passed.")
else:
print("...Test failed. Close figures to continue tests.")
noise = np.einsum('..., ...', factor, np.random.normal(0, 1e-3, (N, 1)))
particles = particles + ut + noise
scan_c = lid_c['scan']
ind_i = np.argmin(np.absolute(ts - lid_c['t'][0][0]))
pos_phy, posX_map, posY_map = mapConvert(scan_c, rpy_robot[:, ind_i],
h_angle[:, ind_i], angles,
particles, N, pos_phy, posX_map,
posY_map, mapfig)
corr = np.zeros((N, 1))
for i in range(N):
size = pos_phy[i].shape[1]
Y = np.concatenate([pos_phy[i], np.zeros((1, size))], axis=0)
corr_cur = maput.mapCorrelation(mapfig['map'], x_im, y_im, Y[0:3, :],
x_range, y_range)
ind = np.argmax(corr_cur)
corr[i] = corr_cur[ind / 3, ind % 3]
particles[i, 0] += x_range[ind / 3]
particles[i, 1] += y_range[ind % 3]
wtmp = np.log(weight) + corr
wtmp_max = wtmp[np.argmax(wtmp)]
lse = np.log(np.sum(np.exp(wtmp - wtmp_max)))
wtmp = wtmp - wtmp_max - lse
weight = np.exp(wtmp)
ind_best = weight.argmax()
x_r = (np.ceil(