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(