Expected Likelihood Particle Filters

Target tracking in cluttered environments is always a complex problem, in particular when dealing with non-linear or non-Gaussian trajectories.

This example shows the implementation of the expected likelihood particle tracker (ELPF) for a multi target case in a cluttered scenario (see [1] for details). This particle filter method is based on the expected likelihood obtained from the mixture model of the target measurement noise and clutter statistics, and it has proven to be lighter in computational needs and with higher accuracy in cluttered scenarios.

This example uses the probabilistic data association (J)PDA to assign detections to tracks (it is also possible to use Efficient Hypothesis Management (EHM) via the Stone Soup plugin PyEHM (PyEHM)). This implementation is not limited to a probabilistic data associator, indeed it is possible to use a distance based data associator such as the (G)NN (nearest neighbour). In comparison we consider a distance based particle filter implementation to understand the benefit of this approach.

The layout of this example follows:

1. The ground truths are created using the transition model;

2. Generate scans containing detections and clutter;

3. Setup the particle filter, data associators and tracker components;

4. Final tracks results are plotted.

General imports

import numpy as np
from datetime import datetime, timedelta
from scipy.stats import uniform

# Simulation parameters
np.random.seed(1908)  # Set a random seed
simulation_start = datetime.now().replace(microsecond=0)
prob_detect = 0.95  # detection probability
simulation_timesteps = 100

# Clutter parameters
clutter_rate = 1
clutter_area = np.array([[-1, 1], [-1, 1]]) * 50
surveillance_area = ((clutter_area[0][1] - clutter_area[0][0]) *
                     (clutter_area[1][1] - clutter_area[1][0]))
clutter_spatial_density = clutter_rate/surveillance_area

Stone Soup imports

from stonesoup.models.transition.linear import \
    CombinedLinearGaussianTransitionModel, ConstantVelocity
from stonesoup.models.measurement.linear import LinearGaussian
from stonesoup.types.state import GaussianState
from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState
from stonesoup.types.detection import TrueDetection
from stonesoup.types.detection import Clutter

1. Create ground truth

Firstly we initialise the transition and measurement models

# Transition model for the particle filer
transition_model = CombinedLinearGaussianTransitionModel(
    [ConstantVelocity(0.1), ConstantVelocity(0.1)])

# A noiseless transition model for the ground truths
gnd_transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.),
                                                              ConstantVelocity(0.)])

# Define a detection models
measurement_model = LinearGaussian(ndim_state=4,
                                   mapping=(0, 2),
                                   noise_covar=np.diag([2, 2]))

# Generate ground truths
truths = set()

# Instantiate the first entry
truth = GroundTruthPath([GroundTruthState([-10, 0.25, -10, 0.50], timestamp=simulation_start)])
for k in range(1, simulation_timesteps):
    truth.append(GroundTruthState(
        gnd_transition_model.function(truth[k-1], noise=True,
                                      time_interval=timedelta(seconds=1)),
        timestamp=simulation_start + timedelta(seconds=k)))
truths.add(truth)

truth = GroundTruthPath([GroundTruthState([-10, 0.25, 20, -0.50], timestamp=simulation_start)])
for k in range(1, simulation_timesteps):
    truth.append(GroundTruthState(
        gnd_transition_model.function(truth[k-1], noise=True,
                                      time_interval=timedelta(seconds=1)),
        timestamp=simulation_start + timedelta(seconds=k)))
truths.add(truth)

# Generate the timestamps
timestamps = [simulation_start]
for k in range(1, simulation_timesteps):
    timestamps.append(simulation_start + timedelta(seconds=k))

2. Generate scans containing detections and clutter

Create a series of scans to collect the detections and clutter from the targets.

scans = []
for k in range(simulation_timesteps):
    measurement_set = set()

    # detections
    for truth in truths:
        if np.random.rand() <= prob_detect:
            measurement = measurement_model.function(truth[k], noise=True)
            measurement_set.add(TrueDetection(state_vector=measurement,
                                              groundtruth_path=truth,
                                              timestamp=truth[k].timestamp,
                                              measurement_model=measurement_model))

    # Clutter detections
    for _ in range(np.random.poisson(clutter_rate)):
        x = uniform.rvs(-20, 40)
        y = uniform.rvs(-30, 70)
        measurement_set.add(Clutter(np.array([[x], [y]]), timestamp=truth[k].timestamp,
                                    measurement_model=measurement_model))
    scans.append(measurement_set)

Visualise the detections and the ground truth

from stonesoup.plotter import AnimatedPlotterly

plotter = AnimatedPlotterly(timesteps=timestamps)
plotter.plot_measurements(scans, [0, 2], measurements_label='Scans')
plotter.plot_ground_truths(truths, [0, 2])
plotter.fig

3. Set up the Particle filter

We have a series of scans containing the detections and clutter, now we can load the tracker components starting from ParticlePredictor, ParticleUpdater and ESSResampler. In the ELPF implementation, the resampler should not be passed to the updater, as usual, but needs to be loaded in the tracker object later otherwise it will result in an error.

In addition to the ELPF implementation we present a standard Particle filter implementation with a distance based data associator for visual comparison of track accuracy.

