getposition2.m

Friedlander算法,假定服务基站的二维坐标为(0,0),基站数为4,多基站的以后上传.test是一个测试程序.

function RR = getPosition2(xyPosi,tdoa)
   P=tdoa;
   S=xyPosi';
   D=inv(diag(P));
   Z=[0 1 0;0 0 1;1 0 0];
   I=eye(3);
   N=(I-Z)*D;
   for len=1:3
      uu(len) = 0.5*((xyPosi(1,len))^2 + (xyPosi(2,len))^2 - P(len)^2);
   end
   uu=N*uu';
   A=N*S;
   RR=inv(A'*A)*A'*uu;