From 6c2ea8cdd9042fa57fd5b085d9f8de3497f394f4 Mon Sep 17 00:00:00 2001 From: kazuki takahashi Date: Sat, 9 Mar 2019 14:15:31 +0900 Subject: [PATCH 1/2] fix LM angle calculation --- SLAM/EKFSLAM/ekf_slam.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index f2c001d2..d5e518c1 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -170,7 +170,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid): delta = lm - xEst[0:2] q = (delta.T @ delta)[0, 0] #zangle = math.atan2(delta[1], delta[0]) - xEst[2] - zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2] + zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0] zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]]) y = (z - zp).T y[1] = pi_2_pi(y[1]) From 78f3d52de4b379547e3500a51b543daa769690ae Mon Sep 17 00:00:00 2001 From: kazuki takahashi Date: Sat, 9 Mar 2019 14:19:08 +0900 Subject: [PATCH 2/2] fix typo in comment --- SLAM/EKFSLAM/ekf_slam.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index d5e518c1..33c631fd 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -20,7 +20,7 @@ SIM_TIME = 50.0 # simulation time [s] MAX_RANGE = 20.0 # maximum observation range 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] +LM_SIZE = 2 # LM state size [x,y] show_animation = True