(整理)Kalman滤波在运动跟踪中的建模.

1、精品文档目录一、 kalman滤波简介 12、 kalman 滤波基本原理 13、 Kalman滤波在运动跟踪中的应用的建模 34、 仿真结果61、 kalman 的滤波效果 62、简单轨迹的kalman 的预测效果73、椭圆运动轨迹的预测94、往返运动归轨迹的预测 105、 参数的选取1 1附录： 13Matlab 程序： 13C语言程序： 13精品文档Kalman滤波在运动跟踪中的应用1、 kalman滤波简介最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家 等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。

2、为了克服这一缺点， 60 年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型， 利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。Kalman滤波是卡尔曼（R.E.kalman） 于 1960年提出的从与被提取信号的有关的观测量中通过算法估计出所需信号的一种滤波算法。他把状态空间的概念引入到随机估计理论中，把信号过程视为白噪声作用下的个线性系统的输出，用状方程来描述这种输入输出关

3、系，估计过程中利用系统状态方程、观测方程和白噪声激励（ 系统噪声和观测噪声） 的统计特性形成滤波算法，由于所用的信息都是时域内的量，所以不但可以对平稳的一维随机过程进估计，也可以对非平稳的、多维随机过程进行估汁。Kalman 滤波是一套由计算机实现的实时递推算法它所处理的对象是随机信号， 利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要估计值（ 系统的状态或参数） 作为滤波器的输出，滤波器的输入与输出之间是由时间更新和观测更新算法联系在一起的，根据系统方程和观测方程估计出所有需要处理的信号。所以，Kalman 滤波与常规滤波的涵义与方法不同，它实质上是一种最优估计法。卡

4、尔曼滤波器是一个“optimal recursive data processing algorithm （最优化自回归数据处理算法），对于解决很大部分的问题，他是最优，效率最高甚至是最有用的2、 kalman滤波基本原理Kalman 滤波器是目标状态估计算法解决状态最优估计的一种常用方法具有计算量小、存储量低、实时性高的优点。实际应用中，可以将物理系统的运行过程看作是一个状态转换过程，卡尔曼滤波将状态空间理论引入到对物理系统的数学建模过程中来。其基本思想是给系统信号和噪声的状态空间建立方程和观测方程， 只用信号的前一个估计值和最近一个观察值就可以在线性无偏最小方差估计准则下对信号的当前值做出

5、最优估计。设一系统所建立的模型为：状态方程：个离散控制过程的系统，X(k)=A X(k-1)+B U(k)+W(k)观测方程：系统的测量值Z(k)=H X(k)+V(k)上两式子中，X(k) 是 k时刻的系统状态，U(k)是 k 时刻对系统的控制量。A和 B 是系统参数，对于多模型系统，他们为矩阵。Z(k) 是 k 时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k) 分别表示过程和测量的 噪 声 。 他 们 被 假 设 成 高 斯 白 噪 声 (White Gaussian Noise) ， 他 们 的covariance 分别是Q， R(这里我们假设他们不随系统

6、状态变化而变化)。对于满足上面的条件( 线性随机微分系统，过程和测量都是高斯白噪声) ， 卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的 covariances 来估算系统的最优化输出(类似上一节那个温度的例子)。首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：X(k|k-1)=A X(k-1|k-1)+B U(k) (1)式 (1) 中， X(k|k-1) 是利用上一状态预测的结果，X(k-1|k-1) 是上一状态最优的结果，U(k) 为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，我

7、们的系统结果已经更新了，可是，对应于X(k|k-1) 的covariance 还没更新。我们用P表示covariance ：P(k|k-1)=A P(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) 的

