tri.m

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

%##################################################
%
%  RFUSP
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%parameters
clear;
DX=100;
DY=100;
DZ=100;
nodeNumber=1002;
comR=20;
percent=15/100;
anchorN=4;
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=2;
   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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dd=zeros(nodeNumber-anchor,4);
for i=1:nodeNumber
   if (node(i).type~=0)
       for j=1:anchor
         d=dst(node(i),node(j));
         dd(i,j)=d;
       end
   end
end

 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;
             xn=node(anchor).tPosition.x;
              yn=node(anchor).tPosition.y;
               zn=node(anchor).tPosition.z;
              dn=dd(i,anchor);
            for j=1:anchor-1
              x1=node(j).tPosition.x;
              y1=node(j).tPosition.y;
               z1=node(j).tPosition.z;
              d1=dd(i,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;
  for i=1:nodeNumber
      if (node(i).type==1 && node(i).finish==1)
          myd=edst(node(i),node(i));
         locait=locait+1;
        myerror=myerror+myd;
         end
  end
  %error=error/locait;
  %locait
  %outR
  ratio=locait/(nodeNumber-anchor)
  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;
  %}