简介一份基于扩展卡尔曼滤波EKF的雷达定位仿真资源融合TDOA到达时间差与AOA到达角度两种定位技术并支持三维空间解算。资源面向雷达定位、目标跟踪相关方向的科研人员和学生适合用于理解EKF在非线性滤波中的实际应用。压缩包共7个文件以5个MATLAB脚本.m为主提供算法仿真实现另含1个fig图形界面文件和1个txt使用说明文档压缩包整体仅20KB轻量便捷。当前已有271人学习下载。资源不仅包含AOA定位、TDOA定位的独立仿真还提供二者融合的混合定位策略所有仿真均基于三维场景设计并同时提供非界面程序与界面版程序配合使用说明可快速上手可直接用于算法验证、课程设计或项目参考。1. 雷达 TDOA/AOA 三维定位EKF.zip 到底在解决什么问题做无源雷达定位的人十有八九经历过这种翻车纯 TDOA 解算双曲线交会看着漂亮目标一机动或量测丢几帧就散架纯 AOA 测角方位角差零点几度三维高度能偏出去几百米。EKF.zip 这个包名把 EKF、TDOA、AOA 三样东西串在一起EKF 做状态估计TDOA 时差量测和 AOA 角度量测做观测最终输出目标三维位置和速度。这套融合不是多一种量测多条路的锦上添花而是毫米波雷达、无源雷达、4D 毫米波雷达做三维目标检测时绕不开的基线框架。适合正在写雷达信号处理 MATLAB 仿真、或者被三维定位精度折磨的从业者可以直接拿本文的模型和代码改成自己的量测方程。2. 三维 TDOA/AOA 观测模型从双曲面交会到雅可比矩阵2.1 TDOA 的几何本质三维定位为什么最少要 4 个接收站TDOA 测量的是信号到达两个接收站的时间差乘上光速就得到距离差r_i - r_j c * (t_i - t_j)其中r_i ||p - s_i||是目标 p 到第 i 个接收站 s_i 的欧氏距离。一个 TDOA 方程在三维空间里定义一张双曲面目标被约束在这张曲面上。三个独立的 TDOA 方程交会出两个点——这就是双叶双曲面的镜像模糊需要额外的先验信息或角度量测才能消掉。所以纯 TDOA 三维定位至少需要 4 个接收站提供 3 个独立 TDOA 量测交会结果仍然带模糊。这就是为什么标题里 TDOA 和 AOA 要一起出现——AOA 不只是提高精度它直接干掉镜像解。TDOA 相比绝对 TOA 的核心优势是消掉了接收站之间的公共时钟偏差被动雷达和电子侦察几乎都用 TDOA。代价是量测方程高度非线性而且噪声经过距离差运算后不再是高斯分布。这正是后面要用 EKF 而不是普通线性卡尔曼滤波的原因——线性卡尔曼要求量测方程是线性的TDOA 这条直接不满足。EKF 的泰勒展开虽然也是近似但至少能把非线性带进框架里算。接收站超过 4 个时还有个选量测的细节固定一个参考站生成 N-1 个 TDOA不要全排列。全排列产生 N(N-1)/2 个 TDOA冗余信息在滤波器里表现为更强的量测相关性R 矩阵会变得非常难建而信息收益几乎没有。我见过有人用 6 个站全排列出 15 个 TDOA 喂给 EKF结果滤波器 P 矩阵小得离谱、真实误差一点没降就是相关性没建对导致的。2.2 AOA 角度量测相位测角、坐标系约定与检索词区分AOA 在雷达里通常来自比相测角或单脉冲测角。两个接收通道的相位差与到达角的关系是Δφ 2π * d * sin(θ) / λd 是阵元间距λ 是波长。这是aoa雷达里角度量测最常见的来源也是雷达信号处理里接收阵列的标准做法。对 TI AWR2243 这类毫米波雷达前端来说回波数据读出来之后一帧点云里每个检测点同时带距离、速度、方位角和俯仰角TDOA 和 AOA 就是从同一帧数据里提取的两类量测融合起来非常自然。这里要提醒一个检索上的坑搜 AOA 会搜到大量 USB AOA 模式的内容那是 Android 外设附件协议和测角 AOA 完全无关。这里说的 AOA 是 Angle of Arrival到达角。坐标系约定不统一是这类代码最常见的移植问题。我用接收站本地笛卡尔坐标目标位置 p [x, y, z]方位角定义为θ atan2(y - y_s, x - x_s)范围 [-π, π]正北方向为 0。俯仰角定义为φ atan2(z - z_s, sqrt((x - x_s)^2 (y - y_s)^2))范围 [-π/2, π/2]。注意 atan2 的实参顺序MATLAB 是 atan2(y, x)C 和 Python 的 math.atan2 也是 atan2(y, x)但有些科学计算库里的 atan2 是 (x, y) 顺序移植代码时这是最容易踩的坑。AOA 量测由哪个站提供常见做法是让参考站通常离目标最近的站同时提供方位角和俯仰角。量测向量里 AOA 不一定只有一组但初次实现建议只用一个站的 AOA。多站 AOA 会显著增加量测方程的非线性程度EKF 的一阶线性化更容易失效调试成本翻倍。2.3 混合量测方程 h(x) 与雅可比矩阵 H状态向量取 6 维x [x, y, z, vx, vy, vz]^T。量测只和位置有关4 站布站、TDOA 参考站为 1、AOA 由站 1 提供时量测方程是z [ r2 - r1; r3 - r1; r4 - r1; atan2(y - y1, x - x1); atan2(z - z1, sqrt((x-x1)^2 (y-y1)^2)) ]5 维量测。EKF 的雅可比矩阵 H 是 h(x) 对状态求偏导速度列全 0H [ (x-x2)/r2 - (x-x1)/r1, (y-y2)/r2 - (y-y1)/r1, (z-z2)/r2 - (z-z1)/r1, 0, 0, 0; (x-x3)/r3 - (x-x1)/r1, (y-y3)/r3 - (y-y1)/r1, (z-z3)/r3 - (z-z1)/r1, 0, 0, 0; (x-x4)/r4 - (x-x1)/r1, (y-y4)/r4 - (y-y1)/r1, (z-z4)/r4 - (z-z1)/r1, 0, 0, 0; -(y-y1)/d1^2, (x-x1)/d1^2, 0, 0, 0, 0; -(z-z1)*(x-x1)/(r1^2*d1), -(z-z1)*(y-y1)/(r1^2*d1), d1/r1^2, 0, 0, 0 ]其中d1 sqrt((x-x1)^2 (y-y1)^2)是目标在站 1 水平面上的投影距离。TDOA 行是两个单位方向向量之差几何意义是双曲面在目标位置的法向量AOA 行是对 atan2 求偏导的标准结果。注意H 矩阵里所有量都要在预测点 x_pred 处求值不是真值。EKF 的全部线性化都以当前最优估计为展开点这是新手最容易搞混的环节。验证 H 有没有写错最直接的办法是拿一组随机坐标做数值差分对比H_num(i,j) (h(x e_j*eps) - h(x - e_j*eps)) / (2*eps)和解析 H 逐元素比对。这个习惯能省掉后面所有玄学排错时间。3. EKF 状态方程与滤波循环F、Q、R 的物理意义和初始化策略3.1 状态向量与运动模型匀速模型下的 F 和 QEKF.zip 这类包里最常见的运动模型是匀速CV模型。状态 6 维状态转移矩阵dt 0.1; % 滤波周期单位秒 F [eye(3), dt * eye(3); % 位置 原位置 速度*dt zeros(3), eye(3)]; % 速度保持不变F 的含义一个周期内位置按速度线性外推速度本身不变。真实目标不可能严格匀速于是把加速度建模为零均值白噪声其功率谱密度记为 q单位 m^2/s^3过程噪声矩阵为q 1.0; % 过程噪声强度按目标机动能力估 Q q * [dt^3/3 * eye(3), dt^2/2 * eye(3); dt^2/2 * eye(3), dt * eye(3)];q 的物理含义是目标加速度方差这是整个滤波器里玄学最重的参数。按目标最大加速度估如果 a_max 是目标能承受的最大加速度q 大致取 a_max^2 / 3。地面慢速目标 q 取 0.01~0.1一般飞行器取 1~10强机动目标取 10~100。q 太小机动段滤波跟不上误差突然拉大再慢慢收敛q 太大匀速段估计跟着量测噪声抖。目标如果是强机动导弹末端、无人机急转弯单模型 CV 不够要上 CA 模型或交互式多模型。但第一次跑通方案时先把 CV 调明白再升级IMM 的状态切换逻辑会掩盖参数问题到时候调起来更痛苦。3.2 量测噪声 RTDOA 公共参考站的噪声相关性R 矩阵描述量测噪声5×5。TDOA 部分来自时延估计精度AOA 部分来自测角精度。单位必须和量测向量一致TDOA 是米距离差AOA 是弧度。先定义单站路径时延的测量标准差 σ_t秒那么单站测距标准差是 σ_r c * σ_t。TDOA 量测 r_i - r_1 的方差是 2*σ_r^2而与 r_j - r_1 的协方差是 σ_r^2——因为它们共享 r_1 的噪声。写成矩阵就是sigma_t 1e-9; % 单站时延标准差 1 ns sigma_r c * sigma_t; % 折算成距离0.3 m R_tdoa sigma_r^2 * (eye(3) ones(3)); % 对角 2σ^2非对角 σ^2如果把这个相关性忽略掉把 R_tdoa 写成sigma_r^2 * eye(3)滤波器会认为三个 TDOA 完全独立重复利用同一份信息P 矩阵严重偏小看起来收敛很快实际上是在自欺欺人。这个坑在第 5 章会具体展开。AOA 噪声来自比相测角的相位噪声和 TDOA 的时延噪声来源不同通常认为相互独立。测角标准差典型值 0.5°~2°换算成弧度后平方。总 R 矩阵块对角拼起来sigma_aoa deg2rad(1); % 测角标准差 1° R blkdiag(R_tdoa, sigma_aoa^2 * eye(2));如果实际场景里 TDOA 是由互相关函数峰值提取的不同 TDOA 之间的相关性其实比这个模型更复杂但公共参考站项是最主要的一阶相关建好它就够用了。3.3 EKF 五步更新公式与初始化粗定位EKF 每个周期五步前两步预测、后三步更新% 预测 x_pred F * x_est; P_pred F * P_est * F Q; % 更新 S H * P_pred * H R; K P_pred * H / S; x_est x_pred K * (z - h(x_pred)); P_est (eye(6) - K * H) * P_pred;S 是新息协方差K 是卡尔曼增益z - h(x_pred)是新息。EKF 和标准 KF 的唯一区别就是 h(x_pred) 和 H 在预测点重新求值代价是线性化误差——这也是 EKF 对初值敏感的根源。初始化是 EKF 最容易翻车的一关。EKF 是局部估计器初值离真值太远第一帧就发散。常见做法有三种第一种用前两帧 TDOA 数据跑一次 Chan 算法或加权最小二乘粗定位得到位置初值速度初值用相邻两帧位置差分第二种用 AOA 视线方向加单个 TDOA 距离差做几何闭式解第三种格点搜索撒多个初值同时跑几帧选平均新息最小的轨迹。我一般用第一种Chan 算法在 TDOA 精度尚可时能给出几十米量级的初值足够喂给 EKF。x0 chan_tdoa(...); % 粗定位见上文 P_init diag([1e6, 1e6, 1e6, 1e3, 1e3, 1e3]);P_init 表示对初值的不确定度位置给 1000 m 量级速度给 30 m/s 量级。宁可给大不要给小P_init 给太小等于告诉滤波器我确定得很但它其实不确定收敛结果的偏差会长期留在估计里。4. 跑通 EKF TDOA/AOA 三维滤波MATLAB 最小实现与参数整定4.1 仿真场景接收站布站与目标航迹生成先搭一个能直接复现的仿真场景。四个接收站矩形布站站 1 同时提供 AOA 测角%% 场景与物理参数 c 299792458; % 光速 m/s dt 0.1; % 滤波周期 100 ms T_end 30; % 总时长 30 s t 0:dt:T_end; n_steps length(t); s_recv [ % 4 个接收站坐标米 0, 0, 50; 5000, 0, 30; 0, 5000, 40; 5000, 5000, 60 ]; %% 目标真实航迹匀速 正弦机动 x_true zeros(n_steps, 6); for k 1:n_steps tk t(k); x_true(k,1) 500 40*tk; x_true(k,2) 3000 - 30*tk 200*sin(0.5*tk); x_true(k,3) 800 5*tk; x_true(k,4) 40; x_true(k,5) -30 100*cos(0.5*tk); x_true(k,6) 5; end航迹设计的用意x 方向匀速、y 方向带正弦机动、z 方向缓变一箭三雕检验滤波器在匀速段和机动段的表现。接收站布成 5 km 边长的矩形是为了让 TDOA 双曲面在三个方向都有较好的交会几何。布站几何直接决定可观测性站点近似共线或共面时某些维度根本约束不住。4.2 量测函数、雅可比矩阵与 EKF 主循环量测函数和雅可比函数单独写成子函数方便将来把仿真量测换成真实雷达数据function z h_func(x, s_recv) % 输入状态 x(6x1)输出量测 z(5x1) p x(1:3); r sqrt(sum((p - s_recv).^2, 2)); % 4 站距离 z zeros(5, 1); z(1:3) r(2:4) - r(1); % TDOA参考站 1 dx p(1) - s_recv(1,1); dy p(2) - s_recv(1,2); z(4) atan2(dy, dx); % 方位角 z(5) atan2(p(3) - s_recv(1,3), ... sqrt(dx^2 dy^2)); % 俯仰角 end function H H_func(x, s_recv) p x(1:3); n size(s_recv, 1); u zeros(n, 3); r zeros(n, 1); for i 1:n u(i,:) (p - s_recv(i,:)) / norm(p - s_recv(i,:)); r(i) norm(p - s_recv(i,:)); end H zeros(5, 6); H(1:3,1:3) u(2:4,:) - u(1,:); % TDOA 行u_i - u_1 dx p(1) - s_recv(1,1); dy p(2) - s_recv(1,2); d1 sqrt(dx^2 dy^2); r1 r(1); H(4,:) [-dy/d1^2, dx/d1^2, 0, 0, 0, 0]; H(5,:) [-(p(3)-s_recv(1,3))*dx/(r1^2*d1), ... -(p(3)-s_recv(1,3))*dy/(r1^2*d1), ... d1/r1^2, 0, 0, 0]; end参数说明p - s_recv是 1×3 行向量减 4×3 矩阵依赖 MATLAB R2016b 以后的隐式展开旧版本要改成bsxfun(minus, p, s_recv)。u 矩阵里存的是目标指向各站的单位方向向量TDOA 的雅可比行直接相减得到这段代码和 2.3 节的公式一一对应。滤波器矩阵与主循环%% 滤波器矩阵 F [eye(3), dt*eye(3); zeros(3), eye(3)]; q 5; % 目标有小幅机动取 5 Q q * [dt^3/3*eye(3), dt^2/2*eye(3); dt^2/2*eye(3), dt*eye(3)]; sigma_t 1e-9; % 单站时延标准差 1 ns → 0.3 m sigma_r c * sigma_t; R_tdoa sigma_r^2 * (eye(3) ones(3)); % 公共参考站相关项 sigma_aoa deg2rad(0.5); % 测角标准差 0.5° R blkdiag(R_tdoa, sigma_aoa^2 * eye(2)); %% 初始化粗定位给的位置带偏差 x0 [560, 3080, 830, 30, 80, 8]; P_init diag([1e6, 1e6, 1e6, 1e3, 1e3, 1e3]); x_est x0; P_est P_init; %% 滤波主循环 for k 1:n_steps % 预测 x_pred F * x_est; P_pred F * P_est * F Q; % 模拟量测真值加噪声 z h_func(x_true(k,:), s_recv); z(1:3) z(1:3) chol(R_tdoa) * randn(3,1); z(4:5) z(4:5) sigma_aoa * randn(2,1); % 更新 hx h_func(x_pred, s_recv); H H_func(x_pred, s_recv); innov z - hx; innov(4:5) mod(innov(4:5) pi, 2*pi) - pi; % 角度缠绕 S H * P_pred * H R; K P_pred * H / S; x_est x_pred K * innov; P_est (eye(6) - K * H) * P_pred; est_store(k,:) x_est; end逻辑说明量测噪声模拟用了chol(R_tdoa) * randn(3,1)生成相关高斯噪声这一步替代 mvnrnd不依赖统计工具箱。新息的角度分量用mod(xpi, 2*pi) - pi做缠绕把 358° 的假新息拉回 -2°没有 Mapping 工具箱的 wrapToPi 也能跑。注意量测 z 保持 atan2 原始输出缠绕只作用于新息这是角度类量测进卡尔曼框架的通行做法。4.3 参数整定顺序先锁 R、再调 Q、最后动 P0我调这套滤波器的顺序是固定的顺序参数初值来源作用1R雷达指标换算σ_t 用时延估计精度σ_aoa 用测角精度传感器的物理属性定死不改2Q目标最大加速度q ≈ a_max^2 / 3机动响应与稳态噪声的平衡旋钮3P0粗定位精度给大一个量级只影响前几十帧收敛后无影响R 定死不动的原因是R 描述的是传感器噪声的统计特性调它等于欺骗滤波器会让 P 矩阵失真。Q 才是真正需要反复调的参数Q 太小机动段误差会突然飙高再慢慢收敛Q 太大匀速段轨迹像毛线一样抖。判断 Q 是否合适的客观指标是第 6 章的 NIS 均值主观指标是轨迹在机动段的跟随延迟——延迟大说明 Q 给小了。5. EKF TDOA 实战避坑初值发散、噪声相关性与布站几何陷阱5.1 初值太远滤波器第一帧就飞到几百公里外现象EKF 启动后 1~2 帧估计位置直接跳到几十上百公里外之后再也拉不回来或者来回震荡P 矩阵对角线迟迟不降。原因EKF 每个周期只在预测点做一次一阶泰勒展开线性化只在局部有效。初值误差几百米时TDOA 双曲面的曲率已经大到一阶近似完全失真卡尔曼增益的方向都是错的滤波自然发散。解决粗定位不是可选项是必经步骤。实测场景里用前后帧的 Chan 或 WLS 结果做初值仿真里就按 3.3 节的方法给。另外两个实用技巧一是限制单帧新息幅度超过 3 倍新息标准差就截断防止一次坏量测把估计打飞二是前 10 帧把 R 临时放大 10 倍让滤波器用小步长爬进真值附近再恢复原 R。5.2 TDOA 公共参考站相关性被忽略滤波器过度自信现象真实 RMSE 约 20 m但 P 矩阵对角线开方只有 5 m。轨迹曲线看着收敛NEES 却超出置信区间几倍——滤波器对自身误差的估计严重失真。原因三个 TDOA 共享参考站 r1 的噪声如果 R 用对角阵等于把同一份噪声当独立的三份用信息被重复计算滤波器以为自己拿到了 3 个独立量测实际有效独立信息远少于 3。解决R_tdoa 必须写成sigma_r^2 * (eye(3) ones(3))。注意如果你用的是差分 TDOA 形式比如 (r2-r1)-(r3-r1)相关性结构完全不同R 要重新推导不能照抄。5.3 AOA 角度缠绕目标过正北时新息跳变 358°现象目标方位角从 179° 连续变到 -179°滤波位置突然向错误方向跳一下航迹出现响尾蛇式的折线。原因atan2 输出限定在 [-π, π]真实角度只变了 2°数值上却显示 358° 的差异。EKF 的新息 z - h(x_pred) 如果不处理就把 358° 当成量测偏差喂给滤波器相当于给了一个巨大的假量测。解决每帧对新息的角度分量做缠绕。wrapToPi 是 Mapping 工具箱的函数没有的话用mod(innov pi, 2*pi) - pi等效。注意处理对象是新息不是量测本身也不要把状态转成角度参与滤波——状态全程用笛卡尔坐标只在输出显示时算角度。5.4 布站近似共面Z 轴可观测性差高度飘现象X、Y 收敛良好Z 轴误差几十米到几百米飘P(3,3) 一直降不下去改 AOA 的俯仰角噪声也没明显效果。原因接收站近似在同一平面时TDOA 双曲面在垂直方向近乎平行几乎不带 Z 轴信息。唯一约束 Z 的是俯仰角 AOA如果测角阵列的俯仰精度不够Z 轴本质上靠过程噪声在撑着。解决布站阶段让接收站之间有 20 m 以上的高差破坏共面性这是最有效的。做不了布站时给俯仰角量测在 R 里设更小的方差如果确实测得准或者用数字高程模型做软约束。诊断方法是看 P 矩阵特征值某个方向特征值远大于其他方向就是不可观测方向。5.5 站间时间不同步系统性误差滤不掉现象仿真里滤波器完美收敛换成实测数据整体偏移稳态误差比 R 预期的还大且滤波时间越长越明显。原因TDOA 的前提是各接收站时钟严格同步。实测站间存在固定时钟偏差和漂移叠加在量测上是非零均值误差。卡尔曼滤波对零均值随机噪声有效对系统偏差只能把它估计出来或者提前标定掉。解决先标定。用已知位置的校准源发信号反推各站相对参考站的时钟偏差在数据预处理阶段扣除。进阶做法是把站间偏差扩进状态向量一起估计但状态维数增加会稀释 TDOA 对位置的约束需要重新做可观测性分析。实测调试的一个经验先用一段已知轨迹的数据把系统误差扣掉再喂 EKF不然你会把系统误差的锅甩给 Q 和 R越调越乱。6. 验证这个滤波器靠不靠谱CRLB、蒙特卡洛与残差检验6.1 CRLB先知道理论精度上限在真值处求 H 和 R算费雪信息矩阵 FIM H^T R^{-1} H位置分量的 CRLB 是 FIM 逆矩阵前三个对角元的开方。EKF 的 RMSE 逼近 CRLB说明量测信息用尽了差好几倍说明模型或参数有问题。这个对照是最快的体检。6.2 蒙特卡洛 NEES滤波器是否诚实跑 N 次蒙特卡洛统计每时刻 RMSE。NEES 判断 P 矩阵是否可信err x_est - x_true; % 末次估计与真值之差 nees err * (P_est \ err); % 单次采样自由度 状态维数 6 avg_nees mean(nees); % N 次平均 ci [chi2inv(0.025, 6*N)/N, chi2inv(0.975, 6*N)/N]; % 95% 区间avg_nees 落在区间内说明 P 与实际误差一致超出上界说明 P 过度自信——5.2 节说的 R 相关性错误就是在这个检验下现形的。6.3 新息白噪声检验量测信息有没有吃干净标准化新息q innov * S^{-1} * innov应服从卡方分布自由度等于量测维数 5。把所有时刻的 q 取平均应该在 5 附近远大于 5 说明模型没建对或 Q、R 给小了远小于 5 说明 R 或 Q 给得太大量测信息没用足。值不值得做这套 EKF TDOA/AOA 融合我的判断标准是已经有稳定的 TDOA 和 AOA 量测源、且需要输出连续轨迹时值得投入只是单帧定位的话Chan 加加权最小二乘就够了EKF 的价值全在时序平滑和状态估计上。我的习惯是每次改完参数先把 NEES 和 NIS 两个数打出来不看轨迹图——轨迹好看不代表滤波器诚实这两个统计量才是黑匣子里的探针。希望帮到你。本文还有配套的精品资源点击获取 EKF融合TDOA/AOA的三维定位：原理、MATLAB实现与实战避坑
简介一份基于扩展卡尔曼滤波EKF的雷达定位仿真资源融合TDOA到达时间差与AOA到达角度两种定位技术并支持三维空间解算。资源面向雷达定位、目标跟踪相关方向的科研人员和学生适合用于理解EKF在非线性滤波中的实际应用。压缩包共7个文件以5个MATLAB脚本.m为主提供算法仿真实现另含1个fig图形界面文件和1个txt使用说明文档压缩包整体仅20KB轻量便捷。当前已有271人学习下载。资源不仅包含AOA定位、TDOA定位的独立仿真还提供二者融合的混合定位策略所有仿真均基于三维场景设计并同时提供非界面程序与界面版程序配合使用说明可快速上手可直接用于算法验证、课程设计或项目参考。1. 雷达 TDOA/AOA 三维定位EKF.zip 到底在解决什么问题做无源雷达定位的人十有八九经历过这种翻车纯 TDOA 解算双曲线交会看着漂亮目标一机动或量测丢几帧就散架纯 AOA 测角方位角差零点几度三维高度能偏出去几百米。EKF.zip 这个包名把 EKF、TDOA、AOA 三样东西串在一起EKF 做状态估计TDOA 时差量测和 AOA 角度量测做观测最终输出目标三维位置和速度。这套融合不是多一种量测多条路的锦上添花而是毫米波雷达、无源雷达、4D 毫米波雷达做三维目标检测时绕不开的基线框架。适合正在写雷达信号处理 MATLAB 仿真、或者被三维定位精度折磨的从业者可以直接拿本文的模型和代码改成自己的量测方程。2. 三维 TDOA/AOA 观测模型从双曲面交会到雅可比矩阵2.1 TDOA 的几何本质三维定位为什么最少要 4 个接收站TDOA 测量的是信号到达两个接收站的时间差乘上光速就得到距离差r_i - r_j c * (t_i - t_j)其中r_i ||p - s_i||是目标 p 到第 i 个接收站 s_i 的欧氏距离。一个 TDOA 方程在三维空间里定义一张双曲面目标被约束在这张曲面上。三个独立的 TDOA 方程交会出两个点——这就是双叶双曲面的镜像模糊需要额外的先验信息或角度量测才能消掉。所以纯 TDOA 三维定位至少需要 4 个接收站提供 3 个独立 TDOA 量测交会结果仍然带模糊。这就是为什么标题里 TDOA 和 AOA 要一起出现——AOA 不只是提高精度它直接干掉镜像解。TDOA 相比绝对 TOA 的核心优势是消掉了接收站之间的公共时钟偏差被动雷达和电子侦察几乎都用 TDOA。代价是量测方程高度非线性而且噪声经过距离差运算后不再是高斯分布。这正是后面要用 EKF 而不是普通线性卡尔曼滤波的原因——线性卡尔曼要求量测方程是线性的TDOA 这条直接不满足。EKF 的泰勒展开虽然也是近似但至少能把非线性带进框架里算。接收站超过 4 个时还有个选量测的细节固定一个参考站生成 N-1 个 TDOA不要全排列。全排列产生 N(N-1)/2 个 TDOA冗余信息在滤波器里表现为更强的量测相关性R 矩阵会变得非常难建而信息收益几乎没有。我见过有人用 6 个站全排列出 15 个 TDOA 喂给 EKF结果滤波器 P 矩阵小得离谱、真实误差一点没降就是相关性没建对导致的。2.2 AOA 角度量测相位测角、坐标系约定与检索词区分AOA 在雷达里通常来自比相测角或单脉冲测角。两个接收通道的相位差与到达角的关系是Δφ 2π * d * sin(θ) / λd 是阵元间距λ 是波长。这是aoa雷达里角度量测最常见的来源也是雷达信号处理里接收阵列的标准做法。对 TI AWR2243 这类毫米波雷达前端来说回波数据读出来之后一帧点云里每个检测点同时带距离、速度、方位角和俯仰角TDOA 和 AOA 就是从同一帧数据里提取的两类量测融合起来非常自然。这里要提醒一个检索上的坑搜 AOA 会搜到大量 USB AOA 模式的内容那是 Android 外设附件协议和测角 AOA 完全无关。这里说的 AOA 是 Angle of Arrival到达角。坐标系约定不统一是这类代码最常见的移植问题。我用接收站本地笛卡尔坐标目标位置 p [x, y, z]方位角定义为θ atan2(y - y_s, x - x_s)范围 [-π, π]正北方向为 0。俯仰角定义为φ atan2(z - z_s, sqrt((x - x_s)^2 (y - y_s)^2))范围 [-π/2, π/2]。注意 atan2 的实参顺序MATLAB 是 atan2(y, x)C 和 Python 的 math.atan2 也是 atan2(y, x)但有些科学计算库里的 atan2 是 (x, y) 顺序移植代码时这是最容易踩的坑。AOA 量测由哪个站提供常见做法是让参考站通常离目标最近的站同时提供方位角和俯仰角。量测向量里 AOA 不一定只有一组但初次实现建议只用一个站的 AOA。多站 AOA 会显著增加量测方程的非线性程度EKF 的一阶线性化更容易失效调试成本翻倍。2.3 混合量测方程 h(x) 与雅可比矩阵 H状态向量取 6 维x [x, y, z, vx, vy, vz]^T。量测只和位置有关4 站布站、TDOA 参考站为 1、AOA 由站 1 提供时量测方程是z [ r2 - r1; r3 - r1; r4 - r1; atan2(y - y1, x - x1); atan2(z - z1, sqrt((x-x1)^2 (y-y1)^2)) ]5 维量测。EKF 的雅可比矩阵 H 是 h(x) 对状态求偏导速度列全 0H [ (x-x2)/r2 - (x-x1)/r1, (y-y2)/r2 - (y-y1)/r1, (z-z2)/r2 - (z-z1)/r1, 0, 0, 0; (x-x3)/r3 - (x-x1)/r1, (y-y3)/r3 - (y-y1)/r1, (z-z3)/r3 - (z-z1)/r1, 0, 0, 0; (x-x4)/r4 - (x-x1)/r1, (y-y4)/r4 - (y-y1)/r1, (z-z4)/r4 - (z-z1)/r1, 0, 0, 0; -(y-y1)/d1^2, (x-x1)/d1^2, 0, 0, 0, 0; -(z-z1)*(x-x1)/(r1^2*d1), -(z-z1)*(y-y1)/(r1^2*d1), d1/r1^2, 0, 0, 0 ]其中d1 sqrt((x-x1)^2 (y-y1)^2)是目标在站 1 水平面上的投影距离。TDOA 行是两个单位方向向量之差几何意义是双曲面在目标位置的法向量AOA 行是对 atan2 求偏导的标准结果。注意H 矩阵里所有量都要在预测点 x_pred 处求值不是真值。EKF 的全部线性化都以当前最优估计为展开点这是新手最容易搞混的环节。验证 H 有没有写错最直接的办法是拿一组随机坐标做数值差分对比H_num(i,j) (h(x e_j*eps) - h(x - e_j*eps)) / (2*eps)和解析 H 逐元素比对。这个习惯能省掉后面所有玄学排错时间。3. EKF 状态方程与滤波循环F、Q、R 的物理意义和初始化策略3.1 状态向量与运动模型匀速模型下的 F 和 QEKF.zip 这类包里最常见的运动模型是匀速CV模型。状态 6 维状态转移矩阵dt 0.1; % 滤波周期单位秒 F [eye(3), dt * eye(3); % 位置 原位置 速度*dt zeros(3), eye(3)]; % 速度保持不变F 的含义一个周期内位置按速度线性外推速度本身不变。真实目标不可能严格匀速于是把加速度建模为零均值白噪声其功率谱密度记为 q单位 m^2/s^3过程噪声矩阵为q 1.0; % 过程噪声强度按目标机动能力估 Q q * [dt^3/3 * eye(3), dt^2/2 * eye(3); dt^2/2 * eye(3), dt * eye(3)];q 的物理含义是目标加速度方差这是整个滤波器里玄学最重的参数。按目标最大加速度估如果 a_max 是目标能承受的最大加速度q 大致取 a_max^2 / 3。地面慢速目标 q 取 0.01~0.1一般飞行器取 1~10强机动目标取 10~100。q 太小机动段滤波跟不上误差突然拉大再慢慢收敛q 太大匀速段估计跟着量测噪声抖。目标如果是强机动导弹末端、无人机急转弯单模型 CV 不够要上 CA 模型或交互式多模型。但第一次跑通方案时先把 CV 调明白再升级IMM 的状态切换逻辑会掩盖参数问题到时候调起来更痛苦。3.2 量测噪声 RTDOA 公共参考站的噪声相关性R 矩阵描述量测噪声5×5。TDOA 部分来自时延估计精度AOA 部分来自测角精度。单位必须和量测向量一致TDOA 是米距离差AOA 是弧度。先定义单站路径时延的测量标准差 σ_t秒那么单站测距标准差是 σ_r c * σ_t。TDOA 量测 r_i - r_1 的方差是 2*σ_r^2而与 r_j - r_1 的协方差是 σ_r^2——因为它们共享 r_1 的噪声。写成矩阵就是sigma_t 1e-9; % 单站时延标准差 1 ns sigma_r c * sigma_t; % 折算成距离0.3 m R_tdoa sigma_r^2 * (eye(3) ones(3)); % 对角 2σ^2非对角 σ^2如果把这个相关性忽略掉把 R_tdoa 写成sigma_r^2 * eye(3)滤波器会认为三个 TDOA 完全独立重复利用同一份信息P 矩阵严重偏小看起来收敛很快实际上是在自欺欺人。这个坑在第 5 章会具体展开。AOA 噪声来自比相测角的相位噪声和 TDOA 的时延噪声来源不同通常认为相互独立。测角标准差典型值 0.5°~2°换算成弧度后平方。总 R 矩阵块对角拼起来sigma_aoa deg2rad(1); % 测角标准差 1° R blkdiag(R_tdoa, sigma_aoa^2 * eye(2));如果实际场景里 TDOA 是由互相关函数峰值提取的不同 TDOA 之间的相关性其实比这个模型更复杂但公共参考站项是最主要的一阶相关建好它就够用了。3.3 EKF 五步更新公式与初始化粗定位EKF 每个周期五步前两步预测、后三步更新% 预测 x_pred F * x_est; P_pred F * P_est * F Q; % 更新 S H * P_pred * H R; K P_pred * H / S; x_est x_pred K * (z - h(x_pred)); P_est (eye(6) - K * H) * P_pred;S 是新息协方差K 是卡尔曼增益z - h(x_pred)是新息。EKF 和标准 KF 的唯一区别就是 h(x_pred) 和 H 在预测点重新求值代价是线性化误差——这也是 EKF 对初值敏感的根源。初始化是 EKF 最容易翻车的一关。EKF 是局部估计器初值离真值太远第一帧就发散。常见做法有三种第一种用前两帧 TDOA 数据跑一次 Chan 算法或加权最小二乘粗定位得到位置初值速度初值用相邻两帧位置差分第二种用 AOA 视线方向加单个 TDOA 距离差做几何闭式解第三种格点搜索撒多个初值同时跑几帧选平均新息最小的轨迹。我一般用第一种Chan 算法在 TDOA 精度尚可时能给出几十米量级的初值足够喂给 EKF。x0 chan_tdoa(...); % 粗定位见上文 P_init diag([1e6, 1e6, 1e6, 1e3, 1e3, 1e3]);P_init 表示对初值的不确定度位置给 1000 m 量级速度给 30 m/s 量级。宁可给大不要给小P_init 给太小等于告诉滤波器我确定得很但它其实不确定收敛结果的偏差会长期留在估计里。4. 跑通 EKF TDOA/AOA 三维滤波MATLAB 最小实现与参数整定4.1 仿真场景接收站布站与目标航迹生成先搭一个能直接复现的仿真场景。四个接收站矩形布站站 1 同时提供 AOA 测角%% 场景与物理参数 c 299792458; % 光速 m/s dt 0.1; % 滤波周期 100 ms T_end 30; % 总时长 30 s t 0:dt:T_end; n_steps length(t); s_recv [ % 4 个接收站坐标米 0, 0, 50; 5000, 0, 30; 0, 5000, 40; 5000, 5000, 60 ]; %% 目标真实航迹匀速 正弦机动 x_true zeros(n_steps, 6); for k 1:n_steps tk t(k); x_true(k,1) 500 40*tk; x_true(k,2) 3000 - 30*tk 200*sin(0.5*tk); x_true(k,3) 800 5*tk; x_true(k,4) 40; x_true(k,5) -30 100*cos(0.5*tk); x_true(k,6) 5; end航迹设计的用意x 方向匀速、y 方向带正弦机动、z 方向缓变一箭三雕检验滤波器在匀速段和机动段的表现。接收站布成 5 km 边长的矩形是为了让 TDOA 双曲面在三个方向都有较好的交会几何。布站几何直接决定可观测性站点近似共线或共面时某些维度根本约束不住。4.2 量测函数、雅可比矩阵与 EKF 主循环量测函数和雅可比函数单独写成子函数方便将来把仿真量测换成真实雷达数据function z h_func(x, s_recv) % 输入状态 x(6x1)输出量测 z(5x1) p x(1:3); r sqrt(sum((p - s_recv).^2, 2)); % 4 站距离 z zeros(5, 1); z(1:3) r(2:4) - r(1); % TDOA参考站 1 dx p(1) - s_recv(1,1); dy p(2) - s_recv(1,2); z(4) atan2(dy, dx); % 方位角 z(5) atan2(p(3) - s_recv(1,3), ... sqrt(dx^2 dy^2)); % 俯仰角 end function H H_func(x, s_recv) p x(1:3); n size(s_recv, 1); u zeros(n, 3); r zeros(n, 1); for i 1:n u(i,:) (p - s_recv(i,:)) / norm(p - s_recv(i,:)); r(i) norm(p - s_recv(i,:)); end H zeros(5, 6); H(1:3,1:3) u(2:4,:) - u(1,:); % TDOA 行u_i - u_1 dx p(1) - s_recv(1,1); dy p(2) - s_recv(1,2); d1 sqrt(dx^2 dy^2); r1 r(1); H(4,:) [-dy/d1^2, dx/d1^2, 0, 0, 0, 0]; H(5,:) [-(p(3)-s_recv(1,3))*dx/(r1^2*d1), ... -(p(3)-s_recv(1,3))*dy/(r1^2*d1), ... d1/r1^2, 0, 0, 0]; end参数说明p - s_recv是 1×3 行向量减 4×3 矩阵依赖 MATLAB R2016b 以后的隐式展开旧版本要改成bsxfun(minus, p, s_recv)。u 矩阵里存的是目标指向各站的单位方向向量TDOA 的雅可比行直接相减得到这段代码和 2.3 节的公式一一对应。滤波器矩阵与主循环%% 滤波器矩阵 F [eye(3), dt*eye(3); zeros(3), eye(3)]; q 5; % 目标有小幅机动取 5 Q q * [dt^3/3*eye(3), dt^2/2*eye(3); dt^2/2*eye(3), dt*eye(3)]; sigma_t 1e-9; % 单站时延标准差 1 ns → 0.3 m sigma_r c * sigma_t; R_tdoa sigma_r^2 * (eye(3) ones(3)); % 公共参考站相关项 sigma_aoa deg2rad(0.5); % 测角标准差 0.5° R blkdiag(R_tdoa, sigma_aoa^2 * eye(2)); %% 初始化粗定位给的位置带偏差 x0 [560, 3080, 830, 30, 80, 8]; P_init diag([1e6, 1e6, 1e6, 1e3, 1e3, 1e3]); x_est x0; P_est P_init; %% 滤波主循环 for k 1:n_steps % 预测 x_pred F * x_est; P_pred F * P_est * F Q; % 模拟量测真值加噪声 z h_func(x_true(k,:), s_recv); z(1:3) z(1:3) chol(R_tdoa) * randn(3,1); z(4:5) z(4:5) sigma_aoa * randn(2,1); % 更新 hx h_func(x_pred, s_recv); H H_func(x_pred, s_recv); innov z - hx; innov(4:5) mod(innov(4:5) pi, 2*pi) - pi; % 角度缠绕 S H * P_pred * H R; K P_pred * H / S; x_est x_pred K * innov; P_est (eye(6) - K * H) * P_pred; est_store(k,:) x_est; end逻辑说明量测噪声模拟用了chol(R_tdoa) * randn(3,1)生成相关高斯噪声这一步替代 mvnrnd不依赖统计工具箱。新息的角度分量用mod(xpi, 2*pi) - pi做缠绕把 358° 的假新息拉回 -2°没有 Mapping 工具箱的 wrapToPi 也能跑。注意量测 z 保持 atan2 原始输出缠绕只作用于新息这是角度类量测进卡尔曼框架的通行做法。4.3 参数整定顺序先锁 R、再调 Q、最后动 P0我调这套滤波器的顺序是固定的顺序参数初值来源作用1R雷达指标换算σ_t 用时延估计精度σ_aoa 用测角精度传感器的物理属性定死不改2Q目标最大加速度q ≈ a_max^2 / 3机动响应与稳态噪声的平衡旋钮3P0粗定位精度给大一个量级只影响前几十帧收敛后无影响R 定死不动的原因是R 描述的是传感器噪声的统计特性调它等于欺骗滤波器会让 P 矩阵失真。Q 才是真正需要反复调的参数Q 太小机动段误差会突然飙高再慢慢收敛Q 太大匀速段轨迹像毛线一样抖。判断 Q 是否合适的客观指标是第 6 章的 NIS 均值主观指标是轨迹在机动段的跟随延迟——延迟大说明 Q 给小了。5. EKF TDOA 实战避坑初值发散、噪声相关性与布站几何陷阱5.1 初值太远滤波器第一帧就飞到几百公里外现象EKF 启动后 1~2 帧估计位置直接跳到几十上百公里外之后再也拉不回来或者来回震荡P 矩阵对角线迟迟不降。原因EKF 每个周期只在预测点做一次一阶泰勒展开线性化只在局部有效。初值误差几百米时TDOA 双曲面的曲率已经大到一阶近似完全失真卡尔曼增益的方向都是错的滤波自然发散。解决粗定位不是可选项是必经步骤。实测场景里用前后帧的 Chan 或 WLS 结果做初值仿真里就按 3.3 节的方法给。另外两个实用技巧一是限制单帧新息幅度超过 3 倍新息标准差就截断防止一次坏量测把估计打飞二是前 10 帧把 R 临时放大 10 倍让滤波器用小步长爬进真值附近再恢复原 R。5.2 TDOA 公共参考站相关性被忽略滤波器过度自信现象真实 RMSE 约 20 m但 P 矩阵对角线开方只有 5 m。轨迹曲线看着收敛NEES 却超出置信区间几倍——滤波器对自身误差的估计严重失真。原因三个 TDOA 共享参考站 r1 的噪声如果 R 用对角阵等于把同一份噪声当独立的三份用信息被重复计算滤波器以为自己拿到了 3 个独立量测实际有效独立信息远少于 3。解决R_tdoa 必须写成sigma_r^2 * (eye(3) ones(3))。注意如果你用的是差分 TDOA 形式比如 (r2-r1)-(r3-r1)相关性结构完全不同R 要重新推导不能照抄。5.3 AOA 角度缠绕目标过正北时新息跳变 358°现象目标方位角从 179° 连续变到 -179°滤波位置突然向错误方向跳一下航迹出现响尾蛇式的折线。原因atan2 输出限定在 [-π, π]真实角度只变了 2°数值上却显示 358° 的差异。EKF 的新息 z - h(x_pred) 如果不处理就把 358° 当成量测偏差喂给滤波器相当于给了一个巨大的假量测。解决每帧对新息的角度分量做缠绕。wrapToPi 是 Mapping 工具箱的函数没有的话用mod(innov pi, 2*pi) - pi等效。注意处理对象是新息不是量测本身也不要把状态转成角度参与滤波——状态全程用笛卡尔坐标只在输出显示时算角度。5.4 布站近似共面Z 轴可观测性差高度飘现象X、Y 收敛良好Z 轴误差几十米到几百米飘P(3,3) 一直降不下去改 AOA 的俯仰角噪声也没明显效果。原因接收站近似在同一平面时TDOA 双曲面在垂直方向近乎平行几乎不带 Z 轴信息。唯一约束 Z 的是俯仰角 AOA如果测角阵列的俯仰精度不够Z 轴本质上靠过程噪声在撑着。解决布站阶段让接收站之间有 20 m 以上的高差破坏共面性这是最有效的。做不了布站时给俯仰角量测在 R 里设更小的方差如果确实测得准或者用数字高程模型做软约束。诊断方法是看 P 矩阵特征值某个方向特征值远大于其他方向就是不可观测方向。5.5 站间时间不同步系统性误差滤不掉现象仿真里滤波器完美收敛换成实测数据整体偏移稳态误差比 R 预期的还大且滤波时间越长越明显。原因TDOA 的前提是各接收站时钟严格同步。实测站间存在固定时钟偏差和漂移叠加在量测上是非零均值误差。卡尔曼滤波对零均值随机噪声有效对系统偏差只能把它估计出来或者提前标定掉。解决先标定。用已知位置的校准源发信号反推各站相对参考站的时钟偏差在数据预处理阶段扣除。进阶做法是把站间偏差扩进状态向量一起估计但状态维数增加会稀释 TDOA 对位置的约束需要重新做可观测性分析。实测调试的一个经验先用一段已知轨迹的数据把系统误差扣掉再喂 EKF不然你会把系统误差的锅甩给 Q 和 R越调越乱。6. 验证这个滤波器靠不靠谱CRLB、蒙特卡洛与残差检验6.1 CRLB先知道理论精度上限在真值处求 H 和 R算费雪信息矩阵 FIM H^T R^{-1} H位置分量的 CRLB 是 FIM 逆矩阵前三个对角元的开方。EKF 的 RMSE 逼近 CRLB说明量测信息用尽了差好几倍说明模型或参数有问题。这个对照是最快的体检。6.2 蒙特卡洛 NEES滤波器是否诚实跑 N 次蒙特卡洛统计每时刻 RMSE。NEES 判断 P 矩阵是否可信err x_est - x_true; % 末次估计与真值之差 nees err * (P_est \ err); % 单次采样自由度 状态维数 6 avg_nees mean(nees); % N 次平均 ci [chi2inv(0.025, 6*N)/N, chi2inv(0.975, 6*N)/N]; % 95% 区间avg_nees 落在区间内说明 P 与实际误差一致超出上界说明 P 过度自信——5.2 节说的 R 相关性错误就是在这个检验下现形的。6.3 新息白噪声检验量测信息有没有吃干净标准化新息q innov * S^{-1} * innov应服从卡方分布自由度等于量测维数 5。把所有时刻的 q 取平均应该在 5 附近远大于 5 说明模型没建对或 Q、R 给小了远小于 5 说明 R 或 Q 给得太大量测信息没用足。值不值得做这套 EKF TDOA/AOA 融合我的判断标准是已经有稳定的 TDOA 和 AOA 量测源、且需要输出连续轨迹时值得投入只是单帧定位的话Chan 加加权最小二乘就够了EKF 的价值全在时序平滑和状态估计上。我的习惯是每次改完参数先把 NEES 和 NIS 两个数打出来不看轨迹图——轨迹好看不代表滤波器诚实这两个统计量才是黑匣子里的探针。希望帮到你。本文还有配套的精品资源点击获取