Minimum Uncertainty JPDA Filters and Coalescence Avoidance for Multiple Object Tracking

Abstract

Two variations of the joint probabilistic data association filter (JPDAF) are derived and simulated in various cases in this paper. First, an analytic solution for an optimal gain that minimizes posterior estimate uncertainty is derived, referred to as the minimum uncertainty JPDAF (M-JPDAF). Second, the coalescence-avoiding JPDAF (C-JPDAF) is derived, which removes coalescence by minimizing a weighted sum of the posterior uncertainty and a measure of similarity between estimated probability densities. Both novel algorithms are tested in much further depth than any prior work to show how the algorithms perform in various scenarios. In particular, the M-JPDAF more accurately tracks objects than the conventional JPDAF in all simulated cases. When coalescence degrades the estimates at too great of a level, and the C-JPDAF is often superior at removing coalescence when its parameters are properly tuned.

Appendices

Appendix: A: M-JPDAF Proofs

Note that all object-related variables refer to the i-th object, so this subscript is omitted from indexing the object. Any subsequent usage in Appendix A is when i refers to matrix rows.

• From Eqs. 16 and 18 the uncertainty cost for any gain K for each estimate is given by

  $$ J_{P}=\text{tr}[P^{-}-(1-\beta_{0})(KHP^{-}+P^{-}H^{T}K^{T})+KEK^{T}]. $$

  (41)

  It can be easily seen that the second derivative of J _P is only a function of the third term

  $$ \text{tr}[KEK^{T}]=\sum\limits_{i=1}^{n}{k_{i}^{T}}Ek_{i} $$

  (42)

  where k _i corresponds to the i-th row of K. Taking derivatives with respect to k _i ,

  $$ {\frac{\partial ({k_{i}^{T}}Ek_{i})}{\partial k_{i}}}=2Ek_{i}, \quad \frac{\partial^{2}({k_{i}^{T}}Ek_{i})}{\partial {k_{i}^{2}}}=2E, $$

  (43)

  which is positive-definite from (ii). As the Hessian is positive-definite, the given optimal gain globally minimizes the cost. Using Eq. 21, the optimal cost is given by,

  $$ J^{*}_{P} = \text{tr}[P^{-}-KEK^{T}]. $$

  (44)

• Suppose that an object is not measured. Let this portion of the innovation update be e ₀=0 _m×1 because no innovation exists where measurements are not available. The Total Probability Theorem holds true for object i:

  $$ \sum\limits_{j=0}^{n_{r}}\beta_{j}=1. $$

  (45)

  Consider the positive-semidefinite matrix ${\Sigma }\in \mathbb {R}^{m\times m}$ from [4, Eq. 1.4.16-(1-10)],

  $$ {\Sigma}=\sum\limits_{j=0}^{n_{r}}\beta_{j}(e_{j}-{\bar{e}})(e_{j}-{\bar{e}})^{T}, $$

  (46)

  which may be manipulated as follows,

  $$ {\Sigma}=\sum\limits_{j=0}^{n_{r}}\beta_{j}e_{j}{e_{j}^{T}} -\sum\limits_{j=0}^{n_{r}}\beta_{j}e_{j}{\bar{e}}^{T} -{\bar{e}}\sum\limits_{j=0}^{n_{r}}\beta_{j}{e_{j}^{T}} +{\bar{e}}{\bar{e}}^{T} =\sum\limits_{j=1}^{n_{r}}\beta_{j}e_{j}{e_{j}^{T}}-{\bar{e}}{\bar{e}}^{T}, $$

  (47)

  because e ₀≡0. From Eq. 17,

  $$ E=(1-\beta_{0})S+{\Sigma}, $$

  (48)

  which yields a positive-definite E because S is positive-definite, Σ is positive-semidefinite, and 0≤β ₀<1. Therefore, E is invertible.

• Let $K_{K}=P^{-}H^{T}S^{-1}\in \mathbb {R}^{n\times m}$ [3]. Suppose that these gains are identical, i.e., K = K _K . Multiplying both sides by E and using Eq. 17,

  $$\begin{array}{@{}rcl@{}} KE&=&K_{K}E \\ (1-\beta_{0})P^{-}H^{T}&=& (1-\beta_{0})P^{-}H^{T}+K_{K}{\Sigma}. \end{array} $$

  (49)

  Therefore if Eq. 49 is true, then K _K Σ=0, which may only occur if the probabilities that each measurement belongs to an object are exactly 0 or 1 or K _K ⊥Σ. None of these conditions are satisfied in general, so it follows that the Kalman gain is different in general from the optimal gain proposed in this paper.

