function XE=Kalman_filter(Ts,offtime,d,Flag)
% Kalman_filter             采用Kalman濾波方法，從觀測數值中得到航跡的估計
% XE                        輸出x軸方向上的誤差
% Ts                        采樣時間，即雷達工作周期
% offtime                   仿真截止時間
% d                         噪聲的標準差值
% Flag                      判斷計算x軸或y軸數據,'0'--x,'1'--y
if nargin>4
    error('輸入的變量過多，請檢查');
end

if offtime<600
    error('仿真時間必須大于600s，請重新輸入');
end

Pv=d*d; % 噪聲的功率
N=ceil(offtime/Ts); % 采樣點數
sigma=10;% 加速度方向的的擾動

switch Flag
    case 0
        a=[zeros(1,400) 0.075*ones(1,200) zeros(1,10) -0.3*ones(1,50) zeros(1,offtime-660)]; % 對不同時段的加速度進行描述
    case 1
        a=[zeros(1,400) 0.075*ones(1,200) zeros(1,10)  0.3*ones(1,50) zeros(1,offtime-660)];
    otherwise
        error('輸入僅能為0或1');
end 

% 定義系統的狀態方程
Phi=[1,Ts;0,1];
Gamma=[Ts*Ts/2;Ts];
C=[1 0];
R=Pv;
Q=sigma^2;W=[];

randn('state',sum(100*clock)); % 設置隨機數發生器
for n=0:Ts:offtime-1
    W(n/Ts+1)=a(n+1)+sigma*randn(1,1);
end


Xest=zeros(2,1); % 用前k-1時刻的輸出值估計k時刻的預測值
Xfli=zeros(2,1); % k時刻Kalman濾波器的輸出值
Xes=zeros(2,1); % 預測輸出誤差
Xef=zeros(2,1); % 濾波后輸出的誤差
Pxe=zeros(2,1); % 預測輸出誤差均方差矩陣 
Px=zeros(2,1);  % 濾波輸出誤差均方差矩陣
XE=zeros(1,N); % 得到最終的濾波輸出值，僅僅考慮距離分量

[x,y]=trajectory(Ts,offtime); % 產生理論的航跡

for i=1:N
   vx(i)=d*randn(1); % 觀測噪聲，兩者獨立
   vy(i)=d*randn(1);
   zx(i)=x(i)+vx(i); % 實際觀測值
   zy(i)=y(i)+vy(i);
end

switch Flag
    case 0
        Xfli=[zx(2) (zx(2)-zx(1))/Ts]'; %利用前兩個觀測值來對初始條件進行估計
        Xef=[-vx(2) Ts*W(1)/2+(vx(1)-vx(2))/Ts]';
        Px=[Pv,Pv/Ts;Pv/Ts,2*Pv/Ts+Ts*Ts*Q/4];

        for k=3:N
        Xest=Phi*Xfli; % 更新該時刻的預測值
        Xes=Phi*Xef+Gamma*W(k-1); % 預測輸出誤差
        Pxe=Phi*Px*Phi'+Gamma*Q*Gamma'; % 預測誤差的協方差陣
        K=Pxe*C'*inv(C*Pxe*C'+R); % Kalman濾波增益
    
        Xfli=Xest+K*(zx(k)-C*Xest); 
        Xef=(eye(2)-K*C)*Xes-K*vx(k);
        Px=(eye(2)-K*C)*Pxe;
        
        XE(k)=Xfli(1,1);
        end
        
        XE(1)=zx(1);XE(2)=zx(2);
        
    case 1
        Xfli=[zy(2) (zy(2)-zy(1))/Ts]'; %利用前兩個觀測值來對初始條件進行估計
        Xef=[-vy(2) Ts*W(1)/2+(vy(1)-vy(2))/Ts]';
        Px=[Pv,Pv/Ts;Pv/Ts,2*Pv/Ts+Ts*Ts*Q/4];

        for k=3:N
        Xest=Phi*Xfli; % 更新該時刻的預測值
        Xes=Phi*Xef+Gamma*W(k-1); % 預測輸出誤差
        Pxe=Phi*Px*Phi'+Gamma*Q*Gamma'; % 預測誤差的協方差陣
        K=Pxe*C'*inv(C*Pxe*C'+R); % Kalman濾波增益
    
        Xfli=Xest+K*(zy(k)-C*Xest); 
        Xef=(eye(2)-K*C)*Xes-K*vy(k);
        Px=(eye(2)-K*C)*Pxe;
        
        XE(k)=Xfli(1,1);
        end
        
        XE(1)=zy(1);XE(2)=zy(2);       
    otherwise
        error('False iuput nargin');
end