【程序人生】Kalman Filter卡尔曼滤波器
Kalman Filter的原理和推导过程
本文将以二维图像中的object track任务为例，说明Kalman Filter的原理和推导过程
假设在二维图像的像素坐标系下，被跟踪的目标用一个宽高分别平行于像素坐标系
uv的box表示，且目标做匀速运动。
系统的状态变量可以用一个八维列向量表示：
[centerx,centery,heightwidth,height,vcenterx,vcentery,vheightwidth,vheight]T，
v代表速度。
注意假设是在真实的情况下，比如通过在某一个固定的位置的摄像头，tarck马路上的车辆，这时匀速运动车辆的box的size也是匀速变化的。
首先，上第一个方程，预测状态方程：
centerx^=centerx+vcenterx
centery^=centery+vcentery
heightwidth^=heightwidth+vheightwidth
height^=height+vheight
vcenterx^=vcenterx
vcentery^=vcentery
vheightwidth^=vheightwidth
vheight^=vheight
上述八个等式写成矩阵形式就是：
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡centerx^centery^heightwidth^height^vcenterx^vcentery^vheightwidth^vheight^⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡1000000001000000001000000001000010001000010001000010001000010001⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤∗⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡centerxcenteryheightwidthheightvcenterxvcenteryvheightwidthvheight⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
也就是：
预测状态方程：Xk∣k−1=FXk−1∣k−1
Xk−1∣k−1是
k−1时刻的最优状态变量，
Xk∣k−1是
k−1时刻，
k时刻的状态变量预测值，
F我们暂时称之为
motion矩阵，在当前假设情景下是固定不变的，我们想求得
Xk∣k，也就是
k时刻的最优状态变量。
除了预测值，还有测量值可以参考。
因为凭借图像信息，只有位置信息可以统计测量值，所以有等式如下：
⎣⎢⎢⎢⎢⎢⎢⎡centerx^/centery^/heightwidth^/height^/⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎡10000100001000010000000000000000⎦⎥⎥⎤∗⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡centerx^centery^heightwidth^height^vcenterx^vcentery^vheightwidth^vheight^⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
这个等式是通过状态变量的预测值，得到测量值的预测值
也就是：
Zk^=HXk∣k−1，通过
k−1时刻，
k时刻的状态变量预测值，得到
k时刻测量值的预测值。
H我们暂时称之为
update矩阵，在当前假设情景下是固定不变的。
k时刻的测量值记为
Zk，这个值是图像中实际测量得到的。
设
k时刻状态变量的真实值为
Xk，测量的噪声为
Vk，有：
Zk=HXk+Vk
如何通过预测值和测量值，得到最优值呢？由一般的反馈思想，有：
更新最优值方程：Xk∣k=Xk∣k−1+Kk(Zk−Zk^)=Xk∣k−1+Kk(Zk−HXk∣k−1)
k时刻状态变量的最优值，等于
k−1时刻对
k时刻状态变量的预测值，加一个增益系数乘测量值和测量值的预测值的差。这个公式是卡尔曼滤波器的第二个方程， 更新最优值方程，它也正是卡尔曼滤波器的输出，
Kk被称为卡尔曼增益。
如和求卡尔曼增益
Kk呢？这里使用最小均方误差的思想。
k时刻，状态变量最优估计值，和，真实值，之间误差的协方差矩阵设为
Pk∣k=E((Xk−Xk∣k)(Xk−Xk∣k)T)
k时刻，系统变量预测值，和，真实值，之间误差的协方差矩阵设为
Pk∣k−1=E((Xk−Xk∣k−1)(Xk−Xk∣k−1)T)
将
Xk∣k=Xk∣k−1+Kk(Zk−Zk^)=Xk∣k−1+Kk(Zk−HXk∣k−1)代入
Pk∣k：
Pk∣k=E([Xk−Xk∣k−1−Kk(Zk−HXk∣k−1)][Xk−Xk∣k−1−Kk(Zk−HXk∣k−1)]T)
Pk∣k=E([Xk−Xk∣k−1−Kk(HXk+Vk−HXk∣k−1)][Xk−Xk∣k−1−Kk(HXk+Vk−HXk∣k−1)]T)
Pk∣k=E([(I−KkH)(Xk−Xk∣k−1)−KkVk][(I−KkH)(Xk−Xk∣k−1)−KkVk]T)
Pk∣k=E([(I−KkH)(Xk−Xk∣k−1)−KkVk][((I−KkH)(Xk−Xk∣k−1))T−(KkVk)T])
Pk∣k=E([(I−KkH)(Xk−Xk∣k−1)−KkVk][(Xk−Xk∣k−1)T(I−KkH)T−VkTKkT])
注意到系统状态变量和测量噪声之间是相互独立的，所以有：
Pk∣k=E([I−KkH][Xk−Xk∣k−1][Xk−Xk∣k−1]T[I−KkH]T+KkVkVkTKkT)
Pk∣k=(I−KkH)E((Xk−Xk∣k−1)(Xk−Xk∣k−1)T)(I−KkH)T+KkE(VkVkT)KkT
设
E(VkVkT)为
R，是系统测量噪声的协方差矩阵，有：
Pk∣k=(I−KkH)Pk∣k−1(I−KkH)T+KkRKkT
Pk∣k=Pk∣k−1−Pk∣k−1HTKkT−KkHPk∣k−1+Kk(HPk∣k−1HT+R)KkT
因为协方差矩阵对角线的值的和就是方差的和，所以对
Pk∣k求矩阵的迹：
一个
n∗n的矩阵
A的迹，是指
A主对角线上各元素的总和，记做
tr(A)，
有定理
tr(AB)=tr(BA)，
有定理
d(X)d(tr(XB))=d(X)d(tr(BX))=BT，
有定理
d(X)d(tr(AXBXT))=AXB+ATXBT
所以：
tr(Pk∣k)=tr(Pk∣k−1)−2∗tr(KkHPk∣k−1)+tr(Kk(HPk∣k−1HT+R)KkT)
因为要求
Kk的值，使得
tr(Pk∣k)最小，所以对
tr(Pk∣k)关于
Kk求导，令其导数为零，有：
d(Kk)d(tr(Pk∣k))=−2Pk∣k−1HT+Kk(HPk∣k−1HT+R)+Kk(HPk∣k−1HT+R)T=0
卡尔曼增益方程：Kk=HPk∣k−1HT+RPk∣k−1HT，这是卡尔曼滤波器的第三个方程，卡尔曼增益方程。
注意，
Pk∣k−1越大，
Kk就越大，权重将更加重视反馈，如果
Pk∣k−1等于0，也就是预测值和真实值相等，那么
Kk等于0，最优估计值等于预测值。
将
Kk反代入
Pk∣k中，有：
Pk∣k=Pk∣k−1−Pk∣k−1HTKkT−KkHPk∣k−1+Kk(HPk∣k−1HT+R)KkT
Pk∣k=Pk∣k−1−HPk∣k−1HT+RPk∣k−1HTHPk∣k−1
Pk∣k=Pk∣k−1−KkHPk∣k−1
更新协方差方程：Pk∣k=(I−KkH)Pk∣k−1，这是卡尔曼滤波器的第四个方程，更新协方差方程。
要计算卡尔曼增益，
H和
R都是已知或是固定的，那么如何求
Pk∣k−1呢？和
Xk∣k、Xk∣k−1一样，也是一个迭代的过程：
Pk∣k−1=E((Xk−Xk∣k−1)(Xk−Xk∣k−1)T)
Xk∣k−1=FXk−1∣k−1
另外，有：
Xk=FXk−1+W，
W是系统过程的噪声
Pk∣k−1=E((FXk−1+W−FXk−1∣k−1)(FXk−1+W−FXk−1∣k−1)T)
Pk∣k−1=E([F(Xk−1−Xk−1∣k−1)+W][F(Xk−1−Xk−1∣k−1)+W]T)
注意到系统状态变量和噪声之间是独立的，所以有：
Pk∣k−1=E([F(Xk−1−Xk−1∣k−1)][F(Xk−1−Xk−1∣k−1)]T)+E(WWT)
设
E(WWT)为
Q，是系统过程噪声的协方差矩阵，
预测协方差方程：Pk∣k−1=FPk−1∣k−1FT+Q，这是卡尔曼滤波器的第五个方程，预测协方差方程
到这里，卡尔曼滤波器的五个方程全部给出，推导完毕。
总结
卡尔曼滤波器的五个方程：
-
预测状态方程：Xk∣k−1=FXk−1∣k−1
-
预测协方差方程：Pk∣k−1=FPk−1∣k−1FT+Q
-
卡尔曼增益方程：Kk=HPk∣k−1HT+RPk∣k−1HT
-
更新最优值方程：Xk∣k=Xk∣k−1+Kk(Zk−Zk^)=Xk∣k−1+Kk(Zk−HXk∣k−1)
-
更新协方差方程：Pk∣k=(I−KkH)Pk∣k−1
其中
motion矩阵
F和
update矩阵
H，根据实际问题情景给出。系统测量噪声的协方差矩阵
R和系统过程噪声的协方差矩阵
Q，也作为已知条件设置为定值。
所以，确定了最早的系统变量的协方差矩阵
P0，可以得到预测协方差，然后得到卡尔曼增益，然后通过系统变量的预测值和测量值得到最优的估计值，并通过卡尔曼增益计算出下面的协方差矩阵。这是一个递推的过程。
结语
如果您有修改意见或问题，欢迎留言或者通过邮箱和我联系。
手打很辛苦，如果我的文章对您有帮助，转载请注明出处。
