From 60c98edf75900d643ddd61cbbee0f9de18fe64ee Mon Sep 17 00:00:00 2001
From: Anton Kullberg <anton.kullberg@liu.se>
Date: Thu, 11 Nov 2021 17:56:39 +0100
Subject: [PATCH] py: vectorized the gating
---
src/gaters.py | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/src/gaters.py b/src/gaters.py
index c3c5ef7..e84211d 100644
--- a/src/gaters.py
+++ b/src/gaters.py
@@ -45,9 +45,9 @@ class MahalanobisGater:
# Derivative
H = self.sensor_model['dhdx'](x.flatten())
# Mahalanobis Gating
- T = np.zeros((y.shape[1],))
- for j in range(y.shape[1]):
- Sk = H@P@H.T+self.sensor_model['R']
- T[j] = eps[:, j].T@np.linalg.inv(Sk)@eps[:, j]
+ Sk = H@P@H.T+self.sensor_model['R']
+ invSk = np.linalg.inv(Sk)
+ # Computes the normalized residual for all measurements simultaneously
+ T = np.diag(np.dot(np.tensordot(eps, invSk, (0,0)), eps))
accepted_meas = T < self.gamma
return accepted_meas
--
GitLab