卡尔曼滤波算法matlab程序

function[x,V,VV,loglik]=kalman_filter(y,A,C,Q,R,init_x,init_V,varargin)%varargin变长度输入宗量：在matlab中varargin提供了一种函数可变参数列表机制。就是说，%使用了“可变参数列表机制”的函数允许调用者调用该函数时根据需要来改变输入参数的个数。%Klanmanfilter%[x,V,VV,loglik]=kalman_filter(y,A,C,Q,R,int_x,int_V,...)%Inputs（输入）%y(:,t)-t时刻的观测量%A-系统矩阵%C-量测矩阵%Q-系统噪声协方差%R-量测噪声协方差%int_x-初始状态向量%int_V-初始的状态协方差%Optionalinputs（可选择的输入）%‘model’-model(t)=mmeansuseparamsfrommodelmattimet[ones(1，T)]%inthiscase,alltheabovematricestakeanadditionalfinaldimension%i.e.A(:,:,m),C(:,:,m),Q(:,:,m),R(:,:,m)%However,int_xandint_Vareindependentofmodel(1)%'u'-u(:,t)thecontrolsignalattimet[[]]（u（：，t）是t时刻控制输入信号）%'B'-B(:,:,m)theinputregressionmatrixformodelm（B（：，m）是输入回归矩阵）%Outputs(whereXisthehiddenstatebeingestimated)（与输入对应的输出）%x(:,t)=E[X(:,t)/y(:,1:t)]已知t时刻和t时刻以前的量测值对x(t)的最小均方误差估计，相当于x(k/k)%V(:,:,t)=Cov[X(:,t)/y(:,1:t)]相当于P(k/k)%VV(:,:,t)=Cov[X(:,t),x(:,t-1)/y(:,1:t)]t=2%loglik=sum{t=1}^TlogP(y(:,t))%Ifaninputsignalisspecified,wealsoconditionon%it:如果指定了输入信号，则对应的状态为%e.g.,x(:,t)=E[X(:,t)|y(:,1:t),u(:,1:t)]%Ifamodelsequenceisspecified,wealsoconditionon%it:如果指定了t时刻的模型序列，则对应的状态为%e.g.,x(:,t)=E[X(:,t)|y(:,1:t),u(:,1:t),m(1:t)][,T]=size(y);ss=size(A,1);%sizeofstatespace%setdefaultparamsmodel=ones(1,T);%产生1*T的全1数组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_update(A(i,i,m),C(:,i,m),Q(i,i,m),R(:,:,m),y(:,t),prevx(i),prevVsmall,...'initial',initial,'u',u(:,t),'B',B(i,:,m));smallP=inv(smallV);prevP(i,i)=smallP;V(:,:,t)=inv(prevP);endendloglik=loglik+LL;end