#!/usr/bin/env python # coding: utf-8 """ ========================================================== 3 - Optimised Sensor Management ========================================================== """ # %% # # This tutorial follows on from the Multi Sensor Management tutorial and explores the use of # external optimisation libraries to overcome the limitations of the brute force optimisation # method introduced in the previous tutorial. # # The scenario in this example is the same as Tutorial 2, simulating 3 # targets moving on nearly constant velocity trajectories and an adjustable number of sensors. # The sensors are # :class:`~.RadarRotatingBearingRange` with a defined field of view which can be pointed in a # particular direction in order # to make an observation. # # The optimised sensor managers are built around the SciPy optimize library. Similar to the brute # force method introduced previously the sensor manager considers all possible configurations of # sensors and actions and here uses an optimising function to optimise over a given reward function, # returning the optimal configuration. # # The :class:`~.UncertaintyRewardFunction` is used for all sensor managers which chooses the configuration # for which the sum of estimated # uncertainties (as represented by the Frobenius norm of the covariance matrix) can be reduced the most by using # the chosen sensing configuration. # # As in the previous tutorials the SIAP [#]_ and uncertainty metrics are used to assess the # performance of the sensor managers. # %% # Sensor Management example # ------------------------- # # Setup # ^^^^^ # # First a simulation must be set up using components from Stone Soup. For this the following imports are required. import numpy as np import random from datetime import datetime, timedelta start_time = datetime.now() from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState # %% # Generate ground truths # ^^^^^^^^^^^^^^^^^^^^^^ # # Generate transition model and ground truths as in Tutorials 1 & 2. # # The number of targets in this simulation is defined by `ntruths` - here there are 3 targets travelling in # different directions. The time the # simulation is observed for is defined by `time_max`. # # We can fix our random number generator in order to probe a particular example repeatedly. This can be undone by # commenting out the first two lines in the next cell. np.random.seed(1990) random.seed(1990) # Generate transition model transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.005), ConstantVelocity(0.005)]) yps = range(0, 100, 10) # y value for prior state truths = [] ntruths = 3 # number of ground truths in simulation time_max = 50 # timestamps the simulation is observed over xdirection = 1 ydirection = 1 # Generate ground truths for j in range(0, ntruths): truth = GroundTruthPath([GroundTruthState([0, xdirection, yps[j], ydirection], timestamp=start_time)], id=f"id{j}") for k in range(1, time_max): truth.append( GroundTruthState(transition_model.function(truth[k - 1], noise=True, time_interval=timedelta(seconds=1)), timestamp=start_time + timedelta(seconds=k))) truths.append(truth) xdirection *= -1 if j % 2 == 0: ydirection *= -1 # %% # Plot the ground truths. This is done using the :class:`~.Plotterly` class from Stone Soup. from stonesoup.plotter import Plotterly # Stonesoup plotter requires sets not lists truths_set = set(truths) plotter = Plotterly() plotter.plot_ground_truths(truths_set, [0, 2]) plotter.fig # %% # Create sensors # ^^^^^^^^^^^^^^ # Create a set of sensors for each sensor management algorithm. As in Tutorial 2 this tutorial uses the # :class:`~.RadarRotatingBearingRange` sensor with the # number of sensors initially set as 2 and each sensor positioned along the line :math:`x=10`, at distance # intervals of 50. total_no_sensors = 2 from stonesoup.types.state import StateVector from stonesoup.sensor.radar.radar import RadarRotatingBearingRange from stonesoup.types.angle import Angle sensor_setA = set() for n in range(0, total_no_sensors): sensor = RadarRotatingBearingRange( position_mapping=(0, 2), noise_covar=np.array([[np.radians(0.5) ** 2, 0], [0, 1 ** 2]]), ndim_state=4, position=np.array([[10], [n * 50]]), rpm=60, fov_angle=np.radians(30), dwell_centre=StateVector([0.0]), max_range=np.inf, resolutions={'dwell_centre': Angle(np.radians(30))} ) sensor_setA.add(sensor) for sensor in sensor_setA: sensor.timestamp = start_time sensor_setB = set() for n in range(0, total_no_sensors): sensor = RadarRotatingBearingRange( position_mapping=(0, 2), noise_covar=np.array([[np.radians(0.5) ** 2, 0], [0, 1 ** 2]]), ndim_state=4, position=np.array([[10], [n * 50]]), rpm=60, fov_angle=np.radians(30), dwell_centre=StateVector([0.0]), max_range=np.inf, resolutions={'dwell_centre': Angle(np.radians(30))} ) sensor_setB.add(sensor) for sensor in sensor_setB: sensor.timestamp = start_time sensor_setC = set() for n in range(0, total_no_sensors): sensor = RadarRotatingBearingRange( position_mapping=(0, 2), noise_covar=np.array([[np.radians(0.5) ** 2, 0], [0, 1 ** 2]]), ndim_state=4, position=np.array([[10], [n * 50]]), rpm=60, fov_angle=np.radians(30), dwell_centre=StateVector([0.0]), max_range=np.inf, resolutions={'dwell_centre': Angle(np.radians(30))} ) sensor_setC.add(sensor) for sensor in sensor_setC: sensor.timestamp = start_time # %% # Create the Kalman predictor and updater # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # # Construct a predictor and updater using the :class:`~.KalmanPredictor` and :class:`~.ExtendedKalmanUpdater` # components from Stone Soup. The measurement model for the updater is `None` as it is an attribute of the sensor. from stonesoup.predictor.kalman import KalmanPredictor predictor = KalmanPredictor(transition_model) from stonesoup.updater.kalman import ExtendedKalmanUpdater updater = ExtendedKalmanUpdater(measurement_model=None) # measurement model is added to detections by the sensor # %% # Run the Kalman filters # ^^^^^^^^^^^^^^^^^^^^^^ # # Create priors which estimate the targets' initial states - these are the same as in the previous # sensor management tutorials. from stonesoup.types.state import GaussianState priors = [] xdirection = 1.2 ydirection = 1.2 for j in range(0, ntruths): priors.append(GaussianState([[0], [xdirection], [yps[j]+0.1], [ydirection]], np.diag([0.5, 0.5, 0.5, 0.5]+np.random.normal(0,5e-4,4)), timestamp=start_time)) xdirection *= -1 if j % 2 == 0: ydirection *= -1 # %% # Initialise the tracks by creating an empty list and appending the priors generated. This needs to be done # separately for each sensor manager method as they will generate different sets of tracks. from stonesoup.types.track import Track # Initialise tracks from the BruteForceSensorManager tracksA = [] for j, prior in enumerate(priors): tracksA.append(Track([prior])) # Initialise tracks from the OptimizeBruteSensorManager tracksB = [] for j, prior in enumerate(priors): tracksB.append(Track([prior])) # Initialise tracks from the OptimizeBasinHoppingSensorManager tracksC = [] for j, prior in enumerate(priors): tracksC.append(Track([prior])) # %% # Create sensor managers # ^^^^^^^^^^^^^^^^^^^^^^ # # The :class:`~.UncertaintyRewardFunction` will be used for each sensor manager as in Tutorials 1 & 2 # and the :class:`~.BruteForceSensorManager` will be used as a comparison to the optimised methods. from stonesoup.sensormanager.reward import UncertaintyRewardFunction from stonesoup.sensormanager import BruteForceSensorManager # %% # Optimised Brute Force Sensor Manager # """""""""""""""""""""""""""""""""""" # # The first optimised method, :class:`~.OptimizeBruteSensorManager` uses :func:`~.scipy.optimize.brute` # which minimizes a function over a given range using a brute force method. This can be tailored by setting # the number of grid points to search over or by adding the use of a polishing function. from stonesoup.sensormanager import OptimizeBruteSensorManager # %% # Optimised Basin Hopping Sensor Manager # """""""""""""""""""""""""""""""""""""" # # The second optimised method, :class:`~.OptimizeBasinHoppingSensorManager` uses :func:`~.scipy.optimize.basinhopping` # which finds the global minimum of a function using the basin-hopping algorithm. This is a combination of a # global stepping algorithm and local minimization at each step. Parameters such as number of basin hopping # iterations or stepsize can be set to tailor the algorithm to requirements. from stonesoup.sensormanager import OptimizeBasinHoppingSensorManager # %% # Initiate sensor managers # ^^^^^^^^^^^^^^^^^^^^^^^^ # # Create an instance of each sensor manager class. For the optimised sensor managers the default settings # will be used meaning only a sensor set and reward function is required. The :class:`~.UncertaintyRewardFunction` # will be used for each sensor manager. For the :class:`~.OptimizeBruteSensorManager` a polishing function # is used by setting `finish=True`. # initiate reward function reward_function = UncertaintyRewardFunction(predictor, updater) bruteforcesensormanager = BruteForceSensorManager(sensor_setA, reward_function=reward_function) optimizebrutesensormanager = OptimizeBruteSensorManager(sensor_setB, reward_function=reward_function, finish=True) optimizebasinhoppingsensormanager = OptimizeBasinHoppingSensorManager(sensor_setC, reward_function=reward_function) # %% # Run the sensor managers # ^^^^^^^^^^^^^^^^^^^^^^^ # # Each sensor management method requires a timestamp and a list of tracks at each time step when calling # the function :meth:`choose_actions`. This returns a mapping of sensors and actions to be taken by each # sensor, decided by the sensor managers. # # For each sensor management method, at each time step the chosen action is given to the sensors and then # measurements taken. At each timestep the tracks are predicted and those with measurements associated are # updated. # # First a hypothesiser and data associator are required for use in each tracker. from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.measures import Mahalanobis hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=5) from stonesoup.dataassociator.neighbour import GNNWith2DAssignment data_associator = GNNWith2DAssignment(hypothesiser) # %% # Run brute force sensor manager # """""""""""""""""""""""""""""" # # Each sensor manager is run in the same way as in the previous tutorials. from ordered_set import OrderedSet import time # Start timer for cell execution time cell_start_time1 = time.time() # Generate list of timesteps from ground truth timestamps timesteps = [] for state in truths[0]: timesteps.append(state.timestamp) for timestep in timesteps[1:]: # Generate chosen configuration chosen_actions = bruteforcesensormanager.choose_actions(tracksA, timestep) # Create empty dictionary for measurements measurementsA = [] for chosen_action in chosen_actions: for sensor, actions in chosen_action.items(): sensor.add_actions(actions) for sensor in sensor_setA: sensor.act(timestep) # Observe this ground truth measurements = sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) measurementsA.extend(measurements) hypotheses = data_associator.associate(tracksA, measurementsA, timestep) for track in tracksA: hypothesis = hypotheses[track] if hypothesis.measurement: post = updater.update(hypothesis) track.append(post) else: # When data associator says no detections are good enough, we'll keep the prediction track.append(hypothesis.prediction) cell_run_time1 = round(time.time() - cell_start_time1, 2) # %% # Plot ground truths, tracks and uncertainty ellipses for each target. The positions of the sensors are indicated # by black x markers. plotterA = Plotterly() plotterA.plot_sensors(sensor_setA) plotterA.plot_ground_truths(truths_set, [0, 2]) plotterA.plot_tracks(set(tracksA), [0, 2], uncertainty=True) plotterA.fig # %% # The resulting plot is exactly the same as Tutorial 2. # %% # Run optimised brute force sensor manager # """""""""""""""""""""""""""""""""""""""" # Start timer for cell execution time cell_start_time2 = time.time() for timestep in timesteps[1:]: # Generate chosen configuration chosen_actions = optimizebrutesensormanager.choose_actions(tracksB, timestep) # Create empty dictionary for measurements measurementsB = [] for chosen_action in chosen_actions: for sensor, actions in chosen_action.items(): sensor.add_actions(actions) for sensor in sensor_setB: sensor.act(timestep) # Observe this ground truth measurements = sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) measurementsB.extend(measurements) hypotheses = data_associator.associate(tracksB, measurementsB, timestep) for track in tracksB: hypothesis = hypotheses[track] if hypothesis.measurement: post = updater.update(hypothesis) track.append(post) else: # When data associator says no detections are good enough, we'll keep the prediction track.append(hypothesis.prediction) cell_run_time2 = round(time.time() - cell_start_time2, 2) # %% # Plot ground truths, tracks and uncertainty ellipses for each target. plotterB = Plotterly() plotterB.plot_sensors(sensor_setB) plotterB.plot_ground_truths(truths_set, [0, 2]) plotterB.plot_tracks(set(tracksB), [0, 2], uncertainty=True) plotterB.fig # %% # Run optimised basin hopping sensor manager # """""""""""""""""""""""""""""""""""""""""" # Start timer for cell execution time cell_start_time3 = time.time() for timestep in timesteps[1:]: # Generate chosen configuration chosen_actions = optimizebasinhoppingsensormanager.choose_actions(tracksC, timestep) # Create empty dictionary for measurements measurementsC = [] for chosen_action in chosen_actions: for sensor, actions in chosen_action.items(): sensor.add_actions(actions) for sensor in sensor_setC: sensor.act(timestep) # Observe this ground truth measurements = sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) measurementsC.extend(measurements) hypotheses = data_associator.associate(tracksC, measurementsC, timestep) for track in tracksC: hypothesis = hypotheses[track] if hypothesis.measurement: post = updater.update(hypothesis) track.append(post) else: # When data associator says no detections are good enough, we'll keep the prediction track.append(hypothesis.prediction) cell_run_time3 = round(time.time() - cell_start_time3, 2) # %% # Plot ground truths, tracks and uncertainty ellipses for each target. plotterC = Plotterly() plotterC.plot_sensors(sensor_setC) plotterC.plot_ground_truths(truths_set, [0, 2]) plotterC.plot_tracks(set(tracksC), [0, 2], uncertainty=True) plotterC.fig # %% # At first glance, the plots for each of the optimised sensor managers show a very # similar tracking performance to the :class:`~.BruteForceSensorManager`. # %% # Metrics # ------- # # As in Tutorials 1 & 2 the SIAP and uncertainty metrics are used to compare # the tracking performance of the sensor managers in more detail. from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics from stonesoup.measures import Euclidean siap_generator = SIAPMetrics(position_measure=Euclidean((0, 2)), velocity_measure=Euclidean((1, 3))) from stonesoup.dataassociator.tracktotrack import TrackToTruth associator = TrackToTruth(association_threshold=30) from stonesoup.metricgenerator.uncertaintymetric import SumofCovarianceNormsMetric uncertainty_generator = SumofCovarianceNormsMetric() # %% # Generate a metric manager for each sensor management method. from stonesoup.metricgenerator.manager import SimpleManager metric_managerA = SimpleManager([siap_generator, uncertainty_generator], associator=associator) metric_managerB = SimpleManager([siap_generator, uncertainty_generator], associator=associator) metric_managerC = SimpleManager([siap_generator, uncertainty_generator], associator=associator) # %% # For each time step, data is added to the metric manager on truths and tracks. # The metrics themselves can then be generated from the metric manager. metric_managerA.add_data(truths, tracksA) metric_managerB.add_data(truths, tracksB) metric_managerC.add_data(truths, tracksC) metricsA = metric_managerA.generate_metrics() metricsB = metric_managerB.generate_metrics() metricsC = metric_managerC.generate_metrics() # %% # SIAP metrics # ^^^^^^^^^^^^ # # First we look at SIAP metrics. We are only interested in the positional accuracy (PA) and # velocity accuracy (VA). These metrics can be plotted to show how they change over time. import matplotlib.pyplot as plt fig, axes = plt.subplots(2) times = metric_managerA.list_timestamps() pa_metricA = metricsA['SIAP Position Accuracy at times'] va_metricA = metricsA['SIAP Velocity Accuracy at times'] pa_metricB = metricsB['SIAP Position Accuracy at times'] va_metricB = metricsB['SIAP Velocity Accuracy at times'] pa_metricC = metricsC['SIAP Position Accuracy at times'] va_metricC = metricsC['SIAP Velocity Accuracy at times'] axes[0].set(title='Positional Accuracy', xlabel='Time', ylabel='PA') axes[0].plot(times, [metric.value for metric in pa_metricA.value], label='BruteForceSensorManager') axes[0].plot(times, [metric.value for metric in pa_metricB.value], label='OptimizeBruteSensorManager') axes[0].plot(times, [metric.value for metric in pa_metricC.value], label='OptimizeBasinHoppingSensorManager') axes[0].legend() axes[1].set(title='Velocity Accuracy', xlabel='Time', ylabel='VA') axes[1].plot(times, [metric.value for metric in va_metricA.value], label='BruteForceSensorManager') axes[1].plot(times, [metric.value for metric in va_metricB.value], label='OptimizeBruteSensorManager') axes[1].plot(times, [metric.value for metric in va_metricC.value], label='OptimizeBasinHoppingSensorManager') axes[1].legend() # %% # Both graphs show that there is little performance difference between the different sensor managers. # Positional accuracy remains consistently good and velocity accuracy improves after overcoming # the initial differences in the priors. # # Uncertainty metric # ^^^^^^^^^^^^^^^^^^ # # Next we look at the uncertainty metric which computes the sum of covariance matrix norms of each state at each # time step. This is plotted over time for each sensor manager method. uncertainty_metricA = metricsA['Sum of Covariance Norms Metric'] uncertainty_metricB = metricsB['Sum of Covariance Norms Metric'] uncertainty_metricC = metricsC['Sum of Covariance Norms Metric'] fig = plt.figure() ax = fig.add_subplot(1, 1, 1) ax.plot([i.timestamp for i in uncertainty_metricA.value], [i.value for i in uncertainty_metricA.value], label='BruteForceSensorManager') ax.plot([i.timestamp for i in uncertainty_metricB.value], [i.value for i in uncertainty_metricB.value], label='OptimizeBruteSensorManager') ax.plot([i.timestamp for i in uncertainty_metricC.value], [i.value for i in uncertainty_metricC.value], label='OptimizeBasinHoppingSensorManager') ax.set_ylabel("Sum of covariance matrix norms") ax.set_xlabel("Time") ax.legend() # %% # The uncertainty metric shows some variation between the sensor management methods, # with a little more variation in the basin hopping method than the brute force methods. # Overall they have a similar performance, improving after overcoming the initial # differences in the priors. # # Cell runtime # ^^^^^^^^^^^^ # # Now let us compare the calculated runtime of the tracking loop for each of the sensor managers. fig = plt.figure() ax = fig.add_subplot(1, 1, 1) ax.set_ylabel('Cell run time (s)') ax.bar(['Brute Force', 'Optimised Brute Force', 'Optimised Basin Hopping'], [cell_run_time1, cell_run_time2, cell_run_time3], color=['tab:blue', 'tab:orange', 'tab:green']) print(f'Brute Force: {cell_run_time1} s') print(f'Optimised Brute Force: {cell_run_time2} s') print(f'Optimised Basin Hopping: {cell_run_time3} s') # %% # These run times show that each of the optimised methods are significantly quicker than the brute force method # whilst maintaining a similar tracking performance. This difference becomes more clear when the complexity of # the situation increases - by including additional sensors for example. # %% # References # ---------- # # .. [#] *Votruba, Paul & Nisley, Rich & Rothrock, Ron and Zombro, Brett.*, **Single Integrated Air # Picture (SIAP) Metrics Implementation**, 2001 # sphinx_gallery_thumbnail_number = 6