#!/usr/bin/env python """ Multi-Sensor Moving Platform Simulation Example =============================================== This example looks at how multiple sensors can be mounted on a single moving platform and exploiting a defined moving platform as a sensor target. """ # %% # Building a Simulated Multi-Sensor Moving Platform # ------------------------------------------------- # This example shows how to set up and configure a simulation environment # to provide a multi-sensor moving platform, 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:`~.MovingPlatform`, # :class:`~.MultiTransitionMovingPlatform` 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 from matplotlib import pyplot as plt import numpy as np # Stone Soup imports: from stonesoup.types.state import State, GaussianState from stonesoup.types.array import StateVector from stonesoup.types.array import CovarianceMatrix from stonesoup.models.transition.linear import ( CombinedLinearGaussianTransitionModel, ConstantVelocity) from stonesoup.predictor.particle import ParticlePredictor from stonesoup.resampler.particle import SystematicResampler from stonesoup.updater.particle import ParticleUpdater from stonesoup.measures import Mahalanobis from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.dataassociator.neighbour import GNNWith2DAssignment from stonesoup.tracker.simple import SingleTargetTracker # Define the simulation start time start_time = datetime.now() # %% # Create a multi-sensor platform # ------------------------------ # We have previously demonstrated how to create a :class:`~.FixedPlatform` which exploited a # :class:`~.RadarRangeBearingElevation` *Sensor* in order to detect and track targets generated # within a :class:`~.MultiTargetGroundTruthSimulator`. # # In this example, we are going to create a moving platform which will be mounted with a pair # of sensors and moves within a 6 dimensional state space according to 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\\ 50\\ 8000\\ 0 \end{bmatrix} # # The platform will be initiated with a near-constant velocity model which has been # parameterised to have zero noise. Therefore, the platform location at time # :math:`k` is given by :math:`F_{k}x_{k-1}` where :math:`F_{k}` is given by: # # .. math:: # F_{k} = \begin{bmatrix} # 1 & \triangle k & 0 & 0 & 0 & 0\\ # 0 & 1 & 0 & 0 & 0 & 0\\ # 0 & 0 & 1 & \triangle k & 0 & 0\\ # 0 & 0 & 0 & 1 & 0 & 0\\ # 0 & 0 & 0 & 0 & 1 & \triangle k \\ # 0 & 0 & 0 & 0 & 0 & 1\\ # \end{bmatrix} # First import the Moving platform from stonesoup.platform.base import MovingPlatform # Define the initial platform position, in this case the origin initial_loc = StateVector([[0], [0], [0], [50], [8000], [0]]) initial_state = State(initial_loc, start_time) # Define transition model and position for 3D platform transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.), ConstantVelocity(0.), ConstantVelocity(0.)]) # create our fixed platform sensor_platform = MovingPlatform(states=initial_state, position_mapping=(0, 2, 4), velocity_mapping=(1, 3, 5), transition_model=transition_model) # %% # With our platform generated, we now need to build a set of sensors which will be mounted onto # the platform. In this case, we will exploit a :class:`~.RadarElevationBearingRangeRate` # and a :class:`~.PassiveElevationBearing` sensor (e.g. an optical sensor, which has no capability # to directly measure range). # # First, we will create a radar which is capable of measuring bearing (:math:`\phi`), # elevation (:math:`\theta`), range (:math:`r`), and range-rate (:math:`\dot{r}`) of the target # platform. # Import a range rate bearing elevation capable radar from stonesoup.sensor.radar.radar import RadarElevationBearingRangeRate # Create a radar sensor radar_noise_covar = CovarianceMatrix(np.diag( np.array([np.deg2rad(3), # Elevation np.deg2rad(3), # Bearing 100., # Range 25.]))) # Range Rate # radar mountings radar_mounting_offsets = StateVector([10, 0, 0]) # e.g. nose cone radar_rotation_offsets = StateVector([0, 0, 0]) # Mount the radar onto the platform radar = RadarElevationBearingRangeRate(ndim_state=6, position_mapping=(0, 2, 4), velocity_mapping=(1, 3, 5), noise_covar=radar_noise_covar, mounting_offset=radar_mounting_offsets, rotation_offset=radar_rotation_offsets, ) sensor_platform.add_sensor(radar) # %% # Our second sensor is a passive sensor, capable of measuring the bearing (:math:`\phi`) and # elevation (:math:`\theta`) of the target platform. For the purposes of this example, we will # assume that the passive sensor is an imager. # The imager sensor model is described by the following equations: # # .. 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 \end{bmatrix} # # * :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}) \ \ # \end{bmatrix} + \dot{\mathbf{x}}_k # # * :math:`\mathbf{z}_k` is Gaussian distributed with covariance :math:`R`, i.e.: # # .. math:: # \mathbf{z}_k \sim \mathcal{N}(0, R) # # .. math:: # R = \begin{bmatrix} # \sigma_{\theta}^2 & 0 \\ # 0 & \sigma_{\phi}^2 \\ # \end{bmatrix} # Import a passive sensor capability from stonesoup.sensor.passive import PassiveElevationBearing imager_noise_covar = CovarianceMatrix(np.diag(np.array([np.deg2rad(0.05), # Elevation np.deg2rad(0.05)]))) # Bearing # imager mounting offset imager_mounting_offsets = StateVector([0, 8, -1]) # e.g. wing mounted imaging pod imager_rotation_offsets = StateVector([0, 0, 0]) # Mount the imager onto the platform imager = PassiveElevationBearing(ndim_state=6, mapping=(0, 2, 4), noise_covar=imager_noise_covar, mounting_offset=imager_mounting_offsets, rotation_offset=imager_rotation_offsets, ) sensor_platform.add_sensor(imager) # %% # Notice that we have added sensors to specific locations on the aircraft, defined by the # ``mounting_offset`` parameter. The values in this array are defined in the platform's local # coordinate frame of reference. Here, an offset of :math:`[0, 8, -1]` means the sensor is # located 8 metres to the right and 1 metre below the centre point of the platform. # # Now that we have mounted the two sensors we can see that the platform object has both associated # with it: sensor_platform.sensors # %% # Create a Target Platform # ------------------------ # There are two ways of generating a target in Stone Soup. Firstly, we can use the inbuilt # ground truth generator functionality within Stone Soup, which we demonstrated in the previous # example, which creates a random target based on our selected parameters. The second method # provides a means to generate a target which will perform specific behaviours, this is the # approach we will take here. # # To create a target which moves in pre-defined sequences, we exploit the fact that platforms can # be used as sensor targets within a simulation, coupled with the # :class:`~.MultiTransitionMovingPlatform` which provides a platform with a pre-defined list of # transition models and transition times. The platform will continue to loop over the transition # sequence provided until the simulation ends. # # When simulating sensor platforms, it is important to note that, within the simulation, Stone Soup # treats all platforms as potential targets. Therefore, multiple sensor platforms *sense* all other # platforms within the simulation (sensor-target geometry dependant). # # For this example, we will create an air target which will fly a sequence of straight and level, # followed by a coordinated turn in the :math:`x-y` plane. This is configured such that the target # will perform each manoeuvre for 8 seconds and will turn through 45 degrees over the course of the # turn manoeuvre. # Import a Constant Turn model to enable target to perform basic manoeuvre from stonesoup.models.transition.linear import KnownTurnRate straight_level = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.), ConstantVelocity(0.), ConstantVelocity(0.)]) # Configure the aircraft turn behaviour turn_noise_diff_coeffs = np.array([0., 0.]) turn_rate = np.pi/32 # specified in radians per seconds... turn_model = KnownTurnRate(turn_noise_diff_coeffs=turn_noise_diff_coeffs, turn_rate=turn_rate) # Configure turn model to maintain current altitude turning = CombinedLinearGaussianTransitionModel( [turn_model, ConstantVelocity(0.)]) manoeuvre_list = [straight_level, turning] manoeuvre_times = [timedelta(seconds=8), timedelta(seconds=8)] # %% # Now that we have created a list of manoeuvre behaviours and durations we can build our # multi-transition moving platform. Because we intend for this platform to be a target we do not # need to attach any sensors to it. # Import a multi-transition moving platform from stonesoup.platform.base import MultiTransitionMovingPlatform initial_target_location = StateVector([[0], [-40], [1800], [0], [8000], [0]]) initial_target_state = State(initial_target_location, start_time) target = MultiTransitionMovingPlatform(transition_models=manoeuvre_list, transition_times=manoeuvre_times, states=initial_target_state, position_mapping=(0, 2, 4), velocity_mapping=(1, 3, 5), sensors=None) # %% # Creating the simulator # ---------------------- # Now that we have built our sensor platform and a target platform, we need to wrap them in a # simulator. Because we do not want any additional ground truth objects, which is how most # simulators work in Stone Soup, we need to use a :class:`~.DummyGroundTruthSimulator` which # returns a set of empty ground truth paths with timestamps. These are then feed into a # :class:`~.PlatformDetectionSimulator` with the two platforms we have already built. # Import the required simulators from stonesoup.simulator.simple import DummyGroundTruthSimulator from stonesoup.simulator.platform import PlatformDetectionSimulator # %% # We now need to create an array of timestamps which starts at *datetime.now()* and enable the # simulator to run for 25 seconds. times = np.arange(0, 24, 1) # 25 seconds timestamps = [start_time + timedelta(seconds=float(elapsed_time)) for elapsed_time in times] truths = DummyGroundTruthSimulator(times=timestamps) sim = PlatformDetectionSimulator(groundtruth=truths, platforms=[sensor_platform, target]) # %% # Create a Tracker # ------------------------------------ # Now that we have set up our sensor platform, target, and simulation, we need to create a tracker. # For this example we will use a Particle Filter as this enables us to handle the non-linear # nature of the imaging sensor. We will also use an inflated constant noise model to account for # target motion uncertainty. # # Note that we don't add a measurement model to the updater. This is because each sensor adds their # measurement model to each detection they generate. The tracker handles this internally by # checking for a measurement model with each detection it receives and applying only the relevant # measurement model. target_transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(5), ConstantVelocity(5), ConstantVelocity(1)]) # First add a Particle Predictor predictor = ParticlePredictor(target_transition_model) # Now create a resampler and particle updater resampler = SystematicResampler() updater = ParticleUpdater(measurement_model=None, resampler=resampler) # Create a particle initiator from stonesoup.initiator.simple import GaussianParticleInitiator, SinglePointInitiator single_point_initiator = SinglePointInitiator( GaussianState([[0], [-40], [2000], [0], [8000], [0]], np.diag([10000, 1000, 10000, 1000, 10000, 1000])), None) initiator = GaussianParticleInitiator(number_particles=500, initiator=single_point_initiator) hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=np.inf) data_associator = GNNWith2DAssignment(hypothesiser) from stonesoup.deleter.time import UpdateTimeStepsDeleter deleter = UpdateTimeStepsDeleter(time_steps_since_update=10) # Create a Kalman single-target tracker tracker = SingleTargetTracker( initiator=initiator, deleter=deleter, detector=sim, data_associator=data_associator, updater=updater ) # %% # The final step is to iterate our tracker over the simulation and plot out the results. Because we # have a bearing-only sensor, it makes sense to animate the plotting of detections. This # animation shows the sensor platform (blue) moving towards the true target position (red). # The estimated target position is shown in black, radar detections are shown in yellow, while the # bearing-only imager detections are coloured green. from matplotlib import animation import matplotlib matplotlib.rcParams['animation.html'] = 'jshtml' from stonesoup.models.measurement.nonlinear import CartesianToElevationBearingRangeRate from stonesoup.functions import sphere2cart fig = plt.figure(figsize=(10, 6)) ax = fig.add_subplot(1, 1, 1) frames = [] for time, ctracks in tracker: artists = [] ax.set_xlabel("$East$") ax.set_ylabel("$North$") ax.set_ylim(0, 2250) ax.set_xlim(-1000, 1000) X = [state.state_vector[0] for state in sensor_platform] Y = [state.state_vector[2] for state in sensor_platform] artists.extend(ax.plot(X, Y, color='b')) for detection in sim.detections: if isinstance(detection.measurement_model, CartesianToElevationBearingRangeRate): x, y = detection.measurement_model.inverse_function(detection)[[0, 2]] color = 'y' else: r = 10000000 # extract the platform rotation offsets _, el_offset, az_offset = sensor_platform.orientation # obtain measurement angles and map to cartesian e, a = detection.state_vector x, y, _ = sphere2cart(r, a + az_offset, e + el_offset) x += detection.measurement_model.translation_offset[0] y += detection.measurement_model.translation_offset[1] color = 'g' X = [sensor_platform.state_vector[0], x] Y = [sensor_platform.state_vector[2], y] artists.extend(ax.plot(X, Y, color=color)) X = [state.state_vector[0] for state in target] Y = [state.state_vector[2] for state in target] artists.extend(ax.plot(X, Y, color='r')) for track in ctracks: X = [state.mean[0] for state in track] Y = [state.mean[2] for state in track] artists.extend(ax.plot(X, Y, color='k')) frames.append(artists) animation.ArtistAnimation(fig, frames) # %% # To increase your confidence with simulated platform targets, it would be good practice to modify # the target to fly pre-defined shapes such as a racetrack-style oval. You could also experiment with # different sensor performance levels to see at what point the tracker is no longer able to # generate a reasonable estimate of the target location. # %% # Key points # ---------- # 1. Platforms, static or moving, can be used as targets for sensor platforms. # 2. Simulations can only be built with known platform behaviours when you want to test specific # scenarios. # 3. A tracker can be configured to exploit all sensor data created in a simulation.