hea.m

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

%##################################################
%
%  RFUSP
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%parameters
clear;
DX=100;
DY=100;
DZ=100;
nodeNumber=1003;
comR=20;
anchorN=3;
frac=1;
anchor=(nodeNumber-anchorN)/(DZ/comR)*frac/100;
derror=1;
lim=1e-2;
cur=1000;%DX*(1-derror);
len=sqrt(DX^2+DY^2+DZ^2);
cur=100;
perror=0.9999;
optR=sqrt((DX*DY/nodeNumber))/sqrt(2);
%anchorN=anchorN+1;

%%%%%%%%%init%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Countanchor=0;
flag=1;
for i=1:nodeNumber
   dx=rand(1,1)*(DX);
   while dx==0||dx==DX
        dx=rand(1,1)*(DX);
   end
  dy=rand(1,1)*(DY);
  while dy==0||dy==DY
        dy=rand(1,1)*(DY);
   end
   dz=rand(1,1)*(DZ);
   while dz==0||dz==DZ
        dz=rand(1,1)*(DZ);
   end
    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=dx;
   node(i).tPosition.y=dy;
   node(i).tPosition.z=dz;
   if node(i).type==0
	%{
    switch flag
        case 1
     node(i).tPosition.x=dx;
     node(i).tPosition.y=0;
     flag=2;
        case 2
      node(i).tPosition.y=dy;
      node(i).tPosition.x=0;   
      flag=3;
        case 3 
    node(i).tPosition.x=dx;
     node(i).tPosition.y=DY;
     flag=4;
        case 4
       node(i).tPosition.y=dy;
     node(i).tPosition.x=DX;
      flag=1;
    end       
       %}
       node(i).tPosition.z=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;
    % hold on;
   % plot(node(i).tPosition.x,node(i).tPosition.y,'r*');
   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;
   node(i).n=0;
end


for j=1:DZ
 perh=0;
   for i=2:nodeNumber
      if node(i).Heigh>=(j-1)*comR &&node(i).Heigh<=j*comR && node(i).type~=2
          node(i).type=2;
          perh=perh+1;
      end
      if perh==anchor
          break;
      end
   end
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
%}
%%%%%anchor%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:nodeNumber
 if (node(i).type==2)
         A=zeros(anchor-1,3);
          B=zeros(anchor-1,1);
          ex=zeros(3,1);
          x=zeros(4,3);
          d=zeros(1,4);
           for j=1:anchorN
              x1=node(j).tPosition.x;
              y1=node(j).tPosition.y;
               z1=node(j).tPosition.z;
               z0=abs(node(j).tPosition.z+(node(j).Heigh-node(i).Heigh));
              d1=dst(node(i),node(j));
              H=abs(node(j).Heigh-node(i).Heigh);
              d2=sqrt(d1^2-H^2);
              x(j,:)=[x1 y1 z1];
              x(j+1,:)=[x1 y1 z0];
              d(j)=d1;
              d(j+1)=d2;
           end
            xn=x(4,1); yn=x(4,2); zn=x(4,3);dn=d(4);
              for j=1:3
             x1=x(j,1); y1=x(j,2); z1=x(j,3);dn=d(j);
              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
           %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(ex(3,1));
               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
           
          %else
           %lineerr=lineerr+1;
         %end
      end
  end
    %%%%%%%%%%%%%count%%%%%%%%%%%%%%%%%%%%%%%%%
  %{
  myerror=0;
  outR=0;
  locait=0;
  test1=0;
  myerror1=0;
  err=0;
  for i=1:nodeNumber
    if (node(i).type==1 && node(i).finish==1)
         myd=edst(node(i),node(i));
           test1=test1+node(i).test;
         if myd<cur
         locait=locait+1;
         err=err+node(i).err;
        myerror1=myerror1+myd;
       else
         node(i).finish=3;
         outR=outR+1;
       end
    end
  end
  ratio=locait/(nodeNumber-anchorN)
  test=test1/locait
myerror=myerror1/locait
  outR;
  err=err/locait
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %{
if derror<1
while outR~=0  
for i=1:nodeNumber
    z=1:3;
      if (node(i).type==1 && node(i).finish==3)
          c=0;
          for j=1:nodeNumber
              if (i~=j && node(j).finish==1)
              d1=abs(node(i).tPosition.x-node(j).tPosition.x);
              d2=abs(node(i).tPosition.y-node(j).tPosition.y);
               d3=abs(node(i).tPosition.z-node(j).tPosition.z);
               if d1<comR || d2<comR || d3<comR
                   c=c+1;
                  z(c)=j;
               end
              end
              if c==3
                  break;
              end
          end
            if c==3
             A=zeros(2,2);
              B=zeros(2,1);
              ex=zeros(2,1);
             last=z(3);
              x3=node(last).ePosition.x;
              y3=node(last).ePosition.y;
               z3=node(last).ePosition.z;
               d3=pdst(node(last),node(i))*(derror);
              for k=1:2
                  id=z(k);
                  x1=node(id).ePosition.x;
                 y1=node(id).ePosition.y;
                 z1=node(id).ePosition.z;
                 d1=pdst(node(id),node(i))*(derror);
                 A(k,1)=2*(x1-x3);A(k,2)=2*(y1-y3);
                 B(k,1)=(x1^2-x3^2)+(y1^2-y3^2)+(d3^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(last).tPosition.z+(node(last).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==3
               node(i).finish=1;
               outR=outR-1;
              end
           end
         % else
           end
  end   
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/DX<cur
         locait=locait+1;
        myerror=myerror+myd;
       else
         node(i).finish=3;
       end
    end
  end
  ratio=locait/(nodeNumber-anchorN)
  myerror=myerror/locait/100
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %}
%
%%%%%%%%%%%%%%%%%%plot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  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==1)
    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,'r*');
    else
     plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'kx');   
     end
 end
 hold on;
end
 grid on;
%}
%{
for i=1:anchorN
    A=node(i);
    for j=1:anchorN
       B=node(j);
        plotplane(A,B);
end
end
%}