function y=jiyutongji()
% complement the procedure of kalman filter
%2007.5.3
%jason

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; % 不含噪位置坐標
    
% x=xx+sigamx*randn(1,maxlen);    
% y=yy+sigamy*randn(1,maxlen);   % 分別給x,y方向的數據加上噪聲
% 
% plot(x,y)
% x_re=x;
% y_re=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.01;
F=[1,T,(alfa*T-1+exp(-alfa*T))/alfa^2;0,1,(1-exp(-alfa*T))/alfa;0,0,exp(-alfa*T)];

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];

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=10;
while (montekaluo<=maxsimu)

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

plot(x,y)
x_re=x;
y_re=y;
        
sigam0=1;
Q=sigam0^2*q;
H=[1,0,0];
R=sigamx^2;
U=U0;

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);
    X1=X+K*(Z-H*X);
    P=(I-K*H)*P;
    sigam=(X1(3)-X(3))^2;
%     sigam=(4-pi)/pi*(a+2/T^2*(X1(2)-X(2)))^2;
%     sigam=0;
    Q=sigam*q;
    X=X1;
    x_re(step)=X(1);
    step=step+1
end

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);
    Y1=Y+K*(Z-H*Y);
    P=(I-K*H)*P;
    sigam=(Y1(3)-Y(3))^2;
%     sigam=(4-pi)/pi*(a+2/T^2*(Y1(2)-Y(2)))^2;
%     sigam=0;
    Q=sigam*q;
    Y=Y1;
    y_re(step)=Y(1);
    step=step+1
end

error1(montekaluo,:)=xx-x_re;
error2(montekaluo,:)=yy-y_re;

% error1(montekaluo,:)=xx-x;
% error2(montekaluo,:)=yy-y;

hold on
plot(x_re,y_re,'r');

montekaluo=montekaluo+1;
end

err_x=sum(error1)/maxsimu;
err_y=sum(error2)/maxsimu;

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

n=1:1:maxlen;
figure(2)
plot(n,err_x)
title('error of x');

figure(3)
plot(n,err_y)
title('error of y');

figure(4)
plot(n,sigam_x)
title('std of x');

figure(5)
plot(n,sigam_y)
title('std of y');