constrained_weighted_nlos.m

这是在uwb系统中,用TOA定位法的整个系统的仿真程序

    clc;
    clear;
    
    n=4;      % 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

    
    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];
    
    p=[1 0 0;
        0 1 0;
        0 0 0];
    q=[0 0 -1]';
    
    A=[]; b=[];
    for an=1:n
        A(an,:)=[bx(an) by(an) -0.5];
    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)=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)=0.95*dt(loop,k)-0*delta*rand+00;         % 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)*100;
%             d(loop,k)=dt(loop,k)+noise(loop,k);         % distance measurement
        end
        clear k
        true_w=dt(loop,:)./d(loop,:);
        
        weights=weights_cal(bx(1:n),by(1:n),d);
        
        step=0.02; wn=0; w=[]; cost=[]; cost(1)=inf;
        theta=[]; 
        
%         for w1=weights(1):step:1
%             for w2=weights(2):step:1
%                 for w3=weights(3):step:1
%                     for w4=weights(4):step:1
                while min(cost)>10;
                        wn=wn+1;
%                         w(wn,:)=[w1 w2 w3 w4];
                        w(wn,:)=[weights(1)+rand*(1-weights(1)) weights(2)+rand*(1-weights(2)) weights(3)+rand*(1-weights(3)) weights(4)+rand*(1-weights(4))];
                        
%                         for bn=1:n
%                             b(bn,:)=0.5*[bx(bn)^2+by(bn)^2-(w(wn,bn)*d(bn))^2];
% %                             b(bn,:)=0.5*[bx(bn)^2+by(bn)^2-(true_w(bn)*d(bn))^2];
%                         end
%                         
%                         [t_cost t_theta]=cost_est(A,b,p,q);
%                         cost(wn)=t_cost;
%                         theta(wn,:)=t_theta';
                        
                        [tex,tey,tcost]=TOA_LS(radius,bx(1:n),by(1:n),w(wn,:).*d(1:n));
                        
                        theta(wn,:)=[tex tey];
                        cost(wn)=tcost;
                        
                        
                        wn
                 end
%                     end % w1
%                 end % w2
%             end % w3
%         end % w4
        
        [m,mi]=min(cost);
        ex=theta(mi,1)
        ey=theta(mi,2)
        [tx(loop) ty(loop)]
        err=sqrt((ex-tx(loop))^2+(ey-ty(loop))^2)
        
        w(mi,:)
        true_w
        
        loop
        
    end % loop