a_toa_analytically.m

Active approach - TOA

function A=A_TOA_analytically(N)
%
% T=A_TOA_analytically(N)
%
% Function gives back the coordinates of the transmitter computed
% analytically. 
%
% N - the input matrix, where the coordinates of the receiver and its  
%     distance from the transmitter are in every row. 
%     The size of the matrix N is (number of receivers)*4.
%
% A - the coordinates of the transmitter. 
%     


% aktivny pristup
% na vstupe bude matica rozmerov n*4, v ktorej budu suradnice prijimacov
% v 3D priestore a ich vzdialenosti od vysielaca

[rows columns] = size(N);
%T=zeros(rows*(rows-1)*(rows-2)/6,3);
n=1;
nula=10^(-10);
C=combination(rows,3);
[Crows Ccolumns]=size(C);
for i=1:Crows
    M=zeros(3,4);
    Q=zeros(rows-3,4);
    k=1;
    h=1;
    for j=1:Ccolumns
        if (C(i,j)>=nula)
            M(k,:)=N(j,:);
            k=k+1; 
        else 
            Q(h,:)=N(j,:);
            h=h+1;
        end
    end
    M;
    K=M-[M(1,1),M(1,2),M(1,3),0; M(1,1),M(1,2),M(1,3),0; M(1,1),M(1,2),M(1,3),0];
    L=Q-[M(1,1)*ones(rows-3,1),M(1,2)*ones(rows-3,1),M(1,3)*ones(rows-3,1),zeros(rows-3,1)];
    [p1,p2]=intersection(K);
    if (abs(imag(p2(1)))>=nula)
        if (abs(imag(p1(1)))<nula) 
            T(n,:)=p1+[M(1,1),M(1,2),M(1,3)];
            n=n+1;
        % else display('zmenit pozicie prijimacov')
        end
    else
        poc1=1;
        poc2=1;
        for k=1:(rows-3)
            if ((abs((sqrt((p1(1)-L(k,1))^2+(p1(2)-L(k,2))^2+(p1(3)-L(k,3))^2))-L(k,4))-(abs(sqrt((p2(1)-L(k,1))^2+(p2(2)-L(k,2))^2+(p2(3)-L(k,3))^2)-L(k,4))))>=nula)
                poc2=poc2+1;
            else poc1=poc1+1;
            end
        end
        if (abs(poc1-poc2)<nula)
            T(n,:)=[(p1(1)+p2(1))/2,(p1(2)+p2(2))/2,(p1(3)+p2(3))/2]+[M(1,1),M(1,2),M(1,3)];
            n=n+1;
        else
            if ((poc1-poc2)>nula)
                T(n,:)=p1+[M(1,1),M(1,2),M(1,3)];
                n=n+1;
            else
                T(n,:)=p2+[M(1,1),M(1,2),M(1,3)];
                n=n+1;
            end
        end
    end
end
T;
A=mean(T);

%% Date: 03.04.2007
%% Author: Maria Riskova