12.2.1 Introduction

Figure 12.1 shows the general tracking problem in the context of automobile tracking. Two scans of data are shown. When the first scan is done, there are two tracks. The uncertainty ellipsoids are shown and they are based on all previous information. In the k − 1 scan (a scan is a set of measurements taken at the same time), three measurements are observed. Each scan has multiple measurements, the measurements in each new scan are numbered beginning with 1, and the measurement numbers are not meant to imply any correlation across subsequent scans. One and 3 are within the ellipsoids of the two tracks, but 2 is in both. It may be a measurement of either of the tracks or a spurious measurement. In scan k, four measurements are taken. Only measurement 4 is in one of the uncertainty ellipsoids. Three may be interpreted as spurious, but it is actually because of a new track from a third vehicle that it separates from the blue track. Measurement 1 is outside of the red ellipsoid, but is actually a good measurement of the red track and (if correctly interpreted) indicates that the model is erroneous. 4 is a good measurement of the blue track and indicates that the model is valid. Measurement 2 of scan k is outside both uncertainty ellipsoids.

“Spurious” means that the measurement is not associated with any tracked object and isn’t a new track. We can’t determine the nature of any measurement without going through the MHT process.

We define a contact as an observation where the signal-to-noise ratio is above a certain threshold. The observation then constitutes a measurement. Low signal-to-noise ratio observations can happen in both optical and radar systems. Thresholding reduces the number of observations that need to be associated with tracks, but may lose valid data. An alternative is to treat all observations as valid, but adjust the measurement error accordingly.

Valid measurements must then be assigned to tracks. An ideal tracking system would be able to categorize each measurement accurately and then assign them to the correct track. The system must also be able to identify new tracks and remove tracks that no longer exist. A tracking system may have to deal with hundreds of objects (perhaps after a collision or because of debris in the road).

A sophisticated system should be able to work with multiple objects as groups or clusters if the objects are more or less moving in the same direction. This reduces the number of states a system must handle. If a system handles groups, then it must be able to handle groups spawning from groups.

If we were confident that we were only tracking one vehicle, all of the data might be incorporated into the state estimate. An alternative is to incorporate only the data within the covariance ellipsoids and treat the remainders as outliers. If the latter strategy were taken, it would be sensible to remember that data in case future measurements were also “outliers,” in which case the filter might go back and incorporate different sets of outliers into the solution. This could easily happen if the model were invalid, for example, if the vehicle, which had been cruising at a constant speed, suddenly began maneuvering and the filter model did not allow for maneuvers.

The multiple model filters helps with the erroneous model problem and should be used any time a vehicle might change mode. It does not tell us how many vehicles we are tracking, however. With multiple models, each model would have its own error ellipsoids and the measurements would fit one better than the other, assuming that one of the models was a reasonable model for the tracked vehicle in its current mode.

12.2.2 Example

Referring to Figure 12.1 in the first scan, we have three measurements. 1 and 3 are associated with existing tracks and are used to update those tracks. 2 could be associated with either. It might be a spurious measurement or it could be a new track, so the algorithm forms a new hypothesis. In scan 2 measurement 4 is associated with the blue track. 1, 2, and 3 are not within the error ellipsoids of either track. Since the figure shows the true track, we can see that 1 is associated with the red track. Both 1 and 2 are just outside the error ellipsoid for the red track. Measurement 2 in scan 2 might be consistent with measurement 2 in scan 1 and could result in a new track. Measurement 3 in scan 2 is a new track, but we likely don’t have enough information to create a track until we have more scans of data.

12.2.3 Algorithm

In classical multiple target tracking, [24], the problem is divided into two steps, association and estimation. Step 1 associates contacts with targets and step 2 estimates each target’s state. Complications arise when there is more than one reasonable way to associate contacts with targets. The multiple hypothesis testing (MHT) approach is to form alternative hypotheses to explain the source of the observations. Each hypothesis assigns observations to targets or false alarms.

There are two basic approaches to MHT [3]. The first, following Reid [21], operates within a structure in which hypotheses are continually maintained and updated as observation data are received. In the second, the track-oriented approach to MHT, tracks are initiated, updated, and scored before being formed into hypotheses. The scoring process consists of comparing the likelihood that the track represents a true target versus the likelihood that it is a collation of false alarms. Thus, unlikely tracks can be deleted before the next stage, in which tracks are formed into a hypothesis. It is a good thing to discard the old hypotheses and start from scratch each time because this approach maintains the important track data while preventing an explosion of an impractically large number of hypotheses.

