Handle Out-of-Sequence Measurements in Multisensor Tracking Systems

Since R2021b

This example shows how to handle out-of-sequence measurements (OOSM) in a multisensor tracking system. The example compares tracking results when OOSM are present using various handling techniques. For more information about OOSM handling techniques see Handle Out-of-Sequence Measurements with Filter Retrodiction example.

Introduction

In a multisensor system, when multiple sensors report to the same tracker, the measurements may arrive at the tracker with a time delay relative to the time when they are generated by the sensor. The delay can be caused by any of the following reasons:

The sensor may require a significant amount of time to process the data. For example, a vision sensor may require tens of milliseconds to detect objects in a frame it captures.
If the sensor and the tracker are connected by a network, there may be a communication delay.
The tracker may update at a different rate from the sensor scan rate. For example, if the tracker is updated just before the sensor measurements arrive, these measurements are considered as out-of-sequence.

There are various OOSM handling techniques. In this example, you explore two techniques:

Neglect: In this technique, any OOSM is simply ignored and not used to update the tracker, as shown in the figure below. This technique is the easiest and is useful in cases where the OOSM is not expected to contain data that would significantly modify the filter state and uncertainty. It is also the most efficient technique, in terms of memory and processing.

Retrodiction: In this technique, the tracker saves its state for the last n steps. When a new set of detections is sent to the tracker, the tracker:

Uses the detection time to classify the detections as: in-sequence (after the last tracker update time) or out-of-sequence (before the last tracker update time).
The tracker neglects any detections with timestamps that are older than the tracker MaxNumOOSMSteps property allows.
The tracker retrodict existing tracks to the time of the out-of-sequence detections and calculates the cost of assignment for each combination of track and detection.
The tracker attempts to assign the out-of-sequence detections to the tracks.
The tracker retrodicts the track to the time of each assigned out-of-sequence detection and retroCorrect the track with the OOSM, see the picture below.
The tracker initializes new tracks from unassigned out-of-sequence detections.
Then, the tracker resumes its usual processing with the in-sequence detections.

The retrodiction technique is more expensive in terms of memory and processing time than the neglect technique. However, it is important to handle OOSM in the following cases:

The sensor that provides OOSM covers areas that are not covered by any other sensor. In that case, neglecting the OOSM would result in complete loss of coverage in the area.
The OOSM contain novel information that other sensors cannot provide. For example, if the OOSM are provided by the only sensor that reports object classification, neglecting the OOSM would result in tracks that have no classification data.
The sensors that report to the tracker operate at a low scan rate. In that case, every detection is important and neglecting OOSM would result in low tracking accuracy.

Define the Scenario and Sensor Lag

To explore OOSM handling techniques and their impact on tracking, you create a simple scenario. In the scenario, there are two moving platforms that approach from left and right to the middle of the scenario, where they move close to each other. There are two radars, one on the left and one on the right. The radar on the left reports detections directly to the tracker with no time delay. The radar on the right reports detections through a network that has a time delay. Note that initially only the radar on the right covers the platform on the right until it reaches an area covered by the left sensor.

The following lines of code create and visualize the scenario.

scenario = createScenario();
[tp, platp, trajp, detp, covp, trp] = createPlotters(scenario);

Figure contains an axes object. The axes object with xlabel X (m), ylabel Y (m) contains 5 objects of type line, patch. One or more of the lines displays its values using only markers These objects represent Targets, Detections, (history), Sensor Coverage, Trajectories.

In this code section you define the time delay between the radar on the right and the tracker. You use the objectDetectionDelay object to add the network time delay to data coming from the right radar sensor, with SensorIndex = 2, to the tracker. Use the slider on line 5 to select the time delay. Valid values range from 0 (no delay) to 3 seconds with a default value of 2 seconds. Once you set the value on the slider, the radar updates its time delay.

