diff --git a/src/gaters.py b/src/gaters.py index c3c5ef7322534807d50372a310f2ac715e01a3e9..e84211d0e556b0f8a5c0333db2289024c2e3a4ac 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