Example of Bearing-only tracking (#823)

docs/examples/bearing_only_example.py

@@ -0,0 +1,247 @@
+"""
+Bearings-only tracking example
+==============================
+
+Non-linear bearing-only target tracking is a complex problem for estimating a
+target's state from bearing measurements from a sensor.
+From bearing-only measurements we can estimate the parameters of the target
+motion (range and course). This is a non-linear problem caused by the non-linearity
+between the measurements and the target state vector.
+
+In this short tutorial we show how we can run a bearing-only simulation inside the Stone Soup framework.
+
+In this tutorial, we simulate a radar placed on top of a moving platform collecting measurements,
+then using the :class:`~.ExtendedKalmanFilter` we track the target. In this example we employ a
+distance-based data associator to merge the hypothesis and the detections from the sensor.
+"""
+
+# %%%
+# Layout
+# ------
+# The layout of this example follows:
+#
+# 1) Create the moving platform and the :class:`~.RadarBearing` detector;
+# 2) Generate the target ground truth paths;
+# 3) Set up the simulation for generating detections from the ground truth paths;
+# 4) Run the simulation and create the plots
+
+# some general imports
+import numpy as np
+from matplotlib import pyplot as plt
+
+from datetime import datetime
+from datetime import timedelta
+
+# Load Stone Soup materials
+from stonesoup.types.state import State, GaussianState
+from stonesoup.types.array import StateVector, CovarianceMatrix
+from stonesoup.models.transition.linear import (CombinedLinearGaussianTransitionModel, ConstantVelocity)
+from stonesoup.models.measurement.nonlinear import Cartesian2DToBearing
+
+# Load the filter components
+from stonesoup.updater.kalman import ExtendedKalmanUpdater
+from stonesoup.predictor.kalman import ExtendedKalmanPredictor
+from stonesoup.deleter.time import UpdateTimeStepsDeleter
+from stonesoup.tracker.simple import SingleTargetTracker
+
+# set a random seed and start of the simulation
+np.random.seed(2001)
+start_time = datetime.now()
+
+# %%
+# 1) Create the moving platform and the Bearing-Only radar
+# --------------------------------------------------------
+# Firstly, we create the initial state of the platform, including the origin point and the
+# cartesian (x, y) movement direction. Then, we create a transition model (in 2D cartesian coordinates)
+# of the platform.
+# At this point, we can setup the Radar which receives only the bearing measurements from the targets using the
+# :class:`~.RadarBearing` sensor.
+
+# Import the platform to place the sensor
+from stonesoup.platform.base import MovingPlatform
+
+# Define the platform location, place it in the origin, and define its Cartesian movements.
+# In addition specify the position and velocity mapping. This is done in 2D Cartesian coordinates.
+
+platform_state_vector = StateVector([[0], [-5], [0], [-7]])
+position_mapping = (0, 2)
+velocity_mapping = (1, 3)
+
+# Create the initial state (position and time)
+platform_state = State(platform_state_vector, start_time)
+
+# Create a platform transition model, let's assume it is moving with constant velocity
+platform_transition_model = CombinedLinearGaussianTransitionModel([
+    ConstantVelocity(0.0), ConstantVelocity(0.0)])
+
+# We can instantiate the platform's initial state, position and velocity mapping, and 
+# the transition model using the  :class:`~.MovingPlatform` platform class.
+platform = MovingPlatform(states=platform_state,
+                          position_mapping=position_mapping,
+                          velocity_mapping=velocity_mapping,
+                          transition_model=platform_transition_model)
+
+# At this stage, we need to create the sensor, let's import the RadarBearing. 
+# This sensor only provides the bearing measurements from the target detections, 
+# the range is not specified.
+from stonesoup.sensor.radar.radar import RadarBearing
+
+# Configure the radar noise, since we are using just a single dimension we need to specify only the
+# noise associated with the bearing dimension, we assume a bearing accuracy of +/- 0.025 degrees for 
+# each measurement
+noise_covar = CovarianceMatrix(np.array(np.diag([np.deg2rad(0.025) ** 2])))
+
+# This radar needs to be informed of the x and y mapping of the target space.
+radar_mapping = (0, 2)
+
+# Instantiate the radar
+radar = RadarBearing(ndim_state=4,
+                     position_mapping=radar_mapping,
+                     noise_covar=noise_covar)
+
+# As presented in the other examples we have to place the sensor on the platform.
+platform.add_sensor(radar)
+# At this point we can also check the offset rotation or the mounting of the radar in respect to the
+# platform as shown in other tutorials.
+
+# %%
+# 2) Generate the ground truth target movements
+# --------------------------------------------------
+# We now build a ground truth simulator of a single target with a transition model
+# and a known initial state.
+
+# Load the single target ground truth simulator
+from stonesoup.simulator.simple import SingleTargetGroundTruthSimulator
+
+# Instantiate the transition model
+transition_model = CombinedLinearGaussianTransitionModel([
+    ConstantVelocity(1.0), ConstantVelocity(1.0)])
+
+# Define the initial target state
+# We use a Gaussian state to specify the initial
+# 2D Cartesian position and velocity, and the accuracy
+# using a covariance matrix.
+initial_target_state = GaussianState([50, 0, 50, 0],
+                                     np.diag([1, 1, 1, 1]) ** 2,
+                                     timestamp=start_time)
+
+# Set up the ground truth simulation
+groundtruth_simulation = SingleTargetGroundTruthSimulator(
+    transition_model=transition_model,
+    initial_state=initial_target_state,
+    timestep=timedelta(seconds=1),
+    number_steps=100
+)
+
+# %%
+# 3) Set up the detection simulation that generates the bearing measurements
+# --------------------------------------------------------------------------
+# After defining the measurement model and simulation, we will use these components to run our example.
+# The measurement model is the :class:`~.Cartesian2DToBearing`.
+
+# Define the measurement model using a Cartesian to bearing
+meas_model = Cartesian2DToBearing(
+    ndim_state=4,
+    mapping=(0, 2),
+    noise_covar=noise_covar)
+
+# Import the PlatformDetectionSimulator
+from stonesoup.simulator.platform import PlatformDetectionSimulator
+
+sim = PlatformDetectionSimulator(groundtruth=groundtruth_simulation,
+                                 platforms=[platform])
+
+# %%
+# 4) Set up the tracker
+# ---------------------
+# Instantiate the filter components
+# Create an Extended Kalman Predictor
+predictor = ExtendedKalmanPredictor(transition_model)
+
+# Create an Extended Kalman Updater
+updater = ExtendedKalmanUpdater(measurement_model=None)
+
+
+# %%
+# Given the complexity of the bearing-only tracking, let's feed the
+# same initial state to both the ground truth measurements and tracker
+# as Stone Soup, currently, does not have a bearing only initiator.
+
+# Instantiate the single point initiator
+from stonesoup.initiator.simple import SinglePointInitiator
+initiator = SinglePointInitiator(
+    prior_state=initial_target_state,
+    measurement_model=meas_model)
+
+# %%
+# Add the hypothesiser components. We use a distance based hypothesiser using a Malahonobis
+# distance to do the data association between the detections and the tracks.
+# Since we consider a single target case a simple nearest neighbour will be enough for the data associator.
+
+# Load the hypothesiser and data associator
+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 NearestNeighbour
+
+data_associator = NearestNeighbour(hypothesiser)
+
+# Instantiate the time based deleter
+deleter = UpdateTimeStepsDeleter(time_steps_since_update=3)
+
+# Build the Kalman tracker
+kalman_tracker = SingleTargetTracker(
+    initiator=initiator,
+    deleter=deleter,
+    detector=sim,
+    data_associator=data_associator,
+    updater=updater
+)
+
+# %%
+# 5) Run the simulation and create the plots
+# ------------------------------------------
+# We have everything for running the simulation, we have the tracker, the sensor
+# detections and platform movements.
+
+kalman_tracks = {}  # Store for plotting later
+groundtruth_paths = {}  # Store for plotting later
+
+# Loop for the tracks and the ground truths
+for time, ctracks in kalman_tracker:
+    for track in ctracks:
+        loc = (track.state_vector[0], track.state_vector[2])
+        if track not in kalman_tracks:
+            kalman_tracks[track] = []
+        kalman_tracks[track].append(loc)
+
+    for truth in groundtruth_simulation.current[1]:
+        loc = (truth.state_vector[0], truth.state_vector[2])
+        if truth not in groundtruth_paths:
+            groundtruth_paths[truth] = []
+        groundtruth_paths[truth].append(loc)
+
+from stonesoup.plotter import AnimatedPlotterly, AnimationPlotter
+
+plotter = AnimationPlotter(legend_kwargs=dict(loc='upper left'))
+plotter.plot_ground_truths(groundtruth_paths, (0,2))
+plotter.plot_tracks(kalman_tracks, (0,2))
+plotter.plot_ground_truths(platform, (0,2), truths_label="Sensor Platform")
+plotter.run()
+
+# %%
+# This concludes this short tutorial on how to setup and run a simple single target
+# simulation using Bearings-Only measurements obtained by a moving sensor using an Extended Kalman Filter.
+
+# %%
+# References
+# ----------
+# [1] Xiangdong Lin, Thiagalingam Kirubarajan, Yaakov Bar-Shalom, Simon Maskell,
+# "Comparison of EKF, pseudomeasurement, and particle filters for a bearing-only target tracking problem", in
+# Signal and Data Processing of Small Targets 2002, 2002, vol 4728, pp. 240-250. doi:10.1117/12.478508
+# [2] Vincent J. Aidala, "Kalman Filter Behavior in Bearing-Only Tracking Applications", IEEE Transactions
+# on Aerospace Electronic Systems, Vol. 15, January 1979