From 0d596ebba0ec9124e10acdd54649fe8df24b2265 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 12 Mar 2018 13:55:18 -0700 Subject: [PATCH] there is a bug ... --- SLAM/FastSLAM/fast_slam.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/SLAM/FastSLAM/fast_slam.py b/SLAM/FastSLAM/fast_slam.py index 63f57bc7..e832057e 100644 --- a/SLAM/FastSLAM/fast_slam.py +++ b/SLAM/FastSLAM/fast_slam.py @@ -16,7 +16,7 @@ Cx = np.diag([0.5, 0.5, math.radians(30.0)])**2 # Simulation parameter -Qsim = np.diag([0.2, math.radians(1.0)])**2 +Qsim = np.diag([0.0, math.radians(0.0)])**2 Rsim = np.diag([1.0, math.radians(10.0)])**2 DT = 0.1 # time tick [s] @@ -100,6 +100,8 @@ def add_new_lm(particle, z): particle.lm[lm_id, 0] = particle.x + r * c particle.lm[lm_id, 1] = particle.y + r * s + # print(particle.lm) + # print(lm_id) # covariance Gz = np.matrix([[c, -r * s], @@ -110,11 +112,17 @@ def add_new_lm(particle, z): return particle +def feature_update(particle, z, R): + + return particle + + def compute_weight(particle, z): lm_id = int(z[0, 2]) lmxy = np.matrix(particle.lm[lm_id, :]) + print(lmxy) # calc landmark xy r = z[0, 0] @@ -154,7 +162,8 @@ def update_with_observation(particles, z): else: w = compute_weight(particles[ip], z[iz, :]) # w = p(z_k | x_k) particles[ip].w = particles[ip].w + w - # particles(i)= feature_update(particles(i), zf, idf, R) + particles[ip] = feature_update( + particles[ip], z[iz, :], Cx[0:2, 0:2]) return particles