• From Eq. 41, the uncertainty cost with the Kalman gain $J_{P_{K}}\in \mathbb {R}$ can be written as

  $$\begin{array}{@{}rcl@{}} J_{P_{K}} &=& \text{tr}[P^{-}-2(1-\beta_{0})P^{-}H^{T}S^{-1}HP^{-}+P^{-}H^{T}S^{-1}ES^{-1}HP^{-1}] \\ &=& \text{tr}\left[P^{-}-(1-\beta_{0})^{2}P^{-}H^{T}\left( \frac{2}{1-\beta_{0}}S^{-1}-\frac{1}{(1-\beta_{0})^{2}}S^{-1}ES^{-1}\right)HP^{-}\right].\\ \end{array} $$

  (50)

  Similarly, the optimal cost $J^{*}_{P}$ may be rewritten as

  $$ J^{*}_{P} = \text{tr}[P^{-}-(1-\beta_{0})^{2}P^{-}H^{T}E^{-1}HP^{-}]. $$

  (51)

  From Eqs. 50 and 51 and using variations of the definition of E from Eq. 17 repeatedly, the difference $J_{P_{K}}-J^{*}_{P}$can be written as

  $$\begin{array}{@{}rcl@{}} J_{P_{K}}-J^{*}_{P} &=& (1-\beta_{0})^{2}\text{tr}\left[P^{-}H^{T}\left( E^{-1}-\frac{2}{1-\beta_{0}}S^{-1}\right.\right.\\ &&\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad+\left.\left.\frac{1}{(1-\beta_{0})^{2}}S^{-1}ES^{-1}\right)HP^{-}\right] \\ &=& \text{tr}\left[P^{-}H^{T}S^{-1}{\Sigma} E^{-1}{\Sigma} S^{-1}HP^{-}\right] \\ &= &\text{tr}\left[K_{K}{\Sigma} E^{-1}{\Sigma} {K_{K}^{T}}\right] =\sum\limits_{i=1}^{n}{\nu_{i}^{T}}E^{-1}\nu_{i}\geq0, \end{array} $$

  (52)

  where ν _i corresponds to the i-th row of K _K Σ and E ⁻¹ is positive-definite because E is positive-definite from (ii). This implies the inequality $J_{P_{K}}\geq J^{*}_{P}$. The only times when K _K becomes optimal are if there exists no measurement origin uncertainty, i.e., Σ=0, or K _K ⊥Σ, and neither scenario is satisfied in general. These results are consistent with those obtained from Eq. 49.

Appendix: B: Derivatives to Obtain the C-JPDAF

The derivative of the similarity cost is obtained as follows. Let ${\mathbf {1}_{bd}}\in \mathbb {R}^{n\times m}$ be defined such that its (b,d)-th element is one and other elements are zero, and let $[K_{p}]_{bd}\in \mathbb {R}$ be the (b,d)-th element of K _p . Then we have

$$ {\frac{\partial (S^{+}_{pq})^{-1}}{\partial [K_{p}]_{bd}}} =-(S^{+}_{pq})^{-1} H_{p} {\frac{\partial P^{+}_{p}}{\partial [K_{p}]_{bd}}} {H_{p}^{T}} (S^{+}_{pq})^{-1} , $$

(53)

where ${\frac {\partial P^{+}_{p}}{\partial [K_{p}]_{bd}}}\in \mathbb {R}^{n\times n}$ is

$$ {\frac{\partial P^{+}_{p}}{\partial [K_{p}]_{bd}}} = \left[(1-\beta_{0,p})({\mathbf{1}_{bd}} H_{p}P_{p}^{-}+P_{p}^{-}{H_{p}^{T}}{\mathbf{1}_{bd}^{T}})-{\mathbf{1}_{bd}} E_{p}{K_{p}^{T}} -K_{p}E_{p}{\mathbf{1}_{bd}^{T}}\right]. $$

(54)

