简介面向SLAM算法研究与学习人群这份MATLAB工程针对多目标定位与建图问题给出了基于平方根无迹卡尔曼滤波SRUKF改进的PHD-SLAM完整实现。压缩包共15个文件包含11个M脚本与4个FIG图形交互文件整体仅230KB代码精简核心功能覆盖观测数据获取、目标检测、数据关联、状态更新与地图构建等关键环节。目前已有275人学习下载。压缩包内提供了PHD-SLAM基础框架、UKF和SRUKF两种滤波核心函数以及主程序SLAM.m和辅助的观测生成函数方便对照普通无迹卡尔曼与平方根无迹卡尔曼在多目标跟踪中的滤波精度与数值稳定性差异随附的FIG文件可直接在MATLAB中打开直观呈现仿真过程中目标状态和地图估计结果。这套实现将随机有限集、概率假设密度等理论转化为可运行的代码既能用于机器人导航方向的课程设计与科研实验也适合作为深入理解SLAM多目标建模与滤波实现的入门参考。1. SRUKF-PHD-SLAM一份能跑的多目标定位建图Matlab包拆开这份代码包之前我原本以为它只是又一个传统的单机器人EKF-SLAM仿真。真正打开后才发现这里的核心不是「同时定位与建图」本身而是把 PHD 滤波器和平方根无迹卡尔曼滤波SRUKF绑定在了一起——目标数量未知、杂波干扰严重的场景才是这个包真正想解决的问题。传统固定状态向量的 SLAM 在地图路标增多、目标出现或消失时会明显吃力而这套 MA_SRUKF-PHD-SLAM 用随机有限集描述环境配合无迹变换处理非线性运动模型在数值稳定性上比直接做 EKF 线性化要扎实不少。适合正在做多目标 SLAM 仿真、课程设计或者想对比 UKF 与 SRUKF 实际效果的读者也适合想找一份可运行 Matlab 工程作为起点的同学。2. 从KF到SRUKF无迹变换、概率密度表示与三个选型理由2.1 卡尔曼家族演进为什么PHD-SLAM不用EKF标准卡尔曼滤波KF假设系统是线性的、噪声是高斯的这种情况下它能给出最优估计。但机器人运动模型和传感器观测模型几乎没有严格线性的最常见的就是带转向角的差速模型位姿更新里带着 cos/sin 项观测模型里又有 atan2 这类角度运算。扩展卡尔曼滤波EKF用一阶泰勒展开把非线性函数线性化本质上是拿切线代替原函数当模型非线性强时切线近似会引入明显的截断误差协方差的传播也就不再可信。无迹卡尔曼滤波UKF走的是另一条路它不线性化函数而是选取一组 sigma 点让这批点经过非线性函数传播后用加权统计的方式恢复出均值和协方差。这种方式对非线性函数的逼近精度能达到三阶而且不需要计算雅可比矩阵实现上也省掉了大量求导工作。SRUKF 则是在 UKF 基础上进一步改进——直接维护协方差的平方根矩阵而不是维护协方差本身在数值稳定性上更有保障。滤波器非线性处理方式数值稳定性计算量EKF一阶泰勒展开一般雅可比出错时容易发散较小UKFsigma点无迹变换较好但仍需要协方差正定中等SRUKFsigma点 平方根传播好避免对非正定矩阵求逆中等偏高在 PHD-SLAM 里选择 UKF/SRUKF 而不是 EKF原因很直接PHD 的强度更新要对大量高斯分量逐个做滤波每个分量的观测预测都带着非线性运算EKF 的线性化误差会被多个分量叠加放大。我一般会建议优先跑一遍 UKF 版本验证逻辑再切到 SRUKF 版本看数值表现。2.2 PHD滤波器与RFS表达多目标环境如何变成一张地图PHD-SLAM 和传统 SLAM 最大的区别在于对环境目标的建模方式。传统方法把地图写成固定维度的状态向量新增一个路标就得扩维目标消失或漏检时还要额外引入管理逻辑。PHD-SLAM 把环境中的路标看作一个随机有限集RFS集合本身的数量是随机变量每个元素是一个目标状态。PHD 函数就是这个随机有限集的强度函数它在状态空间任意区域的积分等于该区域内目标数量的期望值峰值对应的位置就是最可能的目标位置。这套表示给 SLAM 带来的直接好处是目标出现、消失、被遮挡这些情况都不需要显式管理。预测步骤里新目标自然出生更新步骤里权重低的虚假分量自然衰减。实现上PHD 通常用一组加权高斯分量近似每个分量有均值、协方差和权重。权重的大小不代表目标存在概率而是代表强度多个高斯分量可能同时描述同一个目标最后通过剪枝和合并把这些分量收敛成少数几个峰值。2.3 SRUKF在里面改了什么协方差平方根与数值稳定性SRUKF 的核心改动在于协方差表示形式。常规 UKF 每一步更新都要计算 P S * S再在后续步骤里对 P 做 Cholesky 分解或求逆。当状态维度较高、噪声较大或观测异常时P 可能因为数值截断变成非正定矩阵Cholesky 分解直接报错这是 UKF 在工程里最常见的翻车点。SRUKF 全程维护 S 矩阵本身用 QR 分解和 Cholesky 因子的一阶更新来处理 sigma 点绕开了对 P 的显式分解。在 MA_SRUKF_PHD_SLAM 这个环境里SRUKF 的数值优势被进一步放大地图里可能有几十甚至上百个高斯分量每个分量每帧都要执行预测和更新。如果一个分量的协方差轻微失去正定性整个 PHD 循环就可能被中断。SRUKF 的平方根形式相当于给每个分量加了一道数值保险让长时间运行的仿真不至于中途崩掉。提示如果你只跑短时间的单目标仿真UKF 和 SRUKF 的精度差异几乎看不出来但把仿真时长拉长到几百步、路标数增加到几十个后SRUKF 的稳定性收益会非常明显。3. 解包代码库.m文件分工、数据流与核心参数3.1 文件清单主程序、滤波函数与观测工具的分层解压后在 Matlab 里浏览文件列表能看出这个代码包的分层思路。最顶层是 SLAM.m 主程序负责仿真循环的调度中间是 PHD_SLAM_function.m 这个 PHD 框架函数把预测、更新、剪枝、合并这些步骤串起来底层是 UKF_PHD_SLAM_function.m 和 SRUKF_PHD_SLAM_function.m真正执行滤波器计算。另有 MA_SRUKF_PHD_SLAM_function.m 这个整合版本以及若干辅助函数。文件角色关键点SLAM.m主程序入口设置真值地图、调用循环、绘图PHD_SLAM_function.mPHD框架高斯分量预测/更新/剪枝/合并UKF_PHD_SLAM_function.m滤波器实现sigma点采样与权重计算SRUKF_PHD_SLAM_function.m滤波器实现平方根矩阵维护MA_SRUKF_PHD_SLAM_function.m整合版本多源结果融合按需使用getzt.m / getzt1.m观测生成从真值地图模拟传感器量测turemap.m地图生成生成路标真值坐标ztnoise.m / ztnoise1.m噪声生成给观测叠加高斯噪声minerr.m误差统计计算估计轨迹与真值轨迹偏差调用关系上SLAM.m 每一步先调 turemap.m 拿到真值路标再经 getzt.m 得到带噪声的观测序列然后交给 PHD_SLAM_function.m 做递推。UKF 和 SRUKF 的函数只在里面作为状态更新引擎被调用也就是说框架是固定的更换滤波器只需要改函数名。3.2 状态向量设计机器人位姿、路标RFS与观测函数的接口SLAM 中机器人状态通常设计为三维向量 [x, y, θ]分别表示横坐标、纵坐标和航向角。控制输入一般是 [线速度, 角速度] 或者 [左轮速度, 右轮速度]具体要看运动模型怎么定义。这段状态在预测步骤里由运动方程传播在更新步骤里由观测方程校正。路标部分在 PHD 框架里不放在单一状态向量中而是由一组高斯分量共同描述。每个高斯分量形如 {均值 m, 协方差 P, 权重 w}均值是路标在笛卡尔坐标下的位置协方差表示不确定性权重是强度值。状态提取时把权重高于阈值的分量作为目标输出这就完成了「从 PHD 强度到具体地图点」的转换。getzt.m 承担的职责是从真值地图 turemap.m 中读取传感器视场内的路标转换成观测向量后交给滤波器。我一般会留意观测向量是行向量还是列向量——这是 Matlab 代码里最容易出问题的地方。观测通常设计成极坐标形式 [距离 r, 方位角 φ]滤波器内部做预测时再把路标均值映射到极坐标空间做残差计算如果方向搞反矩阵维度立刻对不上。3.3 三个滤波函数的分工PHD骨架、UKF/SRUKF实现与MA整合PHD_SLAM_function.m 更像一个骨架函数它维护高斯分量集合按顺序执行预测、更新、剪枝、合并和状态提取。预测步骤会把上一帧分量按运动模型传播同时叠加过程噪声更新步骤用当前观测对每个分量做滤波增益计算剪枝去除权重过小的分量合并把距离很近的分量并成一个。UKF_PHD_SLAM_function.m 和 SRUKF_PHD_SLAM_function.m 做的事情在接口层是一致的输入分量的均值和协方差以及当前观测输出更新后的均值和协方差。区别在于内部是否对协方差做平方根分解。前者计算 P 后直接用后者用 S 矩阵做传播最后再恢复 P 用于后续步骤。MA_SRUKF_PHD_SLAM_function.m 从命名上推测整合了多个 SRUKF 结果可能是多假设或多次采样的融合做课程设计时可以先放着不跑等确认单滤波器版本没问题再打开对比。4. 跑通主程序三处关键设置与一版可复现的仿真参数4.1 输入参数在哪里改检测概率、杂波率与过程噪声PHD 滤波器区别于普通 SLAM 的一点是它需要设置杂波率和检测概率。检测概率 pd 表示一个真实路标被传感器检测到的概率通常设在 0.85~0.95 之间杂波率 lambda_c 表示每帧观测中虚假量测的期望数量一般按真实路标数量的 10%~20% 来估计。设置太小虚假目标容易被当成真目标设置太大权重被稀释真实目标提取不出来。过程噪声 Q 和观测噪声 R 在 SLAM.m 开头或 PHD_SLAM_function.m 的参数区里改。Q 反映运动模型不可信的程度R 反映传感器量测精度。按照这类仿真的常见做法Q 对角线设为 [0.1; 0.1; 0.01] 量级位置噪声 0.1 米、角度噪声 0.01 弧度R 对角线设为 [0.5; 0.01]距离噪声 0.5 米、角度噪声 0.01 弧度起步跑完后再根据误差曲线调整。% 常见参数区示例具体以实际代码为准 pd 0.9; % 检测概率默认0.9可以覆盖多数场景 lambda_c 5; % 每帧平均杂波数按路标数量的10%-20%估算 Q diag([0.1 0.1 0.01]); % 过程噪声位置0.1m角度0.01rad R diag([0.5 0.01]); % 观测噪声距离0.5m角度0.01rad x0 [0; 0; 0]; % 机器人初始位姿过程噪声矩阵 Q 的维度必须和状态维度匹配。因为在这个仿真里状态是三维 [x, y, θ]所以 Q 是 3×3。观测噪声 R 维度跟随观测向量变化极坐标观测是 [r, φ] 两个量R 就是 2×2。改参数前先确认维度这是避免后文讲到的维度报错最直接的办法。4.2 主循环结构预测、更新、剪枝与地图输出SLAM.m 的主循环结构在这类 PHD-SLAM 仿真里通常是生成当前时刻的观测对机器人位姿做预测对每个高斯分量做预测然后执行更新再做剪枝合并最后提取状态。用类似的伪代码表示如下% SLAM主循环结构结合常见PHD-SLAM仿真整理的示意代码 for k 1:num_steps % 生成观测真值地图 传感器模型 z getzt(x_true(:, k), map, sensor_range, R); % 机器人位姿预测 [x_est(:, k1), P_robot] predict_pose(x_est(:, k), u(:, k), Q); % 对每个高斯分量做预测 for i 1:length(components) components(i).mean f(components(i).mean, u(:, k)); components(i).cov components(i).cov Q; components(i).weight components(i).weight; end % 新生目标出生通常由观测驱动或均匀分布生成 components spawn_birth_components(components, z, lambda_c); % 更新调用UKF或SRUKF函数 components update_components(components, z, pd, lambda_c, R); % 剪枝与合并 components prune_components(components, w_threshold); components merge_components(components, merge_threshold); % 提取地图点 map_est extract_targets(components, w_extract); end这段代码里有几个参数值得注意。w_threshold 是剪枝权重阈值低于它的分量会被剔除一般设为 1e-5 量级太小会让分量数量膨胀拖慢计算太大会丢掉刚出现的新目标merge_threshold 是合并距离阈值两个分量的均值距离小于它且权重都大于阈值时合并成一个典型取值为 4~5 米需要和传感器噪声匹配。4.3 从fig文件观测运行结果真值轨迹、估计轨迹与路标定位压缩包里那几个 .fig 文件是 Matlab 图形文件可以用 openfig 命令直接打开。它们的作用是保存算法运行后的结果图包括真值轨迹与估计轨迹对比、路标真值与估计值对比以及部分中间过程的滤波状态。打开后按住 Ctrl 滚动滚轮可以缩放细节拿来做答辩截图或者代码运行结果的参考图都很方便。我拿到陌生代码包的习惯是先跑一次 SLAM.m再把 .fig 文件打开对照看运行结果和图中展示的结果差距有多大。如果轨迹方向一致但误差较大问题多半出在噪声参数上如果轨迹方向都不对就要回头看运动模型或者航向角的单位是否一致。5. 避坑排查维度、数值发散、观测错位等五个真实翻车点5.1 协方差不正定导致Cholesky分解报错现象运行到 SRUKF_PHD_SLAM_function.m 内部时Matlab 报错说矩阵不是正定矩阵无法完成分解运算。程序在几百步仿真里跑到某一步时随机触发有时能用有时不能用。原因常规 UKF 在更新结束后重算协方差 P如果某一步观测异常或者数值截断P 的最小特征值变成负数Cholesky 分解就失败。SRUKF 版本理论上是平方根形式但如果在代码里没有彻底替换掉对 P 的分解操作同样可能出现这个问题。解决先确认使用的滤波函数是 SRUKF 版本而不是 UKF 版本如果确认无误检查代码里是否仍然存在 chol(P) 或 sqrt(P) 这类调用有的话改为对平方根矩阵操作。应急手段是在 P 的对角线加一个极小量 epsilon 1e-6逼回正定但这只是临时手段长期要看更新公式实现。5.2 getzt返回维度与滤波函数内部不匹配现象报错信息通常写着「Matrix dimensions must agree」位置在观测计算或者残差计算的代码行。点开报错行查看时发现一个量是 1×2另一个量是 2×1。原因getzt.m 生成观测时可能沿用了行向量习惯而滤波函数内部默认状态和观测都是列向量。Matlab 对行向量和列向量做矩阵运算时会隐式扩展或直接报错这个问题在脚本里经常出现因为单一文件内习惯统一跨文件调用时就会暴露。解决在 getzt.m 返回前统一加一行 reshape把观测转成列向量或者在滤波函数入口处做一次 size 检查用 assert 明确维度。我一般在每个函数入口处打印关键变量的 size跑通后去掉 disp。这个方法虽然土但排查这类问题效率很高。5.3 目标数量估计膨胀PHD强度发散现象仿真运行几十步后地图上出现大量分散的估计点目标数量估计值不断上涨从最初的十几个涨到几十个甚至上百个真实路标位置反而不明显。原因杂波率 lambda_c 设置偏低或者检测概率 pd 设置偏高导致虚假观测在更新步骤后保留了大权重。PHD 强度里虚假分量没有被有效衰减状态提取时全都越过阈值被当成目标输出。解决将 lambda_c 调大到每帧真实观测数量的 20%同时把剪枝权重阈值 w_threshold 适当提高。比如真实观测平均每帧 8 个lambda_c 建议设置 3~4而不是设成 1。另一个经验是看更新后总权重——PHD 的性质是权重总和近似目标数期望如果权重总和持续变大杂波参数的嫌疑最大。5.4 估计地图里只有机器人轨迹没有任何路标现象轨迹画得很正常但路标估计点要么完全没有要么只有刚出生的几个点随后又消失。把传感器量程参数改大也没有任何变化。原因turemap.m 生成的路标位置可能落在传感器量程之外getzt.m 按量程过滤后观测数量为零或者传感器量程设置太小路标分布稀疏时大部分时刻都没有量测进来。另一种常见原因是新目标出生强度设置过低即使有观测落入新生分量权重也很低直接在一轮更新后被剪枝掉。解决检查 turemap.m 里路标坐标分布范围再检查 getzt.m 或主程序里的 sensor_range 变量确保传感器覆盖范围至少大于地图对角线的一半。同时把出生分量的权重初始值调高一个数量级比如从 0.01 调到 0.1观察新目标是否能在几个时间步内稳定存活。5.5 minerr.m计算误差时数组越界或结果全为NaN现象最终误差曲线前半段正常后半段突然出现 NaN或者运行结束时提示索引超出数组边界。回看估计轨迹数组发现后面很多位置是空的。原因估计轨迹数组是按固定长度预分配的但某些特殊情况下滤波器没有输出对应帧的状态或者轨迹记录代码把真实步数和数组下标错开了。minerr.m 在计算每个时间步的误差时对空位置执行了数值运算结果变成 NaN。解决记录轨迹前先预分配 full 长度的数组 (num_steps 1)每一步强制写入哪怕该步没有有效状态也写入上一帧的值占位。minerr.m 内比较前先做一次 isfinite 检查跳过非有限数据点。这样既能保证数组长度一致也能在误差统计里把这些无效帧剔除。注意以上五个问题的排查顺序建议按照报错位置分析。运行时报错的看堆栈不报错但结果异常的先查参数再查数据流最后查滤波器内部。前后端配合排查比盲改参数要快得多。6. 把仿真结果用于工程验证误差分析、收敛性检查与后续扩展6.1 minerr.m与RMSE你应该关注的三个数minerr.m 给出了基础误差统计但做课程汇报或实验对比时通常还要补上两个指标RMSE 和 NEES。RMSE 衡量估计轨迹与真值轨迹的总体偏离程度NEES 则检查滤波器协方差是否和实际误差一致——NEES 过大说明滤波器过于自信过小说明对噪声过于悲观。% 误差计算常用结构示意 rmse_pos sqrt(mean(sum((x_est(1:2,:) - x_true(1:2,:)).^2, 1))); errors x_est(1:2,:) - x_true(1:2,:); nees zeros(1, num_steps); for k 1:num_steps nees(k) errors(:,k) / P_robot(1:2,1:2,k) * errors(:,k); end mean_nees mean(nees);P_robot 要取滤波器内部维护的机器人位姿协方差而不是自己重新计算的误差协方差否则 NEES 没有意义。实际运行中如果 NEES 均值明显大于状态维度 2就说明过程噪声 Q 设置偏小可以翻倍尝试。6.2 从单目标到多目标把UKF换成SRUKF前后的效果对比跑通这份代码后我建议你做一次对照实验在相同参数下先后运行 UKF 和 SRUKF 两个版本记录目标数量估计曲线和目标位置误差。这个实验能直观展示平方根形式的价值——在多目标场景下SRUKF 的目标数量估计波动通常更小丢失目标的帧数更少。如果两个版本差异不大说明当前仿真环境偏温和可以增大杂波率、降低检测概率再对比这样差异会更明显。另一个值得做的验证是把传感器量程缩小制造更多目标出生和消亡的时机看 PHD 框架在目标数突变时是否依然稳定。这也是这份代码包最值得学习的地方。从那以后我每次改动参数都强制走一遍「备份原文件、单步运行、观察中间变量尺寸」的流程确认单帧数值正常再做整段仿真长跑避免了调参后跑一整晚最后发现数组越界重来的尴尬。希望帮到你。本文还有配套的精品资源点击获取