% Set the time delay of the sensor
sensorDelay = objectDetectionDelay(SensorIndices = 2, DelaySource = "Property", DelayDistribution = "Constant");
sensorDelay.DelayParameters = 2

sensorDelay = 
  objectDetectionDelay with properties:

        SensorIndices: 2
             Capacity: Inf
          DelaySource: 'Property'
    DelayDistribution: 'Constant'
      DelayParameters: 2

Define the Tracker and OOSM Handling Technique

Both trackerGNN and trackerJPDA System objects allow you to choose the OOSM handling technique by setting the OOSMHandling property. Use the first drop down below to choose between GNN and JPDA. Then use the next drop down to choose between "Neglect" and "Retrodiction". Once selected, the tracker uses the value you choose.

tracker = trackerGNN('FilterInitializationFcn',@initfilter, 'MaxNumTracks', 10, 'ConfirmationThreshold',[2 4],'AssignmentThreshold', [30 inf]);
tracker.OOSMHandling = "Retrodiction"

tracker = 
  trackerGNN with properties:

                  TrackerIndex: 0
       FilterInitializationFcn: @initfilter
                  MaxNumTracks: 10
              MaxNumDetections: Inf
                 MaxNumSensors: 20

                    Assignment: 'MatchPairs'
           AssignmentThreshold: [30 Inf]
          AssignmentClustering: 'off'

                  OOSMHandling: 'Retrodiction'
               MaxNumOOSMSteps: 3

                    TrackLogic: 'History'
         ConfirmationThreshold: [2 4]
             DeletionThreshold: [5 5]

            HasCostMatrixInput: false
    HasDetectableTrackIDsInput: false
               StateParameters: [1x1 struct]

             ClassFusionMethod: 'None'

                     NumTracks: 0
            NumConfirmedTracks: 0

        EnableMemoryManagement: false

Run the Scenario and Analyze Metrics

In this section, you run the scenario with the time delay you set for the right radar and the OOSM handling technique you defined for the tracker.

You define a Generalized Optimal Sub-Pattern Association (GOSPA) metric to evaluate the tracker performance in terms of overall GOSPA, localization, missed targets, and false tracks. For more information about GOSPA, see trackGOSPAMetric.

gospa = trackGOSPAMetric;
lgospa = zeros(1,33);
localization = zeros(1,33);
missTarget = zeros(1,33);
falseTracks = zeros(1,33);
i = 0;

The next code section initializes the scenario and the visualization, resets the tracker, and sets the random seed for repeatable results.

s = rng(2021, "twister");
h = onCleanup(@() rng(s));
delayedDets = {};
detBuffer = {};
restart(scenario);
reset(tracker);
clearPlotterData(tp);
plotTrueTrajectories(trajp, scenario);

Next, you run the main simulation loop. You can see the results in the figure below the block of code. Because the tracker is connected to the left radar, it updates when the left radar finishes a scan, or every 1.7 seconds.

% Main simulation loop
while advance(scenario) && ishghandle(tp.Parent)
    % Generate sensor data
    [dets, configs, sensorConfigPIDs] = detect(scenario);

    % Apply time delay to the detections
    time = scenario.SimulationTime;
    if isLocked(sensorDelay) || ~isempty(dets)
        delayedDets = sensorDelay(dets,time);
    end

    detBuffer = vertcat(detBuffer, delayedDets); %#ok<AGROW>

    [truePosition, meas, measCov] = readData(scenario, detBuffer);

    % Tracker update
    if configs(1).IsScanDone
        if isLocked(tracker) || ~isempty(detBuffer)
            [tracks,~,~,info] = tracker(detBuffer, time);
        else
            tracks = objectTrack.empty;
        end
        detBuffer = {};

        % Update the trackPlotter
        posSelector = [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0];
        trpos = getTrackPositions(tracks, posSelector);
        plotTrack(trp, trpos, string([tracks.TrackID]));

        % Update GOSPA metric
        i = i + 1;
        truths = platformPoses(scenario);
        [lgospa(i), ~, ~, localization(i), missTarget(i), falseTracks(i)] = gospa(tracks,truths(3:4));
    end

    % Update plots
    plotPlatform(platp,truePosition);
    plotDetection(detp,meas,measCov);
    plotCoverage(covp,coverageConfig(scenario));

    drawnow limitrate
