#!/usr/bin/env python """ ============================ 1 - Single Sensor Management ============================ """ # %% # # This tutorial introduces the sensor manager classes in Stone Soup which can be used to build # simple sensor management algorithms for tracking and state estimation. The intention is to # further build on these base classes to develop more complex sensor management algorithms. # # Background # ---------- # # Sensor management is the process of deciding and executing the actions that a sensor, # or group of sensors will take in a specific scenario and with a particular objective, # or objectives in mind. The process involves using information about the scenario to determine # an appropriate action for the sensing system to take. An observation of the state of the # system is then made using the sensing configuration decided by the sensor manager. The # observations are used to update the estimate of the collective states and this update is used # (if necessary) to determine the next action for the sensing system to take. # # A simple example can be imagined using a sensor with a limited field of view which must decide # which direction it should point in at each time step. Alternatively, we might construct an # objective based example by imagining that the desired target is fast moving and the sensor can # only observe one target at a time. If there are multiple targets which could be observed the # sensor manager could choose to observe the target that had the greatest estimated velocity at # the current time. # # The example in this notebook considers two simple sensor management methods and applies them # to the same ground truths in order to quantify the difference in behaviour. The scenario # simulates 3 targets moving on nearly constant velocity trajectories and a radar with a # specified field of view, which can be pointed in a particular direction. # # The first method, using the class :class:`~.RandomSensorManager` chooses a direction to point # in randomly with equal probability. The second method, using the class # :class:`~.BruteForceSensorManager` considers every possible direction the sensor could point # in and uses a reward function to determine the best choice of action. In this example the # reward function aims to reduce the total uncertainty of the track estimates at each time step. # To achieve this the sensor manager chooses to look in the direction which results in the # greatest reduction in uncertainty - as represented by the Frobenius norm of the covariance # matrix. # # Sensor management as a POMDP # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # # Sensor management problems can be considered as Partially Observable Markov Decision Processes # (POMDPs) where observations provide information about the current state of the system but # there is uncertainty in the estimate of the underlying state due to noisy sensors and # imprecise models of target evaluation. # # POMDPs consist of: # * :math:`X_k`, the finite set of possible states for each stage index :math:`k`. # * :math:`A_k`, the finite set of possible actions for each stage index :math:`k`. # * :math:`R_k(x, a)`, the reward function. # * :math:`Z_k`, the finite set of possible observations for each stage index :math:`k`. # * :math:`f_k(x_{k}|x_{k-1})`, a (set of) state transition function(s). (Note that actions are # excluded from the function at the moment. It may be necessary to include them if prior # sensor actions cause the targets to modify their behaviour.) # * :math:`h_k(z_k | x_k, a_k)`, a (set of) observation function(s). # * :math:`\{x\}_k`, the set of states at :math:`k` to be estimated. # * :math:`\{a\}_k`, a set of actions at :math:`k` to be chosen. # * :math:`\{z\}_k`, the observations at :math:`k` returned by the sensor. # * :math:`\Psi_{k-1}`, denotes the complete set of 'intelligence' available to the sensor # manager before deciding on an action at :math:`k`. This includes the prior set of state # estimates :math:`\{x\}_{k-1}`, but may also encompass contextual information, sensor # constraints or mission parameters. # # Figure 1: Illustration of sequential actions and measurements. [#]_ # # .. image:: ../../_static/SM_flow_diagram.png # :width: 800 # :alt: Illustration of sequential actions and measurements # # :math:`\Psi_k` is the intelligence available to the sensor manager at stage index :math:`k`, # to help select the action :math:`a_k` for the system to take. An observation :math:`z_k` is # made by the sensing system, giving information on the state :math:`x_k`. The action # :math:`a_k` and observation :math:`z_k` are added to the intelligence set to generate # :math:`\Psi_{k+1}`, the intelligence available at stage index :math:`k+1`. # # Comparing sensor management methods using metrics # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # # The performance of the two sensor management methods explored in this tutorial can be assessed # using metrics available from the Stone Soup framework. The metrics used to assess the # performance of the different methods are the OSPA metric [#]_, SIAP metrics [#]_ and an # uncertainty metric. Demonstrations of the OSPA and SIAP metrics can be found in the Metrics # Example. # # The uncertainty metric computes the covariance matrices of all target states at each time step # and calculates the sum of their norms. This gives a measure of the total uncertainty across # all tracks at each time step. # %% # 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 ordered_set import OrderedSet from datetime import datetime, timedelta start_time = datetime.now().replace(microsecond=0) from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \ ConstantVelocity from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState # %% # Generate ground truths # ^^^^^^^^^^^^^^^^^^^^^^ # # Following the methods from previous Stone Soup tutorials, we generate a series of combined # linear Gaussian transition models and generate ground truths. Each ground truth is offset in # the y-direction by 10. # # 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 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 # i.e. fk(xk|xk-1) transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.005), ConstantVelocity(0.005)]) yps = range(0, 100, 10) # y value for prior state truths = OrderedSet() ntruths = 3 # number of ground truths in simulation time_max = 20 # timestamps the simulation is observed over timesteps = [start_time + timedelta(seconds=k) for k in range(time_max)] xdirection = 1 ydirection = 1 # Generate ground truths for j in range(0, ntruths): truth = GroundTruthPath([GroundTruthState([0, xdirection, yps[j], ydirection], timestamp=timesteps[0])], 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=timesteps[k])) truths.add(truth) # alternate directions when initiating tracks xdirection *= -1 if j % 2 == 0: ydirection *= -1 # %% # Plot the ground truths. This is done using the :class:`~.AnimatedPlotterly` class from # Stone Soup. from stonesoup.plotter import AnimatedPlotterly plotter = AnimatedPlotterly(timesteps, tail_length=1) plotter.plot_ground_truths(truths, [0, 2]) plotter.fig # %% # Create sensors # ^^^^^^^^^^^^^^ # # Create a sensor for each sensor management algorithm. This tutorial uses the # :class:`~.RadarRotatingBearingRange` sensor. This sensor is an :class:`~.Actionable` so # is capable of returning the actions it can possibly # take at a given time step and can also be given an action to take before taking # measurements. # See the Creating an Actionable Sensor Example for a more detailed explanation of # actionable sensors. # # The :class:`~.RadarRotatingBearingRange` has a dwell centre which is an # :class:`~.ActionableProperty`, so in this case the action is changing the dwell centre # to point in a specific direction. from stonesoup.types.state import StateVector from stonesoup.sensor.radar.radar import RadarRotatingBearingRange sensorA = 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], [0]]), rpm=60, fov_angle=np.radians(30), dwell_centre=StateVector([0.0]), max_range=np.inf ) sensorA.timestamp = start_time sensorB = 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], [0]]), rpm=60, fov_angle=np.radians(30), dwell_centre=StateVector([0.0]), max_range=np.inf ) sensorB.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 # :class:`~.ExtendedKalmanUpdater` is used because it can be used for both linear and nonlinear # measurement models. 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 # ^^^^^^^^^^^^^^^^^^^^^^ # # First, create ``ntruths`` priors which estimate the targets’ initial states, one for each # target. In this example, each prior is offset by 0.5 in the y direction meaning the position of # the track is initially not very accurate. The velocity is also systematically offset by +0.5 # in both the x and y directions. 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 both sensor manager methods as they will generate different # sets of tracks. from stonesoup.types.track import Track # Initialise tracks from the RandomSensorManager tracksA = {Track([prior]) for prior in priors} # Initialise tracks from the BruteForceSensorManager tracksB = {Track([prior]) for prior in priors} # %% # Create sensor managers # ^^^^^^^^^^^^^^^^^^^^^^ # # Next we create our sensor manager classes. Two sensor manager classes are used in this tutorial # - :class:`~.RandomSensorManager` and :class:`~.BruteForceSensorManager`. # # Random sensor manager # """"""""""""""""""""" # # The first sensor manager, :class:`~.RandomSensorManager`, randomly chooses action(s) for # the sensor to take to make an observation. To do this, the :meth:`choose_actions` function # uses :meth:`random.sample()` to draw a random sample from all possible directions the sensor # could point in at each time step. from stonesoup.sensormanager import RandomSensorManager # %% # Brute force sensor manager # """""""""""""""""""""""""" # # The second sensor manager, :class:`~.BruteForceSensorManager`, iterates through every possible # action a sensor can take at a given time step and selects the action(s) which give the maximum # reward as calculated by the reward function. In this example the reward function is used to # select a direction for the sensor to point in such that the total uncertainty of the tracks # will be reduced the most by making an observation in that direction. from stonesoup.sensormanager import BruteForceSensorManager # %% # Reward function # """""""""""""""" # A reward function is used to quantify the benefit of # sensors taking a particular action or set of actions. This can be crafted specifically for an # example to achieve a particular objective. The function used in this example is quite # generic but could be substituted for any callable function which returns a numeric value that # the sensor manager can maximise. # # The :class:`~.UncertaintyRewardFunction` calculates the uncertainty reduction by computing the # difference between the covariance matrix norms of the prediction, and the posterior assuming a # predicted measurement corresponding to that prediction. from stonesoup.sensormanager.reward import UncertaintyRewardFunction # %% # Initiate the sensor managers # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # Create an instance of each sensor manager class. Each class takes in a ``sensor_set``, for this # example it is a set of one sensor. The :class:`~.BruteForceSensorManager` also requires a # callable reward function which here is the :class:`UncertaintyRewardFunction`. randomsensormanager = RandomSensorManager({sensorA}) # initiate reward function reward_function = UncertaintyRewardFunction(predictor, updater) bruteforcesensormanager = BruteForceSensorManager({sensorB}, reward_function=reward_function) # %% # Run the sensor managers # ^^^^^^^^^^^^^^^^^^^^^^^ # # For both methods the :meth:`choose_actions` # function requires a time step and a tracks list as inputs. # # For both sensor management methods, the chosen actions are added to the sensor and # measurements made. Tracks which have been observed by the sensor are updated and those that # haven't been predicted forward. These states are appended to the tracks list. # # First, a hypothesiser and data associator are required for use in both trackers: 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 random sensor manager # """"""""""""""""""""""""" # # Here, the chosen target for observation is selected randomly using the method # :meth:`choose_actions()` from the class :class:`~.RandomSensorManager`. import copy sensor_history_A = dict() for timestep in timesteps[1:]: # Generate chosen configuration # i.e. {a}k chosen_actions = randomsensormanager.choose_actions(tracksA, timestep) # Create empty dictionary for measurements measurementsA = set() for chosen_action in chosen_actions: for sensor, actions in chosen_action.items(): sensor.add_actions(actions) sensorA.act(timestep) # Store sensor history for plotting sensor_history_A[timestep] = copy.copy(sensorA) # Observe this ground truth # i.e. {z}k measurementsA |= sensorA.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) 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) # %% # Plot ground truths, tracks and uncertainty ellipses for each target. This uses the Stone Soup # :class:`~.AnimatedPlotterly`, with added code to plot the field of view of the sensor. import plotly.graph_objects as go from stonesoup.functions import pol2cart plotterA = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterA.plot_sensors(sensorA) plotterA.plot_ground_truths(truths, [0, 2]) plotterA.plot_tracks(tracksA, [0, 2], uncertainty=True, plot_history=False) def plot_sensor_fov(fig_, sensor_history): # Plot sensor field of view trace_base = len(fig_.data) fig_.add_trace(go.Scatter(mode='lines', line=go.scatter.Line(color='black', dash='dash'))) for frame in fig_.frames: traces_ = list(frame.traces) data_ = list(frame.data) x = [0, 0] y = [0, 0] timestring = frame.name timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") if timestamp in sensor_history: sensor_ = sensor_history[timestamp] for i, fov_side in enumerate((-1, 1)): range_ = min(getattr(sensor_, 'max_range', np.inf), 100) x[i], y[i] = pol2cart(range_, sensor_.dwell_centre[0, 0] + sensor_.fov_angle / 2 * fov_side) \ + sensor_.position[[0, 1], 0] else: continue data_.append(go.Scatter(x=[x[0], sensor_.position[0], x[1]], y=[y[0], sensor_.position[1], y[1]], mode="lines", line=go.scatter.Line(color='black', dash='dash'), showlegend=False)) traces_.append(trace_base) frame.traces = traces_ frame.data = data_ plot_sensor_fov(plotterA.fig, sensor_history_A) plotterA.fig # %% # Run brute force sensor manager # """""""""""""""""""""""""""""" # # Here the chosen action is selected based on the difference between the # covariance matrices of the # prediction and posterior, for targets which could be observed by the sensor taking that action # - i.e. pointing its dwell centre in that given direction. # # The :meth:`choose_actions` function from the :class:`~.BruteForceSensorManager` is called at # each time step. This means that at each time step, for each track: # # * A prediction is made for each track and the covariance matrix norms stored. # * For each possible action a sensor could take, a synthetic detection is made using this # sensor configuration. # * A hypothesis is generated based on the stored prediction and synthetic detection. # * This hypothesis is used to do an update and the covariance matrix norms of the update are # stored. # * The difference between the covariance matrix norms of the update and the prediction is # calculated. # # The overall reward is calculated as the sum of the differences between these covariance matrix # norms for all the tracks observed by the possible action. The sensor manager identifies the # action which results in the largest value of this reward, and therefore largest reduction in # uncertainty, and returns the optimal sensor/action mapping. # # The chosen action is given to the sensor, measurements are made, and the tracks updated based # on these measurements. Predictions are made for tracks which have not been observed by the # sensor. sensor_history_B = dict() for timestep in timesteps[1:]: # Generate chosen configuration # i.e. {a}k chosen_actions = bruteforcesensormanager.choose_actions(tracksB, timestep) # Create empty dictionary for measurements measurementsB = set() for chosen_action in chosen_actions: for sensor, actions in chosen_action.items(): sensor.add_actions(actions) sensorB.act(timestep) # Store sensor history for plotting sensor_history_B[timestep] = copy.copy(sensorB) # Observe this ground truth # i.e. {z}k measurementsB |= sensorB.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) 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) # %% # Plot ground truths, tracks and uncertainty ellipses for each target. plotterB = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterB.plot_sensors(sensorB) plotterB.plot_ground_truths(truths, [0, 2]) plotterB.plot_tracks(tracksB, [0, 2], uncertainty=True, plot_history=False) plot_sensor_fov(plotterB.fig, sensor_history_B) plotterB.fig # %% # The smaller uncertainty ellipses in this plot suggest that the # :class:`~.BruteForceSensorManager` provides a much better track than the # :class:`~.RandomSensorManager`. # # Metrics # ------- # # Metrics can be used to compare how well different sensor management techniques are working. # Full explanations of the OSPA # and SIAP metrics can be found in the Metrics Example. from stonesoup.metricgenerator.ospametric import OSPAMetric ospa_generatorA = OSPAMetric(c=40, p=1, generator_name='RandomSensorManager', tracks_key='tracksA', truths_key='truths') ospa_generatorB = OSPAMetric(c=40, p=1, generator_name='BruteForceSensorManager', tracks_key='tracksB', truths_key='truths') from stonesoup.metricgenerator.tracktotruthmetrics import SIAPMetrics from stonesoup.measures import Euclidean siap_generatorA = SIAPMetrics(position_measure=Euclidean((0, 2)), velocity_measure=Euclidean((1, 3)), generator_name='RandomSensorManager', tracks_key='tracksA', truths_key='truths') siap_generatorB = SIAPMetrics(position_measure=Euclidean((0, 2)), velocity_measure=Euclidean((1, 3)), generator_name='BruteForceSensorManager', tracks_key='tracksB', truths_key='truths') # %% # The SIAP metrics require an associator to associate tracks to ground truths. This is done # using the :class:`~.TrackToTruth` associator with an association threshold of 30. from stonesoup.dataassociator.tracktotrack import TrackToTruth associator = TrackToTruth(association_threshold=30) # %% # The OSPA and SIAP metrics don't take the uncertainty of the track into account. The initial # plots of the tracks and ground truths show that by plotting the uncertainty ellipses, there is # visually less uncertainty in the tracks generated by the :class:`~.BruteForceSensorManager`. # # To quantify this, we can use an uncertainty metric to look at the sum of covariance matrix # norms at each time step. This gives a representation of the overall uncertainty of the # tracking over time. from stonesoup.metricgenerator.uncertaintymetric import SumofCovarianceNormsMetric uncertainty_generatorA = SumofCovarianceNormsMetric(generator_name='RandomSensorManager', tracks_key='tracksA') uncertainty_generatorB = SumofCovarianceNormsMetric(generator_name='BruteForceSensorManager', tracks_key='tracksB') # %% # A metric manager is used for the generation of metrics on multiple :class:`~.GroundTruthPath` # and :class:`~.Track` objects. This takes in the metric generators, as well as the associator # required for the SIAP metrics. # # We input the metric generators for both sensor management methods into the same metric manager. from stonesoup.metricgenerator.manager import MultiManager metric_manager = MultiManager([ospa_generatorA, ospa_generatorB, siap_generatorA, siap_generatorB, uncertainty_generatorA, uncertainty_generatorB], 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_manager.add_data({'truths': truths, 'tracksA': tracksA, 'tracksB': tracksB}) metrics = metric_manager.generate_metrics() # %% # OSPA metric # ^^^^^^^^^^^ # First we look at the OSPA metric. This is plotted over time for each sensor manager method. from stonesoup.plotter import MetricPlotter fig = MetricPlotter() fig.plot_metrics(metrics, metric_names=['OSPA distances']) # %% # The :class:`~.BruteForceSensorManager` generally results in a smaller OSPA distance than the # random observations of the :class:`~.RandomSensorManager`, reflecting the better tracking # performance seen in the tracking plots. # # SIAP metrics # ^^^^^^^^^^^^ # Next we look at SIAP metrics. This can be done by generating a table which displays all the # SIAP metrics computed, as seen in the Metrics Example. However, completeness, ambiguity and # spuriousness are not relevant for this example because we are not initiating and deleting # tracks, and we have one track corresponding to each ground truth. Here we only plot positional # accuracy and velocity accuracy over time. fig2 = MetricPlotter() fig2.plot_metrics(metrics, metric_names=['SIAP Position Accuracy at times', 'SIAP Velocity Accuracy at times']) # %% # # Similar to the OSPA distances, the :class:`~.BruteForceSensorManager` generally results in # both a better positional accuracy and velocity accuracy than the random observations of the # :class:`~.RandomSensorManager`. # # Uncertainty metric # ^^^^^^^^^^^^^^^^^^ # Finally, 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. fig3 = MetricPlotter() fig3.plot_metrics(metrics, metric_names=['Sum of Covariance Norms Metric']) # sphinx_gallery_thumbnail_number = 6 # %% # This metric shows that the uncertainty in the tracks generated by the # :class:`~.RandomSensorManager` is much greater than for those generated by the # :class:`~.BruteForceSensorManager`. This is also reflected by the uncertainty ellipses in the # initial plots of tracks and truths. # %% # References # ---------- # # .. [#] *Hero, A.O., Castanon, D., Cochran, D. and Kastella, K.*, **Foundations and Applications # of Sensor Management**. New York: Springer, 2008. # .. [#] *D. Schuhmacher, B. Vo and B. Vo*, **A Consistent Metric for Performance Evaluation of # Multi-Object Filters**, IEEE Trans. Signal Processing 2008 # .. [#] *Votruba, Paul & Nisley, Rich & Rothrock, Ron and Zombro, Brett.*, **Single Integrated Air # Picture (SIAP) Metrics Implementation**, 2001