myrefine.m

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

function [x,y]=myrefine(nodeA,nodeB,nodeC,err)
lim=1e-1;
res=0;
dd=dst(nodeA,nodeB)*err;
res=res+abs(edst(nodeA,nodeB)-dd);
dd=dst(nodeA,nodeC)*err;
res=res+abs(edst(nodeA,nodeC)-dd);
dy=nodeA.ePosition.y;
if (res<1)
step=res/2;
else
    step=res/10;
end
dy=dy+step;;
nodeA.ePosition.y=dy;
res1=0;
dd=dst(nodeA,nodeB)*err;
res1=res1+abs(edst(nodeA,nodeB)-dd);
dd=dst(nodeA,nodeC)*err;
res1=res1+abs(edst(nodeA,nodeC)-dd);
if  res1<res
    flag=1;
else
    flag=-1;
end
dy=dy-step;
nodeA.ePostion.y=dy;
res1=res+step;
while res>lim && res<res1
 res1=res;
    res=0;
 dy=dy+flag*step;
 nodeA.ePostion.y=dy;
dd=dst(nodeA,nodeB)*err;
res=res+abs(edst(nodeA,nodeB)-dd);
dd=dst(nodeA,nodeC)*err;
res=res+abs(edst(nodeA,nodeC)-dd);
end
x=nodeA.ePosition.x;
y=nodeA.ePosition.y;