tps.m

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

function [dsa1,dsb1]=Tps(A,B,S,v,n,dmax,sigma)
dab=dst(A,B);
dsa=dst(A,S);
dsb=dst(S,B);
t1=dsa/v;
t2=dmax/v+dsb/v;
delt=0;
ti=t2-t1;
for i=1:n
err=normrnd(0,sigma^.5,1,1);
delt=delt+err;
end
delt=1+delt/n;
s=ti*delt;
k=s*v-dmax;
yb=B.tPosition.y;
H=S.Heigh;
c=(yb^2-k^2);
a=(-1)*4*c;
b=(-1)*4*k*c;
r=c^2+4*(yb^2)*(H^2);
delta=b^2-4*a*r;
if delta>=0
dsa1=(-b+sqrt(delta))/(2*a);
dsa2=(-b-sqrt(delta))/(2*a);
dsb1=dsa1+k;
dsb2=dsa2+k;
else 
    dsa1=-1;
    dsb1=-1;
    end

if dsa1~=-1 && dsb1~=-1
 test1=dsa1+dsb1;
 test2=dsa2+dsb2;
 if test2>dab
     dsa1=dsa2;
     dsb1=dsb2;
       end
end
%}