直观深入理解Kalman滤波算法和数据关联算法（NN、PDA、JPDA、CJPDA、NNCJPDA）

  当前有两个传感器A和B，对待观测目标的状态进行测量（如两个电子秤对某物体的重量进行测量），这两个传感器的量测结果，是存在测量误差的随机变量（没有误差就不需要使用Kalman Filter了），现在的问题是：

  这里我们设A和B两传感器对目标的量测结果服从高斯分布，也即：

  一个直观的想法是，最终估计输出C是分别对A和B加权的求和结果，也即：

  那么，如何选择一个最优的s呢？   由于A和B是高斯分布，C是A和B按系数s的线性组合，则C也是高斯分布，如下图：

  这里我们认为：

2.对其进行求导，导数为0即可求得使C的方差 最小的k：

3.此时，C的最优估计输出和最小方差为：

当理解到这里时，你已经掌握的Kalman滤波的核心思想：两个高斯随机变量通过比例系数s融合为方差最小的最优估计，这里的s就是后面的卡尔曼增益。 总结： 两个高斯随机变量A和B的最优融合结果为C，其分别满足： A ∼ N ( μ A , σ A 2 ) A \sim N\left( {{\mu }_{A}},\sigma _{A}^{2} \right) A∼N(μA​,σA2​) B ∼ N ( μ B , σ B 2 ) B \sim N\left( {{\mu }_{B}},\sigma _{B}^{2} \right) B∼N(μB​,σB2​) C ∼ N ( μ A δ B 2 + μ B δ A 2 δ A 2 + δ B 2 , σ A 2 σ B 2 σ A 2 + σ B 2 ) C \sim N\left( \frac{\mu_A \delta_B^2 + \mu_B \delta_A^2}{\delta_A^2 + \delta_B^2} ,\frac{\sigma _{A}^{2}\sigma _{B}^{2}}{\sigma _{A}^{2}+\sigma _{B}^{2}} \right) C∼N(δA2​+δB2​μA​δB2​+μB​δA2​​,σA2​+σB2​σA2​σB2​​)

在以上data fusion的基础上增加时间的维度，也即第k帧的概念，用来描述一个时变的系统，且文中没有特意区分标量，矢量和矩阵，请自行区分。 建立大家很常见的模型： 状态转移方程：

量测方程：

依旧是这样最常见的状态转移方程和量测方程，这里请理解：

在实际应用中，已知量有：

这里约定, X k − X_{k}^{-} Xk−​表示第k帧的先验状态估计，由于还没有融合本帧量测 Z k {{Z}_{k}} Zk​ ，所以用“-”号表示少点信息，当融合本帧量测后用 X k + X_{k}^{+} Xk+​表示后验状态估计，也即可以输出的最优状态估计，   Kalman Filter的思想是：利用当前帧的观测 Z k {{Z}_{k}} Zk​和前一帧最优状态估计 X k − 1 + {{X}_{k-1}^+} Xk−1+​来估计当前帧最优状态估计 X k + {{X}_{k}^+} Xk+​

Kalman滤波流程： 1.已知前一帧最优状态估计 X k − 1 + X_{k-1}^{+} Xk−1+​ ，则根据状态转移矩阵计算当前帧先验状态估计 X k − X_{k}^{-} Xk−​及其协方差矩阵 P k − P_{k}^{-} Pk−​：

2.将先验状态估计 X k − X_{k}^{-} Xk−​通过量测矩阵 H H H转换到量测域，得到先验量测估计 H X k − HX_{k}^{-} HXk−​及其协方差矩阵 H P k − H T HP_{k}^{-}{{H}^{T}} HPk−​HT； 3.接下来进行Data Fusion操作，建立后验状态估计方程：

这里的系数K就是卡尔曼增益Kalman Gain，两个输入需要被融合的高斯随机变量分别对应（*表示均值并不用关心，关注方差）：

4.按照上一节的推导思路，可以得到卡尔曼增益K和对应的最优协方差矩阵 P k + P_{k}^+ Pk+​

5.两侧同时消去H可以得到：

  以上便是Kalman Filter的直观推导过程，请对照上图逐步推导理解。将推导过程进行总结，把（7）（8）（12）（13）（14）公式提出来就是最常见的五个Kalman预测更新公式。

  在实际使用Kalman滤波的过程中，第k帧通过检测门限的量测 Z k i Z_{k}^{i} Zki​会有很多个，也会有很多条起始好的航迹使用Kalman Filter维护跟踪过程，那么每条航迹选择哪一个量测用来应用到Kalman Filter中呢？也即数据关联问题，关联已起始航迹与新进量测。

