目标跟踪之滤波器（一）：卡尔曼滤波器

引言
  目标跟踪算法中，一类很重要的模式就是使用检测器 + 滤波器来进行轨迹跟踪。检测器通常可选当前主流检测算法：YOLO、RCNN等系列，滤波器则通常包含均值漂移算法（Mean shift）、粒子滤波算法（Particle Filter）、卡尔曼滤波算法（Kalman Filer）和光流算法等。使用检测器+滤波器的目标跟踪模式极其依赖检测器性能的好坏。随着近些年众多优秀检测算法的提出，这种跟踪模式取得了极好的效果。这里我将介绍目标跟踪算法中常用到的卡尔曼滤波器，后续我会持续介绍其他的滤波器。

卡尔曼滤波器的初识
  卡尔曼滤波器用于在包含不确定信息的系统中做出有根据的预测，即使系统伴随各种噪声，卡尔曼滤波器总能指向正确的方向。这里我先给出卡尔曼滤波器的数学公式，后面我将分别对这些公式进行推导。$$\widehat{x_k}=F_k\widehat{x_{k-1}}+B_k{u}_k$$$$P_k=F_k{P}_{k-1}{F}_k^T+Q_k$$$${\hat{x}}^{\prime }={\hat{x}}_k+K^{\prime }(z_k-H_K{\hat{x}}_k)$$$${P_k}^{\prime }=P_k+K^{\prime }(H_K{P}_k)$$$$K^{\prime }=P_k{H}_K^T(H_K{P}_k{H}_K^T+R_k{)}^{-1}$$
卡尔曼滤波方程推导

卡尔曼预测公式推导
  在做目标跟踪时，我们往往更关注目标的位置和速度两个参量，这里我们选取这两个参量作为我们的跟踪目标变量。以 $p_k$ 表示 k 时刻目标所处的位置，$v_k$ 表示 k 时刻目标的速度，则我们可以通过位置和速度来表示 k 时刻目标所处的状态，即：$$x_k=\left[\begin{array}{c}{p}_k\\ v_k\end{array}\right]$$这里假设位置和速度是随机的变量，都服从高斯分布，均值为 μ，方差为 ${\sigma }^2$，均值代表最可能的状态，方差代表不确定性。通常我们希望变量之间相互独立，但卡尔曼滤波器是假设变量之间是相关的，就像观测位置通常与目标运行速度有关。卡尔曼滤波器力图抓住这种相关性特征，从而使用一个变量状态来推测获取其他变量的状态，达到从含不确定性的测试数据中学习到更多的信息的目的。我们通常用协方差矩阵 $\sum$ 来表示这种相关性，设 $\sum ij$ 表示第 i 个和第 j 个状态之间的相关度，则由 $\sum ij$ 构成对称矩阵 $\sum$ 。我们以高斯分布来建立状态变量，将 k 时刻状态用最佳估计 $\widehat{x_k}$ 和以及协方差矩阵 $P_k$ 来表示，这样一个跟踪问题就实例化为矢量数学模型。$$\widehat{x_k}=\left[\begin{array}{c}position\\ velocity\end{array}\right]，P_k=\left[\begin{array}{cc}{\sum }_{pp}& {\sum }_{pv}\\ {\sum }_{vp}& {\sum }_{vv}\end{array}\right]$$  接下来，我们需要根据当前时刻（ k - 1时刻）状态来预测下一时刻（k时刻）状态，这里使用一个状态转移矩阵 $F_k$ 完成两个状态间的转移。值得注意的是，尽管我们并不知道对下一时刻的所有预测中那个是真正要发生的，但由于预测函数预测了所有可能性，同时给出了新的高斯分布，所以这将不再重要。下面我们要做的工作就是确定这个状态转移矩阵 $F_k$ 。这里任然以位置和速度为例，假设目标做匀速运动，已知 k - 1 时刻的位置为 $P_{k-1}$，速度为 $v_{k-1}$，由运动学公式可得 k 时刻的位置和速度为：$$P_k$$