到达时间差测量目标位置（TDOA）定位

到达时间差测量目标位置（TDOA）

---

基本原理

由数学原理可知，距离两个定点的距离差为常数的动点的轨迹为双曲线。而若要在三维空间里确定一个点，至少需要三个距离差，四个观测点。因此，利用TOOA定位，至少要有四个观测站。

现有坐标为$(x,y,z)$的目标$T$，有$M+1$个观测站，其中有一个主站$S_0$,$M$个副站$S_i$,他们的坐标为$(x_i,y_i,z_i),i=0,1,\cdot \cdot \cdot ,M$。设电磁辐射由目标到达各站的时间为$t_i(i=0,1,2，\cdot \cdot \cdot ,M)$。各副站到达时间与主站到达时间的时间差可写为${\tau }_i(i=1,2,\cdot \cdot \cdot ,M)$。

将到达时间差乘光速，可得$T$到各副站到总站的距离差
$$\Delta r_i=c{\tau }_i$$
这个距离差还可由目标到主站的距离减去目标到副站的距离直接得到，即
$$\Delta r_i=r_i-r_0=[(x-x_i{)}^2+(y-y_i{)}^2+(z-z_i{)}^2{]}^{\frac{1}{2}}-[(x-x_0{)}^2+(y-y_0{)}^2+(z-z_0{)}^2{]}^{\frac{1}{2}}$$
可得
$$(\Delta r_i+r_0{)}^2=r_i^2=(x-x_i{)}^2+(y-y_i{)}^2+(z-z_i{)}^2$$
两边同减去$r_0^2$
$$\begin{array}{rl}\Delta r_i^2+2\Delta r_i{r}_0& =(x-x_i{)}^2+(y-y_i{)}^2+(z-z_i{)}^2-(x-x_0{)}^2+(y-y_0{)}^2+(z-z_0{)}^2\\ & =2x(x_0-x_i)+2y(y_0-y_i)+2z(z_0-z_i)+(x_i^2+y_i^2+z_i^2)-(x_0^2+y_0^2+z_0^2)\end{array}$$
令$d_i^2=x_i^2+y_i^2+z_i^2$，则
$$\Delta r_i^2+2\Delta r_i{r}_0=2x(x_0-x_i)+2y(y_0-y_i)+2z(z_0-z_i)+d_i^2-d_0^2$$
整理可得
$$x(x_0-x_i)+y(y_0-y_i)+z(z_0-z_i)=\Delta r_i{r}_0+\frac{\Delta r_i^2+d_o^2-d_i^2}{2}$$
上述方程应有$i=1,2,\cdot \cdot \cdot ,M个$，$x,y,z$是未知数，将它改写为矩阵形式，如下：
$$\left[\begin{array}{ccc}{x}_1-x_0& y_1-y_0& z_1-z_0\\ x_2-x_0& y_2-y_0& z_2-z_0\\ ⋮& ⋮& ⋮\\ x_M-x_0& y_M-y_0& z_M-z_0\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}-\Delta r_1\\ -\Delta r_2\\ ⋮\\ -\Delta r_M\end{array}\right]r_0+\left[\begin{array}{c}{l}_1\\ l_2\\ ⋮\\ l_M\end{array}\right]其中，l_i=\frac{d_i^2-d_0^2-\Delta r_i^2}{2}$$
即$AX=B$,其中
$$A=\left[\begin{array}{ccc}{x}_1-x_0& y_1-y_0& z_1-z_0\\ x_2-x_0& y_2-y_0& z_2-z_0\\ ⋮& ⋮& ⋮\\ x_M-x_0& y_M-y_0& z_M-z_0\end{array}\right]$$
$$B=r_{0}C+D=r_0\left[\begin{array}{c}-\Delta r_1\\ -\Delta r_2\\ ⋮\\ -\Delta r_M\end{array}\right]+\left[\begin{array}{c}{l}_1\\ l_2\\ ⋮\\ l_M\end{array}\right]$$
根据线性方程组的线性性质，$AX=B$的解为$AX=C$解的$r_0$倍和$AX=D$的解之和。