第k帧通过检测门限的量测 Z k i Z_{k}^{i} Zki​会有很多个，通常使用椭圆跟踪门的方法判断有哪些量测落入当前航迹预测位置处，这些落入的量测称为有效量测，如下为椭圆跟踪门的表述，当然也可以选择矩形和扇形跟踪门。 [ Z k − Z k ∣ k − 1 ] T S ( k ) − 1 [ Z k − Z k ∣ k − 1 ] ≤ γ (15) {{[{{Z}_{k}}-{{Z}_{k|k-1}}]}^T}S{{\left( k \right)}^{-1}}[{{Z}_{k}}-{{Z}_{k|k-1}}]\le \gamma \tag{15} [Zk​−Zk∣k−1​]TS(k)−1[Zk​−Zk∣k−1​]≤γ(15) 其中， S ( k ) S(k) S(k)为新息 Z k − H X k − Z_k-HX_k^{-} Zk​−HXk−​的协方差矩阵 S ( k ) = H P k − H T + R S(k)\text{=}HP_{k}^{-}{{H}^{T}}+R S(k)=HPk−​HT+R，且 Z k ∣ k = H X k − Z_{k|k}=HX_k^{-} Zk∣k​=HXk−​是先验量测估计。一般是先知道 γ \gamma γ, Z k ∣ k Z_{k|k} Zk∣k​和 S ( k ) S(k) S(k)后就能根据（15）确定一个符合马氏距离的虚拟的椭圆跟踪门，落入当前跟踪门内的点就为有效量测。 Z k ∣ k Z_{k|k} Zk∣k​和 S ( k ) S(k) S(k)由Kalman Filter确定， γ \gamma γ根据设定的波门概率 P G P_{G} PG​确定： 椭圆波门示意图入下面图中所示。