# Load the predictor
from stonesoup.predictor.particle import ParticlePredictor
predictor = ParticlePredictor(transition_model)

# Load the resampler
from stonesoup.resampler.particle import ESSResampler
resampler = ESSResampler()

# Load the updater
from stonesoup.updater.particle import ParticleUpdater
updater = ParticleUpdater(measurement_model)

# In the standard particle filter we pass the resampler to the updater
pf_updater = ParticleUpdater(measurement_model, resampler)

After having initialised the predictor, updater and resampler we consider the data associator. This example uses a joint probabilistic data association (JPDA, or PDA in the single target case) for the ELPF. The ELPF can also use a distance-based data associator NearestNeighbour (GNNWith2DAssignment in the multi-target case) using the distances of the points from the tracks.

# load the hypothesiser
from stonesoup.hypothesiser.probability import PDAHypothesiser
hypothesiser = PDAHypothesiser(predictor=predictor,
                               updater=updater,
                               clutter_spatial_density=clutter_spatial_density,
                               prob_detect=prob_detect,
                               prob_gate=0.9999)

from stonesoup.dataassociator.probability import JPDA
data_associator = JPDA(hypothesiser)

With the data associator we need to initialise the tracks by using an initiator and a deleter to pair the detections with the tracks. We consider a time based deleter using UpdateTimeStepsDeleter. We adopt MultiMeasurementInitiator that behaves as an initial tracker, using a distance based data associator and Extended Kalman predictor and updater.

from stonesoup.deleter.time import UpdateTimeStepsDeleter
deleter = UpdateTimeStepsDeleter(3)

from stonesoup.initiator.simple import GaussianParticleInitiator, MultiMeasurementInitiator
from stonesoup.dataassociator.neighbour import GNNWith2DAssignment
from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.measures import Mahalanobis

from stonesoup.updater.kalman import ExtendedKalmanUpdater
initiator_updater = ExtendedKalmanUpdater(measurement_model)
from stonesoup.predictor.kalman import ExtendedKalmanPredictor
initiator_predictor = ExtendedKalmanPredictor(gnd_transition_model)

# Prior state
prior_state = GaussianState(np.array([-10, 0, 0, 0]),
                            np.diag([5, 1, 8, 1])**2,
                            timestamp=simulation_start)

initiator_part = MultiMeasurementInitiator(
    prior_state,
    measurement_model=None,
    deleter=deleter,
    data_associator=GNNWith2DAssignment(
        DistanceHypothesiser(initiator_predictor,
                             initiator_updater,
                             Mahalanobis(),
                             missed_distance=3)),
    updater=initiator_updater,
    min_points=8
    )

initiator = GaussianParticleInitiator(initiator=initiator_part,
                                      number_particles=500)

4. Run the tracker

We have initialised all the relevant components needed for the tracker, we can now pass these all to the MultiTargetExpectedLikelihoodParticleFilter tracker and generate the various tracks.

The particle filter uses a MultiTargetTracker to perform the tracking and a GNNWith2DAssignment data associator and adopts the same initiator and deleter as the ELPF tracker.

# Load the ELPF tracker
from stonesoup.tracker.particle import MultiTargetExpectedLikelihoodParticleFilter
# Load the PF tracker
from stonesoup.tracker.simple import MultiTargetTracker

# ELPF Tracker
tracker = MultiTargetExpectedLikelihoodParticleFilter(
    initiator=initiator,
    deleter=deleter,
    detector=zip(timestamps, scans),
    data_associator=data_associator,
    updater=updater,
    resampler=resampler)  # if the resampler is not specified, ~SystematicResampler is used

# PF tracker
pf_tracker = MultiTargetTracker(
    initiator=GaussianParticleInitiator(initiator=initiator_part,
                                        number_particles=500),
    deleter=deleter,
    detector=zip(timestamps, scans),
    data_associator=GNNWith2DAssignment(
        DistanceHypothesiser(predictor, pf_updater, Mahalanobis(), missed_distance=5)),
    updater=pf_updater)

# Loop over the tracker generate the final tracks
tracks = set()
for time, current_tracks in tracker:
    tracks.update(current_tracks)

# Gather the tracks from the PF
pf_tracks = set()
for time, current_tracks in pf_tracker:
    pf_tracks.update(current_tracks)

# Visualise the final tracks obtained
plotter.plot_tracks(tracks, [0, 2], track_label="ELPF tracks")
plotter.plot_tracks(pf_tracks, [0, 2], track_label="PF tracks", marker=dict(symbol="triangle-up"))
plotter.fig

Conclusion

In this example we have presented how to implement and use the Expected Likelihood Particle filter in a cluttered multi-target scenario.

ELPF calculates the expected likelihood from the measurements and clutter statistics. This enables greater accuracy compared with standard probabilistic data association when dealing with tracking in cluttered environments with non-linear detections.

References

[1]

Marrs, A., Maskell, S., and Bar-Shalom, Y., “Expected likelihood for tracking in clutter with particle filters”, in Signal and Data Processing of Small Targets 2002, 2002, vol. 4728, pp. 230–239. doi:10.1117/12.478507