TDOA/FDOA联合定位算法仿真对比：TSWLS与改进方法解析

TDOA/FDOA联合定位这几个字，搞无源定位的人看到基本都会多停两秒。原因很简单，纯TDOA只能估计目标位置，目标一旦动起来，速度信息缺失会导致跟踪发散；纯FDOA能测速，但位置收敛慢得让人着急。两个观测量捏在一起，位置和速度一把梭，才是工程上真正实用的玩法。而TSWLS作为这个领域里绕不开的经典闭式解，它的性能和坑位我都踩过，这些年也看过不少改进版的思路，这次干脆把仿真过程和对比结果完整拆开聊。

这篇文章适合正在做无源定位、目标跟踪、电子侦察相关课题的同学，也适合想把TDOA/FDOA联合定位算法从公式推到代码落地一把梭的工程师。我会把TSWLS的原理、改进方法的核心思路、仿真场景设计以及几个容易翻车的细节全部铺开讲，最后给出我在实测中踩过的坑和调参心得。

1. 为什么要把TDOA和FDOA放在一起做联合定位

很多初学者容易有个误区：TDOA测时差，FDOA测频差，两个观测量各管各的，联合起来不就是多一个方程而已？实际上远没有这么简单。先说TDOA的原理，一个目标辐射信号到达两个观测站的时间差，乘以光速就是距离差，多个站的距离差方程一联立，目标位置就落在双曲面交点上。但这里有个隐含假设——目标在测量时间内是静止的，或者至少运动对时差的影响可以忽略。目标一旦高速运动，比如飞行器、高轨卫星，单帧TDOA里包含了目标移动带来的时变项，位置估计直接是偏的。

FDOA解决的就是这个动态问题。目标运动导致到达不同观测站的信号产生不同的多普勒频移，站间频差对应的是目标到各站距离变化率之差，本质上是目标速度在站目连线方向上的投影差。有了FDOA测量，目标的运动速度向量就被约束住了。但是纯FDOA定位在几何条件不好时，位置的收敛性极差，因为频差对位置的一阶灵敏度远低于时差。

所以TDOA/FDOA联合定位的核心动机是把"位置敏感"和"速度敏感"两类观测融合，目标状态向量从三维位置扩展到六维（位置加速度）后，方程数量刚好够约束住所有维度。工程上常见的场景是运动目标辐射源定位，比如无人机图传信号源定位、舰载雷达信号侦察、卫星干扰源定位，目标不可能静止等你测完时差再测频差，必须在一个观测帧内同时完成两类测量。这就是联合定位不可替代的价值。

从数学建模角度看，联合定位的观测方程可以写成：

• TDOA测量：目标到第i站与到参考站的距离差，即 r_i1 = ||s - s_i|| - ||s - s_1||，其中s为目标位置，s_i为第i个观测站位置
• FDOA测量：对应的距离变化率差，即 v_i1 = (v - v_i)ᵀ(s - s_i)/||s - s_i|| - (v - v_1)ᵀ(s - s_1)/||s - s_1||，其中v为目标速度，v_i为观测站速度

这个方程组非线性程度很高，而且目标位置和速度在方程里是耦合的，不能像定位静止目标那样分步解算。于是就有了TSWLS这类闭式解法，以及后续一堆改进算法。

2. TSWLS算法拆解：两步加权最小二乘到底做了什么事

TSWLS全称Two-Stage Weighted Least Squares，国内文献一般叫两步加权最小二乘。它的核心思想是用"辅助变量"把非线性方程伪线性化，然后分两步用加权最小二乘做修正。

2.1 第一步：伪线性化与辅助变量引入

直接解 TDOA/FDOA 方程组的难点在于那个范数项 ||s - s_i||，它是目标位置的非线性函数。TSWLS的第一步引入辅助变量 r1 = ||s - s_1||（目标到参考站的真实距离）和其变化率 r1_dot = (v - v_1)ᵀ(s - s_1)/||s - s_1||，把原来的非线性方程改写成关于向量 [s, v, r1, r1_dot] 的伪线性方程。

具体推导过程不啰嗦，核心在于对TDOA方程两边平方，把距离差展开后整理成线性形式。这一步本质上是把二次项的高阶误差吸收到噪声项里。在噪声较小的情况下，这一步得到的估计值已经比较接近真实值了。通常用加权最小二乘求解：

