%%卡爾曼濾波  動目標檢測
%采用基于當前統計模型的自適應算法
%信號模型：X(k+1)=H*X(k)+D*a+W(k)   
%觀測模型：Z(k)=C*X(k)+V(k)

clear
clc
IdealTrack=zeros(2,330);   %%位置 
Vx=zeros(1,330);
Vy=zeros(1,330);           %%速度
Ux=zeros(1,330);
Uy=zeros(1,330);            %%加速度

IdealTrack(1,1)=2000;
IdealTrack(2,1)=10000; 
Vx(1)=0;
Vy(1)=-15;        %%初始點
Ux(1)=0;                      
Uy(1)=0; 

for i=2:330
    if i<=200
        Ux(i)=0;                      
        Uy(i)=0;                     
    elseif i<=300
        Ux(i)=0.075;                      
        Uy(i)=0.075;                     
    elseif i>300&i<=305
        Ux(i)=0;                      
        Uy(i)=0;
    else
        Ux(i)=-0.3;                      
        Uy(i)=0.3;
    end
        Vx(i)=Vx(i-1)+Ux(i-1)*2;
        Vy(i)=Vy(i-1)+Uy(i-1)*2;                   
        IdealTrack(1,i)=IdealTrack(1,i-1)+Vx(i-1)*2+2*Ux(i-1);
        IdealTrack(2,i)=IdealTrack(2,i-1)+Vy(i-1)*2+2*Uy(i-1);
    end
end
Noise=100*randn(2,330);
MeasureTrack=zeros(2,330);
MeasureTrack=IdealTrack+Noise;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for n=1:100
    H=[1 2 1.978 0 0 0
        0 1 1.967 0 0 0 
        0 0 0.9672 0 0 0
        0 0 0 1 2 1.978
        0 0 0 0 1 1.967
        0 0 0 0 0 0.9672];

   Q=2*(0.37/22)*[1.6 2 4/3 0 0 0        
        2 8/3 2 0 0 0                            
        4/3 2 2 0 0 0                            
        0 0 0 1.6 2 4/3                          
        0 0 0 2 8/3 2      
        0 0 0 4/3 2 2];   
 
   H1=[1 2 2 0 0 0 
        0 1 2 0 0 0 
        0 0 1 0 0 0
        0 0 0 1 2 2
        0 0 0 0 1 2
        0 0 0 0 0 1];
   R=[10000 0
        0 10000];
   C=[1 0 0 0 0 0
          0 0 0 1 0 0];
 
   FilterState=zeros(6,1);
   ForcastState=zeros(6,1);

   FilterTrack=zeros(2,330);
   FilterTrack(:,1)=MeasureTrack(:,1);
   FilterTrack(:,2)=MeasureTrack(:,2);
   x2=MeasureTrack(1,2);
   y2=MeasureTrack(2,2);
   Vx2=(MeasureTrack(1,2)-MeasureTrack(1,1))/2;
   Vy2=(MeasureTrack(2,2)-MeasureTrack(2,1))/2;  
   Ux2=Ux(2);
   Uy2=Uy(2);

   FilterState=[x2 Vx2 Ux2 y2 Vy2 Uy2]';
   Pfilter=[10000 5000 0 0 0 0 
    5000 5000 0 0 0 0
    0 0 0 0 0 0
    0 0 0 10000 5000 0                             %%濾波的初值選定
    0 0 0 5000 5000 0
    0 0 0 0 0 0];                        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    for k=3:330
        ForcastState=H1*FilterState;                                         %%預測狀態
        Pforcast=H*Pfilter*H'+Q;                                              %%預測誤差協方差
        Kgain=Pforcast*C'*inv(C*Pforcast*C'+R);                              %%增益
        FilterState=ForcastState+Kgain*(MeasureTrack(:,k)-C*ForcastState);  %%濾波狀態
        Pfilter=(eye(6)-Kgain*C)*Pforcast;                                  %%濾波誤差協方差
        FilterTrack(:,k)=C*FilterState;  
        FilterError(n,k)=sqrt((FilterTrack(1,k)-IdealTrack(1,k))^2+(FilterTrack(2,k)-IdealTrack(2,k))^2);
   end
   for k=1:2
        FilterError(n,k)=sqrt((FilterTrack(1,k)-IdealTrack(1,k))^2+(FilterTrack(2,k)-IdealTrack(2,k))^2); 
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MeanError=sum(FilterError,1)/100;
StandError=zeros(330);
StandardDeviation=zeros(330);
for k=1:330
    for n=1:100
        FFilterError(n,k)=FilterError(n,k)^2;
    end
    StandError=sum(FFilterError,1)/100
   StandardDeviation(k)=sqrt(abs(StandError(k)-MeanError(k)^2));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(2,2,1);
plot(IdealTrack(1,:),IdealTrack(2,:),'b'); 
xlabel('理想航跡和觀測模型');
axis([1000,3000,2000,12000]);
hold on;
plot(MeasureTrack(1,:),MeasureTrack(2,:),'r');    %%繪制理想航跡和觀測模型

subplot(2,2,2);
plot(IdealTrack(1,:),IdealTrack(2,:),'b'); 
axis([1000,3000,2000,12000]);
hold on;
plot(FilterTrack(1,:),FilterTrack(2,:),'k');
xlabel('理想航跡和濾波結果');

subplot(2,2,3);
plot(MeanError);
xlabel('誤差均方差');

subplot(2,2,4);
plot(StandardDeviation);
xlabel('標準差');