matlab对卡尔曼滤波的仿真实现

http://www.paper.edu.cn -1- MATLAB对卡尔曼滤波器的仿真实现 刘丹，朱毅，刘冰 武汉理工大学信息工程学院，武汉（430070） E-mail：liudan_ina@yahoo.com.cn 摘 要：本文以卡尔曼滤波器原理为理论基础，用MATLAB进行卡尔曼滤波器仿真、对比 卡尔曼滤波器的预测效果，对影响滤波其效果的各方面原因进行讨论和比较，按照理论模型 进行仿真编程，清晰地 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 述了编程过程。 关键词：数字信号处理；卡尔曼滤波器；MATLAB；仿真过程 中图分类号: TN912.3 1. 引言 随着信息时代和数字世界的到来，数字信号处理已成为当今一门极其重要的学科和技 术领域。数字信号处理已在通信、语音、图像、自动控制、雷达、军事、航空航天、医疗和 家用电器等众多领域得到了广泛的应用。在数字信号处理中,数字滤波占有极其重要的地位, 目前对数字滤波器的设计有多种 方法 快递客服问题件处理详细方法山木方法pdf计算方法pdf华与华方法下载八字理论方法下载 ,其中著名的 MATLAB 软件包在多个研究领域都有着 广泛的应用,它的频谱分析[1]和滤波器的分析设计功能很强,从而使数字信号处理变得十分简 单、直观。本文分析了数字滤波器的设计方法,举出了基于 MATLAB 软件的信号处理工具 在数字滤波器设计中的应用。 2. 卡尔曼滤波基本原理 卡尔曼滤波过程实际上是获取维纳解的递推运算过程[2]。从维纳解导出的卡尔曼滤波器 实际上是卡尔曼滤波过程结束后达到稳态的情况，这时 Kalman Filtering 的结果与 Wiener Solution是相同的[3]。具体推导如下： )()1|1(ˆ)|(ˆ nGynnxfnnx +−−= )|(ˆ)()( nnxnxne −= 已知由此求 cacGafFGneEn ,)1(( ..min)]([)( 2 −=⎯⎯ →⎯==ε 由 fG f G , 0 ⇒ ⎪⎪⎩ ⎪⎪⎨ ⎧ ∂ ∂ =∂ ∂ ε ε ⑴ )]1|1(ˆ)()[()1|1(ˆ)|(ˆ −−−+−−= nnxacnynGnnxannx 可以是时变的，非平稳的随机信号 ⑵ QnanP +−= )1()( 2ε 均为正数。 ⑶ )( )()( 2 nPCR nCPnG += ⑷ )()](1[)()( nPnCGnG C Pn −−==ε )(nG 是个随时间变化的量，每次输入输出， )(nG 就调整一次，并逐渐逼近 Kalman Filter 的增益 G，而 )1()( −< nn εε ， ( ) ↓↑→ nn ε 。 http://www.paper.edu.cn -2- 当 )1()( −= nn εε 时， GnG =)( 。 稳态时的误差收敛值ε ，由下式给出： 0)1( 2222 22 2 =−+−+ aC QR aC QCaR εε 其中，计算滤波估计的流程图如图 1所示。 图 1 滤波估计流程图 可以看出，滤波过程是以不断地“预测—修正”的递推方式进行计算[4]，先进行预测值计 算，再根据观测值得到的新信息和 kalman增益（加权项），对预测值进行修正。由滤波值可 以得到预测[5]，又由预测可以得到滤波，其滤波和预测相互作用，并不要求存储任何观测数 据，可以进行实时处理。 3. 程序设计 卡尔曼滤波器给出了一个应用状态变量概念的公式。而且，不同于其他的递归滤波器结 构，它只需要记住一步的估计结果。考虑过程噪声和测量噪声两个随机变量的状态模型称为 随机状态模型。 用下面两个方程描述离散状态模型： 1）过程方程： ( 1) ( ) ( ) ( )x k Ax k Bu k w k+ = + + 其中， ( )w k 是由于过程模型的不确定性而产生的模型噪声，它可能是最难量化的 参数 转速和进给参数表a氧化沟运行参数高温蒸汽处理医疗废物pid参数自整定算法口腔医院集中消毒供应 。 2）输出方程： ( ) ( ) ( )y k Cx k v k= + 其中， ( )v k 是测量噪声。 3）过程状态估计： 状态估计分为两步，如图 2所示。 图 2 状态估计 http://www.paper.edu.cn -3- 一个是两个采样周期之间的状态转移阶段,这个阶段叫做 TU(Time Update)阶段： ( | 1) ( 1) ( 1)x k k Ax k Bu k∧ ∧− = − + − ； 另一个是获得 ( )y k 的ｔ时刻过程状态更新阶段，这个阶段叫MU(Measurement Update) 阶段． 4）噪声过程 卡尔曼估计滤波器可以根据控制信号 ( )u k 和测量输出 ( )by k ，来估计过程输出 ( )y k 和 状态变化 ( )x k 。需要的先验知识包括噪声 ( )w k 、 ( )v k 的方差，以及如果不为零时它们的 互相关性。 过程描述（见图 3） 图 3 过程结构框图 ( )x Ax b u w= + +� 这个式子是有两个输入 ( )u t 和 ( )w t 的状态空间描述，可用 ss函数来计算。 % Process state representation Process=ss (Ad, [Bd Bd]), C, 0, Te, ‘inputname’,{‘u’ ‘w’},… ‘outputname’,{‘y’}; ( )w t 和 ( )v t 的方差分别为Q和R。N 为 ( )w t 和 ( )v t 的互相关矩阵。卡尔曼函数根据Q、R、 N 和过程状态描述计算卡尔曼估计滤波器。在这个例子中，模型噪声和测量噪声不相关， 即 0N = 。 % noises n=100; w=0.1*randn(n,1); v=0.3*randn(n,1); Q=std(w).ˆ2; R=std(v).ˆ2; Wv=cov(v,w); N=wv(1,2); % kalman estimator [F_kalman,L,P,M]=kalman(process,Q,R); 模型描述+测量噪声+卡尔曼滤波器（见图 4） 测量噪声加上模型输出组成输出信息。 http://www.paper.edu.cn -4- 图 4 系统结构框图 过程输入向量 [ ]Tu w v 过程输出向量 [ ]Tby y 过程可由如下系统表示： 0 [ ] 0 Tx Ax B B u w v ⎡ ⎤⎢ ⎥= + ⎢ ⎥⎢ ⎥⎣ ⎦ � 0 0 0 [ ] 0 0 1 T b u C y y x w C v ⎡ ⎤⎡ ⎤ ⎡ ⎤ ⎢ ⎥= +⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎢ ⎥⎣ ⎦ 建立包含测量噪声的一般模型的状态空间描述。 % State representation process + measurement noise a=Ad; b=[Bd Bd [0;0]]; c=[C C]; d=[0 0 0;0 0 1]; process=ss (a,b,c,d,Te,’inputname’),{‘u’ ‘w’ ‘v’},… ‘outputname’,{‘y’ ‘yb’}) 然后把这个过程和卡尔曼滤波器联系起来，得到整个过程的状态空间描述，其输入输出如下： 输入向量： [ ]Tu w v 输出向量： ˆ ˆ1 2ˆ T by y y x x⎡ ⎤⎣ ⎦ 函数 parallel用来把卡尔曼滤波器和过程联系起来。然后，函数 feedback用来创建把过 程输出 by 当作卡尔曼滤波器输入的反馈环节，最后由状态模型得到输出。 % Parallel placing process + kalman filter Sysp=parallel (process, F_kalman,1,1,[ ],[ ]); % yv feedback Syspb= feedback (sysp,1,4,2,1); % yv input cancellation Syspb=syspb ([1 2 3 4 5], [1 2 3]); 4. 仿真结果 下列图 5、图 6和图 7分别为控制信号、模拟信号和测量噪声波形图。 http://www.paper.edu.cn -5- 图 5 控制信号 图 6 模拟噪声 图 7 测量噪声 下面分别为有噪声过程（见图 8）和无噪声过程曲线。 图 8有噪声过程的响应曲线 图 9 无噪声过程的响应曲线 如果考虑测量噪声（变量 0.3），得到图 9所示的响应。 卡尔曼滤波器的估计输出接近于无噪声过程的输出（没有测量和模拟噪声） http://www.paper.edu.cn -6- 图 10 估计过程输出 图 11 状态估计 状态估计也令人满意。所以，这样的过程避免了使用昂贵的传感器或者可以在测量比较 困难或有噪声污染时使用。 5. 误差分析 计算各个误差以及误差方差。 % Errors variance calculation % Modeling noise Plot (t,yp-outputs (:,1)); Title (‘Modeling noise’); ec1=std (yp-outputs (:,1)) % Modeling and measurement noises Plot (t,yp-outputs (:,2)); Title (‘Modeling and measurement noises’); ec2=std (yp-outputs (:,2)) %Filtered process Plot (t,yp-outputs (:,3)); Title (‘Filtered process’); ec3=std (yp-outputs (:,3)) 各个误差的图形如下： http://www.paper.edu.cn -7- 误差方差为：ec1=0.0637 误差方差为：ec2=0.3204 误差方差为：ec3=0.0136 图 12 误差波形图 估计输出信号的误差方差约为测量输出信号误差方差的 1/23，约为无测量噪声时输出信 号误差方差的 1/15。 6. 结论 卡尔曼滤波由于其在求解时不需要贮存大量的观测数据，并且当得到新的观测数据时， 可随时算得新的参数滤波值，便于实时地处理观测成果，因此，卡尔曼滤波被越来越多地应 用于动态数据处理中，尤其是 GPS动态数据处理，惯性导航等。本文以MATLAB6.0为例, 介绍卡尔曼滤波器的设计方法，目的是为了熟悉卡尔曼滤波器算法及实现,用MATLAB进行 卡尔曼滤波器仿真、对比卡尔曼滤波器的预测效果。 参考文献 [1] 李勇.《MATLAB辅助现代工程数字信号处理》【M】. 西安：西安电子科技大学出版社 2002年 10月 [2] Crochiere, Rabiner. Optimum FIR Digital Filter Implementations for Decimation, Interpolation, and Narrow-band Filtering【J】，IEEE Trans.Acoustics,Speech,and Signal Processing. 2005, Vol.23, No.5, pp.444-456. [3] Constantinides. Spectral Transformations for Digital Filters 【 J 】 ,Proc.Inst.Elec.Engr. 2006,Vol.117,pp.1585-1590 [4] 姚天任,孙洪.《现代数字信号处理》【M】. 武汉：华中科技大学出版社.1999.11 [5] 王友功，薛培鼎.《数字滤波器与信号处理》【M】.北京：电子工业出版社 2003-08 http://www.paper.edu.cn -8- Emulation of Kalman Filter with Matlab Liu Dan，Zhu Yi，Liu Bing Information Engineering School，Wuhan University of Technology，Wuhan (430070) Abstract This paper carries out the Kalman emulation, contrast the forecast effect of Kalman filter, discuss and compare the various reasons that affect the effort of the filter. And programming emulates according to theoretical model, in writing, divide into step according to the course of entire programming to write, have described programming course distinctly. Keywords：Digital signal processing；Kalman Filter；MATLAB；emulation programming