TDOA/FDOA联合定位仿真：TSWLS算法改进与CRLB性能对比

先说点背景。这个项目是我最近用MATLAB完整跑下来的一套TDOA/FDOA联合定位仿真分析，核心对象是经典的TSWLS两步加权最小二乘算法，以及我用约束修正加最大似然牛顿精化组合出来的改进方法。场景是无源定位里很典型的一种：目标辐射源运动，地面多站接收信号，一边测到达时间差，一边测多普勒频率差，然后联合估计出目标的位置和速度。这个题目看起来不算冷门，但真正把两种算法放在同一个仿真框架里、从建模到蒙特卡洛对比、再到噪声门限和几何因子影响全部走一遍的参考材料其实不多，所以我把自己的仿真过程和踩过的坑整理出来，给做无源定位、分布式探测或者传感器网络定位的同学做个参考。

先说结论放在前面：低噪声下TSWLS并没有很多人想得那么弱，结果和CRLB贴得很好；但噪声一旦上来，TSWLS会出现明显的门限效应，位置估计突然崩掉。改进方法并不复杂，就是用约束把辅助变量的物理意义重新压回去，再做几轮牛顿精化，门限大概能往后推一个量级。这篇文章不堆公式，重点讲清楚每一步为什么这么做，以及仿真里最容易出问题的地方。

1. 联合定位问题建模与仿真场景设计

1.1 为什么非要把TDOA和FDOA绑在一起

先说一个特别直观的类比。TDOA测量的本质是“距离差”，对目标位置的约束很强，但对速度的约束很弱。想象你在一个广场上，几个人听到同一个声音的时间先后不同，你能比较准确地判断声源在哪个方向、大概多远，但声源是静止还是移动、移动多快，仅凭到达时刻几乎给不出答案。FDOA正好反过来，它测的是“距离差变化率”，本质是目标相对各站的径向速度差，对速度很敏感，但如果目标位置本身不确定，光有速度信息也很难把位置收敛出来。

所以更合理的做法是把两类观测量塞进同一个估计问题，一次解出位置和速度。在工程上这也不是为了炫技。很多分布式探测系统本来就能同时给出时延和多普勒频移，前端做一次互相关和FFT就能拿到两组观测量。与其分开解算再把结果融合，不如直接联合估计，信息利用更充分，低速目标因为多普勒差太小单独用FDOA解不动，但联合TDOA之后位置信息能帮速度估计“撑住”。

我这个仿真里设置的是5个静止观测站接收移动辐射源信号。之所以选5站而不是最少的4站，是因为对象是三维位置加三维速度，联合估计的自由度很高，TSWLS辅助变量甚至有8个，属于[s,v,r1,rdot]的组合，站太少矩阵条件数会很难看，误差放大明显。5站在仿真里有一个合理的冗余度，解算稳定，也能更方便地观察GDOP对结果的影响。

1.2 观测方程、坐标约定与量纲统一

先约定符号，仿真里我用站心直角坐标系，位置统一用千米，速度统一用千米每秒。这个选择不是因为什么理论依据，纯粹是工程习惯。量纲问题必须在一开始就定死，不然后面所有噪声协方差矩阵和CRLB对比全是乱的。

目标未知向量是theta = [s; v]，其中s = [x,y,z]^T是目标位置，v = [vx,vy,vz]^T是目标速度。第i个观测站的位置是S_i，速度是V_i，仿真里所有观测站速度设为零向量，代表地面固定站。

TDOA观测量对应的是距离差：

r_i1 = ||s - S_i|| - ||s - S_1|| + epsilon_i

其中S_1是参考站，epsilon_i是距离差测量噪声，方差为sigma_t^2。FDOA观测量对应的是距离差变化率：

rdot_i1 = (v-V_i)^T(s-S_i)/||s-S_i|| - (v-V_1)^T(s-S_1)/||s-S_1|| + eta_i

eta_i是距离差变化率测量噪声，方差为sigma_dot^2。注意这里FDOA不再用时延或频差表示，而是换算成等效的距离差变化率，这样和TDOA的量纲体系一致，仿真代码里处理起来简单很多。雷达/通信专业的同学应该熟悉这个换算关系，多普勒频移f_d和目标径向距离变化率rdot之间就是rdot = -f_d * c / f_c的关系，f_c是载频。这个换算看起来简单，但算错符号会直接导致估计速度反号，我在下面的调试章节会再提一次。

