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

对卡尔曼滤波器的仿真实现刘丹，朱毅，刘冰武汉理工大学信息工程学院，武汉（430070）E-mail：[e-mail]摘要：本文以卡尔曼滤波器原理为理论基础，用MATLAB进行卡尔曼滤波器仿真、对比卡尔曼滤波器的预测效果，对影响滤波其效果的各方面原因进行讨论和比较，按照理论模型进行仿真编程，清晰地表述了编程过程。关键词：数字信号处理；卡尔曼滤波器；MATLAB；仿真过程中图分类号:TN912.31.引言随着信息时代和数字世界的到来，数字信号处理已成为当今一门极其重要的学科和技术领域。数字信号处理已在通信、语音、图像、自动控制、雷达、军事、航空航天、医疗和家用电器等众多领域得到了广泛的应用。在数字信号处理中,数字滤波占有极其重要的地位,目前对数字滤波器的设计有多种方法,其中著名的MATLAB软件包在多个研究领域都有着广泛的应用,它的频谱分析[1]和滤波器的分析设计功能很强,从而使数字信号处理变得十分简单、直观。本文分析了数字滤波器的设计方法,举出了基于MATLAB软件的信号处理工具在数字滤波器设计中的应用。2.卡尔曼滤波基本原理卡尔曼滤波过程实际上是获取维纳解的递推运算过程[2]。从维纳解导出的卡尔曼滤波器实际上是卡尔曼滤波过程结束后达到稳态的情况，这时KalmanFiltering的结果与WienerSolution是相同的[3]。具体推导如下：)()1|1(ˆ)|(ˆnGynnxfnnx+−−=)|(ˆ)()(nnxnxne−=已知由此求cacGafFGneEn,)1((..min)]([)(2−=⎯⎯→⎯==ε由fGfG,0⇒⎪⎪⎩⎪⎪⎨⎧∂∂=∂∂εε⑴)]1|1(ˆ)()[()1|1(ˆ)|(ˆ−−−+−−=nnxacnynGnnxannx可以是时变的，非平稳的随机信号⑵QnanP+−=)1()(2ε均为正数。⑶)()()(2nPCRnCPnG+=⑷)()](1[)()(nPnCGnGCPn−−==ε)(nG是个随时间变化的量，每次输入输出，)(nG就调整一次，并逐渐逼近KalmanFilter的增益G，而)1()(−nnεε，()↓↑→nnε。当)1()(−=nnεε时，GnG=)(。稳态时的误差收敛值ε，由下式给出：0)1(2222222=−+−+aCQRaCQCaRεε其中，计算滤波估计的流程图如图1所示。图1滤波估计流程图可以看出，滤波过程是以不断地“预测—修正”的递推方式进行计算[4]，先进行预测值计算，再根据观测值得到的新信息和kalman增益（加权项），对预测值进行修正。由滤波值可以得到预测[5]，又由预测可以得到滤波，其滤波和预测相互作用，并不要求存储任何观测数据，可以进行实时处理。3.程序设计卡尔曼滤波器给出了一个应用状态变量概念的公式。而且，不同于其他的递归滤波器结构，它只需要记住一步的估计结果。考虑过程噪声和测量噪声两个随机变量的状态模型称为随机状态模型。用下面两个方程描述离散状态模型：1）过程方程：(1)()()()xkAxkBukwk+=++其中，()wk是由于过程模型的不确定性而产生的模型噪声，它可能是最难量化的参数。2）输出方程：()()()ykCxkvk=+其中，()vk是测量噪声。3）过程状态估计：状态估计分为两步，如图2所示。图2状态估计一个是两个采样周期之间的状态转移阶段,这个阶段叫做TU(TimeUpdate)阶段：(|1)(1)(1)xkkAxkBuk∧∧−=−+−；另一个是获得()yk的ｔ时刻过程状态更新阶段，这个阶段叫MU(MeasurementUpdate)阶段．4）噪声过程卡尔曼估计滤波器可以根据控制信号()uk和测量输出()byk，来估计过程输出()yk和状态变化()xk。需要的先验知识包括噪声()wk、()vk的方差，以及如果不为零时它们的互相关性。过程描述（见图3）图3过程结构框图()xAxbuw=++这个式子是有两个输入()ut和()wt的状态空间描述，可用ss函数来计算。%ProcessstaterepresentationProcess=ss(Ad,[BdBd]),C,0,Te,‘inputname’,{‘u’‘w’},…‘outputname’,{‘y’};()wt和()vt的方差分别为Q和R。N为()wt和()vt的互相关矩阵。卡尔曼函数根据Q、R、N和过程状态描述计算卡尔曼估计滤波器。在这个例子中，模型噪声和测量噪声不相关，即0N=。%noisesn=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);%kalmanestimator[F_kalman,L,P,M]=kalman(process,Q,R);模型描述+测量噪声+卡尔曼滤波器（见图4）测量噪声加上模型输出组成输出信息。系统结构框图过程输入向量[]Tuwv过程输出向量[]Tbyy过程可由如下系统表示：0[]0TxAxBBuwv⎡⎤⎢⎥=+⎢⎥⎢⎥⎣⎦000[]001TbuCyyxwCv⎡⎤⎡⎤⎡⎤⎢⎥=+⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎢⎥⎣⎦建立包含测量噪声的一般模型的状态空间描述。%Staterepresentationprocess+measurementnoisea=Ad;b=[BdBd[0;0]];c=[CC];d=[000;001];process=ss(a,b,c,d,Te,’inputname’),{‘u’‘w’‘v’},…‘outputname’,{‘y’‘yb’})然后把这个过程和卡尔曼滤波器联系起来，得到整个过程的状态空间描述，其输入输出如下：输入向量：[]Tuwv输出向量：ˆˆ12ˆTbyyyxx⎡⎤⎣⎦函数parallel用来把卡尔曼滤波器和过程联系起来。然后，函数feedback用来创建把过程输出by当作卡尔曼滤波器输入的反馈环节，最后由状态模型得到输出。%Parallelplacingprocess+kalmanfilterSysp=parallel(process,F_kalman,1,1,[],[]);%yvfeedbackSyspb=feedback(sysp,1,4,2,1);%yvinputcancellationSyspb=syspb([12345],[123]);4.仿真结果下列图5、图6和图7分别为控制信号、模拟信号和测量噪声波形图。测量噪声下面分别为有噪声过程（见图8）和无噪声过程曲线。图8有噪声过程的响应曲线图9无噪声过程的响应曲线如果考虑测量噪声（变量0.3），得到图9所示的响应。卡尔曼滤波器的估计输出接近于无噪声过程的输出（没有测量和模拟噪声）状态估计状态估计也令人满意。所以，这样的过程避免了使用昂贵的传感器或者可以在测量比较困难或有噪声污染时使用。5.误差分析计算各个误差以及误差方差。%Errorsvariancecalculation%ModelingnoisePlot(t,yp-outputs(:,1));Title(‘Modelingnoise’);ec1=std(yp-outputs(:,1))%ModelingandmeasurementnoisesPlot(t,yp-outputs(:,2));Title(‘Modelingandmeasurementnoises’);ec2=std(yp-outputs(:,2))%FilteredprocessPlot(t,yp-outputs(:,3));Title(‘Filteredprocess’);ec3=std(yp-outputs(:,3))各个误差的图形如下：误差方差为：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.OptimumFIRDigitalFilterImplementationsforDecimation,Interpolation,andNarrow-bandFiltering【J】，IEEETrans.Acoustics,Speech,andSignalProcessing.2005,Vol.23,No.5,pp.444-456.[3]Constantinides.SpectralTransformationsforDigitalFilters【J】,Proc.Inst.Elec.Engr.2006,Vol.117,pp.1585-1590[4]姚天任,孙洪.《现代数字信号处理》【M】.武汉：华中科技大学出版社.1999.11[5]王友功，薛培鼎.《数字滤波器与信号处理》【M】.北京：电子工业出版社2003-08，ZhuYi，LiuBingInformationEngineeringSchool，WuhanUniversityofTechnology，Wuhan(430070)AbstractThispapercarriesouttheKalmanemulation,contrasttheforecasteffectofKalmanfilter,discussandcomparethevariousreasonsthataffecttheeffortofthefilter.Andprogrammingemulatesaccordingtotheoreticalmodel,inwriting,divideintostepaccordingtothecourseofentireprogrammingtowrite,havedescribedprogrammingcoursedistinctly.Keywords：Digitalsignalprocessing；KalmanFilter；MATLAB；emulationprogramming