u = (Aᵀ W A)^{-1} Aᵀ W b

其中 W 是 TDOA/FDOA 测量噪声的协方差矩阵 Q 的逆矩阵。这里有个关键点——W 的选取直接影响第一步估计的质量。很多文章里的仿真结果偏乐观，就是因为噪声协方差矩阵设置的太理想化，实际工程里 Q 往往要现场标定。

第一步求出的 u 是一个 8 维向量（位置3维、速度3维、辅助变量r1、r1_dot），但辅助变量 r1 和 r1_dot 并不是独立的，它们和位置、速度存在严格约束关系。这个约束关系没有被利用，所以第一步的估计是次优的。

2.2 第二步：用约束关系做二次修正

第二步专门处理辅助变量与状态向量之间的约束。因为 r1 = ||s - s_1||，把它平方得到 r1² = (x - x1)² + (y - y1)² + (z - z1)²，这个式子把辅助变量和位置分量联系了起来。对时间求导，还能得到 r1·r1_dot = (x - x1)(vx - vx1) + (y - y1)(vy - vy1) + (z - z1)(vz - vz1)，速度分量也被约束住了。

把第一步得到的估计值和真实值之间的偏差写成扰动形式，代入约束方程再次整理成线性方程，再做一次加权最小二乘。第二次估计的结果，理论上精度接近克拉美罗下界CRLB。这也是TSWLS能被广泛接受的原因——闭式解，不需要初值迭代，小噪声下统计最优。

我当年第一次跑完TSWLS仿真时，看到RMSE曲线贴着CRLB走，觉得这算法简直完美。后来把噪声加大才发现事情没那么简单。

2.3 TSWLS的关键局限：为什么高噪声下对不上CRLB

TSWLS最核心的软肋在高噪声场景下暴露得非常明显。第一步伪线性化时，方程两边同时平方引入了噪声的平方项。噪声小的时候平方项是高阶小量，可以忽略；噪声大的时候，平方项带来的偏差会被加权矩阵放大，导致第一步估计严重偏离真实值。更麻烦的是，第一步的权重 W 依赖于测量噪声协方差 Q，但实际误差的统计特性已经和 Q 不匹配了，加权最小二乘的"最优"特性失效。

另一个问题是门限效应。随着噪声功率增大，TSWLS估计误差的RMSE曲线会在某个临界点突然偏离CRLB，呈指数级恶化。这个现象在学术上叫"threshold effect"，实际工程中表现为目标稍微远一点或者信噪比低一点，定位结果直接跳到错误的位置上。我在仿真中见过不少这种情况，TSWLS估计出的目标位置落在完全错误的方向上，速度也对应出错，整个跟踪航迹崩掉。

这两个问题也直接催生了后来各种改进方法——有人做约束加权最小二乘，有人做半正定松弛，有人拿TSWLS当初始值再做最大似然迭代。

3. 改进思路怎么改：约束、松弛与迭代细化

TSWLS的问题根源在于两个地方：第一步没利用辅助变量约束，第二步对扰动项做了线性近似。改进方法基本都围绕这两点展开。

3.1 约束加权最小二乘CWLS的思想

CWLS的思路是与其分两步走，不如把约束条件直接放进优化问题里。具体做法是把辅助变量 r1 和 r1_dot 视作未知量，同时加上 r1² = (x - x1)² + (y - y1)² + (z - z1)² 这个等式约束，构成一个带约束的最小二乘问题。

这个优化问题仍然是非凸的，但可以通过拉格朗日乘子法求解。拉格朗日函数对状态变量求导等于零，得到一个线性方程组，再通过代入约束方程得到一个关于乘子的一元多项式方程。选取合适的根，代回去就得到状态估计。

CWLS相比TSWLS的优势在于没有把噪声平方项直接扔掉，而是通过约束条件把辅助变量和状态之间的非线性关系保留了下来，所以高噪声下的门槛效应出现得更晚，整条RMSE曲线更贴近CRLB。我之前仿真对比时发现，噪声标准差是TSWLS门限点两倍的情况下，CWLS还能勉强维持跟踪。