end

Figure contains an axes object. The axes object with xlabel X (m), ylabel Y (m) contains 10 objects of type line, patch, text. One or more of the lines displays its values using only markers These objects represent Targets, Detections, (history), Sensor Coverage, Trajectories.

You want to analyze the results of the tracker for the selected radar time delay. The following code block shows the four GOSPA metrics. Remember that a lower GOSPA metric value indicates better tracking.

For the default selection of 2 seconds radar time delay and "Retrodiction" OOSM handling technique, the GOSPA metrics show that there are no false tracks and that both platforms are being tracked after 3 tracker updates. After the third update, only the localization errors contribute to the overall GOSPA metric.

Use the time delay slider and the tracker OOSM handling drop down menu in the previous section to choose other combinations and compare them with these values. For example, if the tracker OOSM handling is set to "Neglect", and the radar data delay is 2 seconds, the platform on the right will not be tracked until it enters the coverage area of the left radar. As a result, the missed targets GOSPA remains high for the first part of the scenario until the platform enters the coverage area of the left radar sensor.

% Display GOSPA metrics
figure;
subplot(2,2,1),plot(lgospa);
ylim([0 30]);
grid
title("Total GOSPA")
subplot(2,2,2),plot(localization);
ylim([0 30]);
grid
title("Localization GOSPA")
subplot(2,2,3),plot(missTarget);
ylim([0 30]);
grid
title("Missed Targets GOSPA")
subplot(2,2,4),plot(falseTracks);
ylim([0 30]);
grid
title("False Tracks GOSPA");

Figure contains 4 axes objects. Axes object 1 with title Total GOSPA contains an object of type line. Axes object 2 with title Localization GOSPA contains an object of type line. Axes object 3 with title Missed Targets GOSPA contains an object of type line. Axes object 4 with title False Tracks GOSPA contains an object of type line.

Summary

In this example you learned the importance of handling out-of-sequence measurements (OOSM). You used the retrodiction handling to process detections that arrived late to the tracker and compared the results of retrodiction to the results of neglecting the OOSM.

Supporting Functions

The readData function prepares detection and platform data to update the plotters.

function [position, meas, measCov] = readData(scenario,dets)
allDets = [dets{:}];

if ~isempty(allDets)
    % extract column vector of measurement positions
    meas = cat(2,allDets.Measurement)';

    % extract measurement noise
    measCov = cat(3,allDets.MeasurementNoise);
else
    meas = zeros(0,3);
    measCov = zeros(3,3,0);
end

truePoses = platformPoses(scenario);
position = vertcat(truePoses(:).Position);
end

The createPlotters function creates the plotters and sets up the initial display.

function [tp, platp, trajp, detp, covp, trp] = createPlotters(scenario)
% Create plotters
tp = theaterPlot(XLim = [-1000 1000], YLim = [-1500 500], ZLim = [-1500 200]);
set(tp.Parent, YDir = "reverse", ZDir = "reverse");

% Change to 2-D view
view(2)

platp = platformPlotter(tp, DisplayName = "Targets", MarkerFaceColor = "k");
detp = detectionPlotter(tp, DisplayName = "Detections", MarkerSize = 6, MarkerFaceColor = [0.85 0.325 0.098], MarkerEdgeColor = "k", History = 10000);
covp = coveragePlotter(tp, DisplayName = "Sensor Coverage");
trp = trackPlotter(tp, ConnectHistory = "on", ColorizeHistory = "on");

