TDOA/FDOA联合定位：TSWLS与改进算法的大噪声性能对比

前阵子为了给一组运动目标监测数据挑合适的定位算法，我把TDOA/FDOA联合定位的仿真流程重新完整跑了一遍。以前读文献时总觉得两步加权最小二乘（TSWLS）理所应当是最稳的那个方案，可真把布站、噪声、目标速度这些条件摆到一起，才发现“哪种改进方法更好”这个问题的答案，远不是一句话能说清的。这次我把TSWLS、约束加权最小二乘、半正定松弛、最大似然精化这几类方法放进同一个仿真框架里做了对比，踩了不少坑，也把“较量”背后的分水岭看清楚了。

1. 联合定位的双输出收益：位置与速度凭什么同时估

1.1 TDOA和FDOA各自的短板

TDOA测的是信号到达不同接收站的时间差，本质上是距离差。多站的距离差可以交出一组双曲面，交点就是目标位置。它的优势是只需要各站时间同步，不依赖目标发射时刻，因此在无源场景里非常实用。但单独用TDOA有一个明显短板：它对静止目标很友好，对运动目标却很被动。因为距离差只反映目标当前的几何位置，目标在动这个事实不会直接体现在测量方程里，某些布站构型下甚至会出现位置误差放大得厉害的情况。

FDOA测的是频率差，也就是多普勒差。目标运动时，不同接收站看到的径向速度不一样，导致接收频率出现差异。这个差异本质上包含了目标速度和位置的信息，但它对静止目标无能为力——目标不动，FDOA测量值几乎全是噪声。所以单用FDOA去定位静止目标完全行不通。

把两者放在一起看，逻辑就清楚了：TDOA负责把位置定住，FDOA负责把速度拎出来，同时频率差里隐含的位置信息反过来还能约束位置估计。这个互补关系，是TDOA/FDOA联合定位能同时输出位置和速度的真正原因。

1.2 联合观测方程的建立

工程上建模时，通常先把时差测量换算成等效距离差。设接收站位置为 (s_i)，目标位置为 (u)，目标速度为 (v)，则距离差测量为

[ r_{i1} = r_i - r_1 = |u - s_i| - |u - s_1| + n_{r,i} ]

其中 (r_i = |u - s_i|) 是目标到第 (i) 站的距离，(r_1) 是到参考站的距离，(n_{r,i}) 是等效距离差噪声。频率差测量可以按载波波长 (\lambda) 转成等效距离差变化率：

[ f_{i1} = \frac{\dot{r}_i - \dot{r}1}{\lambda} + n{f,i} ]

而距离变化率

[ \dot{r}_i = \frac{(v - v_i)^T (u - s_i)}{r_i} ]

其中 (v_i) 是接收站运动速度。假设接收站本身不动，则 (v_i = 0)，频率差完全由目标速度贡献。从这些式子能看出来，观测方程是非线性的，而且TDOA和FDOA共享同一个目标位置 (u)，速度 (v) 会同时出现在FDOA里，位置 (u) 又会在FDOA的偏导里出现。这种耦合关系让两类测量不再是独立的两组方程，而是互相牵制的联合方程组。

1.3 联合带来的信息互补逻辑

单从参数数量看，三维定位加定速需要6个未知量（位置3个、速度3个）。如果只用TDOA，一个目标位置有3个未知量，需要的站数至少4个；但速度完全定不了。如果只用FDOA，速度能定，位置又会退化。联合以后，4个站可以提供3个TDOA方程和3个FDOA方程，总共6个方程，刚好匹配6个未知量。再多一个站，冗余度上升，定位精度和稳健性都会改善。

信息互补还在更深一层：FDOA方程里包含位置项，这就好比你在测速度的时候顺带又用卷尺量了一次位置；TDOA方程的残差又可以反过来修正速度估计。联合估计的协方差矩阵会比分开估计显著收紧，只要噪声模型给得准，位置RMSE和速度RMSE都能往下压。这也是后面所有对比算法共同面对的信息红利。

