TDOA定位算法之两步加权最小二乘（TWLS）（完整版附代码）

TDOA观测模型

TDOA（Time Difference Of Arrival）即目标发送信号到达各个观测站的时间差，在三维直角坐标系下，TDOA观测示意图如图１所示。

假设第$i$个观测站的位置向量为${\mathbf{s}}_i^o=[s_{i,x}^o,s_{i,y}^o,s_{i,z}^o{]}^T,1\le i\le M$，则将所有观测站向量组合得到总的观测站位置向量为${\mathbf{s}}^o=[{\mathbf{s}}_1^o,{\mathbf{s}}_2^o,...,{\mathbf{s}}_M^o{]}^T$。目标位置向量定义为${\mathbf{u}}^o=[u_x^o,u_y^o,u_z^o{]}^T$。

为不失一般性，将第一个观测站作为参考观测站。因此，基于图1和定义的位置向量可以得到TDOA的计算方式为：
$${\tau }_{j1}^o=\frac{||{\mathbf{u}}^o-{\mathbf{s}}_j^o|{|}_2}{c}-\frac{||{\mathbf{u}}^o-{\mathbf{s}}_1^o|{|}_2}{c},2\le j\le M\text{\hspace{1em}(1)}$$
${\tau }_{j1}^o$表示目标信号到达第$j$个观测站和参考观测站的时间差真实值。其中$c$表示信号传播速度，通常是已知的。因此可以将TDOA转换为到达距离差（Range Difference Of Arrival，RDOA）：
$$r_{j1}^o=r_j^o-r_1^o=||{\mathbf{u}}^o-{\mathbf{s}}_j^o|{|}_2-||{\mathbf{u}}^o-{\mathbf{s}}_1^o|{|}_2,2\le j\le M\text{\hspace{1em}(2)}$$
考虑观测噪声后得到：
$$r_{j1}=r_{j1}^o+\delta r_{j1}\text{\hspace{1em}(3)}$$
将所有观测站的观测值组合得到向量形式为：
$$\mathbf{r}={\mathbf{r}}^{\mathbf{o}}+{\mathbf{n}}_{\mathbf{r}}=[r_{21}^o,r_{31}^o,...,r_{M1}^o{]}^T+[\delta r_{21},\delta r_{31},...,\delta r_{M1}{]}^T\text{\hspace{1em}(4)}$$
其中${\mathbf{r}}^{\mathbf{o}}=[r_{21}^o,r_{31}^o,...,r_{M1}^o{]}^T$表示真实值观测向量，${\mathbf{n}}_{\mathbf{r}}=[\delta r_{21},\delta r_{31},...,\delta r_{M1}{]}^T$表示观测噪声向量。其中${\mathbf{n}}_{\mathbf{r}}$的协方差矩阵记为${\mathbf{Q}}_{\mathbf{r}}$。

TDOA定位CRLB

未知参数向量为${\mathbf{u}}^{\mathbf{o}}=[u_x^o,u_y^o,u_z^o{]}^T$，测量向量为$\mathbf{r}$ 。因此$\mathbf{r}$关于${\mathbf{u}}^{\mathbf{o}}=[u_x^o,u_y^o,u_z^o{]}^T$的对数似然函数为：
$$ln(p(\mathbf{r}|{\mathbf{u}}^{\mathbf{o}}))=\kappa -(1/2)(\mathbf{r}-{\mathbf{r}}^o{)}^T{\mathbf{Q}}_{\mathbf{r}}^{-1}(\mathbf{r}-{\mathbf{r}}^o)\text{\hspace{1em}(5)}$$
其中$\kappa$表示常数项。
则CRLB表示为：
$$\mathbf{CRLB}=(\left(\frac{\partial {\mathbf{r}}^o}{\partial ({\mathbf{u}}^{\mathbf{o}}{)}^T}\right){\mathbf{Q}}_{\mathbf{r}}^{-1}{\left(\frac{\partial {\mathbf{r}}^o}{\partial ({\mathbf{u}}^{\mathbf{o}}{)}^T}\right)}^T$$