简介这是一份基于Matlab的扩展卡尔曼滤波同时定位与建图仿真模拟器面向机器人导航与地图构建方向的初学者和研究人员。压缩包内共三十二个文件以二十四个源码文件为主体涵盖运动模型、观测模型、状态预测、测量更新、数据关联、地标增广等核心模块另有三个界面图形、三个实验数据和两个说明文档整体大小仅七百二十四KB轻量易部署。已有三百人学习浏览。通过运行模拟器可以直观观察机器人定位与地图生成过程并通过修改噪声参数、初始条件和环境复杂度检验算法的鲁棒性与准确性。源码结构清晰便于对照推导扩展卡尔曼滤波的预测与校正步骤深入理解非线性模型处理和数据关联难题是一套兼具教学与科研价值的入门工具。1. 为什么要自己跑一遍EKF-SLAM先看这份Matlab模拟器能给你什么EKF-SLAM这个名字搞机器人定位的人都不陌生但真正自己动手写过一遍的人并不多。我第一次硬啃论文时状态向量、协方差矩阵、数据关联这些概念就像一团雾后来拿到一个Matlab写的EKF-SLAM Simulator_v1.02跟着代码一步步跑通才算把算法从“黑匣子”变成“看得见的状态更新”。这份资源不是工业级SLAM库而是一套教学向的Matlab仿真器。它把EKF-SLAM拆成了几十个函数文件从运动模型、观测模型到数据关联、状态增广全都有对应代码。你能直接运行ekfslam_sim.m看到机器人一边走一边建图的轨迹也能改噪声参数看地图怎么发散。适合刚接触SLAM的入门者、准备复现论文的学生以及想快速验证算法思路的工程师。下面我按自己拆代码的顺序从算法骨架讲到运行调试最后列出几个我踩过的坑。2. EKF-SLAM的算法骨架状态向量、预测与更新如何在这里落地熟悉EKF的人都知道SLAM的难点在于状态向量不是固定的——机器人每发现一个新地标状态向量就要变长。这份Simulator把这部分拆得很清楚vehicle_model.m、predict.m、observe_model.m这些文件各自负责什么看名字就能猜个大概。我们先从状态表示说起。2.1 状态向量和地标表示看懂代码里那三行核心矩阵在EKF-SLAM里系统状态通常写成机器人位姿加上所有地标位置地标数量是动态增长的。对应到这个Simulatorekfslam_sim.m初始化时会把状态向量构造成三段机器人的x、y、航向角然后是按顺序排列的地标坐标。协方差矩阵P的维度是(32N)×(32N)N是当前已加入地图的地标个数。我一开始常犯的错是把P当成固定矩阵实际上每个新地标加入后P都要增广这份代码里对应的是augment.m和augment_associate_known.m。代码里我一般会先看predict.m它代表EKF的预测步骤function x predict(x, u, dt) % 预测步骤根据控制量u更新机器人位姿 % u [速度v; 转向角delta]dt为采样时间 v u(1); delta u(2); theta x(3); x(1) x(1) v * cos(theta) * dt; x(2) x(2) v * sin(theta) * dt; x(3) pi_to_pi(x(3) v * tan(delta) / L * dt); % L为轴距 end这里的pi_to_pi.m是一个角度归一化函数把角度限制在[-pi, pi]之间。很多新手不写这一步结果航向角越积越大协方差更新全乱套。注意预测只动了机器人位姿部分地标位置没有变因为地图被假设为静止。这是EKF-SLAM里一个基本假设后续所有推导都建立在这个不变性上。预测之后是协方差传播。严格来说predict.m还要返回传播后的P但完整写法里P的更新依赖于运动模型雅可比矩阵。这份代码把雅可比计算放在vehicle_model.m里通过数值或解析方式求出F矩阵然后执行P F * P * F Q。如果你只看了状态更新没看协方差传播后面观测更新里的增益K就会算错最终导致滤波发散。2.2 运动模型与观测模型为什么用扩展卡尔曼而不是普通卡尔曼普通卡尔曼滤波要求系统是线性的但机器人运动模型里出现cos(theta)、sin(theta)观测模型里出现atan2显然是非线性。EKF的做法是对非线性函数做一阶泰勒展开用雅可比矩阵代替状态转移矩阵和观测矩阵。在这个Simulator里vehicle_model.m和observe_model.m分别负责计算雅可比矩阵。以观测模型为例当机器人观测到一个地标时量测是距离r和方位角bearing。对应代码可以写成function [z, H] observe_model(x, landmark) % 计算预测观测值和观测雅可比矩阵H dx landmark(1) - x(1); dy landmark(2) - x(2); q dx^2 dy^2; z [sqrt(q); pi_to_pi(atan2(dy, dx) - x(3))]; H [ -dx/sqrt(q), -dy/sqrt(q), 0, dx/sqrt(q), dy/sqrt(q); dy/q, -dx/q, -1, -dy/q, dx/q ]; end这段代码里z的维度是2×1H的维度是2×(32N)。实际更新时只用与当前地标对应的那两列其余列乘出来是零所以代码里更新步骤需要做列索引切片。这也是为什么KF_simple_update.m和KF_cholesky_update.m两个文件存在——它们提供了两种数值稳定的更新方式后者用Cholesky分解避免协方差矩阵在长时间运行后失去对称正定性。如果跑仿真时发现P矩阵出现NaN多半是直接求逆导致数值爆炸换Cholesky更新会好很多。非线性带来的另一个问题是线性化误差。EKF只在当前估计点做一阶近似如果初始误差太大或者传感器噪声太强一阶近似误差会累积。这份Simulator里还有一个KF_IEKF_update.m对应迭代扩展卡尔曼更新通过多次迭代重新线性化来降低误差。在代码里update.m通常调用普通EKF更新而update_iekf.m则尝试多次修正。实际工程中如果发现轨迹发散可以考虑启用迭代EKF代替普通版本。2.3 从Simulator_v1.02看一次完整的预测-更新循环整个仿真器的核心循环在ekfslam_sim.m里流程大致是先configfile.m读入参数然后循环中predict做一步预测接着get_observations.m根据真实地标生成带噪声的观测data_associate_known.m或data_associate.m做数据关联最后update.m或update_iekf.m做更新。我建议按这个顺序去读代码而不是从第一行顺序读。一个简化版的循环骨架可以写成这样for k 1:sim_step % 控制量按当前航向加一点转向噪声 u compute_steering(x, wp); u add_control_noise(u, Q); % EKF预测 [x, P] predict(x, P, u, dt, Q); % 获取观测并添加观测噪声 z get_observations(x, landmarks); z add_observation_noise(z, R); % 数据关联这里用最简已知对应 [zf, idf] data_associate_known(z, idf); % 更新 [x, P] update(x, P, zf, idf, R); end这里的Q是控制噪声协方差R是观测噪声协方差。这两个矩阵是EKF-SLAM里最关键的参数后面我们会专门实验它们的意义。循环里每一步都依赖于上一时刻的估计所以一旦某一步数据关联错误后面的状态全部跑偏而且很难拉回来。这就是SLAM里“误关联是致命的”这句话的来源。另外注意compute_steering.m和add_control_noise.m这两个文件。compute_steering.m根据预设的路点算转向角add_control_noise.m是给真实控制量加高斯噪声。很多人在理解EKF时只盯着update忽略了“真实值”和“带噪输入”的区别。这个Simulator的好处就是把真实量、带噪观测、滤波估计三者同时打印在图上你能直观看到每一步噪声的影响。3. 把Simulator跑起来文件清单、运行入口和参数改动解压rar后第一件事不是双击运行而是先看文件名。这个资源里面混着功能文件、配置文件和fig图形文件直接全选运行肯定翻车。我一般是先列一个清单把入口和工具分开。3.1 解压后先认文件哪些是入口哪些是工具函数下表是我整理的关键文件分类类别文件名作用入口ekfslam_sim.m主仿真脚本包含主循环和绘图配置configfile.m设置地图、噪声、步数等参数运动模型vehicle_model.m计算运动模型雅可比矩阵预测predict.m预测步骤更新机器人位姿和协方差观测模型observe_model.m计算观测值及观测雅可比矩阵观测获取get_observations.m从真实地标生成仿真观测值噪声生成add_control_noise.m/add_observation_noise.m给控制量和观测加高斯噪声数据关联data_associate_known.m/data_associate.m已知对应/未知对应关联状态更新update.m/update_iekf.mEKF更新步骤后者是迭代EKF协方差更新KF_simple_update.m/KF_cholesky_update.m两种数值更新算法状态增广augment.m/augment_associate_known.m将新地标加入状态向量角度工具pi_to_pi.m角度归一化坐标转换TransformToGlobal.m将观测从机器人坐标系转到全局坐标绘图辅助plot_feature_loci.m/line_plot_conversion.m轨迹和特征轨迹绘制此外还有一个readme.txt建议先读。frontend.m和frontend.fig是配套的界面展示文件主要负责把轨迹和地图呈现在窗口里核心算法不在其中。压缩包里还有一些.fig和.mat文件那些是作者跑完保存的结果图和数据文件可以在Matlab里打开参考但不需要运行。3.2 运行ekfslam_sim.m从配置到出图的完整步骤在Matlab中运行前先打开configfile.m看参数。典型配置长这样% configfile.m 主要参数 sim_step 200; % 仿真步数 dt 0.025; % 采样时间 L 1; % 车辆轴距 Q diag([0.2; 0.1]); % 控制噪声协方差速度、转向 R diag([0.1; 0.05]); % 观测噪声协方差距离、角度运行步骤非常直接把解压后整个文件夹加入Matlab路径或者直接用Matlab打开文件夹。先运行configfile再运行ekfslam_sim。实际上ekfslam_sim.m内部会调用configfile你可以直接只跑主脚本。观察图窗输出机器人真实轨迹、估计轨迹、地标估计位置。sim_step决定仿真长度地标数量在configfile里定义通常是几个固定坐标。注意Q和R的维度必须与控制量、观测量的维度一致否则Matlab会报矩阵维度不匹配。我初次跑的时候把Q写成3×3直接报错后来才发现控制量只有二维一个是速度一个是转向角。如果你用的是Matlab 2023b这类新版本打开老文件时可能遇到编码问题。别急着点运行先在编辑器里用“另存为”选择UTF-8编码或者直接忽略中文注释先跑通再说。2016年之前的Matlab默认GBK编码新版默认UTF-8混着用会出现乱码但不影响代码逻辑。3.3 改噪声参数观察轨迹变化一个最简单的实验理解EKF-SLAM最有效的方式是亲手改噪声。我做过一个实验把观测噪声协方差R从diag([0.1;0.05])改成diag([10;5])其他参数不变。跑出来的估计轨迹会明显抖动地标位置收敛得很慢。原因是距离噪声方差从0.1变成10标准差扩大了10倍滤波器对观测的信任度降低每次更新对状态向量的修正幅度变小。还有一个更直观的实验把控制噪声Q调大比如diag([2;1])预测的不确定性增大EKF会更信任观测。这时会发现状态估计对观测噪声更敏感轨迹更容易受观测离群值影响。反过来Q调小、R调大系统更信任运动模型轨迹相对平滑但遇到系统误差时会产生持续偏移。这种参数敏感性实验比看十遍公式都管用。而且改动只涉及configfile.m不需要碰算法本身。实验时建议每次只改一个参数并且记录下改动前后的轨迹图。我就是这样总结出“Q大信观测、R大信模型”的直觉后来调任何SLAM参数都会先问自己当前传感器哪个更可靠运动模型哪个更可靠这份Simulator的另一个好处是它把真实轨迹和估计轨迹画在一起发散时一眼就能看出来。4. 数据关联与状态增广EKF-SLAM最容易翻车的两个环节即使EKF公式全对SLAM还是可能发散原因多半出在两个地方数据关联和状态增广。这两个问题在这份模拟器里都有对应函数看懂它们才算真正入门。4.1 已知对应与未知对应data_associate_known.m和data_associate.m的差别数据关联解决的问题是当前观测到的特征对应地图里哪个地标在仿真器里有两种情况data_associate_known.m假设观测与地标对应关系已知。通常做仿真验证算法鲁棒性时用因为可以手动控制关联结果排除关联错误干扰。data_associate.m根据观测与地图地标之间的马氏距离找最近邻距离小于某个阈值则匹配否则认为是新地标。我知道很多人跳过data_associate_known.m直接看data_associate.m但那样很难分清误差来自EKF还是关联。建议先跑已知关联版本把滤波调稳再切到未知关联版本。data_associate.m的核心逻辑类似function [zf, idf] data_associate(z, x, P, R, gate) N (length(x) - 3) / 2; % 当前地标数 idf zeros(1, size(z,2)); for i 1:size(z,2) best inf; bestid 0; for k 1:N % 计算第k个地标的预测观测和协方差 [zp, H] observe_model(x, x(32*k-1 : 32*k)); S H * P * H R; innov z(:,i) - zp; innov(2) pi_to_pi(innov(2)); d innov / S * innov; % 马氏距离 if d gate d best best d; bestid k; end end if bestid ~ 0 idf(i) bestid; end end zf z(:, idf0); idf idf(idf0); end这里的gate是关联门限马氏距离小于门限才认为匹配。如果门限设太大会把不同地标误关联设太小真实对应也会被拒绝。我一般先从gate9开始调对应χ²分布2自由度约5.993自由度约7.819算比较宽松的门限。实际使用时要结合传感器噪声水平来定噪声大就放宽噪声小就收紧。注意代码里有一个细节计算马氏距离时角度innovation要经过pi_to_pi归一化。如果不做这一步角度差在π附近会产生虚假的大距离导致该匹配的观测被拒绝。这个坑我自己踩过后来在data_associate.m里看到这行才反应过来。4.2 新地标加入地图augment.m做了什么当观测不能匹配任何已有地标时需要把新地标加入状态向量。augment.m做的事情是用当前机器人位姿和观测值反算地标全局坐标然后扩展状态向量和协方差矩阵。关键代码逻辑可以写成function [x, P] augment(x, P, z, R) % z [距离; 角度] r z(1); bearing z(2); theta x(3); lm_x x(1) r * cos(theta bearing); lm_y x(2) r * sin(theta bearing); % 扩展状态把新地标追加到向量末尾 x [x; lm_x; lm_y]; % 扩展协方差计算雅可比矩阵J形成新的P ... end注意这里如果直接写成lm_x x(1) r * cos(theta bearing)角度必须经过pi_to_pi归一化否则增广出来的地标位置在角度跨±π时会有跳变错误。协方差增广也不是简单地在原P后面补零块而要用运动模型和观测模型的交叉协方差去填充对应块。这也是为什么augment_associate_known.m单独存在——它处理的是已知关联下批量增广的场景。增广步骤的常见错误是把新地标的位置计算直接建立在观测值z上而没有考虑观测噪声。正确做法是用预测观测值和真实观测值的差来更新协方差交叉项。这份Simulator里augment.m的实现比较直白适合学习但如果要用到实际项目建议参考现代SLAM库里的增广写法那里会更关注协方差的一致性。4.3 常见坑观测噪声协方差R设错导致的地图发散我在实验中把R设成diag([0.1;0.05])一切正常但改成diag([0.5;0.5])后地标位置估计开始漂移。原因是距离和角度的噪声单位不同角度噪声用弧度0.5弧度大约28.6度这对激光雷达或视觉观测来说已经很大了。有些地标距离近角度误差会导致位置误差呈线性放大。更隐蔽的坑是R矩阵非对角化。实际传感器噪声往往存在相关性但这份仿真器里R被简化为对角阵。如果硬把它改成非对角需要仔细看observe_model里S H*P*H R的维度一旦加错了矩阵结构更新步骤会直接报维度错误。遇到这种情况我建议先退回对角R确认算法正常后再尝试。另外一个容易忽略的点是add_observation_noise.m里用的噪声标准差必须和configfile.m里的R一致。如果你在配置里写了Rdiag([0.1;0.05])但加噪声时用了randn乘上0.5那么仿真里的实际噪声和滤波器假设的噪声就不匹配。这种不匹配会让EKF表现“过于自信”或“过于保守”最终表现为轨迹慢慢偏离。我调试时总要先检查这两处是否一致避免“自以为改了参数”其实没改到点子上。5. EKF-SLAM避坑指南跑仿真时我踩过的四个具体问题这个模拟器代码不复杂但运行过程中我遇到了不少“玄学”问题表面上看是代码报错实际上是参数或环境问题。列几条最常见和最容易忽略的。5.1 现象运行ekfslam_sim.m时提示“Undefined function or variable pi_to_pi”原因当前路径没包含工具箱函数文件。pi_to_pi.m在压缩包根目录但Matlab的工作目录切到了别的文件夹。解决用addpath把整个文件夹加进路径或者在打开文件时右键文件夹“添加到路径”。如果还报错检查文件名是否被改成pi_to_pi_new.m之类的名称。这类文件名带空格或中文也会导致调用失败。我一开始就是直接把解压出来的子文件夹单独拷到桌面结果pi_to_pi.m没跟着过去折腾了十分钟。在Matlab命令行里执行addpath(genpath(你的解压路径)); savepath; % 保存路径下次启动仍在genpath会把子文件夹全部加入避免遗漏。注意如果路径中包含中文有时savepath会警告但这种警告一般不影响单次运行。5.2 现象仿真跑到一半协方差矩阵P变成NaN原因数值不稳定。最常见的情况是KF_simple_update.m里直接对S求逆当S接近奇异时逆矩阵出现无穷大值。另一个原因是观测噪声R设置过小比如把角度噪声设成1e-6导致观测更新权重过大数值上近似除以零。解决换用KF_cholesky_update.m它利用Cholesky分解避免显式求逆。或者检查R矩阵是否过小确保距离标准差至少大于0.01角度标准差大于0.001。如果依然出现NaN可以在configfile.m里把sim_step减小看看是不是某个特定时间点触发异常。我把KF_simple_update换成KF_cholesky_update的经验是改一行调用即可。在update.m里找类似[x, P] KF_simple_update(...)的位置换成[x, P] KF_cholesky_update(...)。这两个函数的输入输出接口基本一致但内部数值稳定性不同。如果update.m没有直接调用它们那就看ekfslam_sim.m主循环里有没有分支判断。5.3 现象估计轨迹一开始就偏离真实轨迹且越跑越远原因初始位姿或初始协方差设置不合理。configfile.m里如果初始协方差P0过小说明你对初始位姿过于自信但实际初始位姿偏离了真实值。反之P0过大初始几步更新会出现大幅纠正。解决把P0设置成与初始不确定性一致。比如初始位置偏差0.5m就把对应方差设为0.25。另外检查初始状态向量里的地标数量是否为零地标初始状态必须为空或与观测数量对应。如果初始化时错误地在状态向量里预置了地标而实际地图里没有那么多地标更新时索引就会乱套。还有一个容易忽略的点configfile.m里真实地标坐标和初始路点坐标可能会被误写成同一组数据。如果真实地标就在机器人起始位置附近那么初始时刻的观测距离接近零距离观测的雅可比矩阵会出现奇异。我遇到的情况就是机器人刚起步时恰好压在一个地标上导致sqrt(q)很小后续协方差爆炸。遇到这种问题手动把地标坐标挪远一点或者把起始位置改到地图空白区域就行。5.4 现象换一台电脑跑Matlab 2023b中文注释乱码甚至报语法错误原因文件编码不同。老代码用GBK编码新版Matlab默认UTF-8中文注释显示为乱码。如果代码里的字符串或注释中包含了中文字符编码不匹配可能导致解析器把中文当成了非法标识符。解决在Matlab编辑器里用“打开”对话框选择“编码”为GBK或“系统默认”不要直接双击打开。打开后如果看到正常中文就另存为UTF-8。如果只是显示问题不影响运行但如果字符串里的中文引号被识别成了全角符号就会报语法错误。更好的办法是直接用文本编辑器比如VS Code把文件转成UTF-8批量处理所有.m文件。这个坑在新版本Matlab下特别常见尤其是从网上下载的老代码。我在2023b下跑通后顺手把所有文件统一转了一个编码之后再没出现过乱码。6. 从仿真到验证用结果图判断算法是否收敛以及下一步怎么改跑完仿真屏幕上会出现几幅图。压缩包里那些.fig文件是作者保存的历史结果图可以打开参考但更重要的是自己跑出来的实时图。判断EKF-SLAM是否收敛我习惯看三样东西估计轨迹是否与真实轨迹贴合、地标估计误差是否随时间下降、协方差椭圆的尺寸是否逐渐缩小。一个简单的验证办法是记录每个时刻的估计地标位置与真实地标位置的误差画成曲线。如果曲线整体下降并趋于平缓说明收敛如果振荡增大说明某个参数或关联出问题。在Matlab里可以自己加一行% 在ekfslam_sim.m循环末尾记录地标误差 % 假设第1个地标在状态向量中的索引是4:5 landmark_err(k) norm(x(4:5) - true_landmark(1:2));注意这里要按实际状态向量的索引取地标不同配置索引不同。我一般会在configfile.m里先打印一下length(x)确认地标起始位置。如果你中途用augment.m加入了新地标索引还会变稳妥的做法是维护一个地标id到状态向量索引的映射表。如果你想让算法更接近现代SLAM可以尝试把update.m换成update_iekf.m做迭代EKF。迭代EKF可以降低线性化误差尤其在大噪声条件下效果更明显。但代价是每步可能迭代多次计算量增加。在这个Simulator里你只需要修改主循环中的更新函数名其他接口基本兼容。我第一次切到迭代EKF时地标误差曲线明显更平滑收敛速度也快了。另一个可改的方向是把data_associate_known.m换成data_associate.m并在仿真里加入误匹配场景。我在做这个实验时发现一旦故意把两个距离很近的地标放在一起最近邻关联很容易出错结果轨迹跳了一个大弯。后来把门限gate调小并在更新前加一步一致性检验情况才好转。这个经验后来用在实际机器人上也帮我避免了几次由于特征混淆导致的定位丢失。从一个教训说起我刚开始用这个模拟器时每次跑完只看轨迹图觉得差不多了就收工。后来有一次把R调大后轨迹看起来还挺平滑但地标误差已经发散我才意识到必须同时监控地标误差和协方差。从那以后我每次改完参数都强制走一遍“看轨迹、看地标误差、看协方差椭圆”的流程再下一个结论。这个习惯也延续到了其他SLAM项目里。希望这个模拟器能帮你在EKF-SLAM上少走弯路把每一行代码、每一个参数都变成自己的直觉。祝调试顺利。本文还有配套的精品资源点击获取