huber_m_nlos.m

这个程序是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=3;      % the number of base stations
    m=1;    % 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)=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)=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)+(0.5+0.5*rand)*300;
%             d(loop,k)=dt(loop,k)+noise(loop,k);         % distance measurement
        end
        clear k
 
    
        step=50; ran=300;
        
        cx=0;cy=0;
        for xi=-1*radius:step:1*radius
            cx=cx+1;cy=0;
            for yi=-1*radius:step:1*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=-1*radius:step:1*radius;
            yi=-1*radius:step:1*radius;
            
            m_n=min(min(out));
            
        for k1=1:length(xi);
            for k2=1:length(yi);
                if out(k1,k2)==m_n
                    est_x=xi(k1);
                    est_y=yi(k2);
                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=-1*radius:step:1*radius;
            yi=-1*radius:step:1*radius;;
        
        figure,
        subplot(1,2,1)
        meshc(xi,yi,out')
        subplot(1,2,2),hold on
        [C,h] =contour(yi,xi,out');
        clabel(C,h),grid on
        plot(tx,ty,'*','linewidth',2)
        plot(est_x,est_y,'^','linewidth',2)