mobitps.m

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

%##################################################
%
%  RFUSP
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%parameters
clear all;
DX=100;
DY=100;
DZ=100;
nodeNumber=1000;
comR=15;
anchorN=2;
derror=1;
lim=1e-1;
cur=1000;%DX*(1-derror);
len=sqrt(DX^2+DY^2+DZ^2);
cur=1000;
perror=0.9;
optR=sqrt((DX*DY/nodeNumber))/sqrt(2);
V=1500;
dmax=sqrt(DY^2+DZ^2);
cur=dmax/20;
res=0.50;
n=1;
sigma=0;
plo=1;     % plot the figure
%sigma1=sqrt(sigma1);
%anchorN=anchorN+1;
N=10;
d=zeros(nodeNumber,4);
last=1;
for jj=1:1
    jj
    ra=zeros(1,N);
  raT=ra;
merr=zeros(1,N); 
merrT=merr;
  er=zeros(1,N);
  erT=er;
   n=2^(jj+1);
   %
  for tt=1:1
    tt
    if last==1
sigma1=sqrt(sigma+(tt-1)/5);
    else 
sigma1=1.0;
    end
  ratio=0;
  ratioT=0; 
 outR=0;
  
 %test1=0;
  myerror=0;
  myerrorT=0;
  err=0;
  errT=0;
for m=1:1
    m
%%%%%%%%%init%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Countanchor=0;
flag=1;
for i=1:nodeNumber
   dx=rand(1,1)*(DX);
   while dx<=5||dx>=DX-5
        dx=rand(1,1)*(DX);
   end
  dy=rand(1,1)*(DY);
  while dy<=5||dy>=DY-5
        dy=rand(1,1)*(DY);
   end
   dz=rand(1,1)*(DZ);
   while dz<=5||dz==DZ-5
        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).ePosition.x= node(i).tPosition.x;
    node(i).ePosition.y= node(i).tPosition.y;
    node(i).ePosition.z= node(i).tPosition.z;
    node(i).ePosition.x1= node(i).tPosition.x;
    node(i).ePosition.y1= node(i).tPosition.y;
    node(i).ePosition.z1= node(i).tPosition.z;
    node(i).tPosition.z=0;
	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).ePosition.x1=-1;
     node(i).ePosition.y1=-1;
     node(i).ePosition.z1=-1;
     node(i).finish=0;
     node(i).finishT=0;
     end
   node(i).Heigh=node(i).tPosition.z;
   node(i).test=0;
   node(i).res=0;
   node(i).resT=0;
   node(i).X=0;
   node(i).d1=0;
   node(i).d2=0;
   
   end
node(1).tPosition.x=0;
node(1).tPosition.y=0;
node(1).ePosition.x= node(1).tPosition.x;
node(1).ePosition.y= node(1).tPosition.y;
 node(2).tPosition.x=0;
node(2).tPosition.y=DY;
node(2).ePosition.x= node(2).tPosition.x;
node(2).ePosition.y= node(2).tPosition.y;

%{
%%%%%%%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
%}
judge=zeros(anchorN,anchorN);

dx=0;
%step=6;
if last==1
step=1;
else 
step=tt;
end
    
%{
while move<3
 if dy~=DY
    dy=dy+1;
 else
     dy=DY;
     dx=dx-1;
 end
  if dx==0
     break;    
 end
%}
while dx<=DX
node(1).tPosition.x=dx;
node(2).tPosition.x=dx;
for i=1:anchorN
    for j=1:anchorN
        if(i~=j)
            judge(i,j)=pdst(node(i),node(j));
        end
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=3:nodeNumber
    xt=node(i).tPosition.x;
     filter=abs(xt-dx);
    if (node(i).type~=0) && (node(i).finish==0) && (filter<=step/2)
       A=node(1);
       B=node(2);
       S=node(i);
       dd=zeros(1,4);
       [dd(1) dd(2)]=Tps(A,B,S,V,n,dmax,sigma1);
       if dd(1)==-1&&dd(2)==-1
           continue;
       end
