简介一份面向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 保持不变对应匀速直线运动 endEKF 需要这个函数提供状态转移同时调用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配合调用。算法名称是否需要状态先验主要计算方式典型适用场景EKF需要上一帧状态与协方差状态转移 观测雅可比动态轨迹跟踪线性化可接受时UKF需要上一帧状态与协方差sigma 点无迹变换强非线性测距模型、较大初始误差泰勒级数法需要初始坐标迭代最小二乘静态定位、无时间邻域依赖三边测量不需要直接解方程基站数恰好为 3 的简单定位多点定位不需要超定最小二乘基站数大于 3常规部署场景3. Matlab 脚本仓库的主干函数与关键实现剖析3.1 从 demo 脚本看五种算法的启动方式仓库的文件命名很直白每个算法对应一个 demodemo_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的差别在于基站布局范围与目标运动速度。小尺度场景基站间距通常只有几米运动速度较低大尺度场景基站间距可能到十几米甚至几十米标签移动速度也更快。参数小尺度 LOS大尺度 LOS基站间距38 米1040 米目标移动速度 1 m/s 1 m/s建议状态模型CVCA主要误差来源测距噪声几何精度因子GDOP与基站布局在大尺度场景下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 动捕可到 100300 赫兹。常见做法是以 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 和 REKF 和 UKF 如果出现轨迹抖动先不要动算法结构而是检查过程噪声协方差 Q 和量测噪声协方差 R。Q 设置过大会让滤波器过度相信测距观测轨迹噪声明显R 设置过大则轨迹过度平滑在转弯处出现滞后。一个快速经验值是先用测距传感器的标准差平方作为 R再用 0.1 倍的 R 作为 Q 的底数跑通后再根据误差曲线微调。把demo_EKF_algo.m里的初始协方差矩阵拆出来设成可调变量是调试这类滤波器的关键一步。本文还有配套的精品资源点击获取 UWB定位算法Matlab实现：EKF/UKF/泰勒/三边/多点对比分析
简介一份面向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 保持不变对应匀速直线运动 endEKF 需要这个函数提供状态转移同时调用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配合调用。算法名称是否需要状态先验主要计算方式典型适用场景EKF需要上一帧状态与协方差状态转移 观测雅可比动态轨迹跟踪线性化可接受时UKF需要上一帧状态与协方差sigma 点无迹变换强非线性测距模型、较大初始误差泰勒级数法需要初始坐标迭代最小二乘静态定位、无时间邻域依赖三边测量不需要直接解方程基站数恰好为 3 的简单定位多点定位不需要超定最小二乘基站数大于 3常规部署场景3. Matlab 脚本仓库的主干函数与关键实现剖析3.1 从 demo 脚本看五种算法的启动方式仓库的文件命名很直白每个算法对应一个 demodemo_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的差别在于基站布局范围与目标运动速度。小尺度场景基站间距通常只有几米运动速度较低大尺度场景基站间距可能到十几米甚至几十米标签移动速度也更快。参数小尺度 LOS大尺度 LOS基站间距38 米1040 米目标移动速度 1 m/s 1 m/s建议状态模型CVCA主要误差来源测距噪声几何精度因子GDOP与基站布局在大尺度场景下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 动捕可到 100300 赫兹。常见做法是以 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 和 REKF 和 UKF 如果出现轨迹抖动先不要动算法结构而是检查过程噪声协方差 Q 和量测噪声协方差 R。Q 设置过大会让滤波器过度相信测距观测轨迹噪声明显R 设置过大则轨迹过度平滑在转弯处出现滞后。一个快速经验值是先用测距传感器的标准差平方作为 R再用 0.1 倍的 R 作为 Q 的底数跑通后再根据误差曲线微调。把demo_EKF_algo.m里的初始协方差矩阵拆出来设成可调变量是调试这类滤波器的关键一步。本文还有配套的精品资源点击获取