hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=3)
from stonesoup.dataassociator.neighbour import NearestNeighbour
data_associator = NearestNeighbour(hypothesiser)
from stonesoup.types.state import GaussianState
prior = GaussianState([[0], [1], [0], [1]], np.diag([0.25, 0.1, 0.25, 0.1]), timestamp=start_time)
from stonesoup.types.track import Track
track = Track([prior])
for n, (measurements, clutter_set) in enumerate(zip(measurementss, clutter), 1):
detections = clutter_set.copy()
detections.update(measurements) # Add measurements and clutter together
hypotheses = data_associator.associate({track}, detections, start_time+timedelta(seconds=n))
hypothesis = hypotheses[track]
if hypothesis.measurement:
post = updater.update(hypothesis)
track.append(post)
else: # When data associator says no detections are good enough, we'll keep the prediction
track.append(hypothesis.prediction)
# Plot the resulting track
ax.plot([state.state_vector[0, 0] for state in track[1:]], # Skip plotting the prior
[state.state_vector[2, 0] for state in track[1:]],
marker=".")
fig
from matplotlib.patches import Ellipse
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# With these components, we can run the simulated data and clutter through the Kalman filter.
# Create prior
from stonesoup.types.state import GaussianState
prior = GaussianState([[0], [1], [0], [1]], np.diag([1.5, 0.5, 1.5, 0.5]), timestamp=start_time)
# %%
# Loop through the predict, hypothesise, associate and update steps.
from stonesoup.types.track import Track
track = Track([prior])
for n, measurements in enumerate(all_measurements):
hypotheses = data_associator.associate([track],
measurements,
start_time + timedelta(seconds=n))
hypothesis = hypotheses[track]
if hypothesis.measurement:
post = updater.update(hypothesis)
track.append(post)
else: # When data associator says no detections are good enough, we'll keep the prediction
track.append(hypothesis.prediction)
# %%
# Plot the resulting track
plotter.plot_tracks(track, [0, 2], uncertainty=True)
plotter.fig