Fix covariance dimension (#998)

Author: Chris7462

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)