TDOA定位Taylor算法

基于TDOA的经典定位算法分为两类，一类是可以求出解析解的算法，如Fang算法、Chan算法的；另一类是迭代算法，如Taylor算法。

Taylor级数展开法是一种迭代算法，在Taylor级数展开的基础上，利用初始迭代值进行WLS估计，然后求解位置估计误差的局部最小二乘解，并对标签的位置进行更新。Taylor算法的前提是需要标签位置的初始估计值，而算法的主要思想是通过不断迭代来修正待定位标签位置的估计值，最后逐渐逼近标签真实的位置坐标。

References
[1] 宋洋. 超宽带室内定位技术研究[D].西安科技大学,2019.

代码实现（matlab）

X_estimate = 50;
Y_estimate = 50;
Xb = [200 200];
X = [200 0; -100 173; -100 -173];
Noise = 0.1*randn(3,1);
Real_ms = [20 20];

BSN = size(X,1);      
x = X_estimate;
y = Y_estimate;

MS = [x,y];   
iEP = MS;

%% 
Rb = sqrt((Real_ms(1) - Xb(1))^2+(Real_ms(2) - Xb(2))^2);
RD = zeros(BSN,1);
for i = 1:BSN                
    RD(i) = -Rb+sqrt((Real_ms(1)- X(i,1))^2+(Real_ms(2) - X(i,2))^2)+Noise(i);
end
%%
% TDOA协方差矩阵Q
Q = 0.01*eye(BSN);   
delta = [1 1];   
kk = 0;   
% Taylor级数展开法
while ((abs(delta(1)) + abs(delta(2))) > 0.01)   
    R1 = sqrt((iEP(1) - Xb(1))^2 + (iEP(2) - Xb(2))^2);   
    R = zeros(1,BSN);
    kk = kk+1;
    for i = 1: BSN 
        R(i) = sqrt((iEP(1) - X(i,1))^2 + (iEP(2) - X(i,2))^2);
    end
    
    % hi
    hi = zeros(BSN,1);
    for i = 1: BSN
        hi(i) = RD(i) - (R(i) - R1);
    end
    
    % Gi
    Gi = zeros(BSN,2);
    for i = 1: BSN
        Gi(i, 1) = (Xb(1)-iEP(1))/R1 - (X(i,1) - iEP(1))/R(i);
        Gi(i, 2) = (Xb(2)-iEP(2))/R1 - (X(i,2) - iEP(2))/R(i);
    end
    
    % delta
    delta = inv(Gi'*inv(Q)*Gi)*Gi'*inv(Q)*hi;   
    if (abs(delta(1))+abs(delta(2))) > 0.01   
        EP = iEP + delta';   
        iEP = EP;   % 更新迭代值
    end
end

% 输出
z_out = iEP;   % 标签坐标估计值
mse = sqrt((z_out(1)-Real_ms(1))^2+(z_out(2)-Real_ms(2))^2);   % 均方误差