【综合算法】不考虑误差的TDOA定位

不考虑误差的TDOA定位

TDOA是一种无线定位技术，是一种利用时间差进行定位的方法。

不同于TOA，TDOA（到达时间差）是通过检测信号到达两个基站的时间差，而不是到达的绝对时间来确定移动台的位置，降低了时间同步要求。

TDOA至少需要三个已知坐标位置的基站，通过获取不同基站之间的信号传送时间差来定位。假设三个基站坐标分别为$(x_1,y_1),(x_2,y_2),(x_3,y_3)$，以第一个基站为标准，分别得到第二个基站与第一个基站的时间差t1，第三个基站与第一个基站的时间差t2，信号时间差乘以电磁波传播速度，得到距离差$r_{2,1}$和$r_{3,1}$，距离差是已知常量。当我们忽略实际情况中存在的信号误差，TDOA实际上归结求解两根双曲线的交点：

$$\sqrt{(x-x_2)^2+(y-y_2)^2}-\sqrt{(x-x_1)^2+(y-y_1)^2}=r_{2,1} \\ \sqrt{(x-x_3)^2+(y-y_3)^2}-\sqrt{(x-x_1)^2+(y-y_1)^2}=r_{3,1}$$
由于根号的存在，求解变得比较困难，于是用换元法: $$r_i^2=(x-x_i)^2+(y-y_i)^2=K_i-2x_ix-2y_iy+x^2+y^2 \\ K_i = x_i^2+y_i^2$$
又令 $r_{i,1}=r_i-r_1,x_{i,1}=x_i-x_1,y_{i,1}=y_i-y_1$，则：
$$r_i^2=(r_{i,1}+r_1)^2\\ = r_{i,1}^2+2r_{i,1}r_1 + r_1^2 =K_i-2x_ix-2y_iy+x^2+y^2 \\ \rightarrow \begin{bmatrix} x_{i,1} & y_{i,1} \end{bmatrix}\begin{bmatrix} x \\ y \end{bmatrix}=1/2(K_i-K_1 - 2r_{i,1}r_1-r_{i,1}^2)$$ 令i=2,3，带入上述式子中构建一个线性方程组：
$$\begin{bmatrix} x_{2,1} & y_{2,1} \\ x_{3,1} & y_{3,1} \end{bmatrix}\begin{bmatrix} x \\ y \end{bmatrix}=\left \{\begin{bmatrix} - r_{2,1} \\ - r_{3,1}\end{bmatrix}r_1+1/2\begin{bmatrix}K_2-K_1 -r_{2,1}^2) \\ K_3-K_1 -r_{3,1}^2\end{bmatrix}\right \} \\ \rightarrow \begin{bmatrix} x \\ y \end{bmatrix}=-\begin{bmatrix} x_{2,1} & y_{2,1} \\ x_{3,1} & y_{3,1} \end{bmatrix}^{-1}\left \{\begin{bmatrix} r_{2,1} \\ r_{3,1}\end{bmatrix}r_1+ 1/2\begin{bmatrix}-K_2+K_1 +r_{2,1}^2) \\ -K_3+K_1 +r_{3,1}^2\end{bmatrix}\right \} \\ let　P1=-\begin{bmatrix} x_{2,1} & y_{2,1} \\ x_{3,1} & y_{3,1} \end{bmatrix}^{-1},P2=\begin{bmatrix} r_{2,1} \\ r_{3,1}\end{bmatrix},P3=1/2\begin{bmatrix}-K_2+K_1 +r_{2,1}^2 \\ -K_3+K_1 +r_{3,1}^2\end{bmatrix}, X_1=\begin{bmatrix} x_1 \\ y_1 \end{bmatrix} \\ \rightarrow \begin{bmatrix} x-x_1 \\ y-y_1 \end{bmatrix}=P1P2r_1+(P1P3-X1) \\ and　 r_1^2=\begin{bmatrix} x-x_1 \\ y-y_1 \end{bmatrix}^T\begin{bmatrix} x-x_1 \\ y-y_1 \end{bmatrix} \\ =[P1P2r_1+(P1P3-X1)]^T[P1P2r_1+(P1P3-X1)] \\ \rightarrow [(P1P2)^TP1P2-1]r_1^2+[(P1P2)^T(P1P3-X1)+(P1P3-X1)^TP1P2]r_1+(P1P3-X1)^T(P1P3-X1)=0$$ 这是一个关于r1的一元二次方程，其中P1，P2，P3的各个分量是已知的，它们都是常量。解出这个方程可以得到r1的两个值，取其中合理的值为r1，即可解出：
$$\begin{bmatrix} x \\ y \end{bmatrix}=P1P2r_1+P1P3$$

Chan Y T, Ho K C. A simple and efficient estimator for hyperbolic location[J]. IEEE Transactions on Signal Processing, 1994, 42(8):1905-1915.