cal.m

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

function [x,y]=cal(x1,y1,x2,y2,r1,r2)
d1=x1^2-x2^2;
d2=y1^2-y2^2;
d3=x1-x2;
d4=y1-y2;
d5=r1^2-r2^2;
if d3==0
    y=(d5-d1-d2)/(-2*d4);
    x=x1;
elseif d4==0
     x=(d5-d1-d2)/(-2*d3);
     y=y1;
    else
      k=d4/d3;
      a=2*(d3+d4*k);
      b=2*d4*y1-2*d4*k*x1;
      m=d1+d2-d5;
      x=(m-b)/a;
      y=k*(x-x1)+y1;
end
%{
while x>100
        x=100-(x-100);
        x=abs(x);
    end
    while y>100
       y=100-(y-100);
       y=abs(y);
    end
%}
    x=abs(x);
    y=abs(y);