function X = zhao5(BSN2,BS2, MSP2, Radius2, Noise2)
%TAYLORALGORITHM 本函數用于實現無線定位中的TAYLOR算法
%               - BSN  為基站個數，3 < BSN <= 7；
%               - MSP  為移動臺的初始位置, MSx, MSy均為[0,1]之間的數；
%                      特別要注意服務小區與MS之間的關系，MS的位置不能越界。
%               - Noise 測距誤差方差．
%               - R    為小區半徑，單位(meter)；
%               - X    為移動臺經算法處理后的位置.
% 初始參數：
sum=0;
for num=1:100
MS = Radius2*MSP2;
% TDOA協方差矩陣Q：
Q = eye(BSN2-1);
% 初始估計位置：
iEP = MS;
% h0:
for i = 1: BSN2,
    MeaDist(i) = sqrt((MS(1,1) - BS2(1,i))^2 + (MS(2,1) - BS2(2,i))^2);
end
for i = 1: BSN2-1,
    h0(i) = MeaDist(i+1) - MeaDist(1) + Noise2*randn(1);   %TDOA測量值
end
% 算法開始：
for n = 1: 10,
    % Rn:
    R1 = sqrt((iEP(1,1)-BS2(1,1))^2 + (iEP(2,1)-BS2(1,2))^2);
    for i =1: BSN2-1,
        R(i) = sqrt((iEP(1,1) - BS2(1,i+1))^2 + (iEP(2,1) - BS2(2,i+1))^2);        
    end
    
    % ht:
    for i = 1: BSN2-1,
        h(i) = h0(i) - (R(i) - R1);
    end
    ht = h';
    
    % Gt:
    for i = 1: BSN2-1,
        Gt(i, 1) = -iEP(1)/R1 - (BS2(1, i+1) - iEP(2))/R(i);
        Gt(i, 2) = -iEP(2)/R1 - (BS2(2, i+1) - iEP(2))/R(i);
    end
    
    % delt:
    delt = inv(Gt'*inv(Q)*Gt)*Gt'*inv(Q)*ht;
      
    EP = iEP + delt;
end
sum=sum+EP;
end
sum=sum/100