fusion1.m

本程序实现多算法协同定位中第一层数据融合

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