3.2 半正定松弛SDP处理非凸问题

CWLS虽然比TSWLS稳，但拉格朗日乘子法在求解时需要对多项式方程选根，选错根就直接废了。更稳的做法是半正定松弛SDP。

SDP的思想是把原问题中的二次等式约束松弛成半正定矩阵约束，把非凸优化问题转成凸优化问题。用CVX这类工具可以直接求解，不需要考虑选根问题。SDP的性能在高噪声下比CWLS还好，因为它在一开始就没有丢弃任何约束信息，全局最优性在松弛后有保证。

代价是计算量上了一个台阶。TSWLS是纯矩阵运算，毫秒级甚至微秒级；SDP需要迭代求解，单次求解时间在几十到几百毫秒级别，对实时性要求高的场景不太友好。我试过在MATLAB里跑CVX求解SDP，目标数量多的时候一帧数据要处理几百毫秒，做实时定位跟踪基本喘不过气。

3.3 牛顿迭代与ML细化：工程上最实用的组合

工程上最实用的方案反而不是那些花哨的闭式解，而是把TSWLS的估计值当作初始点，然后用高斯牛顿迭代去逼近最大似然解。TDOA/FDOA的测量噪声在高信噪比下近似高斯分布，最大似然估计在理论上就可以达到CRLB。但ML问题的目标函数是非凸的，直接迭代容易陷入局部极值，所以初值很关键。

TSWLS恰好提供了一个足够好的初值。虽然高噪声下TSWLS的估计有偏差，但通常还在真实位置附近，不太会跳到遥远的局部极值。在这个初值基础上做牛顿迭代，每次迭代需要计算目标函数对状态向量的雅可比矩阵和海森矩阵，然后沿着下降方向更新。

这类方法的收敛速度很快，通常五六步就能收敛。我实测过，TSWLS初值加牛顿迭代在高噪声下的性能可以做到和SDP差不多，计算量却低了一两个数量级。如果仿真中既想要精度又不想算到天荒地老，这个组合是首选。

4. 仿真环境搭建与实验设计

算法对比不能凭感觉，必须有一套可复现的仿真环境。下面是我在MATLAB里搭建的实验框架，所有参数都可以直接替换复现。

4.1 观测站布局与目标运动场景

仿真第一步是设置观测站坐标和目标运动轨迹。观测站布局直接影响定位精度的几何稀释因子，所以我设计了两个对比场景。

场景一是"近距离低速目标"，模拟无人机图传信号源定位。观测站布成一个菱形加中心站的构型，坐标如下表：

目标初始位置在 (5000, 6000, 3000)，速度 (20, -15, 5)。这个场景的站间距和目标距离相近，几何条件中等偏优。

场景二是"远距高速目标"，模拟空中目标侦察。观测站仍然在地面，目标在 (50km, 40km, 10km) 位置，速度 (300, -200, 50)。站间距相对目标距离小得多，定位几何变差，更考验算法在不良几何下的鲁棒性。

4.2 噪声建模与协方差矩阵

TDOA和FDOA的测量噪声我在仿真里都建模为零均值高斯分布。TDOA噪声标准差设为σ_t = 10ns，换算成距离大约是3米；FDOA噪声标准差设为σ_f = 0.1Hz。观测站位置误差也要考虑，我加了一个小量级的站址误差，标准差1米。

噪声协方差矩阵Q的构造比较讲究。TDOA和FDOA各站之间不是独立的——因为所有测量都相对于参考站做差，各站与参考站的测量噪声会互相耦合。具体来说，如果各站自身的到达时间到达频率测量噪声相互独立，那么参考站噪声会同时出现在所有TDOA/FDOA测量里，导致它们之间产生相关性。构造Q矩阵时这些相关性一个都不能少，不然你会得到过分乐观的CRLB和算法仿真结果。

Q矩阵的分块结构可以写成：

Q = blkdiag(Q_t, Q_f)

其中Q_t是TDOA噪声协方差，非对角元素为σ_t²/2（因为参考站噪声被减了两次），对角元素为σ_t²。FDOA的Q_f同理。

4.3 仿真流程与评估指标

整体仿真流程是蒙特卡洛循环：

