% Trilateracija v 2D
% Dodatek k vaji 3 pri predmetu ITS

% razliki oddaljenosti
s12=0.66;
s23=0.19;

% lege oddajnikov
ox=[0 2 5];
oy=[0 4 0];

%razdalji med oddajnikoma
l12=sqrt((ox(2)-ox(1))^2+(oy(2)-oy(1))^2);
l23=sqrt((ox(3)-ox(2))^2+(oy(3)-oy(2))^2);

%parametri hiperbole
c12=(l12^2-s12^2)/4;
c23=(l23^2-s23^2)/4;
d12=1.0/sqrt((l12/s12)^2-1);
d23=1.0/sqrt((l23/s23)^2-1);
%središèe hiperbole
dx12=(ox(2)-ox(1))/2;
dy12=(oy(2)-oy(1))/2;
dx23=(ox(3)-ox(2))/2;
dy23=(oy(3)-oy(2))/2;
x012=ox(1)+dx12;
y012=oy(1)+dy12;
x023=ox(2)+dx23;
y023=oy(2)+dy23;
%kota rotacije hiperbole in ustrezne kotne funkcije
f12=atan2(dy12,dx12);
f23=atan2(dy23,dx23);
c12=cos(f12);
s12=sin(f12);
c23=cos(f23);
s23=sin(f23);

%parametrizacija
t=linspace(-10,10,1000);
%koordinate hiperbol v lastnem koordinatnem sistemu
x10=d12*sqrt(t.*t+c12);
x20=d23*sqrt(t.*t+c23);
y0=t;
%translacija in rotacija
x1=x012+x10*c12-y0*s12;
y1=y012+x10*s12+y0*c12;
x2=x023+x20*c23-y0*s23;
y2=y023+x20*s23+y0*c23;

%narišemo sliko:
%-oddajnike z rdeèo zvezdo
%-hiperbolo med oddajnikoma 1 in 2 z modro barvo
%-hiperbolo med oddajnikoma 2 in 3 z zeleno barvo
plot(ox,oy,'r*',x1,y1,'b-',x2,y2,'g-');
axis([-1 6 -1 5]);

