企业官网建设流程全解析

简介：一份面向UWB定位算法研究与实践的Matlab脚本集合，完整覆盖扩展卡尔曼滤波、无迹卡尔曼滤波、泰勒级数位置估计、三边测量与多边测量五种典型算法。资源以状态空间模型为基础，同时给出恒速运动模型与恒加速度运动模型作为状态方程，适用于静态、动态小尺度视距、动态大尺度视距以及体育馆室内等多种定位场景，便于对比分析各算法在不同条件下的定位误差、收敛速度和稳定性。压缩包内文件总数达81个，包含二十六个m格式源码脚本、三十三个txt格式原始观测数据、十九个mat格式实验数据文件，以及许可证、说明文档等辅助材料，整体大小约三十兆字节，目录中示例脚本与辅助函数分离，结构清晰，适合按模块查阅和二次开发。已有三千三百三十三人学习下载，附有多组对比与演示脚本，可直接运行观察五种算法在UWB定位中的具体表现，快速掌握从运动建模、测量更新到位置估算的完整实现思路，是研究高精度室内定位技术的一份扎实参考。

1. UWB多点定位的五条解算路线，先分清输入输出

UWB定位最容易让新手迷失的不是硬件部署，而是同一组测距数据在不同的坐标解算方法里，最终误差能差出一个数量级。这套 Matlab 脚本把五种常见定位算法——扩展卡尔曼滤波、无迹卡尔曼滤波、泰勒级数法、三边测量、多点定位——放进同一个框架里，并附带了静态 LOS、动态小尺度 LOS、动态大尺度 LOS 以及体育馆实测的 UWB 测距数据。对正在做课程设计、算法选型或想评估自研定位方案的人来说，它等于一份可对照真值（Vicon 数据）反复验证的基准实现。无论你是想快速跑通两种经典多点定位算法，还是要对比 EKF 和 UKF 在运动轨迹平滑上的差异，这套脚本都能直接给出可复现的对比结果。

2. 从测距方程到状态空间模型，五种 UWB 定位算法如何取舍

2.1 三边测量与多点定位的观测方程

UWB 定位的底层输入是标签到各基站的距离集合。设第 i 个基站坐标为(xi, yi)，标签坐标(x, y)，则观测方程为：

(x - xi)^2 + (y - yi)^2 = di^2

三边测量取其中三个基站直接解方程；当基站数多于三个时，方程变成超定组，就是多点定位。这个仓库里的performTrilateration.m和performMultilateration.m把这两种做法拆成了独立函数，便于在实跑时替换核心解算模块。

% performMultilateration 中的线性最小二乘构造（常见写法） % 以第一个基站作为参考减掉一次非线性项 A = 2 * [x2 - x1, y2 - y1; x3 - x1, y3 - y1; x4 - x1, y4 - y1]; b = [d1^2 - d2^2 + x2^2 + y2^2 - x1^2 - y1^2; d1^2 - d3^2 + x3^2 + y3^2 - x1^2 - y1^2; d1^2 - d4^2 + x4^2 + y4^2 - x1^2 - y1^2]; p_ls = (A' * A) \ (A' * b); % 无权重最小二乘解

这里 A 矩阵第二列是各基站相对于参考基站的坐标差，b 向量来自距离平方差与坐标平方差的组合。代码里加diag(weights)就变成加权最小二乘，权重通常取测距噪声方差的倒数，常见的做法是用getWeight.m根据距离反算权重，距离越远权重越低，因为 UWB 在远距离下的测距误差往往更大。

2.2 CV/CA 状态模型如何与 EKF/UKF 组合

仓库里明确提到 UWB 系统被假定为状态空间模型，这直接影响 EKF 和 UKF 的实现方式。恒速（CV）模型的状态向量是[x, y, vx, vy]，恒加速度（CA）模型再叠加ax, ay。状态转移函数由citrackStateFcn.m定义，注释里能看出它同时服务 EKF 和 UKF 两条链路。

function x_next = citrackStateFcn(x, dt) % 恒速度 CV 模型状态转移，状态排列 [x; y; vx; vy] x_next = x; x_next(1) = x(1) + x(3) * dt; x_next(2) = x(2) + x(4) * dt; % vx、vy 保持不变，对应匀速直线运动 end