% Plot ground truth trajectories for the moving objects
poses = platformPoses(scenario);
plotPlatform(platp, vertcat(poses.Position));
trajp = trajectoryPlotter(tp, DisplayName = "Trajectories", LineWidth = 1);
plotTrueTrajectories(trajp, scenario);

% Plot coverage
covcon = coverageConfig(scenario);
plotCoverage(covp,covcon);
end

The createScenario function creates the scenario, platforms, and radars.

function scenario = createScenario
% Create Scenario
scenario = trackingScenario;
scenario.StopTime = Inf;
scenario.UpdateRate = 0;


% Create platforms
Tower = platform(scenario, ClassID = 3);
Tower.Dimensions = struct( ...
    Length = 10, ...
    Width = 10, ...
    Height = 60, ...
    OriginOffset = [0 0 30]);
Tower.Trajectory.Position = [-400 500 0];

Tower1 = platform(scenario, ClassID = 3);
Tower1.Dimensions = struct( ...
    Length = 10, ...
    Width = 10, ...
    Height = 60, ...
    OriginOffset = [0 0 30]);
Tower1.Trajectory.Position = [400 500 0];

Plane = platform(scenario, ClassID = 1);
Plane.Dimensions = struct( ...
    Length = 1, ...
    Width = 1, ...
    Height = 1, ...
    OriginOffset = [0 0 0]);
Plane.Signatures = {...
    rcsSignature(...
    Pattern = [20 20;20 20], ...
    Azimuth = [-180 180], ...
    Elevation = [-90;90], ...
    Frequency = [0 1e+20])};
Plane.Trajectory = waypointTrajectory( ...
    [-400 200 0;-50 100 0;-20 -600 0; 0 -1000 0], ...
    [0;22;45;60], ...
    AutoPitch = true, ...
    AutoBank = true);

Plane1 = platform(scenario,ClassID = 1);
Plane1.Dimensions = struct( ...
    Length = 1, ...
    Width = 1, ...
    Height = 1, ...
    OriginOffset = [0 0 0]);
Plane1.Signatures = {...
    rcsSignature(...
    Pattern = [20 20;20 20], ...
    Azimuth = [-180 180], ...
    Elevation = [-90;90], ...
    Frequency = [0 1e+20])};
Plane1.Trajectory = waypointTrajectory( ...
    [525 200 0;50 100 0;20 -600 0; 0 -1000 0], ...
    [0;22;45;60], ...
    AutoPitch = true, ...
    AutoBank = true);


% Create sensors
Sector = fusionRadarSensor(SensorIndex = 1, ...
    UpdateRate = 10, ...
    MountingLocation = [4.85 -4.98 0], ...
    MountingAngles = [0 0 0], ...
    FieldOfView = [5 1], ...
    HasINS = true, ...
    ReferenceRange = 1000, ...
    DetectionCoordinates = "Scenario", ...
    MechanicalAzimuthLimits = [-150 -30]);

Sector1 = fusionRadarSensor(SensorIndex = 2, ...
    UpdateRate = 10, ...
    MountingLocation = [-5.01 -5.04 0], ...
    FieldOfView = [5 1], ...
    HasINS = true, ...
    ReferenceRange = 1000, ...
    DetectionCoordinates = "Scenario", ...
    MechanicalAzimuthLimits = [-150 -30]);


% Assign sensors to platforms
Tower.Sensors = Sector;
Tower1.Sensors = Sector1;
end

The initfilter function initializes a nearly constant velocity trackingEKF filter configured to allow a higher process noise.

function ekf = initfilter(detection)
ekf = initcvekf(detection);
ekf.ProcessNoise = ekf.ProcessNoise*9;
end

The plotTrueTrajectories function plots ground truth trajectories on the theater plot.

function plotTrueTrajectories(trajp, scenario)
trajpos = cell(1,2);
for i = 3:4
    trajpos{i-2} = lookupPose(scenario.Platforms{i}.Trajectory, (0:0.1:60));
end
plotTrajectory(trajp,trajpos);
end