2. TSWLS两步推导：线性化磨平非线性，约束修正拉回偏差

2.1 第一步：设一个辅助变量把方程变直

TSWLS的思路并不复杂：非线性方程之所以难解，是因为距离 (r_i) 带根号。为了消掉根号，第一步引入辅助变量 (r_1)，把目标到参考站的距离当成一个额外的未知量。这样对每一对 (i) 和参考站1，可以把距离差的定义改写成代数式：

[ (r_{i1} + r_1)^2 = |u - s_i|^2 ]

展开后，关键的平方项 (r_1^2) 会与参考站到目标的距离平方相消，最终整理成关于 (u) 和 (r_1) 的线性方程：

[ 2(s_i - s_1)^T u + 2 r_{i1} r_1 = |s_i|^2 - |s_1|^2 - r_{i1}^2 ]

把这个方程在多个站之间联立，就得到一个矩阵形式：

[ G_1 \theta_1 = h_1 + \epsilon_1 ]

其中 (\theta_1 = [u^T, r_1]^T) 是第一步的待估向量。FDOA方程类似，对时间求导后，会多出 (v) 和 (\dot{r}_1)，对应的待估变量扩展到 (\theta = [u^T, r_1, v^T, \dot{r}_1]^T)。TDOA和FDOA方程一起放进同一个增广矩阵 (G)，就成了标准的加权最小二乘形式：

[ \hat{\theta} = (G^T W G)^{-1} G^T W h ]

这里的 (W) 必须取测量误差传播到方程残差后的协方差逆矩阵，不能随手给个单位阵，否则第一步解出来就带偏了。

2.2 第二步：几何约束参与的加权修正

第一步把 (r_1) 当成独立变量来解，优点是方程线性化了，缺点也很明显：(u) 和 (r_1) 本来不是独立的，它们之间必须满足 (r_1 = |u - s_1|)。第一步完全没有用到这个几何约束，所以高噪声下解出来的 (r_1) 和 (u) 会互相矛盾。

第二步的思路是把这个约束重新请回来。记第一步估计值与真值的偏差为 (\Delta \theta)，把约束式在估计点附近展开：

[ r_1^2 = |u - s_1|^2 ]

两边取一阶展开，忽略二阶小量 ( |\Delta u|^2)，就能得到关于 (\Delta \theta) 的一个线性修正方程。联立第一步的残差加权项，再做一次WLS，得到偏差的最优估计 (\hat{\Delta\theta})，最后从第一步估计里减掉，得到修正后的结果。

这一步的效果非常明显。噪声小时，第一步线性化的误差本来就小，第二步只是锦上添花；噪声大时，第一步丢失的几何约束会积累成系统性偏差，第二步能把偏差往回收一截。这也是TSWLS在中等噪声下能贴着CRLB的关键原因。

2.3 推导中容易翻车的三个地方

写代码复现TSWLS时，有三个细节特别容易出错。

第一个是第二步的协方差矩阵。很多初学者图省事，直接复用第一步的 (W)，这是错的。第二步的观测方程是把第一步估计值的误差当作噪声来处理的，残差的协方差必须通过误差传播公式重新推一遍。如果你不重新算，第二步的“加权”就失真了，某些情况下修正量反而会把结果推得更远。

第二个是FDOA线性化时的偏导数漏项。(\dot{r}_i) 对 (u) 有偏导，对 (v) 也有偏导，而 (r_i = |u - s_i|) 本身还出现在分母上。林林总总加在一起，增广矩阵的每个块都要逐一核对维度和符号。我见过不止一个复现代码漏了 (v) 对 (u) 的交叉项，结果FDOA部分基本白测。

