talor.txt

用于无线室内定位的的算法的一部分

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