有了观测方程之后，性能下界CRLB就可以算了。CRLB的意义是给所有无偏估计算法划了一条下限，算法再好也跳不过去。仿真里的做法是对观测函数h(theta)做数值雅可比，再用测量噪声协方差加权：

FIM = J^T * Q^{-1} * J

CRLB = diag(inv(FIM))

其中Q是包含TDOA和FDOA噪声的联合协方差矩阵。仿真里我直接对位置和速度各取前三个对角元素作为CRLB基准。

1.3 布站几何与GDOP初检

布站这件事容易被新手忽略，但仿真里它比算法本身更能决定结果。GDOP可以简单理解成“几何对误差的放大倍数”。观测站围住目标时，各个方向的观测量都能照到，GDOP小；观测站排成一条直线，某个方向的几何约束几乎为零，GDOP巨大，任何算法都救不回来。

这个仿真里的站址我做了稍微不规则的布站，避免太对称导致某些维度退化。参考站和其余4站坐标如下：

目标真值设置为s_true = (12, 8, 1) km，速度真值v_true = (0.3, -0.1, 0.02) km/s。站点把目标基本围在中心区域，GDOP不大，这样后面算法对比才公平。仿真里我还会专门做一个直线布站的反例，放在几何影响那一节，效果对比会非常明显。

提示：仿真第一步不要急着写估计器，先把布站坐标画出来，看看目标是不是在站群“肚子”里。这一步省掉，之后所有算法对比都可能被几何退化带偏。

2. TSWLS两步加权最小二乘算法实现

2.1 两步法的核心设计：辅助变量

TSWLS的原理一句话就能概括：定位方程里有范数，是非线性的，直接最小二乘不好解，那我就引入辅助变量，把非线性方程硬掰成关于辅助变量的伪线性方程。

以TDOA为例。定义r1 = ||s - S_1||为参考站到目标的真实距离，这是一个未知数。对第i个观测站的距离差r_i1，两边平方之后经过代数整理，可以得到一个近似线性方程：

2*(S_i - S_1)^T * s + 2*r_i1*r1 = ||S_i||^2 - ||S_1||^2 - r_i1^2

这个方程里，未知量是s和r1，如果再把FDOA加进来，还需要同时引入目标速度v和参考站距离差变化率rdot1。FDOA部分的线性化思路完全一样，把距离差方程对时间求导，整理成[s; v; r1; rdot1]的线性形式。最终可以统一写成：

h = G * theta_a + e

其中theta_a = [s; v; r1; rdot1]就是辅助变量向量，G是系数矩阵，h是测量构造向量，e是剩余误差。

必须强调，这个“线性化”是伪的。因为方程里出现了r_i1^2这种测量噪声平方项，所以严格来说e并不是零均值高斯噪声，而是一个被噪声平方项污染的非高斯残差。这个细节在低噪声下无所谓，TSWLS照样能逼近CRLB；但噪声大到一定程度，这个平方项开始主导误差，TSWLS的门限效应就出现了。

第二步的作用是利用辅助变量之间的物理约束做校正。辅助变量不是随便凑的，r1和s之间必须满足r1 = ||s - S_1||，rdot1和s、v之间必须满足rdot1 = (v-V_1)^T(s-S_1)/r1。TSWLS第二步把这些约束近似当作线性方程，再用加权最小二乘重新解一遍。问题恰恰出在“近似”二字上。

2.2 加权矩阵构建：共享参考站的坑

第一步加权最小二乘的解是：

theta_a_hat = (G^T * W * G)^{-1} * G^T * W * h

其中W = Q^{-1}，Q是测量噪声协方差。很多参考代码这里会直接写W = diag(1./sigma.^2)，这是错的。原因很简单，TDOA的M-1个观测量都共享参考站，r_21, r_31, ..., r_M1之间天然存在相关，协方差矩阵不是对角阵。

具体来说，如果每个距离差观测值的噪声标准差是sigma_t，那么TDOA部分的协方差矩阵是：