EKF 需要这个函数提供状态转移，同时调用citrackStateJacobianFcn.m得到状态雅可比矩阵；UKF 不需要求雅可比，直接用无迹变换把 sigma 点经过同一个citrackStateFcn传播，再交回citrackMeasurementFcn.m计算观测残差。这个仓库的设计对初学者很友好：想换成 CA 模型，只需替换初始化函数为initConstAcceleration_KF.m，再修改citrackStateFcn增加加速度项，滤波主体不用动。

2.3 泰勒级数法在处理非线性残差时的不同逻辑

泰勒级数法做定位和滤波器思路完全不同。它不维护状态协方差，而是以当前估计位置为展开点，把测距方程线性化，反复求解位置修正量直到收敛。

for iter = 1:10 % 在当前估计位置 (px, py) 处计算各基站预测距离 h = sqrt((px - ax).^2 + (py - ay).^2); r = d_meas - h; % 观测距离与预测距离的残差 J = [-(ax - px) ./ h, -(ay - py) ./ h]; delta = (J' * J) \ (J' * r); px = px + delta(1); py = py + delta(2); if norm(delta) < 1e-3 break; end end

这段代码里 J 是观测函数对位置的雅可比矩阵，delta 是坐标修正量。每次迭代都会把新坐标代回重新计算，直到收敛。初始值如果离真实位置太远，迭代可能发散，所以仓库里常见的处理是先跑一次三边测量或多点定位得到初始值，再交给泰勒级数迭代。这也是为什么demo_Taylor_series_algo.m通常需要和performMultilateration.m配合调用。

3. Matlab 脚本仓库的主干函数与关键实现剖析

3.1 从 demo 脚本看五种算法的启动方式

仓库的文件命名很直白，每个算法对应一个 demo：demo_EKF_algo.m、demo_UKF_algo.m、demo_Taylor_series_algo.m、demo_trilateration_algo.m、demo_multilateration_algo.m。在 Matlab 里执行时，先要把helper_functions路径加进来，否则会报函数找不到。

addpath('helper_functions'); run('demo_multilateration_algo.m');

addpath是把工具函数目录加入工作路径，run执行脚本。脚本内部会通过importfile_Ranges.m读取UWB_data_Ranges下的距离数据，再调用对应算法。如果你在别的项目里复用了某个算法，只需要保留helper_functions和你用到的那个 demo 脚本，其他部分可以删掉。

3.2 performTrilateration 与 performMultilateration 的区别

这两个函数属于静态定位核心，是整份资源里最容易提炼出来复用的部分。三边测量只接受 3 个基站的距离输入，输出是唯一坐标；多点定位接受 N 个基站，内部把它们转成超定方程组求解。

function pos = performMultilateration(anchors, ranges, weights) % anchors: N*2 基站坐标 % ranges: N*1 测距结果 % weights: N*1 可选权重向量 A = buildLinearMatrix(anchors); b = buildRangeVector(anchors, ranges); if nargin < 3 || isempty(weights) weights = ones(size(ranges)); end pos = (A' * diag(weights) * A) \ (A' * diag(weights) * b); end

这段代码把线性化矩阵构建单独封装成buildLinearMatrix和buildRangeVector，便于检查 A 和 b 的中间结果。实际调试时，建议先打印 A 和 b 的数值，看看是否出现矩阵病态。基站坐标如果近似共线，或者标签落在基站围成的凸包外，A 的条件数会很大，解出来的位置会跳变。这是多点定位失效的常见原因，不是算法代码本身的问题。

3.3 EKF/UKF 链路：从 initConstVelocity 到 citrackMeasurementFcn

如果查看demo_EKF_algo.m，会发现滤波器初始化封装在initConstVelocity_KF.m或initConstAcceleration_KF.m里。初始协方差、过程噪声 Q 和量测噪声 R 都会在这里设置。接着创建trackingEKF对象，传入状态转移函数、观测函数以及对应雅可比。

% 初始状态 [x; y; vx; vy]，来自 resource 里初始化函数的输出 ekf = trackingEKF(@citrackStateFcn, @citrackMeasurementFcn, ... initState, ... 'StateTransitionJacobianFcn', @citrackStateJacobianFcn, ... 'MeasurementJacobianFcn', @citrackMeasurementJacobianFcn);

这里trackingEKF需要 System Identification Toolbox 或 Sensor Fusion and Tracking Toolbox。如果不具备工具箱，可以手写 EKF 的状态更新和协方差更新，核心公式就是预测、计算卡尔曼增益、更新状态、更新协方差这四步。UKF 的 demo 则用trackingUKF或unscentedKalmanFilter对象，观测函数仍然复用citrackMeasurementFcn.m。这就是仓库结构比较聪明的地方：同一套观测函数，EKF 和 UKF 共用，避免维护两套测量模型。

