ukf(无迹卡尔曼滤波)算法的matlab程序.function[x,P]=ukf(fstate,x,P,hmeas,z,Q,R)%UKFUnscentedKalmanFilterfornonlineardynamicsystems%[x,P]=ukf(f,x,P,h,z,Q,R)returnsstateestimate,xandstatecovariance,P%fornonlineardynamicsystem(forsimplicity,noisesareassumedasadditive):%x_k+1=f(x_k)+w_k%z_k=h(x_k)+v_k%wherew~N(0,Q)meaningwisgaussiannoisewithcovarianceQ%v~N(0,R)meaningvisgaussiannoisewithcovarianceR%Inputs:f:functionhandleforf(x)%x:"apriori"stateestimate%P:"apriori"estimatedstatecovariance%h:fanctionhandleforh(x)%z:currentmeasurement%Q:processnoisecovariance%R:measurementnoisecovariance%Output:x:"aposteriori"stateestimate%P:"aposteriori"statecovariance%%Example:%{n=3;%numberofstateq=0.1;%stdofprocessr=0.1;%stdofmeasurementQ=q^2*eye(n);%covarianceofprocessR=r^2;%covarianceofmeasurementf=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];%nonlinearstateequationsh=@(x)x(1);%measurementequations=[0;0;1];%initialstatex=s+q*randn(3,1);%initialstate%initialstatewithnoiseP=eye(n);%initialstatecovraianceN=20;%totaldynamicstepsxV=zeros(n,N);%estmate%allocatememorysV=zeros(n,N);%actualzV=zeros(1,N);fork=1:Nz=h(s)+r*randn;%measurmentssV(:,k)=s;%saveactualstatezV(k)=z;%savemeasurment[x,P]=ukf(f,x,P,h,z,Q,R);%ekfxV(:,k)=x;%saveestimates=f(s)+q*randn(3,1);%updateprocessendfork=1:3%plotresultssubplot(3,1,k)plot(1:N,sV(k,:),'-',1:N,xV(k,:),'--')end%}%%ByYiCaoatCranfieldUniversity,04/01/2008%L=numel(x);%numerofstatesm=numel(z);%numerofmeasurementsalpha=1e-3;%default,tunableki=0;%default,tunablebeta=2;%default,tunablelambda=alpha^2*(L+ki)-L;%scalingfactorc=L+lambda;%scalingfactorWm=[lambda/c0.5/c+zeros(1,2*L)];%weightsformeansWc=Wm;Wc(1)=Wc(1)+(1-alpha^2+beta);%weightsforcovariancec=sqrt(c);X=sigmas(x,P,c);%sigmapointsaroundx[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q);%unscentedtransformationofprocess%X1=sigmas(x1,P1,c);%sigmapointsaroundx1%X2=X1-x1(:,ones(1,size(X1,2)));%deviationofX1[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R);%unscentedtransformationofmeasurmentsP12=X2*diag(Wc)*Z2';%transformedcross-covarianceK=P12*inv(P2);x=x1+K*(z-z1);%stateupdateP=P1-K*P12';%covarianceupdatefunction[y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)%UnscentedTransformation%Input:%f:nonlinearmap%X:sigmapoints%Wm:weightsformean%Wc:weightsforcovraiance%n:numerofoutputsoff%R:additivecovariance%Output:%y:transformedmean%Y:transformedsmaplingpoints%P:transformedcovariance%Y1:transformeddeviationsL=size(X,2);y=zeros(n,1);Y=zeros(n,L);fork=1:LY(:,k)=f(X(:,k));y=y+Wm(k)*Y(:,k);endY1=Y-y(:,ones(1,L));P=Y1*diag(Wc)*Y1'+R;functionX=sigmas(x,P,c)%Sigmapointsaroundreferencepoint%Inputs:%x:referencepoint%P:covariance%c:coefficient%Output:%X:SigmapointsA=c*chol(P)';Y=x(:,ones(1,numel(x)));X=[xY+AY-A];