matlab-(含教程)基于kalman滤波的数据预测MATLAB仿真

% kalman filtering load initial_track s y; % y:initial data,s:data with noise T=0.1; % yp denotes the sample value of position % yv denotes the sample value of velocity % Y=[yp(n);yv(n)]; % error deviation caused by the random acceleration % known data Y=zeros(2,200); %产生2*200的全零矩阵 Y0=[0;1]; Y(:,1)=Y0; %Y的第一列等于Y0 A=[1 T 0 1]; B=[1/2*(T)^2 T]'; H=[1 0]; P0=[0 0 0 1]; P=[P0 zeros(2,2*199)]; Q=(0.25)^2; R=(0.25)^2; X=zeros(1,200); % kalman algorithm ieration for n=1:200 i=(n-1)*2+1; K=P(:,i:i+1)*H'*inv(H*P(:,i:i+1)*H'+R);%滤波增益矩阵 Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n)); %状态估计 Y(:,n+1)=A*Y(:,n); %状态一步预测 P(:,i:i+1)=(eye(2,2)-K*H)*P(:,i:i+1);%误差方程阵 P(:,i+2:i+3)=A*P(:,i:i+1)*A'+B*Q*B';%随机线性离散系统kalman滤波基本方程 X(n)=y(n)-Y(1,n); end average=zeros(1,1); variance=zeros(1,1); for i=1:200 average=average+X(i); end average=average/200; for i=1:200 variance=variance+(X(i)-average)^2; end variance=variance/200; % the diagram of position after filtering figure(3) t=0:0.1:20; yp=Y(1,:); plot(t,yp,'-');average axis([0 20 0 20]); xlabel('time'); ylabel('yp position'); title('the track after kalman filtering'); % the diagram of velocity after filtering figure(4) yv=Y(2,:); plot(t,yv,'-');variance xlabel('time'); ylabel('yv velocity'); title('the velocity caused by random acceleration');