clc
clear
N=80;       %time
t = 1:1:N;            
% x0=[50 -30 15000  -50]';%x0 = [0 230 15000 126]';
x0=[100;9.62;200;5.56];
x(:,1) = x0;           % Initial state. 
D=1000;
T=1;
% Q=[T^3/3,T^2/2,0,0;T^2/2,T,0,0;0,0,T^3/3,T^2/2;0,0,T^2/2,T]; % Process noise variance.
Rv=[30 0;0 50];
Rw=[3 0 0 0;0 3 0 0;0 0 3 0;0 0 0 3]; 
% Rw=[T^3/3,T^2/2,0,0;T^2/2,T,0,0;0,0,T^3/3,T^2/2;0,0,T^2/2,T];
p =[30 0 0 0;0 30 0 0;0 0 30 0;0 0 0 30];%x的統計特性，p即為方差
F=[1 1 0 0
    0 1 0 0
    0 0 1 1
    0 0 0 1]; 

% GENERATE PROCESS AND MEASUREMENT NOISE:
% ======================================
w = sqrt(Rw)*randn(4,N);
v =0.5*pi/180*randn(1,N);
for t=1:N
   
        if t==1
            x(:,t)=F*x0+w(:,t);%生成目標i的狀態
        else
            x(:,t)=F*x(:,t-1)+w(:,t);%生成目標i的狀態
        end
        y(1,t)=acos(x(1,t)/sqrt(x(1,t)^2+x(3,t)^2))+v(:,t);%基陣A對目標i的測向角αi
        y(2,t)=acos((x(1,t)-D)/sqrt((x(1,t)-D)^2+x(3,t)^2))+v(:,t);%基陣B對目標i的測向角βi
end
%sigma function
for i=1:N
    [X,Y]=sigma(x(:,i),p,F,Rw);
    [x_gj,y_gj,Px,Py,K]=guji(X,Y,Rw,Rv);
    x1(:,i)=x_gj+K*(y(:,i)-y_gj);
    p1(:,:,i)=Px-K*Py*K';
    p=p1(:,:,i);
end


%  draw the picture
figure(1)
plot(1:80,x(1,:),'b:',1:80,x1(1,:),'r+')
figure(2)
plot(1:80,x(2,:),'b:',1:80,x1(2,:),'r+')
figure(3)
plot(1:80,x(3,:),'b:',1:80,x1(3,:),'r+')
figure(4)
plot(1:80,x(4,:),'b:',1:80,x1(4,:),'r+')
figure(5)
%plot(x(1,:),x(3,:),'g-*',prediction(1,:),prediction(3,:),'r-+')%,r(1,:),r(2,:),'b-'
plot(x(1,:),x(3,:),'b:',x1(1,:),x1(3,:),'r+')   
figure(6)
subplot(221)
plot(1:length(x),abs(x1(1,:)-x(1,:)),'b')
ylabel('x方向位置誤差','fontsize',12);
xlabel('時間/s','fontsize',12);

subplot(222)
plot(1:length(x),abs(x1(2,:)-x(2,:))/10,'b')
ylabel('x方向速度誤差','fontsize',12);
xlabel('時間/s','fontsize',12);

subplot(223)
plot(1:length(x),abs(x1(3,:)-x(3,:)),'b')
ylabel('y方向位置誤差','fontsize',12);
xlabel('時間/s','fontsize',12);


subplot(224)
plot(1:length(x),abs(x1(4,:)-x(4,:)),'b')
ylabel('y方向速度誤差','fontsize',12);
xlabel('時間/s','fontsize',12);