算法思路：计算马氏距离，选择距离先验量测估计 Z k ∣ k Z_{k|k} Zk∣k​最近的量测 Z k j Z_{k}^{j} Zkj​作为最优有效量测。 适用于单目标场景，或波门不重叠的多目标场景 d 2 = [ Z k j − Z k ∣ k − 1 ] ′ S − 1 ( k ) [ Z k j − Z k ∣ k − 1 ] {{d}^{2}}=\mathbf{[}Z_{k}^{j}-Z_{k|k-1}^{{}}\mathbf{{]}'}{{\mathbf{S}}^{-\mathbf{1}}}(k)\mathbf{[}Z_{k}^{j}-Z_{k|k-1}^{{}}\mathbf{]} d2=[Zkj​−Zk∣k−1​]′S−1(k)[Zkj​−Zk∣k−1​]

算法思路：为每个有效量测计算出一个概率 β j {{\beta }_{j}} βj​，并结合其新息计算出一个新的最优量测（红点，虚拟出的点）。

X k + = X k − 1 − + K V k = X k − 1 − + K ∑ j = 1 m k β j v j X_{k}^{+}=X_{k-1}^{-}+K{{V}_{k}}=X_{k-1}^{-}+K\sum\limits_{j=1}^{{{m}_{k}}}{{{\beta }_{j}}{{v}_{j}}} Xk+​=Xk−1−​+KVk​=Xk−1−​+Kj=1∑mk​​βj​vj​

v j = ( Z k j − H X k − ) T S − 1 ( Z k j − H X k − ) {{v}_{j}}={{\left( Z_{k}^{j}-HX_{k}^{-} \right)}^{T}}{{S}^{-1}}\left( Z_{k}^{j}-HX_{k}^{-} \right) vj​=(Zkj​−HXk−​)TS−1(Zkj​−HXk−​)

与PDA类似，只不过加入了多目标场景，需要计算每个有效量测 Z k j Z_{k}^{j} Zkj​到不同目标航迹 t t t的互联概率 β j t {{\beta }_{jt}} βjt​，后像PDA一样分别综合计算出不同航迹对应的最优量测点（红点，虚拟出的点）。计算流程参考文末资料。

确认矩阵 Ω = [ 1 0 1 1 1 1 1 1 0 1 1 0 ] \Omega =\left[ \begin{matrix} 1 & 0 & 1 \\ 1 & 1 & 1 \\ 1 & 1 & 0 \\ 1 & 1 & 0 \\ \end{matrix} \right] Ω=⎣⎢⎢⎡​1111​0111​1100​⎦⎥⎥⎤​

互联矩阵： Ω ( θ 1 ) = [ 0 0 1 0 1 0 1 0 0 1 0 0 ] \Omega \left( {{\theta }_{1}} \right)=\left[ \begin{matrix} 0 & 0 & 1 \\ 0 & 1 & 0 \\ 1 & 0 & 0 \\ 1 & 0 & 0 \\ \end{matrix} \right] Ω(θ1​)=⎣⎢⎢⎡​0011​0100​1000​⎦⎥⎥⎤​

Ω ( θ 2 ) = [ 0 0 1 1 0 0 1 0 0 1 0 0 ] \Omega \left( {{\theta }_{2}} \right)=\left[ \begin{matrix} 0 & 0 & 1 \\ 1 & 0 & 0 \\ 1 & 0 & 0 \\ 1 & 0 & 0 \\ \end{matrix} \right] Ω(θ2​)=⎣⎢⎢⎡​0111​0000​1000​⎦⎥⎥⎤​ P { θ i ∣ Z k } = φ ( θ i ) ! c V φ ( θ i ) × ∏ j = 1 m k { N t j [ Z k j ] } τ j ( θ i ) × ∏ t = 1 T ( P d t ) δ t ( θ i ) ( 1 − P d t ) 1 − δ t ( θ i ) P\{{{\theta }_{i}}|{{\mathbf{Z}}^{k}}\}=\frac{\varphi ({{\theta }_{i}})!}{cV\varphi ({{\theta }_{i}})}\times \prod\limits_{j=1}^{{{m}_{k}}}{{{\left\{ {{N}_{tj}}[Z_{k}^{j}] \right\}}^{{{\tau }_{j}}({{\theta }_{i}})}}}\times \prod\limits_{t=1}^{T}{{{(P_{d}^{t})}^{{{\delta }_{t}}({{\theta }_{i}})}}{{(1-P_{d}^{t})}^{1-{{\delta }_{t}}({{\theta }_{i}})}}} P{θi​∣Zk}=cVφ(θi​)φ(θi​)!​×j=1∏mk​​{Ntj​[Zkj​]}τj​(θi​)×t=1∏T​(Pdt​)δt​(θi​)(1−Pdt​)1−δt​(θi​)

β j t = ∑ i = 1 L p ( θ i ∣ Z K ) w j t ( θ i ) {{\beta }_{jt}}=\sum\limits_{i=1}^{L}{p\left( {{\theta }_{i}}|{{Z}_{K}} \right){{w}_{jt}}}\left( {{\theta }_{i}} \right) βjt​=i=1∑L​p(θi​∣ZK​)wjt​(θi​)

Ω ( β ) = [ β 11 β 12 β 21 β 22 β 31 β 32 β 41 β 42 ] \Omega \left( \beta \right)=\left[ \begin{matrix} {{\beta }_{11}} & {{\beta }_{12}} \\ {{\beta }_{21}} & {{\beta }_{22}} \\ {{\beta }_{31}} & {{\beta }_{32}} \\ {{\beta }_{41}} & {{\beta }_{42}} \\ \end{matrix} \right] Ω(β)=⎣⎢⎢⎡​β11​β21​β31​β41​​β12​β22​β32​β42​​⎦⎥⎥⎤​

标准的JPDA互联概率计算太复杂，精简一下如下： β j t ( k ) = e j t ∑ m = 1 m k e m t + ∑ n = 1 T e j n − e j t + b {{\mathbf{\beta }}_{jt}}(k)=\frac{{{\mathbf{e}}_{jt}}}{\sum\limits_{m=1}^{{{m}_{k}}}{{{\mathbf{e}}_{mt}}+\sum\limits_{n=1}^{T}{{{\mathbf{e}}_{jn}}-{{e}_{jt}}+b}}} βjt​(k)=m=1∑mk​​emt​+n=1∑T​ejn​−ejt​+bejt​​ e i j = exp ⁡ { − 1 2 v ′ i j ( k ) S − 1 ( k ) v i j ( k ) } {{\mathbf{e}}_{ij}}=\exp \{-\frac{1}{2}{{\mathbf{{v}'}}_{ij}}(k){{\mathbf{S}}^{-1}}(k){{\mathbf{v}}_{ij}}(k)\} eij​=exp{−21​v′ij​(k)S−1(k)vij​(k)} b = ( 2 π / γ ) n / 2 λ V k c n ( 1 − P d P G ) / P d b={{(2\pi /\gamma )}^{n/2}}\lambda {{V}_{k}}{{c}_{n}}(1-{{P}_{d}}{{P}_{G}})/{{P}_{d}} b=(2π/γ)n/2λVk​cn​(1−Pd​PG​)/Pd​

NNCJPDA是在NN、JPDA、CJPDA的基础上建立的。 其算法为首先计算出量测与目标航迹的互联概率，然后选取最大的互联概率对应的量测 Z k j Z_{k}^{j} Zkj​和目标航迹进行更新，而将落入目标跟踪波门内的其他量测全部移除，也不再考虑量测落入的其他波门的目标航迹。

参考资料： https://www.bilibili.com/video/BV1ez4y1X7eR https://blog.csdn.net/weixin_39461846/article/details/109072051 https://zhuanlan.zhihu.com/p/176851546 https://zhuanlan.zhihu.com/p/181691698