from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(
4, # Number of state dimensions (position and velocity in 2D)
(0,2), # Mapping measurement vector index to state index
np.array([[0.25, 0], # Covariance matrix for Gaussian PDF
[0, 0.25]])
)
from stonesoup.updater.kalman import KalmanUpdater
updater = KalmanUpdater(measurement_model)
from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.measures import Mahalanobis
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:
from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.measures import Mahalanobis
measure = Mahalanobis()
hypothesiser = FilteredDetectionsGater(DistanceHypothesiser(predictor,
updater,
measure,
missed_distance=3),
metadata_filter="MMSI")
# %%
# We will use a nearest-neighbour association algorithm, passing in the Mahalanobis distance
# hypothesiser built in the previous step.
from stonesoup.dataassociator.neighbour import NearestNeighbour
data_associator = NearestNeighbour(hypothesiser)
# %%
# Creating track initiators and deleters
# --------------------------------------
# We need a method to initiate tracks. For this we will create an initiator component
# and have it generate a :class:`~.GaussianState`. In this case, we'll use a measurement initiator
# which uses the measurements value and model covariance where possible. In this case, as we are
# using position from |AIS|_, we only need to be concerned about defining the velocity prior. The
# prior we are using has the velocity state vector as zero, and variance of 10m/s.
from stonesoup.types.state import GaussianState
from stonesoup.initiator.simple import SimpleMeasurementInitiator
initiator = SimpleMeasurementInitiator(
GaussianState([[0], [0], [0], [0]], np.diag([0, 10, 0, 10])),
measurement_model)
# %% # Now we use the :class:`~.NearestNeighbour` data associator, which picks the hypothesis pair # (predicted measurement and detection) with the highest 'score' (in this instance, those that are # closest to each other). # # .. image:: ../_static/NN_Association_Diagram.png # :width: 500 # :alt: Image showing NN association for one track # # In the diagram above, there are three possible detections to be considered for association (some # of which may be clutter). The detection with a score of :math:`0.4` is selected by the nearest # neighbour algorithm. from stonesoup.dataassociator.neighbour import NearestNeighbour data_associator = NearestNeighbour(hypothesiser) # %% # Run the Kalman filter with the associator # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # 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