% extend kalman filter
clc;
clear;
tic;
T=1;    % 采樣周期
num=10;% 蒙特卡羅次數
N=120/T;% 采樣次數
 %目標運動方程
x=zeros(N,1); y=zeros(N,1);z=zeros(N,1);
vx=zeros(N,1);vy=zeros(N,1);vz=zeros(N,1);
x(1)=100;%目標初始參數
y(1)=50;
z(1)=5;
vx=0.4;vy=0.2;vz=0.01;
for i=1:N-1
    x(i+1)=x(i)+vx*T;
    y(i+1)=y(i)+vy*T;
    z(i+1)=z(i)+vz*T;
end

 %載機1運動方程
xo1=zeros(N,1); yo1=zeros(N,1);zo1=zeros(N,1);
vxo1=0.5;vzo1=0.01;R01=2;w=0.2/R01;ang=0;%載機初始參數        蛇行機動軌跡
xo1(1)=25;yo1(1)=15;zo1(1)=5;
for i=1:N-1
    xo1(i+1)=xo1(i)+vxo1*T;
    ang=ang+w*T;
    yo1(i+1)=15+R01*sin(ang) ;
    zo1(i+1)=zo1(i)+vzo1*T;
end

 
%載機2運動方程
xo2=zeros(N,1); yo2=zeros(N,1);zo2=zeros(N,1);
vxo2=0.45;vyo2=0.25;vzo2=0.01;%載機2初始參數
xo2(1)=22;yo2(1)=18;zo2(1)=3;
for i=1:N-1
    xo2(i+1)=xo2(i)+vxo2*T;
    yo2(i+1)=yo2(i)+vyo2*T ;
    zo2(i+1)=zo2(i)+vzo2*T;
end
figure(1);
plot3(x,y,z,'b-',xo1,yo1,zo1,'g:',xo2,yo2,zo2,'r-');axis([20 180 10 100 1 9]);grid;
 xlabel('x');
ylabel('y');
zlabel('z');
 %仿真測量值
w1=zeros(N,1);w2=zeros(N,1);w3=zeros(N,1);w4=zeros(N,1)
w1=0.01*randn(N,1);%方位角1測量噪聲 
w2=0.01*randn(N,1);%俯仰角1測量噪聲
w3=0.01*randn(N,1);%方位角2測量噪聲 
w4=0.01*randn(N,1);%俯仰角2測量噪聲
fwj1=zeros(N,1);fyj1=zeros(N,1);fwj2=zeros(N,1);fyj2=zeros(N,1);
for i=1:N
    fwj1(i)=atan((y(i)-yo1(i))/(x(i)-xo1(i)))+w1(i);%方位角1測量值
    fyj1(i)=atan(z(i)-zo1(i))/((x(i)-xo1(i))^2+(y(i)-yo1(i))^2)+w2(i);%俯仰角1測量值
    fwj2(i)=atan((y(i)-yo2(i))/(x(i)-xo2(i)))+w3(i);%方位角2測量值
    fyj2(i)=atan(z(i)-zo2(i))/((x(i)-xo2(i))^2+(y(i)-yo2(i))^2)+w4(i);%俯仰角2測量值
end
%濾波初始條件
%     a=(yo1(1)-yo2(1)-xo1(1)*tan(fwj1(1))+xo2(1)*tan(fwj2(1)))/(tan(fwj2(1))-tan(fwj1(1)));
%     b=yo1(1)+((yo1(1)-yo2(1)-xo1(1)*tan(fwj2(1))+xo2(1)*tan(fwj2(1)))/(tan(fwj2(1))-tan(fwj1(1))))*tan(fwj1(1));
%     c=tan(fyj1(1))*sqrt((b-yo1(1))^2+(a-xo1(1))^2)+zo1(1);
for m=1:num  %蒙特卡羅100次仿真
    o=6:6;h=3:6;q=4:4;xk=6:1;perr=6:6;zz=zeros(4,N);
    o=[1,0,0,T,0,0;0,1,0,0,T,0;0,0,1,0,0,T;0,0,0,1,0,0;0,0,0,0,1,0;0,0,0,0,0,1];
    q=[ 0.0001,0,0,0;0,0.0001,0,0;0,0,0.0001,0;0,0,0,0.0001];
    perr=[1000,0,0,0,0,0;
        0,1000,0,0,0,0;
        0,0,100,0,0,0;
        0,0,0,0.1,0,0;
        0,0,0,0,0.01,0;
        0,0,0,0,0,0.001]; %初始均方誤差