第三个是参考站的选择。换一个参考站，TDOA/FDOA的测量组合就全变了，协方差矩阵也要跟着变，最终定位结果稍有差异。如果布站几何不对称，参考站选择还会影响病态程度，建议把每个站轮流当参考站跑一次，观察结果稳定性，这一步能帮你发现很多隐藏问题。

3. 仿真框架搭建：布站、噪声等效和评价尺度

3.1 布站与航迹参数设定

仿真不能一上来就随机撒点，那样对比结果没有可复现性。我用的是一组典型的非对称布站，四个接收站坐标设定为：(S_1=(0,0,0)) km，(S_2=(30,0,0.01)) km，(S_3=(0,30,0.02)) km，(S_4=(25,25,0.03)) km。目标起点的真实位置放在 ((10,10,1)) km，速度设为 ((60,30,0)) m/s。之所以选非对称布站，是因为实际部署很少能做到规则对称，对称布局在仿真里往往会掩盖掉某些算法的病态问题。

噪声等级则按照扫描方式设置：TDOA的等效距离噪声标准差从0.03 m扫到3 m，FDOA的频率噪声按载波频率2.4 GHz换算成等效径向速度误差，从0.01 m/s扫到1 m/s。每档噪声下做1000次蒙特卡洛，目标位置和速度固定不变，每次只用独立测量噪声扰动。这样对比出来的曲线不会因为目标轨迹不同而忽上忽下。

3.2 时差频差的协方差怎么给

这里特别要提醒：TDOA测量通常不是独立同分布的。实际系统里各站先有TOA测量，再以1号站为参考做差分，差分后的噪声天然存在相关性。如果直接把TDOA协方差当对角阵，等于默认了各站TOA之间完全独立差值的协方差仍是对角的，这在数学上站不住脚。

正确做法是构造一个差分矩阵 (D)，把原始的TOA噪声协方差 (Q_t) 左乘右乘变换为

[ Q_{tdoa} = D Q_t D^T ]

FDOA同样处理。TDOA和FDOA两块测量之间存在相关性的话，还要在联合协方差矩阵里填上互协方差块。实际仿真里我可以先假设两类噪声独立，把联合协方差写成块对角，但你要清楚这只是一个近似假设。后面算CRLB和加权矩阵时，我都基于这个块对角协方差推导，尽量保持模型自洽。

3.3 RMSE、CRLB比与偏差三项指标

光看RMSE是不够的，我会同时看三个量：位置和速度的RMSE、RMSE与CRLB的比值、以及估计偏差的范数。

RMSE反映整体精度，CRLB比反映算法离理论极限还有多远。偏差范数则特别重要，因为它能把“随机误差大”和“系统性偏了”区分开。TSWLS在高噪声下失效的主要表现不是RMSE整体变大，而是偏差明显抬升，位置和速度被系统性拉向某个方向。只看RMSE曲线的话，你很难判断问题出在方差还是偏差上。

仿真伪代码大致是这样：

for 每档噪声水平 (sigma_r, sigma_v) for k = 1:1000 % 蒙特卡洛次数 生成真实目标位置和速度 计算理想TDOA/FDOA 叠加零均值高斯噪声，协方差由Q_tdoa和Q_fdoa确定 调用TSWLS / CWLS / SDR / ML函数，得到估计值 记录误差与偏差 end 计算位置RMSE、速度RMSE、CRLB比、偏差范数、平均耗时 end

这个框架的好处是各算法共用同一个测量数据集，比较结果公平且可控。

4. 改进算法三条路线：约束加权、凸松弛和迭代精化

4.1 CWLS：把 (r_1^2 = |u - s_1|^2) 直接写进最优化

CWLS的出发点很朴素：TSWLS把几何约束放在第二步才补救，那为什么不直接在第一步的代价函数里就把这个约束带上？于是目标函数写成

[ \min_{\theta} \quad (h - G\theta)^T W (h - G\theta) ] [ \text{s.t.} \quad r_1^2 = |u - s_1|^2 ]

