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

一、卡尔曼滤波（Kalman Filter）

1.1 Data Fusion问题的引入：

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

如何融合两个传感器的量测结果得到最优的目标状态估计？
（最终结果还是个估计值，只不过是融合了两个传感器量测结果的更好的估计值，肯定不可能获得开了上帝视角的真值）

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

$A\sim N\left({\mu }_A,{\sigma }_A^2\right)$
$B\sim N\left({\mu }_B,{\sigma }_B^2\right)$

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

$$C=A+s\left(B-A\right)s\in \left[0,1\right]\text{\hspace{1em}(1)}$$

其中，s为加权系数，也即：

1. 当s取0时，最终估计输出C完全由A决定;
2. 当s取1时，最终估计输出C完全由B决定；
3. 当s在0到1之间时，最终估计输出C由A和B以不同比例共同决定。

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

  这里我们认为：

方差最小的C对应着最优估计，也即其随机结果最接近目标真值，此时的s也称为最优的s
  问题转换： 求解最优估计输出—>求解最小方差对应的s
  求解过程：
1.C的方差为：
$$\begin{array}{cc}& D\left(C\right)=D\left(A+s\left(B-A\right)\right)\\ & ={\left(1-s\right)}^{2}D\left(A\right)+s^{2}D\left(B\right)\end{array}\text{\hspace{1em}(2)}$$

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

$\begin{array}{cc}& \frac{dD\left(C\right)}{dk}=-2\left(1-s\right)D\left(A\right)+2sD\left(B\right)=0\\ & s=\frac{D\left(A\right)}{D\left(A\right)+D\left(B\right)}\text{=}\frac{{\sigma }_A^2}{{\sigma }_A^2+{\sigma }_B^2}\end{array}$

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

$$C=A+s\left(B-A\right)\text{\hspace{1em}(3)}$$
$$D\left(C\right)=\frac{{\sigma }_A^2{\sigma }_B^2}{{\sigma }_A^2+{\sigma }_B^2}={\sigma }_A^2-s{\sigma }_A^2\text{\hspace{1em}(4)}$$

当理解到这里时，你已经掌握的Kalman滤波的核心思想：两个高斯随机变量通过比例系数s融合为方差最小的最优估计，这里的s就是后面的卡尔曼增益。

总结：
两个高斯随机变量A和B的最优融合结果为C，其分别满足：
$A\sim N\left({\mu }_A,{\sigma }_A^2\right)$
$B\sim N\left({\mu }_B,{\sigma }_B^2\right)$
$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)$

1.2 Kalman Filter

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

$$X_k=A{X}_{k-1}+B{\mu }_k+w_k\text{\hspace{1em}(5)}$$
$\begin{array}{cc}{w}_k\sim N\left(0,Q\right)& Q\sim E\left(w{w}^T\right)\end{array}$

量测方程：

$$Z_k=H{X}_k+v_k\text{\hspace{1em}(6)}$$
$\begin{array}{cc}{v}_k\sim N\left(0,R\right)& R=E\left(v{v}^T\right)\end{array}$

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

1. 状态转移方程是描述运动物体的位置和速度等自身状态信息变化的方程，这些状态是我们不可见的，待估计的量，这里有时间递推的存在；
2. 量测方程是描述物体状态被传感器观测后的状态改变情况，将目标状态这种不可见的信息转变为我们可见的传感器信息，没有时间递推的存在。

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

• $X_{k-1}^{+}$：已经获得的第k-1帧的最优状态估计，为前k-1帧融合出的最优结果；
• $P_{k-1}^{+}$： $X_{k-1}^{+}$对应的协方差矩阵，为前k-1帧融合出$X_{k-1}^{+}$对应的方差。
• $Q$：过程噪声协方差，当前第k帧受过程噪声影响程度；
• $A$：状态转移矩阵，描述相邻两帧状态转移关系；
• $B$：控制矩阵，和控制向量$u$一起，描述状态转移过程中的输入控制；
• $H$：量测矩阵，描述目标状态到量测空间的映射关系；
• $R$：量测噪声协方差，映射过程中受噪声影响程度；

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

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

$$X_k^{-}=A{X}_{k-1}^{+}+B{\mu }_k\text{\hspace{1em}(7)}$$
$$P_k^{-}=A{P}_{k-1}^{+}{A}^T+Q\text{\hspace{1em}(8)}$$

2.将先验状态估计$X_k^{-}$通过量测矩阵$H$转换到量测域，得到先验量测估计$H{X}_k^{-}$及其协方差矩阵$H{P}_k^{-}{H}^T$；
3.接下来进行Data Fusion操作，建立后验状态估计方程：

$$H{X}_k^{+}=H{X}_k^{-}+K\left(Z_k-H{X}_k^{-}\right)\text{\hspace{1em}(9)}$$

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

• 当前帧的量测值$Z_k\sim N\left(\ast ,R\right)$
• 先验量测估计$H{X}_k^{-}\sim N\left(\ast ,H{P}_k^{-}{H}^T\right)$

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

