usp.m

Euclidean节点定位是由美国路特葛斯大学利用距离矢量路由和GPS定位原理提供出一种分布式自主定位算法

%##################################################
%
%  RFUSP
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%parameters
clear;
DX=100;
DY=100;
DZ=100;
nodeNumber=1000;
comR=20;
percent=15/100;
anchorN=nodeNumber*percent;
TTL=sqrt(DX^2+DY^2+DZ+2)/comR;
anchor=4;
curren=0.5;
perror=0.01;
optR=sqrt((DX*DY/nodeNumber))/sqrt(2);
%%%%%%%%%init%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Countanchor=0;
for i=1:nodeNumber
    node(i).ID=i;
   if(Countanchor<anchorN) 
    node(i).type=0; % 0 is anchor,1 is normal;
    Countanchor=Countanchor+1;
   else
       node(i).type=1;
       end
   node(i).tPosition.x=rand(1,1)*DX;
   node(i).tPosition.y=rand(1,1)*DY;
   node(i).tPosition.z=rand(1,1)*DZ;
   if(node(i).type==0)
	node(i).ePosition.x= node(i).tPosition.x;
     node(i).ePosition.y= node(i).tPosition.y;
    node(i).ePosition.z= node(i).tPosition.z;
	node(i).finish=1;
   else
     node(i).ePosition.x=-1;
     node(i).ePosition.y=-1;
     node(i).ePosition.z=-1;
     node(i).finish=0;
     end
   node(i).Heigh=DZ-node(i).tPosition.z;
end
%{
%%%%%%%plot the node%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	for i=1:nodeNumber
 if(node(i).type==0)
     plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'r*');
 else
    plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'+');
 end
 hold on;
end
 grid on;
%%%%%%%%%%%%%%%%%%topoly establishment%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%}
connectivity=0;
 for i=1:nodeNumber
    k=0;
    nodeA=node(i);
     for j=1:nodeNumber
         nodeB=node(j);
         H=abs(nodeA.Heigh-nodeB.Heigh);
       if((j~=i)&& dst(nodeA,nodeB)<=comR && H<comR)
         k=k+1;
           neighbor(i).near(k)=j;
          end
     end
      node(i).neighbors=k;
      connectivity=connectivity+k;
 end
connect=0;
 for i=1:nodeNumber
     if node(i).neighbors==0
         connect=connect+1;
         node(i).finish=-1;
       i;
     end
 end
 connect
 connectivity=connectivity/nodeNumber
%{
%%%%%%%%%%%%%%%%%plot 3Dtopo%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:nodeNumber
    A=node(i);
    for j=1:node(i).neighbors
       B=node(neighbor(i).near(j));
        plotneighbor(A,B);
        %plotplane(A,B);
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%}
 for j=1:anchorN
     info(j).root(1).leaf(1)=j;
     info(j).root(1).n=node(j).neighbors;
     info(j).max=0;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:nodeNumber
   for j=1:anchorN
       myHop(i).hop(j)=-1;
       myHop(i).AHS(j)=-1;
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:anchorN
  vist=zeros(nodeNumber);
%  vistN=nodeNumber-1;
  vistN=0;
  vist(i)=1;
 %%%%% complete hop%%%%%%%%%%%%%%%%%%%%
 step=1;  %%%%hop%%%%%
 count=1;
  while step<=1.5*TTL
 %while vistN~=0
    step=step+1;
    kk=0;
    for j=1:count
        temp=info(i).root(step-1).leaf(j);
        pre=node(temp).neighbors;
      for cc=1:pre
         flag=neighbor(temp).near(cc);
        %H=abs(node(i).Heigh-node(flag).Heigh);
         if (vist(flag)==0 ) %&&H<=comR)
            kk=kk+1;
            info(i).root(step).leaf(kk)=flag;
           vist(flag)=1;
          end
   end
    end
   info(i).root(step).n=kk;
   count=kk;
   vistN=vistN+kk;
   if vistN==nodeNumber-1-connect
       break;
   end
 end
 info(i).max=step;
end
 
