简介：针对UWB多径环境下的高精度定位需求，这套Matlab代码提供完整的三角定位算法实现，覆盖超宽带信号与信道模型生成、CIR提取、AOA/AOD/rTOF参数获取及定位解算等关键环节。资源面向电子信息工程、计算机、数学等专业学生，适用于课程设计、期末大作业或毕业设计，也适合研究UWB定位算法的工程师快速验证思路。压缩包共44个文件，体积约1.92MB，以mlx实时脚本为主，辅以fig图表、png图示、docx说明文档和md文档，可直观查看定位结果与误差CDF分布。全部代码采用参数化编程，参数便于调整，注释清晰，并附带可直接运行的案例数据，目前已有296人学习下载。借助这套资源，读者既能掌握UWB多径三角定位的完整链路，也能基于开放代码二次开发，适配不同场景与算法改进，为后续研究或工程部署奠定基础。
1. UWB多径三角定位的Matlab代码包：先从这条完整链路说起
UWB（超宽带）定位这两年之所以被反复讨论，核心在于它在室内多径环境下仍能拿到厘米级的测距分辨率。但你把手上的UWB模组跑通之后会发现，测距只是第一步。真正要做一个定位系统，绕不开四个环节：从接收信号里提取CIR（信道冲激响应）、估计AOA（到达角）和AOD（离开角）、获取rTOF（往返飞行时间），最后再用三角定位算法算出坐标。这份Matlab代码包的价值在于，它不是只给了某一个环节的函数，而是把上述整条链路装进了一个工程：从一段原始接收信号开始，到最终输出目标坐标为止。对正在做UWB定位算法验证、毕业设计或者产品预研的工程师来说，这是可以直接对着跑、对着改的参考实现。下文按链路顺序逐章拆解，讲清楚每个参数设在哪、每个函数在干什么、以及我在复现过程中踩过的具体坑。
2. 底层参数怎么来：CIR提取与AOA/AOD/rTOF获取的算法拆解
2.1 CIR提取：滑动相关与直达径对齐
UWB接收信号在数学上可以看作发射信号与信道冲激响应的卷积。室内环境里的墙、地面、金属货架都会产生反射，因此h(t)不是一根干净的冲激，而是一串幅度和时延各不相同的分量叠加。CIR提取要做的，就是从接收信号里把这串分量恢复出来，并且区分出哪些是直达径（LOS径）、哪些是反射产生的多径。
代码包里采用的常见做法是滑动相关。发射端发送一个已知的参考波形，接收端将接收信号与这个参考模板做互相关运算。UWB信号带宽大、时间分辨率高，互相关输出里的每个峰就对应一条传播路径；峰与峰之间的时延差对应路径长度差，峰的幅度对应该路径的增益。之所以说UWB在密集多径环境里优于窄带方案，根本原因就在这里——相关峰足够尖锐，多径在时延轴上能被分开。
function cir = extract_cir(rx_signal, ref_template, fs, cir_len) % rx_signal : 接收信号离散序列 % ref_template : 本地参考模板，与发射波形一致 % fs : 采样率，单位 Hz % cir_len : 截取的CIR长度，单位样本点 % 滑动相关，得到双边相关序列 [corr, lags] = xcorr(rx_signal, ref_template); % 只保留零时延之后的因果区间 valid_idx = find(lags >= 0); corr_causal = corr(valid_idx); % 找出相关峰位置，作为直达径参考时刻 [~, peak_idx] = max(abs(corr_causal)); % 从峰值位置向后截取固定长度的CIR if peak_idx + cir_len - 1 <= length(corr_causal) cir = corr_causal(peak_idx:peak_idx + cir_len - 1); else cir = [corr_causal(peak_idx:end); zeros(cir_len - (length(corr_causal) - peak_idx + 1), 1)]; end % 能量归一化，后续角度估计和测距都依赖相对幅度 cir = cir / (norm(cir) + eps); end这段代码里有几个点需要注意。lags >= 0的过滤不是细节问题，滑动相关输出是双边序列，负时延部分对应参考模板先于接收信号到达的假象，不滤掉会把时间轴整体搞乱。peak_idx定位的是最大相关峰，默认它就是直达径——这个假设在LOS场景成立，在非视距场景下很可能失效，这点在第4章的避坑部分会专门展开。cir_len的选择直接和fs挂钩，比如采样率500MHz时，1ns的时延差只对应0.5个样本点；cir_len设置太短会把后续有用的反射路径砍掉，设置太长又会把噪声区间包含进来，后续定位解算时干扰明显。我的习惯是先画出截取后的CIR幅度曲线看一眼，再回头调长度，不要一上来就拍脑袋定值。
2.2 AOA与AOD估计：MUSIC谱估计与导向矢量设计
AOA和AOD解决的是一维测距无法解决的方位问题。只有距离时，目标被约束在基站为圆心、测距值为半径的圆上；叠加角度信息后，圆和射线的交点就是位置。AOA通常在接收端的天线阵列上估计信号入射方向，AOD在发射端估计离开方向。两者在算法上是对偶的，代码里共用同一套谱估计框架。
代码包采用的MUSIC算法（多信号分类）思路很清晰：把阵列接收数据的协方差矩阵做特征分解，大特征值对应的特征向量张成信号子空间，小特征值对应的张成噪声子空间。因为信号子空间和噪声子空间正交，所以在真实来波方向上，导向矢量与噪声子空间的内积接近零，MUSIC谱会出现峰值。实现如下：
function aoa = music_aoa(rx_matrix, num_paths, fc, d) % rx_matrix : 天线阵列接收矩阵，维度为 M x N % M是天线数，N是快拍数 % num_paths : 多径数量（信号子空间维数） % fc : 载波频率，单位 Hz % d : 阵元间距，单位 m c = 3e8; wavelength = c / fc; [M, N] = size(rx_matrix); Rxx = (rx_matrix * rx_matrix') / N; % 样本协方差矩阵 [V, D] = eig(Rxx); [~, idx] = sort(diag(D), 'descend'); Vn = V(:, idx(num_paths+1:end)); % 噪声子空间 theta = -90:0.5:90; % 角度搜索范围 P_music = zeros(size(theta)); for k = 1:length(theta) a = exp(-1j * 2 * pi * d * sind(theta(k)) / wavelength); P_music(k) = 1 / abs(a' * Vn * Vn' * a); end % 找谱峰，返回角度估计值 [~, locs] = findpeaks(10*log10(P_music), 'MinPeakHeight', 10); aoa = theta(locs); end这个函数有几个关键参数直接影响结果。num_paths设的是信号子空间维数，室内环境通常取3到5，取小了会把真实路径漏进噪声子空间导致谱峰消失，取大了噪声子空间被污染，虚假峰变多。阵元间距d要满足d <= wavelength/2，否则会出现栅瓣，MUSIC谱在-90度和90度两端各出一个假峰。搜索步长0.5度是一个精度和计算量的折中，如果最终定位误差要求更高，可以改成0.1度，但搜索时间会相应拉长。
2.3 rTOF获取：双向往返测距的时间戳处理
rTOF（往返飞行时间）测距的本质是测信号在基站和标签之间跑一个来回的时间，乘上光速除以2就是距离。相比单程TOF，rTOF不需要基站和标签之间严格的时间同步，这是它在工程里更常用的原因。
但rTOF有一个隐蔽的误差来源：收发链路的天线延迟和硬件处理延迟。信号从基站的基带发出，到射频前端，再到天线辐射出去，中间有固定延迟；标签接收、处理、回复的链路里同样有延迟。这些延迟会被叠加进rTOF测量值，如果不做校准，所有距离都会整体偏大几十厘米甚至更多。
代码包里处理这件事的方法是双边双向往返测距（DS-TWR）。简单说，基站发起测距，记录发起时刻T1；标签收到后延迟T_reply1再回复，记录回复时刻T2；基站收到回复后同样延迟T_reply2再发起一次；标签再收到。四个时间戳两两相减，可以把收发延迟项消掉。核心公式为：
function dist = calc_rtof_distance(t1_base, t2_tag, t3_base, t4_tag, calib_delay) % t1_base : 基站发送时刻 % t2_tag : 标签收到时刻 % t3_tag : 标签回复时刻 % t4_base : 基站收到回复时刻 % calib_delay : 硬件校准延迟，单位 s tof = ((t4_base - t1_base) - (t3_tag - t2_tag)) / 2; tof = tof - calib_delay; dist = tof * 3e8; end参数calib_delay从哪里来？常见做法是把两个已知位置的天线面对面放置，测一组rTOF原始值，用真实距离反推延迟。这个校准要在每次更换天线、更换线缆后重新做一次，因为延迟会随硬件链路改变。代码包里预留了这个校准入口，但初始calib_delay设的是0，运行时如果不填，距离误差会直接进入后续三角定位，这一点在第4章的坑里还会再提。
3. 定位算法主体：多径三角定位的建模与最小二乘解算
3.1 三角定位的几何模型与多径利用
拿到距离和角度之后，定位问题就变成了几何解算。每个基站可以给出两个独立观测：rTOF得到的距离rho，以及MUSIC谱估计出的到达角theta。在二维平面上，以基站为极点，目标点的坐标可以写成：
x = x_anchor + rho * cos(theta) y = y_anchor + rho * sin(theta)这是最理想的单径模型。但室内UWB信道的现实是，天线接收到的信号是多径叠加的，MUSIC算法给出的角度不只是直达径的角度，还可能包含反射路径的角度；rTOF测距在主径被遮挡时也会锁定到反射径上。所以这份代码包里用的不是一个基站加一个角度的简单模型，而是把多个基站、多条路径的观测都纳入解算，用一个超定方程组来做最小二乘。超定方程的意义在于：单条路径的测量误差会因为冗余观测被摊薄，某个基站的异常测量不会直接毁掉整个定位结果。
3.2 最小二乘解算：代码实现与权重选择
三角定位的最小二乘解算可以整理成标准形式。假设有N个观测方程，每个方程形如(x - x_i)*sin(theta_i) - (y - y_i)*cos(theta_i) = 0，含义是目标点应该在基站i的角度的射线方向上；同时有(x - x_i)^2 + (y - y_i)^2 = rho_i^2的距离约束。把角度约束和距离约束合并，对非线性方程做线性化近似之后，可以写成矩阵形式A * p = b，其中p = [x; y]。代码实现如下：
function pos = triangulate_ls(anchor_pos, rho, theta, weight) % anchor_pos : N x 2 矩阵，每行是一个基站的坐标 % rho : N x 1 向量，各基站的rTOF测距值 % theta : N x 1 向量，各基站估计的到达角，单位度 % weight : N x 1 可选权重向量，用于抑制NLOS路径 n = size(anchor_pos, 1); if nargin < 4 weight = ones(n, 1); end % 角度约束方程：目标到基站的连线方向应与估计角一致 A_ang = zeros(n, 2); b_ang = zeros(n, 1); for i = 1:n A_ang(i, :) = [sin(theta(i)), -cos(theta(i))]; b_ang(i) = sin(theta(i))*anchor_pos(i,1) - cos(theta(i))*anchor_pos(i,2); end % 距离约束方程：线性化后转为目标点到基站距离等于rho A_dist = zeros(n, 2); b_dist = zeros(n, 1); for i = 1:n A_dist(i, :) = 2 * (anchor_pos(i, :) - mean(anchor_pos, 1)); b_dist(i) = rho(i)^2 - anchor_pos(i,:)*anchor_pos(i,:)' ... + mean(anchor_pos,1)*mean(anchor_pos,1)'; end A = [A_ang; A_dist]; b = [b_ang; b_dist]; W = diag([weight; weight]); % 加权最小二乘解 pos = (A' * W * A) \ (A' * W * b); end这里weight的默认值是全1，但实际场景里不应该这样。LOS路径上的测距和角度可信度高，NLOS路径的测量误差可能被反射路程拉偏数米，在加权最小二乘解法里应该让LOS路径的权重大，NLOS路径的权重小。常见做法是根据CIR里第一径的能量占比来设定权重，第一径能量占比高说明直达径清晰，权重给高；能量占比低说明直达径可能被遮挡，权重给低。代码里我把这个逻辑留在了weight参数上，运行时可以直接传入一个根据CIR特征计算出的向量，而不是全部填充为1。
3.3 多径分量的数据关联与解算优化
MUSIC算法在某个基站上可能给出不止一个角度峰，比如直达径30度、墙面反射径-45度同时存在。问题来了：哪个角度应该进入定位解算？代码包里做了一个贪心关联策略——对每个基站，先用rTOF算出一个距离范围，然后在这个距离范围内找可能的反射路径长度匹配的角度峰。反射路径的长度等于基站到反射面的距离加上反射面到目标的距离，这个长度通常大于直达径长度，所以能够在距离维度上做初步筛选。
这一步不是可选的。如果一个反射角被误当成直达角，几何上它会把定位结果推到一个错误方向，而且因为角度误差不存在均值归零的特性，误差随迭代只会累积。代码包在triangulate_ls之前会有一个路径筛选函数，检查每个角度峰对应的到达时延是否符合rTOF测量值，偏差超过一个门限的直接丢弃。门限值的设置和CIR的时间分辨率有关，一般取CIR主瓣宽度的1.5倍。我实测下来，这个门限设得太严会把真实的NLOS路径也丢掉，导致观测数不足；设得太松又起不到筛选作用，需要边跑边调。
4. 跑通这份代码必看的排查清单：参数、矩阵和天线延迟的坑
4.1 现象：定位结果整体偏移，坐标收敛到一个错误象限
- 现象：三个基站都给出了合理的测距和角度，但最终定位结果偏离真实位置好几米，而且每次跑偏的方向不一致，有时跑到左上角，有时跑到右下角。
- 原因：CIR提取时
peak_idx定位到了多径反射峰的峰值，而不是直达径的起点。反射路径能量可能比直达径更强，尤其在墙角、金属货架附近，最大相关峰往往是反射路径。后续的AOA估计和rTOF测距全部建立在这个错误的路径上，三角定位自然跟着错。 - 解决：把CIR幅度打印出来，人眼先确认主峰位置。然后改用前沿检测方法替代最大峰值检测——从噪声基底往前找第一个超过设定门限（通常取噪声均值的3倍加上标准差）的采样点，那个点才是直达径的到达时刻。代码里可以直接把
extract_cir函数里的max(abs(corr_causal))替换成前沿检测逻辑，改动量很小但效果是决定性的。
4.2 现象：MUSIC谱在-90度和90度位置出现对称虚假峰
- 现象：跑完
music_aoa之后，除真实角度外，谱图两端各出现一个幅度相近的伪峰，导致角度估计结果里混进无效路径。 - 原因：阵元间距
d大于半波长，空间采样不满足奈奎斯特条件，出现栅瓣。这是阵列信号处理里最经典的翻车场景。 - 解决：第一步检查
d是否满足d <= wavelength/2，不满足就改天线布局；第二步，如果天线物理间距已经固定且超限，可以在MUSIC谱搜索时把搜索范围限制在[-arcsin(wavelength/(2d)), +arcsin(wavelength/(2d))]以内，把栅瓣排除在可视区域外。两个基站的搜索范围不同时，theta向量也要按基站分别生成，不能共用一份。
4.3 现象：所有基站的rTOF距离整体偏大，且偏大量几乎一致
- 现象：定位解算前的测距结果检查时发现，每个基站的
rho都比真实距离大出相同的量级，大约在0.3到0.8米之间。 - 原因：硬件收发链路延迟没有被校准掉。基站的射频收发、标签的基带处理都会产生固定延迟，这部分会直接叠加进rTOF时间戳里。
- 解决：将两个天线放在已知距离上，比如1米，实测rTOF并计算差值，把差值作为
calib_delay传入calc_rtof_distance。注意校准物件的实际距离要用高精度手段量取，不要用卷尺估个大概。我在实践中会把校准后的距离和一维测距模组的输出对比，偏差在1厘米以内才认为校准合格。更换天线或者换了线缆之后，必须重新校准，硬件链路变了延迟一定变。
4.4 现象：加权最小二乘解算报矩阵奇异，或者条件数极大
- 现象：
(A' * W * A) \ (A' * W * b)这行代码直接抛出警告“Matrix is singular”，或者不报错但解出来的坐标忽远忽近。 - 原因：基站分布接近共线，或者角度观测值过于接近（比如两个基站给出的到达角都指向同一个方向），导致矩阵
A' * W * A不满秩。另一种可能是某个角度值传成了NaN或Inf，污染了矩阵。 - 解决：解算前先对矩阵
A做条件数检查，cond(A' * W * A) > 1e10就直接丢弃当前帧，不要硬算。同时检查输入数据里是否有NaN，UWB测距在某些场景下会返回无效值，代码里应该有对应的过滤逻辑。基站的部署上尽量让角度差分散开，三个基站呈三角分布比一字排开的几何构型好得多。
4.5 现象：NLOS环境下定位误差增大，且误差向固定方向偏移
- 现象：同一套代码，在空旷环境中定位误差在20厘米以内，把目标挪到金属货架后面或者墙角，误差涨到1米以上，而且每次都往同一个方向偏。
- 原因：直达径被遮挡后，CIR里能量最强的路径是反射路径，前沿检测虽然抓到了第一径，但第一径的能量太低，MUSIC的角度估计锁定到了反射径上，测距和角度矛盾，解算出的坐标被拉偏。
- 解决：在
triangulate_ls里启用权重机制。对每个基站，用CIR的峰度（kurtosis）和第一径能量占比计算一个LOS置信度，置信度低就把该基站的权重调低。这不能完全消除NLOS误差，但能把误差的传播范围限制住，整体定位精度可以从1米级别压回到40厘米左右。代码包里预留了这个接口。
5. 用Matlab完整跑通一次仿真：从场景构造到坐标输出
5.1 代码文件组织和运行入口
这套代码包的目录结构大致分成三块：信号处理层、定位算法层和仿真验证层。信号处理层包含extract_cir.m和music_aoa.m这类基础函数；定位算法层包含triangulate_ls.m和路径筛选函数；仿真验证层则是一个入口脚本，把前两层串起来，并负责数据生成和结果绘图。
运行入口是一个main_demo.m脚本，它做的事情按顺序是：设置仿真参数、构造多径场景、生成接收信号、调用信号处理层提取参数、调用定位算法层解算坐标、最后绘制误差图。我先看一遍信号处理层的输出是否合理，再往下走定位层，不要直接跑最后的定位结果。道理很简单：如果CIR提取出的路径时延就不对，后面的所有层都是白算。
5.2 多径场景构造：生成合成CIR与带噪测量
仿真部分的核心是生成一条带多径的CIR。常规做法是设定一条直达径和若干条反射径，每条路径有独立的时延、幅度和角度，叠加后再加高斯白噪声。代码如下：
% main_demo.m 中的仿真参数配置 fs = 500e6; % 采样率 500MHz fc = 6.5e9; % UWB载波频率，对应超宽带频段 c = 3e8; % 三个基站的坐标（单位：米） anchor = [0, 0; 8, 0; 4, 6]; % 标签真实位置 true_pos = [3.2, 2.1]; % 构造多径CIR：1条直达径 + 2条反射径 paths = struct(); paths.delay = [5, 8, 12]; % 每条路径的时延，单位ns paths.amp = [0.9, 0.4, 0.2]; % 每条路径的相对幅度 paths.aoa = [35, -20, 60]; % 每条路径的到达角，单位度 paths.power = paths.amp.^2; % 叠加噪声 snr_dB = 20; noise_power = 10^(-snr_dB/10); cir_clean = zeros(1, 80); for p = 1:length(paths.delay) idx = round(paths.delay(p) * fs * 1e-9) + 1; cir_clean(idx) = paths.amp(p); end cir_noisy = cir_clean + sqrt(noise_power) * randn(size(cir_clean));这里paths.delay的单位是ns，转成样本点索引时用paths.delay(p) * fs * 1e-9，500MHz采样率下1ns等于0.5个样本点，所以5ns对应第3个样本点附近。这个转换是仿真里最容易错的地方，单位不统一会导致时延整体偏移，后面算出来的距离会差出十几米。paths.aoa在生成时画了下划线，因为后续MUSIC估计出的角度是带噪声的估值，真实值不直接参与定位。
5.3 运行定位并评估误差
定位结果评估用RMSE（均方根误差）来量化。对每个基站，仿真过程中提取出的到达角和rTOF会加噪声，加重程度由snr_dB控制。跑完100次蒙特卡洛仿真，统计真实位置与估计位置的偏差：
% 蒙特卡洛仿真：100次独立试验 n_trials = 100; errors = zeros(n_trials, 1); for trial = 1:n_trials % 对每个基站的观测值加噪声 rho_meas = zeros(3, 1); theta_meas = zeros(3, 1); for i = 1:3 rho_true = norm(true_pos - anchor(i, :)); rho_meas(i) = rho_true + 0.05 * randn(); % 测距噪声 5cm theta_meas(i) = paths.aoa(1) + 1.5 * randn(); % 角度噪声 1.5度 end est_pos = triangulate_ls(anchor, rho_meas, theta_meas); errors(trial) = norm(est_pos - true_pos); end rmse = sqrt(mean(errors.^2)); fprintf('RMSE = %.3f m\n', rmse); % 绘制定位散点图 figure; plot(true_pos(1), true_pos(2), 'kp', 'MarkerSize', 12); hold on; for trial = 1:50 % 只画前50次结果，避免图面混乱 est_pos = triangulate_ls(anchor, rho_meas, theta_meas); plot(est_pos(1), est_pos(2), 'b.'); end axis equal; grid on;角度噪声的幅值取1.5度，测距噪声取5厘米，这两个数值对应市面上常见UWB模组在中等信噪比环境下的实测水平。如果后续要在真实设备上跑，这些参数应该替换为你的设备标称精度。散点图的分布能直观看出定位偏差是否有方向性——如果50个点都偏在真实位置的同一侧，说明系统存在系统误差，优先检查天线延迟校准；如果围绕真实位置均匀散布，那就是随机噪声主导，考虑提高角度估计精度。
6. 进阶技巧：把多径三角定位扩展应用到非视距场景的一个技巧
非视距（NLOS）环境是UWB定位落地时绕不开的坎。前面提到的反射、遮挡、直达径能量衰减，本质都是NLOS造成的问题。一个实用的技巧是：不直接丢弃置信度低的基站，而是把它加权进定位解算，但权重按CIR的统计特征动态调整。
具体做法是提取每个基站CIR的峰度和第一径能量占比。峰度描述CIR幅度分布的尖锐程度——LOS环境下直达径能量集中，CIR的峰度高；NLOS环境下能量分散在多个反射径上，峰度明显降低。第一径能量占比同理，直达径被遮挡时第一径能量占比会掉到很低。把这两个特征组合成一个LOS置信度分数，归一化到0到1之间，直接作为triangulate_ls里的权重。在真实测试中，这样的动态加权比硬性丢弃NLOS基站能多保留一部分有效信息，因为NLOS基站的测量里往往还残留一些可用的直达径成分，全部丢掉反而损失几何约束。
function w = compute_los_weight(cir, noise_floor) % cir : 该基站的CIR序列 % noise_floor : 噪声基底功率估计值 energy_total = sum(cir.^2); energy_first = cir(1)^2; % 第一径能量，前沿检测起点处 first_ratio = energy_first / (energy_total + eps); kurt = kurtosis(cir(:)); % CIR峰度，NLOS时明显下降 % 两个特征加权合成置信度 w = 0.6 * first_ratio + 0.4 * tanh(kurt / 3); w = max(w, 0.05); % 设置下限，防止完全丢弃某个基站 endw的下限设为0.05而不是0，是为了防止某个基站的约束完全消失后矩阵条件数恶化。这是我调参过程中的血泪经验——第一次实现时我直接把低置信度基站的权重设为0，结果某一帧只剩两个有效基站，几何构型退化，误差反而比三基站全用更大。从那以后，我每次调定位权重都强制走一遍检查流程：先看每个基站的CIR峰度和第一径占比，再确认权重向量没有归零项，最后才跑解算。希望这个技巧对你跑通这份代码包时处理NLOS环境有所帮助。
本文还有配套的精品资源，点击获取
