%.........卡爾曼濾波算法.........%
clear all;
close all;
randn('state',0);

%.........輸入觀測數(shù)據(jù).........%
load track zt2;
load track_zhenshi zt1;
x=zt1;
z=zt2;

%.........卡爾曼濾波各參數(shù).........%
steps=30;
T=1;
F=[1,T,0,0
   0,1,0,0
   0,0,1,T
   0,0,0,1];
H=[1,0,0,0
   0,0,1,0];
G=[0.5*T^2,0
    T,0
    0,0.5*T^2
    0,T];

%.........初始值設置.........%
x0(:,1)=[300,10,300,10]';
P(:,:,1)=[10,0,0,0
          0,1,0,0
          0,0,10,0
          0,0,0,1];
Xp=zeros(4,steps);  %......4 * 1
Xn=[x0(:,1) zeros(4,steps-1)];
Pp=zeros(4,4*steps);%......4 * 4
Pn=[P(:,:,1) zeros(4,4*(steps-1))];

Kg=zeros(4,2*steps);%......4 * 2
Q=zeros(2,2*steps); %......2 * 2
R=zeros(2,2*steps); %......2 * 2
I=eye(4);

%.........卡爾曼濾波算法.........%
for k=1:(steps-1)
    i=2*k-1;
    j=4*k-3;
    Q(:,i:i+1)=5*eye(2);%...過程噪聲協(xié)方差
    R(:,i:i+1)=1*eye(2);%...觀測噪聲協(xié)方差
    Xp(:,k+1)=F*Xn(:,k);
    Pp(:,j+4:j+7)=F*Pn(:,j:j+3)*F'+G*Q(:,i:i+1)*G';
    Kg(:,i:i+1)=Pp(:,j+4:j+7)*H'*inv(H*Pp(:,j+4:j+7)*H'+R(:,i:i+1));
    Xn(:,k+1)=Xp(:,k+1)+Kg(:,i:i+1)*(z(:,k+1)-H*Xp(:,k+1));   
    Pn(:,j+4:j+7)=(I-Kg(:,i:i+1)*H)*Pp(:,j+4:j+7);
end 

%.........經(jīng)卡爾曼濾波后的誤差.........%
for i=2:steps
    zzkf(i-1)=sqrt((Xn(3,i)-x(2,i))^2+(Xn(1,i)-x(1,i))^2);
    zzvx(i-1)=abs(Xn(2,i)-10);
    zzvy(i-1)=abs(Xn(4,i)-10);
end

%.........經(jīng)卡爾曼濾波后的仿真及誤差分析.........%
 figure(1)
 plot(z(1,1:30),z(2,1:30),'k+',Xn(1,:),Xn(3,:),'ro');
 axis([300 600 300 600]);
 xlabel('X 軸方向位移（單位：m）');
 ylabel('Y 軸方向位移（單位：m）');
 title('目標運動位置真實值/估計值')
 legend('目標運動位置真實值','目標位置估計值',2);
 
 figure(2)
 plot(zzkf,'g');
 axis([0 30 0 5]);
 xlabel('時間（單位：s）');
 ylabel('誤差');
 legend('KF位置估計值誤差',2); 
 title('KF估計值與真實值之間的位置誤差分布')

 figure(3)
 plot([1:steps-1],zzvx,'r');
 axis([1 30 0 5]);
 xlabel('時間（單位：s）');
 ylabel('誤差');
 legend('KF速度（X軸方向）估計值誤差',2); 
 title('KF估計值與真實值之間的X軸速度誤差分布')

 figure(4)
 plot([1:steps-1],zzvy,'r');
 axis([1 30 0 5]);
 xlabel('時間（單位：s）');
 ylabel('誤差');
 legend('KF速度（Y軸方向）估計值誤差',2); 
 title('KF估計值與真實值之間的Y軸速度誤差分布')