From b942c430e6df8d854fe00f9a264efaca94cc90c9 Mon Sep 17 00:00:00 2001 From: sszwfy <476020374@qq.com> Date: Sun, 6 Oct 2019 14:32:22 +0800 Subject: [PATCH 1/4] fix error for goal cost angle --- PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index fa1dcc62..a7d74332 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -167,7 +167,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 From 0c5bb95761c48bfe976395b350cce317e76ff838 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Wed, 9 Oct 2019 19:42:51 +0900 Subject: [PATCH 2/4] Update users_comments.md --- users_comments.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/users_comments.md b/users_comments.md index b1a0b66f..2cfd7a4a 100644 --- a/users_comments.md +++ b/users_comments.md @@ -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 From 415bf654f1992cf108eb761eb474b55892cef47d Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Fri, 11 Oct 2019 20:13:27 +0900 Subject: [PATCH 3/4] Fix #229 --- .../particle_filter/particle_filter.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 8e93893e..a3dd6bcd 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -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.1]) ** 2 # range error +R = np.diag([1.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 +Qsim = np.diag([0.2]) ** 2 +Rsim = np.diag([1.0, np.deg2rad(30.0)]) ** 2 DT = 0.1 # time tick [s] SIM_TIME = 50.0 # simulation time [s] @@ -37,7 +38,6 @@ def calc_input(): def observation(xTrue, xd, u, RFID): - xTrue = motion_model(xTrue, u) # add noise to gps x-y @@ -47,7 +47,7 @@ def observation(xTrue, xd, u, RFID): dx = xTrue[0, 0] - RFID[i, 0] dy = xTrue[1, 0] - RFID[i, 1] - d = math.sqrt(dx**2 + dy**2) + 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]]]) @@ -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], @@ -97,7 +96,7 @@ def calc_covariance(xEst, px, pw): return cov -def pf_localization(px, pw, xEst, PEst, z, u): +def pf_localization(px, pw, z, u): """ Localization with Particle filter """ @@ -105,6 +104,7 @@ 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] @@ -115,8 +115,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] @@ -223,7 +223,7 @@ def main(): xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID) - 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)) From 85bf664a9166d9cb7f9463a801931ceb3af5dec9 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Fri, 11 Oct 2019 20:49:07 +0900 Subject: [PATCH 4/4] fix PF problems --- .../particle_filter/particle_filter.py | 94 +++++++++---------- 1 file changed, 47 insertions(+), 47 deletions(-) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index a3dd6bcd..6643116f 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -12,12 +12,12 @@ 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] @@ -32,30 +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] + 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) @@ -92,6 +92,7 @@ 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 @@ -106,8 +107,8 @@ def pf_localization(px, pw, z, u): 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) @@ -127,30 +128,30 @@ def pf_localization(px, pw, 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,7 +221,7 @@ 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, z, ud) @@ -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")