1、背景介绍
卡尔曼滤波(Kalman)无论是在单目标还是多目标领域都是很常用的一种算法,我们将卡尔曼滤波看做一种运动模型,用来对目标的位置进行预测,并且利用预测结果对跟踪的目标进行修正,属于自动控制理论中的一种方法。
在对视频中的目标进行跟踪时,当目标运动速度较慢时,很容易将前后两帧的目标进行关联,如下图所示:
如果目标运动速度比较快,或者进行隔帧检测时,在后续帧中,目标A已运动到前一帧B所在的位置,这时再进行关联就会得到错误的结果,将A‘与B关联在一起。
那怎么才能避免这种出现关联误差呢?我们可以在进行目标关联之前,对目标在后续帧中出现的位置进行预测,然后与预测结果进行对比关联,如下图所示:
我们在对比关联之前,先预测出A和B在下一帧中的位置,然后再使用实际的检测位置与预测的位置进行对比关联,只要预测足够精确,几乎不会出现由于速度太快而存在的误差。
卡尔曼滤波就可以用来预测目标在后续帧中出现的位置,如下图所示,卡尔曼滤波器就可以根据前面五帧数据目标的位置,预测第6帧目标的位置。
卡尔曼滤波器最大的优点是采用递归的方法来解决线性滤波的问题,它只需要当前的测量值和前一个周期的预测值就能够进行状态估计。由于这种递归方法不需要大量的存储空间,每一步的计算量小,计算步骤清晰,非常适合计算机处理,因此卡尔曼滤波受到了普遍的欢迎,在各种领域具有广泛的应用前景。
2.原理介绍
我们假设一个简单的场景,有一辆小车在行驶,它的速度是v,可以通过观测得到它的位置p,也就是说我们可以实时的观测小车的状态。
场景描述
小车在某一时刻的状态表示为一个向量:
虽然我们比较确定小车此时的状态,无论是计算还是检测都会存在一定的误差,所以我们只能认为当前状态是其真实状态的一个最优估计。那么我们不妨认为当前状态服从一个高斯分布,如下图所示:
高斯分布的中心就是图中:
因为我们有两个变量,所以可以用一个协方差矩阵Pk来表示数据之间的相关性和离散程度:
预测下一时刻的状态
下面我们需要通过小车的当前状态,运用一些物理学的知识来预测它的下一个状态,即通过k-1时刻的位置和速度,可以推测下一个时刻的状态为:
写成矩阵形式就是:
此处的Fk就是状态转移矩阵。
系统的不确定性和相关性可以通过协方差矩阵描述,那根据当前协方差矩阵预测下一时刻的协方差矩阵:
在这里我们用到了协方差的性质:
增加系统的内部控制
我们需要对小车进行控制,比如加速和减速,假设某个时刻我们施加的加速度是\color{green}{\mathbf{a}}a,那么下一时刻的位置和速度则应该为:
再写成矩阵的形式:
其中,Bk我们称为状态控制矩阵,而uk称为状态控制向量,前者表明的是加速减速如何改变小车的状态,而后者则表明控制的力度大小和方向。
考虑系统的外部影响对观测数据的预测
前面我们通过小车的上一个状态,对它的当前状态做了预测,此时我们要考虑对于小车的状态能够观测到什么呢?
小车的当前状态和观测到的数据应该具备某种特定的关系,假设这个关系通过矩阵表示为Hk,如下图所示:
在此前对小车所做的预测状态下,我们的观测值为:
那我们就完成了对观测值的预测:
实际的观测结果
前面推测小车当前的状态,推测了我们的观测数据,但是现实和理想之间必然是存在差距的,我们预测的观测结果和实际的观测结果可能如下图所示:
卡尔曼滤波需要做的最重要的最核心的事就是融合预测和观测的结果,充分利用两者的不确定性来得到更加准确的估计。通俗来说就是怎么从上面的两个椭圆中来得到中间淡黄色部分的高斯分布,看起来这是预测和观测高斯分布的重合部分,也就是概率比较高的部分。
高斯分布的乘积
一维高斯分布来分析比较简单点
两个服从高斯分布的函数相乘:
对于任意两个高斯分布,将二者相乘之后还是高斯分布,我们利用高斯分布的两个特性进行求解,其一是均值处分布函数取极大值,其二是均值处分布曲线的曲率为其二阶导数,我们可以求出:
重新归一化,使总概率为1,可以得到
图中蓝色和橙色两个波形的直接乘积是黄色这个波形,紫色是计算了均值和方差的记过,黄色的分布可以通过紫色的波形乘上一个系数得到。
对于高阶的高斯分布:
新的高斯分布
通过预测和观测值的高斯分布的乘积得到的即是卡尔曼滤波的最优估计
在新的均值和方差计算公式中,我们令:
将式(11)中的两个式子相同的部分用 k 表示为(13),下面进一步将式(13)写成矩阵的形式:
如果 Σ 表示高斯分布的协方差,u 表示每个维度的均值,将它们写成矩阵形式就是:
前面我们已经得到了预测结果和观测结果服从的两个高斯分布,如下:
预测部分:
测量部分:
将它们放到式(15)中算出它们之间的重叠部分
由式(14)可得卡尔曼增益为:
所以我们可以进行如下推导,将式(16)和式(17)两边化简下,注意K可以展开得到卡尔曼滤波对当前状态(基于预测和观测的)最优估计的计算方程:
K′就是卡尔曼增益:
实际中的计算方法
在实际使用卡尔曼滤波的时候,计算的步骤一般为:
预测阶段
更新阶段
最重要的是,我们要时刻关注不断迭代的系统变量,分别是系统的状态:x,其误差协方差矩阵:P,和卡尔曼增益:K。
在实际应用时,对Q和R的选择要依据实际情况来定,可以不断调试来寻找一个最优解,也可以是可变的,只要最终效果能够更好。
总结
1.卡尔曼滤波器中在目标跟踪中的应用
卡尔曼滤波器通过预测目标在后续帧中的位置,避免在进行目标关联时出现误差
卡尔曼滤波器的原理
滤波器根据上一时刻( k -1 时刻) 的值来估计当前时刻( k 时刻) 的状态,得到 k 时刻的先验估计值; 然后使用当前时刻的测量值来更正这个估计值,得到当前时刻的估计值。
目标不确定性和相关性的度量预测目标的下一时刻的状态系统内部的控制和外部的影响利用观测值进行修正实际应用中:预测和更新两个阶段