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

40
Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
View File

@@ -5,7 +5,8 @@ Ensemble Kalman Filter(EnKF) localization sample
author: Ryohei Sasaki(rsasaki0109)
Ref:
- [Ensemble Kalman filtering](https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
Ensemble Kalman filtering
(https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
"""
@@ -13,6 +14,7 @@ import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
# Simulation parameter
Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2
@@ -48,8 +50,8 @@ def observation(xTrue, xd, u, RFID):
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
anglen = angle + np.random.randn() * Q_sim[1, 1] ** 0.5 # add noise
zi = np.array([dn, anglen, RFID[i, 0], RFID[i, 1]])
angle_with_noise = angle + np.random.randn() * Q_sim[1, 1] ** 0.5
zi = np.array([dn, angle_with_noise, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noise to input
@@ -79,10 +81,12 @@ def motion_model(x, u):
def observe_landmark_position(x, landmarks):
landmarks_pos = np.zeros((2 * landmarks.shape[0], 1))
for (i, lm) in enumerate(landmarks):
landmarks_pos[2 * i] = x[0, 0] + lm[0] * math.cos(x[2, 0] + lm[1]) + np.random.randn() * Q_sim[
0, 0] ** 0.5 / np.sqrt(2)
landmarks_pos[2 * i + 1] = x[1, 0] + lm[0] * math.sin(x[2, 0] + lm[1]) + np.random.randn() * Q_sim[
0, 0] ** 0.5 / np.sqrt(2)
index = 2 * i
q = Q_sim[0, 0] ** 0.5
landmarks_pos[index] = x[0, 0] + lm[0] * math.cos(
x[2, 0] + lm[1]) + np.random.randn() * q / np.sqrt(2)
landmarks_pos[index + 1] = x[1, 0] + lm[0] * math.sin(
x[2, 0] + lm[1]) + np.random.randn() * q / np.sqrt(2)
return landmarks_pos
@@ -148,8 +152,9 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative numbers extremely
# close to 0 (~10^-20), catch these cases and set the respective variable to 0
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
# numbers extremely close to 0 (~10^-20), catch these cases and set
# the respective variable to 0
try:
a = math.sqrt(eig_val[big_ind])
except ValueError:
@@ -163,11 +168,11 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0])
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
fx = np.stack([x, y]).T @ rot
px = np.array(fx[:, 0] + xEst[0, 0]).flatten()
py = np.array(fx[:, 1] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
@@ -214,8 +219,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])
for i in range(len(z[:, 0])):
plt.plot([xTrue[0, 0], z[i, 2]], [xTrue[1, 0], z[i, 3]], "-k")
@@ -227,7 +233,7 @@ def main():
np.array(hxDR[1, :]).flatten(), "-k")
plt.plot(np.array(hxEst[0, :]).flatten(),
np.array(hxEst[1, :]).flatten(), "-r")
# plot_covariance_ellipse(xEst, PEst)
plot_covariance_ellipse(xEst, PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)

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")

16
Localization/particle_filter/particle_filter.py
View File

@@ -10,6 +10,7 @@ import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
# Estimation parameter of PF
Q = np.diag([0.2]) ** 2 # range error
@@ -172,8 +173,9 @@ def plot_covariance_ellipse(x_est, p_est): # pragma: no cover
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative numbers extremely
# close to 0 (~10^-20), catch these cases and set the respective variable to 0
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
# numbers extremely close to 0 (~10^-20), catch these cases and set the
# respective variable to 0
try:
a = math.sqrt(eig_val[big_ind])
except ValueError:
@@ -187,9 +189,8 @@ def plot_covariance_ellipse(x_est, p_est): # pragma: no cover
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0])
Rot = np.array([[math.cos(angle), -math.sin(angle)],
[math.sin(angle), math.cos(angle)]])
fx = Rot.dot(np.array([[x, y]]))
rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
fx = rot.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + x_est[0, 0]).flatten()
py = np.array(fx[1, :] + x_est[1, 0]).flatten()
plt.plot(px, py, "--r")
@@ -235,8 +236,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])
for i in range(len(z[:, 0])):
plt.plot([x_true[0, 0], z[i, 1]], [x_true[1, 0], z[i, 2]], "-k")