扩展Kalman滤波EKF和无迹卡尔曼滤波ukf.ppt

三、无迹卡尔曼滤波算法(UKF) %---------------滤波算法----------------for t=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(滤波后,真实值,无滤波);grid on;xlabel(时间 t（s）);ylabel(系统状态A);title(无迹卡尔曼滤波);figure; 三、无迹卡尔曼滤波算法(UKF) plot(tout,xcout(2,:),r,tout,staterout(2,:),g,...? ???tout,stateout(2,:),black);grid on;legend(滤波后,真实值,无滤波);xlabel(时间 t（s）);ylabel(系统状态B);title(无迹卡尔曼滤波);figure;plot(tout,xcout(3,:),r,tout,staterout(3,:),g,...? ???tout,stateout(3,:),black);grid on;legend(滤波后,真实值,无滤波);xlabel(时间 t（s）);ylabel(系统状态C);title(无迹卡尔曼滤波);figure;plot(tout,xcout(4,:),r,tout,staterout(4,:),g,...? ???tout,stateout(4,:),black); 三、无迹卡尔曼滤波算法(UKF) grid on;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);grid on;legend(A,B,C,D);xlabel(时间 t（s）);ylabel(滤波后的状态误差);title(无迹卡尔曼滤波误差);%---------------------------------------------toc;??%计算仿真程序运行时间end) 三、无迹卡尔曼滤波算法(UKF) function [state,stater,yout]=track(state0,stater0 %-----------------------------------%轨迹发生函数%-----------------------------------T=3;F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];G=[T^2/2 0;0 T;T^2/2 0;0 T];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;%用真实值得到测量值，在滤波时结果才会与真实值重合。endfunction??state=systemfun(state0)%-------------------------%系统方程%-------------------------? ??T=3;F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];state=F*state0;end 三、无迹卡尔曼滤波算法(UKF) function??yout=measurefun(state0)%----------------------------%测量方程%------