% 
% 2007.4.1
    
    clc;
    clear;
    
    n=4;      % the number of base stations
    m=100;    % the number of measurements
    
    delta=60;    % variance of measurement noise
    
%     bx(1:n)=0; by(1:5)=0;   % the coordinate of base staions

    
    radius=1000;

    bx=[0   1.5*radius         0               -1.5*radius        -1.5*radius          0               1.5*radius];
    by=[0   sqrt(3)*radius/2   sqrt(3)*radius   sqrt(3)*radius/2   -sqrt(3)*radius/2  -sqrt(3)*radius  -sqrt(3)*radius/2];
    
    p=[1 0 0;
        0 1 0;
        0 0 0];
    q=[0 0 -1]';
    
       noise(1:m,1:n)=0;
       for a=1:n
           noise(:,a)=(add_noise((0.5+0.5*rand(1,m))*300,0.01,800,1000))';
       end     
    
       dt(1:m,1:n)=0;
       d(1:m,1:n)=0;
       
     for loop=1:m;
        
        txr=1*rand*radius;    tx_angle=rand*2*pi;
        tx(loop)=txr*cos(tx_angle);  ty(loop)=txr*sin(tx_angle);
%         tx(loop)=500;  ty(loop)=800;

        for k=1:n
            dt(loop,k)=sqrt((tx(loop)-bx(k))^2+(ty(loop)-by(k))^2);
%             d(loop,k)=0.95*dt(loop,k)-0*delta*rand+00;         % distance measurement
%             d(loop,k)=dt(loop,k)+exprnd(1,1,1)*100;         % distance measurement
            d(loop,k)=dt(loop,k)+(0.5+0.5*rand)*300;
%             d(loop,k)=dt(loop,k)+noise(loop,k);         % distance measurement
        end
        clear k
            
        w=[]; w(1:n,1)=1;
        
            A(1:n,1:3)=0;
            for k=1:n
                A(k,:)=[bx(k) by(k) -0.5];
            end
        b(1:n,1)=0; n_f=0; flag=0;
           
            while flag==0
                n_f=n_f+1;
                b=0.5*[ (bx(1:n).^2)'+(by(1:n).^2)'-(w(:,n_f).*d(loop,:)').^2];
                mroot=root_find(b,bx(1:n),by(1:n),p,q);
        
                if length(mroot)>=1 && mroot(1)~= -1
                    theta=inv(A'*A+mroot*p)*(A'*b-0.5*mroot*q);
                    x(n_f)=theta(1);
                    y(n_f)=theta(2);
                end
                est_d=sqrt((x(n_f)-bx(1:n)).^2+(y(n_f)-by(1:n)).^2);
%                 w(:,n_f+1)=(est_d./(w(:,n_f)'.*d(loop,:)))';
                w(:,n_f+1)=(est_d./(d(loop,:)))';
%                 if max(w(:,n_f+1))>=1
%                     w(:,n_f+1)=w(:,n_f+1)/((1+0.1)*max(w(:,n_f+1)));       %normalized
%                 end                 
                
                if n_f<1000 && n_f>100 && abs(mean(diff(x(round(0.9*n_f):n_f))))<5e-2 && abs(mean(diff(y(round(0.9*n_f):n_f))))<5e-2
                    flag=1;
                end
            end
            
            est_x=mean(x(round(0.9*n_f):n_f));
            est_y=mean(y(round(0.9*n_f):n_f));
        
                [tx(loop) ty(loop)];
                [est_x est_y];
                err_Huber(loop)=sqrt((tx(loop)-est_x)^2+(ty(loop)-est_y)^2);
                [ex,ey]=TOA_LS(radius,bx(1:n),by(1:n),d(loop,:));
                [ex,ey];
                err_LS(loop)=sqrt((tx(loop)-ex)^2+(ty(loop)-ey)^2);
        
        loop
    end
        
    sqrt(sum(err_Huber.^2)/m)
    sqrt(sum(err_LS.^2)/m)