function X = Fusion1(BSN, MSP, Radius, Noise)
%fusion1本函數實現多算法協同定位模型中第一層數據融合
%                 -BSN  為基站個數，3 < BSN <= 7；
%                 -MSP  為移動臺的初始位置, MSx, MSy均為[0,1]之間的數；
%                      特別要注意服務小區與MS之間的關系，MS的位置不能越界。
%                 - R    為小區半徑，單位(meter)；
%                 - X    為移動臺經算法處理后的位置.
%
%%see also Fusion1.m

%   參數檢查：
if  nargout>1,
    error('Too many output arguments.');
end
if nargin<2 | nargin>4,
    error('Wrong number of input arguments.');
end


%算法開始,此算法BSN數目為7，服務BS在原點
BS = Radius*NetworkTop(BSN);   %七個基站的位置
MS = Radius*MSP;               %移動臺的實際位置

%TDOA測量值數目
TN = BSN-1;

ServBS(1,1) = BS(1,1);
ServBS(2,1) = BS(2,1);

for i = 1:TN,

NeighBS(1,i) = BS(1,i+1);
NeighBS(2,i) = BS(2,i+1);
end

%chan 算法定位估計值
%EMSCI = [0,0];
EMSCI = ChanAlgorithm(BSN, MSP, Radius,Noise);%此處只能是Radius不能寫成R，R作為實參傳遞給Fusion1函數
%           EMSC(i ,1) = EMSCI(1);
%           EMSC(i, 2) = EMSCI(2);

% 泰勒算法定位估計值
%EMSTI = [0,0];
EMSTI = TaylorAlgorithm(BSN, MSP, Radius, Noise);
%             EMST(i ,1) = EMSTI(1);
%             EMST(i, 2) = EMSTI(2);
%TDOA測量值

for i = 1: BSN,
    MeaDist(i) = sqrt((MS(1) - BS(1,i))^2 + (MS(2) - BS(2,i))^2);
end
for i = 1: TN,
    MeaTDOA(i) = (MeaDist(i+1) - MeaDist(1) + Noise*randn(1));   %TDOA測量值
end
%計算殘差公式中的復雜部分,CHAN算法的殘差
Xoutc = 0;
Youtc = 0;
for i = 1:TN,
    Xoutc = ( MeaTDOA(i)-(abs( NeighBS(1,i)- EMSCI(1))-abs(ServBS(1,1) - EMSCI(1))))^2 + Xoutc;
    Youtc = ( MeaTDOA(i)-(abs( NeighBS(2,i)- EMSCI(2))-abs(ServBS(2,1) - EMSCI(2))))^2 + Youtc;
end
%CHAN算法殘差表示如下
XRc = 0;
YRc = 0;
XRc = Xoutc/TN;
YRc = Youtc/TN;

%%計算殘差公式中的復雜部分,泰勒算法的殘差

Xoutt = 0;
Youtt = 0;
for i = 1:TN,
    Xoutt = (MeaTDOA(i)-(abs( NeighBS(1,i)- EMSTI(1))-abs(ServBS(1,1) - EMSTI(1))))^2 + Xoutt;
    Youtt = (MeaTDOA(i)-(abs( NeighBS(2,i)- EMSTI(2))-abs(ServBS(2,1) - EMSTI(2))))^2 + Youtt;
end

%%Taylor算法殘差表示如下
XRt = 0;
YRt = 0;
XRt = Xoutt/TN;
YRt = Youtt/TN;
%第一層融合的定位估計值
Xlocat = (inv(XRc)*EMSCI(1) + inv(XRt)*EMSTI(1))/(inv(XRc) + inv(XRt));
Ylocat = (inv(YRc)*EMSCI(2) + inv(YRt)*EMSTI(2))/(inv(YRc) + inv(YRt));
Locat = [Xlocat,Ylocat];
if nargout == 1,
    X = Locat;
elseif nargout == 0,
    disp(Locat);
end