%this funcation is for a 2-dim, noline system ekf 
function a=EKF
clear all;
close all;
clc;
format long g;
%initialization parameter
temp6=zeros(4,200,100);
for n=1:100
T=1;
A=[1 0 T 0;0 1 0 T;0 0 1 0;0 0 0 1];
%B=[T 0;0 T;1 0;0 1;0 0];
Q=diag([10 10 5 5]);
R=diag([0.5 0.01]);
X0=[25050 80050 295 205]';
P0=diag([200 200 10 10]);
aim0=[25000 80000 300 200 ]';   %aim's initialization position
%Y=zeros(2,100);
%SET parameter


%real aim race
temp3=zeros(4,200);
temp2=zeros(4,200);
%temp2(:,1)=X0;
%觀測器的軌跡
observation=zeros(4,200);
k0=1:50;
observation_x1=10000+(1/2)*5*k0.^2;
observation_vx1=5*k0;
k1=1:100;
observation_x2=observation_x1(50)-(1/2)*5*k1.^2+observation_vx1(50)*k1;
observation_vx2=observation_vx1(50)-5*k1;
k2=1:50;
observation_x3=observation_x2(100)+observation_vx2(100)*k2+(1/2)*5*k2.^2;%[1 0 k2 0 (1/2)*k2.^2;0 1 0 k2 0;0 0 1 0 k2;0 0 0 1 0;0 0 0 0 1]*[10000 0 0 0 5]';
observation_vx3=observation_vx2(100)+5*k2;
TEMP=zeros(1,200);
observation=[observation_x1 observation_x2 observation_x3;TEMP;observation_vx1 observation_vx2 observation_vx3;TEMP];%4*200
%目標運動的軌跡
aim=zeros(4,200);
kk=1:200;
aim_x=25000+300*kk;
aim_y=80000+200*kk;
aim_vx=300*ones(1,200);
aim_vy=200*ones(1,200);
aim=[aim_x;aim_y;aim_vx;aim_vy];
%相對位置
temp3=aim-observation;
temp4=zeros(2,200);
temp5=zeros(2,200);

noise_beta=sample_gaussian(0,R(1,1),200);
noise_beta1=sample_gaussian(0,R(2,2),200);
noise_observation=[noise_beta';noise_beta1'];

beta(:,kk)=atan(temp3(1,kk)/temp3(2,kk));
for kkk=1:200
   beta1(:,kkk)=(temp3(3,kkk)*temp3(2,kkk)-temp3(4,kkk)*temp3(1,kkk))/(temp3(1,kkk)^2+temp3(2,kkk)^2);
end
value_observation=[beta;beta1];

temp4=value_observation+noise_observation;

%temp3=re_aim();%真實軌跡
%main
for k=1:200
  %Y=zeros(2,100); 
  %Y=construct(temp3(:,k)',R);   %2*100構造真實的觀測量aim0',/////////////角度變化率接近0.???????
  % temp4(:,k)=mean(Y,2);
  
  
  H=zeros(2,4);
  H=calculate(X0',aim(:,k)'); %計算H
  P1=zeros(4,4);
  P1=A*P0*A'+Q;    %B**inv(B)
  %HT=H';
  %KK1=H*P1*H'+R;
  %KKK=inv(H*P1*H'+R);
  KK=zeros(4,2);
  KK=P1*H'*(H*P1*H'+R)^-1;
  I=eye(4);
  %ZZ=I-KK*H;
  P2=zeros(4,4);
  P2=(I-KK*H)*P1*(I-KK*H)'+KK*R*KK';  %修正后的誤差方差
  P0=zeros(4,4);
  P0=P2;
  YY=zeros(2,1);
  YY=esob(X0',aim(:,k)');     %估計的觀測值
  temp5(:,k)=YY;
  temp1=zeros(4,100);
  X1=zeros(4,1);
  X1=A*X0;
  %for i=1:100
      X2=zeros(4,1);
      X2=X1+KK*(temp4(:,k)-YY);
      %temp1(:,i)=X2;
  %end
  X0=zeros(4,1);
  %X0=mean(temp1,2);
  X0=X2;
  
  temp2(:,k)=X0;
end
aa=zeros(4,200);
for jj=1:200
    aa(:,jj)=abs(aim(:,jj)-temp2(:,jj)+[300,200,0,0]');
end
temp6(:,:,n)=aa;
%for ii=1:100
   % aa=0;
   % aa=a(:,ii);
   % a(:,ii)=0;
   % a(:,ii)=a(:,201-ii);
   % a(:,201-ii)=0;
   % a(:,201-ii)=aa;
%end
end
a=mean(temp6,3);
bb=1:200;
figure(6);
plot(observation(3,:));
figure(1); 
plot(bb,a(1,:));
grid;
%axis([0,200,0,300]);
figure(2);
plot(bb,a(2,:));
grid;
%axis([0,200,0,200]);
figure(3);
plot(bb,a(3,:));
grid;
%axis([0,200,0,15]);
figure(4);
plot(bb,a(4,:));
grid;
%axis([0,200,0,15]);
figure(5);
plot(aim(1,:),aim(2,:));
hold on;
plot(temp2(1,:),temp2(2,:),'r');
figure(7);
plot(bb,temp4(1,:),'r');
hold on;
plot(bb,temp5(1,:));
figure(8);
plot(bb,temp4(2,:),'r');
hold on;
plot(bb,temp5(2,:));