toa_ls.m

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

function [ex,ey, cost]=TOA_LS(radius,bx,by,d)
% estimate the position by TOA Least Square technology
% source:
% IEEE signal processing magazine,July 2005
% "network-based wireless location"
% input
% radius : radius of the home cell
% bx : x coordinate of base staionts
% by : y coordinate of base staionts
% d : distance to each base staions
% output
% ex : x coordinate of estimation position
% ey : y coordinate of estimation position
% written by Tang Hong, email: [e-mail]
% 2006.11.07
% the coordinate of the first base station must be (0,0),
% i.e. bx(1)=0; by(1)=0



    L=length(bx);
    
    H(1:L-1,2)=0;
    b(1:L-1,1)=0;
    for k=2:L;
        H(k-1,:)=[bx(k)-bx(1) by(k)-by(1)];
        b(k-1)=0.5*(bx(k)^2+by(k)^2-(bx(1)^2+by(1)^2)-d(k)^2+d(1)^2);
    end
    
    E=inv((H'*H))*H'*b;
%     E=inv((H'*H)+0.1*eye(2,2))*H'*b;
    
    ex=E(1);
    ey=E(2);
    
    cost=(H*E-b)'*(H*E-b);