资源描述：
《无迹卡尔曼滤波算法.docx》由会员上传分享，免费在线阅读，更多相关内容在教育资源-天天文库。
1、%该文件用于编写无迹卡尔曼滤波算法及其测试%注解：主要子程序包括：轨迹发生器、系统方程% 测量方程、UKF滤波器%作者：Jiangfeng%日期：2012.4.16%---------------------------------------functionUKFmain%------------------清屏----------------closeall;clearall;clc;tic;globalQfn; %定义全局变量%------------------初始化--
2、------------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=st
3、ater0;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,x
4、c,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
5、(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('滤波后','真实值','无滤波');xlab
6、el('时间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
7、,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');gr
8、idon;legend('A','B','C','D');xlabel('时间t（s）');ylabel('滤波后的状态误差');title('无迹卡尔曼滤波误差');%---------------------------------------------toc; %计算仿真程序运行时间endfunction[state,stater,yout]=track(state0,stater0)%--------