%
%%%%count hop%%%%%%%%%%%%%%%%%%%%%%
for i=1:anchorN
    for j=2:info(i).max  %%%%%j is the hop count%%%%%%
        for k=1:info(i).root(j).n
          if(length(info(i).root(j).leaf)~=0)
              my=info(i).root(j).leaf(k);
           myHop(my).hop(i)=j-1;
          end
        end
    end
end
%}  
%%% estimate the AHS%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:anchorN
    hop=0;
    distance=0;
     [xs,idx]=sort(myHop(i).hop);
    for j=1:anchor
       hop=hop+xs(j+1);
      distance=distance+dst(node(i),node(idx(j+1)));
        end
     AHS=distance/hop;
    myHop(i).AHS(i)=AHS;
end

%%%%flooding the AHS%%%%%%%%%%%%%%%%%%%
for i=1:nodeNumber
    for j=1:anchorN
        if(i~=j)
        myHop(i).AHS(j)=myHop(j).AHS(j);
        end
    end
end

%%%%%%%%%caculate the distance %%%%%%%%%%%%%%%%
err=0;
cc=0;
for i=1:nodeNumber
    cc1=0;
    if(node(i).type==1&&node(i).neighbors~=0)
    [xs,idx]=sort(myHop(i).hop);
    %%%%%% approach 1: the least one%%%%%%%%%%%%%%%%%
    idN=idx(1);
    AHS=myHop(i).AHS(idN);
   % AHS=optR;
    %%%%% approach 2: the avreage one%%%%%%%%%%%%%%
    %{
    AHS=0;
    for j=1:anchor
    AHS=AHS+myHop(i).AHS(j);
    end 
    AHS=AHS/anchor;
    %}
     for j=1:anchor
        %dist(i).dst(j).ed=AHS*xs(j);
         dist(i).dst(j).ed=sqrt((AHS*xs(j))+(node(i).Heigh-node(idx(j)).Heigh)^2);
       dist(i).dst(j).td=dst(node(i),node(idx(j)));
       err1=abs((dist(i).dst(j).td-dist(i).dst(j).ed))/dist(i).dst(j).td;
       if(err1>1)
           cc1=cc1+1;
       end
       err=err+err1;
       dist(i).dst(j).id=idx(j);
     end
    end
   if cc1~=0
       cc=cc+1;
   end
  end
  err=err/(nodeNumber-anchorN-connect);
  cc;
  %%%%% the first caculate %%%%%%%%%%%%%%%%%%%%%
  lineerr=0;
  for i=1:nodeNumber
      if (node(i).type==1 && node(i).finish~=-1)
          A=zeros(anchor-1,3);
          B=zeros(anchor-1,1);
          ex=zeros(3,1);
          n=-1;
            for j=1:anchor-1
              x1=node(dist(i).dst(j).id).tPosition.x;
              y1=node(dist(i).dst(j).id).tPosition.y;
               z1=node(dist(i).dst(j).id).tPosition.z;
              xn=node(dist(i).dst(anchor).id).tPosition.x;
              yn=node(dist(i).dst(anchor).id).tPosition.y;
               zn=node(dist(i).dst(anchor).id).tPosition.z;
              d1=dist(i).dst(j).ed;
              dn=dist(i).dst(anchor).ed;
              A(j,1)=2*(x1-xn);A(j,2)=2*(y1-yn);A(j,3)=2*(z1-zn);
              B(j,1)=(x1^2-xn^2)+(y1^2-yn^2)+(z1^2-zn^2)+(dn^2-d1^2);
          end
         % [ex,n]=locatCacul(A,B,0.001,1);
          % if rank(A)==rank([A B])
            ex=A\B;
           % if (ex(1,1)>0&&ex(1,1)<DX)&& (ex(2,1)>0&&ex(2,1)<DY)
               node(i).ePosition.z=abs(node(dist(i).dst(1).id).tPosition.z+(node(dist(i).dst(1).id).Heigh-node(i).Heigh));
               rate=abs(node(i).ePosition.z/ex(3,1));
               node(i).ePosition.x=abs(ex(1,1))*rate;
               node(i).ePosition.y=abs(ex(2,1))*rate;
               
               %node(i).ePosition.z=abs(ex(3,1));
              if node(i).finish~=-1
               node(i).finish=1;
             end
           
          %else
           %lineerr=lineerr+1;
         % end
      end
  end
  %
  %%%%%%%count %%%%%%%%%%%%%%%%%%%%%%%%%%%%
  myerror=0;
  outR=0;
  locait=0;
  for i=1:nodeNumber
      if (node(i).type==1 && node(i).finish==1)
      res=0;
      for j=1:anchor
          idx=dist(i).dst(j).id;
          d=dist(i).dst(j).ed;
          res=res+abs(dst(node(i),node(idx))-d);
      end
      res=res/anchor;
      if res>comR
          node(i).finish=-2;
          outR==outR+1;
      else
       myd=edst(node(i),node(i));
       if  myd<comR
        locait=locait+1;
        myerror=myerror+myd;
        node(i).finish=3; 
       end
      end
      end
  end
  %error=error/locait;
  %locait
  %outR
  ratio=locait/(nodeNumber-connect-anchorN)
  myerror=myerror/locait
  %}
  %{
%%%%%%%plot the node and ite estimate position%%%%%%%%%%%%%%%%%%%%%%
for i=1:nodeNumber
 if(node(i).type==0)
     plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'^');
 else
    if(node(i).finish==3)
    plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'o');
    ploterr(node(i),node(i));
    plot3(node(i).ePosition.x,node(i).ePosition.y,node(i).ePosition.z,'b*');
    else
     plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'kx');   
     end
 end
 hold on;
