py: vectorized the gating

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