github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 16:47:55 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/AtsushiSakai/PythonRobotics

Browse Source
This commit is contained in:
Göktuğ Karakaşlı
2019-10-12 14:40:53 +03:00
parent 8fb4b8a564 0cca27df3a
commit 07f07814a2
3 changed files with 60 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
Localization/particle_filter/particle_filter.py
View File

@@ -6,17 +6,18 @@ author: Atsushi Sakai (@Atsushi_twi)
"""
import numpy as np
import math
import matplotlib.pyplot as plt
import numpy as np
# Estimation parameter of PF
Q = np.diag([0.1])**2 # range error
R = np.diag([1.0, np.deg2rad(40.0)])**2 # input error
Q = np.diag([0.2]) ** 2 # range error
R = np.diag([2.0, np.deg2rad(40.0)]) ** 2 # input error
# Simulation parameter
Qsim = np.diag([0.2])**2
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
Q_sim = np.diag([0.2]) ** 2
R_sim = np.diag([1.0, np.deg2rad(30.0)]) ** 2
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
@@ -31,31 +32,30 @@ show_animation = True
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v, yawrate]]).T
yaw_rate = 0.1 # [rad/s]
u = np.array([[v, yaw_rate]]).T
return u
def observation(xTrue, xd, u, RFID):
def observation(xTrue, xd, u, RF_ID):
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
z = np.zeros((0, 3))
for i in range(len(RFID[:, 0])):
for i in range(len(RF_ID[:, 0])):
dx = xTrue[0, 0] - RFID[i, 0]
dy = xTrue[1, 0] - RFID[i, 1]
d = math.sqrt(dx**2 + dy**2)
dx = xTrue[0, 0] - RF_ID[i, 0]
dy = xTrue[1, 0] - RF_ID[i, 1]
d = math.sqrt(dx ** 2 + dy ** 2)
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] # add noise
zi = np.array([[dn, RFID[i, 0], RFID[i, 1]]])
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
zi = np.array([[dn, RF_ID[i, 0], RF_ID[i, 1]]])
z = np.vstack((z, zi))
# add noise to input
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
ud = np.array([[ud1, ud2]]).T
xd = motion_model(xd, ud)
@@ -64,7 +64,6 @@ def observation(xTrue, xd, u, RFID):
def motion_model(x, u):
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
@@ -93,11 +92,12 @@ def calc_covariance(xEst, px, pw):
for i in range(px.shape[1]):
dx = (px[:, i] - xEst)[0:3]
cov += pw[0, i] * dx.dot(dx.T)
cov /= NP
return cov
def pf_localization(px, pw, xEst, PEst, z, u):
def pf_localization(px, pw, z, u):
"""
Localization with Particle filter
"""
@@ -105,9 +105,10 @@ def pf_localization(px, pw, xEst, PEst, z, u):
for ip in range(NP):
x = np.array([px[:, ip]]).T
w = pw[0, ip]
# Predict with random input sampling
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
ud1 = u[0, 0] + np.random.randn() * R[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R[1, 1] ** 0.5
ud = np.array([[ud1, ud2]]).T
x = motion_model(x, ud)
@@ -115,8 +116,8 @@ def pf_localization(px, pw, xEst, PEst, z, u):
for i in range(len(z[:, 0])):
dx = x[0, 0] - z[i, 1]
dy = x[1, 0] - z[i, 2]
prez = math.sqrt(dx**2 + dy**2)
dz = prez - z[i, 0]
pre_z = math.sqrt(dx ** 2 + dy ** 2)
dz = pre_z - z[i, 0]
w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
px[:, ip] = x[:, 0]
@@ -127,30 +128,30 @@ def pf_localization(px, pw, xEst, PEst, z, u):
xEst = px.dot(pw.T)
PEst = calc_covariance(xEst, px, pw)
px, pw = resampling(px, pw)
px, pw = re_sampling(px, pw)
return xEst, PEst, px, pw
def resampling(px, pw):
def re_sampling(px, pw):
"""
low variance re-sampling
"""
Neff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
if Neff < NTh:
wcum = np.cumsum(pw)
N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
if N_eff < NTh:
w_cum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
resampleid = base + np.random.rand(base.shape[0]) / NP
re_sample_id = base + np.random.rand(base.shape[0]) / NP
inds = []
indexes = []
ind = 0
for ip in range(NP):
while resampleid[ip] > wcum[ind]:
while re_sample_id[ip] > w_cum[ind]:
ind += 1
inds.append(ind)
indexes.append(ind)
px = px[:, inds]
px = px[:, indexes]
pw = np.zeros((1, NP)) + 1.0 / NP # init weight
return px, pw
@@ -158,35 +159,35 @@ def resampling(px, pw):
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)
eig_val, eig_vec = np.linalg.eig(Pxy)
if eigval[0] >= eigval[1]:
bigind = 0
smallind = 1
if eig_val[0] >= eig_val[1]:
big_ind = 0
small_ind = 1
else:
bigind = 1
smallind = 0
big_ind = 1
small_ind = 0
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
# eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
# 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(eigval[bigind])
a = math.sqrt(eig_val[big_ind])
except ValueError:
a = 0
try:
b = math.sqrt(eigval[smallind])
b = math.sqrt(eig_val[small_ind])
except ValueError:
b = 0
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R.dot(np.array([[x, y]]))
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]]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
@@ -197,16 +198,15 @@ def main():
time = 0.0
# RFID positions [x, y]
RFID = np.array([[10.0, 0.0],
[10.0, 10.0],
[0.0, 15.0],
[-5.0, 20.0]])
# RF_ID positions [x, y]
RFi_ID = np.array([[10.0, 0.0],
[10.0, 10.0],
[0.0, 15.0],
[-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.zeros((4, 1))
xTrue = np.zeros((4, 1))
PEst = np.eye(4)
px = np.zeros((4, NP)) # Particle store
pw = np.zeros((1, NP)) + 1.0 / NP # Particle weight
@@ -221,9 +221,9 @@ def main():
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFi_ID)
xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud)
xEst, PEst, px, pw = pf_localization(px, pw, z, ud)
# store data history
hxEst = np.hstack((hxEst, xEst))
@@ -235,7 +235,7 @@ def main():
for i in range(len(z[:, 0])):
plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(RFi_ID[:, 0], RFi_ID[:, 1], "*k")
plt.plot(px[0, :], px[1, :], ".r")
plt.plot(np.array(hxTrue[0, :]).flatten(),
np.array(hxTrue[1, :]).flatten(), "-b")

3
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -155,7 +155,8 @@ def calc_to_goal_cost(traj, goal, config):
dx = goal[0] - traj[-1, 0]
dy = goal[1] - traj[-1, 1]
error_angle = math.atan2(dy, dx)
cost = abs(error_angle - traj[-1, 2])
cost_angle = error_angle - traj[-1, 2]
cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))
return cost

3
users_comments.md
View File

@@ -401,6 +401,9 @@ URL: https://2019.robocup.org/downloads/program/HughesEtAl2019.pdf
6. Hughes, Josie, Masaru Shimizu, and Arnoud Visser. "A review of robot rescue simulation platforms for robotics education." (2019).
URL: https://www.semanticscholar.org/paper/A-Review-of-Robot-Rescue-Simulation-Platforms-for-Hughes-Shimizu/318a4bcb97a44661422ae1430d950efc408097da
7. Ghosh, Ritwika, et al. "CyPhyHouse: A Programming, Simulation, and Deployment Toolchain for Heterogeneous Distributed Coordination." arXiv preprint arXiv:1910.01557 (2019).
URL: https://arxiv.org/abs/1910.01557
# Others
- Autonomous Vehicle Readings https://richardkelley.io/readings