Gaussian-Mixture Probability Hypothesis Density Filter

GM-PHD, proposed by Vo et. al. in 2006 [Vo2006], is one of the close-form implementation of PHD filter for multi-target tracking.

GM-PHD is developed with following assumptions:

1. Target Independence: each target evolves and generates observations independently of one another

2. Poisson Clutter: clutter porcess is Poisson and independent of target-originated measurements.

3. Poisson RFS: the predicted multi-target RFS governed by \(p_{k|k-1}\) is Poisson (See [Mahler2003]).

4. Linear Gaussian Dynamic Model: each target follows a linear Gaussian dynamic model represented by

   \[f_{k|k-1} \left(x|\zeta\right) = \mathcal{N}(x; F_{k-1}\zeta, Q_{k-1}) \]

5. Target Birth: the PHD of the target birth of RFS are gaussian mixtures of the form

   \[D_{\gamma, k}(x) = \sum_{j=1}^{J_{\gamma, k}} w_{\gamma, k}^{(j)} \mathcal{N}(x; m_{\gamma, k}^{(j)}, P_{\gamma, k}^{(j)}) \]

Predictor

If the posterior PHD of multi-target RFS at time \(k-1\) is represented in forms of Gaussian Mixtures:

\[D_{k-1}(x) = \sum_{j=1}^{J_{k-1}} w_{k-1}^{(j)} \mathcal{N}(x; m_{k-1}^{(j)}, P_{k-1}^{(j)}) \]

The predicted intesity \(D_{k|k-1}\) at time \(k\) is composed of three terms: target birth \(D_{\gamma, k}(x)\), target persistence \(D_{S, k|k-1}(x)\) and target spawning \(D_{\beta, k|k-1}(x)\).

\[D_{k|k-1}(x) = D_{S, k|k-1}(x) + D_{\gamma,k}(x) + D_{\beta, k|k-1}(x) \]

In the application of multi-resident tracking, there is no target spawning, so term \(D_{\beta, k|k-1}(x)\) is ignored in this implementation.

The persistence term updates each existing Gaussian Components according to Kalman update equation (for proof, see Kalman Update for Gaussian Components).

\[D_{S, k|k-1}(x) = p_{S, k} \sum_{j=1}^{J_{k-1}} w_{k-1}^{(j)} \mathcal{N}(x; m_{S,k|k-1}^{(j)}, P_{S,k|k-1}^{(j)}) \]

where

\[m_{S, k|k-1}^{(j)} = F_{k} m_{k-1}^{(j)} \]

and

\[P_{S,k|k-1}^{(j)} = F_{k} P_{k-1}^{(j)} F_{k}^T + Q_{k} \]

This calculation is implemented in gmphd_predictor().

Corrector

Assume that the predicted intensity (i.e. the output of predictor) for time \(k\) is a Gaussian mixture of the form

\[D_{k|k-1}(x) = \sum_{j=1}^{J_{k|k-1}} w_{k|k-1}^{(j)} \mathcal{N}(x; m_{k|k-1}^{(j)}, P_{k|k-1}^{(j)}) \]

The posterior intensity is given by

\[D_{k}(x) = (1-p_{D, k}) D_{k|k-1}(x) + \sum_{z \in Z_k} D_{D, k}(x; z) \]

\(p_{D, k}\) is the probability that the target will be observed. The term \((1-p_{D,k}) D_{k|k-1}(x)\) represents the portion that is not observed at time step \(k\) - and thus do not need to be corrected by the set of measurement \(Z_k\) at time \(k\).

The term \(\sum_{z \in Z_k} D_{D, k}(x; z)\) calculated the posterior PHD corrected according to the measurement set at current time step.

In the equation,

\[D_{D, k}(x;z) = \sum_{j=1}^{J_{k|k-1}} w_{k}^{(j)}(z) \mathcal{N}(x; m_{k}^{(j)}(z), P_{k}^{(j)}) \]

The updated weight is

\[w_{k}^{(j)}(z) = \frac{ p_{D, k} w_{k|k-1}^{(j)} q_k^{(j)}(z) }{ \kappa_k(z) + p_{D, k} \sum_{l=1}^{J_{k|k-1}} w_{k|k-1}^{(l)} q_k^{(l)}(z) } \]

where

\[\kappa_k(z) = \lambda_{c} c(z) \]

\[q_k^{(j)}(z) = \mathcal{N}\left(z; H_k m_{k|k-1}^{(j)}, R_{k} + H_{k} P_{k|k-1}^{(j)} H_{k}^T \right) \]

\[m_{k}^{(j)}(z) = m_{k|k-1}^{(j)} + K(z - H_k m_{k|k-1}^{(j)}) \]

\[P_{k}^{(j)}(z) = [I - K_{k}^{j}H_k] P_{k|k-1}^{(j)} \]

\[K_{k}^{j} = P_{k|k-1}^{(j)} H_k^T (H_k P_{k|k-1}^{(j)}H_k^T + R_{k})^{-1} \]

In the equation above, \(I\) stands for the identity matrix.

Appendix

Kalman Update for Gaussian Components

Assume that each target follows a linear Gaussian dynamic model, i.e.

\[f_{k|k-1} \left(x|\zeta\right) = \mathcal{N}(x; F_{k-1}\zeta, Q_{k-1}) \]

where \(\zeta\) is the mean state vector of the target at time \(k-1\), \(F_{k-1}\) is the state linear multiplier of dynamic model, \(Q_{k-1}\) is the covariance matrix of error estimation in dynamic model.

If the posterior intensity of a target is represented by a Gaussian Component

