做定位的同学应该都有体会，TDOA（到达时间差）和FDOA（到达频率差）这两个词听着很熟，但真正要把两者揉进一个滤波器里，让无人机编队一边飞一边把辐射源的位置和速度持续估计出来，总是会遇到一堆“书上没写”的坑。今天这篇就围绕这样一个项目展开：多架无人机作为移动接收站，无源接收辐射源信号，提取TDOA与FDOA两类观测量，再用扩展卡尔曼滤波（EKF）递归估计辐射源的三维位置和速度，整套流程我直接用MATLAB实现了。
这套方案解决的工程问题很明确：在自身不发射电磁波的前提下，利用信号到达多个站的时间差，以及多普勒频移在多个站之间的差异，把非合作目标的运动状态“算”出来。对于雷达对抗、电子侦察、频谱监测、无人机集群协同感知这类场景，多无人机无源定位是很实用的技术路线。如果你正在做协同定位方向的研究或毕业设计，或者刚接触TDOA/FDOA、想找一份能直接跑起来迭代的MATLAB代码作为起点，这篇内容应该能帮你省下不少摸索的时间。
项目本身并不复杂，核心链条是：先构建接收站与目标的运动学模型，再建立TDOA/FDOA观测方程，然后让EKF在每一个滤波周期里完成“预测—更新”，最终输出目标的位置和速度估计。我把整个实现过程拆成四块：定位问题拆解、状态空间建模、MATLAB工程实现、参数整定与排错。每一部分都附上了我实际调试时踩过的坑，可以直接对照参考。
1. 问题拆解：多无人机无源定位到底在解决什么
1.1 先搞懂TDOA和FDOA在测什么
TDOA的物理含义非常直观：同一个辐射源信号，到达两个接收站的时间不一样，这个时间差就是TDOA。假设1号无人机比主站晚接收到100ns的信号，光速取3e8m/s，那么目标到1号无人机的距离，就比到主站的距离远30m。这个差值把目标约束在一条以主站和1号无人机为焦点的双曲线上，三维空间里就是一张双曲面。
如果有4架无人机，以其中1架作为主站，其余3架作为从站，就能形成3个TDOA观测值，对应3张双曲面。理论上这3张双曲面的交点，就是辐射源的三维位置。问题在于这个方程组是高度非线性的，直接解析求解很麻烦，而且测量噪声会让多条双曲面无法交于一点，所以需要滤波器或优化算法来估计。
FDOA的物理含义稍微绕一点。目标或接收站运动时，接收到的信号频率会发生多普勒偏移。不同无人机相对辐射源的径向速度不一样，所以同一辐射源信号在不同无人机上的多普勒偏移也不一样，这个频率偏移的差值就是FDOA。
FDOA的本质是“距离变化率差”。假设目标正在朝主站方向移动，主站收到的信号频率会偏高，而从站可能正“远离”目标，收到的频率偏低，两者一减，就得到FDOA。这个观测量和目标的运动速度直接挂钩，因此FDOA能提供目标速度的强约束。
一句话概括：TDOA管位置，FDOA管速度。如果只用TDOA，目标机动时滤波器会明显滞后，因为位置观测量本身不带速度信息，目标速度只能靠状态方程去“推”；加上FDOA之后，速度被直接观测到，跟踪能力和收敛速度都会明显改善。哪怕目标是静止的，FDOA也能提供额外的几何约束，因为无人机本身在运动，多普勒效应会让观测对位置变化更敏感。所以标题里“TDOA+FDOA”不是炫技，是实际需求。
1.2 为什么是EKF而不是直接解方程
拿到TDOA/FDOA观测值之后，其实有几条路可以选。最直接的是每一帧用高斯牛顿法去解非线性最小二乘，初值给得好时精度不错，缺点是不利用历史信息，每一帧都是独立估计，噪声抑制能力弱。另一条路是把TDOA方程做变量代换，变成伪线性方程组，用最小二乘一次求解，速度快、不需要初值，但在三维场景下容易病态，噪声会被放大。
我最终选择EKF，核心原因是它天然适合“递推估计运动状态”这个问题。EKF在每一次滤波周期里，用当前状态估计点对非线性观测方程做一阶泰勒展开，把非线性问题转换成近似线性滤波，既利用了历史轨迹信息，又通过协方差矩阵动态调整量测与预测的可信度。在中等非线性程度的定位场景里，EKF的精度和效率非常均衡，代码实现也不复杂，比直接解方程更适合作连续跟踪系统。
打个比方：EKF就像开车时用导航，先用速度和方向外推位置，看到路牌（观测量）后结合外推结果和路牌信息做修正。关键在于路牌和导航预测都得可信，导航才准。EKF做的事，就是通过协方差矩阵动态告诉系统“该信谁多一点”。
2. EKF状态空间建模与核心公式推导
2.1 状态量选6维还是7维
状态向量是滤波器的“内存”，决定了系统能估计什么。这个项目里最基本的需求是估计辐射源的三维位置和三维速度，所以状态向量选6维：
x = [rx, ry, rz, vx, vy, vz]^T
前三维是目标在大地坐标系下的位置，后三维是速度。过程模型采用匀速运动模型（CV模型），离散化后的状态转移矩阵和过程噪声协方差矩阵分别为：
F = [I3, dt·I3; 0, I3]
Q = q · [dt³/3·I3, dt²/2·I3; dt²/2·I3, dt·I3]
其中dt是滤波周期，q是过程噪声强度。q的物理含义是目标加速度扰动的方差估计，如果目标可能以a_max的加速度机动，q通常取a_max²。目标机动性越强，q要设得越大，否则滤波器会“跟不上”目标。
有些资料会把辐射源的载频偏差加进状态，做成7维或更高维度，这是系统误差在线估计的思路，适合发射频率不完全已知的场景。但第一个版本我建议先跑通6维，把定位跟踪的主链路搞清楚，再扩展频率偏差估计不迟。状态维度越高，能观性分析和矩阵求逆的压力都会上来，没必要一上来就堆维度。
2.2 观测方程和雅可比矩阵
假设有1个主站和M个从站，每个滤波周期可以得到M个TDOA和M个FDOA，观测向量维度是2M。TDOA观测方程：
Δτ_i = (‖r - r_i‖ - ‖r - r_0‖) / c
FDOA观测方程：
Δf_i = -(fc / c) · ( ((r - r_i)·(v - v_i)) / ‖r - r_i‖ - ((r - r_0)·(v - v_0)) / ‖r - r_0‖ )
其中r和v是目标的位置和速度，r_i和v_i是第i个从站的位置和速度，r_0和v_0是主站的位置和速度，fc是辐射源载频，c是光速。
EKF需要观测方程对状态向量的雅可比矩阵H。TDOA部分相对简单，对位置求导：
∂Δτ_i / ∂r = (1/c) · ( (r - r_i)/‖r - r_i‖ - (r - r_0)/‖r - r_0‖ )
这个式子的几何含义很直观：括号里是两个单位视线方向向量之差，它度量了目标位置变化对距离差的影响灵敏度。目标越靠近两个接收站的基线延长线，这个差向量越小，TDOA对位置变化的灵敏度就越差。
TDOA对速度的偏导为零，因为TDOA观测不显含速度。FDOA对位置和速度都有偏导，公式推导比较繁琐，我强烈建议在实际编码时先用数值差分求雅可比，跑通流程后再手推解析式替换。数值雅可比可以用中心差分实现，比如对状态第j个分量加一个微小扰动ε，然后用(h(x+εe_j) - h(x-εe_j)) / 2ε近似偏导数。中心差分比单侧差分的截断误差小一个量级，在EKF里足够用了。
如果非要手推解析雅可比，FDOA部分先定义单位视线向量u_i = (r - r_i)/‖r - r_i‖，径向速度v_rad,i = u_i^T (v - v_i)，那么FDOA对位置的偏导为：
∂Δf_i / ∂r = -(fc/c) · ( (v - v_i)^T (I - u_i u_i^T) / ‖r - r_i‖ - (v - v_0)^T (I - u_0 u_0^T) / ‖r - r_0‖ )
对速度的偏导为：
∂Δf_i / ∂v = -(fc/c) · ( u_i^T - u_0^T )
这两个式子写成代码时很容易把I - u_i u_i^T的方向弄反，所以我的习惯是先用数值雅可比验证解析式，两者误差在1e-6以内再放心用。调试成本远比自己对着公式盯半宿低。
2.3 EKF递推流程与初始化技巧
EKF的单步递推标准写法是几个矩阵运算，但每一步都值得说清楚。首先是状态预测：
x_pred = F · x P_pred = F · P · Fᵀ + Q
接着计算量测预测z_pred = h(x_pred)，并得到观测雅可比H。然后计算新息协方差S和卡尔曼增益K：
S = H · P_pred · Hᵀ + R K = P_pred · Hᵀ · S⁻¹
最后更新状态和协方差：
x_new = x_pred + K · (z_obs - z_pred) P_new = (I - K · H) · P_pred
初始化是整个滤波器最容易出事的地方。初值x0可以由第一帧TDOA粗定位获得，粗定位可以用网格搜索、高斯牛顿法或lsqnonlin实现，目的是把误差控制在几百米以内，而不是随便给个值让EKF自己拉回来。初始协方差P0要和粗定位误差量级匹配，位置项取(粗定位误差)²，速度项取(最大目标速度×0.3)²。P0设得过大，滤波前期会在真实值附近大幅震荡；设得过小，滤波器会过度相信初值，收敛速度变得很慢。
R矩阵的构造也直接影响滤波质量。TDOA测量误差和FDOA测量误差单位不同，数值差异可能达到十几个数量级，直接塞进R矩阵会让滤波器忽略数值小的量测。我的做法是把TDOA乘上光速c，转成等效距离差，把FDOA乘上波长λ，转成等效速度差，这样观测量的单位统一成米和米/秒，R矩阵对角线都在一个量级，数值稳定性会好很多。
3. MATLAB工程化实现与关键代码
3.1 仿真场景生成：让无人机和目标“飞”起来
我做的仿真场景是4架无人机在3000m高度绕圈飞行，1架作为主站，3架作为从站，相位间隔90度。之所以选圆形航迹，是为了让各站与目标之间的视线方向持续变化，TDOA和FDOA的观测几何信息更丰富。
% 基础参数 c = 3e8; % 光速 m/s fc = 300e6; % 载频 Hz，用于FDOA计算 dt = 0.1; % 滤波周期 s T = 20; % 仿真时长 s tVec = 0:dt:T; N = length(tVec); % 无人机编队：1主站 + 3从站，圆形轨迹 radius = 1500; % 飞行半径 m height = 3000; % 飞行高度 m omega = 0.2; % 绕圈角速度 rad/s uavPos = zeros(4, 3, N); uavVel = zeros(4, 3, N); for k = 1:4 phase0 = (k - 1) * pi / 2; % 相位间隔90度 for n = 1:N th = omega * tVec(n) + phase0; uavPos(k, :, n) = [radius * cos(th), radius * sin(th), height]; uavVel(k, :, n) = [-radius * omega * sin(th), ... radius * omega * cos(th), 0]; end end目标设置为近似匀速直线运动，从[2000, 1500, 0]出发，速度[30, 20, 0] m/s。为了更接近实际，每一步给速度加一点小扰动，模拟轻微机动。
% 目标真实轨迹 targetPos0 = [2000, 1500, 0]; targetVel0 = [30, 20, 0]; targetPos = zeros(3, N); targetVel = zeros(3, N); targetPos(:, 1) = targetPos0; targetVel(:, 1) = targetVel0; for n = 2:N targetVel(:, n) = targetVel(:, n-1) + 0.1 * randn(3, 1); targetPos(:, n) = targetPos(:, n-1) + targetVel(:, n-1) * dt; end这里要说明一点：目标轨迹生成时加的随机扰动，和EKF过程模型里的Q并不是一回事。EKF并不知道目标的真实机动规律，它只是通过Q来告诉滤波器“目标运动模型不太可信，要多依赖量测”。仿真中设置轻微机动，是为了验证Q取值的鲁棒性。
3.2 观测模型与加噪
观测函数h(x)是EKF的核心接口，输入当前时刻的6维状态，输出2M维的观测量。我在代码里做了单位转换，TDOA直接输出等效距离差，FDOA直接输出等效速度差。
function z = hfun(x, uavPos, uavVel) % x: 6维状态 [rx; ry; rz; vx; vy; vz] % 返回: [等效距离差; 等效速度差] c = 3e8; fc = 300e6; lambda = c / fc; M = size(uavPos, 1) - 1; % 从站数量 z = zeros(2 * M, 1); r = x(1:3); v = x(4:6); % 主站 = 第1架无人机 d0 = norm(r - uavPos(1, :)'); vrad0 = ((r - uavPos(1, :)')' * (v - uavVel(1, :)')) / d0; for i = 1:M % 第i个从站 di = norm(r - uavPos(i + 1, :)'); vradi = ((r - uavPos(i + 1, :)')' * (v - uavVel(i + 1, :)')) / di; z(i) = di - d0; % 等效距离差 = c * TDOA z(M + i) = -lambda * (vradi - vrad0); % 等效速度差 = lambda * FDOA end end注意一个关键工程点：量测生成和滤波器内部的h(x)必须完全一致。很多调试事故都是因为量测生成时用了正号、滤波器里却用了负号，结果新息永远带系统偏差，EKF怎么调都收敛不到真值。所以我在写代码时会把“真实量测生成”和“滤波器观测函数”抽成同一个函数，只传不同参数，彻底避免不一致。
加噪时，根据TDOA和FDOA的测量精度生成随机噪声：
sigmaTau = 10e-9; % TDOA标准差 10ns sigmaF = 0.5; % FDOA标准差 0.5Hz noiseZ = [ sigmaTau * c * randn(M, N); sigmaF * lambda * randn(M, N) ]; R = blkdiag((sigmaTau * c)^2 * eye(M), (sigmaF * lambda)^2 * eye(M));这里R矩阵虽然是按对角阵处理的，但实际TDOA观测是由多个从站相对于同一个主站得到的，观测噪声之间存在相关性，严格来说R的非对角项不为零。仿真初期可以先忽略，但要意识到这个近似会让滤波器的估计误差偏乐观。
3.3 EKF主循环代码
数值雅可比函数先实现好，用中心差分：
function H = numericalJacobian(fun, x) n = length(x); h = 1e-6; m = length(fun(x)); H = zeros(m, n); for j = 1:n xp = x; xm = x; xp(j) = xp(j) + h; xm(j) = xm(j) - h; H(:, j) = (fun(xp) - fun(xm)) / (2 * h); end end数值雅可比的好处是调试阶段不用手推公式，不管h(x)写得多复杂，只要能算出函数值，雅可比矩阵就自动出来了。它的缺点是比解析雅可比多调用2n次观测函数，在仿真场景下完全不是问题，实时运行时再换解析式优化。
EKF主循环：
% 状态转移矩阵 F = [eye(3), dt * eye(3); zeros(3), eye(3)]; % 过程噪声强度，根据目标最大机动加速度估计 q = 0.5; Q = q * [dt^3/3 * eye(3), dt^2/2 * eye(3); dt^2/2 * eye(3), dt * eye(3)]; % 初始状态：位置用粗定位结果，速度先给一个小偏差 x = [targetPos0 + [100, -50, 20]'; targetVel0 + [2, -1, 0]']; P = diag([100^2, 100^2, 30^2, 5^2, 5^2, 1^2]); % 初始协方差 estPos = zeros(3, N); estVel = zeros(3, N); for n = 2:N % 预测 x_pred = F * x; P_pred = F * P * F' + Q; % 计算当前时刻观测雅可比 H = numericalJacobian(@(xx) hfun(xx, uavPos(:, :, n), uavVel(:, :, n)), x_pred); % 量测预测与新息 z_pred = hfun(x_pred, uavPos(:, :, n), uavVel(:, :, n)); y = zObs(:, n) - z_pred; % 卡尔曼增益与更新 S = H * P_pred * H' + R; K = P_pred * H' / S; x = x_pred + K * y; P = (eye(6) - K * H) * P_pred; % 记录结果 estPos(:, n) = x(1:3); estVel(:, n) = x(4:6); end这里zObs需要在循环前准备，维度是(2M, N)，第n列就是当前时刻加噪后的TDOA/FDOA观测向量。
3.4 结果评估：RMSE与航迹可视化
滤波器跑完之后，最重要的就是评估它到底准不准。位置和速度的均方根误差（RMSE）是最直接的指标：
posErr = vecnorm(estPos - targetPos); rmsePos = sqrt(mean(posErr.^2)); velErr = vecnorm(estVel - targetVel); rmseVel = sqrt(mean(velErr.^2));只看RMSE还不够，我建议画出估计航迹与真实航迹的对比图，以及位置误差随时间的变化曲线。画出来之后能直观看到几个问题：如果误差曲线一开始有个大尖峰然后迅速回落，说明初值扰动被正确修正，滤波器收敛了；如果误差一直缓慢上升，多半是Q太小，过程模型过于自信；如果误差发散到离谱量级，先检查观测单位和h(x)的正负号。
4. 从发散到收敛：EKF参数整定与实战排错
4.1 EKF发散的三种典型原因及对策
我调试这个项目时遇到过几次发散，复盘下来基本上就是下面这张表里写的三类原因：
| 现象 | 常见原因 | 解决思路 |
|---|---|---|
| 协方差爆炸，状态跳到极大/极小值 | 观测单位不统一，R矩阵对角线尺度失衡 | 统一为等效距离差/等效速度差 |
| 滤波长期不收敛，误差停留在初始量级 | Q太小，或者P0设置不合理 | 调整Q，合理设置P0 |
| 误差收敛到错误值，与真值差一个系统偏移 | 初值远离真值，EKF线性化失效 | 先用粗定位提供初值 |
第一类最隐蔽。TDOA数值在10⁻⁸秒量级，FDOA可能在几十赫兹量级，如果直接放进观测向量，R矩阵对角线元素相差十几个数量级，滤波器实际上会完全忽略TDOA信息，定位结果自然一塌糊涂。解决办法就是我前面强调的，TDOA乘c，FDOA乘λ。
第二类比较容易理解。Q太小意味着滤波器认为目标严格匀速，但真实目标有一点机动，误差就会积累，滤波结果出现滞后。反之Q太大，滤波器的状态会跟着量测噪声剧烈抖动，估计轨迹毛刺很多。Q的初始值可以按目标可能的最大加速度平方来给，然后在一个量级范围内上下调整。
第三类在目标距离远、观测站几何不佳时特别容易出现。EKF本质上是局部线性化方法，初值离真值太远，线性化误差过大，滤波器可能收敛到错误的局部极小值。所以工程实践中很少让EKF自己从零开始找目标，都是先做一次粗定位，再把结果喂给EKF作为初值。
4.2 单位不匹配和量测噪声R怎么调
单位问题虽然看起来是小事，但我在这个项目里确实被它折磨过。当时直接把秒和赫兹塞进观测向量，R矩阵对角线是1e-16和1e1这种差距，滤波器数值上相当于只用了FDOA，位置估计完全拉不回来。后来把TDOA换成等效距离差、FDOA换成等效速度差之后，滤波器立刻正常了。
R矩阵的调整原则是：对角线元素反映各量测噪声的方差，数值越大表示对该量测越不信任。如果对某类量测的精度没把握，就把对应方差调大一点，让滤波器更依赖预测值。需要注意的是，R矩阵不能随意改动观测方程来迁就。观测方程和R必须同步变换，否则新息和新息协方差会系统性不匹配。
还有一个经验：协方差矩阵P和R都必须是半正定对称阵，如果数值计算中出现不对称，可以用(P + P')/2做一次对称化处理。MATLAB里直接做矩阵运算很少出现这个问题，但自己拼R矩阵时列序搞反就会报维度错误。
4.3 初值敏感怎么破：两步定位法
EKF虽然能递推收敛，但初值给得太离谱真的会要命。我给初值设过[0,0,0]，结果前几十个滤波周期都在原地打转，误差曲线像过山车一样。后来学乖了，第一步先用第一帧TDOA做一个粗定位，再把结果作为EKF的初始状态。
粗定位可以用MATLAB的lsqnonlin快速实现，目标函数就是TDOA残差平方和：
fun = @(r) tdoaModel(r, uavPos(:, :, 1), zObs(1:M, 1)); r0 = [0, 0, 1000]; % 初始搜索点，不需要很准 rEst0 = lsqnonlin(fun, r0);这里tdoaModel返回的是各个从站与主站的距离差，用当前状态r算出来，减去观测的距离差。lsqnonlin会自动调整r，使残差最小。速度初值如果一时没有好办法，可以先设为零向量，让EKF在后续滤波周期里逐步修正。两步定位法虽然增加了一点计算量，但能显著提升EKF稳健性，工程上非常划算。
4.4 无人机编队几何构型对定位精度的影响
编队几何的影响，是仿真里最容易被忽略但实际效果最明显的因素。如果所有无人机都在目标同一侧、近似排成一条线，那TDOA方程之间的相关性极强，等效于观测信息高度冗余，三维定位会出现“病态”，尤其是深度方向误差特别大。
我在试验中试过让4架无人机沿同一条直线编队飞行，结果位置估计在垂直于基线方向上的误差非常大，RMSE比圆形编队高了一倍不止。改成圆形编队后，各站从不同方位观察目标，视线方向差异大，TDOA/FDOA信息互补性更强，定位精度立刻提升。
几何构型的定量分析常用几何精度因子（GDOP）来描述。GDOP越小，说明该编队几何下量测误差对定位误差的放大作用越小。简单理解，无人机相对于目标越分散、基线越长，GDOP越好。反之所有无人机挤在一起，GDOP急剧恶化。所以设计仿真场景时，让无人机绕着目标转圈飞行，或者在不同高度、不同方位布站，是保证算法性能的基本功。
我自己把完整流程跑通之后最大的体会是：这套系统的代码量真不算大，真正复杂的是观测模型和参数整定。EKF公式到处都能找到，但“量测生成和h(x)保持一致”“单位变换只做一次但做彻底”“初值别太自信也别太小”这些细节，才是让仿真从“有结果”变成“能收敛”的分水岭。如果你拿到代码后发现滤波总是发散，先把这几个维度挨个排查一遍，八成问题都能解决。后续想继续深入，可以试着把载频偏差加进状态向量，或者把无人机自身的导航误差建模进观测方程，再进一步可以换成UKF应对更强的目标机动。希望这篇能帮你省下几个调试通宵。
