minmax.m

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

%##################################################
%
%  RFUSP
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%parameters
clear;
DX=100;
DY=100;
DZ=100;
nodeNumber=1000;
comR=15;
percent=7/100;
anchorN=nodeNumber*percent;
TTL=sqrt(DX^2+DY^2+DZ+2)/comR;
anchor=3;
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+1;
 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)
          mm1=zeros(anchor,3);
          mm2=zeros(anchor,3);
           for j=1:anchor
              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;
               d1=dist(i).dst(j).ed;
              mm1(j,1)=abs(x1-d1);mm1(j,2)=abs(y1-d1);mm1(j,3)=abs(z1-d1);
              mm2(j,1)=abs(x1+d1);mm2(j,2)=abs(y1+d1);mm2(j,3)=abs(z1+d1);
           end
              ex1=zeros(3);
              ex2=zeros(3);
              ex1=max(mm1); 
              ex2=min(mm2);
              node(i).ePosition.z=abs(node(dist(i).dst(1).id).tPosition.z+(node(dist(i).dst(1).id).Heigh-node(i).Heigh));
              bias=abs(ex1(1)-ex2(1))/2/node(i).ePosition.z;
               node(i).ePosition.x=abs(ex1(1)-ex2(1))/2/bias;
               node(i).ePosition.y=abs(ex1(2)-ex2(2))/2/bias;
               
               node(i).finish=1;         
      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
         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
  outR
  %}
  %{
%%%%%%%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;
%}