8、最优化估算值X(k|k) ：X(k|k)= X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1)(3)其中 Kg 为卡尔曼增益(Kalman Gain) ：Kg(k)= P(k|k-1) H /(H P(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)就是式

9、子(2) 的 P(k-1|k-1) 。这样，算法就可以自回归的运算下去。总结以上内容克制，Kalman滤波器实现的主要五个方程为：(1) 状态向量预报方程X'k AXk 1(2) 状态向量协方差预报方程P'kAkPk 1AkT Qk 1(3)Kalman 加权矩阵 (或增益矩阵)KkPk'HkT(HkPk'HkTRk) 1(4) 状态向量更新方程XkX'k Kk(ZkHkX'k)(5) 状态向量协方差更新方程Pk(I KkHk)Pk'Kalman 滤波预估计就是用前面两个时间更新方程获得先验估计然后通过后面三个状态更新方程对先验估计矫正获

10、得最优估计。根据这 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)&#

11、215; N(k)'4. 更新估计X(k)=X(k)+K(k×) Y(k)-H(k)× X(k)5. 计算更新后估计协防差矩阵C(k) = I-K(k×) H(k)× C(k)× I-K(k)× H(k)'+K(k)× R(k)× K(k)'6. X(k+1) = X(k)C(k+1) = C(k)重复以上六个步骤。三、 Kalman滤波在运动跟踪中的应用的建模卡尔曼滤波器是一个对动态系统的状态序列进行线性最小误差估计的算法，一般用于线性系统。一般在运动跟踪领域中摄像机相对于目标物体运动有时

12、属于非线性系统，但由于在一般运动跟踪问题中图像采集时间间隔较短，可近似将单位时间内目标在图像中的运动看作匀速运动，采用卡尔曼滤波器可以实现对目标运动参数的估计。对于复杂背景下运动目标识别与跟踪问题，要实现实时控制，对算法的实时性和准确性都有较高的要求。如果目标识别算法都是基于像素的全局搜索，则存在显著缺点：1) 全局搜索计算量大、耗时，实时性无法满足；2) 全局搜索抗干扰能力差， 容易受到背景中相似特征物体的干扰。基于卡尔曼滤波器预测功能的运动目标快速跟踪算法可以通过预测目标物体在下一帧中的位置，将全局搜索问题转化为局部搜索，缩小搜索范围，提高算法的实时性。在跟踪运动目标的过程中，由于目标物体

13、单位时间内在图像中的运动可以看作匀速运动，所以可以采用目标某一时刻在图像中的位置和速度来表示目标的运动状态。 为了简化算法的计算复杂度，可以设计2 个卡尔曼滤波器分别描述目标在 X轴和Y轴方向上位置和速度的变化。下面仅讨论X轴方向上卡尔曼滤波器的实现过程，Y轴方向上同理。目标物体运动方程为：xk 1 xk vkTvk 1 vkakT式中xk ， vk ， ak分别为目标在t=k 时刻的 X轴方向的位置、速度和加速度；T为k 帧图像和k+1 帧图像之间的时间间隔，akT 可以当作白噪声处理。写 成矩阵形式如下：Xk xkvkT系统状态方程为：卡尔曼滤波器系统状态矢量为：状态转移矩阵为：H ( k

14、)系统动态噪声矢量为：0系统观测方程为：xkxk 11 0vk卡尔曼滤波器系统观测矢量为：Zkxk观测系数矩阵为：Hk 1 0由观测方程可知，观测噪声为0，所以Rk =0。建立了上述系统状态方程和观测方程之后，就可以利用卡尔曼滤波方程式通过递推方法，不断预测目标在下一帧中的位置。在 t=k 时刻， 对第 k 帧图像利用目标识别算法识别出的目标位置记为xk ，当目标首次出现时，根据此时目标的观测位置初始化滤波器X'0= x0， 0。取值根据实际测量X '1 。 在该位置附近，系统初始状态向量协方差矩阵可以在对角线上取较大值，情况来获得，但在滤波启动一段时间后影响就不大了。取：10

