本文復現文章：

• 王桂杰,焦良葆,曹雪虹.基于最小二乘法的室內三維定位算法研究[J].計算機技術與發展,2020,30(04):69-73.

按照文章的核心算法，復現了TDOA下的最小二乘在三維環境中的改進定位方法，方法可以明顯提升Z軸的定位精度

概述

代碼實現了一種改進的加權最小二乘法（LS）用于三維TDOA定位，主要解決傳統LS方法在高度方向估計精度不足的問題。通過4個基站的到達時間差（TDOA）測量值，實現三維空間中待定位節點的坐標解算。代碼包含以下關鍵模塊：

1. 場景配置：定義基站拓撲與測試點分布
2. 信號建模：生成含噪聲的TDOA測量值
3. 定位解算：
   • 傳統LS方法：直接求解三維坐標
   • 改進LS方法：分層解算（二維平面+高度優化）
4. 性能評估：計算定位誤差并進行三維可視化

TDOA觀測模型：
設基站坐標為${\mathbf{b}}_i=[x_i,y_i,z_i{]}^T$, 待定位點為$\mathbf{p}=[x,y,z{]}^T$，則距離差測量值為：
$$\Delta r_{ij}=\Vert \mathbf{p}?{\mathbf{b}}_i\Vert ?\Vert \mathbf{p}?{\mathbf{b}}_j\Vert +{\epsilon }_{ij}$$
其中${\epsilon }_{ij}$為測量噪聲，服從$\mathcal{N}(0,$