TDOA定位Taylor算法

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

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

标签与基站之间的约束关系可以通过某一函数$f_i\left(x,y,x_i,y_i\right)$来表达，该函数的测量值可以用$M_i$来表示，$M_i=M_i^0+e_i$ ,其中，$M_i^0$为函数的真实值，$e_i$表示测量误差。用$\eta$来代表误差门限值，那么，应在满足条件：$\Delta x+\Delta y<\eta$时停止迭代计算。

假设标签坐标的初始值为$(x_0,y_0)$，真实值为$(x,y)$，且$x=x_0+\Delta x$，$y=y_0+\Delta y$，那么函数$f_i\left(x,y,x_i,y_i\right)$在$(x_0,y_0)$处的Taylor级数展开结果如下：

$$\begin{gathered} f_i\left(x,y,x_i,y_i\right)=f_i\left(x_0,y_0,x_i,y_i\right)+\left(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}\right)f_i\left(x_0,y_0,x_i,y_i\right) \\ +\frac{1}{2!}{\left(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}\right)}^2{f}_i\left(x_0,y_0,x_i,y_i\right)+\cdots \\ +\frac{1}{n!}{\left(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}\right)}^n{f}_i\left(x_0,y_0,x_i,y_i\right)+\cdots \end{gathered}$$

由基于TDOA的定位方法，可以得到以下的方程组：
$$\begin{array}{rl}& R_i=\sqrt{{\left(x-x_i\right)}^2+{\left(y-y_i\right)}^2}\\ & R_{i1}=\sqrt{{\left(x-x_i\right)}^2+{\left(y-y_i\right)}^2}-\sqrt{{\left(x-x_1\right)}^2+{\left(y-y_1\right)}^2}\end{array}$$

其中，$(x,y)$表示标签的坐标，$R_{i1}=R_i-R_1$表示标签到第$i$个基站与到第1个基站的距离差值。

由上式构造函数$f_i\left(x,y,x_i,y_i\right)$：

$$\begin{array}{rl}& f_i\left(x,y,x_i,y_i\right)=c\left(t_i-t_j\right)=R_i-R_j\\ & =\sqrt{{\left(x_i-x\right)}^2+{\left(y_i-y\right)}^2}-\sqrt{{\left(x_j-x\right)}^2+{\left(y_j-y\right)}^2}\end{array}$$

将$f_i\left(x,y,x_i,y_i\right)$在$(x_0,y_0)$处进行泰勒展开，并忽略二阶以上的分量，有：

$$f_i\left(x,y,x_i,y_i\right)=f_i\left(x_0,y_0,x_i,y_i\right)+\left(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}\right)f_i\left(x_0,y_0,x_i,y_i\right)$$

转化为矩阵的形式，有：

$$\psi ={\mathbf{h}}_i-{\mathbf{G}}_i\delta$$

其中，$\psi$为误差矢量，另有

$$\begin{array}{rl}& {\mathbf{h}}_i=\left[\begin{array}{c}{R}_{2,1}-\left(R_2-R_1\right)\\ R_{3,1}-\left(R_3-R_1\right)\\ ⋮\\ R_{L,1}-\left(R_L-R_1\right)\end{array}\right]\\ & {\mathbf{G}}_i=\left[\begin{array}{cc}\frac{x_1-x}{R_1}-\frac{x_2-x}{R_2}& \frac{y_1-y}{R_1}-\frac{y_2-y}{R_2}\\ \frac{x_1-x}{R_1}-\frac{x_3-x}{R_3}& \frac{y_1-y}{R_1}-\frac{y_3-y}{R_3}\\ ⋮& ⋮\\ \frac{x_1-x}{R_1}-\frac{x_L-x}{R_L}& \frac{y_1-y}{R_1}-\frac{y_L-y}{R_L}\end{array}\right]\\ & \delta =\left[\begin{array}{c}\Delta x\\ \Delta y\end{array}\right]\end{array}$$

$R_i(i=1,2,3,\cdots ,L)$表示在一次迭代计算中标签和第i个基站之间的距离。

上式的加权最小二乘解为：

$$\delta =\left[\begin{array}{c}\Delta x\\ \Delta y\end{array}\right]={\left({\mathbf{G}}_i^T{\mathbf{Q}}^{-1}{\mathbf{G}}_i\right)}^{-1}{\mathbf{G}}_i^T{\mathbf{Q}}^{-1}{\mathbf{h}}_i$$

其中，$\mathbf{Q}$表示TDOA测量值的协方差矩阵。在下一次递归计算中，令$x^{\prime }=x_0+\Delta x$，$y^{\prime }=y_0+\Delta y$，更新标签的坐标值进行迭代计算。重复以上过程，直到误差满足设定的门限值时停止迭代计算，即：$\left|\Delta x\right|+\left|\Delta y\right|<\eta$。

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);   % 均方误差

配套pdf下载
https://download.csdn.net/download/gongshouxiayin/13658167