The track-oriented approach recomputes the hypotheses using the newly updated tracks after each scan of data is received. Rather than maintaining, and expanding, hypotheses from scan to scan, the track-oriented approach discards the hypotheses formed on scan k-1. The tracks that survive pruning are predicted to the next scan k where new tracks are formed, using the new observations, and reformed into hypotheses. Except for the necessity to delete some tracks based upon low probability or N-scan pruning, no information is lost because the track scores that are maintained contain all the relevant statistical data.

Track scoring is done using log-likelihood ratios. LR is the likelihood ratio, LLR the log-likelihood ratio, and L is the likelihood.

(12.1)

where the subscript K denotes kinematic (position) and the subscript S denotes signal (measurement). It is assumed that the two are statistically independent.

(12.2)

where H ₁ and H ₀ are the true target and false alarm hypotheses. is a natural logarithm. The likelihood ratio for the kinematic data is the probability that the data are a result of the true target divided by the probability that the data are due to a false alarm:

(12.3)

where

1. 1.

   M in the denominator of the third formula is the measurement dimension

    
2. 2.

   V_C is the measurement volume

    
3. 3.

   S = HPT^T + R the measurement residual covariance matrix

    
4. 4.

   d ² = y^TS ⁻¹y is the normalized statistical distance for the measurement

    

The statistical distance is defined by the residual y, the difference between the measurement and the estimated measurement, and the covariance matrix S. The numerator is the multivariate Gaussian.

12.2.4 Measurement Assignment and Tracks

Each track also spawns a new track assuming that there was no measurement for the track. Thus in this case, three measurements and two tracks result in nine new tracks. Tracks 7–9 are initiated based only on the measurement, which may not be enough information to initiate the full state vector. If this is the case, there would be an infinite number of tracks associated with each measurement, not just one new track. If we have a radar measurement we have azimuth, elevation, range, and range rate. This gives all position states and one velocity state.

12.2.5 Hypothesis Formation

In MHT, a valid hypothesis is any compatible set of tracks. In order for two or more tracks to be compatible, they cannot describe the same object, and they cannot share the same measurement at any of the scans. The task in hypothesis formation is to find one or more combinations of tracks that: 1) are compatible, and 2) maximize some performance function.

Recall that each track is formed as a sequence of measurements across multiple scans. In addition to the raw measurement history, every track also contains a history of state and covariance data that is computed from a Kalman Filter. Kalman Filters are explored in Chapter 8. When a new measurement is appended to an existing track, we are spawning a new track that includes all of the original track’s measurements, plus this new measurement. Therefore, the new track is describing the same object as the original track.

A new measurement can also be used to generate a completely new track that is independent of past measurements. When this is done, we are effectively saying that the measurement does not describe any of the objects that are already being tracked. It therefore must correspond to a new/different object.

In this way, each track is given an object ID to distinguish which object it describes. Within the context of track-tree diagrams, all of the tracks inside the same track-tree have the same object ID. For example, if at some point there are 10 separate track-trees, this means that 10 separate objects are being tracked in the MHT system. When a valid hypothesis is formed, it may turn out that only a few of these objects have compatible tracks.

In addition, we extend the formulation with an option to solve for multiple hypotheses, rather than just one. The algorithm will return the “M best” hypotheses, in descending order of score. This enables tracks to be preserved from alternate hypotheses that may be very close in score to the best.

12.2.6 Track Pruning

We use the results of hypothesis formation to guide track pruning. The parameters N, M, and P can be tuned to improve performance. The objective with pruning is to reduce the number of tracks as much as possible, while not removing any tracks that should be part of the actual true hypothesis.

The second item listed above is to preserve all tracks included in the “M best” hypotheses. Each of these is a full path through a track-tree, which is clear. The third item listed above is similar, but less constrained. Consider one of the tracks in the “M best” hypotheses. We will preserve this full track. In addition, we will preserve all tracks that stem from scan “P” of this track.

