Kalman滤波在运动跟踪中的建模.docx
《Kalman滤波在运动跟踪中的建模.docx》由会员分享，可在线阅读，更多相关《Kalman滤波在运动跟踪中的建模.docx（19页珍藏版）》请在冰点文库上搜索。
Kalman滤波在运动跟踪中的建模
Kalman滤波在运动跟踪中的应用
一、kalman滤波简介
最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。
从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。
为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。
卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：
采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。
它适合于实时处理和计算机运算。
Kalman滤波是卡尔曼（，把信号过程视为白噪声作用下的—个线性系统的输出，用状方程来描述这种输入—输出关系，估计过程中利用系统状态方程、观测方程和白噪声激励（系统噪声和观测噪声）的统计特性形成滤波算法，由于所用的信息都是时域内的量，所以不但可以对平稳的一维随机过程进估计，也可以对非平稳的、多维随机过程进行估汁。
Kalman滤波是一套由计算机实现的实时递推算法．它所处理的对象是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要估计值（系统的状态或参数）作为滤波器的输出，滤波器的输入与输出之间是由时间更新和观测更新算法联系在一起的，根据系统方程和观测方程估计出所有需要处理的信号。
所以，Kalman滤波与常规滤波的涵义与方法不同，它实质上是一种最优估计法。
卡尔曼滤波器是一个“optimalrecursivedataprocessingalgorithm（最优化自回归数据处理算法），对于解决很大部分的问题，他是最优，效率最高甚至是最有用的
二、kalman滤波基本原理
Kalman滤波器是目标状态估计算法解决状态最优估计的一种常用方法具有计算量小、存储量低、实时性高的优点。
实际应用中，可以将物理系统的运行过程看作是一个状态转换过程，卡尔曼滤波将状态空间理论引入到对物理系统的数学建模过程中来。
其基本思想是给系统信号和噪声的状态空间建立方程和观测方程，只用信号的前一个估计值和最近一个观察值就可以在线性无偏最小方差估计准则下对信号的当前值做出最优估计。
设一系统所建立的模型为：
状态方程：
个离散控制过程的系统，
X（k）=AX（k-1）+BU（k）+W（k）
观测方程：
系统的测量值
Z（k）=HX（k）+V（k）
上两式子中，X（k）是k时刻的系统状态，U（k）是k时刻对系统的控制量。
A和B是系统参数，对于多模型系统，他们为矩阵。
Z（k）是k时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。
W（k）和V（k）分别表示过程和测量的噪声。
他们被假设成高斯白噪声（WhiteGaussianNoise），他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。
对于满足上面的条件（线性随机微分系统，过程和测量都是高斯白噪声），卡尔曼滤波器是最优的信息处理器。
下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。
首先我们要利用系统的过程模型，来预测下一状态的系统。
假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X（k|k-1）=AX（k-1|k-1）+BU（k）………
（1）
式
（1）中，X（k|k-1）是利用上一状态预测的结果，X（k-1|k-1）是上一状态最优的结果，U（k）为现在状态的控制量，如果没有控制量，它可以为0。
到现在为止，我们的系统结果已经更新了，可是，对应于X（k|k-1）的covariance还没更新。
我们用P表示covariance：
P（k|k-1）=AP（k-1|k-1）A’+Q………
（2）
式
（2）中，P（k|k-1）是X（k|k-1）对应的covariance，P（k-1|k-1）是X（k-1|k-1）对应的covariance，A’表示 A的转置矩阵，Q是系统过程的covariance。
式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。
结合预测值和测量值，我们可以得到现在状态（k）的最优化估算值X（k|k）：
X（k|k）=X（k|k-1）+Kg（k）（Z（k）-HX（k|k-1））………（3）
其中Kg为卡尔曼增益（KalmanGain）：
Kg（k）=P（k|k-1）H’/（HP（k|k-1）H’+R）………（4）
到现在为止，我们已经得到了k状态下最优的估算值X（k|k）。
但是为了要使卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X（k|k）的covariance：
P（k|k）=（I-Kg（k）H）P（k|k-1）………（5）
其中I为1的矩阵，对于单模型单测量，I=1。
当系统进入k+1状态时，P（k|k）就是式子
（2）的P（k-1|k-1）。
这样，算法就可以自回归的运算下去。
总结以上内容克制，Kalman滤波器实现的主要五个方程为：
（1）状态向量预报方程
（2）状态向量协方差预报方程
（3）Kalman加权矩阵（或增益矩阵）
（4）状态向量更新方程
（5）状态向量协方差更新方程
Kalman滤波预估计就是用前面两个时间更新方程获得先验估计然后通过后面三个状态更新方程对先验估计矫正获得最优估计。
根据这5个公式，可以很容易的实现计算机的程序。
卡尔曼滤波的算法流程为：
1.预估计
X（k）^=F（k,k-1）·X（k-1）
2.计算预估计协方差矩阵
C（k）^=F（k,k-1）×C（k）×F（k,k-1）'+T（k,k-1）×Q（k）×T（k,k-1）'
Q（k）=U（k）×U（k）'
3.计算卡尔曼增益矩阵
K（k）=C（k）^×H（k）'×[H（k）×C（k）^×H（k）'+R（k）]^（-1）
R（k）=N（k）×N（k）'
4.更新估计
X（k）~=X（k）^+K（k）×[Y（k）-H（k）×X（k）^]
5.计算更新后估计协防差矩阵
6.C（k）~=[I-K（k）×H（k）]×C（k）^×[I-K（k）×H（k）]'+K（k）×R（k）×K（k）'
7.X（k+1）=X（k）~
C（k+1）=C（k）
重复以上六个步骤。
三、Kalman滤波在运动跟踪中的应用的建模
卡尔曼滤波器是一个对动态系统的状态序列进行线性最小误差估计的算法，一般用于线性系统。
一般在运动跟踪领域中摄像机相对于目标物体运动有时属于非线性系统，但由于在一般运动跟踪问题中图像采集时间间隔较短，可近似将单位时间内目标在图像中的运动看作匀速运动，采用卡尔曼滤波器可以实现对目标运动参数的估计。
对于复杂背景下运动目标识别与跟踪问题，要实现实时控制，对算法的实时性和准确性都有较高的要求。
如果目标识别算法都是基于像素的全局搜索，则存在显著缺点：
1）全局搜索计算量大、耗时，实时性无法满足；2）全局搜索抗干扰能力差，容易受到背景中相似特征物体的干扰。
基于卡尔曼滤波器预测功能的运动目标快速跟踪算法可以通过预测目标物体在下一帧中的位置，将全局搜索问题转化为局部搜索，缩小搜索范围，提高算法的实时性。
在跟踪运动目标的过程中，由于目标物体单位时间内在图像中的运动可以看作匀速运动，所以可以采用目标某一时刻在图像中的位置和速度来表示目标的运动状态。
为了简化算法的计算复杂度，可以设计2个卡尔曼滤波器分别描述目标在X轴和Y轴方向上位置和速度的变化。
下面仅讨论X轴方向上卡尔曼滤波器的实现过程，Y轴方向上同理。
目标物体运动方程为：
式中
，
，
分别为目标在t=k时刻的X轴方向的位置、速度和加速度；T为k帧图像和k+1帧图像之间的时间间隔，
可以当作白噪声处理。
写成矩阵形式如下：
系统状态方程为：
卡尔曼滤波器系统状态矢量为：
状态转移矩阵为：
系统动态噪声矢量为：
系统观测方程为：
卡尔曼滤波器系统观测矢量为：
观测系数矩阵为：
由观测方程可知，观测噪声为0，所以
=0。
建立了上述系统状态方程和观测方程之后，就可以利用卡尔曼滤波方程式通过递推方法，不断预测目标在下一帧中的位置。
在t=k时刻，对第k帧图像利用目标识别算法识别出的目标位置记为
，当目标首次出现时，根据此时目标的观测位置初始化滤波器
=[
，0]。
系统初始状态向量协方差矩阵可以在对角线上取较大值，取值根据实际测量情况来获得，但在滤波启动一段时间后影响就不大了。
取：
。
系统动态噪声协方差为
，可取为：
。
通过公式
（1），计算得到目标在下一帧图像中的预测位置
。
在该位置附近，对下一帧图像进行局部搜索，识别出的目标质心位置即为
，通过公式
（2）至公式（5）完成对状态向量和状态向量协方差矩阵的更新，为目标位置的下一步预测做好准备，得出新的预测位置
，采用图像处理算法，在该位置进行局部搜索，从而得出新的目标质心位置
，一直迭代计算下去，从而实现对目标物体的跟踪。
简化计算的到以下的实际编程公式：
（1）状态向量预报方程
简化：
Xy=Xb1+Vb1*T;
Vy=Vb1;
（2）状态向量协方差预报方程
简化：
Py=
=
*
*
+Q
Py1=（Pbb1+Pbb3*T）+（Pbb2+Pbb4）*T;
Py2=Pbb2+Pbb4*T;
Py3=Pbb3+Pbb4*T;
Py4=Pbb4;
（3）Kalman加权矩阵（或增益矩阵）
简化：
Kg=
Kg1=Py1/（Py1+R）
Kg2=Py3/（Py1+R）
（4）状态向量更新方程
简化：
Xb=Xy+Kg1（Xnew+Xy）;
b=Vy+Kg2（Xnew+Xy）;
（5）状态向量协方差更新方程
简化：
Pb1=（1-Kg1）*Py1-Kg1Py3;
Pb2=（1-Kg1）*Py2-Kg1Py4;
Pb3=（1-Kg1）*Py3-Kg1Py1;
Pb3=（1-Kg1）*Py4-Kg1Py2;
四、仿真结果
1、kalman的滤波效果
对一个定值函数y=25，加高斯白噪声噪声之后，经卡拉曼滤波可得到的检测对比加噪声的之后的信号（测量信号）和经过kalman滤波后的信号（滤波后的信号），对比两个信号。
从图1的滤波结果中可以看出，经过kalman滤波之后，信号基本上与加高斯白噪声之前的信号一直，误差大大减少，由此可以看出，kalaman滤波对于白噪声有比较好的滤除作用。
图1kalaman滤波滤除信号中的白噪声
2、简单轨迹的kalman的预测效果
使用基本的kalman滤波方法，预测下一个x的位置。
建立模型的方法是：
将目标物体在T时间内的运动可以看作匀速运动，采用目标某一时刻在图像中的位置和速度来表示目标的运动状态。
为了简化算法的计算复杂度，使用两个个卡尔曼滤波器分别描述目标在X轴和Y轴方向上位置和速度的变化，两个滤波器的原理相同。
仿真的结果如下：
左边的运动的轨迹，右边的是kalman的预测的误差。
图2简单轨迹的kalman的预测效果
可以看出当物体匀速运动的时候，kalman的预测比较准确；当物体改变速度的幅度比较大的时候，这是因为速度变化较大的时候，表明加速度的变化比较大，这样用之前的参数预测就会有较大的偏差，所以kalman的误差会比较明显。
3、椭圆运动轨迹的预测
下面是物体运动作椭圆运动的一个预测：
蓝色为实际轨迹，绿色为预测的轨迹，从x方向误差看，kalman的效果比较明显，y方向的预测效果就一般了，这个主要是跟kalman的的滤波参数选取有关，kalman是将运动的加速度看做白噪声处理的，参数的选取与加速度的方差有关。
现在用的参数是参考一些论文的设置，然后自己经过测试选取的。
速度变化大的时候就会预测不太准确，现在写的kalman还不能对速度变化自适应。
图3椭圆运动轨迹的预测
4、往返运动归轨迹的预测
kalman在窗口缩小情况下的跟踪效果，为了效果明显，将运动的轨迹做了调整，在改变速度、改变窗口大小的情况下，做了多组跟踪测试，下面是将其中一组测试速度变化较快、开窗较小的数据，轨迹如下图所示。
结果分析：
在速度变化比较大，开窗比较小的情况下，用kalman的丢帧次数会减少一些，（硬件测试观测的结果）；比较matlab分析的结果，用kalman的预测误差（绿线）比原始的帧间误差（蓝线）要小一些，遇到速度变化比较剧烈的时候，误差较大，易造成丢帧。
整体来说，经过kalman处理，预测数据与真实数据之间的误差有所减小，而且在运动不突变的情况下，预测也相对准确，这是kalman的优势。
（1）运动轨迹
（2）局部放大的轨迹
（3）X方向的误差分析
（4）Y方向的误差分析
图4kalman对往返运动的预测
五、参数的选取
建立模型，状态方程X（k）=AX（k-1）+BU（k）+W（k） 和观测方程Z（k）=HX（k）+V（k）。
式子中，X（k）是k时刻的系统状态，U（k）是k时刻对系统的控制量。
A和B是系统参数，对于多模型系统，他们为矩阵。
Z（k）是k时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。
W（k）和V（k）分别表示过程和测量的噪声。
他们被假设成高斯白噪声（WhiteGaussianNoise），他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。
需要选取的参数主要是一下三个：
初始观测位置：
当目标首次出现时，根据此时目标的观测位置初始化滤波器
=[
，0]。
初始位置的选取会影响到kalman在多少次运算之后才趋向于准确，因为kalman是迭代收敛来逐渐趋向于真实值的，如果开始的初始化位置与实际的位置相差比较大，就需要多迭代几次，这时需要注意的是前几帧的数据将会有较大的误差。
初始状态向量协方差矩阵：
系统初始状态向量协方差矩阵可以在对角线上取较大值，取值根据实际测量情况来获得，但在滤波启动一段时间后影响就不大了。
因为kalman会自行迭代产生新的向量协方差矩阵，直至达到最优。
取：
。
系统动态噪声协方差矩：
系统动态噪声协方差矩阵为
，这个矩阵主要是表征了噪声的特性，也就是加速度的变化特性，这里要根据实际的运动模型来选取，不同的状态下噪声的特性也不同。
可以一般来说，可取为：
。
也可以根据效果修改的到一个较好的参数。
附录：
Matlab程序：
function[X_next]=kalman_f（D_y,N,A,T,Q,H,I,R,Pb1,Xb1）;
fork=1:
N-1;
Y=D_y（k）;%Y=S（k）;%测量值Y更新输入数据Y=[S（k）,V（k）]';
Xy=A*Xb1;%系统的预测值前一时刻X的相关系数?
?
Py=A*Pb1*A'+Q;%Py（k）为X（k\k-1）状态的协方差
Kg=（Py*H'）*inv（H*Py*H'+R）;%卡尔曼增益Kg=（Py+H'）/（H*Py*H'+R）;R=0;
Xb=Xy+Kg*（Y-H*Xy）;%系统的最佳估计值即经过滤波后的信号
Pb=（I-Kg*H）*（Py）;%Pb（k）为X（k\k）状态的协方差即t状态下x（t|t）的相关系数
Xb1=Xb;%更新最佳估计值
Pb1=Pb;%更新误差
Sb（k+1）=Xb（1,1）;
Vb（k+1）=Xb（2,1）;
x_next=A*Xb1;
X_next（k+1）=x_next（1,1）;
End
C语言程序：
/*基本的kalman滤波，预测下一个x的位置；将目标物体在T时间内的运动可以看作匀速运动，采用目标某一时刻在图像中的位置和速度来表示目标的运动状态。
设计2个卡尔曼滤波器分别描述目标在X轴和Y轴方向上位置和速度的变化，为了简化算法的计算复杂度。
下面是讨论X轴方向上卡尔曼滤波器的实现过程，y轴方向上的相同.*/
#include
#include
#include"math.h"
//初始化参数
#defineT1//帧与帧间的时间间隔
#defineR0//观测误差矩阵
#defineQ00.002//系统动态噪声协方差
#defineQ10
#defineQ20
#defineQ30.002
inti;
intX_new;
floatX;
floatX_best,V_best;//预测值
floatX_y,V_y;//估计值
floatX_next;
intX_next1;
floatPy[4];//协方差预测矩阵
floatPb[4]={10,0,0,10};//状态向量协方差更新方程//初始化协方差矩阵
//volatileintPbb[4];
floatKg[2];
intkalman_1（intX_new,intV_new,intflag）//flag=0,第一次进入kalman
{
if（flag==0）//updatathenewnumber
{//initialization
X_best=X_new;//可设置为常数，则V_new就不需要传输了
V_best=V_new;
}
else
{
//状态向量预报方程
X_y=X_best+V_best*T;//iteration
V_y=V_best;
//协方差预测
Py[0]=（Pb[0]+Pb[2]*T）+（Pb[1]+Pb[3]*T）*T+Q0;
Py[1]=Pb[1]+Pb[3]*T+Q1;
Py[2]=Pb[2]+Pb[3]*T+Q2;
Py[3]=Pb[3]+Q3;
//kalman增益矩阵
Kg[0]=Py[0]/（Py[0]+R）;//Kg[0]=1;//
Kg[1]=Py[2]/（Py[0]+R）;//Kg[1]=1;//
//状态向量更新方程
X_best=X_y+Kg[0]*（X_new-X_y）;//forcastthebestx
V_best=V_y+Kg[1]*（X_new-X_y）;
//状态向量协方差更新方程
Pb[0]=（1-Kg[0]）*Py[0]-Kg[0]*Py[2];//Pb1=（1-Kg1）*Py1-Kg1Py3;
Pb[1]=（1-Kg[0]）*Py[1]-Kg[0]*Py[3];//Pb2=（1-Kg1）*Py2-Kg1Py4;
Pb[2]=（1-Kg[0]）*Py[2]-Kg[0]*Py[0];//Pb3=（1-Kg1）*Py3-Kg1Py1;
Pb[3]=（1-Kg[0]）*Py[3]-Kg[0]*Py[1];//Pb3=（1-Kg1）*Py4-Kg1Py2;
}
X_next=X_best+V_best*T;
X_next=（int）（X_next+0.5）;//四舍五入转化为整型数
if（X_next<0）//设定边界值，因为预测的值有可能超出边界，需要
{//约束一下范围
X_next=0;
}
returnX_next;
}
仅供个人用于学习、研究；不得用于商业用途。
Forpersonaluseonlyinstudyandresearch;notforcommercialuse.
NurfürdenpersönlichenfürStudien,Forschung,zukommerziellenZweckenverwendetwerden.
Pourl'étudeetlarechercheuniquementàdesfinspersonnelles;pasàdesfinscommerciales.
толькодлялюдей,которыеиспользуютсядляобучения,исследованийинедолжныиспользоватьсявкоммерческихцелях.
以下无正文
升级会员 