diff --git a/src/gaters.py b/src/gaters.py index 4e651f643c7cbdd49293d0d4e3919fd098db99ff..6727280ab20a61175d25aec588ec72d216d4ec87 100644 --- a/src/gaters.py +++ b/src/gaters.py @@ -5,12 +5,17 @@ class MahalanobisGater: self.sensor_model = sensor_model self.gamma = gamma - def gate(self, x, P, eps): + def gate(self, x, P, meas): + if meas.ndim < 2: + y = meas.expand_dims(meas, 0) + else: + y = meas + eps = y-self.sensor_model['h'](x.flatten())[:, None] # Derivative H = self.sensor_model['dhdx'](x.flatten()) # Mahalanobis Gating - T = np.zeros((eps.shape[1],)) - for j in range(eps.shape[1]): + 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] accepted_meas = T < self.gamma diff --git a/src/trackers.py b/src/trackers.py index b072166a5e98115bbe634ef8dffff05a9a39adda..d07b574172bc30598e8f35df3eefc914fad08c06 100644 --- a/src/trackers.py +++ b/src/trackers.py @@ -13,7 +13,7 @@ class BasicTracker(): yhat = self.filt.sensor_model['h'](x[:2, k]) eps = meas_k-yhat[:, None] # Gating step - accepted_meas = self.gater.gate(x[:, k], P[:, :, k], eps) + accepted_meas = self.gater.gate(x[:, k], P[:, :, k], meas_k) # If any measurements are accepted, select the nearest one if accepted_meas.any(): # Association step @@ -43,7 +43,7 @@ class IMMTracker(): eps = meas_k-yhat[:, None] # Gating step - accepted_meas = self.gater.gate(xhat, Phat, eps) + accepted_meas = self.gater.gate(xhat, Phat, meas_k) # If any measurements are accepted, select the nearest one if accepted_meas.any(): # Association step