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

Replaced sqrt(x**2+y**2) with hypot in SLAM/FastSLAM2/fast_slam2.py

Browse Source
This commit is contained in:
Guillaume Jacquenot
2019-12-07 22:11:28 +01:00
parent 652c32fba9
commit 1d03241d5d
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
SLAM/FastSLAM2/fast_slam2.py
View File

@@ -304,7 +304,7 @@ def observation(xTrue, xd, u, RFID):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise