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

合集下载

基于Kalman滤波算法的运动目标跟踪系统建模与仿真

基于Kalman滤波算法的运动目标跟踪系统建模与仿真甘志英【摘要】鉴于传统的跟踪算法鲁棒性和实时性不足,构建了一种基于Kalman滤波的运动目标跟踪系统.对于运动目标的跟踪,此系统先采用背景差分法检测目标位置,再使用Kalman滤波算法估计目标位置.通过建模仿真,结果表明,该算法能较好预测运动目标的位置,实现了对运动目标的实时跟踪.【期刊名称】《唐山学院学报》【年(卷),期】2018(031)006【总页数】4页(P45-48)【关键词】运动目标跟踪系统;背景差分法;Kalman滤波算法【作者】甘志英【作者单位】唐山学院智能与信息工程学院,河北唐山 063000【正文语种】中文【中图分类】TP391.410 引言运动目标跟踪是机器视觉领域研究的热点与难点。

对运动目标进行跟踪不仅可以提供目标的运动状态和轨迹，也为运动分析、场景理解等提供可靠的数据来源，因此运动目标跟踪系统在智能监控、突发事件检测等方面有着重要的应用。

运动目标跟踪系统一般包括两部分，一是运动目标检测，二是运动目标跟踪[1]。

传统的运动目标检测算法有三种：光流法、背景差分法、帧间差分法[2-4]，其中背景差分法是目前运动目标检测技术中应用最广泛的一种方法。

常用的运动目标跟踪算法主要有基于目标轮廓的跟踪和基于目标特征的跟踪，前者利用目标的边界信息来实现跟踪，可有效克服干扰及遮挡问题，但易受跟踪的初始化影响[5]；而后者利用目标的特征进行匹配，在目标部分遮挡时，利用部分特征仍能进行跟踪，常用的特征有目标质心、颜色特征、角点等[6]。

针对传统跟踪算法鲁棒性和实时性不足的问题，本文基于kalman滤波算法构建一种运动目标跟踪系统，此系统使用背景差分法检测运动目标，通过Kalman滤波预测目标下一状态的位置，并根据当前检测结果，更新Kalman滤波状态。

1 背景差分法背景差分法是利用视频序列中不含前景目标的帧图像作为背景，将各帧图像的像素值与背景的像素值相减，得到差值图像，再进行二值化运算即得到目标。

Kalman滤波的人体运动位置跟踪算法

第24卷　第3期华侨大学学报(自然科学版)V o l.24　N o.3 2003年7月Jou rnal of H uaqiao U n iversity(N atu ral Science)Ju l.2003　　文章编号　100025013(2003)0320254203Ka l man滤波的人体运动位置跟踪算法黄　建　新(华侨大学数学系,福建泉州362011)摘要　基于视频的人体运动跟踪是当前计算机视觉研究的热点,具有广泛的应用领域.文中提出一种基于Kal m an滤波的跟踪算法.合理使用自适应背景颜色模型,能够准确地对人体运动位置进行跟踪.关键词　人体运动跟踪,Kal m an滤波,自适应背景颜色模型中图分类号　T P391.41∶TN713文献标识码　A跟踪视频中的运动人体对象是计算机视觉研究的活跃领域之一,它可以广泛地应用在计算机动画、互动游戏、实时监控和虚拟现实等领域.目前,人们提出的一些算法如光流法〔1〕,只能对背景光线亮度不变的情况下的人体位置进行跟踪.本文提出的算法,可以有效地解决在背景亮度发生变化的条件下人体运动位置跟踪的问题.它利用人体运动约束方程使用Kal m an 滤波器〔2～3〕和自适应背景颜色模型〔4〕.1　自适应背景颜色模型我们对图象中属于背景的象素颜色模型采用高斯模型,即[Λr,Λg,Λb,Ρ2r,Ρ2g,Ρ2b],其中Λr,Λg和Λb分别是象素R GB的均值,Ρ2r,Ρ2g,Ρ2b是它们的方差.在跟踪开始时,我们录入一些不含任何运动人体的纯背景图象,来计算这些模型参数的初始值.然后,使用简单的递归方法计算下一帧中每一个象素的模型参数值.更新算法为Λr(k+1)=Αr+(1-Α)Λr(k),(1)Ρ2r(k+1)=Α(r-Λr(k))2+(1-Α)Ρ2r(k).(2)而Λg,Λb,Ρ2g和Ρ2b的更新算法跟上述一样.在递归计算出某一帧的背景模型之后,即作前景提取算法.若 r-Λr >Ρr,则该象素属于前景.同样地,如果g和b值满足类似的上述条件,则该象素也视为前景.2　使用Ka l man滤波进行预测我们使用矩形框表示被跟踪的运动人体对象区域.描述矩形框的参数使用中心点坐标　收稿日期　2002211217　作者简介　黄建新(19692),男,讲师,E2m ail:jc huang@　基金项目　华侨大学科研基金资助项目(02H ZR13)[x Cen ter ,y Cen ter ]、宽度W idth 和高度H eigh t .在跟踪过程中,我们将它看作一个线性动态系统.为了描述中心点的运动情况,这里使用中心点在x 和y 方向上的位移偏差∃x 和∃y .所以,状态向量定义为S =[∃x ,∃y ,W idth ,H eigh t ].Kal m an 滤波的状态方程定义为S (k +1)=F S (k )+Γ(k ).(3)观测方程为Z (k )=H S (k )+W (k ),(4)F =10000100001001, H =100001000010001.状态预测方程为S b (k )=F S a (k -1)(5)上述方程中下标a 和b 分别表示“修改以前”和“修改以后”,它们在下面方程的含义也一样.协方差预测方程为P b (k )=F (k 1)P a (k -1)F (k -1)T+Q (k ).(6)Kal m an 增益方程为K (k )=P b (k )HT(k )(H (k )P b (k )H T (k )+R (k ))-1.(7)状态修改方程为S a(k )=S b (k )+K (k )[Z (k )-H (k )S b (k )].(8)协方差修改方程为P a(k )=P b (k )-K (k )H (k )P b (k ).(9)3　跟踪实现算法根据上述思想,跟踪算法如下有6个步骤.(1)步骤1.首先设置方差Q ,R 和协方差P 的初始值.(2)步骤2.根据Kal m an 滤波,预测第k +1帧的状态向量.根据预测的(∃x ,∃y )和已知的第k 帧中心点坐标值,求出第k +1帧矩形框的中心点预测坐标值.(3)步骤3.根据方程(1),(2),计算第k +1帧的背景颜色模型.(4)步骤4.对已预测的第k +1帧中矩形框的参数值[x Cen ter ,y Cen ter ,W idth ,H eigh t ]进修正,即对人体位置进行精确地定位,求出它的观察值Z (k +1).修正方法以矩形框顶边为例.根据上述提取前景的方法,计算顶边中属于前景的象素数目.如果它小于(或大于)给定的阈值,则顶边向下(或向上)移动一个象素的位置,直至属于前景的象素数目大于(或小于)给定的阈值.其他边也进行类似地处理.经过这种修正方法获得的矩形框所覆盖的范围就是要跟踪的人体位置.(5)步骤5.将第k +1帧的观察值Z (k +1)代入方程(6),(7),(8)和(9)中.(6)步骤6.如果继续跟踪下一帧,则返回步骤2.4　实验结果我们使用一台普通的摄像机在实验室里拍摄一段视频,应用上述的算法进行跟踪.跟踪结552第3期黄建新:Kal m an 滤波的人体运动位置跟踪算法 652 华侨大学学报(自然科学版) 2003年果如图1所示.(a)第4帧的跟踪结果(b)第30帧的跟踪结果图1　图像系列中人体运动位置跟踪结果5　结束语实验证明,本文提出的人体运动跟踪算法具有较准确的跟踪结果和速度,能够被应用在复杂背景下人体运动位置的跟踪.我们下一步工作就是利用跟踪结果进行人体运动行为分析.参考文献1　T ekalp A M著.数字视频处理[M].崔之祜等译.重庆:电子工业出版社,1990.1～502　W ren C R,Jan i A A,D arrel T,et al.P rinder:R eal2ti m e track ing of the hum an body[J].IEEE PAM I, 1997,19(7):780～7853　罗忠祥,庄越挺,潘云鹤等.基于双摄像机的视频特征跟踪算法研究[J].计算机辅助设计与图形学学报, 2002,14(7):646～6494　A k ita K.I m age sequence analysis of real w o rld hum an mo ti on[J].Pattern R ecogn iti on,1984,17(1):73～83A Track i ng A lgor ithm Ba sed on Ka l man F iltra ton forTrack i ng K i nema tic Position of Hu man BodyH uang J ianx in(D ep t.of M ath.,H uaqiao U niv.,362011,Q uanzhou,Ch ina)Abstract　T rack ing the mo ti on of hum an body based on video is one of ho t spo ts in compu ter visi on re2 search.It has a w ide app licati on.T he au tho r p resen ts here a track ing algo rithm based on Kal m an filtrati on. By u sing a model of adap tive background co lo r,the k inem atic po siti on of hum an body can be exactly tracked. Keywords　track ing the mo ti on of hum an body,Kal m an filtrati on,model of adap tive background co lo r。

卡尔曼滤波在跟踪中的应用

卡尔曼滤波在跟踪中的应用
卡尔曼滤波在跟踪中的应用非常广泛。

例如，在目标跟踪中，卡尔曼滤波可以融合多个传感器的测量数据，准确地估计目标的状态，并实现对目标运动轨迹的跟踪。

卡尔曼滤波假设目标的状态和观测值都服从高斯分布，并利用贝叶斯定理不断更新目标状态的估计值。

具体而言，卡尔曼滤波包括两个主要步骤：预测和更新。

在预测步骤中，卡尔曼滤波器使用目标的先验状态来预测目标的下一个状态。

这涉及到使用目标的运动模型和传感器噪声模型来预测目标的下一个位置和速度。

在更新步骤中，卡尔曼滤波器使用目标的观测数据来更新对目标状态的估计。

这涉及到对目标的观测模型进行建模，并使用该模型来计算出新的状态估计值。

总的来说，卡尔曼滤波是一种强大的工具，可以用于处理存在不确定性的动态系统的状态估计问题。

在目标跟踪领域中，卡尔曼滤波被广泛应用于各种场景，如无人驾驶汽车、无人机跟踪、人脸识别等。

加速度计陀螺仪Kalman滤波-建模及C代码

//float gyro_m：陀螺仪测得的量（角速度） //float incAngle:加速度计测得的角度值 #define dt 0.0015//卡尔曼滤波采样间隔 #define R_angle 0.69 //测量噪声的协方差 #define Q_angle 0.0001//角度噪声方差 #define Q_gyro 0.0003 //陀螺仪噪声方差 float kalmanUpdate(const float gyro_m,const float incAngle) { float K_0;//角度滤波增益 float K_1;//偏差滤波增益 float Y_0; float Y_1； float Rate;//去除偏差后的角速度 float Pdot[4];//计算状态预测协方差矩阵的中间矩阵 float angle_err;//角度偏量（新息） float E;//计算的中间量 static float angle = 0; //下时刻最优估计值角度 static float q_bias = 0; //陀螺仪的偏差（状态分量） static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//状态协方差矩阵 Rate = gyro_m - q_bias; // Kalman Time Update (状态更新) angle += Rate * dt; //作为预测角度 //计算预测协方差矩阵（红色） Pdot[0] = Q_angle - P[0][1] - P[1][0]; Pdot[1] = - P[1][1]; Pdot[2] = - P[1][1]; Pdot[3] = Q_gyro; P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; %% Innovation 信息序列 angle_err = incAngle - angle; //计算角度偏差 E = R_angle + P[0][0]; %% 滤波增益 K_0 = P[0][0] / E; K_1 = P[1][0[0]; Y_1 = P[0][1]; P[0][0] -= K_0 * Y_0; P[0][1] -= K_0 * Y_1; P[1][0] -= K_1 * Y_0; P[1][1] -= K_1 * Y_1; angle += K_0 * angle_err; //给出最优估计值 q_bias += K_1 * angle_err; //给出最优估计值偏差 return angle;}

卡尔曼滤波实现目标跟踪

卡尔曼滤波实现目标跟踪1.系统模型x_k=A_k*x_{k-1}+B_k*u_k+w_k其中，x_k是目标的状态向量，A_k是系统状态转移矩阵，表示目标从k-1时刻到k时刻状态的变化；B_k是控制输入矩阵，表示外部输入对目标状态的影响；u_k是控制输入向量，表示外部输入的值；w_k是过程噪声，表示系统模型的误差。

2.观测模型观测模型描述了如何根据目标状态得到观测值。

观测模型可以用下面的观测方程表示：z_k=H_k*x_k+v_k其中，z_k是观测值，H_k是观测矩阵，表示目标状态到观测值的映射关系；v_k是观测噪声，表示观测数据的误差。

3.初始化在开始跟踪之前，需要对目标的状态进行初始化。

可以根据已有的观测数据和模型来初始化状态向量和协方差矩阵。

4.预测步骤在预测步骤中，根据系统模型和上一时刻的状态估计，可以预测目标的下一时刻状态。

预测的状态估计由下面的方程给出：x_k^-=A_k*x_{k-1}+B_k*u_k其中，x_k^-是预测的状态估计值。

同时，还需要预测状态估计值的协方差矩阵，可以使用下面的方程计算：P_k^-=A_k*P_{k-1}*A_k^T+Q_k其中，P_k^-是预测的协方差矩阵，Q_k是过程噪声的协方差矩阵。

5.更新步骤在更新步骤中，根据观测数据来修正预测的状态估计。

首先，计算创新（innovation）或者观测残差：y_k=z_k-H_k*x_k^-其中，y_k是观测残差。

然后，计算创新的协方差矩阵：S_k=H_k*P_k^-*H_k^T+R_k其中，S_k是创新的协方差矩阵，R_k是观测噪声的协方差矩阵。

接下来，计算卡尔曼增益：K_k=P_k^-*H_k^T*S_k^-1最后，更新估计的目标状态和协方差矩阵：x_k=x_k^-+K_k*y_kP_k=(I-K_k*H_k)*P_k^-其中，I是单位矩阵。

6.重复预测和更新步骤重复进行预测和更新步骤，可以得到目标的状态估计序列和协方差矩阵序列。

卡尔曼滤波在GPS中的应用

本科毕业论文 (设计)题目：卡尔曼滤波在GPS定位中的应用学院：自动化工程学院专业：自动化姓名：指导教师：2010年 6月 4日The Application of Kalman Filtering for GPS Positioning摘要本文提出了一种应用卡尔曼滤波的GPS滤波模型。

目前在提高GPS定位精度的自主式方法研究领域，普遍采用卡尔曼滤波算法对GPS定位数据进行处理。

由于定位误差的存在,在GPS动态导航定位中，为提高定位精度，必须对动态定位数据进行滤波处理。

文中在比较分析各种动态模型的基础上，提出了应用卡尔曼滤波的GPS滤波模型，并通过对实测滤波算例仿真，证实了模型的可行性和有效性。

最后提出了卡尔曼滤波在GPS定位滤波应用中的问题和改进思路。

