If we have a prior on a state (eg. localization) \(p(x_k \vert z^{k-1})\), we can take into account some observations \(z_k\) using the likelihood function
Assume process noise \(Q = \begin{pmatrix}0.1 & 0 \\ 0 & 0.2\end{pmatrix}\) and observation noise \(R = 0.3\)
Object Tracking using KF
Prior
Figure 1: Prior density on \(X\)
Object Tracking using KF
Prediction 1
Figure 2: Prior and prediction of \(X\)
Object Tracking using KF
Observation 1
Figure 3: Prediction of \(X\) and first observation \(z\)
Object Tracking using KF
Update 1
Figure 4: Kalman update using prediction and observation
Object Tracking using KF
Prediction 2
Figure 5: Update result and prediction of next timestep
Object Tracking using KF
Observation 2
Figure 6: Prediction and second observation
Object Tracking using KF
Update 2
Figure 7: Kalman update using prediction and observation
“Simple” Multiple Object Tracking
Single Object Tracking
“Simple” Multiple Object Tracking
Random Finite Sets
The Probability Hypothesis Density Filter
Problematic
How to track a known and constant number of targets?
Figure 8: A problem of multiple object tracking
Conceptual solution: use 1 KF per track, associate measurements to tracks (NN, GNN,…).
Prediction
Figure 9: Independent prediction for each target
Observations and association
Figure 10: Predictions and observations
Update
Figure 11: Update step
Limits
How to deal with
unknown / time-varying number of tracks;
missdetections;
clutter measurements (False Positives);
the confidence into the existence of the targets?
Random Finite Sets
Single Object Tracking
“Simple” Multiple Object Tracking
Random Finite Sets
The Probability Hypothesis Density Filter
What is a Finite Set?
A set is a collection of differentthings, called its elements. Sets are unordered.
\(X = \{\} = \emptyset\) is the empty set.
\(X = \{x\}\) is a singleton.
They may or may not contain an infinite number of elements.
A Finite Set is a set containing a finite number of elements.
The number of elements is the cardinality of the set, denoted \(\lvert X \rvert\)
Figure 12: Example of a set of polygons
Random Finite Set
Definition
A Random Finite Set (RFS) is a set containing a random number of random variables ie,
\[
X = \left\{ x_1, \dots, x_n \right\}
\]
where both the cardinality \(n\) and the \(x_i\) are randomly distributed.
Elements of a RFS
The \(x_i\) may be random vectors of any dimension, for example the position of a target \(\begin{pmatrix}x \\ y \end{pmatrix}\) or its pose \(\begin{pmatrix}x \\ y \\ \theta \end{pmatrix}\).
Set Integral, PDF
Multi-object PDF \(\pi(X) \geq 0\), s.t.
\[
\int \pi(X)dX = 1
\]
based on the set integral1
\[
\int \pi(X) = \sum_{i=0}^{\infty}\rho(i)
\]
With \(\rho(i)\) the cardinality distribution of \(X\):
\(X\) can be approximated by a PPP with intensity function \(\lambda(x) = D_X(x)\)
PHD Filter - Toy Example
2 features located at \(-1.0\) and \(1.0\) along the x axis.
The y axis is such that \(y(x) = PHD(x)\).
The prior is composed of two gaussians \(\mathcal{N}(-1.02, 0.3^2)\) and \(\mathcal{N}(1.1, 0.4^2)\) with respective weights \(0.85\) and \(0.92\).
Figure 17: Prior PHD of the filter
First update
Observations
Suppose we observe one object at \(0.99\), but we miss the object at \(-1\). The noise model is set at \(R = 0.15\).
We can plot the observation above the prior:
Figure 18: Prior PHD and observation
First update
Kalman update
Perform a Kalman update for each component of the prior, using the observation :
Code
# There is only one observation => no loop on observation setw_sum =0tmp_gm = []kalman_updated_gm = []# decompose component as weight + normalfor w, n in prior_gm: S = R + n.var() # innovation covariance K = n.var() / S # Kalman gain error = o_1.mean() - n.mean() # Innovation mu = n.mean() + K * error var = (1- K) * n.var()# Numerator of the weight update weight = Pd * w * norm.pdf(o_1.mean(), n.mean(), S) w_sum += weight tmp_gm.append((weight, norm(mu, sqrt(var))))# Division for weightsfor w, n in tmp_gm: weight = w / (c_z + w_sum) kalman_updated_gm.append((weight, n))
Figure 19: Kalman update using observation
First update
Take into account potentially missed objects
Multiply the predicted (prior) PHD by \(1- P_D\) to add the possibility of missed objects:
Figure 20: Add component to represent miss-detection hypotheses
First update
Add the previous PHDs to finish the update
Figure 21: Updated PHD
Prediction
Propagate previous update result
Scale by \(P_S\) the probability of survival :
Figure 22: Predicted PHD from previous step
Prediction
Birth model
Create a component for each observation in previous step. The birth model is follows a \(\mathcal{N}(\mu, R)\) with weight \(0.2\) (maybe too high in practice but fine for visualization).
Figure 23: Birth model
Prediction
The predicted PHD is the sum of the propagated PHD and the birth PHD.
Figure 24: Predicted PHD
Second update
Observations
Suppose this time we have the following observations (2 OK + clutter) :
Figure 25: Predicted PHD and observation
Second update
Kalman update
Update each gaussian component with each observation.
Code
kalman_updated_gm = []for o in obs: w_sum =0 tmp_gm = []# decompose component as weight + normalfor w, n in predicted_phd: S = R + n.var() # innovation covariance K = n.var() / S # Kalman gain error = o.mean() - n.mean() # Innovation mu = n.mean() + K * error var = (1- K) * n.var()# Numerator of the weight update weight = Pd * w * norm.pdf(o.mean(), n.mean(), S) w_sum += weight tmp_gm.append((weight, norm(mu, sqrt(var))))# Division for weightsfor w, n in tmp_gm: weight = w / (c_z + w_sum) kalman_updated_gm.append((weight, n))
Figure 26: Kalman update using observation
Second update
Take into account potentially missed objects
Multiply the predicted PHD by \(1- P_D\) to add the possibility of missed objects:
Figure 27: Add components to represent miss-detected hypotheses
Second update
Add previous PHDs
Figure 28: Updated PHD
Prediction
Propagate previous update result
Scale by \(P_S\) the probability of survival :
Figure 29: Predicted PHD from previous step
Prediction
Birth model
Create a component for each observation in previous step. The birth model is follows a \(\mathcal{N}(\mu, R)\) with weight \(0.1\).
Figure 30: Birth model
Prediction
The predicted PHD is the sum of the propagated PHD and the birth PHD.
Figure 31: Predicted PHD
Third update
Observations
Suppose this time we have the following observations (2 OK with bigger noise for the 2nd) :
Figure 32: Predicted PHD and observation
Third update
Kalman update
Update each gaussian component with each observation.
Code
kalman_updated_gm = []for o in obs: w_sum =0 tmp_gm = []# decompose component as weight + normalfor w, n in predicted_phd: S = R + n.var() # innovation covariance K = n.var() / S # Kalman gain error = o.mean() - n.mean() # Innovation mu = n.mean() + K * error var = (1- K) * n.var()# Numerator of the weight update weight = Pd * w * norm.pdf(o.mean(), n.mean(), S) w_sum += weight tmp_gm.append((weight, norm(mu, sqrt(var))))# Division for weightsfor w, n in tmp_gm: weight = w / (c_z + w_sum) kalman_updated_gm.append((weight, n))
Figure 33: Kalman update using observation
Third update
Take into account potentially missed objects
Multiply the predicted PHD by \(1- P_D\) to add the possibility of missed objects:
Figure 34: Add components to represent the miss-detection hypotheses
Third update
Add previous PHDs
Figure 35: Updated PHD
Infos:
Number of components: 69, sum of the weights: 2.058122621541668
Approximation using Gaussian Mixtures
We model the PHD and the measurements as gaussian mixtures, ie. a weighted sum of gaussian distributions.
Prediction
Update
Update
Limits of the filter
increasing number of components (\(\times (m_k + 1)\)) at each update => need to prune or merge components;
not very robust to missdetections (no “memory” on \(N\));
approximate the targets as identically distributed.
Other (better) filters exist, like the CPHD, the MB-RFS filter, the PMBM filter…