Figure 12.3 provides an example of which tracks in a track-tree might be preserved. The diagram shows 17 different tracks over five scans. The green track represents one of the tracks found in the set of “M best” hypotheses, from the hypothesis formation step. This track would be preserved. The orange tracks all stem from the node in this track at scan 2. These would be preserved if we set P = 2 from the description above.

12.3.1 Problem

You want to estimate the trajectory of multiple billiard balls. In the billiard ball example, we assume that we have multiple balls moving at once. Let’s say we have a video camera placed above the table, and we have software that can measure the position of each ball for each video frame. That software cannot, however, determine the identity of any ball. This is where MHT comes in. We use MHT to develop a set of tracks for the moving balls.

12.3.2 Solution

The solution is to create a linear Kalman Filter.

12.3.3 How It Works

The core estimation algorithm for the MHT system is the Kalman Filter. The Kalman Filter consists of a simulation of the dynamics and an algorithm to incorporate the measurements. For the examples in this chapter we use a fixed gain Kalman Filter. The model is:

(12.4)

(12.5)

x_k is the state, a column vector that includes position and velocity. y_k is the measurement vector. u_k is the input, the accelerations on the billiard balls. c relates the state to the measurement, y. If the only measurement were position then:

(12.6)

This is a discrete time equation. Since the second column is zero, it is only measuring position. Let’s assume we have no input accelerations. Also assume that the time step is τ. Then our equations become:

(12.7)

(12.8)

where s is position and v is velocity, y_k = s. This says that the new position is the old position plus velocity times time. Our measurement is just position. If there are no external accelerations, the velocity is constant. If we can’t measure acceleration directly then this is our model. Our filter will estimate velocity given changes in position.

A track, in this case, is a sequence of s. MHT assigns measurements, y to the track. If we know that we have only one object and that our sensor is measuring the track accurately, and doesn’t have any false measurements or possibility of missing measurements, we can use the Kalman Filter directly.

The KFBilliardsDemo simulates billiard balls. It includes two functions to represent the dynamics. The first is RHSBilliards, which is the right-hand-side of the billiards ball dynamics, which were just given above. This computes the position and velocity given external accelerations. The function BilliardCollision applies conservation of momentum whenever a ball hits a bumper. Balls can’t collide with other balls. The first part of the script is the simulation that generates a measurement vector for all of the balls. The second part of the script initializes one Kalman Filter per ball. This script perfectly assigns measurements to each track. The function KFPredict is the prediction step, i.e., the simulation of the ball motion. It uses the linear model described above. KFUpdate incorporates the measurements. MHTDistance is just for information purposes. The initial positions and velocity vectors of the balls are random. The script fixes the seed for the random number generator to make every run the same, which is handy for debugging. If you comment out this code each run will be different.

Here, we initialize the ball positions.

We then simulate them. Their motion is in a straight line unless they collide with a bumper.

We then process the measurements through the Kalman Filter. KFPredict predicts the next position of the balls and KFUpdate incorporates measurements. The prediction step does not know about collisions.

The results of the Kalman Filter demo are shown in Figure 12.4, Figure 12.5 and Figure 12.6. The covariances and states for all balls are plotted, but we only show one here. The covariances always follow the same trend with time. As the filter accumulates measurements it adjusts the covariances based on the ratio between the model covariance, i.e., how accurate the model is assumed to be, and the measurement covariances. The covariances are not related to the actual measurements at all. The Kalman Filter errors are shown in Figure 12.6. They are large whenever the ball hits a bumper, since the model does not include collisions with the bumpers. They rapidly decrease because our measurements have little noise.

The following code, excerpted from the above demo, is specialized drawing code to show the billiards on the table. It calls plot for each ball. Colors are taken from the array c and are blue, green, red, cyan, magenta, yellow, and black. You can run this from the command line once you have computed xP and yP, which are the x and y positions of the balls. The code uses the legend handles to associate the balls with the tracks in the plot in the legend. It manually sets the limits (gca as a handle to the current axes).

You can change the covariances, sigP,sigV,sigMeas in the script and see how it impacts the errors and the covariances.

12.4.1 Problem

You want to estimate the trajectory of multiple billiard balls.

12.4.2 Solution

The solution is to create an MHT system with a linear Kalman Filter. This example involves billiard balls bouncing off of the bumpers of a billiard table. The model does not include the bumper collisions.

