From b503bccc4aeeb34329a767a40e76e6e0f3e83f5c Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Thu, 7 Sep 2023 16:50:39 +0100 Subject: [PATCH 01/11] draft track_fusion_example, issue with Particle filters trackfeeder --- docs/examples/track_fusion_example.py | 554 ++++++++++++++++++++++++++ 1 file changed, 554 insertions(+) create mode 100644 docs/examples/track_fusion_example.py diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py new file mode 100644 index 000000000..0e14258f1 --- /dev/null +++ b/docs/examples/track_fusion_example.py @@ -0,0 +1,554 @@ +# %% +# +# Comparing different filters in the context of track fusion +# ---------------------------------------------------------- +# +# This example shows a comparison between a Kalman filter and +# a particle filter in the context of track fusion. This example is +# relevant to show how to get an unique track +# from partial tracks generated from a set of +# different measurements obtained from independent sensors. +# This example simulates the case of a single target moving in +# a 2D cartesian space with measurements obtained from two +# identical, for simplicity, radars and trackers, then we +# compare the resulting composite track obtained by the +# two different tracks generated. Furthermore, we +# measure the performances of the tracks obtained +# by two different filters, Kalman and particle. +# +# This example follows this structure: +# 1) Initialise the sensors and the target trajectory; +# 2) Initialise the filters components and create the tracker; +# 3) Run the trackers, generate the partial tracks and the final +# composite track; +# 4) Compare the obtained tracks with the groundtruth trajectory. +# + +# %% +# 1) Initialise the sensors and the target trajectory +# --------------------------------------------------- +# We start creating two identical, in terms of performances, +# radars using :class:`~.RadarBearingRange` placed on two +# separate :class:`~.FixedPlatform`. For the target we +# simulate a single object moving on a straight trajectory. +# + +# Load the various packages +import numpy as np +from datetime import datetime +from datetime import timedelta +from copy import deepcopy + +# Stone Soup general imports +from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \ + ConstantVelocity +from stonesoup.types.state import GaussianState +from stonesoup.types.array import CovarianceMatrix + + +# Simulation parameters setup +start_time = datetime(2023,8,1, 10,0,0) # For simplicity fix a date +number_of_steps = 75 # Number of timestep for the simulation +np.random.seed(1908) # Random seed for reproducibility + +# Instantiate the target transition model +transition_model = CombinedLinearGaussianTransitionModel([ + ConstantVelocity(0.00), ConstantVelocity(0.00)]) + +# Define the initial target state +initial_target_state = GaussianState([25, 1, 75, -0.5], + np.diag([1, 1, 1, 1]) ** 2, + timestamp=start_time) + +# Single target groundtruth Simulator +from stonesoup.simulator.simple import SingleTargetGroundTruthSimulator + +# Set up the ground truth simulation +groundtruth_simulation = SingleTargetGroundTruthSimulator( + transition_model=transition_model, + initial_state=initial_target_state, + timestep=timedelta(seconds=1), + number_steps=number_of_steps) + +# Load a clutter model +from stonesoup.models.clutter.clutter import ClutterModel + +# Define the clutter model +clutter_model = ClutterModel( + clutter_rate=1.0, + distribution=np.random.default_rng().uniform, + dist_params=((0,120), (-5,105))) +# dist_params describe the area where the clutter is detected + +# Define the clutter area and spatial density for the tracker +clutter_area = np.prod(np.diff(clutter_model.dist_params)) +clutter_spatial_density = clutter_model.clutter_rate/clutter_area + +# Instantiate the radars to collect measurements - Use a BearingRange radar +from stonesoup.sensor.radar.radar import RadarBearingRange + +# Let's assume that both radars have the same noise covariance for simplicity +# These radars will have the +/-0.005 degrees accuracy in bearing and 2 meters in range +radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 2.01**2])) + +# Define the specifications of the two radars +radar1 = RadarBearingRange( + ndim_state= 4, + position_mapping= (0, 2), + noise_covar= radar_noise, + clutter_model= clutter_model, + max_range= 3000) + +# deep copy the first radar specs. Changes in the first one does not influence the second +radar2 = deepcopy(radar1) + +# Import the platform to place the sensors on +from stonesoup.platform.base import FixedPlatform + +# Instantiate the first sensor platform and add the sensor +sensor1_platform = FixedPlatform( + states=GaussianState([10,0,80,0], np.diag([1,0,1,0])), + position_mapping= (0, 2), + sensors= [radar1]) + +# Instantiate the second one +sensor2_platform = FixedPlatform( +states=GaussianState([75,0,10,0], np.diag([1,0,1,0])), + position_mapping= (0, 2), + sensors= [radar2]) + +# Load the platform detection simulator - Let's use a simulator for each radar +# Instantiate the simulators +from stonesoup.simulator.platform import PlatformDetectionSimulator + +radar_simulator1 = PlatformDetectionSimulator( + groundtruth= groundtruth_simulation, + platforms= [sensor1_platform]) + +radar_simulator2 = PlatformDetectionSimulator( + groundtruth= groundtruth_simulation, + platforms= [sensor2_platform]) + +# %% +# Visualise the detections from the sensors +# ----------------------------------------- +# Before creating the different trackers components +# let's visualise the target and its detections from the +# two sensors. In this way we can appreciate +# how the measurements are different and can lead to two separate +# tracks. +# + +# Load the stone soup plotter +from stonesoup.plotter import Plotterly + +# Lists to hold the detections from each sensor +s1_detections = [] +s2_detections = [] + +# show1 = deepcopy(radar_simulator1) +# show2 = deepcopy(radar_simulator2) +# # Extract the generator function for the detections +# g1 = show1.detections_gen() +# g2 = show2.detections_gen() +# +# truths = set() +# +# # Iterate over the time steps, extracting the detections and truths +# for _ in range(number_of_steps): +# s1_detections.append(next(g1)[1]) +# s2_detections.append(next(g2)[1]) +# +# # Generate the ground truth +# #truths = set(groundtruth_simulation.groundtruth_paths) +# +# # Plot the groundtruth and detections from the two radars +# plotter = Plotterly() +# #plotter.plot_ground_truths(truths, [0, 2]) +# plotter.plot_measurements(s1_detections, [0, 2], marker= dict(color='orange', symbol='305'), +# measurements_label='Sensor 1 measurements') +# plotter.plot_measurements(s2_detections, [0, 2], marker= dict(color='blue', symbol='0'), +# measurements_label='Sensor 2 measurements') +# plotter.plot_sensors({sensor1_platform,sensor2_platform}, [0,1], marker= dict(color='black', symbol= '1', +# size=10)) +# plotter.fig.show() +# sys.exit() + +# %% +# 2) Initialise the trackers components +# ------------------------------------- +# We have initialised the sensors and the +# target path, we can see that the +# detections from the two sensors +# are slightly different one from the +# other, that will lead to two separate +# tracks. Now, we initialise the two trackers +# one using a Kalman filter and the other +# a particle filter. +# + +# Let's consider a Distance based hypothesiser +from stonesoup.hypothesiser.distance import DistanceHypothesiser +from stonesoup.measures import Mahalanobis +from stonesoup.dataassociator.neighbour import GNNWith2DAssignment +from stonesoup.deleter.time import UpdateTimeStepsDeleter + +# Load the kalman filter components +from stonesoup.updater.kalman import ExtendedKalmanUpdater +from stonesoup.predictor.kalman import ExtendedKalmanPredictor +from stonesoup.initiator.simple import SimpleMeasurementInitiator, GaussianParticleInitiator + +# Load a single target tracker +from stonesoup.tracker.simple import SingleTargetTracker + +# define an helper function to minimise the number of times +# we have to initialise the same tracker object + +def general_tracker(tracker_class, detector, + filter_updater, initiator, deleter, + data_associator): + """ + Helper function to initialise the trackers + """ + + tracker = tracker_class( + initiator= initiator, + detector= detector, + updater= filter_updater, + data_associator= data_associator, + deleter=deleter) + return tracker + + +# instantiate the Kalman filter predictor +KF_predictor = ExtendedKalmanPredictor(transition_model) + +# Instantiate the Kalman filter updater +KF_updater = ExtendedKalmanUpdater(measurement_model= None) + +# create an track initiator placed on the target track origin +initiator = SimpleMeasurementInitiator( + prior_state=initial_target_state, + measurement_model=None) + +# define the hypothesiser +hypothesiser_KF = DistanceHypothesiser( + predictor=KF_predictor, + updater= KF_updater, + measure= Mahalanobis(), + missed_distance= 5 +) + +# define the distance data associator +data_associator_KF = GNNWith2DAssignment(hypothesiser_KF) + +# define a track time deleter +deleter = UpdateTimeStepsDeleter(5) + +# prepare the particle components +from stonesoup.predictor.particle import ParticlePredictor +from stonesoup.updater.particle import ParticleUpdater +from stonesoup.resampler.particle import ESSResampler + +# Instantiate the predictor, particle resampler and particle +# filter updater +PF_predictor = ParticlePredictor(transition_model) +resampler = ESSResampler() +PF_updater = ParticleUpdater(measurement_model= None, + resampler= resampler) + +hypothesiser_PF = DistanceHypothesiser( + predictor= PF_predictor, + updater= PF_updater, + measure= Mahalanobis(), + missed_distance= 5) + +# define the data associator +data_associator_PF = GNNWith2DAssignment(hypothesiser_PF) + +# To instantiate the track initiator we define a prior state +# as gaussian state with the target track origin +prior_state= SimpleMeasurementInitiator( + prior_state=GaussianState([25, 1, 70, -0.5], + np.diag([1, 0.01, 1, 0.01]) ** 2)) + +# Particle filter initiator +PF_initiator = GaussianParticleInitiator( + initiator= prior_state, + number_particles= 500) + +# %% +# At this stage we have all the components needed to +# perform the tracking using both Kalman and Particle +# filters. We need to create a way to perform the track fusion. +# To perform such fusion, we employ the covariance +# intersection algorithm +# adopting the :class:`ChernoffUpdater` class, and +# treating the tracks as measurements and we consider +# the measurements as GaussianMixture objects - [explain better] + +# Instantiate a dummy detector to read the detections +from stonesoup.buffered_generator import BufferedGenerator +from stonesoup.reader.base import DetectionReader + +class DummyDetector(DetectionReader): + def __init__(self, *args, **kwargs): + self.current = kwargs['current'] + + @BufferedGenerator.generator_method + def detections_gen(self): + yield self.current + + +# Load the ChernoffUpdater components for track fusion +from stonesoup.updater.chernoff import ChernoffUpdater +from stonesoup.updater.pointprocess import PHDUpdater +from stonesoup.mixturereducer.gaussianmixture import GaussianMixtureReducer +from stonesoup.hypothesiser.gaussianmixture import GaussianMixtureHypothesiser +from stonesoup.tracker.pointprocess import PointProcessMultiTargetTracker +from stonesoup.types.state import TaggedWeightedGaussianState +from stonesoup.feeder.track import Tracks2GaussianDetectionFeeder + +# create the Chernoff Updater +ch_updater = ChernoffUpdater(measurement_model=None) + +# Instantiate the PHD Updater, including the probability of +# detection and probability of survival +updater = PHDUpdater(updater= ch_updater, + clutter_spatial_density= clutter_spatial_density, + prob_detection= 0.9, + prob_survival= 0.9) + +# Create a base hypothesiser using the Chernoff updater and +# the kalman predictor +base_hypothesiser = DistanceHypothesiser( + predictor= KF_predictor, + updater= ch_updater, + measure= Mahalanobis(), + missed_distance= 100) + +# Instantiate the Gaussian Mixture hypothesiser +hypothesiser= GaussianMixtureHypothesiser(base_hypothesiser, + order_by_detection= True) + +# Gaussian mixture reducer to prune the various tracks +ch_reducer = GaussianMixtureReducer( + prune_threshold= 1e-10, + pruning= True, + merge_threshold= 100, + merging= True) + +# Create the covariance for the birth of the tracks, +# large on the x,y location and smaller on the velocities +birth_covar = CovarianceMatrix(np.diag([100, 1, 100, 1])) + +# Define the Chernoff updated birth components for the tracks +ch_birth_component = TaggedWeightedGaussianState( + state_vector=[25, 1, 70, -0.5], # Initial target state + covar=birth_covar**2, + weight= 0.5, + tag = TaggedWeightedGaussianState.BIRTH, + timestamp= start_time) + +# Instantiate the Track fusion tracker using Point Process Tracker +track_fusion_tracker = PointProcessMultiTargetTracker( + detector= None, + hypothesiser= hypothesiser, + updater= updater, + reducer= ch_reducer, + birth_component= ch_birth_component, + extraction_threshold= 0.9) + +# %% +# 3) Run the trackers, generate the partial tracks and the final composite track; +# ------------------------------------------------------------------------------- +# So far we have shown how to instantiate the various tracker components +# as well as the track fusion tracker. Now, we run the trackers to generate +# the tracks and we perform the track fusion. Furthermore, we want to measure +# how good are the track fusions in comparisons to the groundtruths, so we +# instantiate a metric manager to measure the various distances. +# + +# Instantiate the metric manager +from stonesoup.metricgenerator.basicmetrics import BasicMetrics + +basic_KF = BasicMetrics(generator_name='Kalman Filter', tracks_key='KF_fused_tracks', + truths_key='truths') + +basic_KF1 = BasicMetrics(generator_name='Kalman Filter', tracks_key='KF_2_tracks', + truths_key='truths') + +basic_KF2 = BasicMetrics(generator_name='Kalman Filter', tracks_key='KF_2_tracks', + truths_key='truths') + +basic_PF = BasicMetrics(generator_name='Particle Filter', tracks_key='PF__fused_tracks', + truths_key='truths') +basic_PF1 = BasicMetrics(generator_name='Particle Filter', tracks_key='PF_1_tracks', + truths_key='truths') +basic_PF2 = BasicMetrics(generator_name='Particle Filter', tracks_key='PF_2_tracks', + truths_key='truths') + +# Load the OSPA metric managers +from stonesoup.metricgenerator.ospametric import OSPAMetric +ospa_KF_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_KF_truths', + tracks_key= 'KF_fused_tracks', truths_key='truths') +ospa_PF_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_PF_truths', + tracks_key= 'PF_fused_tracks', truths_key='truths') + +ospa_KF1_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_KF_1_truths', + tracks_key= 'KF_1_tracks', truths_key='truths') +ospa_PF1_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_PF_1_truths', + tracks_key= 'PF_2_tracks', truths_key='truths') + +ospa_KF2_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_KF_2_truths', + tracks_key= 'KF_1_tracks', truths_key='truths') +ospa_PF2_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_PF_2_truths', + tracks_key= 'PF_2_tracks', truths_key='truths') + +# Define a data associator between the tracks and the truths +from stonesoup.dataassociator.tracktotrack import TrackToTruth +associator = TrackToTruth(association_threshold= 30) + +from stonesoup.metricgenerator.manager import MultiManager +metric_manager = MultiManager([basic_KF, + basic_KF1, + basic_KF2, + basic_PF, + basic_PF1, + basic_PF2, + ospa_KF_truth, + ospa_KF1_truth, + ospa_KF2_truth, + ospa_PF_truth, + ospa_PF1_truth, + ospa_PF2_truth], + associator) + + +# Create the tracks for the Particle filters, kalman and merged ones +PF_track1, PF_track2, KF_track1, KF_track2 = set(), set(), set(), set() +PF_fused_track, KF_fused_track = set(), set() + +# Instantiate the various trackers using the general_tracker function, +# we assign a unique tracker the detections from a specific radar simulator +KF_tracker_1 = general_tracker(SingleTargetTracker, radar_simulator1, KF_updater, + initiator, deleter, data_associator_KF) + +KF_tracker_2 = general_tracker(SingleTargetTracker, radar_simulator2, KF_updater, + initiator, deleter, data_associator_KF) + +PF_tracker_1 = general_tracker(SingleTargetTracker, radar_simulator1, PF_updater, + PF_initiator, deleter, data_associator_PF) + +PF_tracker_2 = general_tracker(SingleTargetTracker, radar_simulator2, PF_updater, + PF_initiator, deleter, data_associator_PF) + +# Load the detection generator +g1 = radar_simulator1.detections_gen() +g2 = radar_simulator2.detections_gen() + +# Loop on the simulation steps +for _ in range(number_of_steps): + + radar1_detections = next(g1) + + # Run the Particle Filter tracker on the radars measurements + PF_tracker_1.detector = DummyDetector(current= radar1_detections) + PF_tracker_1.__iter__() + _, PF_sensor_track1 = next(PF_tracker_1) + PF_track1.update(PF_sensor_track1) + + # Run the Kalman Filter tracker on the radars measurements + KF_tracker_1.detector = DummyDetector(current=radar1_detections) + KF_tracker_1.__iter__() + _, KF_sensor_track1 = next(KF_tracker_1) + KF_track1.update(KF_sensor_track1) + + radar2_detections = next(g2) + + PF_tracker_2.detector = DummyDetector(current=radar2_detections) + PF_tracker_2.__iter__() + _, PF_sensor_track2 = next(PF_tracker_2) + PF_track2.update(PF_sensor_track2) + + KF_tracker_2.detector = DummyDetector(current=radar2_detections) + KF_tracker_2.__iter__() + time, KF_sensor_track2 = next(KF_tracker_2) + KF_track2.update(KF_sensor_track2) + + # load the various bits + metric_manager.add_data({'KF_1_tracks': KF_track1}, overwrite=False) + metric_manager.add_data({'KF_2_tracks': KF_track2}, overwrite=False) + metric_manager.add_data({'PF_1_tracks': PF_track1}, overwrite=False) + metric_manager.add_data({'PF_2_tracks': PF_track2}, overwrite=False) + + # We have now the track for each radar now let's perform the + # track fusion + # for PF_track_meas in [PF_track1, PF_track2]: + # dummy_detector_PF = DummyDetector(current=[time, PF_track_meas]) + # track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector_PF) + # track_fusion_tracker.__iter__() + # _, tracks = next(track_fusion_tracker) + # PF_fused_track.update(tracks) + # # ERROR TypeError: __init__() missing 1 required positional argument: 'covar' + #metric_manager.add_data({'PF_fused_track': PF_fused_track}, overwrite=False) + + for KF_track_meas in [KF_track1, KF_track2]: + dummy_detector_KF = DummyDetector(current=[time, KF_track_meas]) + track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector_KF) + track_fusion_tracker.__iter__() + _, tracks = next(track_fusion_tracker) + KF_fused_track.update(tracks) + metric_manager.add_data({'KF_fused_track': KF_fused_track}, overwrite=False) + +truths = set() +truths = set(groundtruth_simulation.groundtruth_paths) + +metric_manager.add_data({'truths': truths}, overwrite=False) + +# %% +# Let's visualise the various tracks and detections in the cases +# using the Kalman and Particle filters. +# +plotter = Plotterly() +#plotter.plot_tracks(PF_track1, [0,2]) +#plotter.plot_tracks(PF_track2, [0,2]) +#plotter.plot_tracks(PF_fused_track, [0, 2]) +plotter.plot_tracks(KF_fused_track, [0, 2]) +plotter.plot_tracks(KF_track1, [0, 2]) +plotter.plot_tracks(KF_track2, [0, 2]) +plotter.plot_ground_truths(truths, [0, 2]) +plotter.fig.show() + + +# %% +# 4) Compare the obtained tracks with the groundtruth trajectory. +# --------------------------------------------------------------- +# At this stage we have almost completed our example. We have created the +# detections from the radars, performed the tracking and the +# fusion of the tracks. Now we use the :class:`~.MetricManager` +# to generate summary statistics on the accuracy of the tracks +# in comparison to the groundtruth measurements. +# + +# Loaded the plotter for the various metrics. +from stonesoup.plotter import MetricPlotter + +metrics = metric_manager.generate_metrics() +graph = MetricPlotter() +graph.plot_metrics(metrics, generator_names=['OSPA_KF_truths', + 'OSPA_PF_truths'], + color=['green', 'orange']) +graph.axes[0].set(ylabel='OSPA metrics', title='OSPA distances over time') +graph.fig.show() + +# %% +# Conclusions +# ----------- +# In this example we have shown how it is possible to +# merge the tracks generated by independent trackers +# run on sets of data obtained by separate sensors. We +# have, also, compared how the Kalman and the Particle +# filters behave in these cases, making track to track +# comparisons. + From f6fb1d55056a76c06253381d22fdb42baa810500 Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Tue, 12 Sep 2023 15:11:10 +0100 Subject: [PATCH 02/11] small checks and changes --- docs/examples/track_fusion_example.py | 31 ++++++++++++++------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index 0e14258f1..20ed9aeb7 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -31,7 +31,8 @@ # radars using :class:`~.RadarBearingRange` placed on two # separate :class:`~.FixedPlatform`. For the target we # simulate a single object moving on a straight trajectory. -# +# The example setup is simple to it is easier to understand +# how the Stone soup components are working. # Load the various packages import numpy as np @@ -88,7 +89,7 @@ from stonesoup.sensor.radar.radar import RadarBearingRange # Let's assume that both radars have the same noise covariance for simplicity -# These radars will have the +/-0.005 degrees accuracy in bearing and 2 meters in range +# These radars will have the +/-0.005 degrees accuracy in bearing and +/- 2 meters in range radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 2.01**2])) # Define the specifications of the two radars @@ -97,7 +98,7 @@ position_mapping= (0, 2), noise_covar= radar_noise, clutter_model= clutter_model, - max_range= 3000) + max_range= 3000) # max_range can be removed and use the default value # deep copy the first radar specs. Changes in the first one does not influence the second radar2 = deepcopy(radar1) @@ -183,7 +184,7 @@ # are slightly different one from the # other, that will lead to two separate # tracks. Now, we initialise the two trackers -# one using a Kalman filter and the other +# components, one using a Kalman filter and the other # a particle filter. # @@ -201,7 +202,7 @@ # Load a single target tracker from stonesoup.tracker.simple import SingleTargetTracker -# define an helper function to minimise the number of times +# Lets define an helper function to minimise the number of times # we have to initialise the same tracker object def general_tracker(tracker_class, detector, @@ -275,7 +276,7 @@ def general_tracker(tracker_class, detector, # Particle filter initiator PF_initiator = GaussianParticleInitiator( initiator= prior_state, - number_particles= 500) + number_particles= 500) # low number for quicker computations # %% # At this stage we have all the components needed to @@ -283,9 +284,9 @@ def general_tracker(tracker_class, detector, # filters. We need to create a way to perform the track fusion. # To perform such fusion, we employ the covariance # intersection algorithm -# adopting the :class:`ChernoffUpdater` class, and +# adopting the :class:`~.ChernoffUpdater` class, and # treating the tracks as measurements and we consider -# the measurements as GaussianMixture objects - [explain better] +# the measurements as GaussianMixture objects. # Instantiate a dummy detector to read the detections from stonesoup.buffered_generator import BufferedGenerator @@ -331,7 +332,7 @@ def detections_gen(self): hypothesiser= GaussianMixtureHypothesiser(base_hypothesiser, order_by_detection= True) -# Gaussian mixture reducer to prune the various tracks +# Gaussian mixture reducer to prune and merge the various tracks ch_reducer = GaussianMixtureReducer( prune_threshold= 1e-10, pruning= True, @@ -362,7 +363,7 @@ def detections_gen(self): # %% # 3) Run the trackers, generate the partial tracks and the final composite track; # ------------------------------------------------------------------------------- -# So far we have shown how to instantiate the various tracker components +# So far we have shown how to instantiate the various tracker components # as well as the track fusion tracker. Now, we run the trackers to generate # the tracks and we perform the track fusion. Furthermore, we want to measure # how good are the track fusions in comparisons to the groundtruths, so we @@ -424,7 +425,6 @@ def detections_gen(self): ospa_PF2_truth], associator) - # Create the tracks for the Particle filters, kalman and merged ones PF_track1, PF_track2, KF_track1, KF_track2 = set(), set(), set(), set() PF_fused_track, KF_fused_track = set(), set() @@ -468,7 +468,7 @@ def detections_gen(self): PF_tracker_2.detector = DummyDetector(current=radar2_detections) PF_tracker_2.__iter__() - _, PF_sensor_track2 = next(PF_tracker_2) + time, PF_sensor_track2 = next(PF_tracker_2) PF_track2.update(PF_sensor_track2) KF_tracker_2.detector = DummyDetector(current=radar2_detections) @@ -477,8 +477,8 @@ def detections_gen(self): KF_track2.update(KF_sensor_track2) # load the various bits - metric_manager.add_data({'KF_1_tracks': KF_track1}, overwrite=False) - metric_manager.add_data({'KF_2_tracks': KF_track2}, overwrite=False) +# metric_manager.add_data({'KF_1_tracks': KF_track1}, overwrite=False) +# metric_manager.add_data({'KF_2_tracks': KF_track2}, overwrite=False) metric_manager.add_data({'PF_1_tracks': PF_track1}, overwrite=False) metric_manager.add_data({'PF_2_tracks': PF_track2}, overwrite=False) @@ -490,7 +490,7 @@ def detections_gen(self): # track_fusion_tracker.__iter__() # _, tracks = next(track_fusion_tracker) # PF_fused_track.update(tracks) - # # ERROR TypeError: __init__() missing 1 required positional argument: 'covar' + # ERROR TypeError: __init__() missing 1 required positional argument: 'covar' #metric_manager.add_data({'PF_fused_track': PF_fused_track}, overwrite=False) for KF_track_meas in [KF_track1, KF_track2]: @@ -499,6 +499,7 @@ def detections_gen(self): track_fusion_tracker.__iter__() _, tracks = next(track_fusion_tracker) KF_fused_track.update(tracks) + sys.exit() metric_manager.add_data({'KF_fused_track': KF_fused_track}, overwrite=False) truths = set() From 220946123a6a04ccdb1780da9db41c7aa8cae6d2 Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Thu, 21 Sep 2023 14:59:02 +0100 Subject: [PATCH 03/11] small checks --- docs/examples/track_fusion_example.py | 78 +++++++++++++++++++-------- 1 file changed, 55 insertions(+), 23 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index 20ed9aeb7..ba4dc373b 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -53,9 +53,12 @@ np.random.seed(1908) # Random seed for reproducibility # Instantiate the target transition model -transition_model = CombinedLinearGaussianTransitionModel([ +gnd_transition_model = CombinedLinearGaussianTransitionModel([ ConstantVelocity(0.00), ConstantVelocity(0.00)]) +transition_model = CombinedLinearGaussianTransitionModel([ + ConstantVelocity(0.01), ConstantVelocity(0.01)]) + # Define the initial target state initial_target_state = GaussianState([25, 1, 75, -0.5], np.diag([1, 1, 1, 1]) ** 2, @@ -66,7 +69,7 @@ # Set up the ground truth simulation groundtruth_simulation = SingleTargetGroundTruthSimulator( - transition_model=transition_model, + transition_model=gnd_transition_model, initial_state=initial_target_state, timestep=timedelta(seconds=1), number_steps=number_of_steps) @@ -356,10 +359,37 @@ def detections_gen(self): detector= None, hypothesiser= hypothesiser, updater= updater, - reducer= ch_reducer, - birth_component= ch_birth_component, + reducer= deepcopy(ch_reducer), + birth_component= deepcopy(ch_birth_component), extraction_threshold= 0.9) +from stonesoup.initiator.simple import MultiMeasurementInitiator +from stonesoup.types.state import GaussianState +from stonesoup.dataassociator.neighbour import GNNWith2DAssignment +from stonesoup.hypothesiser.distance import DistanceHypothesiser +from stonesoup.measures import Mahalanobis +init = MultiMeasurementInitiator( + GaussianState( + np.array([[20], [0], [10], [0]]), # Prior State + np.diag([1, 0, 1, 0])), + measurement_model=None, + deleter=deleter, + data_associator=ch_birth_component, + # GNNWith2DAssignment( + # DistanceHypothesiser(PF_predictor, updater, Mahalanobis(), missed_distance=5)), + updater=updater, + min_points=2) + +from stonesoup.tracker.simple import MultiTargetTracker +test_track_fusion = MultiTargetTracker( +updater = updater, +#hypothesiser = hypothesiser, + initiator= init, + detector = None, + deleter= deleter, + data_associator= data_associator_PF +) + # %% # 3) Run the trackers, generate the partial tracks and the final composite track; # ------------------------------------------------------------------------------- @@ -484,24 +514,25 @@ def detections_gen(self): # We have now the track for each radar now let's perform the # track fusion - # for PF_track_meas in [PF_track1, PF_track2]: - # dummy_detector_PF = DummyDetector(current=[time, PF_track_meas]) - # track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector_PF) + for PF_track_meas in [PF_track1, PF_track2]: + dummy_detector_PF = DummyDetector(current=[time, PF_track_meas]) + test_track_fusion.detector = Tracks2GaussianDetectionFeeder(dummy_detector_PF) + test_track_fusion.__iter__() + _, tracks = next(test_track_fusion) + PF_fused_track.update(tracks) + # for KF_track_meas in [KF_track1, KF_track2]: + # dummy_detector_KF = DummyDetector(current=[time, KF_track_meas]) + # track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector_KF) # track_fusion_tracker.__iter__() # _, tracks = next(track_fusion_tracker) - # PF_fused_track.update(tracks) + # KF_fused_track.update(tracks) + # sys.exit() + metric_manager.add_data({'KF_fused_track': KF_fused_track}, overwrite=False) + + # ERROR TypeError: __init__() missing 1 required positional argument: 'covar' #metric_manager.add_data({'PF_fused_track': PF_fused_track}, overwrite=False) - for KF_track_meas in [KF_track1, KF_track2]: - dummy_detector_KF = DummyDetector(current=[time, KF_track_meas]) - track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector_KF) - track_fusion_tracker.__iter__() - _, tracks = next(track_fusion_tracker) - KF_fused_track.update(tracks) - sys.exit() - metric_manager.add_data({'KF_fused_track': KF_fused_track}, overwrite=False) - truths = set() truths = set(groundtruth_simulation.groundtruth_paths) @@ -510,14 +541,15 @@ def detections_gen(self): # %% # Let's visualise the various tracks and detections in the cases # using the Kalman and Particle filters. -# + +print(KF_fused_track) plotter = Plotterly() -#plotter.plot_tracks(PF_track1, [0,2]) -#plotter.plot_tracks(PF_track2, [0,2]) +plotter.plot_tracks(PF_track1, [0,2]) +plotter.plot_tracks(PF_track2, [0,2]) #plotter.plot_tracks(PF_fused_track, [0, 2]) -plotter.plot_tracks(KF_fused_track, [0, 2]) -plotter.plot_tracks(KF_track1, [0, 2]) -plotter.plot_tracks(KF_track2, [0, 2]) +# plotter.plot_tracks(KF_fused_track, [0, 2], line=dict(color="brown")) +# plotter.plot_tracks(KF_track1, [0, 2]) +# plotter.plot_tracks(KF_track2, [0, 2]) plotter.plot_ground_truths(truths, [0, 2]) plotter.fig.show() From 13f5dde3a8744f3eb736391977e32b015b8f4c60 Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Fri, 13 Oct 2023 15:02:35 +0100 Subject: [PATCH 04/11] completed the example regarding the track fusion comparison between the PF and the KF, added the change in the feeder tracking for track fusion, it works ok --- docs/examples/track_fusion_example.py | 449 ++++++++++++++------------ stonesoup/feeder/track.py | 2 + 2 files changed, 247 insertions(+), 204 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index ba4dc373b..fe259be7f 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -1,8 +1,13 @@ +#!/usr/bin/env python +# coding: utf-8 + +""" +========================================================== +Comparing different filters in the context of track fusion +========================================================== +""" + # %% -# -# Comparing different filters in the context of track fusion -# ---------------------------------------------------------- -# # This example shows a comparison between a Kalman filter and # a particle filter in the context of track fusion. This example is # relevant to show how to get an unique track @@ -10,11 +15,11 @@ # different measurements obtained from independent sensors. # This example simulates the case of a single target moving in # a 2D cartesian space with measurements obtained from two -# identical, for simplicity, radars and trackers, then we +# identical, for simplicity, radars and trackers. We # compare the resulting composite track obtained by the # two different tracks generated. Furthermore, we # measure the performances of the tracks obtained -# by two different filters, Kalman and particle. +# by two different Kalman and particle filters. # # This example follows this structure: # 1) Initialise the sensors and the target trajectory; @@ -34,52 +39,63 @@ # The example setup is simple to it is easier to understand # how the Stone soup components are working. -# Load the various packages +# %% +# General imports +# ^^^^^^^^^^^^^^^ + import numpy as np from datetime import datetime from datetime import timedelta from copy import deepcopy +# %% # Stone Soup general imports +# ^^^^^^^^^^^^^^^^^^^^^^^^^^ +# from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \ ConstantVelocity from stonesoup.types.state import GaussianState from stonesoup.types.array import CovarianceMatrix +from stonesoup.simulator.simple import SingleTargetGroundTruthSimulator - +# %% # Simulation parameters setup -start_time = datetime(2023,8,1, 10,0,0) # For simplicity fix a date +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^ +# +start_time = datetime.now().replace(microsecond=0) # For simplicity fix a date number_of_steps = 75 # Number of timestep for the simulation np.random.seed(1908) # Random seed for reproducibility +n_particles= 2**8 # Instantiate the target transition model gnd_transition_model = CombinedLinearGaussianTransitionModel([ ConstantVelocity(0.00), ConstantVelocity(0.00)]) +# the transition model needs to have little more noise for the PF transition_model = CombinedLinearGaussianTransitionModel([ - ConstantVelocity(0.01), ConstantVelocity(0.01)]) + ConstantVelocity(0.2), ConstantVelocity(0.2)]) # Define the initial target state -initial_target_state = GaussianState([25, 1, 75, -0.5], - np.diag([1, 1, 1, 1]) ** 2, +initial_target_state = GaussianState([25, 0.5, 75, -0.25], + np.diag([5, 1, 5, 1]) ** 2, timestamp=start_time) -# Single target groundtruth Simulator -from stonesoup.simulator.simple import SingleTargetGroundTruthSimulator # Set up the ground truth simulation groundtruth_simulation = SingleTargetGroundTruthSimulator( transition_model=gnd_transition_model, initial_state=initial_target_state, - timestep=timedelta(seconds=1), + timestep=timedelta(seconds=2), number_steps=number_of_steps) + +# %% # Load a clutter model +# ^^^^^^^^^^^^^^^^^^^^ from stonesoup.models.clutter.clutter import ClutterModel -# Define the clutter model clutter_model = ClutterModel( - clutter_rate=1.0, + clutter_rate=0.6, distribution=np.random.default_rng().uniform, dist_params=((0,120), (-5,105))) # dist_params describe the area where the clutter is detected @@ -88,12 +104,21 @@ clutter_area = np.prod(np.diff(clutter_model.dist_params)) clutter_spatial_density = clutter_model.clutter_rate/clutter_area +# %% +# Instantiate the sensors, platforms and simulators +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +# # Instantiate the radars to collect measurements - Use a BearingRange radar from stonesoup.sensor.radar.radar import RadarBearingRange +# Import the platform to place the sensors on +from stonesoup.platform.base import FixedPlatform +# Load the platform detection simulator - Let's use a simulator for each radar +# Instantiate the simulators +from stonesoup.simulator.platform import PlatformDetectionSimulator # Let's assume that both radars have the same noise covariance for simplicity # These radars will have the +/-0.005 degrees accuracy in bearing and +/- 2 meters in range -radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 2.01**2])) +radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 2.0**2])) # Define the specifications of the two radars radar1 = RadarBearingRange( @@ -106,25 +131,21 @@ # deep copy the first radar specs. Changes in the first one does not influence the second radar2 = deepcopy(radar1) -# Import the platform to place the sensors on -from stonesoup.platform.base import FixedPlatform - # Instantiate the first sensor platform and add the sensor sensor1_platform = FixedPlatform( - states=GaussianState([10,0,80,0], np.diag([1,0,1,0])), + states=GaussianState([10, 0, 80, 0], + np.diag([1, 0, 1, 0])), position_mapping= (0, 2), sensors= [radar1]) # Instantiate the second one sensor2_platform = FixedPlatform( -states=GaussianState([75,0,10,0], np.diag([1,0,1,0])), + states=GaussianState([75, 0, 10, 0], + np.diag([1, 0, 1, 0])), position_mapping= (0, 2), sensors= [radar2]) -# Load the platform detection simulator - Let's use a simulator for each radar -# Instantiate the simulators -from stonesoup.simulator.platform import PlatformDetectionSimulator - +# create the radar simulators radar_simulator1 = PlatformDetectionSimulator( groundtruth= groundtruth_simulation, platforms= [sensor1_platform]) @@ -133,9 +154,16 @@ groundtruth= groundtruth_simulation, platforms= [sensor2_platform]) +# create copy of the simulators +from itertools import tee +_, *r1simulators = tee(radar_simulator1, 3) +_, *r2simulators = tee(radar_simulator2, 3) + + + # %% # Visualise the detections from the sensors -# ----------------------------------------- +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # Before creating the different trackers components # let's visualise the target and its detections from the # two sensors. In this way we can appreciate @@ -150,33 +178,30 @@ s1_detections = [] s2_detections = [] -# show1 = deepcopy(radar_simulator1) -# show2 = deepcopy(radar_simulator2) +detections1 = deepcopy(radar_simulator1) +detections2 = deepcopy(radar_simulator2) + # # Extract the generator function for the detections -# g1 = show1.detections_gen() -# g2 = show2.detections_gen() -# -# truths = set() -# -# # Iterate over the time steps, extracting the detections and truths -# for _ in range(number_of_steps): -# s1_detections.append(next(g1)[1]) -# s2_detections.append(next(g2)[1]) -# -# # Generate the ground truth -# #truths = set(groundtruth_simulation.groundtruth_paths) -# -# # Plot the groundtruth and detections from the two radars -# plotter = Plotterly() -# #plotter.plot_ground_truths(truths, [0, 2]) -# plotter.plot_measurements(s1_detections, [0, 2], marker= dict(color='orange', symbol='305'), -# measurements_label='Sensor 1 measurements') -# plotter.plot_measurements(s2_detections, [0, 2], marker= dict(color='blue', symbol='0'), -# measurements_label='Sensor 2 measurements') -# plotter.plot_sensors({sensor1_platform,sensor2_platform}, [0,1], marker= dict(color='black', symbol= '1', -# size=10)) -# plotter.fig.show() -# sys.exit() +g1 = detections1.detections_gen() +g2 = detections2.detections_gen() + + +# Iterate over the time steps, extracting the detections and truths +for _ in range(number_of_steps): + s1_detections.append(next(g1)[1]) + s2_detections.append(next(g2)[1]) + + +# Plot the detections from the two radars +plotter = Plotterly() +plotter.plot_measurements(s1_detections, [0, 2], marker= dict(color='orange', symbol='305'), + measurements_label='Sensor 1 measurements') +plotter.plot_measurements(s2_detections, [0, 2], marker= dict(color='blue', symbol='0'), + measurements_label='Sensor 2 measurements') +plotter.plot_sensors({sensor1_platform,sensor2_platform}, [0,1], marker= dict(color='black', symbol= '1', + size=10)) +plotter.fig + # %% # 2) Initialise the trackers components @@ -187,8 +212,13 @@ # are slightly different one from the # other, that will lead to two separate # tracks. Now, we initialise the two trackers -# components, one using a Kalman filter and the other -# a particle filter. +# components, one using a extended Kalman filter +# and a particle filter. +# + +# %% +# Stone soup tracker components +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # # Let's consider a Distance based hypothesiser @@ -205,9 +235,13 @@ # Load a single target tracker from stonesoup.tracker.simple import SingleTargetTracker +# prepare the particle components +from stonesoup.predictor.particle import ParticlePredictor +from stonesoup.updater.particle import ParticleUpdater +from stonesoup.resampler.particle import ESSResampler + # Lets define an helper function to minimise the number of times # we have to initialise the same tracker object - def general_tracker(tracker_class, detector, filter_updater, initiator, deleter, data_associator): @@ -228,7 +262,7 @@ def general_tracker(tracker_class, detector, KF_predictor = ExtendedKalmanPredictor(transition_model) # Instantiate the Kalman filter updater -KF_updater = ExtendedKalmanUpdater(measurement_model= None) +KF_updater = ExtendedKalmanUpdater(measurement_model=None) # create an track initiator placed on the target track origin initiator = SimpleMeasurementInitiator( @@ -238,9 +272,9 @@ def general_tracker(tracker_class, detector, # define the hypothesiser hypothesiser_KF = DistanceHypothesiser( predictor=KF_predictor, - updater= KF_updater, - measure= Mahalanobis(), - missed_distance= 5 + updater=KF_updater, + measure=Mahalanobis(), + missed_distance=5 ) # define the distance data associator @@ -249,23 +283,18 @@ def general_tracker(tracker_class, detector, # define a track time deleter deleter = UpdateTimeStepsDeleter(5) -# prepare the particle components -from stonesoup.predictor.particle import ParticlePredictor -from stonesoup.updater.particle import ParticleUpdater -from stonesoup.resampler.particle import ESSResampler - # Instantiate the predictor, particle resampler and particle # filter updater PF_predictor = ParticlePredictor(transition_model) resampler = ESSResampler() -PF_updater = ParticleUpdater(measurement_model= None, - resampler= resampler) +PF_updater = ParticleUpdater(measurement_model=None, + resampler=resampler) hypothesiser_PF = DistanceHypothesiser( - predictor= PF_predictor, - updater= PF_updater, - measure= Mahalanobis(), - missed_distance= 5) + predictor=PF_predictor, + updater=PF_updater, + measure=Mahalanobis(), + missed_distance=10) # define the data associator data_associator_PF = GNNWith2DAssignment(hypothesiser_PF) @@ -273,13 +302,13 @@ def general_tracker(tracker_class, detector, # To instantiate the track initiator we define a prior state # as gaussian state with the target track origin prior_state= SimpleMeasurementInitiator( - prior_state=GaussianState([25, 1, 70, -0.5], - np.diag([1, 0.01, 1, 0.01]) ** 2)) + prior_state=GaussianState([25, 0.5, 70, -0.25], + np.diag([10, 2, 10, 2]) ** 2)) # Particle filter initiator PF_initiator = GaussianParticleInitiator( - initiator= prior_state, - number_particles= 500) # low number for quicker computations + initiator=prior_state, + number_particles=n_particles) # %% # At this stage we have all the components needed to @@ -304,6 +333,11 @@ def detections_gen(self): yield self.current +# %% +# Stone soup track fusion components +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +# + # Load the ChernoffUpdater components for track fusion from stonesoup.updater.chernoff import ChernoffUpdater from stonesoup.updater.pointprocess import PHDUpdater @@ -318,29 +352,29 @@ def detections_gen(self): # Instantiate the PHD Updater, including the probability of # detection and probability of survival -updater = PHDUpdater(updater= ch_updater, - clutter_spatial_density= clutter_spatial_density, - prob_detection= 0.9, - prob_survival= 0.9) +updater = PHDUpdater(updater=ch_updater, + clutter_spatial_density=clutter_spatial_density, + prob_detection=0.9, + prob_survival=0.9) # Create a base hypothesiser using the Chernoff updater and # the kalman predictor base_hypothesiser = DistanceHypothesiser( - predictor= KF_predictor, - updater= ch_updater, - measure= Mahalanobis(), - missed_distance= 100) + predictor=KF_predictor, + updater=ch_updater, + measure=Mahalanobis(), + missed_distance=100) # Instantiate the Gaussian Mixture hypothesiser hypothesiser= GaussianMixtureHypothesiser(base_hypothesiser, - order_by_detection= True) + order_by_detection=True) # Gaussian mixture reducer to prune and merge the various tracks ch_reducer = GaussianMixtureReducer( - prune_threshold= 1e-10, - pruning= True, - merge_threshold= 100, - merging= True) + prune_threshold=1e-10, + pruning=True, + merge_threshold=100, + merging=True) # Create the covariance for the birth of the tracks, # large on the x,y location and smaller on the velocities @@ -348,52 +382,26 @@ def detections_gen(self): # Define the Chernoff updated birth components for the tracks ch_birth_component = TaggedWeightedGaussianState( - state_vector=[25, 1, 70, -0.5], # Initial target state + state_vector=[25, 0.5, 70, -0.25], # Initial target state covar=birth_covar**2, - weight= 0.5, - tag = TaggedWeightedGaussianState.BIRTH, - timestamp= start_time) + weight=0.5, + tag =TaggedWeightedGaussianState.BIRTH, + timestamp=start_time) # Instantiate the Track fusion tracker using Point Process Tracker track_fusion_tracker = PointProcessMultiTargetTracker( - detector= None, - hypothesiser= hypothesiser, - updater= updater, - reducer= deepcopy(ch_reducer), - birth_component= deepcopy(ch_birth_component), - extraction_threshold= 0.9) - -from stonesoup.initiator.simple import MultiMeasurementInitiator -from stonesoup.types.state import GaussianState -from stonesoup.dataassociator.neighbour import GNNWith2DAssignment -from stonesoup.hypothesiser.distance import DistanceHypothesiser -from stonesoup.measures import Mahalanobis -init = MultiMeasurementInitiator( - GaussianState( - np.array([[20], [0], [10], [0]]), # Prior State - np.diag([1, 0, 1, 0])), - measurement_model=None, - deleter=deleter, - data_associator=ch_birth_component, - # GNNWith2DAssignment( - # DistanceHypothesiser(PF_predictor, updater, Mahalanobis(), missed_distance=5)), + detector=None, + hypothesiser=hypothesiser, updater=updater, - min_points=2) - -from stonesoup.tracker.simple import MultiTargetTracker -test_track_fusion = MultiTargetTracker( -updater = updater, -#hypothesiser = hypothesiser, - initiator= init, - detector = None, - deleter= deleter, - data_associator= data_associator_PF -) + reducer=deepcopy(ch_reducer), + birth_component=deepcopy(ch_birth_component), + extraction_threshold=0.9) +track_fusion_tracker2 = deepcopy(track_fusion_tracker) # %% # 3) Run the trackers, generate the partial tracks and the final composite track; # ------------------------------------------------------------------------------- -# So far we have shown how to instantiate the various tracker components +# So far, we have shown how to instantiate the various tracker components # as well as the track fusion tracker. Now, we run the trackers to generate # the tracks and we perform the track fusion. Furthermore, we want to measure # how good are the track fusions in comparisons to the groundtruths, so we @@ -403,42 +411,68 @@ def detections_gen(self): # Instantiate the metric manager from stonesoup.metricgenerator.basicmetrics import BasicMetrics -basic_KF = BasicMetrics(generator_name='Kalman Filter', tracks_key='KF_fused_tracks', +basic_KF = BasicMetrics(generator_name='KF_fused', tracks_key='KF_fused_tracks', truths_key='truths') -basic_KF1 = BasicMetrics(generator_name='Kalman Filter', tracks_key='KF_2_tracks', +basic_KF1 = BasicMetrics(generator_name='KF1', tracks_key='KF_1_tracks', truths_key='truths') -basic_KF2 = BasicMetrics(generator_name='Kalman Filter', tracks_key='KF_2_tracks', +basic_KF2 = BasicMetrics(generator_name='KF2', tracks_key='KF_2_tracks', truths_key='truths') -basic_PF = BasicMetrics(generator_name='Particle Filter', tracks_key='PF__fused_tracks', +basic_PF = BasicMetrics(generator_name='PF_fused', tracks_key='PF_fused_tracks', truths_key='truths') -basic_PF1 = BasicMetrics(generator_name='Particle Filter', tracks_key='PF_1_tracks', +basic_PF1 = BasicMetrics(generator_name='PF1', tracks_key='PF_1_tracks', truths_key='truths') -basic_PF2 = BasicMetrics(generator_name='Particle Filter', tracks_key='PF_2_tracks', +basic_PF2 = BasicMetrics(generator_name='PF2', tracks_key='PF_2_tracks', truths_key='truths') -# Load the OSPA metric managers -from stonesoup.metricgenerator.ospametric import OSPAMetric -ospa_KF_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_KF_truths', - tracks_key= 'KF_fused_tracks', truths_key='truths') -ospa_PF_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_PF_truths', - tracks_key= 'PF_fused_tracks', truths_key='truths') - -ospa_KF1_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_KF_1_truths', - tracks_key= 'KF_1_tracks', truths_key='truths') -ospa_PF1_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_PF_1_truths', - tracks_key= 'PF_2_tracks', truths_key='truths') - -ospa_KF2_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_KF_2_truths', - tracks_key= 'KF_1_tracks', truths_key='truths') -ospa_PF2_truth = OSPAMetric(c=40, p=1, generator_name='OSPA_PF_2_truths', - tracks_key= 'PF_2_tracks', truths_key='truths') +# Load the SIAP metric +from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics +from stonesoup.measures import Euclidean + +siap_kf_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_KF_fused-truth', + tracks_key='KF_fused_tracks', + truths_key='truths' + ) +siap_kf1_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_KF1-truth', + tracks_key='KF_1_tracks', + truths_key='truths' + ) +siap_kf2_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_KF2-truth', + tracks_key='KF_2_tracks', + truths_key='truths' + ) + +siap_pf_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_PF_fused-truth', + tracks_key='PF_fused_tracks', + truths_key='truths' + ) +siap_pf1_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_PF1-truth', + tracks_key='PF_1_tracks', + truths_key='truths' + ) + +siap_pf2_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_PF2-truth', + tracks_key='PF_2_tracks', + truths_key='truths' + ) # Define a data associator between the tracks and the truths from stonesoup.dataassociator.tracktotrack import TrackToTruth -associator = TrackToTruth(association_threshold= 30) +associator = TrackToTruth(association_threshold=30) from stonesoup.metricgenerator.manager import MultiManager metric_manager = MultiManager([basic_KF, @@ -447,49 +481,55 @@ def detections_gen(self): basic_PF, basic_PF1, basic_PF2, - ospa_KF_truth, - ospa_KF1_truth, - ospa_KF2_truth, - ospa_PF_truth, - ospa_PF1_truth, - ospa_PF2_truth], + siap_kf_truth, + siap_kf1_truth, + siap_kf2_truth, + siap_pf_truth, + siap_pf1_truth, + siap_pf2_truth], associator) -# Create the tracks for the Particle filters, kalman and merged ones +# Create the tracks for the particle filters, Kalman and merged ones PF_track1, PF_track2, KF_track1, KF_track2 = set(), set(), set(), set() PF_fused_track, KF_fused_track = set(), set() # Instantiate the various trackers using the general_tracker function, # we assign a unique tracker the detections from a specific radar simulator -KF_tracker_1 = general_tracker(SingleTargetTracker, radar_simulator1, KF_updater, +KF_tracker_1 = general_tracker(SingleTargetTracker, r1simulators[0], KF_updater, initiator, deleter, data_associator_KF) -KF_tracker_2 = general_tracker(SingleTargetTracker, radar_simulator2, KF_updater, +KF_tracker_2 = general_tracker(SingleTargetTracker, r2simulators[0], KF_updater, initiator, deleter, data_associator_KF) -PF_tracker_1 = general_tracker(SingleTargetTracker, radar_simulator1, PF_updater, +PF_tracker_1 = general_tracker(SingleTargetTracker, r1simulators[1], PF_updater, PF_initiator, deleter, data_associator_PF) -PF_tracker_2 = general_tracker(SingleTargetTracker, radar_simulator2, PF_updater, +PF_tracker_2 = general_tracker(SingleTargetTracker, r2simulators[1], PF_updater, PF_initiator, deleter, data_associator_PF) # Load the detection generator g1 = radar_simulator1.detections_gen() g2 = radar_simulator2.detections_gen() +# Each tracker will have the same data +gg1 = deepcopy(radar_simulator1).detections_gen() +gg2 = deepcopy(radar_simulator2).detections_gen() + + # Loop on the simulation steps -for _ in range(number_of_steps): +for aa in range(number_of_steps): radar1_detections = next(g1) # Run the Particle Filter tracker on the radars measurements - PF_tracker_1.detector = DummyDetector(current= radar1_detections) + PF_tracker_1.detector = DummyDetector(current=radar1_detections) PF_tracker_1.__iter__() _, PF_sensor_track1 = next(PF_tracker_1) PF_track1.update(PF_sensor_track1) + rd1 = next(gg1) # Run the Kalman Filter tracker on the radars measurements - KF_tracker_1.detector = DummyDetector(current=radar1_detections) + KF_tracker_1.detector = DummyDetector(current=rd1) KF_tracker_1.__iter__() _, KF_sensor_track1 = next(KF_tracker_1) KF_track1.update(KF_sensor_track1) @@ -498,59 +538,57 @@ def detections_gen(self): PF_tracker_2.detector = DummyDetector(current=radar2_detections) PF_tracker_2.__iter__() - time, PF_sensor_track2 = next(PF_tracker_2) + pf_time, PF_sensor_track2 = next(PF_tracker_2) PF_track2.update(PF_sensor_track2) - KF_tracker_2.detector = DummyDetector(current=radar2_detections) + rd2 = next(gg2) + + KF_tracker_2.detector = DummyDetector(current=rd2) KF_tracker_2.__iter__() - time, KF_sensor_track2 = next(KF_tracker_2) + kf_time, KF_sensor_track2 = next(KF_tracker_2) KF_track2.update(KF_sensor_track2) - # load the various bits -# metric_manager.add_data({'KF_1_tracks': KF_track1}, overwrite=False) -# metric_manager.add_data({'KF_2_tracks': KF_track2}, overwrite=False) - metric_manager.add_data({'PF_1_tracks': PF_track1}, overwrite=False) - metric_manager.add_data({'PF_2_tracks': PF_track2}, overwrite=False) - # We have now the track for each radar now let's perform the # track fusion for PF_track_meas in [PF_track1, PF_track2]: - dummy_detector_PF = DummyDetector(current=[time, PF_track_meas]) - test_track_fusion.detector = Tracks2GaussianDetectionFeeder(dummy_detector_PF) - test_track_fusion.__iter__() - _, tracks = next(test_track_fusion) + dummy_detector_PF = DummyDetector(current=[pf_time, PF_track_meas]) + track_fusion_tracker2.detector = Tracks2GaussianDetectionFeeder(dummy_detector_PF) + track_fusion_tracker2.__iter__() + _, tracks = next(track_fusion_tracker2) PF_fused_track.update(tracks) - # for KF_track_meas in [KF_track1, KF_track2]: - # dummy_detector_KF = DummyDetector(current=[time, KF_track_meas]) - # track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector_KF) - # track_fusion_tracker.__iter__() - # _, tracks = next(track_fusion_tracker) - # KF_fused_track.update(tracks) - # sys.exit() - metric_manager.add_data({'KF_fused_track': KF_fused_track}, overwrite=False) + for KF_track_meas in [KF_track1, KF_track2]: + dummy_detector_KF = DummyDetector(current=[kf_time, KF_track_meas]) + track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector_KF) + track_fusion_tracker.__iter__() + _, tracks = next(track_fusion_tracker) + KF_fused_track.update(tracks) + + +# load the various tracks +metric_manager.add_data({'KF_1_tracks': KF_track1, + 'KF_2_tracks': KF_track2, + 'PF_1_tracks': PF_track1, + 'PF_2_tracks': PF_track2}, overwrite=False) - # ERROR TypeError: __init__() missing 1 required positional argument: 'covar' - #metric_manager.add_data({'PF_fused_track': PF_fused_track}, overwrite=False) +metric_manager.add_data({'KF_fused_tracks': KF_fused_track, + 'PF_fused_tracks': PF_fused_track, + }, overwrite=False) truths = set() truths = set(groundtruth_simulation.groundtruth_paths) - metric_manager.add_data({'truths': truths}, overwrite=False) - # %% # Let's visualise the various tracks and detections in the cases # using the Kalman and Particle filters. - -print(KF_fused_track) -plotter = Plotterly() -plotter.plot_tracks(PF_track1, [0,2]) -plotter.plot_tracks(PF_track2, [0,2]) -#plotter.plot_tracks(PF_fused_track, [0, 2]) -# plotter.plot_tracks(KF_fused_track, [0, 2], line=dict(color="brown")) -# plotter.plot_tracks(KF_track1, [0, 2]) -# plotter.plot_tracks(KF_track2, [0, 2]) +plotter.plot_tracks(PF_track1, [0,2], line=dict(color="orange"), track_label='PF partial track') +plotter.plot_tracks(PF_track2, [0,2], line=dict(color="gold"), track_label='PF partial track') +plotter.plot_tracks(PF_fused_track, [0, 2], line=dict(color="red"), track_label='PF fused track') +plotter.plot_tracks(KF_fused_track, [0, 2], line=dict(color="blue"), track_label='KF fused track') +plotter.plot_tracks(KF_track1, [0, 2], line=dict(color="cyan"), track_label='KF partial track') +plotter.plot_tracks(KF_track2, [0, 2], line=dict(color="azure"), track_label='KF partial track') plotter.plot_ground_truths(truths, [0, 2]) + plotter.fig.show() @@ -569,10 +607,14 @@ def detections_gen(self): metrics = metric_manager.generate_metrics() graph = MetricPlotter() -graph.plot_metrics(metrics, generator_names=['OSPA_KF_truths', - 'OSPA_PF_truths'], - color=['green', 'orange']) -graph.axes[0].set(ylabel='OSPA metrics', title='OSPA distances over time') + +graph.plot_metrics(metrics, generator_names=['SIAP_KF_fused-truth', + 'SIAP_PF_fused-truth', + 'SIAP_KF1-truth', + 'SIAP_KF2-truth', + 'SIAP_PF1-truth', + 'SIAP_PF2-truth'], + color=['blue', 'red', 'cyan', 'azure', 'orange', 'gold']) graph.fig.show() # %% @@ -580,8 +622,7 @@ def detections_gen(self): # ----------- # In this example we have shown how it is possible to # merge the tracks generated by independent trackers -# run on sets of data obtained by separate sensors. We -# have, also, compared how the Kalman and the Particle +# ran on sets of data obtained by separate sensors. We +# have, also, compared how the Kalman and the particle # filters behave in these cases, making track to track # comparisons. - diff --git a/stonesoup/feeder/track.py b/stonesoup/feeder/track.py index a157f4d53..37f2aebea 100644 --- a/stonesoup/feeder/track.py +++ b/stonesoup/feeder/track.py @@ -27,6 +27,8 @@ def data_gen(self): detections.add( GaussianDetection.from_state( track.state, + state_vector=track.mean, + covar=track.covar, measurement_model=LinearGaussian( dim, list(range(dim)), np.asarray(track.covar)), metadata=metadata, From b7fecf6f968a8c0a5108144e8844e244fd6e5466 Mon Sep 17 00:00:00 2001 From: jwright2 Date: Tue, 31 Oct 2023 22:32:48 +0000 Subject: [PATCH 05/11] Update grammar and white space of Track Fusion example --- docs/examples/track_fusion_example.py | 150 +++++++++++++------------- 1 file changed, 74 insertions(+), 76 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index fe259be7f..d5fb692d6 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -10,11 +10,11 @@ # %% # This example shows a comparison between a Kalman filter and # a particle filter in the context of track fusion. This example is -# relevant to show how to get an unique track +# relevant to show how to get a unique track # from partial tracks generated from a set of # different measurements obtained from independent sensors. # This example simulates the case of a single target moving in -# a 2D cartesian space with measurements obtained from two +# a 2D Cartesian space with measurements obtained from two # identical, for simplicity, radars and trackers. We # compare the resulting composite track obtained by the # two different tracks generated. Furthermore, we @@ -32,11 +32,11 @@ # %% # 1) Initialise the sensors and the target trajectory # --------------------------------------------------- -# We start creating two identical, in terms of performances, -# radars using :class:`~.RadarBearingRange` placed on two +# We start creating two identical, in terms of performance, +# radars using the :class:`~.RadarBearingRange` sensor placed on two # separate :class:`~.FixedPlatform`. For the target we # simulate a single object moving on a straight trajectory. -# The example setup is simple to it is easier to understand +# The example setup is simple so it is easier to understand # how the Stone soup components are working. # %% @@ -44,8 +44,7 @@ # ^^^^^^^^^^^^^^^ import numpy as np -from datetime import datetime -from datetime import timedelta +from datetime import datetime, timedelta from copy import deepcopy # %% @@ -62,10 +61,10 @@ # Simulation parameters setup # ^^^^^^^^^^^^^^^^^^^^^^^^^^^ # -start_time = datetime.now().replace(microsecond=0) # For simplicity fix a date +start_time = datetime.now().replace(microsecond=0) # For simplicity fix a date number_of_steps = 75 # Number of timestep for the simulation np.random.seed(1908) # Random seed for reproducibility -n_particles= 2**8 +n_particles = 2**8 # Instantiate the target transition model gnd_transition_model = CombinedLinearGaussianTransitionModel([ @@ -97,7 +96,7 @@ clutter_model = ClutterModel( clutter_rate=0.6, distribution=np.random.default_rng().uniform, - dist_params=((0,120), (-5,105))) + dist_params=((0, 120), (-5, 105))) # dist_params describe the area where the clutter is detected # Define the clutter area and spatial density for the tracker @@ -108,7 +107,7 @@ # Instantiate the sensors, platforms and simulators # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # -# Instantiate the radars to collect measurements - Use a BearingRange radar +# Instantiate the radars to collect measurements - Use a :class:`~.RangeBearingRange` radar. from stonesoup.sensor.radar.radar import RadarBearingRange # Import the platform to place the sensors on from stonesoup.platform.base import FixedPlatform @@ -122,11 +121,11 @@ # Define the specifications of the two radars radar1 = RadarBearingRange( - ndim_state= 4, - position_mapping= (0, 2), - noise_covar= radar_noise, - clutter_model= clutter_model, - max_range= 3000) # max_range can be removed and use the default value + ndim_state=4, + position_mapping=(0, 2), + noise_covar=radar_noise, + clutter_model=clutter_model, + max_range=3000) # max_range can be removed and use the default value # deep copy the first radar specs. Changes in the first one does not influence the second radar2 = deepcopy(radar1) @@ -135,24 +134,24 @@ sensor1_platform = FixedPlatform( states=GaussianState([10, 0, 80, 0], np.diag([1, 0, 1, 0])), - position_mapping= (0, 2), - sensors= [radar1]) + position_mapping=(0, 2), + sensors=[radar1]) # Instantiate the second one sensor2_platform = FixedPlatform( states=GaussianState([75, 0, 10, 0], np.diag([1, 0, 1, 0])), - position_mapping= (0, 2), - sensors= [radar2]) + position_mapping=(0, 2), + sensors=[radar2]) # create the radar simulators radar_simulator1 = PlatformDetectionSimulator( - groundtruth= groundtruth_simulation, - platforms= [sensor1_platform]) + groundtruth=groundtruth_simulation, + platforms=[sensor1_platform]) radar_simulator2 = PlatformDetectionSimulator( - groundtruth= groundtruth_simulation, - platforms= [sensor2_platform]) + groundtruth=groundtruth_simulation, + platforms=[sensor2_platform]) # create copy of the simulators from itertools import tee @@ -160,11 +159,10 @@ _, *r2simulators = tee(radar_simulator2, 3) - # %% # Visualise the detections from the sensors # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# Before creating the different trackers components +# Before creating the different trackers components, # let's visualise the target and its detections from the # two sensors. In this way we can appreciate # how the measurements are different and can lead to two separate @@ -194,12 +192,12 @@ # Plot the detections from the two radars plotter = Plotterly() -plotter.plot_measurements(s1_detections, [0, 2], marker= dict(color='orange', symbol='305'), +plotter.plot_measurements(s1_detections, [0, 2], marker=dict(color='orange', symbol='305'), measurements_label='Sensor 1 measurements') -plotter.plot_measurements(s2_detections, [0, 2], marker= dict(color='blue', symbol='0'), +plotter.plot_measurements(s2_detections, [0, 2], marker=dict(color='blue', symbol='0'), measurements_label='Sensor 2 measurements') -plotter.plot_sensors({sensor1_platform,sensor2_platform}, [0,1], marker= dict(color='black', symbol= '1', - size=10)) +plotter.plot_sensors({sensor1_platform, sensor2_platform}, [0, 1], + marker=dict(color='black', symbol='1', size=10)) plotter.fig @@ -211,8 +209,8 @@ # detections from the two sensors # are slightly different one from the # other, that will lead to two separate -# tracks. Now, we initialise the two trackers -# components, one using a extended Kalman filter +# tracks. Now, we initialise the components of the two trackers, +# one using a extended Kalman filter # and a particle filter. # @@ -250,10 +248,10 @@ def general_tracker(tracker_class, detector, """ tracker = tracker_class( - initiator= initiator, - detector= detector, - updater= filter_updater, - data_associator= data_associator, + initiator=initiator, + detector=detector, + updater=filter_updater, + data_associator=data_associator, deleter=deleter) return tracker @@ -301,9 +299,9 @@ def general_tracker(tracker_class, detector, # To instantiate the track initiator we define a prior state # as gaussian state with the target track origin -prior_state= SimpleMeasurementInitiator( +prior_state = SimpleMeasurementInitiator( prior_state=GaussianState([25, 0.5, 70, -0.25], - np.diag([10, 2, 10, 2]) ** 2)) + np.diag([10, 2, 10, 2]) ** 2)) # Particle filter initiator PF_initiator = GaussianParticleInitiator( @@ -312,13 +310,13 @@ def general_tracker(tracker_class, detector, # %% # At this stage we have all the components needed to -# perform the tracking using both Kalman and Particle +# perform the tracking using both Kalman and particle # filters. We need to create a way to perform the track fusion. # To perform such fusion, we employ the covariance # intersection algorithm # adopting the :class:`~.ChernoffUpdater` class, and # treating the tracks as measurements and we consider -# the measurements as GaussianMixture objects. +# the measurements as :class:`~.GaussianMixture` objects. # Instantiate a dummy detector to read the detections from stonesoup.buffered_generator import BufferedGenerator @@ -326,6 +324,7 @@ def general_tracker(tracker_class, detector, class DummyDetector(DetectionReader): def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) self.current = kwargs['current'] @BufferedGenerator.generator_method @@ -366,8 +365,8 @@ def detections_gen(self): missed_distance=100) # Instantiate the Gaussian Mixture hypothesiser -hypothesiser= GaussianMixtureHypothesiser(base_hypothesiser, - order_by_detection=True) +hypothesiser = GaussianMixtureHypothesiser(base_hypothesiser, + order_by_detection=True) # Gaussian mixture reducer to prune and merge the various tracks ch_reducer = GaussianMixtureReducer( @@ -385,7 +384,7 @@ def detections_gen(self): state_vector=[25, 0.5, 70, -0.25], # Initial target state covar=birth_covar**2, weight=0.5, - tag =TaggedWeightedGaussianState.BIRTH, + tag=TaggedWeightedGaussianState.BIRTH, timestamp=start_time) # Instantiate the Track fusion tracker using Point Process Tracker @@ -404,7 +403,7 @@ def detections_gen(self): # So far, we have shown how to instantiate the various tracker components # as well as the track fusion tracker. Now, we run the trackers to generate # the tracks and we perform the track fusion. Furthermore, we want to measure -# how good are the track fusions in comparisons to the groundtruths, so we +# how good are the track fusion methods are in comparison to the groundtruths, so we # instantiate a metric manager to measure the various distances. # @@ -415,17 +414,17 @@ def detections_gen(self): truths_key='truths') basic_KF1 = BasicMetrics(generator_name='KF1', tracks_key='KF_1_tracks', - truths_key='truths') + truths_key='truths') basic_KF2 = BasicMetrics(generator_name='KF2', tracks_key='KF_2_tracks', - truths_key='truths') + truths_key='truths') basic_PF = BasicMetrics(generator_name='PF_fused', tracks_key='PF_fused_tracks', truths_key='truths') basic_PF1 = BasicMetrics(generator_name='PF1', tracks_key='PF_1_tracks', - truths_key='truths') + truths_key='truths') basic_PF2 = BasicMetrics(generator_name='PF2', tracks_key='PF_2_tracks', - truths_key='truths') + truths_key='truths') # Load the SIAP metric from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics @@ -438,17 +437,17 @@ def detections_gen(self): truths_key='truths' ) siap_kf1_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), - velocity_measure=Euclidean((1, 3)), - generator_name='SIAP_KF1-truth', - tracks_key='KF_1_tracks', - truths_key='truths' - ) + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_KF1-truth', + tracks_key='KF_1_tracks', + truths_key='truths' + ) siap_kf2_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), - velocity_measure=Euclidean((1, 3)), - generator_name='SIAP_KF2-truth', - tracks_key='KF_2_tracks', - truths_key='truths' - ) + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_KF2-truth', + tracks_key='KF_2_tracks', + truths_key='truths' + ) siap_pf_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), velocity_measure=Euclidean((1, 3)), @@ -457,18 +456,18 @@ def detections_gen(self): truths_key='truths' ) siap_pf1_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), - velocity_measure=Euclidean((1, 3)), - generator_name='SIAP_PF1-truth', - tracks_key='PF_1_tracks', - truths_key='truths' - ) + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_PF1-truth', + tracks_key='PF_1_tracks', + truths_key='truths' + ) siap_pf2_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), - velocity_measure=Euclidean((1, 3)), - generator_name='SIAP_PF2-truth', - tracks_key='PF_2_tracks', - truths_key='truths' - ) + velocity_measure=Euclidean((1, 3)), + generator_name='SIAP_PF2-truth', + tracks_key='PF_2_tracks', + truths_key='truths' + ) # Define a data associator between the tracks and the truths from stonesoup.dataassociator.tracktotrack import TrackToTruth @@ -496,13 +495,13 @@ def detections_gen(self): # Instantiate the various trackers using the general_tracker function, # we assign a unique tracker the detections from a specific radar simulator KF_tracker_1 = general_tracker(SingleTargetTracker, r1simulators[0], KF_updater, - initiator, deleter, data_associator_KF) + initiator, deleter, data_associator_KF) KF_tracker_2 = general_tracker(SingleTargetTracker, r2simulators[0], KF_updater, initiator, deleter, data_associator_KF) PF_tracker_1 = general_tracker(SingleTargetTracker, r1simulators[1], PF_updater, - PF_initiator, deleter, data_associator_PF) + PF_initiator, deleter, data_associator_PF) PF_tracker_2 = general_tracker(SingleTargetTracker, r2simulators[1], PF_updater, PF_initiator, deleter, data_associator_PF) @@ -575,21 +574,20 @@ def detections_gen(self): 'PF_fused_tracks': PF_fused_track, }, overwrite=False) -truths = set() truths = set(groundtruth_simulation.groundtruth_paths) metric_manager.add_data({'truths': truths}, overwrite=False) # %% # Let's visualise the various tracks and detections in the cases -# using the Kalman and Particle filters. -plotter.plot_tracks(PF_track1, [0,2], line=dict(color="orange"), track_label='PF partial track') -plotter.plot_tracks(PF_track2, [0,2], line=dict(color="gold"), track_label='PF partial track') +# using the Kalman and particle filters. +plotter.plot_tracks(PF_track1, [0, 2], line=dict(color="orange"), track_label='PF partial track') +plotter.plot_tracks(PF_track2, [0, 2], line=dict(color="gold"), track_label='PF partial track') plotter.plot_tracks(PF_fused_track, [0, 2], line=dict(color="red"), track_label='PF fused track') plotter.plot_tracks(KF_fused_track, [0, 2], line=dict(color="blue"), track_label='KF fused track') plotter.plot_tracks(KF_track1, [0, 2], line=dict(color="cyan"), track_label='KF partial track') plotter.plot_tracks(KF_track2, [0, 2], line=dict(color="azure"), track_label='KF partial track') plotter.plot_ground_truths(truths, [0, 2]) -plotter.fig.show() +plotter.fig # %% @@ -615,7 +613,7 @@ def detections_gen(self): 'SIAP_PF1-truth', 'SIAP_PF2-truth'], color=['blue', 'red', 'cyan', 'azure', 'orange', 'gold']) -graph.fig.show() +graph.fig # %% # Conclusions From 844cef8ce1445d04b4d6a7f870191c67ff8a85a0 Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Thu, 21 Dec 2023 17:47:01 +0000 Subject: [PATCH 06/11] updated the example using the multifeeder class and improved the text and example like --- docs/examples/track_fusion_example.py | 259 +++++++++++--------------- 1 file changed, 111 insertions(+), 148 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index d5fb692d6..6d1840a1a 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -8,25 +8,25 @@ """ # %% -# This example shows a comparison between a Kalman filter and -# a particle filter in the context of track fusion. This example is +# This example shows a comparison between a Kalman filter algorithms +# and particle filter in the context of track fusion. This example is # relevant to show how to get a unique track -# from partial tracks generated from a set of +# from partial tracks generated from set of # different measurements obtained from independent sensors. # This example simulates the case of a single target moving in # a 2D Cartesian space with measurements obtained from two -# identical, for simplicity, radars and trackers. We -# compare the resulting composite track obtained by the -# two different tracks generated. Furthermore, we -# measure the performances of the tracks obtained -# by two different Kalman and particle filters. +# identical, for simplicity, radars with own trackers. +# We present a comparison of the resulting composite track +# obtained by the two different partial tracks generated. +# Furthermore, we measure the track-to-truth accuracy of +# the final track obtained by different algorithms. # # This example follows this structure: # 1) Initialise the sensors and the target trajectory; -# 2) Initialise the filters components and create the tracker; -# 3) Run the trackers, generate the partial tracks and the final +# 2) Initialise the filters components and create the trackers; +# 3) Run the trackers, generate the partial tracks and merge the # composite track; -# 4) Compare the obtained tracks with the groundtruth trajectory. +# 4) Evaluate the obtained tracks with the groundtruth trajectory. # # %% @@ -37,7 +37,7 @@ # separate :class:`~.FixedPlatform`. For the target we # simulate a single object moving on a straight trajectory. # The example setup is simple so it is easier to understand -# how the Stone soup components are working. +# how the algorithm components work. # %% # General imports @@ -61,8 +61,8 @@ # Simulation parameters setup # ^^^^^^^^^^^^^^^^^^^^^^^^^^^ # -start_time = datetime.now().replace(microsecond=0) # For simplicity fix a date -number_of_steps = 75 # Number of timestep for the simulation +start_time = datetime.now().replace(microsecond=0) +number_of_steps = 75 # Number of timestep of the simulation np.random.seed(1908) # Random seed for reproducibility n_particles = 2**8 @@ -70,31 +70,29 @@ gnd_transition_model = CombinedLinearGaussianTransitionModel([ ConstantVelocity(0.00), ConstantVelocity(0.00)]) -# the transition model needs to have little more noise for the PF +# the transition model needs to have little more process noise for the PF transition_model = CombinedLinearGaussianTransitionModel([ - ConstantVelocity(0.2), ConstantVelocity(0.2)]) + ConstantVelocity(0.5), ConstantVelocity(0.5)]) # Define the initial target state initial_target_state = GaussianState([25, 0.5, 75, -0.25], - np.diag([5, 1, 5, 1]) ** 2, + np.diag([100, 1, 100, 1]), timestamp=start_time) - # Set up the ground truth simulation groundtruth_simulation = SingleTargetGroundTruthSimulator( transition_model=gnd_transition_model, initial_state=initial_target_state, - timestep=timedelta(seconds=2), + timestep=timedelta(seconds=1), number_steps=number_of_steps) - # %% # Load a clutter model # ^^^^^^^^^^^^^^^^^^^^ from stonesoup.models.clutter.clutter import ClutterModel clutter_model = ClutterModel( - clutter_rate=0.6, + clutter_rate=1.0, distribution=np.random.default_rng().uniform, dist_params=((0, 120), (-5, 105))) # dist_params describe the area where the clutter is detected @@ -106,8 +104,8 @@ # %% # Instantiate the sensors, platforms and simulators # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# # Instantiate the radars to collect measurements - Use a :class:`~.RangeBearingRange` radar. +# from stonesoup.sensor.radar.radar import RadarBearingRange # Import the platform to place the sensors on from stonesoup.platform.base import FixedPlatform @@ -116,8 +114,8 @@ from stonesoup.simulator.platform import PlatformDetectionSimulator # Let's assume that both radars have the same noise covariance for simplicity -# These radars will have the +/-0.005 degrees accuracy in bearing and +/- 2 meters in range -radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 2.0**2])) +# These radars will have the +/-0.005 degrees accuracy in bearing and +/- 2.5 meters in range +radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 2.5**2])) # Define the specifications of the two radars radar1 = RadarBearingRange( @@ -144,28 +142,29 @@ position_mapping=(0, 2), sensors=[radar2]) +# create copy of the simulators +from itertools import tee +gt_sims = tee(groundtruth_simulation, 2) + # create the radar simulators radar_simulator1 = PlatformDetectionSimulator( - groundtruth=groundtruth_simulation, + groundtruth=gt_sims[0], platforms=[sensor1_platform]) radar_simulator2 = PlatformDetectionSimulator( - groundtruth=groundtruth_simulation, + groundtruth=gt_sims[1], platforms=[sensor2_platform]) -# create copy of the simulators -from itertools import tee -_, *r1simulators = tee(radar_simulator1, 3) -_, *r2simulators = tee(radar_simulator2, 3) - +radar1plot, radar1KF, radar1PF = tee(radar_simulator1, 3) +radar2plot, radar2KF, radar2PF = tee(radar_simulator2, 3) # %% # Visualise the detections from the sensors # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# Before creating the different trackers components, +# Before preparing the different trackers components, # let's visualise the target and its detections from the # two sensors. In this way we can appreciate -# how the measurements are different and can lead to two separate +# how the measurements are different and can lead to separate # tracks. # @@ -176,19 +175,14 @@ s1_detections = [] s2_detections = [] -detections1 = deepcopy(radar_simulator1) -detections2 = deepcopy(radar_simulator2) - -# # Extract the generator function for the detections -g1 = detections1.detections_gen() -g2 = detections2.detections_gen() - +grountruth_generator = groundtruth_simulation.groundtruth_paths_gen() +truths = set() # Iterate over the time steps, extracting the detections and truths -for _ in range(number_of_steps): - s1_detections.append(next(g1)[1]) - s2_detections.append(next(g2)[1]) - +for (time, sd1), (_, sd2) in zip(radar1plot, radar2plot): + s1_detections.append(sd1) + s2_detections.append(sd2) + truths.update(next(grountruth_generator)[1]) # consider only the path # Plot the detections from the two radars plotter = Plotterly() @@ -198,18 +192,18 @@ measurements_label='Sensor 2 measurements') plotter.plot_sensors({sensor1_platform, sensor2_platform}, [0, 1], marker=dict(color='black', symbol='1', size=10)) +plotter.plot_ground_truths(truths, [0, 2]) plotter.fig - # %% # 2) Initialise the trackers components # ------------------------------------- # We have initialised the sensors and the # target path, we can see that the # detections from the two sensors -# are slightly different one from the -# other, that will lead to two separate -# tracks. Now, we initialise the components of the two trackers, +# differ one from the other, that will lead to +# two separate tracks. +# Now, we initialise the components of the two trackers, # one using a extended Kalman filter # and a particle filter. # @@ -226,8 +220,8 @@ from stonesoup.deleter.time import UpdateTimeStepsDeleter # Load the kalman filter components -from stonesoup.updater.kalman import ExtendedKalmanUpdater -from stonesoup.predictor.kalman import ExtendedKalmanPredictor +from stonesoup.updater.kalman import UnscentedKalmanUpdater +from stonesoup.predictor.kalman import UnscentedKalmanPredictor from stonesoup.initiator.simple import SimpleMeasurementInitiator, GaussianParticleInitiator # Load a single target tracker @@ -257,29 +251,29 @@ def general_tracker(tracker_class, detector, # instantiate the Kalman filter predictor -KF_predictor = ExtendedKalmanPredictor(transition_model) +KF_predictor = UnscentedKalmanPredictor(transition_model) # Instantiate the Kalman filter updater -KF_updater = ExtendedKalmanUpdater(measurement_model=None) - -# create an track initiator placed on the target track origin -initiator = SimpleMeasurementInitiator( - prior_state=initial_target_state, - measurement_model=None) +KF_updater = UnscentedKalmanUpdater(measurement_model=None) # define the hypothesiser hypothesiser_KF = DistanceHypothesiser( predictor=KF_predictor, updater=KF_updater, measure=Mahalanobis(), - missed_distance=5 + missed_distance=10 ) # define the distance data associator data_associator_KF = GNNWith2DAssignment(hypothesiser_KF) # define a track time deleter -deleter = UpdateTimeStepsDeleter(5) +deleter = UpdateTimeStepsDeleter(3) + +# create an track initiator placed on the target track origin +initiator = SimpleMeasurementInitiator( + prior_state=initial_target_state, + measurement_model=None) # Instantiate the predictor, particle resampler and particle # filter updater @@ -292,7 +286,7 @@ def general_tracker(tracker_class, detector, predictor=PF_predictor, updater=PF_updater, measure=Mahalanobis(), - missed_distance=10) + missed_distance=5) # define the data associator data_associator_PF = GNNWith2DAssignment(hypothesiser_PF) @@ -313,24 +307,9 @@ def general_tracker(tracker_class, detector, # perform the tracking using both Kalman and particle # filters. We need to create a way to perform the track fusion. # To perform such fusion, we employ the covariance -# intersection algorithm -# adopting the :class:`~.ChernoffUpdater` class, and -# treating the tracks as measurements and we consider -# the measurements as :class:`~.GaussianMixture` objects. - -# Instantiate a dummy detector to read the detections -from stonesoup.buffered_generator import BufferedGenerator -from stonesoup.reader.base import DetectionReader - -class DummyDetector(DetectionReader): - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.current = kwargs['current'] - - @BufferedGenerator.generator_method - def detections_gen(self): - yield self.current - +# intersection algorithm adopting the +# :class:`~.ChernoffUpdater` class, treating the tracks +# as :class:`~.GaussianMixture` detections. # %% # Stone soup track fusion components @@ -345,6 +324,7 @@ def detections_gen(self): from stonesoup.tracker.pointprocess import PointProcessMultiTargetTracker from stonesoup.types.state import TaggedWeightedGaussianState from stonesoup.feeder.track import Tracks2GaussianDetectionFeeder +from stonesoup.feeder.multi import MultiDataFeeder # create the Chernoff Updater ch_updater = ChernoffUpdater(measurement_model=None) @@ -353,8 +333,8 @@ def detections_gen(self): # detection and probability of survival updater = PHDUpdater(updater=ch_updater, clutter_spatial_density=clutter_spatial_density, - prob_detection=0.9, - prob_survival=0.9) + prob_detection=0.99, + prob_survival=0.95) # Create a base hypothesiser using the Chernoff updater and # the kalman predictor @@ -362,7 +342,7 @@ def detections_gen(self): predictor=KF_predictor, updater=ch_updater, measure=Mahalanobis(), - missed_distance=100) + missed_distance=15) # Instantiate the Gaussian Mixture hypothesiser hypothesiser = GaussianMixtureHypothesiser(base_hypothesiser, @@ -377,13 +357,13 @@ def detections_gen(self): # Create the covariance for the birth of the tracks, # large on the x,y location and smaller on the velocities -birth_covar = CovarianceMatrix(np.diag([100, 1, 100, 1])) +birth_covar = CovarianceMatrix(np.diag([50, 5, 50, 5])) # Define the Chernoff updated birth components for the tracks ch_birth_component = TaggedWeightedGaussianState( state_vector=[25, 0.5, 70, -0.25], # Initial target state covar=birth_covar**2, - weight=0.5, + weight=1, tag=TaggedWeightedGaussianState.BIRTH, timestamp=start_time) @@ -394,17 +374,19 @@ def detections_gen(self): updater=updater, reducer=deepcopy(ch_reducer), birth_component=deepcopy(ch_birth_component), - extraction_threshold=0.9) + extraction_threshold=0.95) +# Copy the track fusion tracker track_fusion_tracker2 = deepcopy(track_fusion_tracker) + # %% -# 3) Run the trackers, generate the partial tracks and the final composite track; -# ------------------------------------------------------------------------------- +# 3) Run the trackers, generate the partial tracks and merge the final composite track; +# ------------------------------------------------------------------------------------- # So far, we have shown how to instantiate the various tracker components -# as well as the track fusion tracker. Now, we run the trackers to generate +# as well as the fusion tracker. Now, we run the trackers to generate # the tracks and we perform the track fusion. Furthermore, we want to measure -# how good are the track fusion methods are in comparison to the groundtruths, so we -# instantiate a metric manager to measure the various distances. +# the accuracy of the tracks obtained via the fusion algorithm compared with +# the truths. To do so we instantiate a metric manager to evalute the various distances. # # Instantiate the metric manager @@ -494,77 +476,55 @@ def detections_gen(self): # Instantiate the various trackers using the general_tracker function, # we assign a unique tracker the detections from a specific radar simulator -KF_tracker_1 = general_tracker(SingleTargetTracker, r1simulators[0], KF_updater, +KF_tracker_1 = general_tracker(SingleTargetTracker, radar1KF, KF_updater, initiator, deleter, data_associator_KF) -KF_tracker_2 = general_tracker(SingleTargetTracker, r2simulators[0], KF_updater, +KF_tracker_2 = general_tracker(SingleTargetTracker, radar2KF, KF_updater, initiator, deleter, data_associator_KF) -PF_tracker_1 = general_tracker(SingleTargetTracker, r1simulators[1], PF_updater, +PF_tracker_1 = general_tracker(SingleTargetTracker, radar1PF, PF_updater, PF_initiator, deleter, data_associator_PF) -PF_tracker_2 = general_tracker(SingleTargetTracker, r2simulators[1], PF_updater, +PF_tracker_2 = general_tracker(SingleTargetTracker, radar2PF, PF_updater, PF_initiator, deleter, data_associator_PF) -# Load the detection generator -g1 = radar_simulator1.detections_gen() -g2 = radar_simulator2.detections_gen() +PartialTrackPF1, TrackFusionPF1 = tee(PF_tracker_1, 2) +PartialTrackPF2, TrackFusionPF2 = tee(PF_tracker_2, 2) +PartialTrackKF1, TrackFusionKF1 = tee(KF_tracker_1, 2) +PartialTrackKF2, TrackFusionKF2 = tee(KF_tracker_2, 2) -# Each tracker will have the same data -gg1 = deepcopy(radar_simulator1).detections_gen() -gg2 = deepcopy(radar_simulator2).detections_gen() +# Create the detector feeding the tracker algorithms +track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder( + MultiDataFeeder([TrackFusionKF1, TrackFusionKF2])) +track_fusion_tracker2.detector = Tracks2GaussianDetectionFeeder( + MultiDataFeeder([TrackFusionPF1, TrackFusionPF2])) -# Loop on the simulation steps -for aa in range(number_of_steps): +iter_fusion_tracker = iter(track_fusion_tracker) +iter_fusion_tracker2 = iter(track_fusion_tracker2) - radar1_detections = next(g1) +# Loop over the timestep doing the track fusion and partial tracks +for _ in range(number_of_steps): + for _ in range(2): + _, tracks = next(iter_fusion_tracker) + KF_fused_track.update(tracks) + del tracks + _, tracks = next(iter_fusion_tracker2) + PF_fused_track.update(tracks) - # Run the Particle Filter tracker on the radars measurements - PF_tracker_1.detector = DummyDetector(current=radar1_detections) - PF_tracker_1.__iter__() - _, PF_sensor_track1 = next(PF_tracker_1) + _, PF_sensor_track1 = next(iter(PartialTrackPF1)) PF_track1.update(PF_sensor_track1) - rd1 = next(gg1) - # Run the Kalman Filter tracker on the radars measurements - KF_tracker_1.detector = DummyDetector(current=rd1) - KF_tracker_1.__iter__() - _, KF_sensor_track1 = next(KF_tracker_1) - KF_track1.update(KF_sensor_track1) - - radar2_detections = next(g2) - - PF_tracker_2.detector = DummyDetector(current=radar2_detections) - PF_tracker_2.__iter__() - pf_time, PF_sensor_track2 = next(PF_tracker_2) + _, PF_sensor_track2 = next(iter(PartialTrackPF2)) PF_track2.update(PF_sensor_track2) - rd2 = next(gg2) + _, KF_sensor_track1 = next(iter(PartialTrackKF1)) + KF_track1.update(KF_sensor_track1) - KF_tracker_2.detector = DummyDetector(current=rd2) - KF_tracker_2.__iter__() - kf_time, KF_sensor_track2 = next(KF_tracker_2) + _, KF_sensor_track2 = next(iter(PartialTrackKF2)) KF_track2.update(KF_sensor_track2) - # We have now the track for each radar now let's perform the - # track fusion - for PF_track_meas in [PF_track1, PF_track2]: - dummy_detector_PF = DummyDetector(current=[pf_time, PF_track_meas]) - track_fusion_tracker2.detector = Tracks2GaussianDetectionFeeder(dummy_detector_PF) - track_fusion_tracker2.__iter__() - _, tracks = next(track_fusion_tracker2) - PF_fused_track.update(tracks) - - for KF_track_meas in [KF_track1, KF_track2]: - dummy_detector_KF = DummyDetector(current=[kf_time, KF_track_meas]) - track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector_KF) - track_fusion_tracker.__iter__() - _, tracks = next(track_fusion_tracker) - KF_fused_track.update(tracks) - - -# load the various tracks +# Add the tracks to the metric manager metric_manager.add_data({'KF_1_tracks': KF_track1, 'KF_2_tracks': KF_track2, 'PF_1_tracks': PF_track1, @@ -579,25 +539,28 @@ def detections_gen(self): # %% # Let's visualise the various tracks and detections in the cases # using the Kalman and particle filters. -plotter.plot_tracks(PF_track1, [0, 2], line=dict(color="orange"), track_label='PF partial track') -plotter.plot_tracks(PF_track2, [0, 2], line=dict(color="gold"), track_label='PF partial track') +plotter.plot_tracks(PF_track1, [0, 2], line=dict(color="orange"), track_label='PF partial track 1') +plotter.plot_tracks(PF_track2, [0, 2], line=dict(color="gold"), track_label='PF partial track 2') plotter.plot_tracks(PF_fused_track, [0, 2], line=dict(color="red"), track_label='PF fused track') plotter.plot_tracks(KF_fused_track, [0, 2], line=dict(color="blue"), track_label='KF fused track') -plotter.plot_tracks(KF_track1, [0, 2], line=dict(color="cyan"), track_label='KF partial track') -plotter.plot_tracks(KF_track2, [0, 2], line=dict(color="azure"), track_label='KF partial track') -plotter.plot_ground_truths(truths, [0, 2]) +plotter.plot_tracks(KF_track1, [0, 2], line=dict(color="cyan"), track_label='KF partial track 1') +plotter.plot_tracks(KF_track2, [0, 2], line=dict(color="azure"), track_label='KF partial track 2') plotter.fig # %% -# 4) Compare the obtained tracks with the groundtruth trajectory. +# 4) Evaluate the obtained tracks with the groundtruth trajectory. # --------------------------------------------------------------- # At this stage we have almost completed our example. We have created the # detections from the radars, performed the tracking and the # fusion of the tracks. Now we use the :class:`~.MetricManager` # to generate summary statistics on the accuracy of the tracks -# in comparison to the groundtruth measurements. +# by comparing them with the groundtruth measurements. +# +# If we consider the SIAP metrics, we can appreciate that +# the fused tracks have a lower error compared to the +# partial tracks obtained with the single instruments. # # Loaded the plotter for the various metrics. @@ -622,5 +585,5 @@ def detections_gen(self): # merge the tracks generated by independent trackers # ran on sets of data obtained by separate sensors. We # have, also, compared how the Kalman and the particle -# filters behave in these cases, making track to track +# filters behave in these cases, making track to truth # comparisons. From 3414ffc5b0f01454c2061bfec8846ea724cbefee Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Thu, 15 Feb 2024 17:39:08 +0000 Subject: [PATCH 07/11] adressing the comments on the example, rewording the text and the sectioning --- docs/examples/track_fusion_example.py | 333 ++++++++++++-------------- 1 file changed, 153 insertions(+), 180 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index 6d1840a1a..5c03fbc98 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -8,36 +8,31 @@ """ # %% -# This example shows a comparison between a Kalman filter algorithms -# and particle filter in the context of track fusion. This example is -# relevant to show how to get a unique track -# from partial tracks generated from set of -# different measurements obtained from independent sensors. -# This example simulates the case of a single target moving in -# a 2D Cartesian space with measurements obtained from two -# identical, for simplicity, radars with own trackers. -# We present a comparison of the resulting composite track -# obtained by the two different partial tracks generated. -# Furthermore, we measure the track-to-truth accuracy of +# This example shows a comparison between a Kalman filter algorithms and particle filter in the +# context of track fusion. This example is relevant to show how to get a unique track +# from partial tracks generated from set of different measurements obtained from independent sensors. +# +# This example simulates the case of a single target moving in a 2D Cartesian space with +# measurements obtained from two identical, for simplicity, radars with their own trackers. +# We present a comparison of the resulting composite track obtained by the two different +# partial tracks generated. Furthermore, we measure the track-to-truth accuracy of # the final track obtained by different algorithms. # # This example follows this structure: -# 1) Initialise the sensors and the target trajectory; -# 2) Initialise the filters components and create the trackers; -# 3) Run the trackers, generate the partial tracks and merge the -# composite track; -# 4) Evaluate the obtained tracks with the groundtruth trajectory. +# +# 1. Initialise the sensors and the target trajectory; +# 2. Initialise the filters components and create the trackers; +# 3. Run the trackers, generate the partial tracks and merge the composite track; +# 4. Evaluate the obtained tracks with the groundtruth trajectory. # # %% -# 1) Initialise the sensors and the target trajectory +# 1. Initialise the sensors and the target trajectory # --------------------------------------------------- -# We start creating two identical, in terms of performance, -# radars using the :class:`~.RadarBearingRange` sensor placed on two -# separate :class:`~.FixedPlatform`. For the target we -# simulate a single object moving on a straight trajectory. -# The example setup is simple so it is easier to understand -# how the algorithm components work. +# We start creating two identical, in terms of performance, radars using the +# :class:`~.RadarBearingRange` sensor placed on two separate :class:`~.FixedPlatform`. +# For the target we simulate a single object moving on a straight trajectory. +# The example setup is simple so it is easier to understand how the algorithm components work. # %% # General imports @@ -46,37 +41,47 @@ import numpy as np from datetime import datetime, timedelta from copy import deepcopy +from itertools import tee # %% # Stone Soup general imports # ^^^^^^^^^^^^^^^^^^^^^^^^^^ -# + from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \ ConstantVelocity from stonesoup.types.state import GaussianState from stonesoup.types.array import CovarianceMatrix from stonesoup.simulator.simple import SingleTargetGroundTruthSimulator +from stonesoup.models.clutter.clutter import ClutterModel +# Instantiate the radars to collect measurements - Use a :class:`~.RadarBearingRange` radar.# +from stonesoup.sensor.radar.radar import RadarBearingRange + +# Import the platform to place the sensors on +from stonesoup.platform.base import FixedPlatform + +# Load the platform detection simulator +from stonesoup.simulator.platform import PlatformDetectionSimulator # %% # Simulation parameters setup # ^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# + start_time = datetime.now().replace(microsecond=0) number_of_steps = 75 # Number of timestep of the simulation np.random.seed(1908) # Random seed for reproducibility -n_particles = 2**8 +n_particles = 2**10 # Instantiate the target transition model gnd_transition_model = CombinedLinearGaussianTransitionModel([ ConstantVelocity(0.00), ConstantVelocity(0.00)]) -# the transition model needs to have little more process noise for the PF +# the transition model needs to have little more process noise for the Particle filter transition_model = CombinedLinearGaussianTransitionModel([ ConstantVelocity(0.5), ConstantVelocity(0.5)]) # Define the initial target state initial_target_state = GaussianState([25, 0.5, 75, -0.25], - np.diag([100, 1, 100, 1]), + np.diag([10, 1, 10, 1]), timestamp=start_time) # Set up the ground truth simulation @@ -89,7 +94,6 @@ # %% # Load a clutter model # ^^^^^^^^^^^^^^^^^^^^ -from stonesoup.models.clutter.clutter import ClutterModel clutter_model = ClutterModel( clutter_rate=1.0, @@ -104,18 +108,10 @@ # %% # Instantiate the sensors, platforms and simulators # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# Instantiate the radars to collect measurements - Use a :class:`~.RangeBearingRange` radar. -# -from stonesoup.sensor.radar.radar import RadarBearingRange -# Import the platform to place the sensors on -from stonesoup.platform.base import FixedPlatform -# Load the platform detection simulator - Let's use a simulator for each radar -# Instantiate the simulators -from stonesoup.simulator.platform import PlatformDetectionSimulator # Let's assume that both radars have the same noise covariance for simplicity -# These radars will have the +/-0.005 degrees accuracy in bearing and +/- 2.5 meters in range -radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 2.5**2])) +# These radars will have the +/-0.005 degrees accuracy in bearing and +/- 5 meters in range +radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 5])) # Define the specifications of the two radars radar1 = RadarBearingRange( @@ -128,7 +124,7 @@ # deep copy the first radar specs. Changes in the first one does not influence the second radar2 = deepcopy(radar1) -# Instantiate the first sensor platform and add the sensor +# prepare the first sensor platform and add the sensor sensor1_platform = FixedPlatform( states=GaussianState([10, 0, 80, 0], np.diag([1, 0, 1, 0])), @@ -142,8 +138,7 @@ position_mapping=(0, 2), sensors=[radar2]) -# create copy of the simulators -from itertools import tee +# create copy of the simulators, in this way all trackers will receive the same detections gt_sims = tee(groundtruth_simulation, 2) # create the radar simulators @@ -161,15 +156,18 @@ # %% # Visualise the detections from the sensors # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# Before preparing the different trackers components, -# let's visualise the target and its detections from the -# two sensors. In this way we can appreciate -# how the measurements are different and can lead to separate +# Before preparing the different trackers components, let's visualise the target and its detections from the +# two sensors. In this way we can appreciate how the measurements are different and can lead to separate # tracks. # - -# Load the stone soup plotter +# Stone Soup plotting and metric imports from stonesoup.plotter import Plotterly +from stonesoup.metricgenerator.basicmetrics import BasicMetrics +from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics +from stonesoup.measures import Euclidean +from stonesoup.plotter import MetricPlotter +from stonesoup.dataassociator.tracktotrack import TrackToTruth +from stonesoup.metricgenerator.manager import MultiManager # Lists to hold the detections from each sensor s1_detections = [] @@ -198,22 +196,18 @@ # %% # 2) Initialise the trackers components # ------------------------------------- -# We have initialised the sensors and the -# target path, we can see that the -# detections from the two sensors -# differ one from the other, that will lead to -# two separate tracks. -# Now, we initialise the components of the two trackers, -# one using a extended Kalman filter +# We have initialised the sensors and the target trajectory, we can see that the +# detections from the two sensors differ one from the other, that will lead to two separate tracks. +# Now, we initialise the components of the two trackers, one using a Extended Kalman filter # and a particle filter. # # %% -# Stone soup tracker components +# Stone Soup tracker components # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # -# Let's consider a Distance based hypothesiser +# Let's consider a distance based hypothesiser from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.measures import Mahalanobis from stonesoup.dataassociator.neighbour import GNNWith2DAssignment @@ -250,10 +244,8 @@ def general_tracker(tracker_class, detector, return tracker -# instantiate the Kalman filter predictor +# instantiate the Extended Kalman filter predictor and updater KF_predictor = UnscentedKalmanPredictor(transition_model) - -# Instantiate the Kalman filter updater KF_updater = UnscentedKalmanUpdater(measurement_model=None) # define the hypothesiser @@ -261,8 +253,7 @@ def general_tracker(tracker_class, detector, predictor=KF_predictor, updater=KF_updater, measure=Mahalanobis(), - missed_distance=10 -) + missed_distance=10) # define the distance data associator data_associator_KF = GNNWith2DAssignment(hypothesiser_KF) @@ -286,7 +277,7 @@ def general_tracker(tracker_class, detector, predictor=PF_predictor, updater=PF_updater, measure=Mahalanobis(), - missed_distance=5) + missed_distance=9) # define the data associator data_associator_PF = GNNWith2DAssignment(hypothesiser_PF) @@ -295,26 +286,22 @@ def general_tracker(tracker_class, detector, # as gaussian state with the target track origin prior_state = SimpleMeasurementInitiator( prior_state=GaussianState([25, 0.5, 70, -0.25], - np.diag([10, 2, 10, 2]) ** 2)) + np.diag([5, 2, 5, 2])**2)) # Particle filter initiator PF_initiator = GaussianParticleInitiator( initiator=prior_state, number_particles=n_particles) -# %% -# At this stage we have all the components needed to -# perform the tracking using both Kalman and particle -# filters. We need to create a way to perform the track fusion. -# To perform such fusion, we employ the covariance -# intersection algorithm adopting the -# :class:`~.ChernoffUpdater` class, treating the tracks -# as :class:`~.GaussianMixture` detections. - # %% # Stone soup track fusion components # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # +# At this stage we have all the components needed to perform the tracking using both Kalman and particle +# filters. We need to create a way to perform the track fusion. +# To perform such fusion, we employ the covariance intersection algorithm adopting the +# :class:`~.ChernoffUpdater` class, treating the tracks as :class:`~.GaussianMixture` detections. + # Load the ChernoffUpdater components for track fusion from stonesoup.updater.chernoff import ChernoffUpdater @@ -380,18 +367,95 @@ def general_tracker(tracker_class, detector, track_fusion_tracker2 = deepcopy(track_fusion_tracker) # %% -# 3) Run the trackers, generate the partial tracks and merge the final composite track; -# ------------------------------------------------------------------------------------- +# 3. Run the trackers, generate the partial tracks and merge the final composite track +# ------------------------------------------------------------------------------------ # So far, we have shown how to instantiate the various tracker components # as well as the fusion tracker. Now, we run the trackers to generate -# the tracks and we perform the track fusion. Furthermore, we want to measure -# the accuracy of the tracks obtained via the fusion algorithm compared with -# the truths. To do so we instantiate a metric manager to evalute the various distances. +# the tracks and we perform the track fusion. + +# Create the tracks for the particle filters, Kalman and merged ones +PF_track1, PF_track2, KF_track1, KF_track2 = set(), set(), set(), set() +PF_fused_track, KF_fused_track = set(), set() + +# Instantiate the various trackers using the general_tracker function, +# we assign a unique tracker the detections from a specific radar simulator +KF_tracker_1 = general_tracker(SingleTargetTracker, radar1KF, KF_updater, + initiator, deleter, data_associator_KF) + +KF_tracker_2 = general_tracker(SingleTargetTracker, radar2KF, KF_updater, + initiator, deleter, data_associator_KF) + +PF_tracker_1 = general_tracker(SingleTargetTracker, radar1PF, PF_updater, + PF_initiator, deleter, data_associator_PF) + +PF_tracker_2 = general_tracker(SingleTargetTracker, radar2PF, PF_updater, + PF_initiator, deleter, data_associator_PF) + +PartialTrackPF1, TrackFusionPF1 = tee(PF_tracker_1, 2) +PartialTrackPF2, TrackFusionPF2 = tee(PF_tracker_2, 2) +PartialTrackKF1, TrackFusionKF1 = tee(KF_tracker_1, 2) +PartialTrackKF2, TrackFusionKF2 = tee(KF_tracker_2, 2) + +# Create the detector feeding the tracker algorithms +track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder( + MultiDataFeeder([TrackFusionKF1, TrackFusionKF2])) + +track_fusion_tracker2.detector = Tracks2GaussianDetectionFeeder( + MultiDataFeeder([TrackFusionPF1, TrackFusionPF2])) + +# Create a iterator for the trackers +iter_fusion_tracker = iter(track_fusion_tracker) +iter_fusion_tracker2 = iter(track_fusion_tracker2) + +# Loop over the timestep doing the track fusion and partial tracks +for _ in range(number_of_steps): + for _ in range(2): + _, tracks = next(iter_fusion_tracker) + KF_fused_track.update(tracks) + del tracks + _, tracks = next(iter_fusion_tracker2) + PF_fused_track.update(tracks) + + _, PF_sensor_track1 = next(iter(PartialTrackPF1)) + PF_track1.update(PF_sensor_track1) + + _, PF_sensor_track2 = next(iter(PartialTrackPF2)) + PF_track2.update(PF_sensor_track2) + + _, KF_sensor_track1 = next(iter(PartialTrackKF1)) + KF_track1.update(KF_sensor_track1) + + _, KF_sensor_track2 = next(iter(PartialTrackKF2)) + KF_track2.update(KF_sensor_track2) + +truths = set(groundtruth_simulation.groundtruth_paths) + +# %% +# Let's visualise the various tracks and detections in the cases +# using the Kalman and particle filters. + +plotter.plot_tracks(PF_track1, [0, 2], line=dict(color="orange"), track_label='PF partial track 1') +plotter.plot_tracks(PF_track2, [0, 2], line=dict(color="gold"), track_label='PF partial track 2') +plotter.plot_tracks(PF_fused_track, [0, 2], line=dict(color="red"), track_label='PF fused track') +plotter.plot_tracks(KF_fused_track, [0, 2], line=dict(color="blue"), track_label='KF fused track') +plotter.plot_tracks(KF_track1, [0, 2], line=dict(color="cyan"), track_label='KF partial track 1') +plotter.plot_tracks(KF_track2, [0, 2], line=dict(color="azure"), track_label='KF partial track 2') + +plotter.fig + +# %% +# 4. Evaluate the obtained tracks with the groundtruth trajectory. +# ---------------------------------------------------------------- +# At this stage we have almost completed our example. We have created the detections from the radars, +# performed the tracking and the fusion of the tracks. Now we use the :class:`~.MetricManager` +# to generate summary statistics on the accuracy of the tracks by comparing them with the +# groundtruth trajectory. +# +# If we consider the SIAP metrics, we can appreciate that the fused tracks have a lower error +# compared to the partial tracks obtained with the single instruments. # # Instantiate the metric manager -from stonesoup.metricgenerator.basicmetrics import BasicMetrics - basic_KF = BasicMetrics(generator_name='KF_fused', tracks_key='KF_fused_tracks', truths_key='truths') @@ -409,9 +473,6 @@ def general_tracker(tracker_class, detector, truths_key='truths') # Load the SIAP metric -from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics -from stonesoup.measures import Euclidean - siap_kf_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), velocity_measure=Euclidean((1, 3)), generator_name='SIAP_KF_fused-truth', @@ -452,10 +513,9 @@ def general_tracker(tracker_class, detector, ) # Define a data associator between the tracks and the truths -from stonesoup.dataassociator.tracktotrack import TrackToTruth associator = TrackToTruth(association_threshold=30) -from stonesoup.metricgenerator.manager import MultiManager +# Create the metric manager metric_manager = MultiManager([basic_KF, basic_KF1, basic_KF2, @@ -470,102 +530,16 @@ def general_tracker(tracker_class, detector, siap_pf2_truth], associator) -# Create the tracks for the particle filters, Kalman and merged ones -PF_track1, PF_track2, KF_track1, KF_track2 = set(), set(), set(), set() -PF_fused_track, KF_fused_track = set(), set() - -# Instantiate the various trackers using the general_tracker function, -# we assign a unique tracker the detections from a specific radar simulator -KF_tracker_1 = general_tracker(SingleTargetTracker, radar1KF, KF_updater, - initiator, deleter, data_associator_KF) - -KF_tracker_2 = general_tracker(SingleTargetTracker, radar2KF, KF_updater, - initiator, deleter, data_associator_KF) - -PF_tracker_1 = general_tracker(SingleTargetTracker, radar1PF, PF_updater, - PF_initiator, deleter, data_associator_PF) - -PF_tracker_2 = general_tracker(SingleTargetTracker, radar2PF, PF_updater, - PF_initiator, deleter, data_associator_PF) - -PartialTrackPF1, TrackFusionPF1 = tee(PF_tracker_1, 2) -PartialTrackPF2, TrackFusionPF2 = tee(PF_tracker_2, 2) -PartialTrackKF1, TrackFusionKF1 = tee(KF_tracker_1, 2) -PartialTrackKF2, TrackFusionKF2 = tee(KF_tracker_2, 2) - -# Create the detector feeding the tracker algorithms -track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder( - MultiDataFeeder([TrackFusionKF1, TrackFusionKF2])) - -track_fusion_tracker2.detector = Tracks2GaussianDetectionFeeder( - MultiDataFeeder([TrackFusionPF1, TrackFusionPF2])) - -iter_fusion_tracker = iter(track_fusion_tracker) -iter_fusion_tracker2 = iter(track_fusion_tracker2) - -# Loop over the timestep doing the track fusion and partial tracks -for _ in range(number_of_steps): - for _ in range(2): - _, tracks = next(iter_fusion_tracker) - KF_fused_track.update(tracks) - del tracks - _, tracks = next(iter_fusion_tracker2) - PF_fused_track.update(tracks) - - _, PF_sensor_track1 = next(iter(PartialTrackPF1)) - PF_track1.update(PF_sensor_track1) - - _, PF_sensor_track2 = next(iter(PartialTrackPF2)) - PF_track2.update(PF_sensor_track2) - - _, KF_sensor_track1 = next(iter(PartialTrackKF1)) - KF_track1.update(KF_sensor_track1) - - _, KF_sensor_track2 = next(iter(PartialTrackKF2)) - KF_track2.update(KF_sensor_track2) - # Add the tracks to the metric manager metric_manager.add_data({'KF_1_tracks': KF_track1, 'KF_2_tracks': KF_track2, 'PF_1_tracks': PF_track1, - 'PF_2_tracks': PF_track2}, overwrite=False) - -metric_manager.add_data({'KF_fused_tracks': KF_fused_track, + 'PF_2_tracks': PF_track2, + 'KF_fused_tracks': KF_fused_track, 'PF_fused_tracks': PF_fused_track, - }, overwrite=False) - -truths = set(groundtruth_simulation.groundtruth_paths) -metric_manager.add_data({'truths': truths}, overwrite=False) -# %% -# Let's visualise the various tracks and detections in the cases -# using the Kalman and particle filters. -plotter.plot_tracks(PF_track1, [0, 2], line=dict(color="orange"), track_label='PF partial track 1') -plotter.plot_tracks(PF_track2, [0, 2], line=dict(color="gold"), track_label='PF partial track 2') -plotter.plot_tracks(PF_fused_track, [0, 2], line=dict(color="red"), track_label='PF fused track') -plotter.plot_tracks(KF_fused_track, [0, 2], line=dict(color="blue"), track_label='KF fused track') -plotter.plot_tracks(KF_track1, [0, 2], line=dict(color="cyan"), track_label='KF partial track 1') -plotter.plot_tracks(KF_track2, [0, 2], line=dict(color="azure"), track_label='KF partial track 2') - -plotter.fig - - -# %% -# 4) Evaluate the obtained tracks with the groundtruth trajectory. -# --------------------------------------------------------------- -# At this stage we have almost completed our example. We have created the -# detections from the radars, performed the tracking and the -# fusion of the tracks. Now we use the :class:`~.MetricManager` -# to generate summary statistics on the accuracy of the tracks -# by comparing them with the groundtruth measurements. -# -# If we consider the SIAP metrics, we can appreciate that -# the fused tracks have a lower error compared to the -# partial tracks obtained with the single instruments. -# + 'truths': truths}, overwrite=False) # Loaded the plotter for the various metrics. -from stonesoup.plotter import MetricPlotter - metrics = metric_manager.generate_metrics() graph = MetricPlotter() @@ -581,9 +555,8 @@ def general_tracker(tracker_class, detector, # %% # Conclusions # ----------- -# In this example we have shown how it is possible to -# merge the tracks generated by independent trackers -# ran on sets of data obtained by separate sensors. We -# have, also, compared how the Kalman and the particle -# filters behave in these cases, making track to truth -# comparisons. +# In this example we have shown how it is possible to merge the tracks generated by independent trackers +# ran on sets of data obtained by separate sensors. We have, also, compared how the Kalman and the particle +# filters behave in these cases, making track-to-truth comparisons. + +# sphinx_gallery_thumbnail_number = 2 \ No newline at end of file From 67193fb5e01fcde332511a69ff5780713c91bc03 Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Fri, 16 Feb 2024 09:35:28 +0000 Subject: [PATCH 08/11] fix the issue with invertible matrix that was appearing from time to time --- docs/examples/track_fusion_example.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index 5c03fbc98..47fddd546 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -68,7 +68,7 @@ start_time = datetime.now().replace(microsecond=0) number_of_steps = 75 # Number of timestep of the simulation -np.random.seed(1908) # Random seed for reproducibility +np.random.seed(2000) # Random seed for reproducibility n_particles = 2**10 # Instantiate the target transition model @@ -96,7 +96,7 @@ # ^^^^^^^^^^^^^^^^^^^^ clutter_model = ClutterModel( - clutter_rate=1.0, + clutter_rate=0.9, distribution=np.random.default_rng().uniform, dist_params=((0, 120), (-5, 105))) # dist_params describe the area where the clutter is detected @@ -159,7 +159,8 @@ # Before preparing the different trackers components, let's visualise the target and its detections from the # two sensors. In this way we can appreciate how the measurements are different and can lead to separate # tracks. -# + +#%% # Stone Soup plotting and metric imports from stonesoup.plotter import Plotterly from stonesoup.metricgenerator.basicmetrics import BasicMetrics @@ -298,8 +299,9 @@ def general_tracker(tracker_class, detector, # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # # At this stage we have all the components needed to perform the tracking using both Kalman and particle -# filters. We need to create a way to perform the track fusion. -# To perform such fusion, we employ the covariance intersection algorithm adopting the +# filters. +# +# To perform the track fusion, we employ the covariance intersection algorithm adopting the # :class:`~.ChernoffUpdater` class, treating the tracks as :class:`~.GaussianMixture` detections. @@ -439,7 +441,7 @@ def general_tracker(tracker_class, detector, plotter.plot_tracks(PF_fused_track, [0, 2], line=dict(color="red"), track_label='PF fused track') plotter.plot_tracks(KF_fused_track, [0, 2], line=dict(color="blue"), track_label='KF fused track') plotter.plot_tracks(KF_track1, [0, 2], line=dict(color="cyan"), track_label='KF partial track 1') -plotter.plot_tracks(KF_track2, [0, 2], line=dict(color="azure"), track_label='KF partial track 2') +plotter.plot_tracks(KF_track2, [0, 2], line=dict(color="skyblue"), track_label='KF partial track 2') plotter.fig @@ -447,7 +449,7 @@ def general_tracker(tracker_class, detector, # 4. Evaluate the obtained tracks with the groundtruth trajectory. # ---------------------------------------------------------------- # At this stage we have almost completed our example. We have created the detections from the radars, -# performed the tracking and the fusion of the tracks. Now we use the :class:`~.MetricManager` +# performed the tracking and the fusion of the tracks. We employ the :class:`~.MetricManager` # to generate summary statistics on the accuracy of the tracks by comparing them with the # groundtruth trajectory. # From bfc09cab6a40caf862438ede38bc0f9f9fbafc44 Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Fri, 5 Apr 2024 15:18:59 +0100 Subject: [PATCH 09/11] removed basic metric not used and improvements in the text --- docs/examples/track_fusion_example.py | 33 ++++----------------------- 1 file changed, 4 insertions(+), 29 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index 47fddd546..5196a7bc8 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -8,7 +8,7 @@ """ # %% -# This example shows a comparison between a Kalman filter algorithms and particle filter in the +# This example shows a comparison between a Kalman filter algorithm and particle filter in the # context of track fusion. This example is relevant to show how to get a unique track # from partial tracks generated from set of different measurements obtained from independent sensors. # @@ -96,7 +96,7 @@ # ^^^^^^^^^^^^^^^^^^^^ clutter_model = ClutterModel( - clutter_rate=0.9, + clutter_rate=0.75, distribution=np.random.default_rng().uniform, dist_params=((0, 120), (-5, 105))) # dist_params describe the area where the clutter is detected @@ -160,8 +160,6 @@ # two sensors. In this way we can appreciate how the measurements are different and can lead to separate # tracks. -#%% -# Stone Soup plotting and metric imports from stonesoup.plotter import Plotterly from stonesoup.metricgenerator.basicmetrics import BasicMetrics from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics @@ -457,23 +455,6 @@ def general_tracker(tracker_class, detector, # compared to the partial tracks obtained with the single instruments. # -# Instantiate the metric manager -basic_KF = BasicMetrics(generator_name='KF_fused', tracks_key='KF_fused_tracks', - truths_key='truths') - -basic_KF1 = BasicMetrics(generator_name='KF1', tracks_key='KF_1_tracks', - truths_key='truths') - -basic_KF2 = BasicMetrics(generator_name='KF2', tracks_key='KF_2_tracks', - truths_key='truths') - -basic_PF = BasicMetrics(generator_name='PF_fused', tracks_key='PF_fused_tracks', - truths_key='truths') -basic_PF1 = BasicMetrics(generator_name='PF1', tracks_key='PF_1_tracks', - truths_key='truths') -basic_PF2 = BasicMetrics(generator_name='PF2', tracks_key='PF_2_tracks', - truths_key='truths') - # Load the SIAP metric siap_kf_truth = SIAPMetrics(position_measure=Euclidean((0, 2)), velocity_measure=Euclidean((1, 3)), @@ -518,13 +499,7 @@ def general_tracker(tracker_class, detector, associator = TrackToTruth(association_threshold=30) # Create the metric manager -metric_manager = MultiManager([basic_KF, - basic_KF1, - basic_KF2, - basic_PF, - basic_PF1, - basic_PF2, - siap_kf_truth, +metric_manager = MultiManager([siap_kf_truth, siap_kf1_truth, siap_kf2_truth, siap_pf_truth, @@ -561,4 +536,4 @@ def general_tracker(tracker_class, detector, # ran on sets of data obtained by separate sensors. We have, also, compared how the Kalman and the particle # filters behave in these cases, making track-to-truth comparisons. -# sphinx_gallery_thumbnail_number = 2 \ No newline at end of file +# sphinx_gallery_thumbnail_number = 2 From 5f83d19c1bc6c26c2efbe77f6aa6a01759fcd012 Mon Sep 17 00:00:00 2001 From: jwright2 Date: Wed, 17 Apr 2024 11:37:09 +0100 Subject: [PATCH 10/11] Minor formatting and spelling fixes --- docs/examples/track_fusion_example.py | 73 +++++++++++++-------------- 1 file changed, 36 insertions(+), 37 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index 5196a7bc8..fd4d544ac 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -9,8 +9,8 @@ # %% # This example shows a comparison between a Kalman filter algorithm and particle filter in the -# context of track fusion. This example is relevant to show how to get a unique track -# from partial tracks generated from set of different measurements obtained from independent sensors. +# context of track fusion. This example is relevant to show how to get a unique track from partial +# tracks generated from set of different measurements obtained from independent sensors. # # This example simulates the case of a single target moving in a 2D Cartesian space with # measurements obtained from two identical, for simplicity, radars with their own trackers. @@ -29,10 +29,11 @@ # %% # 1. Initialise the sensors and the target trajectory # --------------------------------------------------- -# We start creating two identical, in terms of performance, radars using the -# :class:`~.RadarBearingRange` sensor placed on two separate :class:`~.FixedPlatform`. +# We start by creating two identical, in terms of performance, radars using the +# :class:`~.RadarBearingRange` sensor placed on two separate sensor platforms of type +# :class:`~.FixedPlatform`. # For the target we simulate a single object moving on a straight trajectory. -# The example setup is simple so it is easier to understand how the algorithm components work. +# This example is set up such that it is easy to understand how the algorithm components work. # %% # General imports @@ -110,7 +111,7 @@ # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # Let's assume that both radars have the same noise covariance for simplicity -# These radars will have the +/-0.005 degrees accuracy in bearing and +/- 5 meters in range +# These radars will have a variance of 0.005 degrees in bearing and 5 meters in range radar_noise = CovarianceMatrix(np.diag([np.deg2rad(0.005), 5])) # Define the specifications of the two radars @@ -131,7 +132,7 @@ position_mapping=(0, 2), sensors=[radar1]) -# Instantiate the second one +# Instantiate the second sensor platform with the second sensor sensor2_platform = FixedPlatform( states=GaussianState([75, 0, 10, 0], np.diag([1, 0, 1, 0])), @@ -156,12 +157,11 @@ # %% # Visualise the detections from the sensors # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# Before preparing the different trackers components, let's visualise the target and its detections from the -# two sensors. In this way we can appreciate how the measurements are different and can lead to separate -# tracks. +# Before preparing the different trackers components, we can visualise the target ground truth and +# its detections from the two sensors. +# In this way we can appreciate how the measurements are different and can lead to separate tracks. from stonesoup.plotter import Plotterly -from stonesoup.metricgenerator.basicmetrics import BasicMetrics from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics from stonesoup.measures import Euclidean from stonesoup.plotter import MetricPlotter @@ -172,14 +172,14 @@ s1_detections = [] s2_detections = [] -grountruth_generator = groundtruth_simulation.groundtruth_paths_gen() +groundtruth_generator = groundtruth_simulation.groundtruth_paths_gen() truths = set() # Iterate over the time steps, extracting the detections and truths for (time, sd1), (_, sd2) in zip(radar1plot, radar2plot): s1_detections.append(sd1) s2_detections.append(sd2) - truths.update(next(grountruth_generator)[1]) # consider only the path + truths.update(next(groundtruth_generator)[1]) # consider only the path # Plot the detections from the two radars plotter = Plotterly() @@ -193,11 +193,12 @@ plotter.fig # %% -# 2) Initialise the trackers components +# 2. Initialise the trackers components # ------------------------------------- # We have initialised the sensors and the target trajectory, we can see that the -# detections from the two sensors differ one from the other, that will lead to two separate tracks. -# Now, we initialise the components of the two trackers, one using a Extended Kalman filter +# detections from the two sensors differ from one another, that will lead to two separate tracks. +# +# Now, we initialise the components of the two trackers, one using an Extended Kalman filter # and a particle filter. # @@ -225,7 +226,8 @@ from stonesoup.updater.particle import ParticleUpdater from stonesoup.resampler.particle import ESSResampler -# Lets define an helper function to minimise the number of times + +# Let's define a helper function to minimise the number of times # we have to initialise the same tracker object def general_tracker(tracker_class, detector, filter_updater, initiator, deleter, @@ -260,13 +262,12 @@ def general_tracker(tracker_class, detector, # define a track time deleter deleter = UpdateTimeStepsDeleter(3) -# create an track initiator placed on the target track origin +# create a track initiator placed on the target track origin initiator = SimpleMeasurementInitiator( prior_state=initial_target_state, measurement_model=None) -# Instantiate the predictor, particle resampler and particle -# filter updater +# Instantiate the predictor, particle resampler and particle filter updater PF_predictor = ParticlePredictor(transition_model) resampler = ESSResampler() PF_updater = ParticleUpdater(measurement_model=None, @@ -281,8 +282,7 @@ def general_tracker(tracker_class, detector, # define the data associator data_associator_PF = GNNWith2DAssignment(hypothesiser_PF) -# To instantiate the track initiator we define a prior state -# as gaussian state with the target track origin +# Initiator with a prior Gaussian state with the target track origin prior_state = SimpleMeasurementInitiator( prior_state=GaussianState([25, 0.5, 70, -0.25], np.diag([5, 2, 5, 2])**2)) @@ -296,10 +296,10 @@ def general_tracker(tracker_class, detector, # Stone soup track fusion components # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # -# At this stage we have all the components needed to perform the tracking using both Kalman and particle -# filters. +# At this stage we have all the components needed to perform the tracking using both Kalman and +# particle filters. # -# To perform the track fusion, we employ the covariance intersection algorithm adopting the +# To perform the track fusion, we employ the covariance intersection algorithm implemented in the # :class:`~.ChernoffUpdater` class, treating the tracks as :class:`~.GaussianMixture` detections. @@ -354,7 +354,7 @@ def general_tracker(tracker_class, detector, tag=TaggedWeightedGaussianState.BIRTH, timestamp=start_time) -# Instantiate the Track fusion tracker using Point Process Tracker +# Instantiate the Track fusion tracker using the Point Process Tracker track_fusion_tracker = PointProcessMultiTargetTracker( detector=None, hypothesiser=hypothesiser, @@ -369,9 +369,8 @@ def general_tracker(tracker_class, detector, # %% # 3. Run the trackers, generate the partial tracks and merge the final composite track # ------------------------------------------------------------------------------------ -# So far, we have shown how to instantiate the various tracker components -# as well as the fusion tracker. Now, we run the trackers to generate -# the tracks and we perform the track fusion. +# So far, we have shown how to instantiate the various tracker components as well as the fusion +# tracker. Now, we run the trackers to generate the tracks, and perform the track fusion. # Create the tracks for the particle filters, Kalman and merged ones PF_track1, PF_track2, KF_track1, KF_track2 = set(), set(), set(), set() @@ -403,7 +402,7 @@ def general_tracker(tracker_class, detector, track_fusion_tracker2.detector = Tracks2GaussianDetectionFeeder( MultiDataFeeder([TrackFusionPF1, TrackFusionPF2])) -# Create a iterator for the trackers +# Create an iterator for the trackers iter_fusion_tracker = iter(track_fusion_tracker) iter_fusion_tracker2 = iter(track_fusion_tracker2) @@ -446,10 +445,10 @@ def general_tracker(tracker_class, detector, # %% # 4. Evaluate the obtained tracks with the groundtruth trajectory. # ---------------------------------------------------------------- -# At this stage we have almost completed our example. We have created the detections from the radars, -# performed the tracking and the fusion of the tracks. We employ the :class:`~.MetricManager` -# to generate summary statistics on the accuracy of the tracks by comparing them with the -# groundtruth trajectory. +# At this stage we have almost completed our example. We have created the detections from the +# radars, performed the tracking and the fusion of the tracks. We now employ the +# :class:`~.MetricManager` to generate summary statistics on the accuracy of the tracks by comparing +# them with the ground truth trajectory. # # If we consider the SIAP metrics, we can appreciate that the fused tracks have a lower error # compared to the partial tracks obtained with the single instruments. @@ -532,8 +531,8 @@ def general_tracker(tracker_class, detector, # %% # Conclusions # ----------- -# In this example we have shown how it is possible to merge the tracks generated by independent trackers -# ran on sets of data obtained by separate sensors. We have, also, compared how the Kalman and the particle -# filters behave in these cases, making track-to-truth comparisons. +# In this example we have shown how it is possible to merge the tracks generated by independent +# trackers ran on sets of detections obtained from separate sensors. Finally we compared how the +# Kalman and the particle filters behave in these cases, making track-to-truth comparisons. # sphinx_gallery_thumbnail_number = 2 From c333da6086b9c30b4418f66c290a5f19274fefb1 Mon Sep 17 00:00:00 2001 From: Alberto Acuto Date: Thu, 18 Apr 2024 14:22:36 +0100 Subject: [PATCH 11/11] differentiate clutter and detections, removed general tracker --- docs/examples/track_fusion_example.py | 82 +++++++++++++++------------ 1 file changed, 45 insertions(+), 37 deletions(-) diff --git a/docs/examples/track_fusion_example.py b/docs/examples/track_fusion_example.py index fd4d544ac..facedd5a0 100644 --- a/docs/examples/track_fusion_example.py +++ b/docs/examples/track_fusion_example.py @@ -54,6 +54,8 @@ from stonesoup.types.array import CovarianceMatrix from stonesoup.simulator.simple import SingleTargetGroundTruthSimulator from stonesoup.models.clutter.clutter import ClutterModel +from stonesoup.types.detection import Clutter + # Instantiate the radars to collect measurements - Use a :class:`~.RadarBearingRange` radar.# from stonesoup.sensor.radar.radar import RadarBearingRange @@ -183,10 +185,19 @@ # Plot the detections from the two radars plotter = Plotterly() -plotter.plot_measurements(s1_detections, [0, 2], marker=dict(color='orange', symbol='305'), +plotter.plot_measurements([d for ds in s1_detections for d in ds if not isinstance(d, Clutter)], + [0, 2], marker=dict(color='red'), measurements_label='Sensor 1 measurements') + +plotter.plot_measurements([d for ds in s1_detections for d in ds if isinstance(d, Clutter)], + [0, 2], marker=dict(color='red', symbol='star-triangle-up'), measurements_label='Sensor 1 measurements') -plotter.plot_measurements(s2_detections, [0, 2], marker=dict(color='blue', symbol='0'), + +plotter.plot_measurements([d for ds in s2_detections for d in ds if not isinstance(d, Clutter)], + [0, 2], marker=dict(color='blue'), measurements_label='Sensor 2 measurements') +plotter.plot_measurements([d for ds in s2_detections for d in ds if isinstance(d, Clutter)], + [0, 2], marker=dict(color='blue', symbol='star-triangle-up'), measurements_label='Sensor 2 measurements') + plotter.plot_sensors({sensor1_platform, sensor2_platform}, [0, 1], marker=dict(color='black', symbol='1', size=10)) plotter.plot_ground_truths(truths, [0, 2]) @@ -226,25 +237,6 @@ from stonesoup.updater.particle import ParticleUpdater from stonesoup.resampler.particle import ESSResampler - -# Let's define a helper function to minimise the number of times -# we have to initialise the same tracker object -def general_tracker(tracker_class, detector, - filter_updater, initiator, deleter, - data_associator): - """ - Helper function to initialise the trackers - """ - - tracker = tracker_class( - initiator=initiator, - detector=detector, - updater=filter_updater, - data_associator=data_associator, - deleter=deleter) - return tracker - - # instantiate the Extended Kalman filter predictor and updater KF_predictor = UnscentedKalmanPredictor(transition_model) KF_updater = UnscentedKalmanUpdater(measurement_model=None) @@ -376,19 +368,30 @@ def general_tracker(tracker_class, detector, PF_track1, PF_track2, KF_track1, KF_track2 = set(), set(), set(), set() PF_fused_track, KF_fused_track = set(), set() -# Instantiate the various trackers using the general_tracker function, # we assign a unique tracker the detections from a specific radar simulator -KF_tracker_1 = general_tracker(SingleTargetTracker, radar1KF, KF_updater, - initiator, deleter, data_associator_KF) - -KF_tracker_2 = general_tracker(SingleTargetTracker, radar2KF, KF_updater, - initiator, deleter, data_associator_KF) - -PF_tracker_1 = general_tracker(SingleTargetTracker, radar1PF, PF_updater, - PF_initiator, deleter, data_associator_PF) - -PF_tracker_2 = general_tracker(SingleTargetTracker, radar2PF, PF_updater, - PF_initiator, deleter, data_associator_PF) +KF_tracker_1 = SingleTargetTracker(initiator=initiator, + detector=radar1KF, + updater=KF_updater, + data_associator=data_associator_KF, + deleter=deleter) + +KF_tracker_2 = SingleTargetTracker(initiator=initiator, + detector=radar2KF, + updater=KF_updater, + data_associator=data_associator_KF, + deleter=deleter) + +PF_tracker_1 = SingleTargetTracker(initiator=PF_initiator, + detector=radar1PF, + updater=PF_updater, + data_associator=data_associator_PF, + deleter=deleter) + +PF_tracker_2 = SingleTargetTracker(initiator=PF_initiator, + detector=radar2PF, + updater=PF_updater, + data_associator=data_associator_PF, + deleter=deleter) PartialTrackPF1, TrackFusionPF1 = tee(PF_tracker_1, 2) PartialTrackPF2, TrackFusionPF2 = tee(PF_tracker_2, 2) @@ -406,6 +409,11 @@ def general_tracker(tracker_class, detector, iter_fusion_tracker = iter(track_fusion_tracker) iter_fusion_tracker2 = iter(track_fusion_tracker2) +PartialTrackPF1_iter = iter(PartialTrackPF1) +PartialTrackPF2_iter = iter(PartialTrackPF2) +PartialTrackKF1_iter = iter(PartialTrackKF1) +PartialTrackKF2_iter = iter(PartialTrackKF2) + # Loop over the timestep doing the track fusion and partial tracks for _ in range(number_of_steps): for _ in range(2): @@ -415,16 +423,16 @@ def general_tracker(tracker_class, detector, _, tracks = next(iter_fusion_tracker2) PF_fused_track.update(tracks) - _, PF_sensor_track1 = next(iter(PartialTrackPF1)) + _, PF_sensor_track1 = next(PartialTrackPF1_iter) PF_track1.update(PF_sensor_track1) - _, PF_sensor_track2 = next(iter(PartialTrackPF2)) + _, PF_sensor_track2 = next(PartialTrackPF2_iter) PF_track2.update(PF_sensor_track2) - _, KF_sensor_track1 = next(iter(PartialTrackKF1)) + _, KF_sensor_track1 = next(PartialTrackKF1_iter) KF_track1.update(KF_sensor_track1) - _, KF_sensor_track2 = next(iter(PartialTrackKF2)) + _, KF_sensor_track2 = next(PartialTrackKF2_iter) KF_track2.update(KF_sensor_track2) truths = set(groundtruth_simulation.groundtruth_paths)