%##################################################%%  RFUSP%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%parametersclear;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; endconnect=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);    endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%} 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;    endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%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    endend%}  %%% 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    endend%%%%%%%%%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