03_OptimisedSensorManagement.py

#!/usr/bin/env python
# coding: utf-8

"""
===============================
3 - Optimised Sensor Management
===============================
"""

# %%
#
# This tutorial follows on from the Multiple 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 SciPy's 'optimize' library. Similar to the
# brute force method introduced previously, the sensor manager considers all possible
# configurations of sensors and actions and uses an optimising function with a
# specified 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 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
# ^^^^^^^^^^^^^^^^^^^^^^
#
# 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 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 = 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)

    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

# Stonesoup plotter requires sets not lists

plotter = AnimatedPlotterly(timesteps, tail_length=1)
plotter.plot_ground_truths(truths, [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.

n_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, n_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,
        resolution=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, n_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,
        resolution=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, n_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,
        resolution=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 = {Track([prior]) for prior in priors}

# Initialise tracks from the OptimizeBruteSensorManager
tracksB = {Track([prior]) for prior in priors}

# Initialise tracks from the OptimizeBasinHoppingSensorManager
tracksC = {Track([prior]) for prior in priors}

# %%
# 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 sensor manager, :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 sensor manager, :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 step size
# 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``
# to `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. The tracks are predicted and those with associated
# measurements 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
from collections import defaultdict
import time
import copy

# Start timer for cell execution time
cell_start_time1 = time.time()

sensor_history_A = defaultdict(dict)
for timestep in timesteps[1:]:

    # Generate chosen configuration
    chosen_actions = bruteforcesensormanager.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)

    for sensor in sensor_setA:
        sensor.act(timestep)
        sensor_history_A[timestep][sensor] = copy.copy(sensor)

        # Observe this ground truth
        measurementsA |= sensor.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)

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. This uses the Stone Soup
# :class:`~.AnimatedPlotterly` plotter, 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(sensor_setA)
plotterA.plot_ground_truths(truths, [0, 2])
plotterA.plot_tracks(set(tracksA), [0, 2], uncertainty=True, plot_history=False)


def plot_sensor_fov(fig_, sensor_set, sensor_history):
    # Plot sensor field of view
    trace_base = len(fig_.data)
    for _ in sensor_set:
        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)

        timestring = frame.name
        timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S")

        for n_, sensor_ in enumerate(sensor_set):
            x = [0, 0]
            y = [0, 0]

            if timestamp in sensor_history:
                sensor_ = sensor_history[timestamp][sensor_]
                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 + n_)

        frame.traces = traces_
        frame.data = data_


plot_sensor_fov(plotterA.fig, sensor_setA, sensor_history_A)
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()

sensor_history_B = defaultdict(dict)
for timestep in timesteps[1:]:

    # Generate chosen configuration
    chosen_actions = optimizebrutesensormanager.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)

    for sensor in sensor_setB:
        sensor.act(timestep)
        sensor_history_B[timestep][sensor] = copy.copy(sensor)

        # Observe this ground truth
        measurementsB |= sensor.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)

cell_run_time2 = round(time.time() - cell_start_time2, 2)

# %%
# Plot ground truths, tracks and uncertainty ellipses for each target.

plotterB = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10)
plotterB.plot_sensors(sensor_setB)
plotterB.plot_ground_truths(truths, [0, 2])
plotterB.plot_tracks(tracksB, [0, 2], uncertainty=True, plot_history=False)
plot_sensor_fov(plotterB.fig, sensor_setB, sensor_history_B)
plotterB.fig

# %%
# Run optimised basin hopping sensor manager
# """"""""""""""""""""""""""""""""""""""""""

# Start timer for cell execution time
cell_start_time3 = time.time()

sensor_history_C = defaultdict(dict)
for timestep in timesteps[1:]:

    # Generate chosen configuration
    chosen_actions = optimizebasinhoppingsensormanager.choose_actions(tracksC, timestep)

    # Create empty dictionary for measurements
    measurementsC = set()

    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)
        sensor_history_C[timestep][sensor] = copy.copy(sensor)

        # Observe this ground truth
        measurementsC |= sensor.measure(OrderedSet(truth[timestep] for truth in truths),
                                        noise=True)

    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 = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10)
plotterC.plot_sensors(sensor_setC)
plotterC.plot_ground_truths(truths, [0, 2])
plotterC.plot_tracks(tracksC, [0, 2], uncertainty=True, plot_history=False)
plot_sensor_fov(plotterC.fig, sensor_setC, sensor_history_C)
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_generatorA = SIAPMetrics(position_measure=Euclidean((0, 2)),
                              velocity_measure=Euclidean((1, 3)),
                              generator_name='BruteForceSensorManager',
                              tracks_key='tracksA',
                              truths_key='truths')

siap_generatorB = SIAPMetrics(position_measure=Euclidean((0, 2)),
                              velocity_measure=Euclidean((1, 3)),
                              generator_name='OptimizeBruteSensorManager',
                              tracks_key='tracksB',
                              truths_key='truths')

siap_generatorC = SIAPMetrics(position_measure=Euclidean((0, 2)),
                              velocity_measure=Euclidean((1, 3)),
                              generator_name='OptimizeBasinHoppingSensorManager',
                              tracks_key='tracksC',
                              truths_key='truths')

from stonesoup.dataassociator.tracktotrack import TrackToTruth

associator = TrackToTruth(association_threshold=30)

from stonesoup.metricgenerator.uncertaintymetric import SumofCovarianceNormsMetric

uncertainty_generatorA = SumofCovarianceNormsMetric(generator_name="BruteForceSensorManager",
                                                    tracks_key="tracksA")

uncertainty_generatorB = SumofCovarianceNormsMetric(generator_name="OptimizeBruteSensorManager",
                                                    tracks_key="tracksB")

uncertainty_generatorC = SumofCovarianceNormsMetric(
    generator_name="OptimizeBasinHoppingSensorManager",
    tracks_key="tracksC")

# %%
# Generate a metric manager.

from stonesoup.metricgenerator.manager import MultiManager

metric_manager = MultiManager([siap_generatorA,
                               siap_generatorB,
                               siap_generatorC,
                               uncertainty_generatorA,
                               uncertainty_generatorB,
                               uncertainty_generatorC],
                              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,
                         'tracksC': tracksC})

metrics = metric_manager.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.

from stonesoup.plotter import MetricPlotter

fig = MetricPlotter()
fig.plot_metrics(metrics, metric_names=['SIAP Position Accuracy at times',
                                        'SIAP Velocity Accuracy at times'],
                 color=['blue', 'orange', 'green'])

# %%
# 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.

fig2 = MetricPlotter()
fig2.plot_metrics(metrics, metric_names=['Sum of Covariance Norms Metric'],
                  color=['blue', 'orange', 'green'])

# %%
# 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.

import matplotlib.pyplot as plt

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')

# %%
#
# The run times show that the optimised methods are significantly quicker than the brute
# force method whilst maintaining 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