这个约束是二次等式约束，最优解可以通过拉格朗日乘子法转换为广义特征值问题求解，不需要迭代。它是凸化的近亲，但保留了问题本身的闭式结构，计算量只比TSWLS略大一点。

仿真里CWLS的表现很有意思：中等噪声下它和TSWLS差距很小，但噪声增大后，它的偏差明显小于TSWLS。原因不难理解，约束在求解过程中一直被满足，就不会出现TSWLS第二步修正不彻底的问题。它的缺点是只对单个二次约束有效，如果测量模型复杂度上升，约束一多，闭式解就不存在了。

4.2 SDR：凸化之后没有局部极值

SDR走的是另一条路：把非线性等式约束松弛成半定约束。具体做法是把待求变量通过矩阵变量重新组织，让非凸的双线性项变成线性项，再对秩1约束做凸松弛。得到的半定规划是凸问题，用现成求解器求出来就是全局最优，不存在局部极小点的问题。

SDR在理论上非常漂亮，但代价是慢。单次求解几百毫秒级别，跟TSWLS的几毫秒完全不在一个量级。仿真里它最大的价值是当“天花板”来用，用来确认其他算法离全局最优有多远。事实上，在所有对比方法里，SDR的RMSE最贴近CRLB，偏差也最小，尤其是在大噪声区，优势非常稳定。

4.3 以TSWLS为初值的最大似然精化

最大似然估计直接对原始非线性观测模型建模，优化目标为

[ \min_{u,v} \quad (z - h(u,v))^T Q^{-1} (z - h(u,v)) ]

这个目标是非凸的，直接用牛顿迭代容易掉进局部极值。但有一个实用技巧：用TSWLS的结果做初值，然后高斯牛顿法只迭代几步，往往就能大幅改善精度，同时又不会太慢。因为TSWLS初值在小噪声区已经足够接近真值，局部收敛区基本一步就进去了。

我把这个组合叫做“TSWLS+ML”，它本质上是务实派选手。仿真里它的位置RMSE几乎全程贴着CRLB，速度RMSE也很稳，单次耗时比纯ML从零开始迭代少很多。如果你要在实际系统里选一个兼顾精度和实时性的方案，它比纯SDR合适得多。

5. 仿真结果：从贴近CRLB到阈值失效的分水岭

5.1 中等噪声区：差距毫厘之间

当TDOA等效距离噪声在0.3 m左右、FDOA等效速度噪声在0.01 m/s量级时，TSWLS、CWLS、SDR和TSWLS+ML四种方法的位置RMSE和CRLB之间的比值都落在1.02到1.1之间。这个区间内，所有算法看起来都“够用”，TSWLS并没有落后到需要替换的地步。

速度估计的表现也类似。FDOA信息的加入让速度估计有了直接观测来源，几类方法的RMSE差距被压缩到很小的范围内。从工程角度看，这个噪声段选哪种算法都差别不大，重点会转向计算复杂度。TSWLS和CWLS在这时最吸引人，因为它们快，而且快得不是一点点。

5.2 大噪声区：TSWLS的偏差先崩了

当TDOA等效距离噪声升高到3 m、FDOA等效速度噪声升高到1 m/s时，分水岭就出现了。TSWLS的位置RMSE与CRLB的比值急剧上涨到8倍左右，偏差范数同步飙升。CWLS仍能坚持在2倍左右，SDR和TSWLS+ML则稳定在1.2倍上下。

速度估计的差距更典型。TSWLS在高噪声区的速度偏差明显增大，直观解释是第一步线性化误差与FDOA方程里的分母项 (r_i) 耦合后被放大了，第二步修正对这种系统性误差的补偿能力有限。而CTWLS在约束层面天然锁住了一部分几何关系，SDR的全局最优特性又锁住了另一部分误差来源。

下面是仿真里几个典型噪声档位的对比结果：

