做无源定位仿真的人基本都会碰到TDOA/FDOA联合定位这个方向。TDOA给出距离差约束FDOA给出距离差变化率约束两者组合起来可以在同一套观测站下同时解目标的位置和速度。而TSWLSTwo-Step WLS两步加权最小二乘是这个领域最经典的闭式解之一很多论文拿它当基线然后再折腾各种改进方案。这篇内容就围绕“TDOA/FDOA联合定位仿真分析TSWLS与改进方法的较量”展开把我自己实际跑仿真时的建模思路、算法推导、代码流程以及踩过的坑完整写出来。适合正在做无源定位、电子侦察、传感器网络定位相关课题的同学也适合刚接触定位算法、想搞清楚“为什么闭式解有偏差”“改进方法到底改了什么”的人。看完之后你能直接搭起一套可复现的对比仿真框架并理解每种方法在什么条件下会赢、在什么条件下会崩。1. 定位问题的数学建模与仿真思路1.1 TDOA/FDOA联合定位到底在解什么假设你有M个接收站站址坐标已知目标是一个运动辐射源需要同时估计它的位置和速度。每个站收到同一个辐射源信号站与站之间存在时间差对应信号到达距离差这就是TDOA同时因为目标和站之间存在径向相对运动信号载频会有多普勒偏移不同站测到的频率差就是FDOA。用符号表示会清楚很多。目标位置记作 u[x,y,z]T目标速度记作 v[vx,vy,vz]T。接收站i的位置记作 si速度记作 \dot{s}i。目标到站i的真实距离为 ri||u-si||距离变化率为\dot{r}i (v - \dot{s}i)T (u - si) / ri以站1作为参考站TDOA测量得到的是距离差 r{i1} ri - r1FDOA测量得到的是距离差变化率 \dot{r}{i1} \dot{r}i - \dot{r}1。把所有可用的差测量量堆成一个向量维数是2(M-1)未知量是6个位置3个加速度3个所以理论上M≥4就能解算。这里我得先强调一个认知TDOA和FDOA不是“两套独立估计然后融合”的关系而是把位置和速度放进同一个参数向量里联立求解。这样做的优势是信息矩阵能够同时约束位置和速度维尤其是当目标运动速度较快、观测时间窗口较短时FDOA提供的多普勒梯度能明显改善速度维的可观测性。如果只做TDOA目标一旦变成运动状态位置估计会混入速度偏差只做FDOA位置维又容易退化。联合求解是定位运动目标的必经之路。1.2 克拉美罗界CRB先算清楚仿真的核心不只是把算法跑通而是要判断“这个算法的精度离理论极限还有多远”。所以CRBCramer-Rao Bound是必需的地板。它表示在无偏估计条件下任何估计方法能达到的最小方差。对于高斯噪声模型Fisher信息矩阵可以用观测函数对参数的真值偏导直接构造。把观测向量记作 z h(θ) n其中 θ [uT, vT]Th(θ)是理想无噪测量映射n是零均值高斯噪声。CRB的计算需要对每个测量量求真实偏导而不是用伪线性化后的矩阵。仿真里用数值差分就好简单而且不容易出错。得到CRB后后面所有RMSE对比都用CRB归一化这样能消除仿真单位不同带来的判断差异也能一眼看出算法在大噪声下的门限行为。先花时间把CRB路径打通后面所有对比实验才有参照系。这一步不能省否则三个算法之间差0.1米还是差0.1倍都会被主观感觉带偏。2. TSWLS算法的两步结构与推导细节2.1 第一步伪线性化与加权最小二乘TSWLS的核心思想是引入中间变量把非线性方程写成线性方程。注意这里不是对测量方程直接做泰勒展开而是通过数学变形构造出 h - Gθ 的形式让未知量一次性从非线性中剥离出来。从距离平方关系出发。ri2 (u-si)T(u-si)。对参考站同样成立。两式相减利用 ri r1 r{i1}可以得到只含 u 和 r1 的线性方程2(si - s1)T u 2 r{i1} r1 siTsi - s1Ts1 - r{i1}2这个式子可以整理成 G_i θ h_i 的形式θ增加一个维度 r1。类似的对FDOA部分把距离平方求导后两站相减可以得到包含 u、v、r1、\dot{r}1 的线性方程。合并所有站对应的行列得到整体系统G θ h e其中 e 是线性化误差它和测量噪声 n 之间近似满足 e ≈ BnB 是一个包含真实距离 r_i 和真实距离变化率 \dot{r}_i 的矩阵。于是可以取权重 W (B Q BT)-1得到第一步解θ1 (GT W G)-1 GT W h这里有个实操细节值得多说一句W里的B矩阵依赖真实目标距离而真实距离恰恰是我们想求的未知量。工程上标准的做法是先设 WI 求一次粗略解再用这个粗略解重构B然后重新加权迭代。我实际测试下来迭代2到3次就能收敛到比较稳定的结果再多迭代反而容易把噪声放大。2.2 第二步利用变量间约束修正第一步求出的 θ1 向量里各分量并不是相互独立的比如目标到参考站的距离 r1 和位置 u 必须满足 ||u-s1||r1速度 v 和距离变化率 \dot{r}1 也必须满足一致性关系。但第一步WLS完全忽略了这些约束所以估计精度不会太好尤其是大噪声场景下偏差明显。第二步做的事情是把 θ1 当作带噪的观测值利用约束关系做一次精化。设真实θ0满足约束 g(θ0)0对约束做一阶泰勒展开g(θ1) ≈ ∂g/∂θ · Δθ其中 Δθ θ1 - θ0。加上加权最小二乘的目标函数min (θ1 - θ)T P-1 (θ1 - θ)其中 P (GT W G)-1拉格朗日乘子法可以直接给出闭式修正δθ -P JT (J P JT)-1 g(θ1)然后更新θ2 θ1 δθ。这一步本质上是一个“投影操作”把第一步解的估计点拉回约束面附近。做完这一步位置和速度的精度通常会有明显提升在中等以上信噪比下能非常接近CRB。2.3 TSWLS的短板为什么还需要改进写仿真报告时很多人都会画一条“TSWLS对CRB”的曲线。结果往往是噪声小的时候两条线几乎重合但噪声大到一定程度后TSWLS的RMSE会突然偏离CRB出现一个上翘的“门限效应”。这个现象的本质在于伪线性化方程里存在测量平方项比如 r{i1}2噪声通过平方项进入线性方程后就不再是零均值而是留下一个正偏置。大噪声下这个偏置会把估计往远离真值的方向拉。同时第二步只考虑了部分约束关系额外的代数约束被丢弃了信息利用不充分。这些短板决定了改进方法的切入点一是对约束做更充分的处理二是直接回到非线性代价函数做迭代精化。下面两种改进方案分别对应这两条路线。3. 改进方法约束加权最小二乘与ML迭代精化3.1 方法一约束加权最小二乘CWLSCWLS的思路很直接第一步照旧第二步不再采用“先拉格朗日解析修正”的方式而是把约束直接作为硬约束放进优化模型里求最优解。数学模型变成这样min (θ1 - θ)T P-1 (θ1 - θ) s.t. g(θ) 0这本质上是一个带等式约束的二次规划问题。约束 g(θ) 是非线性的直接全局求解有难度仿真中一般有两种做法。第一种是固定点线性化在 θ1 附近对约束做一阶展开得到一个线性等式约束然后用KKT拉格朗日乘子法求闭式解。第二种是半定松弛SDR把二次等式约束写成矩阵形式利用半定规划松弛成凸问题再用现成的凸优化工具求全局解。我在仿真里推荐先用第一种。原因很简单闭式可解计算量小而且在小到中等噪声下已经能获得近似CRB的精度。半定松弛适合做理论验证能保证全局最优但计算复杂度高跑蒙特卡洛时会让循环慢到让人怀疑人生。CWLS相对TSWLS的改进在于它在修正估计时把约束关系作为一个整体参与优化而不是像TSWLS那样只对约束的一阶局部分量做投影。约束越多、站数越多这种全局优化的优势越明显。3.2 方法二ML-Newton迭代精化第二种改进路线更朴素——既然初始解已经比较接近真值为什么不直接对原始非线性测量方程做最大似然求解高斯噪声下ML准则等价于最小二乘代价函数为J(θ) (z - h(θ))T Q-1 (z - h(θ))用高斯牛顿法迭代θ(k1) θ(k) (HT Q-1 H)-1 HT Q-1 (z - h(θ(k)))其中 H 是 h(θ) 对 θ 的雅可比矩阵可以在当前估计点数值计算。这个方法的关键是初值不能离真值太远否则很容易收敛到局部极值或者发散。所以工程上标准的组合是先用TSWLS得到初值再迭代ML精化形成“闭式解 局部搜索”的管线。ML迭代的精度优势在于它优化的是最初的测量方程本身没有伪线性化引入的平方项偏差也不需要人为构造权重矩阵。在噪声较大、TSWLS已经开始出现门限效应的时候ML迭代依然能稳定地贴合CRB更久。但它的代价是每轮计算量大且依赖初值质量不能保证全局收敛。仿真中如果初始解偏差超过某个阈值迭代就可能飞走。3.3 三种方法的定位精度与计算量对比为了直观对比我把三种方法的特点列成一张表方便对比时心里有数。指标TSWLSCWLSML迭代闭式解是是/半定松弛可选否迭代计算复杂度最低中等偏高小噪声精度接近CRB逼近CRB逼近CRB大噪声鲁棒性出现门限效应门限略延后门限明显延后初值依赖无依赖θ1强烈依赖初值实现难度低中中从表格可以看出来TSWLS的价值是作为快速基线和初始值生成器CWLS是“闭式解里最能榨干约束信息”的方案ML迭代则是“精度天花板最高、风险也最高”的方案。真正跑仿真时三者不是互斥关系而是递进关系。4. 仿真环境构建与对比实验方案4.1 仿真参数与站址布设仿真参数直接影响结论的普适性这里我给出自己常用的参数设置供直接参考。接收站数量M5站址设置在地面附近避免共面退化。参考站设在原点其余四个站分布在三维空间的不同方位保证目标对每个站的视角有明显差异。目标位置设在约3km外速度设置为带有横向和纵向分量的运动矢量避免贴近纯径向运动导致FDOA信息退化。载频设置会影响FDOA噪声到速度噪声的换算。以L波段2GHz为例波长约0.15m1Hz的FDOA测量误差对应约0.15m/s的速度误差这个换算关系后续分析时很常用。这里再强调一次站址布设的重要性。仿真时最容易犯的错误是随意生成站址坐标结果站与站几乎落在一个平面上导致位置估计的垂直分量方差爆炸。跑完才发现数据全废。建议先做一次几何精度因子GDOP检查确保布站几何不病态再开始蒙特卡洛循环。4.2 蒙特卡洛仿真流程与代码骨架整套仿真流程可以按下面几步走生成真实目标参数和站址计算无噪测量值在无噪值上叠加高斯噪声分别用TSWLS、CWLS、ML迭代估计位置和速度统计RMSE和偏差最后与CRB做对比。下面这段MATLAB风格的伪代码可以直接套用修改% 参数设置 c 3e8; % 光速 fc 2e9; % 载频 lambda c / fc; % 波长 M 5; % 接收站数 % 站址坐标与速度站固定 S [0,0,0; 1000,500,0; -800,600,200; 200,-1000,50; 600,300,-150]; Sdot zeros(M,3); % 目标真实位置与速度 u_true [3000, 2000, 1000]; v_true [-100, 50, 20]; % 噪声标准差 sigma_tdoa 1e-9; % 1ns sigma_fdoa 1.0; % 1Hz % 无噪TDOA/FDOA r_true sqrt(sum((S - u_true).^2, 2)); r_dot_true sum((v_true - Sdot).*(S - u_true), 2) ./ r_true; tdoa_true (r_true(2:end) - r_true(1)) / c; fdoa_true (r_dot_true(2:end) - r_dot_true(1)) / lambda; % 叠加噪声 z [tdoa_true sigma_tdoa*randn(M-1,1); fdoa_true sigma_fdoa*randn(M-1,1)]; % TSWLS: theta1 inv(G*W*G)*G*W*h % CWLS: 在theta1附近带约束二次优化 % ML: 以TSWLS结果作为初值高斯牛顿迭代 % RMSE统计噪声协方差矩阵 Q 要按块对角构造TDOA部分因为参考站共用存在相关性不能简单按对角阵处理。这是很多第一次做联合定位仿真的人容易踩的坑会直接影响W矩阵构造和CRB计算的正确性。4.3 评价指标RMSE、偏差和CRB归一化三个指标必须一起看位置RMSE、速度RMSE、估计偏差Bias。只看RMSE会掩盖估计是否有偏而TSWLS在大噪声下的典型病症就是偏差急剧增大RMSE反而被偏差主导。把RMSE除以对应CRB的平方根再看这个比值随噪声标准差变化的上翘点就能很直观地判断每种算法的有效工作范围。在中等噪声区间三种方法的位置RMSE/CRB之比约在1到1.2之间属于正常。噪声增大到某个门限以上TSWLS的比值可能跳到3以上ML迭代可能还能维持在1.5以内。这个门限对应的噪声水平和伪线性化中噪声平方项开始主导误差的时刻基本一致。5. 结果分析与踩坑实录5.1 典型仿真结果谁更接近CRB在小噪声区间TSWLS、CWLS和ML迭代的RMSE几乎重合都贴着CRB走。这说明TSWLS在中高信噪比下已经没有多少提升空间所谓“改进方法”在此区间胜之不武。真正拉开差距的是大噪声区间。我实测的一组典型数据大致是这样的TDOA噪声从1ns增加到50ns、FDOA噪声1Hz固定时TSWLS的位置RMSE/CRB倍数在20ns左右开始上翘CWLS能撑到约35nsML迭代能撑到约45ns以上。速度维的差异更明显因为FDOA对应测量在TSWLS伪线性化的平方项里引入了更大的偏置项。换成数字更直观噪声较小时三者位置RMSE都在0.8米量级TSWLS在噪声加大后跑到3米以上CWLS约1.5米ML迭代约1.1米。速度RMSE的差距比位置更大TSWLS有时候会直接偏离到两倍以上。5.2 门限效应与初值敏感性改进方法不是全能的。CWLS在门限附近的表现其实也依赖第一步TSWLS解的质量但因为它对约束做了更全局的利用所以门限会稍微延后。ML迭代初值用TSWLS一旦TSWLS解偏差过大导致迭代发散ML结果可能比TSWLS还差需要加收敛保护和迭代上限。我的处理办法是在迭代中对每一步的增量做线搜索目标代价函数变差时自动减小步长同时设置最大迭代次数和残差阈值。这个细节看起来不起眼却能避免大量“不收敛到正确解但迭代没报错”的假结果。做蒙特卡洛循环之前先挑几组中等噪声和几组大噪声样本单独观察迭代过程确认收敛行为正常再全量跑否则几千次循环里混入一部分发散样本平均RMSE会被严重拉高。5.3 仿真中常见的几个坑先说权重矩阵迭代。TSWLS里的W矩阵依赖BB依赖真实距离。用估计值重构B时迭代次数并非越多越好。前文提到过我测试的结论是2到3次足够继续迭代会让噪声对权重矩阵的反向影响变大精度反而下降。再说CRB计算。有些初学者的做法是把伪线性方程里的G矩阵直接当作CRB的观测矩阵用这会高估CRB导致所有算法看起来都“太好了”。正确做法是回到原始测量 z h(θ)n对 h(θ) 求真实偏导。如果不想手推雅可比用数值差分完全可行但微元不能取得太大建议取目标尺度的万分之一量级。另一个容易忽略但非常现实的问题是FDOA噪声换算。仿真时习惯直接给FDOA加福斯特噪声比如1Hz但不同载频下相同频率误差对应的速度误差不同。2GHz下1Hz对应0.15m/s如果有人把单位搞混算出来的速度RMSE会差出一个量级对比结论就失去意义。最后提醒一下目标速度接近零时FDOA几乎不提供有效信息联合定位退化为纯TDOA问题此时TSWLS的第二步约束也会退化某些站的伪线性方程会接近线性相关矩阵求逆变成病态问题。跑仿真的时候最好先检查一下这个退化条件否则又会出现一批奇怪的异常点。5.4 仿真结论对工程实践的启示这次对比仿真给我的体会是不要迷信任何一种单一算法。真实设备场景中测量噪声往往会比仿真模型复杂得多包括站址误差、钟差、多径、目标加速度扰动等闭式解的鲁棒性优势不能轻易丢掉。工程上比较实用的做法是保持TSWLS作为快速初值和故障兜底在其基础上增加ML精化模块当噪声水平较低时直接输出TSWLS结果省算力当检测到目标残差较大或疑似动态机动时再启动迭代精化。CWLS则更适合作为中等算力约束下的精度升级选项。这套方法后续想扩展的话还可以往几个方向走把站址误差加入状态向量做联合估计或者扩展到多目标情形下的数据关联问题又或者把观测时间窗口切片做多帧融合。这些都是在同一套仿真骨架上改观测模型和状态维度复杂度可控值得按需求继续深入。 
长河君 · 2026/10/13 20:26:20 · 阅读 1.2k TDOA/FDOA联合定位仿真分析：TSWLS与改进方法对比
做无源定位仿真的人基本都会碰到TDOA/FDOA联合定位这个方向。TDOA给出距离差约束FDOA给出距离差变化率约束两者组合起来可以在同一套观测站下同时解目标的位置和速度。而TSWLSTwo-Step WLS两步加权最小二乘是这个领域最经典的闭式解之一很多论文拿它当基线然后再折腾各种改进方案。这篇内容就围绕“TDOA/FDOA联合定位仿真分析TSWLS与改进方法的较量”展开把我自己实际跑仿真时的建模思路、算法推导、代码流程以及踩过的坑完整写出来。适合正在做无源定位、电子侦察、传感器网络定位相关课题的同学也适合刚接触定位算法、想搞清楚“为什么闭式解有偏差”“改进方法到底改了什么”的人。看完之后你能直接搭起一套可复现的对比仿真框架并理解每种方法在什么条件下会赢、在什么条件下会崩。1. 定位问题的数学建模与仿真思路1.1 TDOA/FDOA联合定位到底在解什么假设你有M个接收站站址坐标已知目标是一个运动辐射源需要同时估计它的位置和速度。每个站收到同一个辐射源信号站与站之间存在时间差对应信号到达距离差这就是TDOA同时因为目标和站之间存在径向相对运动信号载频会有多普勒偏移不同站测到的频率差就是FDOA。用符号表示会清楚很多。目标位置记作 u[x,y,z]T目标速度记作 v[vx,vy,vz]T。接收站i的位置记作 si速度记作 \dot{s}i。目标到站i的真实距离为 ri||u-si||距离变化率为\dot{r}i (v - \dot{s}i)T (u - si) / ri以站1作为参考站TDOA测量得到的是距离差 r{i1} ri - r1FDOA测量得到的是距离差变化率 \dot{r}{i1} \dot{r}i - \dot{r}1。把所有可用的差测量量堆成一个向量维数是2(M-1)未知量是6个位置3个加速度3个所以理论上M≥4就能解算。这里我得先强调一个认知TDOA和FDOA不是“两套独立估计然后融合”的关系而是把位置和速度放进同一个参数向量里联立求解。这样做的优势是信息矩阵能够同时约束位置和速度维尤其是当目标运动速度较快、观测时间窗口较短时FDOA提供的多普勒梯度能明显改善速度维的可观测性。如果只做TDOA目标一旦变成运动状态位置估计会混入速度偏差只做FDOA位置维又容易退化。联合求解是定位运动目标的必经之路。1.2 克拉美罗界CRB先算清楚仿真的核心不只是把算法跑通而是要判断“这个算法的精度离理论极限还有多远”。所以CRBCramer-Rao Bound是必需的地板。它表示在无偏估计条件下任何估计方法能达到的最小方差。对于高斯噪声模型Fisher信息矩阵可以用观测函数对参数的真值偏导直接构造。把观测向量记作 z h(θ) n其中 θ [uT, vT]Th(θ)是理想无噪测量映射n是零均值高斯噪声。CRB的计算需要对每个测量量求真实偏导而不是用伪线性化后的矩阵。仿真里用数值差分就好简单而且不容易出错。得到CRB后后面所有RMSE对比都用CRB归一化这样能消除仿真单位不同带来的判断差异也能一眼看出算法在大噪声下的门限行为。先花时间把CRB路径打通后面所有对比实验才有参照系。这一步不能省否则三个算法之间差0.1米还是差0.1倍都会被主观感觉带偏。2. TSWLS算法的两步结构与推导细节2.1 第一步伪线性化与加权最小二乘TSWLS的核心思想是引入中间变量把非线性方程写成线性方程。注意这里不是对测量方程直接做泰勒展开而是通过数学变形构造出 h - Gθ 的形式让未知量一次性从非线性中剥离出来。从距离平方关系出发。ri2 (u-si)T(u-si)。对参考站同样成立。两式相减利用 ri r1 r{i1}可以得到只含 u 和 r1 的线性方程2(si - s1)T u 2 r{i1} r1 siTsi - s1Ts1 - r{i1}2这个式子可以整理成 G_i θ h_i 的形式θ增加一个维度 r1。类似的对FDOA部分把距离平方求导后两站相减可以得到包含 u、v、r1、\dot{r}1 的线性方程。合并所有站对应的行列得到整体系统G θ h e其中 e 是线性化误差它和测量噪声 n 之间近似满足 e ≈ BnB 是一个包含真实距离 r_i 和真实距离变化率 \dot{r}_i 的矩阵。于是可以取权重 W (B Q BT)-1得到第一步解θ1 (GT W G)-1 GT W h这里有个实操细节值得多说一句W里的B矩阵依赖真实目标距离而真实距离恰恰是我们想求的未知量。工程上标准的做法是先设 WI 求一次粗略解再用这个粗略解重构B然后重新加权迭代。我实际测试下来迭代2到3次就能收敛到比较稳定的结果再多迭代反而容易把噪声放大。2.2 第二步利用变量间约束修正第一步求出的 θ1 向量里各分量并不是相互独立的比如目标到参考站的距离 r1 和位置 u 必须满足 ||u-s1||r1速度 v 和距离变化率 \dot{r}1 也必须满足一致性关系。但第一步WLS完全忽略了这些约束所以估计精度不会太好尤其是大噪声场景下偏差明显。第二步做的事情是把 θ1 当作带噪的观测值利用约束关系做一次精化。设真实θ0满足约束 g(θ0)0对约束做一阶泰勒展开g(θ1) ≈ ∂g/∂θ · Δθ其中 Δθ θ1 - θ0。加上加权最小二乘的目标函数min (θ1 - θ)T P-1 (θ1 - θ)其中 P (GT W G)-1拉格朗日乘子法可以直接给出闭式修正δθ -P JT (J P JT)-1 g(θ1)然后更新θ2 θ1 δθ。这一步本质上是一个“投影操作”把第一步解的估计点拉回约束面附近。做完这一步位置和速度的精度通常会有明显提升在中等以上信噪比下能非常接近CRB。2.3 TSWLS的短板为什么还需要改进写仿真报告时很多人都会画一条“TSWLS对CRB”的曲线。结果往往是噪声小的时候两条线几乎重合但噪声大到一定程度后TSWLS的RMSE会突然偏离CRB出现一个上翘的“门限效应”。这个现象的本质在于伪线性化方程里存在测量平方项比如 r{i1}2噪声通过平方项进入线性方程后就不再是零均值而是留下一个正偏置。大噪声下这个偏置会把估计往远离真值的方向拉。同时第二步只考虑了部分约束关系额外的代数约束被丢弃了信息利用不充分。这些短板决定了改进方法的切入点一是对约束做更充分的处理二是直接回到非线性代价函数做迭代精化。下面两种改进方案分别对应这两条路线。3. 改进方法约束加权最小二乘与ML迭代精化3.1 方法一约束加权最小二乘CWLSCWLS的思路很直接第一步照旧第二步不再采用“先拉格朗日解析修正”的方式而是把约束直接作为硬约束放进优化模型里求最优解。数学模型变成这样min (θ1 - θ)T P-1 (θ1 - θ) s.t. g(θ) 0这本质上是一个带等式约束的二次规划问题。约束 g(θ) 是非线性的直接全局求解有难度仿真中一般有两种做法。第一种是固定点线性化在 θ1 附近对约束做一阶展开得到一个线性等式约束然后用KKT拉格朗日乘子法求闭式解。第二种是半定松弛SDR把二次等式约束写成矩阵形式利用半定规划松弛成凸问题再用现成的凸优化工具求全局解。我在仿真里推荐先用第一种。原因很简单闭式可解计算量小而且在小到中等噪声下已经能获得近似CRB的精度。半定松弛适合做理论验证能保证全局最优但计算复杂度高跑蒙特卡洛时会让循环慢到让人怀疑人生。CWLS相对TSWLS的改进在于它在修正估计时把约束关系作为一个整体参与优化而不是像TSWLS那样只对约束的一阶局部分量做投影。约束越多、站数越多这种全局优化的优势越明显。3.2 方法二ML-Newton迭代精化第二种改进路线更朴素——既然初始解已经比较接近真值为什么不直接对原始非线性测量方程做最大似然求解高斯噪声下ML准则等价于最小二乘代价函数为J(θ) (z - h(θ))T Q-1 (z - h(θ))用高斯牛顿法迭代θ(k1) θ(k) (HT Q-1 H)-1 HT Q-1 (z - h(θ(k)))其中 H 是 h(θ) 对 θ 的雅可比矩阵可以在当前估计点数值计算。这个方法的关键是初值不能离真值太远否则很容易收敛到局部极值或者发散。所以工程上标准的组合是先用TSWLS得到初值再迭代ML精化形成“闭式解 局部搜索”的管线。ML迭代的精度优势在于它优化的是最初的测量方程本身没有伪线性化引入的平方项偏差也不需要人为构造权重矩阵。在噪声较大、TSWLS已经开始出现门限效应的时候ML迭代依然能稳定地贴合CRB更久。但它的代价是每轮计算量大且依赖初值质量不能保证全局收敛。仿真中如果初始解偏差超过某个阈值迭代就可能飞走。3.3 三种方法的定位精度与计算量对比为了直观对比我把三种方法的特点列成一张表方便对比时心里有数。指标TSWLSCWLSML迭代闭式解是是/半定松弛可选否迭代计算复杂度最低中等偏高小噪声精度接近CRB逼近CRB逼近CRB大噪声鲁棒性出现门限效应门限略延后门限明显延后初值依赖无依赖θ1强烈依赖初值实现难度低中中从表格可以看出来TSWLS的价值是作为快速基线和初始值生成器CWLS是“闭式解里最能榨干约束信息”的方案ML迭代则是“精度天花板最高、风险也最高”的方案。真正跑仿真时三者不是互斥关系而是递进关系。4. 仿真环境构建与对比实验方案4.1 仿真参数与站址布设仿真参数直接影响结论的普适性这里我给出自己常用的参数设置供直接参考。接收站数量M5站址设置在地面附近避免共面退化。参考站设在原点其余四个站分布在三维空间的不同方位保证目标对每个站的视角有明显差异。目标位置设在约3km外速度设置为带有横向和纵向分量的运动矢量避免贴近纯径向运动导致FDOA信息退化。载频设置会影响FDOA噪声到速度噪声的换算。以L波段2GHz为例波长约0.15m1Hz的FDOA测量误差对应约0.15m/s的速度误差这个换算关系后续分析时很常用。这里再强调一次站址布设的重要性。仿真时最容易犯的错误是随意生成站址坐标结果站与站几乎落在一个平面上导致位置估计的垂直分量方差爆炸。跑完才发现数据全废。建议先做一次几何精度因子GDOP检查确保布站几何不病态再开始蒙特卡洛循环。4.2 蒙特卡洛仿真流程与代码骨架整套仿真流程可以按下面几步走生成真实目标参数和站址计算无噪测量值在无噪值上叠加高斯噪声分别用TSWLS、CWLS、ML迭代估计位置和速度统计RMSE和偏差最后与CRB做对比。下面这段MATLAB风格的伪代码可以直接套用修改% 参数设置 c 3e8; % 光速 fc 2e9; % 载频 lambda c / fc; % 波长 M 5; % 接收站数 % 站址坐标与速度站固定 S [0,0,0; 1000,500,0; -800,600,200; 200,-1000,50; 600,300,-150]; Sdot zeros(M,3); % 目标真实位置与速度 u_true [3000, 2000, 1000]; v_true [-100, 50, 20]; % 噪声标准差 sigma_tdoa 1e-9; % 1ns sigma_fdoa 1.0; % 1Hz % 无噪TDOA/FDOA r_true sqrt(sum((S - u_true).^2, 2)); r_dot_true sum((v_true - Sdot).*(S - u_true), 2) ./ r_true; tdoa_true (r_true(2:end) - r_true(1)) / c; fdoa_true (r_dot_true(2:end) - r_dot_true(1)) / lambda; % 叠加噪声 z [tdoa_true sigma_tdoa*randn(M-1,1); fdoa_true sigma_fdoa*randn(M-1,1)]; % TSWLS: theta1 inv(G*W*G)*G*W*h % CWLS: 在theta1附近带约束二次优化 % ML: 以TSWLS结果作为初值高斯牛顿迭代 % RMSE统计噪声协方差矩阵 Q 要按块对角构造TDOA部分因为参考站共用存在相关性不能简单按对角阵处理。这是很多第一次做联合定位仿真的人容易踩的坑会直接影响W矩阵构造和CRB计算的正确性。4.3 评价指标RMSE、偏差和CRB归一化三个指标必须一起看位置RMSE、速度RMSE、估计偏差Bias。只看RMSE会掩盖估计是否有偏而TSWLS在大噪声下的典型病症就是偏差急剧增大RMSE反而被偏差主导。把RMSE除以对应CRB的平方根再看这个比值随噪声标准差变化的上翘点就能很直观地判断每种算法的有效工作范围。在中等噪声区间三种方法的位置RMSE/CRB之比约在1到1.2之间属于正常。噪声增大到某个门限以上TSWLS的比值可能跳到3以上ML迭代可能还能维持在1.5以内。这个门限对应的噪声水平和伪线性化中噪声平方项开始主导误差的时刻基本一致。5. 结果分析与踩坑实录5.1 典型仿真结果谁更接近CRB在小噪声区间TSWLS、CWLS和ML迭代的RMSE几乎重合都贴着CRB走。这说明TSWLS在中高信噪比下已经没有多少提升空间所谓“改进方法”在此区间胜之不武。真正拉开差距的是大噪声区间。我实测的一组典型数据大致是这样的TDOA噪声从1ns增加到50ns、FDOA噪声1Hz固定时TSWLS的位置RMSE/CRB倍数在20ns左右开始上翘CWLS能撑到约35nsML迭代能撑到约45ns以上。速度维的差异更明显因为FDOA对应测量在TSWLS伪线性化的平方项里引入了更大的偏置项。换成数字更直观噪声较小时三者位置RMSE都在0.8米量级TSWLS在噪声加大后跑到3米以上CWLS约1.5米ML迭代约1.1米。速度RMSE的差距比位置更大TSWLS有时候会直接偏离到两倍以上。5.2 门限效应与初值敏感性改进方法不是全能的。CWLS在门限附近的表现其实也依赖第一步TSWLS解的质量但因为它对约束做了更全局的利用所以门限会稍微延后。ML迭代初值用TSWLS一旦TSWLS解偏差过大导致迭代发散ML结果可能比TSWLS还差需要加收敛保护和迭代上限。我的处理办法是在迭代中对每一步的增量做线搜索目标代价函数变差时自动减小步长同时设置最大迭代次数和残差阈值。这个细节看起来不起眼却能避免大量“不收敛到正确解但迭代没报错”的假结果。做蒙特卡洛循环之前先挑几组中等噪声和几组大噪声样本单独观察迭代过程确认收敛行为正常再全量跑否则几千次循环里混入一部分发散样本平均RMSE会被严重拉高。5.3 仿真中常见的几个坑先说权重矩阵迭代。TSWLS里的W矩阵依赖BB依赖真实距离。用估计值重构B时迭代次数并非越多越好。前文提到过我测试的结论是2到3次足够继续迭代会让噪声对权重矩阵的反向影响变大精度反而下降。再说CRB计算。有些初学者的做法是把伪线性方程里的G矩阵直接当作CRB的观测矩阵用这会高估CRB导致所有算法看起来都“太好了”。正确做法是回到原始测量 z h(θ)n对 h(θ) 求真实偏导。如果不想手推雅可比用数值差分完全可行但微元不能取得太大建议取目标尺度的万分之一量级。另一个容易忽略但非常现实的问题是FDOA噪声换算。仿真时习惯直接给FDOA加福斯特噪声比如1Hz但不同载频下相同频率误差对应的速度误差不同。2GHz下1Hz对应0.15m/s如果有人把单位搞混算出来的速度RMSE会差出一个量级对比结论就失去意义。最后提醒一下目标速度接近零时FDOA几乎不提供有效信息联合定位退化为纯TDOA问题此时TSWLS的第二步约束也会退化某些站的伪线性方程会接近线性相关矩阵求逆变成病态问题。跑仿真的时候最好先检查一下这个退化条件否则又会出现一批奇怪的异常点。5.4 仿真结论对工程实践的启示这次对比仿真给我的体会是不要迷信任何一种单一算法。真实设备场景中测量噪声往往会比仿真模型复杂得多包括站址误差、钟差、多径、目标加速度扰动等闭式解的鲁棒性优势不能轻易丢掉。工程上比较实用的做法是保持TSWLS作为快速初值和故障兜底在其基础上增加ML精化模块当噪声水平较低时直接输出TSWLS结果省算力当检测到目标残差较大或疑似动态机动时再启动迭代精化。CWLS则更适合作为中等算力约束下的精度升级选项。这套方法后续想扩展的话还可以往几个方向走把站址误差加入状态向量做联合估计或者扩展到多目标情形下的数据关联问题又或者把观测时间窗口切片做多帧融合。这些都是在同一套仿真骨架上改观测模型和状态维度复杂度可控值得按需求继续深入。