huber_m_nlos.asv

这个程序是TD-S-CDMA系统中,TDOA定位方法的系统程序.

% NLOS mitigation based on Huber M estimation
% 2007.3.18
% written by Tang Hong
% IEEE Trans. On signal processing,vol47,No.4, 1999
% " robust Huber adaptive filter"

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

       noise(1:m,1:n)=0;
       for a=1:n
           noise(:,a)=(add_noise((0.5+0.5*rand(1,m))*300,0.03,300,400))';
       end 
    
    radius=1000;

    bx=[   1.5*radius         0               -1.5*radius        -1.5*radius          0               1.5*radius];
    by=[   sqrt(3)*radius/2   sqrt(3)*radius   sqrt(3)*radius/2   -sqrt(3)*radius/2  -sqrt(3)*radius  -sqrt(3)*radius/2];
       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)=50;  ty(loop)=80;
        
        for k=1:n
            dt(loop,k)=sqrt((tx(loop)-bx(k))^2+(ty(loop)-by(k))^2);
%             d(loop,k)=dt(loop,k)+1*delta*randn+100;         % distance measurement
            d(loop,k)=dt(loop,k)+exprnd(1,1,1)*100;         % distance measurement
%             d(loop,k)=dt(loop,k)+rand*100;         % distance measurement
        end
        clear k
 
    
        step=50; ran=300;
        
        cx=0;cy=0;
        for xi=-radius:step:radius
            cx=cx+1;cy=0;
            for yi=-radius:step:radius
                cy=cy+1;  h_err=0;
                err=(sqrt((xi-bx(1:n)).^2+(yi-by(1:n)).^2)-d(loop,:));
                for k=1:n;
                    h_err=h_err+Huber_M(err(k),2*delta);
%                     h_err=h_err+(err(k)^2);
%                     h_err=h_err+abs(err(k));
                end
                out(cx,cy)=sum(h_err);
            end
        end
        
        
            xi=-radius:step:radius;
            yi=-radius:step:radius;
            
            m_n=min(min(out));
            
        for a=1:length(xi);
            for b=1:length(yi);
                if out(a,b)==m_n
                    est_x=xi(a);
                    est_y=yi(b);
                end
            end
        end
                
        
        [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)
    
        xi=tx(loop)-ran:step:tx(loop)+ran;
        yi=ty(loop)-ran:step:ty(loop)+ran;
        
%         figure,
%         subplot(1,2,1)
%         mesh(xi,yi,out)
%         subplot(1,2,2),hold on
%         [C,h] =contour(xi,yi,out);
%         clabel(C,h),grid on
%         plot(tx,ty,'*','linewidth',2)