iterative_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=2000;    % 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.03,300,400))';
       end        
       
     for loop=1:m;
        
        txr=0.5*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
            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
        
        N=500;
        x=[];y=[];
%         x(1)=tx(loop)-600*(rand-0.5);y(1)=ty(loop)-600*(rand-0.5);
        x(1)=0; y(1)=0;

        step=0.1;    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)-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,:));
            
            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(tx(loop),ty(loop),'*','linewidth',2)
%         plot(x(1),y(1),'S','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)