Q_t = sigma_t^2 * (eye(M-1) + ones(M-1))

主对角线上是各自噪声方差，非对角线上因为共同包含参考站噪声，协方差同样是sigma_t^2。FDOA部分同理，也是非对角阵。如果TDOA和FDOA噪声相互独立，联合协方差就是块对角。

这个坑我一开始也踩过。第一次仿真我图省事把W设成对角阵，结果低噪声下位置RMSE勉强还行，速度RMSE比CRLB差了好几倍，怎么调都下不去。后来重新按完整协方差公式构建Q，结果立刻贴住了CRLB。所以如果你们仿真结果永远怼不到CRLB，第一反应应该查这个协方差矩阵，而不是怀疑算法写错了。

2.3 TSWLS的MATLAB核心实现

给出我当时仿真的核心函数框架，关键矩阵的组装方式保留注释。代码不是能直接复制就跑的完整工程，但核心逻辑可以复现。

function [s_est, v_est] = tswls_tdoa_fdoa(S, r, rdot, fc) % S : 观测站位置，Mx3，第1行为参考站 % r : 距离差观测向量，(M-1)x1，相对参考站 % rdot : 距离差变化率观测向量，(M-1)x1，相对参考站 % fc : 载频，用于单位换算，非必需参数 M = size(S, 1); S1 = S(1, :); % 参考站坐标 % 第一步：组装 TDOA 线性方程 % 对每个非参考站 i=2..M % G_t 第 i-1 行 = [ 2*(S_i-S_1)^T, 0_3, 2*r(i-1), 0 ] % h_t 第 i-1 行 = ||S_i||^2 - ||S_1||^2 - r(i-1)^2 G_t = zeros(M-1, 8); h_t = zeros(M-1, 1); for i = 2:M row = i - 1; ds = S(i, :) - S1; G_t(row, 1:3) = 2 * ds; G_t(row, 7) = 2 * r(row); h_t(row) = S(i, :) * S(i, :)' - S1 * S1' - r(row)^2; end % 同理可以组装 FDOA 部分 G_f, h_f % 这里省略符号推导，组装逻辑和 TDOA 对称 % G_f 第 i-1 行 = [ 2*(V_i-V_1)^T, 2*(S_i-S_1)^T, 2*rdot(i-1), 2*r(i-1) ] % h_f 第 i-1 行 = 2*(S_i-S_1)^T*V_1 - ... % 甚至可以分块检查：先单独写G_t再写G_f，合并不容易出错 G = [G_t; G_f]; h = [h_t; h_f]; % 噪声协方差矩阵 Q（按共享参考站公式） Q = blkdiag( ... ); % 分别构造 Q_t, Q_f 再合并 W = inv(Q); % 第一步加权最小二乘 theta_a = (G' * W * G) \ (G' * W * h); % 第二步：约束修正，重新组装校正方程 % 核心是利用 r1_hat = sqrt(||s_hat-S1||^2) 构造新的 h2, G2 % 再用加权最小二乘得到最终结果 % ... 组装过程略 ... [s_est, v_est] = solve_correction(theta_a, S, W); end

第二步的组装代码比较长，思路是：把第一步估计出的s_hat代入几何约束，构造关于delta_s和delta_v的线性校正方程。由于第一步结果本身没有严格满足约束，校正方程里会多出一个残差项，加权最小二乘的核心作用就是把这项残差的影响压下去。

注意：TSWLS代码写完后先不要直接对比CRLB。用一组极低噪声数据自测，比如TDOA噪声设为1e-6 km，这时解应该和真值非常接近。如果这一步都对不上，公式或者符号方向肯定有问题，先排查矩阵组装。

3. 改进方法：约束修正与最大似然牛顿精化

3.1 TSWLS为什么会在高噪声下崩掉

TSWLS两步设计很巧妙，但它的软肋也很明显：第二步对约束的处理是“近似的”，它把r1^2 = ||s - S1||^2这个非线性约束当成近似线性化处理了。低噪声时线性化误差可以忽略，高噪声时第一步估计出的辅助变量已经偏离真实值很远，再用这个偏离的估计去做约束线性化，误差会被二次放大。

