简单易懂详解卡尔曼滤波与数据关联算法（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(\frac{{\mu }_A{\delta }_B^2+{\mu }_B{\delta }_A^2}{{\delta }_A^2+{\delta }_B^2},{\sigma }_A^2$

推荐阅读

• 简单易懂详解卡尔曼滤波与数据关联算法（NN, PDA, JPDA, CJPDA, NNCJPDA）