Note
Go to the end to download the full example code or to run this example in your browser via Binder
2 - Multiple Sensor Management
This tutorial follows on from the Single Sensor Management tutorial and further explores how existing Stone Soup features can be used to build simple sensor management algorithms for tracking and state estimation. This second tutorial demonstrates the limitations of the brute force optimisation method introduced in the previous tutorial by increasing the number of sensors used in the scenario.
Introducing multiple sensors
The example in this tutorial considers the same sensor management methods as in Tutorial 1 and applies them to the same set of ground truths in order to observe the difference in tracks. The scenario simulates 3 targets moving on nearly constant velocity trajectories and in this case an adjustable number of sensors. The sensors are simple radar with a defined field of view which can be pointed in a particular direction in order to make an observation.
The first method, using the class RandomSensorManager chooses a target for each sensor to
observe randomly with equal probability.
The second method, uses the class BruteForceSensorManager and aims to reduce the total
uncertainty of the track estimates at each
time step. To achieve this the sensor manager considers all possible configurations of directions for the sensors
to point in. The sensor manager 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 observing
the targets within the chosen sensing configuration.
The introduction of multiple sensors means an increase in the possible combinations of action choices that the brute force sensor manager must consider. This brute force optimisation method of looking at every possible combination of actions becomes very slow as more sensors are introduced, demonstrating the limitations of using this method in more complex scenarios.
As in the first tutorial the OSPA [1], SIAP [2] 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 Tutorial 1.
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 = OrderedSet()
ntruths = 3 # number of ground truths in simulation
time_max = 50 # 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 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 1 this tutorial uses the
RadarRotatingBearingRange sensor with the
number of sensors initially set as 2. Each sensor is positioned along the line \(x=10\), at distance
intervals of 50.
Increasing the number of sensors above 2 significantly increases the run time of the sensor manager due to the
increase in combinations to be considered by the BruteForceSensorManager.
Note that in Tutorial 1 we did not set the resolution for the dwell centre whereas here we are setting it to 30
degrees. This is because for the brute force algorithm with multiple sensors, using the default resolution of 1
degree is not practical. These limitations due to combinatorics are discussed further later.
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
Create the Kalman predictor and updater
Construct a predictor and updater using the KalmanPredictor and 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 first sensor management tutorial.
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.
Create sensor managers
Next we create our sensor management classes. As in Tutorial 1, two sensor manager classes are used -
RandomSensorManager and BruteForceSensorManager.
Random sensor manager
The first method RandomSensorManager, randomly chooses the action(s) for the sensors to
take to make an observation. To do this the
choose_actions() function uses random.choice() to choose a direction for each
sensor to observe from the possible actions it can take. It returns the chosen configuration of sensors and
actions to be taken as a mapping.
from stonesoup.sensormanager import RandomSensorManager
Brute force sensor manager
The second method BruteForceSensorManager chooses the configuration of sensors and actions which results
in the greatest reward as calculated by the reward function.
In this example this is the largest difference between the uncertainty covariances of the target predictions and posteriors assuming a predicted measurement corresponding to that prediction. This means the sensor manager chooses to point the sensors in directions such that the uncertainty will be reduced the most by making observations in those directions.
from stonesoup.sensormanager import BruteForceSensorManager
Reward function
The 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. The sum
of these differences is returned as a metric for that configuration.
from stonesoup.sensormanager.reward import UncertaintyRewardFunction
Initiate sensor managers
Create an instance of each sensor manager class. Both sensor managers take in a sensor_set.
The BruteForceSensorManager also requires a callable reward function which is initiated here
from the UncertaintyRewardFunction.
randomsensormanager = RandomSensorManager(sensor_setA)
# initiate reward function
reward_function = UncertaintyRewardFunction(predictor, updater)
bruteforcesensormanager = BruteForceSensorManager(sensor_setB,
reward_function=reward_function)
Run the sensor managers
Both sensor management methods require a timestamp and a list of tracks at each time step when calling
the function choose_actions(). This returns a mapping of sensors and actions to be taken by each
sensor, decided by the sensor managers.
For both sensor management methods, at each time step the chosen action is given to the sensors and then measurements taken. The tracks are updated based on these measurements with predictions made for tracks which have not been observed.
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 choose_actions() from the class
RandomSensorManager. This returns a mapping of sensors to actions where actions are a
ChangeDwellAction, selected at random.
from ordered_set import OrderedSet
from collections import defaultdict
import copy
sensor_history_A = defaultdict(dict)
for timestep in timesteps[1:]:
# Generate chosen configuration
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)
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)
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
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(sensor_setA)
plotterA.plot_ground_truths(truths, [0, 2])
plotterA.plot_tracks(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