github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

fix LM angle calculation

Browse Source
This commit is contained in:
kazuki takahashi
2019-03-09 14:15:31 +09:00
parent 17e0f49511
commit 6c2ea8cdd9
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
SLAM/EKFSLAM/ekf_slam.py
View File

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