Fix covariance dimension (AtsushiSakai#998)

alvinxds · Apr 28, 2024 · c76f747 · c76f747
1 parent 8f84f0f
commit c76f747
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py
@@ -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)