function X = TaylorAlgorithm(BSN, MSP, Radius, Noise)
%
%TAYLORALGORITHM 本函數用于實現無線定位中的TAYLOR算法
%               - BSN  為基站個數，3 < BSN <= 7；
%               - MSP  為移動臺的初始位置, MSx, MSy均為[0,1]之間的數；
%                      特別要注意服務小區與MS之間的關系，MS的位置不能越界。
%               - Noise 測距誤差方差．
%               - R    為小區半徑，單位(meter)；
%               - X    為移動臺經算法處理后的位置.
%See also: TaylorAlgorithm.m

% 李金倫，西南交通大學
%       10 December, 2004, 第一版

% 參數檢查：
if  nargout ~= 1 & nargout ~= 0,
    error('Too many output arguments.');
end
if nargin ~= 4,
    error('Wrong number of input arguments.');
end

% 初始參數：
BS = Radius*NetworkTop(BSN);
MS = Radius*MSP;

% TDOA協方差矩陣Q：
Q = eye(BSN-1);
    
% 初始估計位置：
iEP = MS;

% h0:
for i = 1: BSN,
    MeaDist(i) = sqrt((MS(1) - BS(1,i))^2 + (MS(2) - BS(2,i))^2);
end
for i = 1: BSN-1,
    h0(i) = MeaDist(i+1) - MeaDist(1) + Noise*randn(1);   %TDOA測量值
end

% 算法開始：
for n = 1: 10,
    % Rn:
    R1 = sqrt(iEP(1)^2 + iEP(2)^2);
    for i =1: BSN-1,
        R(i) = sqrt((iEP(1) - BS(1,i+1))^2 + (iEP(2) - BS(2,i+1))^2);        
    end
    
    % ht:
    for i = 1: BSN-1,
        h(i) = h0(i) - (R(i) - R1);
    end
    ht = h';
    
    % Gt:
    for i = 1: BSN-1,
        Gt(i, 1) = -iEP(1)/R1 - (BS(1, i+1) - iEP(2))/R(i);
        Gt(i, 2) = -iEP(2)/R1 - (BS(2, i+1) - iEP(2))/R(i);
    end
    
    % delt:
    delt = inv(Gt'*inv(Q)*Gt)*Gt'*inv(Q)*ht;
    
    EP = iEP + delt';

    iEP = EP;
end

% 結果輸出:
if nargout == 1,
    X = EP;
elseif nargout == 0,
    disp(EP);
end