function y=Kalman_BasedOnStatistic()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kalman filter for maneuvering target track
% using "in current" statistical model
% 2007.6.1
% 姓名：黃石生 
% 學號：06020032
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
maxlen=900;  % 對應在畫圖中的最大采樣點數,為方便分析實驗結果，本程序增加了采樣點數

v=300;     % 飛行器的速度

x0=-198000;  % 飛行器x坐標的位置

sigamx=100;  % 觀測值的x方向的標準差
sigamy=100;  % 觀測值的y方向的標準差

T=2;  % 采樣間隔

len=abs(x0)/T/v      % 對應在第一段勻速直線運動過程中的采樣數

for i=1:len
    x(i)=i*T*v+x0;
end

a=20;      % 轉彎加速度
r=v^2/a;   % 由此確定的轉彎半徑

y0=r;  % 為方便起見，取半徑即為y的坐標
y(1:len)=y0*ones(1,len);

w=a/v;   % 由此確定的角頻率

time_circle=pi/w;  % 在圓弧形轉彎中用的時間

len2=floor(time_circle/T);  % 對應在圓弧形彎道的采樣點數

for i=1:len2
    temp=i*T*w;
    x(len+i)=r*sin(temp);
    y(len+i)=r*cos(temp); % 圓弧處的位置坐標
end

time_after_circle=T-(time_circle-len2*T);

for i=len+len2+1:maxlen
    x(i)=-(i-len-len2-1)*T*v-v*time_after_circle;
    y(i)=-y0;  % 第二段勻速直線運動過程的坐標
end

% 不含噪位置坐標，用于存儲，方便后面分析結果  
xx=x;
yy=y;   
% 以上是仿真產生飛行軌跡
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 以下為Kalman濾波過程
% 由于對x,y是獨立進行觀測的，本程序分別對x,y坐標進行濾波
% 采用改進的“當前”統計模型
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 濾波初始狀態變量
x_o=[x(3),(x(3)-x(2))/T,(x(3)+x(1)-2*x(2))/T]';
y_o=[y(3),(y(3)-y(2))/T,(y(3)+y(1)-2*y(2))/T]';

% 初始濾波誤差方差矩陣
P0=zeros(3,3);
P0(1,1)=sigamx^2;
P0(1,2)=sigamx^2/T;
P0(2,1)=sigamx^2/T;
P0(2,2)=2*sigamx^2/T^2;
P0(3,3)=sigamx^2;

I=eye(3);

alfa=0.001; % 對應機動時間常數的倒數

% F為狀態轉移矩陣
F=[1,T,(alfa*T-1+exp(-alfa*T))/alfa^2;0,1,(1-exp(-alfa*T))/alfa;0,0,exp(-alfa*T)];

% 生成系統誤差協方差矩陣Q
q11=(1-exp(-2*alfa*T)+2*alfa*T+2*alfa^3*T^3/3-2*alfa^2*T^2-4*alfa*T*exp(-alfa*T))/alfa^4;
q12=(exp(-2*alfa*T)+1-2*exp(-alfa*T)+2*alfa*T*exp(-alfa*T)-2*alfa*T+alfa^2*T^2)/alfa^3;
q13=(1-exp(-2*alfa*T)-2*alfa*T*exp(-alfa*T))/alfa^2;
q22=(4*exp(-alfa*T)-3-exp(-2*alfa*T)+2*alfa*T)/alfa^2;
q23=(exp(-2*alfa*T)+1-2*exp(-alfa*T))/alfa;
q33=1-exp(-2*alfa*T);
q=[q11,q12,q13;0,q22,q23;0,0,q33];

% 矩陣U
u1=(-T+alfa*T^2/2+(1-exp(-alfa*T))/alfa)/alfa;
u2=T-(1-exp(-alfa*T))/alfa;
u3=alfa*(1-exp(-alfa*T));

U0=[u1,u2,u3]';

montekaluo=1; % 蒙特卡洛仿真指標
maxsimu=100;  % 蒙特卡洛仿真次數
while (montekaluo<=maxsimu)

x=xx+sigamx*randn(1,maxlen);    
y=yy+sigamy*randn(1,maxlen);   % 分別給x,y方向的數據加上噪聲

plot(x,y)
x_re=x; % x_re,y_re分別表示濾波后位置的估計
y_re=y;
        
sigam0=1; % 機動加速度方差初始值，在濾波過程中自適應調整
Q=sigam0^2*q; % 系統誤差協方差矩陣
H=[1,0,0]; % 測量矩陣
R=sigamx^2; % 觀測誤差協方差矩陣
U=U0;