关键词 GPS 卡尔曼滤波定位误差AbstractThis article proposed applies the GPS filter model of the Kalman filtering. At present, to improve GPS positioning accuracy in the autonomous areas of research methods, we commonly use Kalman filter algorithm to process GPS location data.As a result of the position error existence in the GPS dynamic navigation localization, we must carry on filter processing to the dynamic localization data for the enhancement pointing accuracy．In the base of comparing each kind of dynamic model, this article proposed applies the GPS filter model of the Kalman filtering，the actual examples of filter calculation are simulated, it confirmed that the model is feasibility and validity. Finally, this article also proposed the existing problems and improving the idea ofthe applications of Kalman filter in GPS positioning.Keywords GPS Kalman filtering Positioning error目录前言 (1)第1章绪论 (3)1.1GPS的简介及应用 (3)1.2本课题的背景及意义 (5)1.3国内外研究动态及发展趋势 (7)1.4目前GPS定位系统面临着新的困扰和挑战 (5)第2章 GPS全球定位系统及GPS定位误差分析 (8)2.1GPS全球定位系统组成部分 (8)2.1.1 GPS卫星星座 (8)2.1.2 地面支持系统 (9)2.1.3 用户部分 (10)2.2GPS定位原理和测速原理 (16)2.2.1 卫星无源测距定位和伪距测量定位原理 (17)2.2.2 多普勒测量定位原理 (193)2.2.3 GPS测速原理 (214)2.3GPS定位误差分析 (225)2.3.1 星钟误差 (225)2.3.2 星历误差 (225)2.3.3 电离层和对流层的延迟误差 (236)2.3.4 多路径效应引起的误差 (246)2.3.5 接收设备误差 (246)2.3.6 GPS测速误差 (257)第3章卡尔曼滤波理论 (27)3.1卡尔曼滤波理论的工程背景 (27)3.2卡尔曼滤波理论 (28)第4章卡尔曼滤波在GPS定位中的应用 (34)4.1卡尔曼滤波在GPS定位中的应用概述 (34)4.2运动载体的动态模型 (35)4.3卡尔曼滤波模型 (36)4.3.1 状态方程 (36)4.3.2系统的量测方程 (37)4.4滤波仿真和结论 (37)第5章卡尔曼滤波在GPS定位应用中的问题和改进思路 (40)5.1对野值的处理 (40)5.2对状态以及观测噪声方差阵的处理 (41)5.3对观测噪声和测量噪声的处理 (42)结论 (30)谢辞 (31)参考文献 (47)前言自从赫兹证明了麦克斯韦的电磁波辐射理论以后，人们便开始了对无线电导航定位系统研究。

基于Kalman滤波器的车式移动机器人跟踪方法

型，如文献『３】中提出的直接利用反馈信息对机器人
进行控制．但这种控制方式对于需要知道车头方向
系统状态初值的情况下，利用输出信号的量测数据和系统模型方程，实时获得系统状态变量和输入信号的最优估计。Ｋａｌｍａｎ滤波器不仅能对目标轨迹进
程分为两种基本策略：一种是对机器人建立模型，
并通过模型进行算法的应用；另一种则是不建立模
动态模型精度较低，故很少使用。Ｋａｌｍａｎ滤波是在
已知系统和量测的数学模型、量测噪声统计特性及
ｗａｓｃａｒｒｉｅｄｏｕｔａｆｔｅｒｍｏｄｅｌｉｎｇ，ｌｉｎｅａｒｉｚａｔｉｏｎａｎｄｄｉｓｅｒｅｔｉｚａｔｉｏｎ，ａｎｄＫａｌｍａｎｉｆｌｔｅｒｗａｓｉｎｔｒｏｄｕｃｅｄｔｈｅｎ．Ｂｕｉｌｄｉｎｇｅｘｐｅｒｉ・
ｌａ－ｔｉｍｅｒｔａｊｅｃｔｏｉｒｅｓａｎｄｓｔａｔｅｓ．
Ｋｅｙｗｏｒｄｓ：ｅａｒ－ｔｙｐｅｍｏｂｉｌｅｒｏｂｏｔ；ｍｏｄｅｌｉｎｇ；Ｋｌａｍａｎｉｆｌｔｅｒ；ｍｕｌｔｉ — ｔａｒｇｅｔｔｒａｃｋｉｎｇ；ｔｒａｊｅｃｔｏｙｒｏｐｔｉｍｉｚａｔｉｏｎ

卡尔曼滤波运动模型

