github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Using scipy.spatial.rotation matrix (#335)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-06-07 20:28:29 +09:00
committed by GitHub
parent d1bde5835f
commit b8afdb10d8
26 changed files with 1002 additions and 931 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
Localization/histogram_filter/histogram_filter.py
View File

@@ -28,11 +28,11 @@ MOTION_STD = 1.0 # standard deviation for motion gaussian distribution
RANGE_STD = 3.0 # standard deviation for observation gaussian distribution
# grid map param
XY_RESO = 0.5 # xy grid resolution
MINX = -15.0
MINY = -5.0
MAXX = 15.0
MAXY = 25.0
XY_RESOLUTION = 0.5 # xy grid resolution
MIN_X = -15.0
MIN_Y = -5.0
MAX_X = 15.0
MAX_Y = 25.0
# simulation parameters
NOISE_RANGE = 2.0 # [m] 1σ range noise parameter
@@ -41,17 +41,17 @@ NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter
show_animation = True
class GridMap():
class GridMap:
def __init__(self):
self.data = None
self.xy_reso = None
self.minx = None
self.miny = None
self.maxx = None
self.maxx = None
self.xw = None
self.yw = None
self.xy_resolution = None
self.min_x = None
self.min_y = None
self.max_x = None
self.max_y = None
self.x_w = None
self.y_w = None
self.dx = 0.0 # movement distance
self.dy = 0.0 # movement distance
@@ -64,10 +64,10 @@ def histogram_filter_localization(grid_map, u, z, yaw):
return grid_map
def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
def calc_gaussian_observation_pdf(grid_map, z, iz, ix, iy, std):
# predicted range
x = ix * gmap.xy_reso + gmap.minx
y = iy * gmap.xy_reso + gmap.miny
x = ix * grid_map.xy_resolution + grid_map.min_x
y = iy * grid_map.xy_resolution + grid_map.min_y
d = math.hypot(x - z[iz, 1], y - z[iz, 2])
# likelihood
@@ -76,16 +76,16 @@ def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
return pdf
def observation_update(gmap, z, std):
def observation_update(grid_map, z, std):
for iz in range(z.shape[0]):
for ix in range(gmap.xw):
for iy in range(gmap.yw):
gmap.data[ix][iy] *= calc_gaussian_observation_pdf(
gmap, z, iz, ix, iy, std)
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] *= calc_gaussian_observation_pdf(
grid_map, z, iz, ix, iy, std)
gmap = normalize_probability(gmap)
grid_map = normalize_probability(grid_map)
return gmap
return grid_map
def calc_input():
@@ -112,8 +112,8 @@ def motion_model(x, u):
def draw_heat_map(data, mx, my):
maxp = max([max(igmap) for igmap in data])
plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.get_cmap("Blues"))
max_value = max([max(i_data) for i_data in data])
plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues"))
plt.axis("equal")
@@ -140,43 +140,47 @@ def observation(xTrue, u, RFID):
return xTrue, z, ud
def normalize_probability(gmap):
sump = sum([sum(igmap) for igmap in gmap.data])
def normalize_probability(grid_map):
sump = sum([sum(i_data) for i_data in grid_map.data])
for ix in range(gmap.xw):
for iy in range(gmap.yw):
gmap.data[ix][iy] /= sump
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] /= sump
return gmap
return grid_map
def init_gmap(xy_reso, minx, miny, maxx, maxy):
def init_grid_map(xy_resolution, min_x, min_y, max_x, max_y):
grid_map = GridMap()
grid_map.xy_reso = xy_reso
grid_map.minx = minx
grid_map.miny = miny
grid_map.maxx = maxx
grid_map.maxy = maxy
grid_map.xw = int(round((grid_map.maxx - grid_map.minx) / grid_map.xy_reso))
grid_map.yw = int(round((grid_map.maxy - grid_map.miny) / grid_map.xy_reso))
grid_map.xy_resolution = xy_resolution
grid_map.min_x = min_x
grid_map.min_y = min_y
grid_map.max_x = max_x
grid_map.max_y = max_y
grid_map.x_w = int(round((grid_map.max_x - grid_map.min_x)
/ grid_map.xy_resolution))
grid_map.y_w = int(round((grid_map.max_y - grid_map.min_y)
/ grid_map.xy_resolution))
grid_map.data = [[1.0 for _ in range(grid_map.yw)] for _ in range(grid_map.xw)]
grid_map.data = [[1.0 for _ in range(grid_map.y_w)]
for _ in range(grid_map.x_w)]
grid_map = normalize_probability(grid_map)
return grid_map
def map_shift(grid_map, x_shift, y_shift):
tgmap = copy.deepcopy(grid_map.data)
tmp_grid_map = copy.deepcopy(grid_map.data)
for ix in range(grid_map.xw):
for iy in range(grid_map.yw):
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
nix = ix + x_shift
niy = iy + y_shift
if 0 <= nix < grid_map.xw and 0 <= niy < grid_map.yw:
grid_map.data[ix + x_shift][iy + y_shift] = tgmap[ix][iy]
if 0 <= nix < grid_map.x_w and 0 <= niy < grid_map.y_w:
grid_map.data[ix + x_shift][iy + y_shift] =\
tmp_grid_map[ix][iy]
return grid_map
@@ -185,22 +189,26 @@ def motion_update(grid_map, u, yaw):
grid_map.dx += DT * math.cos(yaw) * u[0]
grid_map.dy += DT * math.sin(yaw) * u[0]
x_shift = grid_map.dx // grid_map.xy_reso
y_shift = grid_map.dy // grid_map.xy_reso
x_shift = grid_map.dx // grid_map.xy_resolution
y_shift = grid_map.dy // grid_map.xy_resolution
if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0: # map should be shifted
grid_map = map_shift(grid_map, int(x_shift), int(y_shift))
grid_map.dx -= x_shift * grid_map.xy_reso
grid_map.dy -= y_shift * grid_map.xy_reso
grid_map.dx -= x_shift * grid_map.xy_resolution
grid_map.dy -= y_shift * grid_map.xy_resolution
grid_map.data = gaussian_filter(grid_map.data, sigma=MOTION_STD)
return grid_map
def calc_grid_index(gmap):
mx, my = np.mgrid[slice(gmap.minx - gmap.xy_reso / 2.0, gmap.maxx + gmap.xy_reso / 2.0, gmap.xy_reso),
slice(gmap.miny - gmap.xy_reso / 2.0, gmap.maxy + gmap.xy_reso / 2.0, gmap.xy_reso)]
def calc_grid_index(grid_map):
mx, my = np.mgrid[slice(grid_map.min_x - grid_map.xy_resolution / 2.0,
grid_map.max_x + grid_map.xy_resolution / 2.0,
grid_map.xy_resolution),
slice(grid_map.min_y - grid_map.xy_resolution / 2.0,
grid_map.max_y + grid_map.xy_resolution / 2.0,
grid_map.xy_resolution)]
return mx, my
@@ -217,7 +225,7 @@ def main():
time = 0.0
xTrue = np.zeros((4, 1))
grid_map = init_gmap(XY_RESO, MINX, MINY, MAXX, MAXY)
grid_map = init_grid_map(XY_RESOLUTION, MIN_X, MIN_Y, MAX_X, MAX_Y)
mx, my = calc_grid_index(grid_map) # for grid map visualization
while SIM_TIME >= time:
@@ -234,8 +242,9 @@ def main():
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
draw_heat_map(grid_map.data, mx, my)
plt.plot(xTrue[0, :], xTrue[1, :], "xr")
plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")