%%%%%%%%%%%%%3DTPS%%%%%%%%%%%%%%%%%%%%%%%%%%%       
       x=A.tPosition.x;
       y=sqrt(dd(1)^2-S.Heigh^2);
       node(i).ePosition.z1=abs(A.tPosition.z+(A.Heigh-node(i).Heigh));
       node(i).ePosition.x1=x;
       node(i).ePosition.y1=y;
       T=node(i);
       dd1=sqrt((T.ePosition.x1-A.tPosition.x)^2+(T.ePosition.y1-A.tPosition.y)^2+(T.ePosition.z1-A.tPosition.z)^2);
       dd2=sqrt((T.ePosition.x1-B.tPosition.x)^2+(T.ePosition.y1-B.tPosition.y)^2+(T.ePosition.z1-B.tPosition.z)^2);
       d1=dst(node(i),A);
       d2=dst(node(i),B);
       node(i).resT=abs(dd1-d1+dd2-d2)/2;
       node(i).errT=sqrt((T.ePosition.x1-T.tPosition.x)^2+(T.ePosition.y1-T.tPosition.y)^2+(T.ePosition.z1-T.tPosition.z)^2);
       node(i).finishT=1;
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

       for j=1:anchorN
          dd1=dst(node(i),node(j));
         d(i,j)=dd1;
         d(i,j+2)=dd(j);
         H=abs(node(i).Heigh-node(j).Heigh);
          r(j)=sqrt(dd(j)^2-H^2);
        end
      found=0;
      a1=-1;
      a2=-1;
      x=0;y=0;
      tx=0;ty=0;
         for k=1:anchorN-1
          for j=k+1:anchorN
             if(k~=j)
               d1=r(k)+r(j);
               d2=abs(r(k)-r(j));
               ca1=judge(k,j)/d1;
                ca2=judge(k,j)/d2;
                %if ca1>=perror&&ca1<=1+(1-perror) || ca2>=perror&&ca2<=1+(1-perror)
              % if round(ca1*100/100)==1 || round(ca2*100/100)==1
                if (ca1>=1-lim||ca2>=1-lim) %&&(ca1>=1+lim||ca2>=1+lim)
                     a1=k;a2=j;
                      node(i).test=max([a1 a2]);
                      %node(i).err=([abs(d1-judge(k,j))+abs(d2-judge(k,j))])/2;
                       x1=node(a1).tPosition.x;
                      y1=node(a1).tPosition.y;
                      x2=node(a2).tPosition.x;
                     y2=node(a2).tPosition.y;
                  % if x1~=x2 || y1~=y2
                     found=1;
                    break;
                   %end
                 end
             end
          end
          if found==1
              break;
          end             
      end
  
       if found==1
       r1=r(a1);
       r2=r(a2);
      % [node(i).ePosition.x,node(i).ePosition.y]=mypoint(x1,y1,x2,y2,r1,r2);
        [x,y]=cal(x1,y1,x2,y2,r1,r2);
        node(i).ePosition.z=abs(node(a1).tPosition.z+(node(a1).Heigh-node(i).Heigh));
       
       while y>DY
       y=DY-(y-DY);
       y=abs(y);
       end
       %}
        node(i).finish=1;
         node(i).ePosition.x=x;
         node(i).ePosition.y=y;
         %node(i).ePosition.z=node(i).Heigh;
         node(i).res=abs((edst(node(i),node(1))-dd(1)+edst(node(i),node(2))-dd(2)))/2;
         node(i).err=(edst(node(i),node(i)));
         node(i).X=node(1).tPosition.x;
         node(i).d1=dd(1);
         node(i).d2=dd(2);
        end
end
 end
dx=dx+step;
end
    %%%%%%%%%%%%%count%%%%%%%%%%%%%%%%%%%%%%%%%
   myerror1=0;
   myerrT=0;
   err1=0;
   errrT=0;
  locait=0; 
  locaitT=0;
  for i=1:nodeNumber
    if (node(i).type==1 && node(i).finish==1)
         myd=node(i).err;
           %test1=test1+node(i).test;
         if myd<cur
         locait=locait+1;
         err1=err1+node(i).res;
        myerror1=myerror1+myd;
       else
         node(i).finish=3;
         outR=outR+1;
       end
    end
    if(node(i).type==1 && node(i).finishT==1)
        mydT=node(i).errT;
        %mydT=sqrt((T.ePosition.x1-T.tPosition.x)^2+(T.ePosition.y1-T.tPosition.y)^2+(T.ePosition.z1-T.tPosition.z)^2);
       if mydT<cur
         locaitT=locaitT+1;
         errrT=errrT+node(i).resT;
         myerrT=myerrT+mydT;
       end
    end 
  end
  ratio=ratio+locait/(nodeNumber-anchorN);
  % test=test+test1/locait;
 myerror=myerror+myerror1/locait;
 
  err=err+err1/locait;
  ratioT=ratioT+locaitT/(nodeNumber-anchorN);
  % test=test+test1/locait;
 myerrorT=myerrorT+myerrT/locaitT;
 
  errT=errT+errrT/locaitT;
end
ra(1,tt)=ratio/m;
merr(1,tt)=myerror/m;
er(1,tt)=err/m;
raT(1,tt)=ratioT/m;
merrT(1,tt)=myerrorT/m;
erT(1,tt)=errT/m;
  end
if last==1
  fname=sprintf('mydataS1sigma%.0f.dat',jj);
  else
fname=sprintf('mydata%.0f.dat',jj);
  end
if plo==0
save( fname,'ra','merr','er','raT','merrT','erT','-ASCII'); 
end
end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%{
  %%%%%%%%%%%%%refine %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 for i=3:nodeNumber
   if node(i).finish~=0
   myres=abs(node(i).res);
    reID=-1;
    if myres>res
    A=node(1);
    B=node(2);
    y=node(i).ePosition.y;
    X=node(i).X;
    y=y+res;
    node(i).ePosition.y=y;
    A.tPosition.x=X;
    B.tPosition.x=X;
    dres=abs((edst(node(i),A)-node(i).d1)+(edst(node(i),B)-node(i).d2))/2;
   if dres<myres
     relD=1;
     myres=dres;
     node(i).res=dres;
   else
    y=y-res;
     node(i).ePosition.y=y;
     relD=-1;
   end
   j=0;
   while myres>res && j<=20
       y=y+relD*res;
      node(i).ePosition.y=y;
     dres=abs((edst(node(i),A)-node(i).d1)+(edst(node(i),B)-node(i).d2))/2;
      myres=dres;
      j=j+1;
      j
      node(i).res=dres;
   end
    end
   end
 end
 
   
 %%%%%%%%%%%%%%count%%%%%%%%%%%%%%%%%%%%%%%%%
 outR=0;
  locait=0;
  test1=0;
  myerror1=0;
  err=0;
  for i=1:nodeNumber
    if (node(i).type==1 && node(i).finish~=0)
         myd=edst(node(i),node(i));
           test1=test1+node(i).test;
         if myd<cur
         locait=locait+1;
         err=err+node(i).res;
        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
  %}
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 

%%%%%%%%%%%%%%%%%%plot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if plo==1
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||node(i).finish==2)
    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;
end
%}
%{
for i=1:anchorN
    A=node(i);
    for j=1:anchorN
       B=node(j);
        plotplane(A,B);
end
enddelt=delt+ti*(1+err)-dmax/v;
%}