运动模型
1. 匀速模型匀速模型假设⻋车辆以较为恒定的速度行行驶，则匀速模型为：
˙ (t) = AX(t) + w(t) X
其中，
A=
(
)
X(t) =
(
)
A= w(t) =
0 ( a(t) )
0 (0
1 0)
X(t) = R(t) =
x(t) (x ˙ (t) ) 0 2) σa
Hale Waihona Puke 0 (02 为加速度的方方差。其中，σa
如下形式：若仅有位置测量值，则：
1 4 T 8 k 1 3 T 3 k 1 2 T 2 k
假定定位系统不能直接测量到加速度，因此，观测方方程的参数在不同条件下有
H = (1
0
0)
Y (k) = (y(k))
R(k) = (σx2 )
若位置和速度测量值都存在，则：
H = (1
0
0)
y(k) Y (k) = (y ˙ (k) )
σx2 R(k) = ( σxx ˙
σxx ˙ 2 σx ˙ )
当位置是唯一一的状态观测量时，初始化滤波器需要三个等间隔（
T0 = T1 = T2 ）的测量值Y (0) 、Y (1) 和Y (2) ，这时系统的初始状态和相应的
状态误差矩阵为：
⎛ ⎜ ⎜ =⎜
⎞ ⎟ ⎟ ⎟
⎛ ⎞ Y1 (2) ⎜ ⎟ 1 ⎜ ⎟ ^ (2|2) = ⎜ (Y1 (2) − Y1 (1)) X ⎟ T ⎜ ⎟ ⎜ 1 (Y (2) − 2Y (1) + Y (0)) ⎟ 1 1 ⎝ T2 1 ⎠ ⎛ 1 2 2 σx ⎜ σx T ⎜ 1 2 2 2 T2 2 T3 2 P(2|2) = ⎜ σ σ + σa + σ˙ ⎜ T x 2 x 4 5 x T ⎜ 3 2 7 2 2 ⎜ 1 σ2 σx + T σa x ˙ ⎝ T2 40 T3

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

目录一、kalman滤波简介 1二、kalman滤波基本原理 (1)三、Kalman滤波在运动跟踪中的应用的建模 (3)四、仿真结果 (6)1、kalman的滤波效果 (6)2、简单轨迹的kalman的预测效果 (7)3、椭圆运动轨迹的预测 (9)4、往返运动归轨迹的预测 (10)五、参数的选取 (11)附录： (13)Matlab程序： (13)C语言程序： (13)Kalman滤波在运动跟踪中的应用一、kalman滤波简介最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。

从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。

为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。

卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。

它适合于实时处理和计算机运算。

Kalman滤波是卡尔曼(R.E.kalman)于1960年提出的从与被提取信号的有关的观测量中通过算法估计出所需信号的一种滤波算法。

他把状态空间的概念引入到随机估计理论中，把信号过程视为白噪声作用下的—个线性系统的输出，用状方程来描述这种输入—输出关系，估计过程中利用系统状态方程、观测方程和白噪声激励(系统噪声和观测噪声)的统计特性形成滤波算法，由于所用的信息都是时域内的量，所以不但可以对平稳的一维随机过程进估计，也可以对非平稳的、多维随机过程进行估汁。

Kalman滤波是一套由计算机实现的实时递推算法．它所处理的对象是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要估计值(系统的状态或参数)作为滤波器的输出，滤波器的输入与输出之间是由时间更新和观测更新算法联系在一起的，根据系统方程和观测方程估计出所有需要处理的信号。

所以，Kalman滤波与常规滤波的涵义与方法不同，它实质上是一种最优估计法。

卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法），对于解决很大部分的问题，他是最优，效率最高甚至是最有用的二、kalman滤波基本原理Kalman滤波器是目标状态估计算法解决状态最优估计的一种常用方法具有计算量小、存储量低、实时性高的优点。

实际应用中，可以将物理系统的运行过程看作是一个状态转换过程，卡尔曼滤波将状态空间理论引入到对物理系统的数学建模过程中来。

其基本思想是给系统信号和噪声的状态空间建立方程和观测方程，只用信号的前一个估计值和最近一个观察值就可以在线性无偏最小方差估计准则下对信号的当前值做出最优估计。

设一系统所建立的模型为：状态方程：个离散控制过程的系统，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（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。

下面我们来用他们结合他们的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。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。

式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。

但是为了要使卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新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)状态向量预报方程1~'~-=k k X A X (2)状态向量协方差预报方程11'--+=k T k k k k Q A P A P(3)Kalman 加权矩阵(或增益矩阵)1)'('-+=k T k k k T k k k R H P H H P K(4)状态向量更新方程)'~('~~k k k k k k X H Z K X X -+= (5)状态向量协方差更新方程')(k k k k P H K I P -=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. 计算更新后估计协防差矩阵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 滤波在运动跟踪中的应用的建模卡尔曼滤波器是一个对动态系统的状态序列进行线性最小误差估计的算法，一般用于线性系统。

