无迹卡尔曼滤波算法

%该文件用于编写无迹卡尔曼滤波算法及其测试%注解：主要子程序包括：轨迹发生器、系统方程%测量方程、UKF滤波器%作者：Jiangfeng%日期：2012.4.16%---------------------------------------functionUKFmain%------------------清屏----------------closeall;clearall;clc;tic;globalQfn;%定义全局变量%------------------初始化--------------stater0=[220;1;55;-0.5];%标准系统初值state0=[200;1.3;50;-0.3];%测量状态初值%--------系统滤波初始化p=[0.005000;00.00500;000.0050;0000.005];%状态误差协方差初值n=4;T=3;Qf=[T^2/20;0T;T^2/20;0T];%--------------------------------------stater=stater0;state=state0;xc=state;staterout=[];stateout=[];xcout=[];errorout=[];tout=[];t0=1;h=1;tf=1000;%仿真时间设置%---------------滤波算法----------------fort=t0:h:tf[state,stater,yc]=track(state,stater);%轨迹发生器：标准轨迹和输出[xc,p]=UKFfiter(@systemfun,@measurefun,xc,yc,p);error=xc-stater;%滤波处理后的误差staterout=[staterout,stater];stateout=[stateout,state];errorout=[errorout,error];xcout=[xcout,xc];tout=[tout,t];end%---------------状态信息图像---------------figure;plot(tout,xcout(1,:),'r',tout,staterout(1,:),'g',...tout,stateout(1,:),'black');legend('滤波后','真实值','无滤波');gridon;xlabel('时间t（s）');ylabel('系统状态A');title('无迹卡尔曼滤波');figure;plot(tout,xcout(2,:),'r',tout,staterout(2,:),'g',...tout,stateout(2,:),'black');gridon;legend('滤波后','真实值','无滤波');xlabel('时间t（s）');ylabel('系统状态B');title('无迹卡尔曼滤波');figure;plot(tout,xcout(3,:),'r',tout,staterout(3,:),'g',...tout,stateout(3,:),'black');gridon;legend('滤波后','真实值','无滤波');xlabel('时间t（s）');ylabel('系统状态C');title('无迹卡尔曼滤波');figure;plot(tout,xcout(4,:),'r',tout,staterout(4,:),'g',...tout,stateout(4,:),'black');gridon;legend('滤波后','真实值','无滤波');xlabel('时间t（s）');ylabel('系统状态D');title('无迹卡尔曼滤波');figure;plot(tout,errorout(1,:),'r',tout,errorout(2,:),'g',...tout,errorout(3,:),'black',tout,errorout(4,:),'b');gridon;legend('A','B','C','D');xlabel('时间t（s）');ylabel('滤波后的状态误差');title('无迹卡尔曼滤波误差');%---------------------------------------------toc;%计算仿真程序运行时间endfunction[state,stater,yout]=track(state0,stater0)%-----------------------------------%轨迹发生函数%-----------------------------------T=3;F=[1T00;0100;001T;0001];G=[T^2/20;0T;T^2/20;0T];V=0.005*randn(2,1);W=0.008*randn(1,1);state=F*state0+G*V;stater=F*stater0;yout=atan(stater0(3)/stater0(1))+W;%用真实值得到测量值，在滤波时结果才会与真实值重合。endfunctionstate=systemfun(state0)%-------------------------%系统方程%-------------------------T=3;F=[1T00;0100;001T;0001];state=F*state0;endfunctionyout=measurefun(state0)%----------------------------%测量方程%----------------------------yout=atan(state0(3)/state0(1));endfunction[xc,p]=UKFfiter(systemfun,measurefun,xc0,yc,p0)%------------------------------------------%此程序用于描述UKF（无迹kalman滤波）算法%作者:Jiangfeng%日期：2012.3.17%------------------------------------------globalQfn;%----------------参数注解-------------------%xc0---状态初值(列向量)yc---系统测量值%p0----状态误差协方差n----系统状态量数%systemfun---系统方程measurefun--测量方程%---------------滤波初始化------------------alp=1;%default,tunablekap=-1;%default,tunablebeta=2;%default,tunablelamda=alp^2*(n+kap)-n;%scalingfactornc=n+lamda;%scalingfactorWm=[lamda/nc0.5/nc+zeros(1,2*n)];%weightsformeansWc=Wm;Wc(1)=Wc(1)+(1-alp^2+beta);%weightsforcovariancens=sqrt(nc);%-------------------------------------------sxk=0;spk=0;syk=0;pyy=0;pxy=0;p=p0;%--------------构造sigma点-----------------pk=ns*chol(p);%B=chol(A);meant:A'*A=B;sigma=xc0;fork=1:2*nif(k=n)sigma=[sigma,xc0+pk(:,k)];elsesigma=[sigma,xc0-pk(:,k-n)];endend%-------------时间传播方程----------------forks=1:2*n+1sigma(:,ks)=systemfun(sigma(:,ks));%利用系统方程对状态预测sxk=Wm(ks)*sigma(:,ks)+sxk;end%----------完成对Pk的估计forkp=1:2*n+1spk=Wc(kp)*(sigma(:,kp)-sxk)*(sigma(:,kp)-sxk)'+spk;endspk=spk+Qf*0.005*Qf';%-----------------------forkg=1:2*n+1gamma(kg)=measurefun(sigma(:,kg));endforky=1:2*n+1syk=syk+Wm(ky)*gamma(ky);end%--------------测量更新方程--------------forkpy=1:2*n+1pyy=Wc(kpy)*(gamma(kpy)-syk)*(gamma(kpy)-syk)'+pyy;endpyy=pyy+0.008;forkxy=1:2*n+1pxy=Wc(kxy)*(sigma(:,kxy)-sxk)*(gamma(kxy)-syk)'+pxy;endkgs=pxy/pyy;%修正系数xc=sxk+kgs*(yc-syk);%测量信息修正状态p=spk-kgs*pyy*kgs';%误差协方差阵更新%-------------------------------------end