12.4.3 How It Works

The following code adds the MHT functionality. It first runs the demo, just like in the example above, and then tries to sort the measurements into tracks. It only has two balls. When you run the demo you will see the graphical user interface (GUI), Figure 12.7, and the Tree, Figure 12.8, change as the simulation progresses. We only include the MHT code in the following listing.

Figure 12.7 shows the MHT GUI. This shows the GUI at the end of the simulation. The table shows scans on the x-axis and tracks on the y-axis (vertical). Each track is numbered as xxx.yyy, where xxx is the track and yyy is the tag. Every track is assigned a new tag number. For example, 95.542 is track 95 and tag 542 means it is the 542nd track generated. The numbers in the table show the measurements associated with the track and the scan. TRK 3.21 and TRK 3.57 are duplicates. In both cases one measurement per scan is associated with the TRK. Their scores are the same because they are consistent. We can only pick one or the other for our hypothesis. TRK 95.542 doesn’t get a measurement from scan 77, but for the rest of the scans it gets measurement 2. Scans 77 through 80 are active. A scan is a set of four position measurements. The summary shows there are seven active tracks, but we know (although the software does not necessarily) that there are only four balls in play. The number of scans are the ones currently in use to determine valid tracks. There are two active hypotheses.

Figure 12.8 shows the decision tree. You can see that with scan 80, two new tracks are created. This means that MHT thinks that there could be as many as four tracks. However, at this point only two tracks, 3 and 95, have multiple measurements associated with them.

Figure 12.9 shows the information window. This shows the MHT algorithm’s thinking. It gives the decisions made with each scan.

The demo shows that the MHT algorithm correctly associates measurements with tracks.

12.5.1 Problem

You want to estimate the position of an object moving in one direction with unknown accelerations.

12.5.2 Solution

The solution is to create a linear Kalman Filter with an acceleration state.

12.5.3 How It Works

In this demo, we have a model of objects that includes an unknown acceleration state.

(12.9)

(12.10)

where s is position, v is velocity and a is acceleration. y_k = s. τ is the time step. The input to the acceleration state is the time rate of change of acceleration.

The function DoubleIntegratorWithAccel creates the matrices shown above:

with τ = 0.5 s.

We will set up the simulation so that one object has no acceleration, but starts in front of the other. The other will overtake the first. We want to see if MHT can sort out the trajectories. Passing would happen all the time with autonomous driving.

The following code implements the Kalman Filters for two vehicles. The simulation runs first to generate the measurements. The Kalman Filter runs next. Note that the plot array is updated after the filter update. This keeps it in sync with the simulation.

We use PlotSet with the argument ’plot set’ to group inputs and the argument ’legend’ to put legends on each plot. ’plot set’ takes a cell array of 1 × n arrays and ’legend’ takes a cell array of cell arrays as inputs. We don’t need to numerically integrate the equations of motion because the state equations have already done that. You can always propagate a linear model in this fashion. We set the model noise matrix using aRand, but don’t actually input any random accelerations. As written, our model is perfect, which is never true in a real system, hence the need for model uncertainty.

Figure 12.10 shows the states and the errors. The filters track all three states for both objects pretty well. The acceleration and velocity estimates converge with 10 s. It does a good job of estimating the fixed disturbance acceleration despite only having a position, s, measurement.

12.6.1 Problem

You want to estimate the position of an object moving in one direction with measurements that need to be associated with a track.

12.6.2 Solution

The solution is to create an MHT system with the Kalman Filter as the state estimator.

12.6.3 How It Works

The MHT code is shown below. We append the MHT software to the script shown above. The Kalman Filters are embedded in the MHT software. We first run the simulation and gather the measurements and then process them in the MHT code.

Figure 12.11 shows the states and the errors. The MHT-hypothesized tracks are a good fit to the data.

Figure 12.12 shows the MHT GUI and the tree. Track 1 contains only measurements from object 2. Track 2 contains only measurements from object 1. Three hundred and fifty-four and 360 are spurious tracks. Three hundred and fifty-four has a measurement 1 for scan 177, but none for the following scan. Three hundred and sixty was created on scan 180 and has just one measurement. One and 2 have the same score. The results show that the MHT software has successfully sorted out the measurements and assigned them correctly. At this point, the end of the sim, four scans are active.