写这个项目之前我先描述一个场景你手上有4架无人机在空中巡逻目标是定位另一架正在飞行的未知无人机。对方不配合也不发应答信号你唯一能收到的是它自己辐射出来的遥控信号和图传信号。把这些信号分别接入4架无人机的接收机你可以测出信号到达不同接收机的时间差TDOA以及由于目标和接收机彼此运动造成的到达频率差FDOA。把这两类测量丢进扩展卡尔曼滤波器EKF就能在MATLAB里递推估计出目标的位置和速度。这是多无人机无源定位里一条非常经典、也非常成熟的技术路线民用安防监测、低空交通管理、应急搜救定位里都有它的实际应用场景。本文按“建模—推导—代码—实验—调参”的顺序完整展开先把TDOA/FDOA的物理含义和观测方程讲清楚再推导EKF必须用的雅可比矩阵接着给出可以直接组合运行的MATLAB核心代码最后聊精度评估和实操中踩过的坑。内容适合正在做无源定位方向毕设、研一刚进项目组被要求快速上手EKF、或者从纯仿真向工程落地过渡的同学参考。1. TDOA和FDOA究竟给滤波方程“喂”了什么1.1 TDOA一组距离差曲线决定目标位置TDOA的物理过程很简单目标无人机发出一帧信号由于目标到各接收站的距离不同信号到达各站的时间有先后。把任意一个接收站与参考站的到达时刻相减得到时间差乘以光速就是距离差。先说几何意义如果只有两个接收站和一个参考站目标到这两站的距离差是定值在三维空间里对应一个双曲面多组接收站组合会形成多个双曲面它们的交点就是目标位置。这就是经典的“双曲线定位”也是无源定位最核心的几何基础。但在实际系统里要注意一点TDOA测量值并不是直接从时钟上读出来的而是通过对两路接收信号做相关峰估计得到的。所以TDOA的测量精度跟信号带宽、信噪比、积累时间强相关。带宽越宽相关峰越尖锐时间测量误差越小信噪比越高、积累时间越长抗噪声能力越强。这也是为什么很多无源定位系统倾向于做宽带信号接收处理。在仿真项目里你不需要去实现相关峰估计直接把TDOA看作带高斯噪声的测量值即可。但心里要清楚这个噪声的方差在真实系统里不是随便填的它由前端的信号处理能力决定。1.2 FDOA目标速度的“影子测量”FDOA的本质是多普勒频差。目标无人机以某个速度飞行时它辐射的载频会被多普勒效应改变而改变量取决于目标相对于接收站的径向速度。不同接收站看到的径向速度不同因此测到的多普勒频率也不同两两相减就得到频率差。频率差和目标相对接收机的距离变化率之差直接成正比。而距离变化率恰好等于目标速度在目标-接收机视线方向上的投影。于是FDOA观测携带了目标速度在多个视线方向上的投影信息多个投影联合起来就可以重构目标速度向量。这个信息非常特殊它在观测量里直接包含速度不需要靠位置差分去“猜”速度这对滤波器的收敛速度和稳定性非常有帮助。1.3 为什么不单用其中一个如果只使用TDOA观测方程本质上是位置的函数和目标速度几乎没有直接关系。那速度信息从哪来只能靠滤波器预测模型里的过程噪声去“带动”速度项这会导致速度估计像盲人摸象收敛慢且容易漂。如果只使用FDOA问题恰好反过来频率差虽然强烈依赖目标速度但也依赖目标相对接收机的几何位置如果位置先验不准速度信息同样不可靠。更关键的是位置和速度在运动学方程里天然耦合——位置的变化就是速度的积分速度的变化会产生位置变化。把TDOA和FDOA拼进同一个观测向量让滤波器每一帧同时利用位置信息和速度信息整个系统的可观测性会显著增强收敛速度和稳态精度都能明显改善。这也是标题里“结合TDOA和FDOA”的核心原因两者不是简单的信息叠加而是互补。2. 为什么我用EKF直解而不是一上来就转无迹或粒子滤波2.1 非线性问题到底出在哪如果目标运动模型取匀速直线CV那么状态转移方程是线性的这部分没有任何难度。难点在于观测方程——TDOA观测里有范数运算FDOA观测里有视线方向单位向量和速度的点乘整个观测方程关于状态是非线性的。标准的卡尔曼滤波建立在“线性高斯”假设上直接用线性卡尔曼处理不了这种非线性观测。EKF的核心思想就是在当前估计点附近对观测方程做一阶泰勒展开用雅可比矩阵代替线性系统的观测矩阵。说人话就是反正非线性那就把它局域近似成线性再用卡尔曼框架去递推。2.2 EKF、UKF、粒子滤波怎么挑UKF无迹卡尔曼滤波不需要算雅可比用一组Sigma点去逼近状态分布经过非线性变换后的均值和协方差对强非线性的处理能力更好。粒子滤波更极端完全放弃高斯假设用一堆随机粒子去近似任意分布。但在这个项目里我的选择很明确先用EKF。原因是多方面的。第一TDOA/FDOA观测方程的非线性程度不算夸张在滤波器估计状态较好时一阶泰勒展开的截断误差完全可控。第二每一帧数据要处理多个接收站、多组观测计算量必须控制在中低量级EKF是开销最小的方案。第三也是我最看重的调试过程里雅可比矩阵能帮你逐项排查错误比如某个观测量的偏导写反了、维度对不上数值差分一验就能揪出来。换成UKF如果滤波发散你可能连问题出在哪都说不清。所以我的建议是先用EKF把系统整体跑通确认模型、数据、代码链路没问题再根据实际精度需求决定要不要换成UKF。反过来如果你一上来就上粒子滤波状态维度6维粒子数一取几千收敛慢、调试难很容易陷入“滤波结果一团糟但不知道是模型问题还是粒子数问题”的泥潭。2.3 什么时候必须升级滤波器EKF的短板在于强机动场景。比如目标做大过载转弯、突然急加减速CV模型严重失配EKF很容易发散。这时候有两个方向一个是用交互式多模型IMM在“匀速直线转弯加速”多个模型之间自动切换另一个是直接换UKF或粒子滤波它们对非线性模型的适配性更好。但在绝大多数匀速或者弱机动场景下EKF已经完全够用。项目起步阶段不需要为极端情况过度设计。3. 状态模型与观测模型的形式化设计3.1 状态向量与状态转移矩阵状态向量选6维x [px, py, pz, vx, vy, vz]ᵀ分别是目标在三维空间中的位置和速度。如果目标运动更复杂可以扩展到9维加入加速度项但我强烈建议从6维起步。原因有两点一是状态维度越高可观测性条件越苛刻需要的信息量越多二是过程噪声矩阵的调整难度会成倍上升。6维状态在4个接收站的观测条件下单帧已经有6维观测值3个TDOA 3个FDOA信息和状态量同维加上时间累积性能已经相当可观。CV模型离散化的状态转移矩阵是分块对角形式F [I₃, dt·I₃; 0₃, I₃]也就是说位置加一步速度乘以采样间隔速度保持上一时刻的值不变。3.2 过程噪声Q和测量噪声R的经验设置过程噪声用于刻画模型不匹配的扰动。这里我常用“加速度白噪声”假设目标在采样间隔内受到一个随机加速度扰动标准差记为σ_acc。那么过程噪声可以写成G [0.5·dt²·I₃; dt·I₃] Q G · (σ_acc²·I₃) · Gᵀ展开之后Q [0.25·dt⁴·Qa, 0.5·dt³·Qa; 0.5·dt³·Qa, dt²·Qa]其中Qa σ_acc²·I₃。这个形式比简单地对角化设置Q更符合物理直觉因为它正确刻画了位置噪声和速度噪声之间的时间相关性——位置误差在积分作用下和速度误差是耦合的。σ_acc的取值取决于目标机动强度。我常用的范围是0.1到1 m/s²。目标运动越平稳取值越小越机动取值越大。测量噪声R矩阵就直接按测量精度来设。仿真中我会把σ_tdoa和σ_fdoa当作真值去生成带噪测量然后用它们的平方构造R阵。例如4个接收站、1个参考站观测向量是6维R就是R blkdiag(σ_tdoa²·I₃, σ_fdoa²·I₃)注意这里隐藏了一个简化假设3个TDOA之间是独立的3个FDOA之间也是独立的。严格来说各TDOA共用同一个参考站所以它们之间是有相关性的完整的R应该考虑参考站测量噪声传播带来的非对角项。但在工程上为了降低复杂度很多人直接忽略这个相关性。实测效果差别不特别大如果后续发现滤波器持续不收敛可以回头审视这个假设。3.3 参考站选择与观测向量构造N架接收机里选1号站做参考站。TDOA观测就是“目标到站i的距离减目标到参考站的距离再除以光速”总共有N-1个FDOA观测就是“目标相对站i的距离变化率减目标相对参考站的距离变化率再乘 -fc/c”同样是N-1个。把TDOA和FDOA按固定顺序拼成一个列向量z [τ₂, f₂, τ₃, f₃, ..., τₙ, fₙ]ᵀ观测维度就是2(N-1)。当N4观测是6维和状态向量同维。参考站的选取在几何分布上也有讲究。比较理想的参考站是位于接收机阵列中心附近、且离目标几何距离适中的站。如果固定选1号站而1号站和某个接收站离得太近那组TDOA观测的区分度就差等效于引入冗余信息。实际项目里我会根据站间时钟同步质量和几何分布动态换参考站但换站时必须同步更新观测方程和雅可比矩阵否则滤波器会立刻出问题。4. 雅可比矩阵推导EKF的“命门”EKF能在工程里跑起来最核心的数学步骤就是求观测方程对状态向量的偏导——雅可比矩阵H。这块最容易出错也最值得花时间推导清楚。4.1 TDOA观测的雅可比先定义一些通用变量目标位置p接收站位置pᵢ目标到接收站的距离rᵢ ||p - pᵢ||视线单位向量uᵢ (p - pᵢ)/rᵢ。第i个TDOA观测是τᵢ (rᵢ - r_ref)/c对它求偏导∂τᵢ/∂p (uᵢ - u_ref)ᵀ / c ∂τᵢ/∂v 0这里全部转成行向量形式方便在MATLAB里直接拼进H矩阵的某一行。4.2 FDOA观测的雅可比FDOA要复杂一截。先定义距离变化率dᵢ (v - vᵢ)·uᵢ表示目标相对接收机的视线方向上的相对速度大小。FDOA观测是fᵢ -fc/c · (dᵢ - d_ref)这里的物理含义是目标相对不同接收站的径向速度不同导致接收频率产生不同偏移做差后就得到频率差。求偏导先来dᵢ对位置p的导数∂dᵢ/∂p (v - vᵢ)ᵀ / rᵢ - dᵢ · uᵢᵀ / rᵢ第一项来自分母的rᵢ对p的梯度第二项来自视线方向变化后速度投影角度改变的影响。这个结果建议自己完整推一遍不要只抄结论因为只要忽略其中一项FDOA观测的更新方向就会偏差反映在滤波结果上就是速度估计持续偏置。再看dᵢ对速度v的导数∂dᵢ/∂v uᵢᵀ这一步很直观相对速度在视线方向上的投影对目标速度求导自然等于视线方向向量本身。合成FDOA观测的雅可比∂fᵢ/∂p -fc/c · (∂dᵢ/∂p - ∂d_ref/∂p) ∂fᵢ/∂v -fc/c · (∂dᵢ/∂v - ∂d_ref/∂v)4.3 用数值差分验证H矩阵写代码最怕的就是“公式看着对、代码写出来错”的情况。验证H矩阵有一个非常可靠的方法数值差分。具体做法是取一个预测状态x_pred给它每个分量加一个极小扰动δ比如1e-6重新计算观测值然后用差分公式近似雅可比矩阵的每一项H_num(:, j) ≈ (h(x_pred δ·eⱼ) - h(x_pred - δ·eⱼ)) / (2δ)然后把数值差分结果和解折计算的H对比看最大误差是否在10⁻⁶量级附近。如果在说明雅可比组装基本正确如果差一个量级甚至差符号那就是某项偏导写错了。这个检查流程我几乎每次改完代码都会跑一遍尤其是在修改了坐标定义、参考站选择或者接收机速度建模方式之后。它能帮你省掉大量“滤波器发散但搞不清是哪里错”的排查时间。同时要注意H矩阵的维度2(N-1) × 6。组装时建议按“每个站一行TDOA、一行FDOA”的顺序和观测向量z里的排列顺序严格对应否则观测量和H行错位滤波器会发散得毫无规律。5. MATLAB实战一个能跑的EKFTDOA/FDOA示例5.1 场景参数设定下面的代码片段可以拼成一个完整的仿真脚本。先定义基本场景%% 场景参数 c 3e8; % 光速 fc 2.4e9; % 信号载频 dt 0.1; % 采样间隔 T 30; % 仿真时长 N T / dt 1; % 采样点数 % 4架接收无人机的位置和速度 rx_pos [0 0 100; 600 0 150; 0 600 80; 600 600 120]; % 每行一个站 rx_vel [10 0 0; 0 10 0; -10 0 0; 0 -10 0]; % 接收机保持缓慢运动 ref 1; % 参考站编号 % 测量噪声 sigma_tdoa 1e-7; % 100 ns对应距离误差约30米 sigma_fdoa 1; % 1 Hz % 目标真实初始状态 x_true0 [1200; 800; 500; 25; -15; 0];为什么要让接收机运动这是很多初学者容易忽略的点。FDOA观测本身依赖接收机和目标之间的相对运动如果大家都静止多普勒频差完全来自目标自身运动对速度的估计精度会下降。接收机保持缓慢运动相当于在每个视线方向上都引入了额外的多普勒变化梯度会显著改善FDOA信息的可观测性。5.2 生成目标轨迹与带噪观测先按照CV模型推真实轨迹再在每个时刻生成带噪声的TDOA/FDOA观测向量%% 生成真实轨迹和观测 x_true zeros(6, N); z_meas zeros(2*(size(rx_pos,1)-1), N); x_true(:,1) x_true0; F [eye(3), dt*eye(3); zeros(3), eye(3)]; for k 2:N x_true(:,k) F * x_true(:,k-1); row 0; for i 1:size(rx_pos,1) if i ref, continue; end p_t x_true(1:3, k); v_t x_true(4:6, k); p_i rx_pos(i,:); p_r rx_pos(ref,:); v_i rx_vel(i,:); v_r rx_vel(ref,:); r_i norm(p_t - p_i); r_r norm(p_t - p_r); u_i (p_t - p_i) / r_i; u_r (p_t - p_r) / r_r; % TDOA观测 z_meas(row1, k) (r_i - r_r) / c sigma_tdoa * randn(); % FDOA观测 d_i dot(v_t - v_i, u_i); d_r dot(v_t - v_r, u_r); z_meas(row2, k) -fc/c * (d_i - d_r) sigma_fdoa * randn(); row row 2; end end注意FDOA观测里的负号来自多普勒频移的经典近似接收频率偏移量和距离变化率之间有一个 -fc/c 的比例关系。方向搞错整个滤波器就会往反方向修正这是实战中一个特别隐蔽的坑。5.3 EKF主循环滤波主循环分预测、雅可比计算、更新三步%% EKF初始化 x_est x_true0 [150*randn(3,1); 10*randn(3,1)]; P_est blkdiag(150^2*eye(3), 10^2*eye(3)); pos_err zeros(1, N); vel_err zeros(1, N); for k 2:N % 预测 F [eye(3), dt*eye(3); zeros(3), eye(3)]; x_pred F * x_est; sigma_acc 0.5; Qa sigma_acc^2 * eye(3); G [0.5*dt^2*eye(3); dt*eye(3)]; Q G * Qa * G; P_pred F * P_est * F Q; % 观测预测与雅可比 z_pred zeros(6, 1); H zeros(6, 6); p_t x_pred(1:3); v_t x_pred(4:6); row 0; for i 1:size(rx_pos,1) if i ref, continue; end p_i rx_pos(i,:); p_r rx_pos(ref,:); v_i rx_vel(i,:); v_r rx_vel(ref,:); r_i norm(p_t - p_i); r_r norm(p_t - p_r); u_i (p_t - p_i) / r_i; u_r (p_t - p_r) / r_r; % TDOA预测 z_pred(row1) (r_i - r_r) / c; H(row1, 1:3) (u_i - u_r) / c; % FDOA预测 d_i dot(v_t - v_i, u_i); d_r dot(v_t - v_r, u_r); z_pred(row2) -fc/c * (d_i - d_r); dd_i_dp (v_t - v_i) / r_i - dot(v_t - v_i, u_i) * u_i / r_i; dd_r_dp (v_t - v_r) / r_r - dot(v_t - v_r, u_r) * u_r / r_r; H(row2, 1:3) -fc/c * (dd_i_dp - dd_r_dp); H(row2, 4:6) -fc/c * (u_i - u_r); row row 2; end % 更新 R blkdiag(sigma_tdoa^2 * eye(3), sigma_fdoa^2 * eye(3)); S H * P_pred * H R; K P_pred * H / S; x_est x_pred K * (z_meas(:,k) - z_pred); P_est (eye(6) - K*H) * P_pred * (eye(6) - K*H) K*R*K; pos_err(k) norm(x_est(1:3) - x_true(1:3,k)); vel_err(k) norm(x_est(4:6) - x_true(4:6,k)); end注意到没有P_est的更新我特意用了Joseph形式P (I - KH)P_pred(I - KH)ᵀ KRKᵀ而不是常见的简化形式 P (I - KH)P_pred。原因在于P矩阵在长时间滤波里必须保持对称正定Joseph形式在数值上稳健得多能有效避免舍入误差导致的协方差矩阵“变负”或者“失去对称性”。5.4 跑完你能期待什么样的结果按照上面这组参数跑完EKF通常会在3到5秒内收敛。σ_tdoa取100ns时位置稳态RMSE大概在20到40米的范围σ_fdoa取1Hz时速度稳态RMSE大概在1到2 m/s的量级。如果把σ_tdoa从100ns压到10ns位置误差会显著下降能做到5到15米。之所以误差范围有浮动和接收机几何构型以及单次仿真随机性有关要多跑几次蒙特卡洛取统计结果。画图部分很简单可以画出真实轨迹和估计轨迹的三维对比再加一条位置误差和速度误差随时间变化的曲线。注意收敛前几秒误差会很大画图时不用截掉保留它能直观看到滤波器从初值误差里恢复的过程。6. 用RMSE和几何分析系统评估定位精度6.1 不要只看单次轨迹曲线很多初学者跑完一次仿真看轨迹跟真实轨迹贴合得不错就认为滤波效果很好。但单次轨迹里带着强烈的随机性某个噪声样本凑巧好或者凑巧差结果都可能偏离真实水平。正确的做法是做蒙特卡洛统计同一组参数跑50到100次每次重新生成随机测量噪声统计每一时刻的位置误差和速度误差画出中位数曲线同时统计稳态段的中位数、P95和最大值。这样既能评估平均性能也能看到滤波器在恶劣噪声样本下的鲁棒性。6.2 噪声水平如何影响精度用表格给一个参考趋势前提是接收机构型保持正方形分布不变σ_tdoa (ns)σ_fdoa (Hz)位置RMSE (m)速度RMSE (m/s)100.55~100.3~0.850112~250.8~2100120~401~2100525~503~6这个表格不是绝对数值不同构型、不同运动轨迹下会有差异但可以清晰看出一个规律位置误差主要由TDOA精度主导速度误差主要由FDOA精度主导。当然两者也有交叉影响因为滤波状态是耦合的。6.3 接收机几何构型的影响接收站构型对精度的影响非常大。最简单的理解方式接收站之间的间距越大、空间分布越分散目标所处的“观测圆锥”就越窄定位误差椭圆就越小。如果4个接收站挤在一个狭长区域里目标在某个方向上的距离差观测几乎没有区分度误差会被拉成一条长椭圆这就是几何精度因子GDOP的概念。实操中值得做一组对比实验正方形构型、直线构型、密集构型分别跑EKF并统计位置RMSE。你会发现正方形构型明显优于直线构型因为直线构型在垂直于直线方向的观测自由度严重不足。这直接关系到现场无人机的布站方案——如果布站不合理再强的滤波算法也救不回来。7. 调参和工程落地时最容易踩的坑7.1 初值给得不好滤波直接发散EKF对初值非常敏感。如果位置初值偏差达到几百米甚至上公里滤波器在最初几帧的雅可比矩阵完全失去参考意义增益矩阵K算出来的修正方向可能是错的越修越偏最后P矩阵迅速膨胀状态就发散掉了。实践中常用的解决办法是先用前5到10帧观测做一次最小二乘批处理粗略解出位置初值再用位置差分粗略估计速度初值然后用这些结果初始化EKF。批处理不需要特别精确它只负责把滤波器拉到“收敛域”内剩下的精细估计交给EKF慢慢做。7.2 协方差矩阵发散的几个信号滤波过程中如果发现P矩阵对角线出现负值或者P_pred经过几次更新后非对称说明数值稳定性出问题了。常见原因有三个一是测量噪声R阵给得太小滤波器“过于信任”测量增益过高二是Q阵给得太小滤波器“过于信任”预测导致增益过低长期不更新后协方差异常收缩三是状态量纲差异过大位置是米、速度是米/秒在矩阵运算里可能产生条件数接近机器精度的中间矩阵。前两个问题通过调整σ_acc和σ_tdoa来解决第三个问题可以考虑在状态里使用位置和速度归一化单位比如把速度单位设为百米每秒避免矩阵条件数过大。7.3 FDOA退化的几何原因FDOA观测在最理想的情况下是目标相对不同接收站视线方向上的速度投影差。如果目标与所有接收站近似共线那么所有视线方向几乎重合频率差在各站之间高度相关速度信息的独立维度几乎为零。此时FDOA对速度估计的贡献趋近于零只能靠TDOA随时间变化来恢复可观测性结果就是速度收敛明显变慢甚至稳态误差偏大。这个问题只能通过布站优化解决让接收站环绕目标不要排成一条线尤其要避免“参考站-目标-其他站”近似共线的布局。7.4 数据关联问题和多目标场景真实的多目标场景里接收机测出的每一组TDOA/FDOA并不知道来自哪架目标必须先做数据关联。如果关联错了滤波器会把A目标的测量值用来修正B目标的轨迹结果就是算法瞬间发散。仿真里通常默认关联已知但工程实现绝不能忽略这个前置步骤。简单的工程方案是先用门限过滤根据预测残差的马氏距离判断测量是否属于当前航迹不属于就暂时丢弃或者另开航迹。复杂场景就要上JPDA联合概率数据关联或者MHT多假设跟踪了。7.5 同步精度是最大的工程限制TDOA有一个严格前提所有接收站的时间必须同步。同步误差有多大定位误差就有多大——1ns的同步误差对应约0.3米的距离误差。机载条件下常用GPS秒脉冲作为粗同步再靠信号本身的互相关做细同步整体同步精度能做到10到50ns已经不错。如果实测平台的同步精度只有微秒级那再精细的EKF也无法把定位误差压下来。很多团队实验做出来定位不准算法原理没问题最后定位到是同步链路的问题这种情况我见过太多次了。最后分享一点个人体会这个EKFTDOA/FDOA项目我在MATLAB里从头到尾跑通过之后最大的收获反而不是EKF公式本身而是养成了一个很实用的工作习惯先做仿真数据生成器让数据从已知模型里长出来再让滤波器进场。这样一旦滤波发散你能立刻判断是模型问题、数据问题还是代码问题而不是对着一个真实采集的数据集瞎猜。后续如果要从仿真走向机载实飞建议先把接收站的时钟同步、自定位精度这两块抠准算法层面的增益才能真实体现出来。希望这篇记录能帮你少走点弯路。