%%%%%%%%%%%%%%%%%%%%%%
% 對x坐標進行濾波
X=x_o; % 位置初值
P=P0; % 濾波方差初值
step=4;
while(step<=maxlen)
    a=X(3);  % 用濾波值作為機動加速的估計，進行自適應調整
    X=F*X+U*a; 
    P=F*P*F'+Q; % 狀態預測
    S=H*P*H'+R; % 新息協方差 
    K=P*H'*inv(S); % 濾波增益
    Z=x(step); % 觀測向量
    Innovationx(step-3)=Z-X(1); % 計算預測產生的信息
    X1=X+K*(Z-H*X); % 濾波
    P=(I-K*H)*P; % 調整濾波方差矩陣
    sigam=(4-pi)/pi*(a+2/T^2*(X1(2)-X(2)))^2;  % 對機動加速度的方差進行自適應調整
    Tempx(step-3)=sigam; % 存儲機動加速度的方差，便于分析實驗結果
    Q=sigam*q; % 調整機動加速度協方差矩陣
    X=X1;
    x_re(step)=X(1); % 濾波的位置分量
    vx_re(step-2)=X(2); % 濾波后的速度分量
    ax_re(step-2)=X(3); % 濾波后的加速度分量
    step=step+1;
end
%%%%%%%%%%%%%%%%%%%%
% 對y坐標進行濾波
% 各行代碼的意義參見上述過程
Y=y_o;
P=P0;
R=sigamy^2;
step=4;
while(step<=maxlen)
    a=Y(3);
    Y=F*Y+U*a;
    P=F*P*F'+Q;
    S=H*P*H'+R;
    K=P*H'*inv(S);
    Z=y(step);
    Innovationy(step-3)=Z-Y(1);
    Y1=Y+K*(Z-H*Y);
    P=(I-K*H)*P;
    sigam=(4-pi)/pi*(a+2/T^2*(Y1(2)-Y(2)))^2;
    Tempy(step-3)=sigam;
    Q=sigam*q;
    Y=Y1;
    y_re(step)=Y(1);
    vy_re(step-2)=Y(2);
    ay_re(step-2)=Y(3);
    step=step+1;
end

%%統計各采樣點產生的新息
Innox(montekaluo,:)=Innovationx;
Innoy(montekaluo,:)=Innovationy;

%% 統計各采樣點位置濾波誤差
error1(montekaluo,:)=xx-x_re;
error2(montekaluo,:)=yy-y_re;

%% 統計各采樣點濾波后的速度分量
vx(montekaluo,:)=vx_re;
vy(montekaluo,:)=vy_re;

%% 統計各采樣點濾波后的機動加速度分量
ax(montekaluo,:)=ax_re;
ay(montekaluo,:)=ay_re;

%% 統計各采樣點機動加速度的方差
Tempxt(montekaluo,:)=Tempx;
Tempyt(montekaluo,:)=Tempy;


hold on
l1=plot(x_re,y_re,'r');
set(l1,'LineWidth',2);
title('Kalman濾波結果')

montekaluo=montekaluo+1;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 計算濾波過程中產生的新息
Innox_ave=sum(Innox)/maxsimu;
Innoy_ave=sum(Innoy)/maxsimu;

figure(2)
plot(1:maxlen-3,Innox_ave);
title('x方向新息');

figure(3)
plot(1:maxlen-3,Innoy_ave);
title('y方向新息');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 計算濾波后x,y坐標誤差均值與方差
err_x=sum(error1)/maxsimu;
err_y=sum(error2)/maxsimu;

sigam_x=std(error1);
sigam_y=std(error2);

n=1:1:maxlen;
figure(4)
plot(n,err_x)
title('x方向誤差的均值');

figure(5)
plot(n,err_y)
title('y方向誤差的均值');

figure(6)
plot(n,sigam_x)
title('x方向誤差的標準差');

figure(7)
plot(n,sigam_y)
title('y方向誤差的標準差');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 計算x,y方向各采樣點的速度與加速度
vx_ave=sum(vx)/maxsimu;
vy_ave=sum(vy)/maxsimu;

ax_ave=sum(ax)/maxsimu;
ay_ave=sum(ay)/maxsimu;

figure(8)
plot(1:maxlen-2,vx_ave)
title('x方向的速度');

figure(9)
plot(1:maxlen-2,vy_ave)
title('y方向的速度');

figure(10)
plot(1:maxlen-2,ax_ave)
title('x方向的加速度');

figure(11)
plot(1:maxlen-2,ay_ave)
title('y方向的加速度');



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 計算x,y方向各采樣點的機動加速度的方差
Tempx_ave=sum(Tempxt)/maxsimu;
Tempy_ave=sum(Tempyt)/maxsimu;

figure(12)
plot(1:maxlen-3,Tempx_ave);
title('x方向機動加速度的方差');

figure(13)
plot(1:maxlen-3,Tempy_ave);
title('y方向機動加速度的方差');