for mc = 1:num_mc % 生成真实目标位置和速度 s_true = [5000 6000 3000]; v_true = [20 -15 5]; % 计算真实TDOA和FDOA值 tdoa_true = compute_tdoa(s_true, station_pos); fdoa_true = compute_fdoa(s_true, v_true, station_pos, station_vel); % 加噪声 noise = mvnrnd(zeros(1, 2*(N-1)), Q, 1); tdoa_meas = tdoa_true + noise(1:N-1); fdoa_meas = fdoa_true + noise(N:end); % 分别用TSWLS、CWLS、SDP、TSWLS+Newton估计 s_est_tswls = tswls_solver(tdoa_meas, fdoa_meas, station_pos, station_vel, Q); s_est_cwls = cwls_solver(tdoa_meas, fdoa_meas, station_pos, station_vel, Q); % ... 其他算法 ... % 记录误差 err_tswls(mc) = norm(s_est_tswls(1:3) - s_true); % ... 其他误差记录 ... end rmse = sqrt(mean(err.^2));

评估指标用RMSE（均方根误差），分别算位置估计的RMSE和速度估计的RMSE。另外要和CRLB对比，CRLB通过Fisher信息矩阵求逆得到：

% 计算雅可比矩阵 J = jacobian_measurement(s_true, v_true, station_pos, station_vel); fim = J' * inv(Q) * J; crlb = inv(fim); rmse_crlb_pos = sqrt(trace(crlb(1:3,1:3))); rmse_crlb_vel = sqrt(trace(crlb(4:6,4:6)));

关键指标是RMSE与CRLB的比值，这个比值越接近1说明算法越接近理论最优。脱离CRLB谈定位误差都是耍流氓。

5. 结果对比：TSWLS与改进方法的核心差距

仿真跑完，数据会说话。我分别在小噪声、中噪声、大噪声三个档位下做了500次蒙特卡洛仿真，下面是核心结论。

5.1 位置与速度估计的RMSE对比

小噪声场景：σ_t = 10ns，σ_f = 0.1Hz。这时候TSWLS的表现非常亮眼，位置RMSE大概只有CRLB的1.3倍左右，速度RMSE大约CRLB的1.5倍。CWLS和SDP在小噪声下的优势不明显，RMSE都在CRLB的1.1倍上下，牛顿迭代细化后更是基本贴着CRLB走。

中噪声场景：σ_t = 100ns，σ_f = 1Hz。这是TSWLS开始挣扎的区域。位置RMSE升到了CRLB的2.5倍左右，速度RMSE更惨，差不多CRLB的4倍。CWLS还能保持在1.5倍以内，SDP和牛顿迭代依然贴着CRLB。这说明中噪声下TSWLS引入的平方项偏差已经不可忽视了，但CWLS通过约束条件把这些信息保留住，所以没有出现明显的性能滑坡。

大噪声场景：σ_t = 1μs，σ_f = 10Hz。TSWLS彻底放飞自我，位置RMSE是CRLB的几十倍甚至上百倍，估计出的目标位置经常性落在完全错误的方向上，误差分布远不是高斯分布，而是重尾分布。CWLS性能也开始劣化，大约CRLB的4倍左右，但至少还能看出目标大概在哪。SDP依然稳，RMSE保持在CRLB的1.5倍以内，但代价是计算时间快接近一秒了。

5.2 门限效应：低信噪比下的表现差异

把噪声σ_t从10ns逐步加大到1μs，按对数坐标画出RMSE曲线，能非常清晰地看到经典的门限效应。TSWLS在σ_t = 300ns附近出现拐点，之前RMSE缓慢上升，之后突然加速偏离CRLB，曲线斜率明显变陡。CWLS的门限点比TSWLS晚了将近一个数量级，在σ_t = 800ns附近才开始偏离。

SDP和牛顿迭代的方法基本没有出现明显的门限效应，在整个噪声区间内RMSE都紧贴着CRLB。这说明它们对噪声的鲁棒性本质上来源于利用了更多约束信息，而不是简单的闭式解公式。

实际做目标检测跟踪时，门限效应意味着"有时准有时完全不准"，对系统设计来说是致命的。在项目里，如果目标距离和信噪比波动范围大，只靠TSWLS做定位会经常出现航迹中断或跳变，这时候改进方法的优势就体现出来了。