Using Eqs. 53 and 54, we have

$$\begin{array}{@{}rcl@{}} {\frac{\partial (S^{+}_{pq})^{-1}}{\partial [K_{p}]_{bd}}}&=&-{S^{+}_{pq}}^{-1} H_{p} [(1-\beta_{0,p})({\mathbf{1}_{bd}} H_{p}P_{p}^{-}+P_{p}^{-}{H_{p}^{T}}{\mathbf{1}_{bd}^{T}})-{\mathbf{1}_{bd}} E_{p}{K_{p}^{T}} \\ && -K_{p}E_{p}{\mathbf{1}_{bd}^{T}}] {H_{p}^{T}} {S^{+}_{pq}}^{-1}. \end{array} $$

(55)

From Eqs. 33 and 55, ${\frac {\partial u_{pq}}{\partial [K_{p}]_{bd}}}\in \mathbb {R}$ is obtained as

$$\begin{array}{@{}rcl@{}} {\frac{\partial u_{pq}}{\partial [K_{p}]_{bd}}} &=& (H_{p}{\mathbf{1}_{bd}}{\bar{e}}_{p})^{T} {S_{pq}^{+}}^{-1} (H_{p}K_{p}{\bar{e}}_{p}-\hat{z}^{+}_{q}) \\ && +(H_{p}K_{p}{\bar{e}}_{p}-\hat{z}^{+}_{q})^{T} {\frac{\partial (S^{+}_{pq})^{-1}}{\partial [K_{p}]_{bd}}} (H_{p}K_{p}{\bar{e}}_{p}-\hat{z}^{+}_{q}) \\ &&+(H_{p}K_{p}{\bar{e}}_{p}-\hat{z}^{+}_{q})^{T} {S_{pq}^{+}}^{-1} (H_{p}{\mathbf{1}_{bd}}{\bar{e}}_{p}) \\ &=& \left\{ 2(H_{p}{\mathbf{1}_{bd}}{\bar{e}}_{p})^{T} {S_{pq}^{+}}^{-1} -(H_{p}K_{p}{\bar{e}}_{p}-\hat{z}^{+}_{q})^{T} {S^{+}_{pq}}^{-1} H_{p} [(1-\beta_{0,p})\right.\\ &&\left.\times({\mathbf{1}_{bd}} H_{p}P_{p}^{-} +P_{p}^{-}{H_{p}^{T}}{\mathbf{1}_{bd}^{T}})-{\mathbf{1}_{bd}} E_{p}{K_{p}^{T}}-K_{p}E_{p}{\mathbf{1}_{bd}^{T}}] {H_{p}^{T}} {S^{+}_{pq}}^{-1}\right\}\\ &&\times(H_{p}K_{p}{\bar{e}}_{p}-\hat{z}^{+}_{q}), \end{array} $$

(56)

Then from Eqs. 32 and 56, we obtain ${\frac {\partial J_{S}}{\partial [K_{p}]_{bd}}}\in \mathbb {R}$ as

$$\begin{array}{@{}rcl@{}} {\frac{\partial J_{S}}{\partial [K_{p}]_{bd}}}&=& -a\sum\limits_{q,q\neq p}\exp (-au_{pq}){\frac{\partial u_{pq}}{\partial [K_{p}]_{bd}}} \\ &=& -a\sum\limits_{q,q\neq p}\exp (-au_{pq}) \{ 2(H_{p}{\mathbf{1}_{bd}}{\bar{e}}_{p})^{T} {S_{pq}^{+}}^{-1} \\ && -(H_{p}K_{p}{\bar{e}}_{p}-\hat{z}^{+}_{q})^{T} {S^{+}_{pq}}^{-1} H_{p} [(1-\beta_{0,p})({\mathbf{1}_{bd}} H_{p}P_{p}^{-} +P_{p}^{-}{H_{p}^{T}}{\mathbf{1}_{bd}^{T}}) \\ && -{\mathbf{1}_{bd}} E_{p}{K_{p}^{T}} -K_{p}E_{p}{\mathbf{1}_{bd}^{T}}] {H_{p}^{T}} {S^{+}_{pq}}^{-1} \}(H_{p}K_{p}{\bar{e}}_{p}-\hat{z}^{+}_{q}). \end{array} $$

(57)