locatcacul.m

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

function [x,n]=locatCacul(a,b,E,p)

temp1=size(a);temp2=size(b);

if temp1(1)~=temp1(2)|temp1(1)~=temp2(1)|temp2(2)~=1|det(a)==0

   n=0; 

    return;

end

%%化为三角矩阵过程

leng=temp1(1);N=-tril(a,-1)-triu(a,1);M=a+N;N=-a;

t=1;

for i=1:leng

    t=t*M(i,i);

end

if t==0

    n=0;
end

x=zeros(leng,1);

t=0;x1=zeros(leng,1);

for i=1:leng

       for j=1:leng

        if j<i

            x(i)=N(i,j)*x(j)+x(i);

        end

        if j>=i

           x(i)=N(i,j)*x1(j)+x(i);

        end

    end

    x(i)=x1(i)+(x(i)+b(i))/M(i,i)*p;

end

for i=1:leng   

    t=t+(x1(i)-x(i))^2;

end

e=E^2;

n=1;

while t>e

   t=0;

   x1=x;

   x=zeros(leng,1);

   for i=1:leng

       temp=0;

       for j=1:leng

           if j<i

              x(i)=N(i,j)*x(j)+x(i);

           end

           if j>=i

              x(i)=N(i,j)*x1(j)+x(i);

           end

       end

       x(i)=x1(i)+(x(i)+b(i))/M(i,i)*p;

       t=t+(x1(i)-x(i))^2;

   end

   n=n+1;

   if t>1000

       break;

      n=0;

   end  

end