\[f_{k-1}(x) = w_{k-1}\mathcal{N}(x; m_{k-1}, P_{k-1}) \]

where \(m_{k-1}\) is the mean vector and \(P_{k-1}\) is the covariance matrix.

According to the linear Gaussian dynamic model, at time \(k\), the updated probability density of the target will be

\[f_{k}(x) = w_{k-1}\mathcal{N}(x; m_{k}, P_{k}) \]

where

\[m_{k} = F_{k-1}m_{k-1} \]

and

\[P_{k} = Q_{k-1} + F_{k-1} P_{k-1} F_{k-1}^T \]

This calculation is implemented by kalman_update().

Exemple

A target is modeled by a dynamic model

\[x_{k+1} = F_{k} x_{k} + w_{k} \]

where the error term \(w_{k}\) has a mean of zero and covariance of \(Q_{k}\).

At time \(k\), random variable \(x_{k}\) follows Gaussian distribution \(\mathcal{N}(x_{k}; m_{k}, P_{k})\).

The mean of variable \(x_{k+1}\) is

\[\begin{aligned} E[x_{k+1}] & = {} E[F_{k} x_{k} + w_{k}]\\ & = {} F_{k} E[x_{k}] + E[w_{k}] \\ & = {} F_{k} m_{k} + 0\\ & = {} F_{k} m_{k} \end{aligned} \]

The covariance of variable \(x_{k+1}\) is

\[\begin{aligned} cov[x_{k+1}] & = {} E[x_{k+1}x_{k+1}^T] - E[x_{k+1}]E[x_{k+1}^T]\\ & = {} E[F_{k}x_{k}x_{k}^TF_{k}^T + w_{k}w_{k}^T] - F_{k}m_{k}m_{k}^TF_{k}^T\\ & = {} F_{k} E[x_{k}x_{k}^T]F_{k}^T + Q_{k} - F_{k}m_{k}m_{k}^TF_{k}^T\\ & = {} F_{k} (cov[x] + E[x] E[x^T]) F_{k}^T + Q_{k} - F_{k}m_{k}m_{k}^TF_{k}^T\\ & = {} F_{k} (P_{k} + m_{k}m_{k}^T) F_{k}^T + Q_{k} - F_{k}m_{k}m_{k}^TF_{k}^T\\ & = {} F_{k} P_{k} F_{k}^T + Q_{k} \end{aligned} \]

Gaussian Mixture Corrector Proof

Assume that target measurement follows a linear Gaussian observation model

\[L(z|x_k) = \mathcal{N}(z; H_k x_k, R_k) \]

According to Theorem 6 in [Mahler2003], the “Single-Sensor” Bayes Update formula for PHD is

\[D_{k}(x) \cong \left(1 - p_D(x) + \sum_{z\in Z_k} \frac{ p_D(x) L_z(x) }{ \lambda c(z) + D_{k|k-1}[p_DL_z] } \right) D_{k|k-1}(x) \]

Term \(1-p_D(x)\) is the portion of predicted PHD that does not need to be corrected (as the portion is not observed).

You can view the nominator of the fraction is the weight to the predicted PHD based on the likelihood of getting the observation \(z\). The denominator of the fraction can be viewed as a normalization factor [Mahler2004].

\[L(z, x_k) = \mathcal{N}(z; H_k x, R_k) \sum_{i=1}^{J_{k|k-1}} w_{k|k-1}^{(i)} \mathcal{N}(x; m_{k|k-1}^{(i)}, P_{k|k-1}^{(i)}) \]

\[\begin{aligned} & \frac{ p_D(x) L_z(x) }{ \lambda c(z) + D_{k|k-1}[p_DL_z] } \\ = {} & \frac{ p_{D, k} \mathcal{N}(z; H_k x, R_k) \sum_{i=1}^{J_{k|k-1}} w_{k|k-1}^{(i)} \mathcal{N}(x; m_{k|k-1}^{(i)}, P_{k|k-1}^{(i)}) }{ \lambda c(z) + p_{D, k} \int \mathcal{N}(z; H_k \zeta, R_k) \sum_{i=1}^{J_{k|k-1}} w_{k|k-1}^{(i)} \mathcal{N}(\zeta; m_{k|k-1}^{(i)}, P_{k|k-1}^{(i)}) d\zeta } \\ = {} & \frac{ p_{D, k} \sum_{i=1}^{J_{k|k-1}} w_{k|k-1}^{(i)} \mathcal{N}(z; H_k x, R_k) \mathcal{N}(x; m_{k|k-1}^{(i)}, P_{k|k-1}^{(i)}) }{ \lambda c(z) + p_{D, k} \sum_{i=1}^{J_{k|k-1}} w_{k|k-1}^{(i)} \int \mathcal{N}(z; H_k \zeta, R_k) \mathcal{N}(\zeta; m_{k|k-1}^{(i)}, P_{k|k-1}^{(i)}) d\zeta } \end{aligned} \]

Based on calculus,

\[\begin{aligned} & \mathcal{N}(z; H_k x, R_k) \mathcal{N}(x; m_{k|k-1}^{(i)}, P_{k|k-1}^{(i)}) \\ = {} & \mathcal{N}(z; H_k m_{k|k-1}^{(i)}, R_k + H_k P_{k|k-1}^{(i)} H_k^T) \cdot \\ & \mathcal{N}(x; m_{k|k-1}^{(i)} - k(z - H_k m_{k|k-1}^{(i)}), (1-kH_k) P_{k|k-1}^{(i)}) \end{aligned} \]