卡尔曼滤波器

我们观测到的数据总是包含噪声的，为了得到更准确的结果，卡尔曼最早在1960年提出卡尔曼滤波器，Kalman Filter 的目的是利用先验知识，根据一批采样数据$(X_1, X2, …,X_n)$估计对象在n时刻的状态$Z_n$。例如我们在跟踪飞行器的时候，我们对它的运动状态并非一无所知，我们知道很多牛顿力学、运动学知识可以帮助我们做出判断。

¶1. 滤波器公式

状态空间表达：

$$ \left\{\begin{array}{c} Z_{n}=g\left(Z_{n-1}, V_{n}\right) \\ X_{n}=h\left(Z_{n}, W_{n}\right) \end{array}\right. $$

第一个方程-状态方程，它主导了我们研究对象的行为，状态都是不能被直接观测到的；第二个方程-观测方程；$V_n$和$W_n$分别是状态噪声和观测噪声。我们的任务是根据前n个时刻的数据得到n时刻的状态，我们可以用递归的思想，即：
$$
Z_{n-1 \mid n-1} \rightarrow Z_{n \mid n-1} \rightarrow Z_{n \mid n}
$$
我们有前$n-1$时刻数据得到的$n-1$时刻的状态$Z_{n-1}$，然后分两步走，先由前$n-1$时刻数据预测n时刻的状态，再由第n时刻的数据对预测的n时刻的状态作修正。

假定状态空间表达是线性的，噪声是零均值的高斯噪声，两个噪声独立，即：

$$ \left\{\begin{array}{c}Z_{n}=G_nZ_{n-1}+V_n \\X_{n}=H_nZ_n+W_n\end{array}\right. $$

可以推出卡尔曼滤波器的公式如下：

$$ \left\{\begin{array}{c} \text {(1)}& Z_{n \mid n-1}&=& G_{n} Z_{n-1 \mid n-1} \hfill \\ &\sum_{n \mid n-1}&=& G_{n} \sum_{n-1 \mid n-1} G_{n}^{T}+R_{n} \hfill \\ (1.5)& A_{n}&=&\sum_{n \mid n-1} H_{n}^{T}\left(H_{n} \sum_{n \mid n-1} H_{n}^{T}+S_{n}\right)^{-1} \hfill \\ \text { (2) }& Z_{n \mid n}&=&Z_{n \mid n-1}+A_{n}\left(X_{n}-H_{n} Z_{n \mid n-1}\right) \hfill \\ &\sum_{n \mid n}&=&\left(I-A_{n} H_{n}\right) \sum_{n \mid n-1} \hfill \end{array}\right. $$

上述公式的其中一种推导可以参考张颢老师的课程P7：https://www.bilibili.com/video/BV1Yq4y1f7ns?p=7

也可以看这个博客的推导：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/或其他教科书等。

¶2. 在多目标跟踪中的应用

在多目标跟踪模型如SORT中，就使用了卡尔曼滤波器进行预测，使用上一帧的结果预测这一帧的结果，再用这一帧网络输出的结果进行校准，python代码中的超参和变量定义如下：

状态 (x,P)，其中x为当前的状态列向量，P是当前的状态协方差，状态转移矩阵F，过程噪声矩阵Q；

测量(z, R)，测量函数H；

在SORT中使用的是匀速模型，状态的维度为7，分别是(cx, cy, s=w*h, r=w/h, cx’, cy’, s’), 观测的维度是4，分别是(cx, cy, s=w*h, r=w/h)

SORT的匹配方法使用了以IoU为代价矩阵的匈牙利算法

• 对于匹配上的检测目标，使用检测结果更新轨迹的卡尔曼滤波器
• 对于没匹配上的检测目标，初始化新的轨迹
• 对于没匹配上的轨迹，如果轨迹超过max_age次没更新就删除轨迹

最终的输出结果为：当前所有轨迹中（当前帧更新了）且（至少最近min_hits次都检测到了）的轨迹

SORT的更多信息见：https://www.cnblogs.com/TaipKang/p/15054030.html

在其他的算法中，对SORT会进行一些细微的修改，比如匈牙利算法的代价矩阵改为L2距离或马氏距离，或者将匈牙利匹配改为贪婪算法，再比如使用两阶段关联：简单来说就是设置了两个阈值，分别是$T_h$和 $T_l$，其中$T_h > T_l$。首先进行对置信度大于$T_h$的Box做正常的匹配关联。在第二阶段，只要Box的置信度高于$T_l$就允许进行关联，但是由于这一段检测结果质量较差，所以检测结果不用于更新运动模型，并且使用运动预测结果代替检测结果输出。

¶3. 局限性

系统必须是线性系统，噪声必须是高斯噪声

如果定义的状态方程参数与实际不符，会发生模型失配