卡尔曼滤波

卡尔曼滤波卡尔曼滤波（Kalmanfiltering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。斯坦利·施密特(StanleySchmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling(1958),Kalman(1960)与KalmanandBucy(1961)发表。数据滤波是去除噪声还原真实数据的一种数据处理技术,Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态.由于,它便于计算机编程实现,并能够对现场采集的数据进行实时的更新和处理,Kalman滤波是目前应用最为广泛的滤波方法,在通信,导航,制导与控制等多领域得到了较好的应用.中文名卡尔曼滤波器，Kalman滤波，卡曼滤波外文名KALMANFILTER表达式X(k)=AX(k-1)+BU(k)+W(k)提出者斯坦利·施密特提出时间1958应用学科天文，宇航，气象适用领域范围雷达跟踪去噪声适用领域范围控制、制导、导航、通讯等现代工程1背景斯坦利·施密特(StanleySchmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时,发现他的方法对于解决阿波罗计划的轨道预测很有用,后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling(1958),Kalman(1960)与KalmanandBucy(1961)发表。2定义传统的滤波方法，只能是在有用信号与噪声具有不同频带的条件下才能实现．20世纪40年代，N．维纳和A．H．柯尔莫哥罗夫把信号和噪声的统计性质引进了滤波理论，在假设信号和噪声都是平稳过程的条件下，利用最优化方法对信号真值进行估计，达到滤波目的，从而在概念上与传统的滤波方法联系起来，被称为维纳滤波。这种方法要求信号和噪声都必须是以平稳过程为条件。60年代初，卡尔曼(R．E．Kalman)和布塞(R．S．Bucy)发表了一篇重要的论文《线性滤波和预测理论的新成果》，提出了一种新的线性滤波和预测理由论，被称之为卡尔曼滤波。特点是在线性状态空间表示的基础上对有噪声的输入和观测信号进行处理，求取系统状态或真实信号。这种理论是在时间域上来表述的，基本的概念是：在线性系统的状态空间表示基础上，从输出和输入观测数据求系统状态的最优估计。这里所说的系统状态，是总结系统所有过去的输入和扰动对系统的作用的最小参数的集合，知道了系统的状态就能够与未来的输入与系统的扰动一起确定系统的整个行为。卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。例如在图像处理方面，应用卡尔曼滤波对由于某些噪声影响而造成模糊的图像进行复原。在对噪声作了某些统计性质的假定后，就可以用卡尔曼的算法以递推的方式从模糊图像中得到均方差最小的真实图像，使模糊的图像得到复原。3性质①卡尔曼滤波是一个算法，它适用于线性、离散和有限维系统。每一个有外部变量的自回归移动平均系统(ARMAX)或可用有理传递函数表示的系统都可以转换成用状态空间表示的系统，从而能用卡尔曼滤波进行计算。②任何一组观测数据都无助于消除x(t)的确定性。增益K(t)也同样地与观测数据无关。③当观测数据和状态联合服从高斯分布时用卡尔曼递归公式计算得到的是高斯随机变量的条件均值和条件方差，从而卡尔曼滤波公式给出了计算状态的条件概率密度的更新过程线性最小方差估计，也就是最小方差估计。4形式卡尔曼滤波已经有很多不同的实现，卡尔曼最初提出的形式一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman,Thornton开发的平方根滤波器的变种。最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。5实例卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的，包含噪声的观察序列中预测出物体的坐标位置及速度。在很多工程应用(雷达、计算机视觉)中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要话题。6应用比如，在雷达中，人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。扩展卡尔曼滤波（EXTENDKALMANFILTER,EKF）扩展卡尔曼滤波器是由kalmanfilter考虑时间非线性的动态系统，常应用于目标跟踪系统。7状态估计状态估计状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。比如对飞行器状态估计。状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。最常用的是最小二乘估计，线性最小方差估计、最小方差估计、递推最小二乘估计等。其他如风险准则的贝叶斯估计、最大似然估计、随机逼近等方法也都有应用。状态量受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是最优估计。真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推最优估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。理论卡尔曼滤波理论的提出，克服了威纳滤波理论的局限性使其在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代工程方面。8MATLAB程序MATLABN=200;w(1)=0;w=randn(1,N)x(1)=0;a=1;fork=2:N;x(k)=a*x(k-1)+w(k-1);endV=randn(1,N);q1=std(V);Rvv=q1.^2;q2=std(x);Rxx=q2.^2;q3=std(w);Rww=q3.^2;c=0.2;Y=c*x+V;p(1)=0;s(1)=0;fort=2:N;p1(t)=a.^2*p(t-1)+Rww;b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));p(t)=p1(t)-c*b(t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');function[x,V,VV,loglik]=kalman_filter(y,A,C,Q,R,init_x,init_V,varargin)%Kalmanfilter.%[x,V,VV,loglik]=kalman_filter(y,A,C,Q,R,init_x,init_V,...)%%INPUTS:%y(:,t)-theobservationattimet%A-thesystemmatrix%C-theobservationmatrix%Q-thesystemcovariance%R-theobservationcovariance%init_x-theinitialstate(column)vector%init_V-theinitialstatecovariance%%OPTIONALINPUTS(string/valuepairs[defaultinbrackets])%'model'-model(t)=mmeansuseparamsfrommodelmattimet[ones(1,T)]%Inthiscase,alltheabovematricestakeanadditionalfinaldimension,%i.e.,A(:,:,m),C(:,:,m),Q(:,:,m),R(:,:,m).%However,init_xandinit_Vareindependentofmodel(1).%'u'-u(:,t)thecontrolsignalattimet[[]]%'B'-B(:,:,m)theinputregressionmatrixformodelm%%OUTPUTS(whereXisthehiddenstatebeingestimated)%x(:,t)=E[X(:,t)|y(:,1:t)]%V(:,:,t)=Cov[X(:,t)|y(:,1:t)]%VV(:,:,t)=Cov[X(:,t),X(:,t-1)|y(:,1:t)]t=2%loglik=sum{t=1}^TlogP(y(:,t))%%Ifaninputsignalisspecified,wealsoconditiononit:%e.g.,x(:,t)=E[X(:,t)|y(:,1:t),u(:,1:t)]%Ifamodelsequenceisspecified,wealsoconditiononit:%e.g.,x(:,t)=E[X(:,t)|y(:,1:t),u(:,1:t),m(1:t)][osT]=size(y);ss=size(A,1);%sizeofstatespace%setdefaultparamsmodel=ones(1,T);u=[];B=[];ndx=[];args=varargin;nargs=length(args);fori=1:2:nargsswitchargscase'model',model=args{i+1};case'u',u=args{i+1};case'B',B=args{i+1};case'ndx',ndx=args{i+1};otherwise,error(['unrecognizedargument'args])endendx=zeros(ss,T);V=zeros(ss,ss,T);VV=zeros(ss,ss,T);loglik=0;fort=1:Tm=model(t);ift==1%prevx=init_x(:,m);%prevV=init_V(:,:,m);prevx=init_x;prevV=init_V;initial=1;elseprevx=x(:,t-1);prevV=V(:,:,t-1);initial=0;endifisempty(u)[x(:,t),V(:,:,t),LL,VV(:,:,t)]=...kalman_update(A(:,:,m),C(:,:,m),Q(:,:,m),R(:,:,m),y(:,t),prevx,prevV,'initial',initial);elseifisempty(ndx)[x(:,t),V(:,:,t),LL,VV(:,:,t)]=...kalman_update(A(:,:,m),C(:,:,m),Q(:,:,m),R(:,:,m),y(:,t),prevx,prevV,...'initial',initial,'u',u(:,t),'B',B(:,:,m));elsei=ndx;%copyoverallelements;onlysomewillgetupdatedx(:,t)=prevx;prevP=inv(prevV);prevPsmall=prevP(i,i);prevVsmall=inv(prevPsmall);[x(i,t),smallV,LL,VV(i,i,t)]=...kalman_upda