抑制Kalman滤波发散性的研究_张参贵

Kalman滤波在运动跟踪中建模

目录 一、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滤波器是目标状态估计算法解决状态最优估计的一种常用方法具有计算量小、存储量低、实时性高的优点。实际应用中，可以将物理系统的运行过程看作是一个状态转换过程，卡尔曼滤波将状态空间理论引入到对物理系统的数学建模过程中来。其基本思想是给系统信号和噪声的状态空间建立方程和观测方程，只用信号的前一个估计值和最近一个观察值就可以在线性无偏最小方差估计准则下对信号的当前值做出最优估计。 设一系统所建立的模型为：

利用扩展卡尔曼滤波算法进行目标状态估计

1）扩展卡尔曼的递推公式的程序 function [x_kk,p_kk]=KF(x_k1k1,p_k1k1,yk) TT=0.2; ztzy=[1 TT TT^2/2;0 1 TT;0 0 1]; F=[ztzy zeros(3,3) zeros(3,3);zeros(3,3) ztzy zeros(3,3);zeros(3,3) zeros(3,3) ztzy]; gr=[TT^2/2 TT 1]'; tou=[gr zeros(3,1) zeros(3,1);zeros(3,1) gr zeros(3,1);zeros(3,1) zeros(3,1) gr]; R=[20 0 0;0 5*2*pi/6000 0;0 0 5*2*pi/6000]; x_kk1=F*x_k1k1; x=x_kk1(1,1); y=x_kk1(4,1); h=x_kk1(7,1); h_k=[(x^2+y^2+h^2)^(1/2);atan2(y,x);atan2(h,sqrt(x^2+y^2))]; H_k=[1/(x^2+y^2+h^2)^(1/2)*x 0 0 1/(x^2+y^2+h^2)^(1/2)*y 0 0 1/(x^2+y^2+h^2)^(1/2)*h 0 0; -y/x^2/(1+y^2/x^2) 0 0 1/x/(1+y^2/x^2) 0 0 0 0 0; -h/(x^2+y^2)^(3/2)*x/(1+h^2/(x^2+y^2)) 0 0 -h/(x^2+y^2)^(3/2)*y/(1+h^2/(x^2+y^2)) 0 0 1/(x^2+y^2)^(1/2)/(1+h^2/(x^2+y^2)) 0