15、 0P000 10 。系统动态噪声协方差为Q0 ，可取为：10 0Q000 10 。通过公式 (1) ， 计算得到目标在下一帧图像中的预测位置对下一帧图像进行局部搜索，识别出的目标质心位置即为Z 1，通过公式(2) 至公式 (5) 完成对状态向量和状态向量协方差矩阵的更新，为目标位置的下一步预测做好准备，得出新的预测位置X '2， 采用图像处理算法，在该位置进行局部搜索，从而得出新的目标质心位置Z 2，一直迭代计算下去，从而实现对目标物体的跟踪。简化计算的到以下的实际编程公式：(1) 状态向量预报方程X' k AX k 1简化： Xy=Xb1+ Vb1*T ;Vy=Vb1 ;(

16、2) 状态向量协方差预报方程P'k AkPk 1AkT Qk 1简化：Py=10Py1Py2 1TP(b1)1P(b1)2Py3Py4= 01P(b1)3P(b1)4T 1Py3 Py4+QPy1= (Pbb1+ Pbb3*T)+(Pbb2+Pbb4) *T;Py2= Pbb2+Pbb4*T;Py3= Pbb3+Pbb4*T;Py4= Pbb4;(3)Kalman 加权矩阵(或增益矩阵)KkPk'HkT(HkPk'HkT Rk) 1Kg1简化：Kg=Kg2Kg1=Py1/( Py1+R)Kg2= Py3/( Py1+R)(4) 状态向量更新方程XkX'k Kk(

17、Zk HkX'k )简化： Xb=Xy+ Kg1(Xnew+ Xy) ;b=Vy+ Kg2(Xnew+ Xy) ;(5) 状态向量协方差更新方程Pk ( I Kk H k )Pk'简化： Pb1=(1- Kg1)* Py1- Kg1 Py3;Pb2=(1- Kg1)* Py2- Kg1 Py4;Pb3=(1- Kg1)* Py3- Kg1 Py1;Pb3=(1- Kg1)* Py4- Kg1 Py2;4、 仿真结果1、 kalman 的滤波效果对一个定值函数y=25， 加高斯白噪声噪声之后，经卡拉曼滤波可得到的检测对比加噪声的之后的信号（测量信号）和经过kalman 滤波后的信

18、号（滤波后的信号） ，对比两个信号。从图1 的滤波结果中可以看出，经过kalman 滤波之后，信号基本上与加高斯白噪声之前的信号一直，误差大大减少，由此可以看出，kalaman 滤波对于白噪声有比较好的滤除作用。图 1 kalaman 滤波滤除信号中的白噪声2、简单轨迹的kalman 的预测效果使用基本的kalman 滤波方法，预测下一个x 的位置。建立模型的方法是：将目标物体在T 时间内的运动可以看作匀速运动，采用目标某一时刻在图像中的位置和速度来表示目标的运动状态。为了简化算法的计算复杂度，使用两个个卡尔曼滤波器分别描述目标在X轴和 Y轴方向上位置和速度的变化，两个滤波器的原理相同。仿真的

19、结果如下：左边的运动的轨迹，右边的是kalman 的预测的误差。图 2 简单轨迹的kalman 的预测效果可以看出当物体匀速运动的时候，kalman 的预测比较准确；当物体改变速度的幅度比较大的时候，这是因为速度变化较大的时候，表明加速度的变化比较大，这样用之前的参数预测就会有较大的偏差，所以kalman 的误差会比较明显。3、椭圆运动轨迹的预测下面是物体运动作椭圆运动的一个预测：蓝色为实际轨迹，绿色为预测的轨迹， 从 x 方向误差看，kalman 的效果比较明显，y 方向的预测效果就一般了，这个主要是跟kalman 的的滤波参数选取有关，kalman 是将运动的加速度看做白噪声处理的，参数的

20、选取与加速度的方差有关。现在用的参数是参考一些论文的设置，然后自己经过测试选取的。速度变化大的时候就会预测不太准确，现在写的kalman 还不能对速度变化自适应。图 3 椭圆运动轨迹的预测4、往返运动归轨迹的预测kalman 在窗口缩小情况下的跟踪效果，为了效果明显，将运动的轨迹做了调整，在改变速度、改变窗口大小的情况下，做了多组跟踪测试，下面是将其中一组测试速度变化较快、开窗较小的数据，轨迹如下图所示。结果分析：在速度变化比较大，开窗比较小的情况下，用kalman 的丢帧次数会减少一些，（硬件测试观测的结果）；比较 matlab 分析的结果，用kalman的预测误差（绿线）比原始的帧间误差（

21、蓝线）要小一些，遇到速度变化比较剧烈的时候，误差较大，易造成丢帧。整体来说，经过kalman 处理，预测数据与真实数据之间的误差有所减小，而且在运动不突变的情况下，预测也相对准确，这是 kalman 的优势。1) 运动轨迹( 2) 局部放大的轨迹3)X 方向的误差分析( 4) Y 方向的误差分析4 kalman 对往返运动的预测5、 参数的选取建立模型，状态方程X(k)=A X(k-1)+B U(k)+W(k) 和观测方程Z(k)=HX(k)+V(k) 。 式子中， X(k) 是 k 时刻的系统状态，U(k) 是 k 时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)

