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

firx fast slam bug

Browse Source
This commit is contained in:
Atsushi Sakai
2018-11-29 20:30:51 +09:00
parent e0c15292b2
commit 44231dcab7
3 changed files with 15 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
SLAM/FastSLAM1/fast_slam1.py
View File

@@ -190,15 +190,14 @@ def compute_weight(particle, z, Q):
dx = z[0, 0: 2].T - zp
dx[1, 0] = pi_2_pi(dx[1, 0])
S = particle.lmP[2 * lm_id:2 * lm_id + 2]
try:
invS = np.linalg.inv(S)
invS = np.linalg.inv(Sf)
except np.linalg.linalg.LinAlgError:
print("singuler")
return 1.0
num = math.exp(-0.5 * dx.T * invS * dx)
den = 2.0 * math.pi * math.sqrt(np.linalg.det(S))
den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
w = num / den