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

Fixed occasional math domain error

Browse Source
This commit is contained in:
Daniel Ingram
2018-03-13 22:19:20 -04:00
parent 3601bd187e
commit ca7195f415
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
Localization/particle_filter/particle_filter.py
View File

@@ -169,8 +169,15 @@ def plot_covariance_ellipse(xEst, PEst):
smallind = 0
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
a = math.sqrt(eigval[bigind])
b = math.sqrt(eigval[smallind])
#eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
#close to 0 (~10^-20), catch these cases and set the respective variable to 0
try: a = math.sqrt(eigval[bigind])
except ValueError: a = 0
try: b = math.sqrt(eigval[smallind])
except ValueError: b = 0
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])