%     a=(yo1(1)-yo2(1)-xo1(1)*tan(fwj1(1))+xo2(1)*tan(fwj2(1)))/(tan(fwj2(1))-tan(fwj1(1)));
%     b=yo1(1)+((yo1(1)-yo2(1)-xo1(1)*tan(fwj2(1))+xo2(1)*tan(fwj2(1)))/(tan(fwj2(1))-tan(fwj1(1))))*tan(fwj1(1));
%     c=tan(fyj1(1))*sqrt((b-yo1(1))^2+(a-xo1(1))^2)+zo1(1);
%        xk=[a;b;c;0;0;0];
%     xk=[100;70;3;0.2;0.15;0.15];
   xk=[120;80;5;0.4;0.2;0.01];
   
    for r=1:N;%濾波過程
      xk1=o*xk;
      perr1=o*(perr)*o';
      rxy1=sqrt((xk1(2,1)-yo1(r))^2+(xk1(1,1)-xo1(r))^2);
      rxy2=sqrt((xk1(2,1)-yo2(r))^2+(xk1(1,1)-xo2(r))^2);
      rl=sqrt((xk1(2,1)-yo1(r))^2+(xk1(1,1)-xo1(r))^2+(xk1(3,1)-zo1(r))^2);
      r2=sqrt((xk1(2,1)-yo2(r))^2+(xk1(1,1)-xo2(r))^2+(xk1(3,1)-zo2(r))^2);
      fwj11=atan((xk1(2,1)-yo1(r))/(xk1(1,1)-xo1(r)));
      fyj11=atan((xk1(3,1)-zo1(r))/rxy1);
      fwj21=atan((xk1(2,1)-yo2(r))/(xk1(1,1)-xo2(r)));
      fyj21=atan((xk1(3,1)-zo2(r))/rxy2);
      zz(:,r)=[fwj1(r)-fwj11;fyj1(r)-fyj11;fwj2(r)-fwj21;fyj2(r)-fyj21] ;%實際值與測量值的差
      h=[-sin(fwj11)/rxy1 cos(fwj11)/rxy1 0 0 0 0;-cos(fwj11)*sin(fyj11)/rl -sin(fwj11)*sin(fyj11)/rl cos(fyj11)/rl 0 0 0;-sin(fwj21)/rxy2 cos(fwj21)/rxy2 0 0 0 0;;-cos(fwj21)*sin(fyj21)/r2 -sin(fwj21)*sin(fyj21)/r2 cos(fyj21)/r2 0 0 0];
      k=perr1*h'*inv(h*perr1*h'+q);
      xk=xk1+k*zz(:,r);
      perr=(eye(6)-k*h)*perr1;
      xks(r)=xk(1,1);
      yks(r)=xk( 2,1);
      zks(r)=xk(3,1);
      vkxs(r)=xk(4,1);
      vkys(r)=xk(5,1);
      vkzs(r)=xk(6,1);
      xk1s(r)=xk1(1,1);
      yk1s(r)=xk1(3,1);
      perr11(r)=perr(1,1);
      perr12(r)=perr(1,2);
      perr22(r)=perr(2,2);
      rex(m,r)=xks(r);
      rey(m,r)=yks(r);
      rez(m,r)=zks(r);
      vrex(m,r)=vkxs(r);
      vrey(m,r)=vkys(r);
      vrez(m,r) =vkzs(r);
  end 
end
ex=0;
ey=0;
ez=0;
vex=0;
vey=0;
vez=0;
xt=0;
yt=0;
zt=0;
ex1=N:1;ey1=N:1;ez1=N:1;rex1=N:1;%位置誤差變量
vex1=N:1;vey1=N:1;vez1=N:1;vksv=N:1;%速度誤差變量
xt1=N:1;yt1=N:1;zt1=N:1;%平均目標位置點
for i=2:N
    for j=1:num
        %位置誤差計算
        ex=ex+x(i)-rex(j,i);%真實位置減去濾波值的累計量
        ey=ey+y(i)-rey(j,i);
        ez=ez+z(i)-rez(j,i);
        %速度誤差計算
        vex=vex+vx-vrex(j,i);
        vey=vey+vy-vrey(j,i);
        vez=vez+vz-vrez(j,i);
        %估計軌跡計算
        xt=xt+rex(j,i);
        yt=yt+rey(j,i);
        zt=zt+rez(j,i);
    end 
    %位置.速度誤差平均值
    ex1(i)=ex/num;
    ey1(i)=ey/num;
    ez1(i)=ez/num;
    vex1(i)=vex/num;
    vey1(i)=vey/num;
    vez1(i)=vez/num;
    xt1(i)=xt/num;
    yt1(i)=yt/num;
    zt1(i)=zt/num;
    ex=0;  ey=0; ez=0;
    vex=0; vey=0; vez=0;
    xt=0;yt=0;zt=0;
    rex1(i)=sqrt( ex1(i)^2+ey1(i)^2+ez1(i)^2);%距離絕對誤差
%     error(i)=100*rex1(i)/RR(i);%距離相對誤差
   vksv(i)=sqrt(vex1(i)^2+vey1(i)^2+vez1(i)^2); %速度絕對誤差
%    error1(i)=100*vksv(i)/sqrt(vx^2+vy^2+vz^2);%速度相對誤差
 
end 

% %對蒙特卡羅結果取平均
% xpj=sum(rex)/100;
% ypj=sum(rey)/100;
% 
% 
% zpj=sum(rez)/100;


figure(2);
plot(rex1);axis([0 300 0 30]);grid; %距離絕對誤差曲線
  legend('距離絕對誤差曲線');
xlabel('采樣時間t（s）');
ylabel('距離絕對誤差(km)');
figure(3);
plot(vksv);axis([0 300 0 0.6]);grid; %速度絕對誤差曲線
  legend('速度絕對誤差曲線');
xlabel('采樣時間t（s）');
ylabel('速度絕對誤差(km)');
% figure(4);
% plot3(x,y,z,'b-', xpj,ypj,zpj,'r:');axis([100 300 50 150 2 10]);grid;

% figure(5);
% plot(ex1,'b');
%  axis([0 300 0 30]);
%  legend('X方向平均誤差');
% xlabel('采樣時間t（s）');
% ylabel('X方向平均誤差(m)');
% 
% figure(6);
% plot(ey1,'b');
%  axis([0 300 0 30]);
%  legend('Y方向平均誤差');
% xlabel('采樣時間t（s）');
% ylabel('Y方向平均誤差(m)');
% 
% figure(7);
% plot(ez1,'b');
%  axis([0 300 0 30]);
%  legend('Z方向平均誤差');
% xlabel('采樣時間t（s）');
% ylabel('Z方向平均誤差(m)');

time=toc