另一个更本质的问题是伪线性方程里的噪声平方项。回到TDOA整理后的方程，噪声会进入2*r_i1*r1和r_i1^2这些项。低噪声时平方项是二阶小量，可以当作高斯噪声混过去；高噪声时平方项不再是小量，估计算法就产生了系统性偏差，表现就是RMSE曲线突然翘起来，不再贴近CRLB。

所以我做的改进方向很明确，就两条：

1. 用更强的约束修正把解拉回到物理可行的流形上；
2. 用最大似然准则做几轮牛顿迭代，把伪线性化带来的偏差彻底清掉。

3.2 改进思路一：约束投影修正

约束投影的思路非常粗暴，但实测有效。TSWLS第一步得到s_hat、v_hat、r1_hat、rdot1_hat之后，这些量不满足物理约束。那我干脆用s_hat重新计算距离，得到投影后的r1_proj = ||s_hat - S1||，再算投影后的rdot1_proj = (v_hat-V1)^T(s_hat-S1)/r1_proj。

然后把投影后的辅助变量作为第二步的增强输入，重新组装约束方程再解一次。这个过程可以迭代两三轮，每一轮都把解往物理约束的流形上推一点。这个做法理论高度肯定比不上论文里的拉格朗日乘子法或半定松弛，但它有一个工程上非常宝贵的优点：稳定，几乎不会发散，而且实现简单。

我在实验时对比过，单纯加约束投影，高噪声下的位置RMSE就已经比原版TSWLS下降了一半以上。说明TSWLS在高噪声下的不少偏差确实来源于辅助变量脱离物理约束。

3.3 改进思路二：最大似然牛顿精化

约束投影修的是“辅助变量的一致性”，但它没有直接针对原观测模型做优化。真正要把估计精度推到CRLB附近，还得回到最大似然准则。

最大似然估计的本质就是最小化加权残差：

J(theta) = (h_meas - h_model(theta))^T * Q^{-1} * (h_meas - h_model(theta))

这里的h_meas是距离差和距离差变化率的直接测量值，h_model(theta)是给定目标状态theta后按真实非线性观测方程算出的预测值。这个代价函数是非线性的，无法直接求闭式解，需要用迭代方法。高斯牛顿迭代公式是：

theta_new = theta_old + (J^T * Q^{-1} * J)^{-1} * J^T * Q^{-1} * e

其中e是测量残差向量，J是h_model对theta的雅可比矩阵，可以在当前估计点计算数值雅可比。数值雅可比的实现特别简单：

function J = numerical_jacobian(h_model, theta, Q_inv) % h_model: 函数句柄，输入theta，输出预测观测向量 % theta : 当前估计值，6x1 % Q_inv : 测量噪声协方差逆矩阵，可选 n = length(theta); e = 1e-6; h0 = h_model(theta); J = zeros(length(h0), n); for k = 1:n thp = theta; thp(k) = thp(k) + e; thm = theta; thm(k) = thm(k) - e; J(:, k) = (h_model(thp) - h_model(thm)) / (2*e); end end

牛顿精化的初值非常关键。用纯牛顿法从头开始迭代，代价函数非凸，大概率会掉进局部极小。所以我的组合策略是：先用TSWLS得到粗估计，再用约束投影修正几轮，最后用高斯牛顿精化5到10次。这个流程下来的结果，在中高噪声下依然能贴着CRLB。

迭代时还有一个稳定技巧，更新量算出来后先检查范数，如果单步更新超过某个阈值就按比例缩小。本质上就是给牛顿法加了一个步长限制，避免一步迈到某个奇异区域里。代码里甚至可以退化成最陡下降法，稳健性还会更好。

4. 仿真对比与结果分析

4.1 蒙特卡洛仿真设置

整个仿真跑的是蒙特卡洛实验，单次估计不能说明问题，必须有统计意义。设置如下：

每次蒙特卡洛都重新生成一组独立的高斯噪声，叠加到真实测量值上，然后用两种算法分别估计。位置RMSE和速度RMSE分别按公式统计：

RMSE_pos = sqrt(mean(sum((s_hat - s_true).^2, 2)))

RMSE_vel = sqrt(mean(sum((v_hat - v_true).^2, 2)))

噪声等级我会在分析中逐步升高，观察门限效应出现的时机。

