kalman.zip

clear all; clc; %===============================·??????°=================================== sigma=10000; T=2; t=300; Vx=15; H=[1 0 0;0 1 0]; Q=[1 0 0;0 1 T;0 0 1]; eXk(:,t)=[0 0 0]';eXz(:,t)=[0 0 0]';eeXz(:,t)=[0 0]'; N=10;%?????¨?????? for i=1:N for j=1:t Zk(:,j)=[2000+wgn(1,1,40);-10000+Vx*T*(j-1)+wgn(1,1,40)]; end for j=1:300 if j==1 Xk(:,1)=[Zk(1,1),Zk(2,1),0]';Xk1(:,1)=Xk(:,1); Xk(:,2)=[Zk(1,2),Zk(2,2),(Zk(2,2)-Zk(2,1))/T]';Xk1(:,2)=Xk(:,2); Pk=[sigma,0,0;0,sigma,sigma/T;0,sigma/T,2*sigma/T]; else if j>2 Xk1(:,j)=Q*Xk(:,j-1);%?¤?? Pk1=Q*Pk*Q';%?¤???ó????·??? Kk=Pk1*H'*inv(H*Pk1*H'+sigma*eye(2));%kalman???? Xk(:,j)=Xk1(:,j)+Kk*(Zk(:,j)-H*Xk1(:,j));%???¨ Pk=(eye(3)-Kk*H)*Pk1;%???¨??·??? end end %% %1000???ó???ù eXk(:,j)=eXk(:,j)+Xk(:,j)/N;%???¨ eXz(:,j)=eXz(:,j)+([2000;-10000+Vx*(j-1)*T;0]-Xk(:,j))/N;%???¨?ó???ù?? eeXz(:,j)=eeXz(:,j)+[(2000-Xk(1,j))^2;(-10000+Vx*(j-1)*T-Xk(2,j))^2]/N;%???¨?ó??±ê×??? end end %===========================????==================================== %?????ì?????????ì?? subplot(2,1,1);j=0:0.1:t;plot(-10000+Vx*(j-1)*T,2000); title('目标真实轨迹');xlabel('X（米）)');ylabel('Y(米)'); subplot(2,1,2);plot(Zk(2,:),Zk(1,:)); title('测量轨迹');xlabel('X(米)');ylabel('Y(米)'); %???¨????·??????????¨??·??? figure; subplot(2,1,1); plot(Xk(2,:),Xk(1,:),'g'); title('单次滤波数据曲线');xlabel('X(米)');ylabel('Y(米)'); subplot(2,1,2); plot(eXk(2,:),eXk(1,:));title('1000次滤波数据曲线'); xlabel('X(米)');ylabel('Y(米)'); % j=1:t; figure;subplot(211);plot(j,eXz(2,:));title('x滤波误差均值曲线'); xlabel('采样次数');ylabel('Y（米）'); subplot(212); for j=1:t eeXz(1,j)=sqrt(eeXz(1,j)-eXz(1,j)^2); eeXz(2,j)=sqrt(eeXz(2,j)-eXz(2,j)^2); end j=1:t; plot(j,eeXz(2,:));title('x滤波误差标准差曲线'); xlabel('采样次数');ylabel('Y(米)'); figure;subplot(211);plot(j,eXz(1,:)); title('y滤波误差均值曲线');xlabel('采样次数');ylabel('Y(米）'); subplot(212);plot(j,eeXz(1,:));title('y滤波误差标准差曲线'); xlabel('采样次数');ylabel('Y(米)'); figure;subplot(211);plot(j,Xk(3,:));title('单次滤波速度估计'); subplot(212);plot(j,eXk(3,:));title('100次滤波速度估计'); xlabel('采样次数');ylabel('Y(米)');