diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py
index 17051623ca..ba54a3d12b 100644
--- 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)