4.2 低噪声下的表现：两种算法都能贴住CRLB

先看低噪声结果，这是我最开始跑的对照实验：

这个结果验证了两个结论。第一，CRLB作为性能基准是可达到的，低噪声下两种算法与CRLB的差距都在两倍以内。第二，TSWLS不是不靠谱的算法，它在噪声条件好的时候完全可以作为工程基线方案。实际上很多实际系统里的测量精度本身就落在低噪声区间，直接用TSWLS也能交出不错的成绩。

这个阶段改进方法相对TSWLS的优势并不大，位置RMSE大约优化了三分之一。真正拉开差距要到高噪声区，后面这部分才是改进方法价值最大化的场景。

4.3 中高噪声下的门限效应：TSWLS先崩

固定FDOA噪声在低噪声水平，只提高TDOA噪声。某次典型扫描的结果如下：

可以明显看到，TSWLS在TDOA噪声从0.15km往0.6km走的过程中，RMSE从0.82km一路飙升到4.6km，彻底脱离CRLB。而改进方法始终在一个合理的倍数范围内，位置RMSE差不多是CRLB的1.4倍左右，没有出现崩溃式发散。

这就是我前面提到的门限效应。当噪声大到一定程度，TSWLS伪线性方程里的噪声平方项开始主导误差，第二步约束修正又不够强，于是估计结果“轰”地一下飞出去了。改进方法把这个问题推迟了差不多一个量级，因为在牛顿精化阶段，完全回到了原非线性观测模型，没有伪线性化噪声污染。

这个现象对实际使用有一个直接启发：如果你的系统测量噪声水平在门限附近徘徊，一定要慎用基础TSWLS，哪怕它在低噪声下看起来和CRLB贴合得完美无缺。

4.4 GDOP影响：布站不行，算法再好也白搭

我特意做了一组直线布站的反例：把5个站全部放在一条直线上，目标仍在中心附近，其余仿真条件完全不变。结果低噪声下TSWLS的位置RMSE直接到了0.35km，改进方法也只有0.12km，而同样的算法在五边形包围布站下分别只有0.018km和0.012km。

这个差距不是算法能弥补的。直线布站导致垂直于直线方向的几何约束几乎为零，CRLB本身就已经很大，所有算法在GDOP面前都是平等的。所以做算法对比前必须先检查GDOP。

仿真里检查GDOP可以用一个非常朴素的近似：对布站矩阵求条件数，或者对G^T*W*G求条件数。条件数巨大，说明几何已经退化到病态了。我通常会在估计算法跑之前先打印一行条件数，小于1e6才继续。

5. 常见问题与排查技巧

5.1 解出的位置跑到几百公里外

这个问题几乎每个跑TSWLS的人都遇到过，包括我自己。解算结果完全离谱，位置跑到几百公里甚至上千公里外，第一反应不要怀疑算法，先检查以下三件事：

一是量纲。观测站坐标用了km，距离差用了m，噪声协方差用了km，三者混在一起，矩阵条件数直接爆炸。把单位统一成同一套再跑。

二是加权矩阵是不是奇异。参考站共享导致协方差矩阵非对角，如果你用inv(diag(var))这种简化写法，不会奇异但会错误；如果你用完整协方差矩阵时某些观测站之间距离差完全一样，可能造成矩阵近似奇异，检查一下cond(Q)。

三是伪线性方程里h和G的组装方向。TDOA方程的符号非常容易写反，比如S_i-S_1写成S_1-S_i，结果就是估计位置跑到镜像点。自测方法很笨但有效：把TDOA噪声设为1e-8的超小值，看估计结果是不是和真值重合。如果不重合，基本能断定是矩阵组装或符号问题。

5.2 蒙特卡洛结果离CRLB总是差好几倍

如果低噪声下结果都离CRLB很远，大概率不是算法精度问题，而是你的CRLB计算或者噪声协方差模型有问题。我遇到过三种典型情况。

第一种，噪声协方差没有考虑共享参考站相关性。这个在前面2.2节已经详细说过，不再重复。

第二种，蒙特卡洛次数太少。少于100次时，RMSE统计波动非常大，有时候看起来比CRLB差很多其实是统计样本不够。建议至少跑300到500次。

