From c9a9e48f72be83e8bc70cb2f7d89ccc73cf7bd29 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 2 Dec 2018 21:58:26 +0900 Subject: [PATCH] remove np.matrix --- SLAM/FastSLAM1/fast_slam1.py | 3 +- SLAM/FastSLAM2/fast_slam2.py | 155 +++++++++++++++++------------------ 2 files changed, 77 insertions(+), 81 deletions(-) diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index 528fe790..08f4db6a 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -132,7 +132,8 @@ def compute_jacobians(particle, xf, Pf, Q): d2 = dx**2 + dy**2 d = math.sqrt(d2) - zp = np.array([[d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]]).T + zp = np.array( + [d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]).reshape(2, 1) Hv = np.array([[-dx / d, -dy / d, 0.0], [dy / d2, -dx / d2, -1.0]]) diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index 16628975..2fcde3d9 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -27,7 +27,7 @@ M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association. STATE_SIZE = 3 # State size [x,y,yaw] LM_SIZE = 2 # LM srate size [x,y] N_PARTICLE = 100 # number of particle -NTH = N_PARTICLE / 1.0 # Number of particle for re-sampling +NTH = N_PARTICLE / 1.5 # Number of particle for re-sampling show_animation = True @@ -41,9 +41,9 @@ class Particle: self.yaw = 0.0 self.P = np.eye(3) # landmark x-y positions - self.lm = np.matrix(np.zeros((N_LM, LM_SIZE))) + self.lm = np.zeros((N_LM, LM_SIZE)) # landmark position covariance - self.lmP = np.matrix(np.zeros((N_LM * LM_SIZE, LM_SIZE))) + self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE)) def fast_slam2(particles, u, z): @@ -97,7 +97,7 @@ def predict_particles(particles, u): px[0, 0] = particles[i].x px[1, 0] = particles[i].y px[2, 0] = particles[i].yaw - ud = u + (np.matrix(np.random.randn(1, 2)) * R).T # add noise + ud = u + (np.random.randn(1, 2) @ R).T # add noise px = motion_model(px, ud) particles[i].x = px[0, 0] particles[i].y = px[1, 0] @@ -108,9 +108,9 @@ def predict_particles(particles, u): def add_new_lm(particle, z, Q): - r = z[0, 0] - b = z[0, 1] - lm_id = int(z[0, 2]) + r = z[0] + b = z[1] + lm_id = int(z[2]) s = math.sin(pi_2_pi(particle.yaw + b)) c = math.cos(pi_2_pi(particle.yaw + b)) @@ -119,10 +119,10 @@ def add_new_lm(particle, z, Q): particle.lm[lm_id, 1] = particle.y + r * s # covariance - Gz = np.matrix([[c, -r * s], - [s, r * c]]) + Gz = np.array([[c, -r * s], + [s, r * c]]) - particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz * Q * Gz.T + particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz @ Q @ Gz.T return particle @@ -133,44 +133,45 @@ def compute_jacobians(particle, xf, Pf, Q): d2 = dx**2 + dy**2 d = math.sqrt(d2) - zp = np.matrix([[d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]]).T + zp = np.array( + [d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]).reshape(2, 1) - Hv = np.matrix([[-dx / d, -dy / d, 0.0], - [dy / d2, -dx / d2, -1.0]]) + Hv = np.array([[-dx / d, -dy / d, 0.0], + [dy / d2, -dx / d2, -1.0]]) - Hf = np.matrix([[dx / d, dy / d], - [-dy / d2, dx / d2]]) + Hf = np.array([[dx / d, dy / d], + [-dy / d2, dx / d2]]) - Sf = Hf * Pf * Hf.T + Q + Sf = Hf @ Pf @ Hf.T + Q return zp, Hv, Hf, Sf def update_KF_with_cholesky(xf, Pf, v, Q, Hf): - PHt = Pf * Hf.T - S = Hf * PHt + Q + PHt = Pf @ Hf.T + S = Hf @ PHt + Q S = (S + S.T) * 0.5 SChol = np.linalg.cholesky(S).T SCholInv = np.linalg.inv(SChol) - W1 = PHt * SCholInv - W = W1 * SCholInv.T + W1 = PHt @ SCholInv + W = W1 @ SCholInv.T - x = xf + W * v - P = Pf - W1 * W1.T + x = xf + W @ v + P = Pf - W1 @ W1.T return x, P def update_landmark(particle, z, Q): - lm_id = int(z[0, 2]) - xf = np.matrix(particle.lm[lm_id, :]).T - Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2, :]) + lm_id = int(z[2]) + xf = np.array(particle.lm[lm_id, :]).reshape(2, 1) + Pf = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2]) zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q) - dz = z[0, 0: 2].T - zp + dz = z[0:2].reshape(2, 1) - zp dz[1, 0] = pi_2_pi(dz[1, 0]) xf, Pf = update_KF_with_cholesky(xf, Pf, dz, Q, Hf) @@ -183,21 +184,20 @@ def update_landmark(particle, z, Q): def compute_weight(particle, z, Q): - lm_id = int(z[0, 2]) - xf = np.matrix(particle.lm[lm_id, :]).T - Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2]) + lm_id = int(z[2]) + xf = np.array(particle.lm[lm_id, :]).reshape(2, 1) + Pf = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2]) zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q) - dz = z[0, 0: 2].T - zp + dz = z[0:2].reshape(2, 1) - zp dz[1, 0] = pi_2_pi(dz[1, 0]) try: invS = np.linalg.inv(Sf) except np.linalg.linalg.LinAlgError: - print("singuler") return 1.0 - num = math.exp(-0.5 * dz.T * invS * dz) + num = math.exp(-0.5 * dz.T @ invS @ dz) den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf)) w = num / den @@ -207,22 +207,22 @@ def compute_weight(particle, z, Q): def proposal_sampling(particle, z, Q): - lm_id = int(z[0, 2]) - xf = np.matrix(particle.lm[lm_id, :]).T - Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2]) + lm_id = int(z[2]) + xf = particle.lm[lm_id, :].reshape(2, 1) + Pf = particle.lmP[2 * lm_id:2 * lm_id + 2] # State - x = np.matrix([[particle.x, particle.y, particle.yaw]]).T + x = np.array([particle.x, particle.y, particle.yaw]).reshape(3, 1) P = particle.P zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q) Sfi = np.linalg.inv(Sf) - dz = z[0, 0: 2].T - zp - dz[1, 0] = pi_2_pi(dz[1, 0]) + dz = z[0:2].reshape(2, 1) - zp + dz[1] = pi_2_pi(dz[1]) Pi = np.linalg.inv(P) - particle.P = np.linalg.inv(Hv.T * Sfi * Hv + Pi) # proposal covariance - x += particle.P * Hv.T * Sfi * dz # proposal mean + particle.P = np.linalg.inv(Hv.T @ Sfi @ Hv + Pi) # proposal covariance + x += particle.P @ Hv.T @ Sfi @ dz # proposal mean particle.x = x[0, 0] particle.y = x[1, 0] @@ -233,21 +233,20 @@ def proposal_sampling(particle, z, Q): def update_with_observation(particles, z): - for iz in range(len(z[:, 0])): - - lmid = int(z[iz, 2]) + for iz in range(len(z[0, :])): + lmid = int(z[2, iz]) for ip in range(N_PARTICLE): # new landmark if abs(particles[ip].lm[lmid, 0]) <= 0.01: - particles[ip] = add_new_lm(particles[ip], z[iz, :], Q) + particles[ip] = add_new_lm(particles[ip], z[:, iz], Q) # known landmark else: - w = compute_weight(particles[ip], z[iz, :], Q) + w = compute_weight(particles[ip], z[:, iz], Q) particles[ip].w *= w - particles[ip] = update_landmark(particles[ip], z[iz, :], Q) - particles[ip] = proposal_sampling(particles[ip], z[iz, :], Q) + particles[ip] = update_landmark(particles[ip], z[:, iz], Q) + particles[ip] = proposal_sampling(particles[ip], z[:, iz], Q) return particles @@ -263,20 +262,20 @@ def resampling(particles): for i in range(N_PARTICLE): pw.append(particles[i].w) - pw = np.matrix(pw) + pw = np.array(pw) - Neff = 1.0 / (pw * pw.T)[0, 0] # Effective particle number + Neff = 1.0 / (pw @ pw.T) # Effective particle number # print(Neff) if Neff < NTH: # resampling wcum = np.cumsum(pw) base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE - resampleid = base + np.random.rand(base.shape[1]) / N_PARTICLE + resampleid = base + np.random.rand(base.shape[0]) / N_PARTICLE inds = [] ind = 0 for ip in range(N_PARTICLE): - while ((ind < wcum.shape[1] - 1) and (resampleid[0, ip] > wcum[0, ind])): + while ((ind < wcum.shape[0] - 1) and (resampleid[ip] > wcum[ind])): ind += 1 inds.append(ind) @@ -294,41 +293,42 @@ def resampling(particles): def calc_input(time): - if time <= 3.0: + if time <= 3.0: # wait at first v = 0.0 yawrate = 0.0 else: v = 1.0 # [m/s] yawrate = 0.1 # [rad/s] - u = np.matrix([v, yawrate]).T + u = np.array([v, yawrate]).reshape(2, 1) return u def observation(xTrue, xd, u, RFID): + # calc true state xTrue = motion_model(xTrue, u) - # add noise to gps x-y - z = np.matrix(np.zeros((0, 3))) + # add noise to range observation + z = np.zeros((3, 0)) for i in range(len(RFID[:, 0])): dx = RFID[i, 0] - xTrue[0, 0] dy = RFID[i, 1] - xTrue[1, 0] d = math.sqrt(dx**2 + dy**2) - angle = math.atan2(dy, dx) - xTrue[2, 0] + angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) if d <= MAX_RANGE: dn = d + np.random.randn() * Qsim[0, 0] # add noise anglen = angle + np.random.randn() * Qsim[1, 1] # add noise - zi = np.matrix([dn, pi_2_pi(anglen), i]) - z = np.vstack((z, zi)) + zi = np.array([dn, pi_2_pi(anglen), i]).reshape(3, 1) + z = np.hstack((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] + OFFSET_YAWRATE_NOISE - ud = np.matrix([ud1, ud2]).T + ud = np.array([ud1, ud2]).reshape(2, 1) xd = motion_model(xd, ud) @@ -337,15 +337,15 @@ def observation(xTrue, xd, u, RFID): def motion_model(x, u): - F = np.matrix([[1.0, 0, 0], - [0, 1.0, 0], - [0, 0, 1.0]]) + F = np.array([[1.0, 0, 0], + [0, 1.0, 0], + [0, 0, 1.0]]) - B = np.matrix([[DT * math.cos(x[2, 0]), 0], - [DT * math.sin(x[2, 0]), 0], - [0.0, DT]]) + B = np.array([[DT * math.cos(x[2, 0]), 0], + [DT * math.sin(x[2, 0]), 0], + [0.0, DT]]) - x = F * x + B * u + x = F @ x + B @ u x[2, 0] = pi_2_pi(x[2, 0]) @@ -374,10 +374,9 @@ def main(): N_LM = RFID.shape[0] # State Vector [x y yaw v]' - xEst = np.matrix(np.zeros((STATE_SIZE, 1))) - xTrue = np.matrix(np.zeros((STATE_SIZE, 1))) - - xDR = np.matrix(np.zeros((STATE_SIZE, 1))) # Dead reckoning + xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation + xTrue = np.zeros((STATE_SIZE, 1)) # True state + xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning # history hxEst = xEst @@ -408,21 +407,17 @@ def main(): plt.plot(RFID[:, 0], RFID[:, 1], "*k") for iz in range(len(z[:, 0])): - lmid = int(z[iz, 2]) - plt.plot([xEst[0, 0], RFID[lmid, 0]], [ - xEst[1, 0], RFID[lmid, 1]], "-k") + lmid = int(z[2, iz]) + plt.plot([xEst[0], RFID[lmid, 0]], [ + xEst[1], RFID[lmid, 1]], "-k") for i in range(N_PARTICLE): plt.plot(particles[i].x, particles[i].y, ".r") plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb") - plt.plot(np.array(hxTrue[0, :]).flatten(), - np.array(hxTrue[1, :]).flatten(), "-b") - plt.plot(np.array(hxDR[0, :]).flatten(), - np.array(hxDR[1, :]).flatten(), "-k") - plt.plot(np.array(hxEst[0, :]).flatten(), - np.array(hxEst[1, :]).flatten(), "-r") - + plt.plot(hxTrue[0, :], hxTrue[1, :], "-b") + plt.plot(hxDR[0, :], hxDR[1, :], "-k") + plt.plot(hxEst[0, :], hxEst[1, :], "-r") plt.plot(xEst[0], xEst[1], "xk") plt.axis("equal") plt.grid(True)