5.3 计算复杂度与工程可用性

精度之外，计算量是工程落地的硬指标。我分别在MATLAB里用tic/toc粗略测了单次定位的平均耗时，结果如下表：

可以看到TSWLS快得离谱，这也是它至今没有被淘汰的原因。很多工程场景对精度要求没那么苛刻，但要求定位解算时间在毫秒级，比如导引头、弹载设备这类计算资源受限的平台，闭式解就是唯一选择。

TSWLS加牛顿迭代是我在项目里最常用的组合。和SDP相比精度差距小，速度却快了一个数量级以上。建议有实时性要求的工程直接用这个方案，精度不够就加一两轮迭代，稳得很。

6. 仿真中容易踩的坑和排查技巧

这部分的每个坑我都实打实踩过，写出来帮大家省时间。

6.1 矩阵病态与数值稳定性问题

TSWLS里的矩阵求逆是最容易翻车的地方。当观测站和目标距离远大于站间基线时，比如远距高速目标场景，伪线性方程矩阵的条件数可以达到10^10甚至更高。条件数越大，求逆误差越严重，最后估计值里全是数值噪声。

解决办法有几个。一是用伪逆pinv代替普通inv，在MATLAB里这两种算法的数值稳定性完全不同。二是对矩阵做预处理，比如列归一化能让条件数降低几个数量级。三是在求逆时加一个小的正则化项，等价于在AᵀWA上加一个扰动矩阵，但会引入轻微偏差。我在仿真里首选pinv，简单有效。

6.2 布站几何对可观测性的影响

观测站和目标之间的几何关系直接决定定位精度，这是常识，但初学者最容易忽略。TDOA定位要求目标不在观测站所在平面内，FDOA定位要求目标不是静止的且目标速度不在站目连线方向上。这两个条件不满足，对应维度的信息量为零，矩阵必然奇异。

我在设计仿真时测试过一组极端设置：所有观测站在同一高度且目标也在同一高度，这时候TDOA的所有测量都在一个水平面内，没有任何垂直方向的信息，z方向位置完全不可观。仿真跑出来z误差巨大，位置RMSE爆炸。如果你也遇到某个维度误差特别大的情况，先检查布站几何，别急着改算法。

另外，参考站的选择也有门道。参考站放在整个布站阵列的中心位置时，矩阵条件数通常更小；放在边缘会导致部分站距差测量值相关性增大，影响解算稳定性。仿真里可以对比不同参考站的RMSE，实测差异可能有两到三倍。

6.3 FDOA模糊度问题与去模糊处理

FDOA测量来自多普勒频率差，如果接收机是相参体制，频率测量可能存在周期模糊，这会导致FDOA测量值有整周期偏差。在仿真里我遇到过FDOA模糊导致目标速度完全错乱的情况。

处理方式是先做粗定位得到目标位置的粗略估计，然后根据粗定位结果推导出FDOA的整周期数，把模糊解去除，再用去模糊后的精确FDOA做一次精定位。仿真中如果要模拟这个环节，可以在测量生成阶段人为加上整周期偏差，验证去模糊的算法逻辑是否闭环。

6.4 调参经验：CRLB先行，逐级加噪声

最后分享一个我的标准调试流程。拿到一个新的定位算法，先跑一遍CRLB作为基准，确认理论下界和仿真场景匹配。然后用极小噪声跑一遍算法，检验闭式解在小噪声下的性能是否接近CRLB，这一步通过说明算法主体逻辑没有问题。最后逐级加大噪声，观察RMSE曲线偏离CRLB的拐点，这个拐点对应的噪声水平就是该算法的适用边界。

这个方法能帮你在五分钟内判断一个算法能不能用、能用在哪里。比我当年一上来就跑大噪声仿真，结果不断怀疑人生要高效得多。

TDOA/FDOA联合定位的算法选型，本质是在精度、实时性和鲁棒性之间做权衡。TSWLS作为经典闭式解，在小噪声和几何条件好的场景下依然是最优选择，又轻又快又准；而真正的实战环境中噪声不会那么温柔，改进方法尤其是TSWLS初值加牛顿迭代的组合，是精度和效率平衡得最好的方案。希望这份仿真复盘能帮你少走些弯路。