当$M=3$时，A为方阵，根据$Cramer$法则，其解可表示为$x_{ij}=\frac{|A_j|}{|A|}$，其中$A_j$是把$A$中的第$j$列元素换成常数项所得行列式。
可解得：
$$\{\begin{array}{l}x=a_1{r}_0+b_1\\ y=a_2{r}_0+b_2\\ z=a_3{r}_0+b_3\end{array}其中，a_i为AX=C得各个解，b_i为AX=D得各个解$$
在上述解中，$r_0$为未知参数，可通过下述方法求得:
由于$r_0^2=(x-x_0{)}^2+(y-y_0{)}^2+(z-z_0{)}^2$,将解带入，有
$$\begin{array}{rl}{r}_0^2& =(a_1{r}_0+b_1-x_0{)}^2+(a_2{r}_0+b_2-y_0{)}^2+(a_3{r}_0+b_3-z_0{)}^2\\ & =(a_1^2+a_2^2+a_3^2)r_0^2+2r_0(a_1{b}_1+a_2{b}_2+a_3{b}_3-a_1{x}_0-a_2{y}_0-a_3{z}_0)+(x_0-b_1{)}^2+(y_0-b_2{)}^2+(z_0-b_3{)}^2\end{array}$$
上述关与$r_0$得方程可写为
$$A{r}_0^2+2B{r}_0+C=0$$
其中
$$\{\begin{array}{l}A=a_1^2+a_2^2+a_3^2-1\\ B=a_1(b_1-x_0)+a_2(b_2-y_0)+a_3(b_3-z_0)\\ C=(x_0-b_1{)}^2+(y_0-b_2{)}^2+(z_0-b_3{)}^2\end{array}$$
根据一元二次方程求根公式可得
$$r_0=-\frac{B\pm \sqrt{B^2-AC}}{A}$$
根据求得$r_0$解的情况，可分为
$$\{\begin{array}{l}1.r_0有两个根（其中一个为虚假解），得到两个定位结果，即定位模糊，需要增加观测站，排除虚假解\\ 2.r_0有一个根，可唯一确定目标位置\\ 3.r_0无解，无法确定目标位置\end{array}$$

---

误差分析

我们对$\Delta r_i=r_i-r_0=[(x-x_i{)}^2+(y-y_i{)}^2+(z-z_i{)}^2{]}^{\frac{1}{2}}-[(x-x_0{)}^2+(y-y_0{)}^2+(z-z_0{)}^2{]}^{\frac{1}{2}}$两边进行全微分，可得
$$\begin{array}{rl}\mathrm{d}\Delta r_i& =\frac{(x-x_i)\mathrm{d}x+(x-x_i)\mathrm{d}{x}_i+(y-y_i)\mathrm{d}y+(y-y_i)\mathrm{d}{y}_i+(z-z_i)\mathrm{d}z+(z-z_i)\mathrm{d}{z}_i}{\sqrt{(x-x_i{)}^2+(y-y_i{)}^2+(z-z_i{)}^2}}-\frac{(x-x_0)\mathrm{d}x+(x-x_0)\mathrm{d}{x}_0+(y-y_0)\mathrm{d}y+(y-y_0)\mathrm{d}{y}_0+(z-z_0)\mathrm{d}z+(z-z_0)\mathrm{d}{z}_0}{\sqrt{(x-x_0{)}^2+(y-y_0{)}^2+(z-z_0{)}^2}}\\ & =(\frac{x-x_i}{r_i}-\frac{x-x_0}{r_0})\mathrm{d}x+(\frac{y-y_i}{r_i}-\frac{y-y_0}{r_0})\mathrm{d}y+(\frac{z-z_i}{r_i}-\frac{z-z_0}{r_0})\mathrm{d}z+(k_i-k_0)\text{其中}{k}_i=\frac{x-x_i}{r_i}\mathrm{d}{x}_i+\frac{y-y_i}{r_i}\mathrm{d}{y}_i\frac{z-z_i}{r_i}\mathrm{d}{z}_i\end{array}$$
上述方程共有$i=1,2,\cdot \cdot \cdot ,M$个，写成矩阵形式为
$$\mathrm{d}R=H\mathrm{d}X+\mathrm{d}S$$
其中
$$\mathrm{d}R={\left[\begin{array}{cccc}\mathrm{d}\Delta r_1& \mathrm{d}\Delta r_1& \cdots & \mathrm{d}\Delta r_M\end{array}\right]}^T$$
$$H=\left[\begin{array}{ccc}\frac{x-x_1}{r_i}-\frac{x-x_0}{r_0}& \frac{y-y_1}{r_i}-\frac{y-y_0}{r_0}& \frac{z-z_1}{r_i}-\frac{z-z_0}{r_0}\\ \frac{x-x_2}{r_i}-\frac{x-x_0}{r_0}& \frac{y-y_2}{r_i}-\frac{y-y_0}{r_0}& \frac{z-z_2}{r_i}-\frac{z-z_0}{r_0}\\ ⋮& ⋮& ⋮\\ \frac{x-x_M}{r_i}-\frac{x-x_0}{r_0}& \frac{y-y_M}{r_i}-\frac{y-y_0}{r_0}& \frac{z-z_M}{r_i}-\frac{z-z_0}{r_0}\end{array}\right]$$
$$\mathrm{d}X={\left[\begin{array}{ccc}\mathrm{d}x& \mathrm{d}y& \mathrm{d}z\end{array}\right]}^T$$
$$\mathrm{d}S={\left[\begin{array}{cccc}{k}_1-k_0& k_2-k_0& \cdots & k_M-k_0\end{array}\right]}^T$$
可见，定位误差由时差误差造成的距离误差$\mathrm{d}R$和站址误差$\mathrm{d}S$决定，所以
$$\begin{array}{rl}\mathrm{d}X& =H^{-1}(\mathrm{d}R-\mathrm{d}S)\\ & =(H^{T}H{)}^{-1}{H}^T(\mathrm{d}R-\mathrm{d}S)\end{array}$$
定位误差的协方差矩阵为
$$\begin{array}{rl}cov(\mathrm{d}X)& =E[\mathrm{d}X\mathrm{d}{X}^T]\\ & =E[[(H^{T}H{)}^{-1}{H}^T(\mathrm{d}R-\mathrm{d}S)][(H^{T}H{)}^{-1}{H}^T(\mathrm{d}R-\mathrm{d}S){]}^T]\\ & =E[[(H^{T}H{)}^{-1}{H}^T(\mathrm{d}R-\mathrm{d}S)][(\mathrm{d}{R}^T-\mathrm{d}{S}^T)(H(H^{T}H{)}^{-1})]]\\ & =(H^{T}H{)}^{-1}{H}^{T}E[(\mathrm{d}R\mathrm{d}{R}^T)+(\mathrm{d}S\mathrm{d}{S}^T)]H(H^{T}H{)}^{-1}\text{设站址误差和距离差误差相互独立，因此}E[\mathrm{d}R\mathrm{d}{S}^T]=E[\mathrm{d}S\mathrm{d}{R}^T]=0\\ & =(H^{T}H{)}^{-1}{H}^T[cov(\mathrm{d}R)+cov(\mathrm{d}S)]H(H^{T}H{)}^{-1}\end{array}$$
若设距离误差${\left[\begin{array}{cccc}\mathrm{d}\Delta r_1& \mathrm{d}\Delta r_1& \cdots & \mathrm{d}\Delta r_M\end{array}\right]}^T$为服从$N(0,{\sigma }_{ri})$的正态分布，$\mathrm{d}\Delta r_i,\mathrm{d}\Delta r_j$间的相关系数为
$${\eta }_{ij}=\frac{cov(\mathrm{d}\Delta r_i,\mathrm{d}\Delta r_j)}{{\sigma }_{ri}{\sigma }_{rj}}$$
因此，其协方差矩阵可写为
$$\left[\begin{array}{cccc}{\sigma }_{r1}^2& {\eta }_{12}{\sigma }_{r1}{\sigma }_{r2}& \cdots & {\eta }_{1M}{\sigma }_{r1}{\sigma }_{rM}\\ {\eta }_{21}{\sigma }_{r2}{\sigma }_{r1}& {\sigma }_{r2}^2& \cdots & {\eta }_{2M}{\sigma }_{r2}{\sigma }_{rM}\\ ⋮& ⋮& ⋮& ⋮\\ {\eta }_{M1}{\sigma }_{rM}{\sigma }_{r1}& {\eta }_{M2}{\sigma }_{rM}{\sigma }_{r2}& \cdots & {\sigma }_{rM}^2\end{array}\right]$$
设站址误差为在$(x,y,z)$三个方向独立分布，分布函数为$N(0,{\sigma }_s)$，则
$$\begin{array}{rl}cov(\mathrm{d}{S}_i,\mathrm{d}{S}_j)& =E[(k_i-k_0)(k_j-k_0)]\\ & =E[[(\frac{x-x_i}{r_i}\mathrm{d}{x}_i+\frac{y-y_i}{r_i}\mathrm{d}{y}_i+\frac{z-z_i}{r_i}\mathrm{d}{z}_i)-(\frac{x-x_0}{r_0}\mathrm{d}{x}_0+\frac{y-y_0}{r_0}\mathrm{d}{y}_0+\frac{z-z_0}{r_0}\mathrm{d}{z}_0)]\cdot [(\frac{x-x_j}{r_j}\mathrm{d}{x}_j+\frac{y-y_j}{r_j}\mathrm{d}{y}_j+\frac{z-z_j}{r_j}\mathrm{d}{z}_j)-(\frac{x-x_0}{r_0}\mathrm{d}{x}_0+\frac{y-y_0}{r_0}\mathrm{d}{y}_0+\frac{z-z_0}{r_0}\mathrm{d}{z}_0)]]\\ & =E[[(e_{xi}\mathrm{d}{x}_i+e_{yi}\mathrm{d}{y}_i+e_{zi}\mathrm{d}{z}_i)-(e_{x0}\mathrm{d}{x}_0+e_{y0}\mathrm{d}{y}_0+e_{z0}\mathrm{d}{z}_0)]\cdot [(e_{xj}\mathrm{d}{x}_j+e_{yj}\mathrm{d}{y}_j+e_{zj}\mathrm{d}{z}_j)-(e_{x0}\mathrm{d}{x}_0+e_{y0}\mathrm{d}{y}_0+e_{z0}\mathrm{d}{z}_0)]]\\ & =\{\begin{array}{l}2{\sigma }_s^{2}i=j\\ {\sigma }_s^{2}i\ne j\text{不同方向，不同站址的误差相互独立，互协方差为0}\end{array}\end{array}$$

由此，$cov(\mathrm{d}R)+cov(\mathrm{d}S)=[f_{ij}{]}_{M\cdot M}$的每个元素，可写为
$$\{\begin{array}{l}{\sigma }_{ri}^2+2{\sigma }_s^{2}i=j\\ {\eta }_{ij}{\sigma }_{ri}{\sigma }_{rj}+{\sigma }_s^{2}i\ne j\end{array}$$
则定位的总协方差可以写为：
$$P=cov(\mathrm{d}X)==(H^{T}H{)}^{-1}{H}^T[f_{ij}{]}_{M\cdot M}H(H^{T}H{)}^{-1}$$
它是一个$[3\cdot 3]$的方阵。定位的几何精度可以写为:
$$GDOP=\sqrt{P(1,1)+P(2,2)+P(3,3)}$$
因此，使用TDOA定位时，影响其精度的因素主要有站址误差和时间差测量误差。