$$K=\frac{H{P}_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\text{\hspace{1em}(10)}$$
$$H{P}_k^{+}{H}^T\text{=}H{P}_k^{-}{H}^T-KH{P}_k^{-}{H}^T\text{\hspace{1em}(11)}$$

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

$$K=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\text{\hspace{1em}(12)}$$
$$X_k^{+}=X_k^{-}+K\left(Z_k-H{X}_k^{-}\right)\text{\hspace{1em}(13)}$$
$$P_k^{+}\text{=}{P}_k^{-}-KH{P}_k^{-}\text{=}\left(I-KH\right)P_k^{-}\text{\hspace{1em}(14)}$$

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

1.3 代码实现（Matlab）

%此程序用于展示一个简单的传统kalman Filter demo
N = 100;%总时间状态数
A = [1 1; 0 1];%状态转移矩阵
Q = [1 0; 0 0];%过程噪声协方差矩阵
H = [1 0; 0 1];%观测矩阵
R = [100 0; 0 100];%量测噪声协方差矩阵
%量测协方差相比过程噪声协方差越大，量测数据对最终输出越小

X0 = [1.0 0];%初始估计状态
P0 = [1 0; 0 1];%初始估计误差

%自己造一个状态转移信号和量测信号,实际由传感器获得
W = [normrnd(0, sqrt(Q(1,1)), [1,N]);
     normrnd(0, sqrt(Q(2,2)), [1,N])];
V = [normrnd(0, sqrt(R(1,1)), [1,N]);
     normrnd(0, sqrt(R(2,2)), [1,N])];
Xt = zeros(2,N);Xt(:,1) = X0';
Zt = zeros(2,N+1);
for i = 1:N
    Xt(:,i+1) = A*Xt(:,i) + W(:,i);
    Zt(:,i+1) = H*Xt(:,i+1) + V(:,i);
end

X_fliter = zeros(2,N+1);
%Kalman滤波
for i = 1:N 
    %预测部分
    X1_ = A*Xt(:,i);
    P1_ = A*P0*A.' + Q;
    %更新部分
    K = (P1_*H.')/(H*P1_*H.'+R);
    X1 = X1_ + K*(Zt(:,i+1) - H*X1_);
    P0 = (eye(2) - K*H)*P1_;
    %记录输出
    X_fliter(:,i+1) = X1;
end

figure,hold on
t = [1:N+1];
plot(t,X_fliter(1,:),'-b*',t,Zt(1,:),'-*r',t,Xt(1,:),'-^g');

二、数据关联(Data Association)

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

2.0 跟踪门

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


椭圆波门示意图入下面图中所示。

2.1 最近邻域法(Nearest Neighbor，NN)

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

2.2 概率数据关联(Probabilistic Data Association，PDA)

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

$X_k^{+}=X_{k-1}^{-}+K{V}_k=X_{k-1}^{-}+K\sum _{j=1}^{m_k}{\beta }_j{v}_j$

$v_j={\left(Z_k^j-H{X}_k^{-}\right)}^T{S}^{-1}\left(Z_k^j-H{X}_k^{-}\right)$

2.3 联合概率数据关联 (Joint Probabilistic Data Association, JPDA)

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

确认矩阵
$$\Omega =\left[\begin{array}{ccc}1& 0& 1\\ 1& 1& 1\\ 1& 1& 0\\ 1& 1& 0\end{array}\right]$$

互联矩阵：
$$\Omega \left({\theta }_1\right)=\left[\begin{array}{ccc}0& 0& 1\\ 0& 1& 0\\ 1& 0& 0\\ 1& 0& 0\end{array}\right]$$

$$\Omega \left({\theta }_2\right)=\left[\begin{array}{ccc}0& 0& 1\\ 1& 0& 0\\ 1& 0& 0\\ 1& 0& 0\end{array}\right]$$
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​)

$${\beta }_{jt}=\sum _{i=1}^{L}p\left({\theta }_i|Z_K\right)w_{jt}\left({\theta }_i\right)$$

$$\Omega \left(\beta \right)=\left[\begin{array}{cc}{\beta }_{11}& {\beta }_{12}\\ {\beta }_{21}& {\beta }_{22}\\ {\beta }_{31}& {\beta }_{32}\\ {\beta }_{41}& {\beta }_{42}\end{array}\right]$$

2.4 CJPDA(Cheap JPDA)

标准的JPDA互联概率计算太复杂，精简一下如下：
$${\beta }_{jt}(k)=\frac{{\mathbf{e}}_{jt}}{\sum _{m=1}^{m_k}{\mathbf{e}}_{mt}+\sum _{n=1}^T{\mathbf{e}}_{jn}-e_{jt}+b}$$
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\pi /\gamma )}^{n/2}\lambda V_k{c}_n(1-P_d{P}_G)/P_d$$

2.5 NNCJPDA

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

2.6 工程实现

参考资料：
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