构造无迹卡尔曼滤波器_无迹卡尔曼滤波_matlab

%------------------清屏---------------- close all;clear all; %关闭所有文件，清除所有变量 clc; tic; %清屏、记录程序开始时间 global Qf n; %定义全局变量 %------------------初始化-------------- stater0=[220; 1;55;-0.5]; %标准系统初值 state0=[200;1.3;50;-0.3]; %测量状态初值 %--------系统滤波初始化 p=[0.005 0 0 0;0 0.005 0 0; 0 0 0.005 0;0 0 0 0.005]; %状态误差协方差初值 n=4; T=3; Qf=[T^2/2 0;0 T;T^2/2 0;0 T]; %-------------------------------------- stater=stater0;state=state0; xc=state; staterout=[]; stateout=[];xcout=[]; errorout=[];tout=[]; t0=1; h=1; tf=1000; %仿真时间设置 %---------------滤波算法---------------- 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; 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'); 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; %计算仿真程序运行时间