22、是 k 时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。W(k)和 V(k) 分别表示过程和测量的 噪 声 。 他 们 被 假 设 成 高 斯 白 噪 声 (White Gaussian Noise) ， 他 们 的 covariance 分别是Q， R(这里我们假设他们不随系统状态变化而变化)。需要选取的参数主要是一下三个：初始观测位置：当目标首次出现时，根据此时目标的观测位置初始化滤波器X'0= x0， 0。初始位置的选取会影响到kalman 在多少次运算之后才趋向于准确，因为kalman 是迭代收敛来逐渐趋向于真实值的，如果开始的初始化位置与实际的位置相差比较大，就

23、需要多迭代几次，这时需要注意的是前几帧的数据将会有较大的误差。初始状态向量协方差矩阵：系统初始状态向量协方差矩阵可以在对角线上取较大值， 取值根据实际测量情况来获得，但在滤波启动一段时间后影响就不大了。因为 kalman 会自行迭代产生新的向量协方差矩阵，直至达到最优。取：10 0P000 10 。系统动态噪声协方差矩：系统动态噪声协方差矩阵为Q0，这个矩阵主要是表征了噪声的特性，也就是加速度的变化特性，这里要根据实际的运动模型来选10 0 Q0取，不同的状态下噪声的特性也不同。可以一般来说，可取为：0 10 。也可以根据效果修改的到一个较好的参数。附录 ：Matlab 程序 ：functio

24、n X_next= kalman_f(D_y,N,A,T,Q,H,I,R,Pb1,Xb1);for k=1:N-1;Y=D_y(k);%Y=S(k);%测量值Y 更新输入数据Y=S(k),V(k)'Xy=A*Xb1;Py=A*Pb1*A'+Q;%系统的预测值前一时刻X 的相关系数?%Py(k)为 X(kk-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);%P

25、b(k)为 X(kk)状态的协方差即 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);EndC 语言程序：/* 基本的 kalman 滤波，预测下一个x的位置；将目标物体在T时间内的运动可以看作匀速运动， 采用目标某一时刻在图像中的位置和速度来表示目标的运动状态。设计 2 个卡尔曼滤波器分别描述目标在X轴和Y轴方向上位置和速度的变化，为了简化算法的计算复杂度。下面是讨论X 轴方向上卡尔曼滤波器的实现过程，y 轴方向

26、上的相同.*/#include <stdio.h>#include <stdlib.h>#include "math.h"/ 初始化参数#define T 1 / 帧与帧间的时间间隔#define R 0 /观测误差矩阵#define Q0 0.002 /系统动态噪声协方差#define Q1 0#define Q2 0#define Q3 0.002int i;int X_new;float X;float X_best,V_best;/预测值float X_y,V_y; / 估计值float X_next;int X_next1;float Py4; / 协方差预测矩阵float Pb4=10,0,0,10; /