From b0ccf363c294d2ba39e8426b669b4689a7be8342 Mon Sep 17 00:00:00 2001
From: FXCarl <fxcarlcn@gmail.com>
Date: Fri, 3 Jan 2020 14:37:49 +0800
Subject: [PATCH] New way to calc covariance

the err ellipse looks strange in original version
---
 Localization/particle_filter/particle_filter.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py
index bfef3ed929..841613c49c 100644
--- a/Localization/particle_filter/particle_filter.py
+++ b/Localization/particle_filter/particle_filter.py
@@ -88,11 +88,11 @@ def gauss_likelihood(x, sigma):
 
 def calc_covariance(xEst, px, pw):
     cov = np.zeros((3, 3))
-
-    for i in range(px.shape[1]):
-        dx = (px[:, i] - xEst)[0:3]
-        cov += pw[0, i] * dx.dot(dx.T)
-    cov /= NP
+    # https://www.gnu.org/software/gsl/doc/html/statistics.html#weighted-samples
+    # https://www.visiondummy.com/2014/04/draw-error-ellipse-representing-covariance-matrix/
+    # 99% sqrt(9.210) ≈ 3.0347
+    tx = 3.0347 * (NP * pw) * ((px - xEst)[0:3] ** 2)
+    cov = tx @ tx.T
 
     return cov