这个表格基本还原了我这次仿真的核心结论：改进方法的优势不在顺风局，而在逆风局。

5.3 计算代价与稳定性的权衡

差距毫厘的区间内选谁都可以，但上了系统就不是纯看RMSE了。SDR虽然稳，200毫秒的单次耗时在实时跟踪场景里几乎不可接受；TSWLS虽然快，在大噪声区又是全面崩盘。真正实用的是TSWLS+ML这个组合：它用TSWLS提供初值，用几次迭代把精度拉回CRLB附近，同时保持了毫秒级的计算效率。

CWLS则处在中间位置。它比TSWLS稍慢，但在大噪声区的稳定表现远好于TSWLS，是“不想让系统复杂度上升太多、又需要一定鲁棒性”时的折中选择。

6. 仿真复盘：参数细节与不容易写进论文的坑

6.1 布站几何病态：共线布置直接让TSWLS失灵

仿真里我第一次尝试把四个站放在一条直线上，TDOA方程组的空间维度直接坍缩，位置估计只能在直线两侧模糊出镜像点，TSWLS的矩阵一步就接近奇异，输出结果毫无意义。即使是接近共线的布局，条件数也会变得很大，FDOA方程对目标速度的分辨能力同样下降。

这个问题不是某一种算法能单靠改进解决的，它属于测量几何本身的信息不足。遇到这种情况，优先调整布站构型，或者把轨迹分段后做多帧累积。算法层面的补救有限，不要指望TSWLS或者SDR能变出信息。

6.2 量纲混用是联合定位的大敌

TDOA和FDOA天然带着两套量纲：一个是长度，一个是长度除以时间。仿真时如果不统一量纲就堆协方差矩阵，加权矩阵会倾斜，位置和速度的贡献失衡，CRLB也会算错。

统一量纲的方法很简单，FDOA的频差测量乘以载波波长，得到等效距离变化率，就和TDOA统一到了长度量纲。频率噪声也要同步转换成等效速度噪声再进协方差矩阵。我一度因为忘了这个转换，在速度RMSE曲线里看到莫名其妙的高斯曲线，排查了好几轮才发现是协方差矩阵里两个对角块差了12个数量级所致。

6.3 固定的噪声种子与公平对比

不同算法之间做对比时，如果每一次蒙特卡洛都重新生成随机噪声，那么各算法得到的测量数据集可能不同。虽然大数定律下最终RMSE趋同，但中间曲线会毛刺丛生，很难判断某个波动是算法差异还是随机噪声差异。

我在仿真里固定了随机种子，并且让所有算法按同一批噪声数据跑。这样每条误差曲线的每个点都是相同测量集下的不同估计，对比才足够公平。另一个细节是，Report里的RMSE和CRLB比建议在每档噪声下单独计算，不要把所有噪声档位混合在一起统计，否则高噪声档会主导均值，掩盖中等噪声下算法的真实水平。

还有个容易被忽视的点：如果仿真里加入接收站站址误差，噪声模型不能只对TOA和频率建协方差。站址误差会在测量方程里引入额外扰动项，需要把它的标准差单独设定并参与到误差传播里。我试过叠加0.1 m量级的站址误差后，所有算法的RMSE下限都被抬高，CRLB比值反而普遍下降，因为模型本身已经包含站址误差的协方差了。

仿真做到最后，我个人对这套对比框架的感受是：别再迷信某个算法在所有场景下的“最优”结论。TSWLS在整个中等噪声区的表现完全对得起它的计算量，真正需要警惕的是高噪声区的阈值效应；而当你面对未知噪声条件的系统时，TSWLS+ML这种组合是最省心的稳妥选择——它保留了TSWLS的速度，又用少量迭代把精度拉回CRLB附近。下次再做类似定位仿真，我会直接把布站几何诊断、协方差量纲和噪声种子控制这三个环节放到代码最前面，它们比多跑几组对比算法更能决定仿真结论是否可信。