#!/usr/bin/env python """ Sensor Platform Simulation ========================== This example looks at how platforms and sensors can be used within the Stone Soup simulation capability. """ # %% # Building a Simulated Sensor Platform # ------------------------------------ # The focus of this example is to show how to set up and configure simulations. As such, the # application of a tracker will not be covered in detail. For more information about trackers # and how to configure them, review of the tutorials and demonstrations is recommended. # # This example makes use of Stone Soup :class:`~.FixedPlatform` and :class:`~.Sensor` objects. # # In order to configure platforms, sensors, and the simulation, we will need to import some specific # Stone Soup objects. As these have been introduced in previous tutorials, they are imported # upfront. New functionality within this example will be imported at the relevant point # to draw attention to the new features. # Some general imports and set up from datetime import datetime from datetime import timedelta import numpy as np # Stone Soup imports: 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 CartesianToElevationBearingRange from stonesoup.updater.kalman import UnscentedKalmanUpdater from stonesoup.predictor.kalman import UnscentedKalmanPredictor from stonesoup.deleter.time import UpdateTimeStepsDeleter from stonesoup.tracker.simple import MultiTargetTracker from matplotlib import pyplot as plt # Define the simulation start time start_time = datetime.now().replace(microsecond=0) # %% # Create a Platform # ----------------- # The first element we need to create is a platform. For this first example, we will build a static # (or *fixed*) platform which is located at the origin. We are going to work in a 6-dimensional # state space, and our platform will have the following :math:`\mathbf{x}`. # # .. math:: # \mathbf{x} = \begin{bmatrix} # x\\ \dot{x}\\ y\\ \dot{y}\\ z\\ \dot{z} \end{bmatrix} # = \begin{bmatrix} # 0\\ 0\\ 0\\ 0\\ 0\\ 0 \end{bmatrix} # # Because the platform is static, we only need to define :math:`(x, y, z)`. Any internal # interactions within the platform which requires knowledge of platform velocity # :math:`(\dot{x}, \dot{y}, \dot{z})` will be returned :math:`(0, 0, 0)`. # Firstly, import the fixed platform from stonesoup.platform.base import FixedPlatform # Define the initial platform position - in this case the origin platform_state_vector = StateVector([[0], [0], [0]]) position_mapping = (0, 1, 2) # Create the initial state (position, time) platform_state = State(platform_state_vector, start_time) # create our fixed platform platform = FixedPlatform(states=platform_state, position_mapping=position_mapping) # %% # We have now created a platform within Stone Soup and located it at the origin of our state space. # As previously stated, the platform will have a velocity :math:`(\dot{x}, \dot{y}, \dot{z})` of # :math:`(0, 0, 0)` which we can check: platform.velocity # %% # We can also query the platform orientation: platform.orientation # %% # Create a Sensor # --------------- # Now that we have a platform, the next step is to create a sensor which can be added to it. # A radar will be created which is capable of measuring the range, bearing, and elevation of the # target relative to the sensor. # # The :class:`~.RadarRangeBearingElevation` class provides a sensor wrapper around the # :class:`~.CartesianToElevationBearingRange` measurement model. The measurement model provides a # time-invariant measurement model where measurements are assumed to be received in the form of # elevation (:math:`\theta`), bearing (:math:`\phi`), and range (:math:`r`) with Gaussian noise in # each dimension. # # The model is described by the following equation: # # .. math:: # \mathbf{z}_k = h(\mathbf{x}_k, \dot{\mathbf{x}}_k) # # where :math:`\mathbf{z}_k` is a measurement vector of the form: # # .. math:: # \mathbf{z}_k = \begin{bmatrix} \theta \\ \phi \\ r \end{bmatrix} # # and :math:`h` is a non-linear model function of the form: # # .. math:: # h(\mathbf{x}_k,\dot{\mathbf{x}}_k) = \begin{bmatrix} # \arcsin{(\mathcal{z}/\sqrt{\mathcal{x}^2 + \mathcal{y}^2 +\mathcal{z}^2})} \\ # \arctan{(\mathcal{y},\mathcal{x})} \\ # \sqrt{\mathcal{x}^2 + \mathcal{y}^2 + \mathcal{z}^2} # \end{bmatrix} + \dot{\mathbf{x}}_k # # and :math:`\mathbf{z}_k` is Gaussian distributed with covariance :math:`R`: # # .. math:: # \mathbf{z}_k \sim \mathcal{N}(0,R) # # .. math:: # R = \begin{bmatrix} # \sigma_{\theta}^2 & 0 & 0 \\ # 0 & \sigma_{\phi}^2 & 0 \\ # 0 & 0 & \sigma_{r}^2 # \end{bmatrix} # # We now create our radar: # Import a radar sensor model from stonesoup.sensor.radar.radar import RadarElevationBearingRange # First we need to configure a radar # Generate a radar sensor with a suitable measurement accuracy noise_covar = CovarianceMatrix(np.array(np.diag([np.deg2rad(3)**2, np.deg2rad(0.15)**2, 25**2]))) # this radar measures range with an accuracy of +/- 25m, elevation accuracy +/- 3 # degrees, and bearing accuracy of +/- 0.15 degrees # The radar needs to be informed of where x, y, and z are in the target state space radar_mapping = (0, 2, 4) # Instantiate the radar radar = RadarElevationBearingRange(ndim_state=6, position_mapping=radar_mapping, noise_covar=noise_covar) # %% # Attach the sensor to the platform # --------------------------------- # Now that we have created our radar sensor, we need to mount the sensor onto our platform. # # Sensors can be mounted with two additional parameters: mounting offset and rotation offset. # # The mounting offset: # # * defines how the sensors position is offset from the platform # * defaults to a position offset of zero. # # The rotation offset: # # * defines the sensor's orientation relative to that of the platform # * defaults to an orientation offset of zero. # # The default assumption is that the sensor is located at the centre point of the platform and # orientated to align with the platform body. Here, we are happy to use the default assumptions # and therefore the sensor can be added. platform.add_sensor(radar) # %% # As before, we can query the platform to demonstrate that it has a sensor mounted: platform.sensors # %% # You will notice that ``platform.sensors`` returns a list which contains our single sensor. # This hints at the multi-sensor platform functionality which is shown in a subsequent example. # # We can also check that the default mounting offsets have been applied: radar.mounting_offset # %% # and that the rotation offsets have been applied: radar.rotation_offset # %% # Building a simulation # --------------------- # Now that we have created a sensor platform, we need to build a simulation which generates targets # for the sensor to detect and track. We are going to use a # :class:`~.MultiTargetGroundTruthSimulator`. This simulator enables multiple ground truth targets # to be created based on a number of user-defined parameters. # # In this example, targets are initiated with values based upon a mean state and a covariance, # using a Gaussian assumption. This is done by creating a :class:`~.GaussianState` object which # describes the distribution which we want our targets to be drawn from. Targets will be generated # using the following parameters: # # * :math:`x` is Gaussian-distributed around the platform location with variance of # :math:`\mathrm{2}km`. # * :math:`y` is Gaussian-distributed around the platform location with variance of # :math:`\mathrm{2}km`. # * :math:`z` is Gaussian-distributed around an altitude of :math:`\mathrm{9}km` with variance # of :math:`\mathrm{0.1}km`. # * :math:`\dot{x}` is Gaussian-distributed around :math:`\mathrm{100}ms^{-1}` with variance of # :math:`\mathrm{50}ms^{-1}`. # * :math:`\dot{y}` is Gaussian-distributed around :math:`\mathrm{100}ms^{-1}` with variance # of :math:`\mathrm{50}ms^{-1}`. # * :math:`\dot{z}` is Gaussian-distributed around :math:`\mathrm{0}ms^{-1}` with variance of # :math:`\mathrm{1}ms^{-1}`. # # We will also configure our simulator to randomly create and delete targets based on a specified # birth rate and death rate. We set the birth rate to be 0.10 - on any given time step there is # a 10% chance of a new target being initiated. The death rate has been set to 0.01 - on any given # time step, there is a 1% chance that a target will be removed from the simulation. # # The above setup will provide a case which loosely approximates an air surveillance radar at an # airport. from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator # Set a constant velocity transition model for the targets transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.5), ConstantVelocity(0.5), ConstantVelocity(0.1)]) # Define the Gaussian State from which new targets are sampled on initialisation initial_target_state = GaussianState(StateVector([[0], [0], [0], [0], [9000], [0]]), CovarianceMatrix(np.diag([2000, 50, 2000, 50, 100, 1]))) groundtruth_sim = MultiTargetGroundTruthSimulator( transition_model=transition_model, # target transition model initial_state=initial_target_state, # add our initial state for targets timestep=timedelta(seconds=1), # time between measurements number_steps=120, # 2 minute birth_rate=0.10, # 10% chance of a new target being born death_probability=0.01 # 1% chance of a target being killed ) # %% # With our ground truth simulation now set up, we need to add our platform into the simulation # capability. This is done using the :class:`~.PlatformDetectionSimulator` class. This simulator # allows a *list* of platforms to be added into the simulation. When the simulation is processed, # the platforms are able to make detections of both the ground truth targets and other platforms. # # In this case we have a single platform. Therefore, the radar sensor will only be able to make # measurements of the ground truth objects generated by the simulator. # Import the PlatformDetectionSimulator from stonesoup.simulator.platform import PlatformDetectionSimulator sim = PlatformDetectionSimulator(groundtruth=groundtruth_sim, platforms=[platform]) # %% # Creating the Tracker Components # ------------------------------- # As stated above, the aim of this example is to show how the :class:`~.Platform`, # :class:`~.Sensor`, and :class:`~.Simulator` classes work within Stone Soup. We will therefore # quickly build an Unscented Kalman Filter which initiates measurements using a simple heuristic # initiation and deletes any track where no detection is associated for 2 consecutive time steps. # There are a number of tutorials for how to build the tracking components provided in the # :ref:`auto_tutorials/index:Tutorials`. # Create an Unscented Kalman Predictor predictor = UnscentedKalmanPredictor(transition_model) # Create an Unscented Kalman Updater # Note our sensor adds a measurement model to detections updater = UnscentedKalmanUpdater(measurement_model=None) # %% # When we build our updater, we do not provide a measurement model. This is because we have defined # a measurement model which is attached to our radar sensor. Each detection made by this sensor # will have our radar measurement model associated with it. In Stone Soup, the # :class:`~.Updater` checks the detections provided and will use any measurement model attached # to them. # %% # Setup Initiator class for the Tracker # ------------------------------------- # We will now build a simple heuristic initiator. This assumes that most of the deviation is caused # by the bearing measurement error. It converts the bearing error into :math:`x, y` components # using the target bearing. For z, we simply use :math:`r*\sigma_{\theta}^2` (this ignores # any bearing or range related components). Velocity covariances are just based on expected velocity # range of targets. from stonesoup.types.state import GaussianState from stonesoup.types.update import GaussianStateUpdate from stonesoup.initiator.simple import SimpleMeasurementInitiator from stonesoup.types.track import Track from stonesoup.types.hypothesis import SingleHypothesis class Initiator(SimpleMeasurementInitiator): def initiate(self, detections, timestamp, **kwargs): max_dev = 500. tracks = set() measurement_model = self.measurement_model for detection in detections: state_vector = measurement_model.inverse_function( detection) model_covar = measurement_model.covar() el_az_range = np.sqrt(np.diag(model_covar)) # elev, az, range std_pos = detection.state_vector[2, 0]*el_az_range[1] stdx = np.abs(std_pos*np.sin(el_az_range[1])) stdy = np.abs(std_pos*np.cos(el_az_range[1])) stdz = np.abs(detection.state_vector[2, 0]*el_az_range[0]) if stdx > max_dev: print('Warning - X Deviation exceeds limit!!') if stdy > max_dev: print('Warning - Y Deviation exceeds limit!!') if stdz > max_dev: print('Warning - Z Deviation exceeds limit!!') c0 = np.diag(np.array([stdx, 50.0, stdy, 50.0, stdz, 10.0])**2) tracks.add(Track([GaussianStateUpdate( state_vector, c0, SingleHypothesis(None, detection), timestamp=detection.timestamp) ])) return tracks meas_model = CartesianToElevationBearingRange( ndim_state=6, mapping=np.array([0, 2, 4]), noise_covar=noise_covar) prior_state = GaussianState( np.array([[0], [0], [0], [0], [0], [0]]), np.diag([1000, 50, 1000, 50, 1000, 10.0])**2) initiator = Initiator(prior_state, measurement_model=meas_model) # %% # Now that we have set up out tracking scenario, we can wrap our simulation environment within a # :class:`~.MultiTargetTracker`. This combines our tracker configurations with the simulation we # previously created into a single iterable object. from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.measures import Mahalanobis hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=3) from stonesoup.dataassociator.neighbour import NearestNeighbour data_associator = NearestNeighbour(hypothesiser) deleter = UpdateTimeStepsDeleter(time_steps_since_update=2) # Create a Kalman multi-target tracker kalman_tracker = MultiTargetTracker( initiator=initiator, deleter=deleter, detector=sim, data_associator=data_associator, updater=updater ) # %% # The final step is to iterate our tracker over the simulation: kalman_tracks = {} # Store for plotting later groundtruth_paths = {} # Store for plotting later detections = [] # Store for plotting later 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_sim.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) for detection in sim.detections: detect_state = detection.measurement_model.inverse_function(detection) loc = (detect_state[0], detect_state[2]) detections.append(loc) # %% # Plotting the outputs # -------------------- # First we will plot the ground truth paths (red) which have been generated in the simulation step: fig = plt.figure(figsize=(10, 6)) ax = fig.add_subplot(1, 1, 1) ax.set_xlabel("$East$") ax.set_ylabel("$North$") ax.set_ylim(-10000, 10000) ax.set_xlim(-10000, 10000) for key in groundtruth_paths: X = [coord[0] for coord in groundtruth_paths[key]] Y = [coord[1] for coord in groundtruth_paths[key]] ax.plot(X, Y, color='r') # Plot true locations in red # plot platform location ax.scatter(0, 0, color='y') # %% # If we overlay the detections (black) onto the ground truth paths (red), we can see how the # sensor performs, generating detections based upon the :class:`~.MeasurementModel` we provided it # with. The platform location is shown in yellow: fig = plt.figure(figsize=(10, 6)) ax = fig.add_subplot(1, 1, 1) ax.set_xlabel("$East$") ax.set_ylabel("$North$") ax.set_ylim(-10000, 10000) ax.set_xlim(-10000, 10000) for key in groundtruth_paths: X = [coord[0] for coord in groundtruth_paths[key]] Y = [coord[1] for coord in groundtruth_paths[key]] ax.plot(X, Y, color='r') # plot true locations in red X = [coord[0] for coord in detections] Y = [coord[1] for coord in detections] ax.scatter(X, Y, color='k') # plot detections in black # plot platform location ax.scatter(0, 0, color='y') # %% # Now, we overlay the ground truth locations (red), detections (black) and tracks (blue). This # shows all the stages of the tracker simulation we have built in a single figure. # The platform location is shown in yellow: fig = plt.figure(figsize=(10, 6)) ax = fig.add_subplot(1, 1, 1) ax.set_xlabel("$East$") ax.set_ylabel("$North$") ax.set_ylim(-10000, 10000) ax.set_xlim(-10000, 10000) for key in groundtruth_paths: X = [coord[0] for coord in groundtruth_paths[key]] Y = [coord[1] for coord in groundtruth_paths[key]] ax.plot(X, Y, color='r') # Plot true locations in red for key in kalman_tracks: X = [coord[0] for coord in kalman_tracks[key]] Y = [coord[1] for coord in kalman_tracks[key]] ax.plot(X, Y, color='b') # Plot track estimates in blue X = [coord[0] for coord in detections] Y = [coord[1] for coord in detections] ax.scatter(X, Y, color='k') # Plot detections in black # plot platform location ax.scatter(0, 0, color='y') # %% # Finally, we can plot the estimated tracks (blue) alongside the ground truth paths (red). # Because we used a noisy sensor, this view makes it easier to quickly see the tracker performance. # The platform location is shown in yellow. fig = plt.figure(figsize=(10, 6)) ax = fig.add_subplot(1, 1, 1) ax.set_xlabel("$East$") ax.set_ylabel("$North$") ax.set_ylim(-10000, 10000) ax.set_xlim(-10000, 10000) for key in groundtruth_paths: X = [coord[0] for coord in groundtruth_paths[key]] Y = [coord[1] for coord in groundtruth_paths[key]] ax.plot(X, Y, color='r') # Plot true locations in red for key in kalman_tracks: X = [coord[0] for coord in kalman_tracks[key]] Y = [coord[1] for coord in kalman_tracks[key]] ax.plot(X, Y, color='b') # Plot track estimates in blue # plot platform location ax.scatter(0, 0, color='y') # sphinx_gallery_thumbnail_number = 3 # %% # To familiarise yourself with sensors, it is recommended that you investigate changing the # parameters within the sensor Measurement Model to see the impact on detections and ultimately # tracker performance. We used a *hard* association logic coupled with a relatively noisy sensor. # A suggested further exercise is to modify this example to use a *soft* association step such # as Probabilistic Data Association (:class:`~.PDA`) or Joint Probabilistic Data Association # (:class:`~.JPDA`). # %% # Key points # ---------- # 1. Sensor platforms, which combine :class:`~.Sensor` and :class:`~.Platform` classes can be # created in Stone Soup and used as part of a tracking simulation. # 2. When using a :class:`~.Sensor` to generate detections, there is no need to provide an # :class:`~.Updater` with a :class:`~.MeasurementModel` as each detection is attributed with the # relevant sensors measurement model. # 3. Sensors will generate detections of all platforms within the simulation, not just ground # truth objects.