iterative_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=500;    % 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;
       
     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;
        end
        clear k
        
        N=500;
        x(1:N)=0;y(1:N)=0;
        x(1)=tx(loop)-600*(rand-0.5);y(1)=ty(loop)-600*(rand-0.5);
        step=0.01;
        for in_loop=1:N
            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,:));
        end
                
        figure,hold on,grid on
        plot(x,y,'linewidth',2)

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