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

Fix covariance dimension (#998)

Browse Source
This commit is contained in:
Yi-Chen Zhang
2024-04-28 07:55:25 -04:00
committed by GitHub
parent 8f84f0f551
commit c76f74703f
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Localization/particle_filter/particle_filter.py
View File

@@ -96,10 +96,10 @@ def calc_covariance(x_est, px, pw):
calculate covariance matrix
see ipynb doc
"""
cov = np.zeros((3, 3))
cov = np.zeros((4, 4))
n_particle = px.shape[1]
for i in range(n_particle):
dx = (px[:, i:i + 1] - x_est)[0:3]
dx = (px[:, i:i + 1] - x_est)
cov += pw[0, i] * dx @ dx.T
cov *= 1.0 / (1.0 - pw @ pw.T)