3.4 泰勒级数脚本如何避免迭代发散

demo_Taylor_series_algo.m内部一般先用三边测量产生初值，再用迭代修正。如果你拿到的数据是动态运动轨迹，建议每个时间戳都以上一帧的泰勒结果为初值，而不是每帧都重新从零迭代。否则遇到测距毛刺，迭代可能跳到错误收敛点。

4. 跑通对比实验：静态 LOS、动态场景与体育馆实测

4.1 静态 LOS 对比脚本的输入输出

compare_five_algorithms_static_LOS_scenario.m是最容易跑通的入口。它会读入静态 LOS 场景下的 UWB 测距数据，然后依次执行五种算法，在同一个坐标系里画散点或轨迹对比。

% 一键运行静态对比 results = compare_five_algorithms_static_LOS_scenario;

这个脚本不返回显著结果时，通常是因为当前工作目录不在仓库根目录。建议先用dir查看文件是否可见，再把仓库目录设为当前文件夹。静态场景里五种算法的差距一般不大，重点看 Taylor 级数和多点定位是否收敛到同一区域。

4.2 小尺度与大尺度动态 LOS 场景的参数差异

compare_five_algorithms_dynamic_small_scale_LOS_scenario.m和compare_five_algorithms_dynamic_large_sclae_LOS_scenario.m的差别在于基站布局范围与目标运动速度。小尺度场景基站间距通常只有几米，运动速度较低；大尺度场景基站间距可能到十几米甚至几十米，标签移动速度也更快。

在大尺度场景下，EKF 的 CV 模型会出现明显的轨迹滞后，换成initConstAcceleration_KF.m后能改善匀速直线段的跟随效果。修改方式是在 demo 脚本的初始化位置替换函数名，不需要改动滤波器链路。

4.3 用 Vicon 真值数据评估定位轨迹

仓库里Vicon_mat文件夹保存了 Vicon 动捕系统的位置真值，Sporthall_data_Ranges则保存体育馆实测 UWB 测距值。compare_EKF_UKF_Multilat_TS_Trilat_Vicon_demo_lab.m专门做真值对比，跑完后会计算每个算法的均方根误差和最大位置误差。这个脚本是评估算法优劣最重要的工具，因为只有与真值对齐，才能判断是状态模型的问题还是测距噪声的影响。注意 UWB 数据和 Vicon 数据的采样率往往不同，脚本里通常需要按时间戳做插值对齐，你可以检查它是否用了interp1或resample。

5. 在 Matlab 里做 UWB 定位算法扫掠的进阶技巧

5.1 统一时间戳与采样率

对比 UWB 和 Vicon 轨迹的第一步不是算误差，而是把时间戳对齐。UWB 的测距输出通常是几十赫兹，Vicon 动捕可到 100～300 赫兹。常见做法是以 UWB 时间戳为基准，对 Vicon 数据做线性插值：

vicon_interp_x = interp1(vicon_t, vicon_x, uwb_t, 'linear');

vicon_t是 Vicon 原始时间戳，uwb_t是 UWB 数据时间戳。插值后计算误差才有意义，否则时间偏差会直接折算成位置误差。

5.2 用函数句柄把五种算法放进同一个循环

想批量对比算法在不同噪声方差下的表现，可以写一个统一接口，把五个 demo 里的核心函数打包成句柄：

algos = {@EKF_solve, @UKF_solve, @Taylor_solve, @Trilat_solve, @Multilat_solve}; for i = 1:length(algos) err = run_loss(algos{i}, data); fprintf('algorithm %d RMSE: %.3f m\n', i, err); end

这样可以在同一份测试数据上反复跑，避免每个 demo 单独修改输出格式。

5.3 定位发散时先调 Q 和 R

EKF 和 UKF 如果出现轨迹抖动，先不要动算法结构，而是检查过程噪声协方差 Q 和量测噪声协方差 R。Q 设置过大会让滤波器过度相信测距观测，轨迹噪声明显；R 设置过大则轨迹过度平滑，在转弯处出现滞后。一个快速经验值是先用测距传感器的标准差平方作为 R，再用 0.1 倍的 R 作为 Q 的底数，跑通后再根据误差曲线微调。把demo_EKF_algo.m里的初始协方差矩阵拆出来，设成可调变量，是调试这类滤波器的关键一步。

本文还有配套的精品资源，点击获取