clear all
ux=0.01; uy=0.01; % m/s^2
t=4; % sampling time in sec
% Mat
PHI=[1 t 0 0;0 1 0 0;0 0 1 t;0 0 0 1];
Ta=[t^2/2 0; t 0; 0 t^2/2; 0 t];
H=[1 0 0 0;
   0 0 1 0];
UXY=[ux; uy];
%% Plot Screen
xmin=-40000; xmax=40000;
ymin=-40000; ymax=40000;
axis equal;
axis([-40000 40000 -40000 40000]);
grid;
hold on
xlabel('X-axis Km');
ylabel('Y-axis Km');
title('Target Motion')
plot([xmax xmin],[(ymax+ymin)/2 (ymax+ymin)/2],'g');
plot([(xmax+xmin)/2 (xmax+xmin)/2],[ymax ymin],'g');
radar=[0 0; -40000 -40000; -35000 35000];
% Radar No.1
xr1=0;yr1=0;text(xr1+1000,yr1,'Radar No.1')
plot([-1000+xr1 1000+xr1],[1000+yr1 1000+yr1],'r',[1000+xr1 1000+xr1],[1000+yr1 -1000+yr1],'r',[1000+xr1 -1000+xr1],[-1000+yr1 -1000+yr1],'r',[-1000+xr1 -1000+xr1],[-1000+yr1 1000+yr1],'r')

mean_t=4; %stdev_t=0.134;
SEED=1;MAXTARGET=2;Maxscan=80;gt1=4;
% Target No.1
TSTART(1)=1; TSTOPS(2)=50;
XI(1,1)=-30000;YI(1,1)=-11800;
VX(1,1)=300;VY(1,1)=0;Ldot(1)=-6*pi/180;
% Target No.2
TSTART(1)=1; TSTOPS(2)=50;
XI(1,2)=-30000; YI(1,2)=-10000;
VX(1,2)=300; VY(1,2)=0; Ldot(2)=-6*pi/180;
n=1;
fid=fopen('datafile1.mat','wt+','n');

for j=1:Maxscan
   if j <= 20
      [XI,YI,VX,VY] = linear1(XI,YI,VX,VY,gt1,SEED,MAXTARGET,TSTART,TSTOPS);
   elseif (20 < j) & (j <= 60)
      [XI,YI,VX,VY]=weave1(XI,YI,VX,VY,gt1,Ldot,SEED,MAXTARGET,TSTART,TSTOPS);
   else 
      [XI,YI,VX,VY] = linear1(XI,YI,VX,VY,gt1,SEED,MAXTARGET,TSTART,TSTOPS);
   end
   for tg = 1:MAXTARGET
     range2 = sqrt((YI(1,tg)-radar(1,2))^2 +(XI(1,tg)-radar(1,1))^2);
     theta2 = atan2 ((YI(1,tg)-radar(1,2)) ,(XI(1,tg)-radar(1,1)))*180/pi;
     if theta2 < 0
        theta2 = 360 + theta2;
     end   
     mean_range2=0; stdev_range2=5; mean_bearing2=0; stdev_bearing2=1;
     r_noise2 = mean_range2 +(stdev_range2*randn(1));
     bearing_noise2 = mean_bearing2 +(stdev_bearing2*randn(1));
     range2  = range2 + r_noise2;
     theta2  = theta2 + bearing_noise2;
     rv_time = (theta2/90);
     %[tg rv_time range2  r_noise2  theta2  bearing_noise2]
     XIi(1,tg) = radar(1,1) + range2*cos(theta2*pi/180);
     YIi(1,tg) = radar(1,2) + range2*sin(theta2*pi/180);
     data_rec(tg,:) = [j tg rv_time XIi(1,tg) YIi(1,tg) XI(1,tg) YI(1,tg)];
     n=n+1;
     x1(j,tg)=XIi(1,tg);
     y1(j,tg)=YIi(1,tg);
     x2(j,tg)=XI(1,tg);
     y2(j,tg)=YI(1,tg);
  end
  data_rec_s = sortrows( data_rec,3);
  for ii = 1:MAXTARGET % Store target data in a file datafile1.mat
     % Scan_No target_No revisit_time x-pos+noise y-axis+noise x-pos_true y-pos_true 
     fprintf(fid,'%3.0f\t %2.0f\t %1.3f\t  %6.1f\t %6.2f\t %6.1f\t %6.1f\t \n', data_rec_s(ii,:));
  end
end
fclose(fid);
plot(x1(:,1),y1(:,1),'b',x2(:,1),y2(:,1),'r',x1(:,2),y1(:,2),'g',x2(:,2),y2(:,2),'y');