TDOA之chan算法

主函数部分

函数调用部分：

function Zp=myChan(BSN,BS,R)
Q = eye(BSN-1);
K1 = 0;
for i=1:BSN-1
    K(i)=BS(1,i+1)^2+BS(2,i+1)^2;
end
for i= 1:BSN-1
    Ga(i,1) = -BS(1,i+1);
    Ga(i,2) = -BS(2,i+1);
    Ga(i,3) = -R(i);
end
for i= 1:BSN-1
    h(i) = 0.5*(R(i)^2-K(i)+K1);
end
Za0 = inv(Ga'*inv(Q)*Ga)*Ga'*inv(Q)*h';
B =eye(BSN-1);
for i = 1:BSN-1
    B(i,i) = sqrt((BS(1,i+1) - Za0(1))^2+(BS(2,i+1) - Za0(2))^2);
end
FI = B*Q*B;
Za1 = inv(Ga'*inv(FI)*Ga)*Ga'*inv(FI)*h';
CovZa = inv (Ga'*inv(FI)*Ga);
sB = eye(3);
for i = 1:3;
    sB(i,i) = Za1(i);
end
sFI = 4*sB*CovZa*sB;
sGa = [1,0;0,1;1,1];
sh = [Za1(1)^2;Za1(2)^2;Za1(3)^2];
Za2 = inv(Ga'*inv(sFI)*Ga)*Ga'*inv(sFI)*h'; 
sZ = sqrt(abs(Za2));
if nargout == 1;
    Zp = sZ;
elseif nargout == 0;
    disp(sZ);
end