#!/usr/bin/env python # coding: utf-8 """ ========================================================================= Multi-Sensor Fusion: Covariance Intersection Using Tracks as Measurements ========================================================================= """ # %% # Background # ---------- # The Covariance Intersection Algorithm from Julier and Uhlmann [#]_ is a popular algorithm for # track-to-track fusion in target tracking systems. This approach is highly appealing due to its # robustness, simple structure, and applicability to any tracking system that uses Gaussians as the # basis for tracking. Generalisations to non-Gaussian systems have been proposed based on the # exponential mixture density structure of the algorithm. The approach is based on a simple rule # called the Chernoff Fusion Rule. However, due to the non-Bayesian formulation of the rule, it # cannot be integrated straightforwardly into multi-target tracking algorithms which are based on # Bayesian formulations. # # A new Bayesian formulation for covariance intersection was recently proposed which allows for the # integration of the approach into multi-target tracking algorithms. [#]_ The new formulation # recasts the fusion rule as a Bayesian update rule that calculates a normalisation constant which # enables integration into different multi-target tracking algorithms. # # In this example we demonstrate the approach with different multi-target trackers in a # multi-platform scenario where the sensors output estimated target tracks instead of raw # measurements. In real life situations, such sensors make multi-target tracking more accessible to # new researchers because the researchers don't have to know about or implement target filtering # and/or tracking algorithms on their own. However, when there are multiple sensors measuring the # same target space and they all produce estimated tracks, as demonstrated in this example, it is # not immediately clear how to combine this information into a single set of tracks. This is where # covariance intersection comes in. # # The concept of covariance intersection relies on the aforementioned Chernoff fusion rule [#]_ : # # .. math:: # p_{\omega}(x_{k}) = \frac{p_{1}(x_{k})^{\omega}p_{2}(x_{k})^{1-\omega}}{\int p_{1}(x)^{\omega}p_{2}(x)^{1-\omega}dx} # # # In situations where :math:`p_1(x)` and :math:`p_2(x)` are multivariate Gaussian distributions, # this formula is equal to the Covariance Intersection Algorithm from Julier and Uhlmann. In the # Covariance Intersection Algorithm, the weighting parameter, :math:`\omega \in [0, 1]` is chosen # using an optimization algorithm. In this example, we have set it to :math:`0.5` for simplicity. # # We also introduce the following identity. Given two Gaussians, :math:`N(x ; a, A)` and # :math:`N(x ; b, B)` with the same dimension, we have: # # .. math:: # \left({\frac{\mathcal{N}(x ; a, A)}{\mathcal{N}(x ; a, A)}}\right)^\omega \mathcal{N}(x ; b, B) \propto \mathcal{N}(a ; b, V) \mathcal{N}(x ; d, D) # # # where # # .. math:: # D &= \left(\omega A^{-1}+(1-\omega) B^{-1}\right)^{-1} \\ # d &= D\left(\omega A^{-1} {a}+(1-\omega ) B^{-1} {b}\right) \\ # V &= A/(1-\omega )+ B/\omega # # # This example considers the Gaussian mixture probability hypothesis density (GM-PHD) algorithm # as the tracker for the track-to-track fusion. The following table shows the formulas used in the # regular GM-PHD, and the GM-PHD covariance intersector algorithm. # # .. image:: ../_static/covariance_intersection_formulas.png # :width: 700 # :alt: Formulas for the posterior/fused intensity, gaussian components, updated weight, updated # mean, updated covariance, innovation, Kalman gain, and innovation covariance in the # GM-PHD and the GM-PHD Covariance Intersector algorithm. # # # The specifics for implementing the Covariance Intersection Algorithm in several popular # multi-target tracking algorithms was expanded upon recently by Clark et al [#]_. The work # includes a discussion of Stone Soup and and is used as the basis for this example. # %% # The rest of this example will continue as follows: # * Create a simulator for the ground truth # * Create 2 radar simulators, one on the ground and one that is airborne # * Make a JPDA tracker for the first radar, and a Gaussian mixture linear complexity with # cumulants (GM-LCC) tracker for the second. These will mimic the situation where the radar # sensors outputs tracks instead of raw measurements. # * Create a GM-PHD tracker that will perform measurement fusion, using all measurements from # both radars. This is created to compare with the covariance intersection method. # * Create a GM-PHD tracker that will perform track fusion via covariance intersection using # the :class:`ChernoffUpdater` class. # * Create metric managers for each of the four trackers # * Set up the detection feeders. Each tracker will receive measurements using a custom # :class:`DummyDetector` class. The track fusion tracker will also use the # :class:`Tracks2GaussianDetectionFeeder` class. # * Run the simulation # * Plot the resulting tracks and the metrics over time # %% from copy import deepcopy import numpy as np from datetime import datetime start_time = datetime.now() num_steps = 50 # %% # 1: Create a Ground Truth Simulator # ---------------------------------- # We will simulate the paths of two targets using the :class:`~.MultiTargetGroundTruthSimulator`. # We can dictate the starting states of the two targets using the `preexisting_states` parameter. # The targets start at [-100, -200, 500] and [0, 300, 500] respectively. Their initial velocities # are [4, 0.5, 0] and [5, -0.5, 0] and they move according to a constant velocity transition model # with noise. # %% from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel,\ ConstantVelocity truth_transition_model = CombinedLinearGaussianTransitionModel( (ConstantVelocity(0.5), ConstantVelocity(0.5), ConstantVelocity(0.5))) from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator from stonesoup.types.state import GaussianState gt_simulator = MultiTargetGroundTruthSimulator( transition_model=truth_transition_model, initial_state=GaussianState([0, 0, 0, 0, 500, 0], np.diag([100, 1, 100, 1, 100, 1]), timestamp=start_time), birth_rate=0, death_probability=0, number_steps=num_steps, preexisting_states=[[-100, 4, -200, 0.5, 500, 0], [0, 5, 300, -0.5, 500, 0]] ) # %% # 2: Create Two Radars and a Detection Simulation # -------------------------------------------------- # The two radars can share the same clutter model. # %% from stonesoup.models.clutter.clutter import ClutterModel clutter_model = ClutterModel( clutter_rate=2.0, distribution=np.random.default_rng().uniform, dist_params=((-600.0, 600.0), (-600.0, 600.0), (250.0, 750.0)) ) # %% # The first radar will be airborne, at an altitude of approximately 3000 m. It makes detections # with an elevation, bearing, and range measurement model. By setting the `max_range` to 3500, we # can ensure that it does not make detections of the other radar (which will be far away on the # ground). We will later do a similar thing with the second radar. This mimics a real-life scenario # where each radar is outside the field-of-view of the other. # %% from stonesoup.sensor.radar.radar import RadarElevationBearingRange from stonesoup.types.array import CovarianceMatrix from stonesoup.types.array import StateVector from stonesoup.platform.base import MovingPlatform from stonesoup.types.state import State radar1 = RadarElevationBearingRange( ndim_state=6, position_mapping=(0, 2, 4), noise_covar=CovarianceMatrix(np.diag([np.deg2rad(0.005), np.deg2rad(0.005), 0.05])), mounting_offset=StateVector([10, 0, 0]), clutter_model=clutter_model, max_range=3500 ) # Mount the radar onto a moving platform. The platform starts at [-250, 50, 3000] # with velocity [1, 5, 0] and moves according to a constant velocity model with low noise sensor1_initial_loc = StateVector([[-250], [1], [50], [5], [3000], [0]]) initial_state = State(sensor1_initial_loc, start_time) sensor1_transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.3), ConstantVelocity(0.3), ConstantVelocity(0.3)]) sensor1_platform = MovingPlatform( states=initial_state, position_mapping=(0, 2, 4), velocity_mapping=(1, 3, 5), transition_model=sensor1_transition_model, sensors=[radar1] ) # %% # The second radar will be stationary on the ground at the point [2000, 50, 0]. This radar also # measures in 3D using bearing, range, and elevation. # %% radar2_noise_covar = CovarianceMatrix(np.diag([np.deg2rad(0.005), np.deg2rad(0.005), 0.05])) radar2 = RadarElevationBearingRange( ndim_state=6, position_mapping=(0, 2, 4), noise_covar=radar2_noise_covar, clutter_model=clutter_model, max_range=3000 ) # Make a platform and mount the radar from stonesoup.platform.base import FixedPlatform sensor2_platform = FixedPlatform( State([2000, 0, 50, 0, 0, 0]), position_mapping=[0, 2, 4], sensors=[radar2] ) # %% # Now we can pass the platforms into a detection simulator. At each timestep, the simulator will # return the detections from the `sensor1_platform`, then the detections from the # `sensor2_platform`. # %% from stonesoup.simulator.platform import PlatformDetectionSimulator radar_simulator = PlatformDetectionSimulator( groundtruth=gt_simulator, platforms=[sensor1_platform, sensor2_platform] ) # %% # Let's briefly visualize the truths and measurements before we move on. Note that the final # simulation will not have the same truths because the ground truth generator is randomized. But # this gives an idea of what it will look like. The detections from the first sensor (airborne) # will be plotted in blue, and the detections from the second sensor are in red. The clutter from # both sensors are plotted in yellow. The sensor locations will be plotted in green Xs. # %% from stonesoup.plotter import Plotter, Dimension # Lists to hold the detections from each sensor and the path of the airborne radar s1_detections = [] s2_detections = [] radar1_path = [] # Extract the generator function from a copy of the simulator sim = deepcopy(radar_simulator) g = sim.detections_gen() # Iterate over the time steps, extracting the detections, truths, and airborne sensor path for _ in range(num_steps): s1_detections.append(next(g)[1]) s2_detections.append(next(g)[1]) radar1_path.append(sim.platforms[0].position) truths = set(sim.groundtruth.groundtruth_paths) # Plot the truths and detections plotter = Plotter(dimension=Dimension.THREE) plotter.plot_ground_truths(truths, [0, 2, 4]) plotter.plot_measurements(s1_detections, [0, 2, 4], color='blue') plotter.plot_measurements(s2_detections, [0, 2, 4], color='red') # Plot the radar positions plotter.ax.plot(*zip(*radar1_path), marker='x', color='green') plotter.ax.plot(2000, 50, 0, marker='x', color='green') # %% # 3: Make Trackers for the Radars # ------------------------------- # The airborne radar will be tracking using a JPDA tracker, and the stationary one will use a # GM-LCC. These trackers will not be given the platform detection simulation objects as parameters, # we will feed the measurements later to ensure that that the same measurements are used in the # fusion trackers. # To start, we can calculate the clutter spatial density. # %% clutter_area = np.prod(np.diff(clutter_model.dist_params)) clutter_spatial_density = clutter_model.clutter_rate/clutter_area # %% # JPDA Tracker # ^^^^^^^^^^^^ # %% from stonesoup.hypothesiser.probability import PDAHypothesiser from stonesoup.updater.kalman import ExtendedKalmanUpdater from stonesoup.predictor.kalman import ExtendedKalmanPredictor from stonesoup.dataassociator.probability import JPDA from stonesoup.deleter.error import CovarianceBasedDeleter from stonesoup.initiator.simple import MultiMeasurementInitiator from stonesoup.tracker.simple import MultiTargetMixtureTracker # Updater jpda_updater = ExtendedKalmanUpdater(measurement_model=None) # Data Associator predictor = ExtendedKalmanPredictor(truth_transition_model) hypothesiser = PDAHypothesiser( predictor=predictor, updater=jpda_updater, clutter_spatial_density=clutter_spatial_density, prob_detect=0.9 ) data_associator = JPDA(hypothesiser=hypothesiser) # Deleter covariance_limit_for_delete = 500 deleter = CovarianceBasedDeleter(covar_trace_thresh=covariance_limit_for_delete) # Initiator s_prior_state = GaussianState([0, 0, 0, 0, 500, 0], np.diag([0, 50, 0, 50, 0, 50])) from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.measures import Mahalanobis hypothesiser = DistanceHypothesiser( predictor, jpda_updater, measure=Mahalanobis(), missed_distance=3 ) from stonesoup.dataassociator.neighbour import GNNWith2DAssignment initiator_associator = GNNWith2DAssignment(hypothesiser) initiator_deleter = CovarianceBasedDeleter(covar_trace_thresh=500) initiator = MultiMeasurementInitiator( prior_state=s_prior_state, measurement_model=None, deleter=initiator_deleter, data_associator=initiator_associator, updater=jpda_updater, min_points=2 ) jpda_tracker = MultiTargetMixtureTracker( initiator=initiator, deleter=deleter, detector=None, data_associator=data_associator, updater=jpda_updater ) # %% # GM-LCC Tracker # ^^^^^^^^^^^^^^ # %% from stonesoup.updater.pointprocess import LCCUpdater from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.measures import Mahalanobis from stonesoup.hypothesiser.gaussianmixture import GaussianMixtureHypothesiser from stonesoup.mixturereducer.gaussianmixture import GaussianMixtureReducer from stonesoup.types.state import TaggedWeightedGaussianState from stonesoup.tracker.pointprocess import PointProcessMultiTargetTracker # Updater kalman_updater = ExtendedKalmanUpdater(measurement_model=None) updater = LCCUpdater( updater=kalman_updater, clutter_spatial_density=clutter_spatial_density, normalisation=True, prob_detection=0.9, prob_survival=0.9, mean_number_of_false_alarms=clutter_model.clutter_rate, variance_of_false_alarms=100 ) # Hypothesiser kalman_predictor = ExtendedKalmanPredictor(truth_transition_model) base_hypothesiser = DistanceHypothesiser( predictor=kalman_predictor, updater=kalman_updater, measure=Mahalanobis(), missed_distance=15, include_all=False ) hypothesiser = GaussianMixtureHypothesiser( base_hypothesiser, order_by_detection=True ) # Reducer reducer = GaussianMixtureReducer( prune_threshold=1E-3, pruning=True, merge_threshold=200, merging=True ) # Birth component birth_covar = CovarianceMatrix(np.diag([10000, 10, 10000, 10, 10000, 10])) birth_component = TaggedWeightedGaussianState( state_vector=[0, 0, 0, 0, 500, 0], covar=birth_covar**2, weight=0.5, tag=TaggedWeightedGaussianState.BIRTH, timestamp=start_time ) # Tracker gmlcc_tracker = PointProcessMultiTargetTracker( detector=None, hypothesiser=deepcopy(hypothesiser), updater=deepcopy(updater), reducer=deepcopy(reducer), birth_component=deepcopy(birth_component), extraction_threshold=0.90, ) # %% # 4: Make GM-PHD Tracker For Measurement Fusion # --------------------------------------------- # This tracker can use many of the same elements as the GM-LCC one. # %% from stonesoup.updater.pointprocess import PHDUpdater updater = PHDUpdater( kalman_updater, clutter_spatial_density=clutter_spatial_density, prob_detection=0.9, prob_survival=0.9 ) meas_fusion_tracker = PointProcessMultiTargetTracker( detector=None, hypothesiser=deepcopy(hypothesiser), updater=deepcopy(updater), reducer=deepcopy(reducer), birth_component=deepcopy(birth_component), extraction_threshold=0.90, ) # %% # 5: Define a GM-PHD Tracker for Track Fusion # ------------------------------------------- # Track fusion using covariance intersection is implemented in Stone Soup using the # :class:`ChernoffUpdater` class. For use in a GM-PHD, we insert the :class:`ChernoffUpdater` as # the base updater, instead of a typical :class:`~.KalmanUpdater`. The `clutter_spatial_density` # parameter now refers to the estimated intensity of false tracks. Since the previous tracker will # (hopefully) have ignored some of the clutter, we can use a smaller intensity than in the previous # trackers. The `omega` parameter is also adjustable. We will set it to 0.5 for now. # # The remaining tracker parameters have been kept the same as the measurement fusion tracker except # where noted. This will ensure a fair comparison of the results. # %% from stonesoup.updater.chernoff import ChernoffUpdater from stonesoup.measures import Euclidean # Updater ch_updater = ChernoffUpdater(measurement_model=None) updater = PHDUpdater( ch_updater, clutter_spatial_density=1E-15, prob_detection=0.9, prob_survival=0.9 ) # Hypothesiser # The states being used as measurements are in Cartesian space. We will use Euclidean distance in # the :class:`~.DistanceHypothesiser`, meaning that we need a bigger missed distance than the # previous hypothesiser which used the Mahalanobis distance. kalman_predictor = ExtendedKalmanPredictor(truth_transition_model) base_hypothesiser = DistanceHypothesiser( kalman_predictor, ch_updater, Euclidean(), missed_distance=300, include_all=False ) hypothesiser = GaussianMixtureHypothesiser(base_hypothesiser, order_by_detection=True) # Reducer # The states tend to have low weights when they are first initialized using this method, so we will # keep the pruning threshold low. ch_reducer = GaussianMixtureReducer( prune_threshold=1E-10, pruning=True, merge_threshold=200, merging=True ) # Birth component birth_covar = CovarianceMatrix(np.diag([100000, 100, 100000, 100, 100000, 100])) ch_birth_component = TaggedWeightedGaussianState( state_vector=[0, 0, 0, 0, 500, 0], covar=birth_covar**2, weight=0.5, tag=TaggedWeightedGaussianState.BIRTH, timestamp=start_time ) # Make tracker track_fusion_tracker = PointProcessMultiTargetTracker( detector=None, hypothesiser=hypothesiser, updater=updater, reducer=deepcopy(ch_reducer), birth_component=deepcopy(ch_birth_component), extraction_threshold=0.90, ) # %% # 6: Make Metric Managers # ----------------------- # We will track the metrics of each of the four trackers for comparison. # %% from stonesoup.metricgenerator.basicmetrics import BasicMetrics from stonesoup.metricgenerator.ospametric import OSPAMetric from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics from stonesoup.metricgenerator.uncertaintymetric import SumofCovarianceNormsMetric from stonesoup.dataassociator.tracktotrack import TrackToTruth from stonesoup.metricgenerator.manager import SimpleManager # Make the basic metric manager basic_generator = BasicMetrics() ospa_generator = OSPAMetric(c=10, p=1, measure=Euclidean([0, 2, 4])) siap_generator = SIAPMetrics(position_measure=Euclidean(), velocity_measure=Euclidean()) uncertainty_generator = SumofCovarianceNormsMetric() associator = TrackToTruth(association_threshold=30) base_metric_manager = SimpleManager([basic_generator, ospa_generator, siap_generator, uncertainty_generator], associator=associator) sensor1_mm, sensor2_mm = deepcopy(base_metric_manager), deepcopy(base_metric_manager) meas_fusion_mm, track_fusion_mm = deepcopy(base_metric_manager), deepcopy(base_metric_manager) # %% # 7: Set Up the Detection Feeders # ------------------------------- # As one final step before running the simulation, we will write a little class which feeds the # detections for a single timestep. This makes sure that the two radars and the measurement # fusion tracker are getting the same measurements. # # The track fusion tracker will also use the :class:`~.Tracks2GaussianDetectionFeeder` class to # feed the tracks as measurements. At each time step, the resultant live tracks from the JPDA and # GM-LCC trackers will be put into a :class:`~.Tracks2GaussianDetectionFeeder` (using the # :class:`~.DummyDetector` we write below). The feeder will take the most recent state from each # track and turn it into a :class:`~.GaussianDetection` object. The set of detection objects will # be returned and passed into the tracker. # %% from stonesoup.feeder.track import Tracks2GaussianDetectionFeeder 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 # %% # 8: Run Simulation # ----------------- # %% sensor1_detections, sensor2_detections = [], [] jpda_tracks, gmlcc_tracks = set(), set() meas_fusion_tracks, track_fusion_tracks = set(), set() sim_generator = radar_simulator.detections_gen() for t in range(num_steps): # Run JPDA tracker from sensor 1 s1d = next(sim_generator) sensor1_detections.extend(s1d[1]) # hold in list for plotting # Pass the detections into a DummyDetector and set it up as an iterable jpda_tracker.detector = DummyDetector(current=s1d) jpda_tracker.__iter__() # Run the tracker and store the resulting tracks _, sensor1_tracks = next(jpda_tracker) jpda_tracks.update(sensor1_tracks) # Run GM-LCC tracker from sensor 2 s2d = next(sim_generator) sensor2_detections.extend(s2d[1]) # hold in list for plotting # Pass the detections into a DummyDetector and set it up as an iterable gmlcc_tracker.detector = DummyDetector(current=s2d) gmlcc_tracker.__iter__() # Run the tracker and store results time, sensor2_tracks = next(gmlcc_tracker) gmlcc_tracks.update(sensor2_tracks) # Run the GM-PHD for measurement fusion. This one gets called twice, once for each set of # detections. This ensures there is only one detection per target. for detections in [s1d, s2d]: meas_fusion_tracker.detector = DummyDetector(current=detections) meas_fusion_tracker.__iter__() _, tracks = next(meas_fusion_tracker) meas_fusion_tracks.update(tracks) # Run the GM-PHD for track fusion. Similar to the measurement fusion, this tracker gets run # twice, once for each set of tracks. for tracks_as_meas in [sensor1_tracks, sensor2_tracks]: dummy_detector = DummyDetector(current=[time, tracks_as_meas]) track_fusion_tracker.detector = Tracks2GaussianDetectionFeeder(dummy_detector) track_fusion_tracker.__iter__() _, tracks = next(track_fusion_tracker) track_fusion_tracks.update(tracks) # ---------------------------------------------------------------------- # Add ground truth data to metric managers truths = radar_simulator.groundtruth.current for manager in [sensor1_mm, sensor2_mm, meas_fusion_mm, track_fusion_mm]: manager.add_data(groundtruth_paths=truths[1], overwrite=False) # Add measurements to metric managers sensor1_mm.add_data(detections=s1d[1], overwrite=False) sensor2_mm.add_data(detections=s2d[1], overwrite=False) meas_fusion_mm.add_data(detections=s1d[1], overwrite=False) meas_fusion_mm.add_data(detections=s2d[1], overwrite=False) track_fusion_mm.add_data(detections=s1d[1], overwrite=False) track_fusion_mm.add_data(detections=s2d[1], overwrite=False) # Ensure that all tracks have been extracted from the trackers jpda_tracks.update(jpda_tracker.tracks) gmlcc_tracks.update(gmlcc_tracker.tracks) meas_fusion_tracks.update(meas_fusion_tracker.tracks) track_fusion_tracks.update(track_fusion_tracker.tracks) # Remove tracks that have just one state in them as they were probably from clutter jpda_tracks = set([track for track in jpda_tracks if len(track) > 1]) gmlcc_tracks = set([track for track in gmlcc_tracks if len(track) > 1]) meas_fusion_tracks = set([track for track in meas_fusion_tracks if len(track) > 1]) track_fusion_tracks = set([track for track in track_fusion_tracks if len(track) > 1]) # Add tracks to metric managers sensor1_mm.add_data(tracks=jpda_tracks, overwrite=False) sensor2_mm.add_data(tracks=gmlcc_tracks, overwrite=False) meas_fusion_mm.add_data(tracks=meas_fusion_tracks, overwrite=False) track_fusion_mm.add_data(tracks=track_fusion_tracks, overwrite=False) # %% # 9: Plot the Results # -------------------- # Next, we will plot all of the resulting tracks and measurements. This will be done in two plots. # The first plot will show all of the data, and the second plot will show a closer view of one # resultant track. # # These plots are done in 2D to make them more readable. We invite the reader to explore the plot # interactively using the following line in an active Jupyter session. # # %matplotlib widget # %% plotter1, plotter2 = Plotter(), Plotter() for plotter in [plotter1, plotter2]: plotter.plot_ground_truths(set(radar_simulator.groundtruth.groundtruth_paths), [0, 2], color='black') plotter.plot_measurements(sensor1_detections, [0, 2], color='orange', marker='*', measurements_label='Measurements - Airborne Radar') plotter.plot_measurements(sensor2_detections, [0, 2], color='blue', marker='*', measurements_label='Measurements - Ground Radar') plotter.plot_tracks(jpda_tracks, [0, 2], color='red', track_label='Tracks - Airborne Radar (JPDAF)') plotter.plot_tracks(gmlcc_tracks, [0, 2], color='purple', track_label='Tracks - Ground Radar (GM-LCC)') plotter.plot_tracks(meas_fusion_tracks, [0, 2], color='green', track_label='Tracks - Measurement Fusion (GM-PHD)') plotter.plot_tracks(track_fusion_tracks, [0, 2], color='pink', track_label='Tracks - Covariance Intersection (GM-PHD)') # Format the legend a bit. Set the position outside of the plot, and # swap the order of the clutter and ground radar measurements pos = plotter.ax.get_position() plotter.ax.set_position([pos.x0, pos.y0, pos.width * 0.7, pos.height]) k = list(plotter.legend_dict.keys()) k[2], k[3] = k[3], k[2] v = list(plotter.legend_dict.values()) v[2], v[3] = v[3], v[2] plotter.ax.legend(handles=v, labels=k, loc='lower center', bbox_to_anchor=(0.5, -0.5)) plotter1.fig.show() track = track_fusion_tracks.pop() x_min = min([state.state_vector[0] for state in track]) x_max = max([state.state_vector[0] for state in track]) y_min = min([state.state_vector[2] for state in track]) y_max = max([state.state_vector[2] for state in track]) plotter2.ax.set_xlim(x_min-50, x_max+50) plotter2.ax.set_ylim(y_min-50, y_max+50) plotter2.fig.show() # %% # Now we will plot the metrics. First, we call a function for each sensor manager to calculate # the metrics. # %% s1_metrics = sensor1_mm.generate_metrics() s2_metrics = sensor2_mm.generate_metrics() meas_fusion_metrics = meas_fusion_mm.generate_metrics() track_fusion_metrics = track_fusion_mm.generate_metrics() # %% # Now we can plot them. The SIAP and OSPA metrics can be done together in a loop. The # Track-To-Truth ratio needs to be done separately so that it can be calculated at each # timestep. # %% from matplotlib import pyplot as plt from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics # Legend labels for each type of tracker labels = ['Airborne Radar (JPDAF)', 'Ground Radar (GM-LCC)', 'Measurement Fusion (GM-PHD)', 'Covariance Intersection (GM-PHD)'] linestyles = ['dashed', 'dotted', 'solid', 'dashdot'] # Iterate through the SIAP and OSPA metrics for metric_name in ['SIAP Position Accuracy at times', 'SIAP Velocity Accuracy at times', 'SIAP Spuriousness at times', 'SIAP Completeness at times', 'SIAP Ambiguity at times', 'OSPA distances', 'Sum of Covariance Norms Metric']: fig, ax = plt.subplots() # Plot the metrics from each metric manager for tracker_metrics, label, line in zip([s1_metrics, s2_metrics, meas_fusion_metrics, track_fusion_metrics], labels, linestyles): metrics = tracker_metrics[metric_name] ax.plot([m.value for m in metrics.value], linewidth=2, label=label, linestyle=line) # Set x and y labels and title ax.set_xlabel("Time $(s)$") if metric_name.startswith('OSPA'): ax.set_title('OSPA Distance') ax.set_ylabel('Distance') ax.set_ylim(0, 12) # change y axis range for OSPA distance elif metric_name.startswith('Sum of'): ax.set_title(metric_name) ax.set_ylabel('Sum of Covariance Norms') else: ax.set_title(metric_name) ax.set_ylabel(metric_name[5:-9]) # Add units to y axis where applicable if metric_name.startswith('SIAP Position') or metric_name.startswith('SIAP Velocity') \ or metric_name.startswith('OSPA'): ax.set_ylabel(ax.yaxis.get_label().get_text() + ' $(m)$') # Add legend ax.legend(loc='center left', bbox_to_anchor=(1.0, 0.5)) # Plot Track to Truth Ratio fig, ax = plt.subplots() times = sensor1_mm.list_timestamps() # Iterate through the metric managers. For each one, go through the list of all timesteps # and calculate the ratio at that time for manager, label, line in zip([sensor1_mm, sensor2_mm, meas_fusion_mm, track_fusion_mm], labels, linestyles): ratios = [] for time in times: num_tracks = SIAPMetrics.num_tracks_at_time(manager=manager, timestamp=time) num_truths = SIAPMetrics.num_truths_at_time(manager=manager, timestamp=time) ratios.append(num_tracks / num_truths) plt.plot(ratios, linewidth=2, label=label, linestyle=line) ax.set_title('Track-to-Truth Ratio') ax.set_ylabel('Ratio') ax.set_xlabel('Time $(s)$') ax.legend(loc='center left', bbox_to_anchor=(1.0, 0.5)) # sphinx_gallery_thumbnail_number = 3 # %% # References # ---------- # .. [#] Julier, S. J. and Uhlmann, J. K., “General decentralized data fusion # with covariance intersection,” Handbook of multisensor data fusion: # theory and practice, pp. 319–344, 2009. # # .. [#] Clark, D. E. and Campbell, M. A., “Integrating covariance intersection # into Bayesian multi-target tracking filters,” preprint on TechRxiv. # submitted to IEEE Transactions on Aerospace and Electronic Systems. # # .. [#] Hurley, M. B., “An information theoretic justification for covariance # intersection and its generalization,” in Proceedings of the Fifth # International Conference on Information Fusion. FUSION 2002.(IEEE Cat. # No. 02EX5997), vol. 1. IEEE, 2002, pp. 505–511 # # .. [#] Clark, D. and Hunter, E. and Balaji, B. and O'Rourke, S., “Centralized multi-sensor # multi-target data fusion with tracks as measurements,” to be submitted to SPIE Defense and # Security Symposium 2023.