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

Merge pull request #178 from Taka-Kazu/modify-ekfslam

Browse Source 
fixed bug of ekf_slam.py
This commit is contained in:
Atsushi Sakai
2019-03-10 08:32:57 +09:00
committed by GitHub
parent 17e0f49511 78f3d52de4
commit f419f6ac27
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -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
@@ -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])