weighted_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=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
    
    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;
       
       noise(1:m,1:n)=0;
       for a=1:n
           noise(:,a)=(add_noise((0.5+0.5*rand(1,m))*300,0.01,300,400))';
       end
       
     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)*200;         % distance measurement
            d(loop,k)=dt(loop,k)+(0.5+0.5*rand)*300;         % distance measurement
%             d(loop,k)=dt(loop,k)+300;         % distance measurement
%             d(loop,k)=dt(loop,k)+noise(loop,k);         % distance measurement
        end
        clear k
        
        N=10;
        x=[];y=[];
%         x(1)=tx(loop)-600*(rand-0.5);y(1)=ty(loop)-600*(rand-0.5);
        x(1)=0; y(1)=0;
%         x(1)=1000*rand; y(1)=1000*rand;

        w(1:N,1:n)=0;     w(1,:)=1;
        step=0.02;
        di(1:N,1:n)=0; di(1,:)=d(loop,:);
        flag=0; in_loop=0;
        
        while flag==0;
            in_loop=in_loop+1;
            te(in_loop,:)=sqrt((x(in_loop)-bx(1:n)).^2+(y(in_loop)-by(1:n)).^2)-w(in_loop,:).*d(loop,:);
            
            x(in_loop+1)=x(in_loop)-step*sum(te(in_loop,:).*(x(in_loop)-bx(1:n))./(d(loop,:)));
            y(in_loop+1)=y(in_loop)-step*sum(te(in_loop,:).*(y(in_loop)-by(1:n))./(d(loop,:)));
            
            w(in_loop+1,:)=sqrt((x(in_loop+1)-bx(1:n)).^2+(y(in_loop+1)-by(1:n)).^2)./(w(in_loop,:).*d(loop,:));
            if max(w(in_loop+1,:))>1
                w(in_loop+1,:)=w(in_loop+1,:)/((1+0.1)*max(w(in_loop+1,:)));       %normalized
            end
            
            if in_loop > 1e4 || ( in_loop > 100 && abs(mean(diff(x(round(0.8*in_loop):in_loop)))) < 1e-2 && abs(mean(diff(y(round(0.8*in_loop):in_loop)))) < 1e-2 )
                flag=1;
            end             
            
        end
                
%         figure,hold on,grid on
%         plot(x,y,'linewidth',2)
%         plot(x(1),y(1),'S','linewidth',2)
%         plot(tx(loop),ty(loop),'*','linewidth',2)

        est_x=mean(x(round(0.9*in_loop):in_loop));
        est_y=mean(y(round(0.9*in_loop):in_loop));
        
        [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)