计算机视觉(多目标跟踪)算法中卡尔曼滤波算法详解

一、背景详解

卡尔曼滤波（Kalman filter）是一种高效的自回归滤波器，它能在存在诸多不确定性情况的组合信息中估计动态系统的状态，是一种强大的、通用性极强的工具。

只要是存在不确定信息的动态系统，卡尔曼滤波(Kalman)就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰，卡尔曼滤波通常也能很好的弄清楚究竟发生了什么，找出现象间不易察觉的相关性因此卡尔曼滤波(Kalman)非常适合不断变化的系统，它的优点还有内存占用较小（只需保留前一个状态）、速度快，是实时问题和嵌入式系统的理想选择(参见YOLOv5和DeepSORT在Jetson NX板上的部署)。

卡尔曼滤波（Kalman)无论是在单目标还是多目标领域都是很常用的一种算法，我们将卡尔曼滤波看做一种运动模型，用来对目标的位置进行预测，并且利用预测结果对跟踪的目标进行修正，属于自动控制理论中的—种方法。 在对视频中的目标进行跟踪时，当目标运动速度较慢时，很容易将前后两帧的目标进行关联：可以利用之前几帧的位置来预测下一帧的位置，即可关联同一目标。

二、卡尔曼滤波(Kalman)原理

目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

有一辆小车在行驶，它的速度是v，可以通过观测得到它的位置p,也就是说我们可以实时的观测小车的状态。

小车在某一时刻的状态表示为一个向量。

​ 虽然我们比较确定小车此时的状态，无论是计算还是检测都会存在一定的误差，所以我们只能认为当前状态是其真实状态的一个最优估计。那么我们不妨认为当前状态服从一个高斯分布。

考虑轨道上的一个小车，无外力作用，它在时刻t的状态向量只与t-1相关：（状态向量就是描述它的t=0时刻所有状态的向量，比如：[速度大小5m/s, 速度方向右, 位置坐标0]，反正有了这个向量就可以完全预测t=1时刻小车的状态）那么根据t=0时刻的初值，理论上我们可以求出它任意时刻的状态。

当然，实际情况不会这么美好。这个递推函数可能会受到各种不确定因素的影响（内在的外在的都算，比如刮风下雨地震，小车结构不紧密，轮子不圆等等）导致并不能精确标识小车实际的状态。

我们假设每个状态分量受到的不确定因素都服从正态分布。现在仅对小车的位置进行估计请看下图：t=0时小车的位置服从红色的正态分布。

根据小车的这个位置，我们可以预测出t=1时刻它的位置：

分布变“胖”了，这很好理解——因为在递推的过程中又加了一层噪声，所以不确定度变大了。为了避免纯估计带来的偏差，我们在t=1时刻对小车的位置坐标进行一次雷达测量，当然雷达对小车距离的测量也会受到种种因素的影响，于是测量结果告诉我们，小车t=1时的位置服从蓝色分布：

现在我们得到两个不同的结果。前面有人提过加权，Kalman的牛逼之处就在于找到了相应权值，使红蓝分布合并为下图这个绿色的正态分布（啰嗦一句，这个绿色分布均值位置在红蓝均值间的比例称为Kalman增益(比如下图中近似0.8)，就是各种公式里的K(t)）。

绿色分布不仅保证了在红蓝给定的条件下，小车位于该点的概率最大，而且，而且，它居然还是一个正态分布，这意味着可以把它当做初值继续往下算了！这是Kalman滤波能够迭代的关键。

最后，把绿色分布当做第一张图中的红色分布对t=2时刻进行预测，算法就可以开始循环往复了。那么绿色分布是怎么得出的呢？其实可以通过多种方式推导出来。有最大似然法、Ricatti方程法，以及直接对高斯密度函数变形法。另外，由于我只对小车位移这个一维量做了估计，因此Kalman增益是标量，通常情况下它都是一个矩阵。而且如果估计多维量，还应该引入协方差矩阵的迭代。

Kalman滤波算法的本质就是利用两个正态分布的融合仍是正态分布这一特性进行迭代而已。

代码实践

这个实践的已知量是导弹做水平运动，已知导弹每个时刻的速度dv,和速度测量仪器的标准差是v_std，还已知每个时刻用GPS测量出导弹位置position_noise以及GPS的方差是predict_var。（注意标准差的平方是方差，不用觉得奇怪）
我们需要用卡尔曼滤波根据这些信息获得融合两种传感器后的位置信息position_predict：

# -*- coding: utf-8 -*-

import numpy as np

# 模拟数据
t = np.linspace(1,100,100)
a = 0.5
position = (a * t**2)/2

position_noise = position+np.random.normal(0,120,size=(t.shape[0]))
import matplotlib.pyplot as plt
plt.plot(t,position,label='truth position')
plt.plot(t,position_noise,label='only use measured position')


# 初试的估计导弹的位置就直接用GPS测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0
odo_var = 120**2 #这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低
v_std = 50 # 测量仪器的方差
for i in range(1,t.shape[0]):
  
    dv =  (position[i]-position[i-1]) + np.random.normal(0,50) # 模拟从IMU读取出的速度
    position_predict = position_predict + dv # 利用上个时刻的位置和速度预测当前位置
    predict_var += v_std**2 # 更新预测数据的方差
    # 下面是Kalman滤波
    position_predict = position_predict*odo_var/(predict_var + odo_var)+position_noise[i]*predict_var/(predict_var + odo_var)
    predict_var = (predict_var * odo_var)/(predict_var + odo_var)**2
    predicts.append(position_predict)

    
plt.plot(t,predicts,label='kalman filtered position')

plt.legend()
plt.show()

结果：

三、总结

总的来说，卡尔曼滤波器是一个状态估计器，它利用传感器融合、信息融合来提高系统的精度。通常，我们要观测一个系统的状态，有两种手段。一种是通过系统的状态转移方程，并结合上一时刻的状态推得下一时刻的状态。一种是借助 辅助系统(量测系统) 的测量得到系统状态。这两种方式都有各自的不确定性，卡尔曼滤波可以将这两者做到最优结合（加权平均），使得我们估计的状态的不确定性小于其中任何一种。所以权重的选择至关重要，它意味着我们更信任哪一种方式得出的状态（当然是更加信任不确定性较小的状态）。

参考文献

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
https://aistudio.baidu.com/aistudio/projectdetail/2025419?channelType=0&channel=0

https://aistudio.baidu.com/aistudio/projectdetail/2079759?channelType=0&channel=0

https://zhuanlan.zhihu.com/p/39912633