SMC-PHD Filter
Posted on
This post is based on Ristic, B., Clark, D., & Vo, B. (2010). Improved SMC implementation of the PHD filter. 2010 13th International Conference on Information Fusion, 1–8.
At time $k$,
- $n_k$ object states: $x_{k,1},\ldots,x_{k,n_k}\in\cX\subseteq\bbR^{n_x}$
- $m_k$ measurements (detections): $z_{k,1},\ldots,z_{k,m_k}\in\cZ\subseteq \bbR^{n_z}$
- $X_k = \{x_{k,1},\ldots,x_{k,n_k}\}\in\cF(\cX)$
- $Z_k = \{z_{k,1},\ldots,z_{k,m_k}\}\in\cF(\cZ)$
Objective: determine at each time step $k$ the posterior probability density of multi-object state $f_{k\mid k}(X_k\mid Z_{1:k})$
Suppose that $f_{k-1\mid k-1}(X_{k-1}\mid Z_{1:k-1})$ is known and a new set of measurements $Z_k$. Then the predicted multi-object posterior densities
\[f_{k\mid k-1}(X_k\mid Z_{1:k-1})=\int\Pi_{k\mid k-1}(X_k\mid X_{k-1})f_{k-1\mid k-1}(X_{k-1}\mid Z_{1:k-1})dX_{k-1}\]and the updated posterior densities
\[f_{k\mid k}(X_k\mid Z_{1:k}) = \frac{\vartheta_k(Z_k\mid X_k)f_{k\mid k-1}(X_k\mid Z_{1:k-1})}{\int\vartheta_k(Z_k\mid X)f_{k\mid k-1}(X\mid Z_{1:k-1})dX}\]Since $f_{k\mid k}(X_k\mid Z_{1:k})$ is defined over $\cF(\cX)$, practical implementation of the multi-object Bayes nonlinear filter is challenging and usually limited to a small number of objects.
In order to overcome this limitation, Mahler proposed to propagate only the first-order statistical moment of $f_{k\mid k}(X\mid Z_{1:k})$, the intensity function or PHD (Probability Hypothesis Density)
\[D_{k\mid k}(x\mid Z_{1:k}) = \int \delta_X(x)f_{k\mid k}(X\mid Z_{1:k})\delta X\]defined over $\cX$, where $\delta_X(x)=0$ if $X=\emptyset$ and otherwise,
\[\delta_X(x) = \sum_{w\in X}\delta_w(x)\]The PHD has the property that
\[\int_\cX D_{k\mid k}(x\mid Z_{1:k})dx = \nu_{k\mid k}\in\bbR\]is the expected number of objects in the state space $\cX$.
PHD
\[D_\Xi(x) = \E[\delta_\Xi(x)] = \int \delta_X(x)\cdot f_\Xi(X)\delta X\]The PHD is a density function but not a probability density. The integral of $D_\Xi(x)$ in any region $S$ is the expected number of objects of $\Xi$ in that region:
\[\int_S D_\Xi(x)dx = \E[\vert S\cap \Xi\vert]\,.\]PHDs can be constructed from fundamental multitarget descriptors in three ways:
- from multitarget densities via set integration
- from belief-mass functions via set differentiation
- from p.g.fl.s via functional differentiation
\begin{align}
D_\Xi(x) &=\int \delta_X(x)\cdot f_\Xi(x)\delta X\
&=\sum_{n=1}^\infty \frac{1}{n!}\int [\delta_{x_1}(x)+\cdots + \delta_{x_n}(x)]f_\Xi(\{x_1,\ldots,x_n\})dx_1\cdots dx_n\
&=\sum_{n=1}^\infty \frac{n}{n!}\int f_\Xi(\{x,w_1,\ldots,w_{n-1}\})dw_1\ldots dw_{n-1}\
&=\sum_{i=0}^\infty \frac{1}{i!}\int f_\Xi(\{x,w_1,\ldots,w_{n-1}\})dw_1\ldots dw_{i}\
&=\int f_\Xi(\{x\}\cup W)\delta W\,.
\end{align}
(c.f. 9.18)
Abbreviate $D_{k\mid k}(x\mid Z_{1:k})$ as $D_{k\mid k}(x)$, the prediction equation of the PHD filter is given by
\[D_{k\mid k-1}(x) = \gamma_{k\mid k-1}(x) + \langle p_S D_{k-1\mid k-1}, \pi_{k\mid k-1}(x\mid \cdot)\rangle\]where
- $\gamma_{k\mid k-1}(x)$ is the PHD of object births between time $k$ and $k+1$
- $p_S(x’):=p_{S,k\mid k-1}(x’)$ is the probability that a target with state $x’$ at time $k-1$ will survive until time $k$
The update step of the PHD filter is
\[D_{k\mid k}(x) = ...\]The SMC implementation of the PHD filter is based on the random sample approximation of the PHD:
\[D_{k\mid k}(x)\approx \sum_{n=1}^Nw_{k\mid k}^n\delta_{x_{k\mid k}^n}(x)\,.\]For large $N$, the sum of importance weights approximates the expected number of objects
\[\sum_{n=1}^Nw_{k\mid k}^n\approx \nu_{k\mid k}\,.\]c.f. SMC.
Treat $D_{k\mid k}(x)$ as $\gamma(\cdot)$, $\nu$ as $Z$. Here weighted $w$ has been divided by $N$.
Birth density diven by measurements
place the newborn particles in the region of the state-space $x\in\cX$ for which the likelihood $g_k(z\mid x)$ will have high values.
New formulation of the PHD filter
Let $x=(y,\beta)$, where $\beta=1$ for a newborn object, while 0 for a persistent object.
A newborn object becomes a persisting object at the next time, but a persisting object cannot become a newborn object.
\[\pi_{k\mid k-1}(x\mid x') = \pi_{k\mid k-1}(y\mid y')\pi_{k\mid k-1}(\beta\mid\beta')\]The PHD filter prediction equation for augmented state vector is given by
\[D_{k\mid k-1}(y,\beta) = \gamma_{k\mid k-1}(y,\beta) + \sum_{\beta'=0}^1...\]and the updated step is also changed.