% Lab#3ver.2: Extended KF Algorithm
% by D.C.Chang NCU/CE  2021/3/9
% 
clear all

T=0.1;    %sampling period, sec
num=300;
%% Target curve nad measurement generation.
x=zeros(1,num);
y=zeros(1,num);
x(1)=100;        % m
y(1)=300;        % m
xv=30;           % m/sec
yv=-5;           % m/sec
for n=2:num
    x(n)=x(n-1)+T*xv;
    y(n)=y(n-1)+T*yv;
end
x_noise=20*randn(1,num);               % std 20 m 
y_noise=20*randn(1,num);               % std 20 m
xn=x+x_noise;
yn=y+y_noise;
r=sqrt(xn.^2+yn.^2);
theta=atan(yn./xn);

%%The EKF Tracking Algorithm
xb=[110 25 250 -10]';
P=diag(ones(1,4));
phi=[1 T 0 0
     0 1 0 0
     0 0 1 T
     0 0 0 1];
 Q=diag([1 1 1 1]);
 R=[20^2  0            %range error std 20 m
     0    0.025^2];    %theta error std 0.025 rad (about 1.4 degrees)
 xm=zeros(num,4);
 xm(1,:)=xb;
 for n=2:num
     xb=phi*xb;          %xb: state vector
     %-----Extended Kalman Filter--------------------    
     P=
     H=   
     K=
     z=[r(n) theta(n)]';
     xb=
     P=
     %%--------------------------------------
     xm(n,:)=xb;
 end
 
figure(1)
plot(x,y,'k')
hold on
plot(r.*cos(theta), r.*sin(theta),':r') 
plot(xm(:,1),xm(:,3),'b')
hold off
legend('real track','measurement data','EKF tracking curve')
axis([100 1000 0 400])
xlabel('x-axis (m)')
ylabel('y-axis (m)')
hold off

figure(2)
plot(xv*ones(1,num),'--b')
hold on
plot(yv*ones(1,num),'--r')
plot(xm(:,2),'b') 
plot(xm(:,4),'r')
legend('real velocity on x-axis','real velocity on y-axis','estimated velocity on x-axis','estimated velocity on y-axis')
xlabel('number of samples')
ylabel('velocity (m/sec)')
hold off