一般在运动跟踪领域中摄像机相对于目标物体运动有时属于非线性系统，但由于在一般运动跟踪问题中图像采集时间间隔较短，可近似将单位时间内目标在图像中的运动看作匀速运动，采用卡尔曼滤波器可以实现对目标运动参数的估计。

对于复杂背景下运动目标识别与跟踪问题，要实现实时控制，对算法的实时性和准确性都有较高的要求。

如果目标识别算法都是基于像素的全局搜索，则存在显著缺点：1)全局搜索计算量大、耗时，实时性无法满足；2)全局搜索抗干扰能力差，容易受到背景中相似特征物体的干扰。

基于卡尔曼滤波器预测功能的运动目标快速跟踪算法可以通过预测目标物体在下一帧中的位置，将全局搜索问题转化为局部搜索，缩小搜索范围，提高算法的实时性。

在跟踪运动目标的过程中，由于目标物体单位时间内在图像中的运动可以看作匀速运动，所以可以采用目标某一时刻在图像中的位置和速度来表示目标的运动状态。

为了简化算法的计算复杂度，可以设计2个卡尔曼滤波器分别描述目标在X 轴和Y 轴方向上位置和速度的变化。

下面仅讨论X 轴方向上卡尔曼滤波器的实现过程，Y 轴方向上同理。

目标物体运动方程为：⎩⎨⎧+=+=++T a v v T v x x k k k k k k 11 式中k x ，k v ，k a 分别为目标在t=k 时刻的X 轴方向的位置、速度和加速度；T 为k 帧图像和k+1帧图像之间的时间间隔，T a k 可以当作白噪声处理。

写成矩阵形式如下：系统状态方程为：⎪⎪⎭⎫ ⎝⎛+⎪⎪⎭⎫ ⎝⎛⎪⎪⎭⎫ ⎝⎛=⎪⎪⎭⎫ ⎝⎛++T a v x T v x k k k k k 010111卡尔曼滤波器系统状态矢量为：T k k k v x X ][+= 状态转移矩阵为：⎪⎪⎭⎫ ⎝⎛=101)(T k H 系统动态噪声矢量为：⎪⎪⎭⎫ ⎝⎛=T a w k k 0 系统观测方程为：()⎪⎪⎭⎫ ⎝⎛=+k k k v x x 011卡尔曼滤波器系统观测矢量为：k k x Z =观测系数矩阵为：[]01=k H由观测方程可知，观测噪声为0，所以k R =0。

建立了上述系统状态方程和观测方程之后，就可以利用卡尔曼滤波方程式通过递推方法，不断预测目标在下一帧中的位置。

在t=k 时刻，对第k 帧图像利用目标识别算法识别出的目标位置记为k x ，当目标首次出现时，根据此时目标的观测位置初始化滤波器0'~X =[0x ，0]。

系统初始状态向量协方差矩阵可以在对角线上取较大值，取值根据实际测量情况来获得，但在滤波启动一段时间后影响就不大了。

取：⎪⎪⎭⎫ ⎝⎛=1000100P 。

系统动态噪声协方差为0Q ，可取为：⎪⎪⎭⎫ ⎝⎛=1000100Q 。

通过公式(1)，计算得到目标在下一帧图像中的预测位置1'~X 。

在该位置附近，对下一帧图像进行局部搜索，识别出的目标质心位置即为1Z ，通过公式(2)至公式(5)完成对状态向量和状态向量协方差矩阵的更新，为目标位置的下一步预测做好准备，得出新的预测位置2'~X ，采用图像处理算法，在该位置进行局部搜索，从而得出新的目标质心位置2Z ，一直迭代计算下去，从而实现对目标物体的跟踪。