From c76f74703f6b9f7b2ec3bf993eb9c9c5d2ccb4ac Mon Sep 17 00:00:00 2001 From: Yi-Chen Zhang Date: Sun, 28 Apr 2024 07:55:25 -0400 Subject: [PATCH] Fix covariance dimension (#998) --- Localization/particle_filter/particle_filter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 17051623..ba54a3d1 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)