From 93d3b5e38394ec606b9896ac8ca0b1d5d9b52f1c Mon Sep 17 00:00:00 2001 From: Alberto Acuto <37350705+A-acuto@users.noreply.github.com> Date: Mon, 6 Nov 2023 15:29:35 +0000 Subject: [PATCH] Example of Bearing-only tracking (#823) --- docs/examples/bearing_only_example.py | 247 ++++++++++++++++++++++++++ 1 file changed, 247 insertions(+) create mode 100644 docs/examples/bearing_only_example.py diff --git a/docs/examples/bearing_only_example.py b/docs/examples/bearing_only_example.py new file mode 100644 index 000000000..42f1eeab5 --- /dev/null +++ b/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