第三种，CRLB计算用的雅可比矩阵是在真值处求的，而算法本身在有限样本下存在细微偏差，这个偏差在低噪声下应该可以忽略。如果你发现即使在500次蒙特卡洛下依然差3倍以上，建议先跑一个已知的简单场景，比如把目标放到所有站包围的中心，排除GDOP影响，再定位问题是几何、算法还是统计。

5.3 牛顿精化迭代发散

改进方法里牛顿法迭代发散也是常见问题。症状很典型：第一步TSWLS解还算正常，牛顿迭代两三轮后更新量突然变得极大，然后估计位置飞走。

排查方法有三个。

第一，检查初值。牛顿法对初值极其敏感，如果TSWLS结果的误差已经大到和噪声同量级，牛顿法很容易踩到代价函数的高曲率区域，一步更新就越界。这时候不要直接用原始TSWLS结果，中间先做约束投影修正。

第二，限制步长。更新量delta_theta算出来后，如果范数超过了当前估计位置的10%到20%，就按比例缩回去。这个在代码里就是两行的事，效果立竿见影。

第三，加阻尼。把高斯牛顿的更新公式改成带阻尼的Levenberg-Marquardt形式，本质上是在J^T*Q^{-1}*J对角线上加一个小的正则项。这个方法对病态雅可比非常有效，代价是收敛速度慢一点，但在仿真里无所谓，稳定优先。

5.4 调试检查表

把调试过程中积累的经验整理成一个检查表，按顺序排查基本能解决大部分仿真问题：

6. 工程落地与扩展建议

6.1 从仿真走向工程需要补什么

这套仿真模型在理想假设下效果不错，但从仿真到实际系统还有几条关键差距。首先是测量模型，仿真里直接给了距离差和距离差变化率的真实值加高斯噪声，工程上TDOA是从时延估计得到的，FDOA是从多普勒估计得到的，它们各自的误差和信号带宽、积累时间强相关，不是简单的高斯白噪声。做链路级仿真时应该引入前端的估计方差模型。

其次是观测站位置误差。仿真里观测站坐标是精确已知的，实际系统中站址本身有误差，而且不同观测站之间还有一个时间同步误差，这个误差在TDOA定位里是“致命伤”，经常直接淹没算法本身的性能差异。

最后是目标运动模型。仿真里目标是匀速直线运动，实际无源定位目标往往有机动，这时候即使定位算法本身没问题，也需要引入滤波框架，比如用EKF或UKF把每次定位结果和运动模型融合起来。

6.2 还能加什么仿真实验

这个仿真项目后续可以扩展的方向很多，如果继续做下去，我建议优先做这几个实验。

一个是布站优化。既然知道GDOP对定位性能影响这么大，可以把CRLB的迹作为优化目标，用遗传算法或梯度下降自动搜索最优站址。这个实验做出来，比单独调参有意义得多。

一个是多径和传播模型的影响。实际环境中目标信号经过地面反射、建筑物遮挡，TDOA/FDOA测量里会出现异常值。把传播模型仿真加进来，模拟多径环境下的测量误差，再测试这两种算法的稳健性，会暴露很多高斯噪声下看不到的问题。

第三个是和跟踪滤波联合。把TDOA/FDOA联合定位作为量测方程，放进卡尔曼滤波框架里做动目标跟踪，这在实际系统里更常见。定位算法给滤波提供量测，滤波反过来也能约束机动目标的位置速度变化。

6.3 一点个人体会

最后说点实在的。这个项目做下来，我个人最大的感受是：算法对比不是看谁在某个工况下更强，而是要看清楚各自适用范围。TSWLS胜在简单、计算量小、低噪声下性能优异，作为工程基线非常合适。改进方法虽然代码多了一些步骤，但换来的是高噪声下整整一个量级的性能裕量，在工程风险控制上非常值。

如果你也有类似仿真要做，我建议先不要纠结于复杂推导，老老实实把观测模型、噪声协方差和CRLB写对，然后跑一遍从低噪声到高噪声的门限扫描。当你亲眼看到RMSE曲线像鱼竿一样翘起来的那一刻，你对这些算法的理解会比读十篇论文都深。那根翘起的曲线，就是算法适用范围最直观的边界。