end
 grid on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%}
%%%%%%%%%%refine%%%%%%%%%%%%%%%%%%%%%%%%%%%
lineerr=0;
  for i=1:nodeNumber
      if (node(i).type==1 && node(i).finish~=3)
          n=0;
            for j=1:node(i).neighbors
                id= neighbor(i).near(j);
              if node(id).finish~=-1
                n=n+1;
                last=j;
              end
            end
            if n<3 
                continue;
            end
             A=zeros(n-1,3);
             B=zeros(n-1,1);
            ex=zeros(3,1);
            k=1;
             xn=node(last).ePosition.x;
              yn=node(last).ePosition.y;
               zn=node(last).ePosition.z;
               dn=dst(node(last),node(i))*(1-perror);
            for j=1:node(i).neighbors
                id= neighbor(i).near(j);
                 if node(id).finish~=-1 && j~=last
              x1=node(id).ePosition.x;
              y1=node(id).ePosition.y;
               z1=node(id).ePosition.z;
               d1=dst(node(id),node(i))*(1-perror);
               A(k,1)=2*(x1-xn);A(k,2)=2*(y1-yn);A(k,3)=2*(z1-zn);
              B(k,1)=(x1^2-xn^2)+(y1^2-yn^2)+(z1^2-zn^2)+(dn^2-d1^2);
              k=k+1;
                 end
            end
         % [ex,n]=locatCacul(A,B,0.001,1);
          % if rank(A)==rank([A B])
            ex=A\B;
           % if (ex(1,1)>0&&ex(1,1)<DX)&& (ex(2,1)>0&&ex(2,1)<DY)
              node(i).ePosition.z=abs(node(dist(i).dst(1).id).tPosition.z+(node(dist(i).dst(1).id).Heigh-node(i).Heigh));
              node(i).ePosition.x=abs(ex(1,1));
               node(i).ePosition.y=abs(ex(2,1));
               
               %node(i).ePosition.z=abs(ex(3,1));
              if node(i).finish~=-1
               node(i).finish=1;
              end
           %end
         % else
           lineerr=lineerr+1;
        end
  end
  %%%%%%%%count %%%%%%%%%%%%%%%%%%%%%%%%%%%%
  myerror=0;
  outR=0;
  locait=0;
  for i=1:nodeNumber
    if (node(i).type==1 && node(i).finish~=-1)
      myd=edst(node(i),node(i));
       if  myd<comR
        locait=locait+1;
        myerror=myerror+myd/comR;
       end 
    end
  end
  ratio=locait/(nodeNumber-connect-anchorN)
  myerror=myerror/locait