简介这份资源是面向无线定位与室内追踪方向学习者的实践代码包聚焦AOA到达角与TOF飞行时间在WiFi信号中的融合应用适合具备一定通信或信号处理基础、希望动手复现人体定位追踪流程的开发者与研究人员。包内共8个文件以7个m脚本和1个md说明文档为主压缩包约6KB体量轻便便于快速阅读与二次修改。脚本覆盖信号生成、环境仿真、TOF与AOA计算等环节配合README可帮助读者理解从CSI数据到位置估计的完整链路并对照三角几何与多AP测距思路验证算法效果。目前已有345人学习适合作为室内定位入门练手或课程设计参考也可在此基础上扩展轨迹跟踪与多目标场景。1. 从一串下划线说起mD_track 里的 AOA 与 TOF 到底在算什么第一次看到mD_track-master_AOA_定位_tof_wifi_tofaoa_这串名字多数人的反应是「信息量好大但不知道从哪下手」。拆开看其实就三件事一个叫 mD_track 的跟踪工程骨架、AOA到达角定位、以及 TOF飞行时间测距后面还挂着 wifi 和 tofaoa 两个限定词。它要解决的问题很具体——在室内或遮挡环境里用无线信号同时拿到「方向」和「距离」再把这两类观测量喂进一个跟踪器输出连续、平滑、可用的目标轨迹。适合谁做室内定位、人员跟踪、资产盘点、机器人室内导航的工程师尤其是手里已经有 WiFi 网卡或 UWB/TOF 模组、想把原始测量变成轨迹的那批人。单靠 AOA 只能得到一条射线目标在射线上哪个位置不知道单靠 TOF 只能得到一个球面方向不知道。两者一融合射线和球面求交位置就唯一了。这就是 tofaoa 这个词的由来也是整个工程的价值锚点。mD_track 在这里扮演的是「时间维度上的滤波器」——单帧解算出来的坐标抖动大、跳点多跟踪器用运动模型把多帧串起来才能得到能看的轨迹。下面按「先立住原理、再跑通最小链路、然后踩坑、最后进阶」的顺序讲透。2. AOA 与 TOF 的测量原理和融合选型为什么不是简单取平均2.1 AOA 到底在测什么相位差与阵列几何AOA 的本质是测同一信号到达多个天线单元的相位差或时间差。以最常见的均匀线阵ULA为例相邻阵元间距 d信号入射角 θ载波波长 λ则相邻阵元相位差为Δφ 2π · d · sin(θ) / λ反解出 θ arcsin(Δφ · λ / (2π · d))。这里有个硬约束d 必须小于 λ/2否则会出现相位模糊grating lobe同一个相位差对应多个角度解算直接翻车。2.4GHz 的 λ 约 12.5cm所以阵元间距通常取 6cm 左右5GHz 的 λ 约 6cm间距取 3cm。这是选型第一课频段越高阵列越紧凑但穿透和绕射越差。实际工程里很少用两根天线因为单基线只能测一维角且精度有限。常见做法是 4 阵元或 8 阵元的线阵/面阵用 MUSIC 或 ESPRIT 这类子空间算法做超分辨估计。MUSIC 的代价是每帧要做协方差矩阵特征分解算力开销不小如果目标数少、信噪比高直接用相位差反解加卡尔曼平滑也能凑合。我一般会先跑相位差法验证链路通不通再上 MUSIC 压精度。2.2 TOF 测距从 RTT 到双边测距TOF 测的是信号往返时间乘以光速。WiFi 场景下常用的是 RTTRound Trip Time即发起方发一个帧、响应方回一个帧测总耗时再减去处理时延。单边测距的误差主要来自双方时钟不同步所以工程上更常用双边测距Two-Way Ranging距离 c · (T_round1 - T_reply1 T_round2 - T_reply2) / 4其中 T_round 是本地记录的往返时间T_reply 是对端记录的处理时间。这样时钟偏差被抵消掉。UWB 的 TOF 精度能到厘米级WiFi 的 RTT 精度通常在 1~3 米取决于带宽和信道。带宽越大时间分辨率越高这是为什么 80MHz 信道比 20MHz 信道测距更准。2.3 融合选型松耦合还是紧耦合拿到 AOA 和 TOF 后融合方式分两派。松耦合是各自先解算出角度和距离再在位置层做加权或卡尔曼融合紧耦合是把原始相位差和原始时间戳一起丢进一个滤波器。松耦合实现简单、模块解耦适合快速验证紧耦合精度上限高但对噪声模型和同步要求苛刻。我的建议是如果 AOA 和 TOF 来自同一块板子、共用时钟优先紧耦合如果来自不同设备、时间戳对不齐老老实实松耦合否则融合出来的轨迹会比单模还差。下面这张表是两种方式的对比维度松耦合紧耦合实现难度低高时钟同步要求帧级即可采样级精度上限中高调试难度低可单独验证高问题定位难适用场景多设备异构单板同源3. 用 Python 跑通 AOATOF 最小融合链路3.1 环境准备与依赖先建一个干净环境依赖只装必要的避免版本冲突。numpy 做矩阵运算scipy 做滤波matplotlib 只用来画轨迹验证。python -m venv venv source venv/bin/activate pip install numpy scipy matplotlib不要一上来就装一堆定位框架最小链路自己手写一遍才知道每一步在算什么。等链路通了再考虑引入现成的滤波库。3.2 生成模拟 AOA 与 TOF 观测没有硬件也能验证算法先造数据。下面这段代码模拟一个目标沿直线运动每帧产生带噪声的角度和距离观测。import numpy as np np.random.seed(42) def simulate_track(n_frames200, dt0.1): # 目标从 (1,1) 匀速运动到 (5,5)单位米 t np.arange(n_frames) * dt true_x 1.0 4.0 * t / t[-1] true_y 1.0 4.0 * t / t[-1] # 基站放在原点 true_range np.sqrt(true_x**2 true_y**2) true_aoa np.arctan2(true_y, true_x) # 弧度 # 加噪声AOA 标准差 3 度TOF 标准差 0.3 米 aoa_meas true_aoa np.random.normal(0, np.deg2rad(3), n_frames) tof_meas true_range np.random.normal(0, 0.3, n_frames) return true_x, true_y, aoa_meas, tof_meas true_x, true_y, aoa_meas, tof_meas simulate_track() print(首帧 AOA(度):, np.rad2deg(aoa_meas[0])) print(首帧 TOF(米):, tof_meas[0])逻辑说明true_aoa用 arctan2 算真实角度噪声用高斯分布叠加AOA 噪声给 3 度、TOF 给 0.3 米这两个量级接近真实 WiFi 设备的水平。参数n_frames控制轨迹长度dt是帧间隔改这两个值可以模拟不同运动速度。注意角度单位全程用弧度只在打印时转成度混用单位是新手最常见的翻车点。3.3 单帧解算射线与圆求交有了角度和距离单帧位置就是极坐标转直角坐标def single_frame_solve(aoa, tof): x tof * np.cos(aoa) y tof * np.sin(aoa) return x, y x_raw, y_raw single_frame_solve(aoa_meas, tof_meas) err_raw np.sqrt((x_raw - true_x)**2 (y_raw - true_y)**2) print(单帧解算平均误差(米):, err_raw.mean())这段代码把每帧的 AOA 和 TOF 直接转成坐标。跑出来平均误差大概在 0.3~0.5 米但你会看到轨迹毛刺很多相邻帧之间跳变明显。这就是为什么需要跟踪器——单帧解算只是「测量」不是「轨迹」。参数上如果 TOF 噪声更大误差会线性放大如果 AOA 噪声大在远距离处误差会被距离放大因为角度误差乘以距离等于横向位置误差。3.4 卡尔曼滤波把毛刺磨平用一个匀速CV模型的卡尔曼滤波把轨迹串起来。状态量取 [x, y, vx, vy]观测量取 [x, y]。from scipy.linalg import block_diag def kalman_track(z_x, z_y, dt0.1, q0.01, r0.5): n len(z_x) # 状态转移矩阵 F np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # 观测矩阵 H np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) # 过程噪声 Q np.eye(4) * q # 观测噪声 R np.eye(2) * r x np.zeros(4) P np.eye(4) est np.zeros((n, 2)) for i in range(n): # 预测 x F x P F P F.T Q # 更新 z np.array([z_x[i], z_y[i]]) y z - H x S H P H.T R K P H.T np.linalg.inv(S) x x K y P (np.eye(4) - K H) P est[i] x[:2] return est est kalman_track(x_raw, y_raw) err_kf np.sqrt((est[:,0] - true_x)**2 (est[:,1] - true_y)**2) print(卡尔曼滤波后平均误差(米):, err_kf.mean())逻辑说明F是匀速模型的状态转移H只观测位置。q是过程噪声越大越信任观测、轨迹越跟手但越抖r是观测噪声越大越信任模型、轨迹越平滑但滞后越明显。这两个参数是调参的核心q0.01、r0.5是我在 0.3 米观测噪声下的常用起点。跑完你会看到平均误差降到 0.2 米左右且轨迹连续。注意P的初始化不要给太小否则滤波器前期过度自信收敛慢。4. 避坑与排查AOATOF 融合里最容易翻车的五件事4.1 角度跳变相位模糊没处理干净现象轨迹在某些角度附近突然跳到镜像位置比如目标在 30 度解算出来是 150 度。原因阵元间距大于 λ/2或者解模糊算法在低信噪比下选错了分支。解决先确认 d λ/2再在解算后加一个角度连续性检查——如果当前帧角度和上一帧差超过阈值比如 60 度标记为可疑帧用上一帧角度外推值替代等下一帧确认后再恢复。4.2 距离漂移时钟没对齐现象静止目标的 TOF 测距缓慢增大或减小几秒内漂移几十厘米。原因双方时钟频率有偏差双边测距虽然抵消了固定偏差但抵消不了频率漂移。解决定期做零距离校准或者在滤波器里把距离加一个缓慢变化的偏置状态一起估计。我一般会在状态量里加一个bias项让滤波器自己学这个漂移。4.3 融合后比单模还差噪声模型给错了现象松耦合融合出来的轨迹误差比单用 TOF 还大。原因AOA 和 TOF 的噪声量级差太多加权时没按方差分配权重角度噪声被过度信任。解决在融合前先分别统计两个观测量的实际方差用方差倒数做加权。卡尔曼滤波里就是设对R矩阵AOA 转成位置后的等效方差要按距离平方缩放。4.4 轨迹滞后过程噪声太小现象目标转弯时轨迹切内角跟不上真实路径。原因q给太小滤波器过度信任匀速模型。解决把q调大或者换成交互式多模型IMM用匀速和匀加速两个模型并行按似然度切换。IMM 实现复杂一些但对机动目标效果明显。4.5 多目标串轨数据关联没做现象两个目标靠近时轨迹互换分开后各自跟错人。原因只做了滤波没做数据关联最近邻匹配在交叉点必然出错。解决引入匈牙利算法做帧间关联或者用 JPDA 做概率关联。简单场景下给每个目标维护一个门限只关联门限内的观测也能缓解大部分串轨。5. 进阶把 AOATOF 跟踪做成可复用的工程模块链路跑通后下一步是把它变成能复用的模块。我习惯把整个流程拆成三层测量层、解算层、跟踪层。测量层负责从硬件拿原始相位和时间戳输出统一的Measurement结构解算层做 AOA 和 TOF 的独立解算与质量评估跟踪层只吃解算结果不关心底层是什么硬件。这样换硬件时只改测量层算法层不动。验证方法上除了看平均误差更要看误差的分布和时序。我一般会画三张图误差随时间的曲线、误差随距离的曲线、误差随角度的曲线。如果误差随距离线性增长说明角度噪声是主因如果误差在某些角度区间突然变大说明阵列在那个方向有盲区。这些图比一个平均误差数字有用得多。一个具体技巧在跟踪器输出端加一个「新息门限」监控。新息是观测减去预测的残差如果连续多帧新息超过 3 倍标准差说明要么目标机动了要么测量出错了。这时候可以自动降低观测权重或者触发重新初始化。这个机制帮我挡掉过很多次因为硬件偶发丢包导致的轨迹飞点。还有个习惯所有参数都写进配置文件不要硬编码在代码里。q、r、门限、阵元间距这些不同场景要调硬编码改一次编译一次效率太低。配置文件加注释写清楚每个参数的物理含义和调整方向三个月后回来看还能看懂。最后说个血泪教训别在没做时间同步的情况下上紧耦合。我曾经把两个不同设备的数据直接丢进一个滤波器结果轨迹比单模还差排查了两天才发现时间戳差了 40 毫秒。后来老老实实先做帧级对齐再逐步收紧。融合的前提是同步同步没做好融合就是负优化。希望帮到你。本文还有配套的精品资源点击获取