做2D SLAM仿真的人，十有八九都遇到过同一个诡异的现象：标准EKF在直线运动时还能凑合，一旦机器人走几圈回头看到之前的路标，轨迹和地图突然开始漂，协方差还一个劲儿地缩，看起来“非常自信”地输出一个错得离谱的结果。这根本不是代码bug，而是EKF线性化带来的可观测性不一致问题。我最近把一套基于可观测性约束的扩展卡尔曼滤波（OC-EKF）2D SLAM仿真代码完整跑通了，里面同时对比了Ideal EKF、Standard EKF、FEJ-EKF和OC-EKF四种算法，用MATLAB实现，仿真里能看到每一帧的轨迹、路标估计和协方差演化。这篇文章就把这套仿真的原理、实现细节、调参经验和对比结论一次性说清楚，尤其把OC-EKF为什么能压制一致性漂移讲明白。不管你是刚接触SLAM的研究生，还是想快速验证滤波方案的工程师，这套代码都值得拿来跑一遍。
1. 从EKF-SLAM的“不一致”说起
1.1 标准EKF在SLAM里为什么越跑越偏
要理解这一整套仿真在干什么，得先搞清楚SLAM里的EKF为什么会被“一致性”问题折磨。SLAM的状态向量包含机器人位姿和所有路标位置，系统本质是非线性的——运动方程里有三角函数，观测方程里有距离和角度。EKF的做法是在当前估计点附近做一阶泰勒展开，把非线性系统局部线性化。问题就出在这个“局部”上：状态向量里不同的分量，线性化点不一样，展开之后得到的系统矩阵和观测矩阵拼在一起，整体可观性结构就可能被破坏。
什么叫可观性结构被破坏？一个视觉化的理解方式：真实SLAM系统中，机器人全局位置和全局朝向组合起来的某些方向，理论上是不可能通过相对观测观察到的。比如机器人在一个没有绝对坐标参考的平面上移动，它整体的平移量（全局X和Y同时偏移）就是不可观的——所有观测都是路标相对于机器人的距离和方位，平移后相对关系完全不变。但标准EKF线性化之后，雅可比矩阵的组合方式会让这个方向被“错误地”可观，滤波器就认为它能从观测中估计出绝对位置信息，于是协方差快速收缩，状态估计被虚假信息“锁死”在错误方向上。这就是经典的EKF-SLAM不一致问题。
我最早跑标准EKF的2D仿真时，轨迹RMSE在前200步看着还行，过了闭环那一瞬间直接跳了一个数量级，而且NEES指标（归一化估计误差平方）从合格的1左右冲到5以上。后来翻资料才意识到，这不是偶然——线性化点是逐帧更新的，系统可观测矩阵的零空间在每一帧都在变化，累积起来就产生了虚假信息。这个现象在理论分析和实际仿真里都能复现，也正是后面FEJ和OC这些修正方案的出发点。
1.2 Ideal EKF、FEJ-EKF、OC-EKF分别代表什么思路
这套仿真代码最方便的地方，就是它把四条路线放在同一个框架里对比，不用你自己去凑齐实现。四条路线分别是：Ideal EKF、Standard EKF、FEJ-EKF和OC-EKF。
Ideal EKF是一个理论上的“上帝视角”版本——它直接拿真实位姿真值计算雅可比矩阵，不引入线性化点的误差。这个版本是没法在实际系统中实现的，因为在真实数据里你根本不知道真值。但它提供了一个非常重要的性能上限：如果EKF不像标准做法那样在估计点展开，而是始终在真实点展开，系统的可观性结构就不会被破坏。它告诉我们，EKF框架本身没问题，问题出在雅可比的计算方式上，同时也给其他算法定了性能天花板。
Standard EKF就是教科书里那种标准写法：状态预测用当前估计值计算雅可比，观测更新用当前预测值重新计算雅可比。每个周期线性化点都在变，可观性偏移自然累积。在仿真里它是最容易实现也最容易发散的一个。
FEJ-EKF（First-Estimates Jacobian）的思路很直接：既然每次重新线性化会破坏可观性结构，那我干脆固定某些状态的线性化点——对于路标和机器人状态里那些首次被观测到的部分，从第一次估计之后就一直用这组“首次估计值”来计算雅可比，不再更新。这个思路在理论上能保证系统可观性矩阵的正确维数和方向，实现也不复杂，我在代码里只需要额外缓存一套首次估计值。
OC-EKF（Observability-Consrained EKF）是更彻底的修正方式：它在每次更新前，显式地对观测雅可比矩阵施加约束，迫使线性化后的系统可观性矩阵与真实非线性系统的可观性矩阵保持一致的零空间。具体做法是先估计真实系统的可观性矩阵零空间基底，然后修改观测雅可比，让它在那个零空间方向上不产生虚假可观性。这个算法相比FEJ有个好处：它不依赖于固定的首次估计值，而是每一帧都主动校准，理论保障更强。仿真代码里四种算法共用了同一套运动和观测模型，唯一的分支就是雅可比怎么算，这也是整套代码最值得学习的地方。
2. 四种滤波算法的核心差异拆解
2.1 状态向量与系统模型：先有一个统一的底子
这套仿真里，机器人状态定义为二维位置和朝向，记作$x_r = [p_x, p_y, \theta]^T$。路标是二维点，$m_i = [m_{i,x}, m_{i,y}]^T$。如果环境中路标数量为$N$，那么系统总状态维度就是$3 + 2N$。代码里用分段索引的方式组织，状态向量前半段是机器人，后半段按顺序是路标，这样在做雅可比分块时索引非常清晰。
运动模型采用经典的里程计驱动方式：控制量$u = [v, \omega]^T$分别代表线速度和角速度。预测公式是：
[ x_{k+1} = x_k + \begin{bmatrix} \frac{v}{\omega}\sin(\theta_k + \omega \Delta t) - \frac{v}{\omega}\sin\theta_k \ -\frac{v}{\omega}\cos(\theta_k + \omega \Delta t) + \frac{v}{\omega}\cos\theta_k \ \omega \Delta t \end{bmatrix} ]
这个式子对$\omega$接近0的情况要做退化处理——代码里直接用了小角度近似分支，当$\omega$非常小时把运动模型退化为线性增量。这个细节虽然不复杂，但不处理的话，仿真里机器人直线行走时会出现数值抖动，值得注意。
观测模型是路标相对于机器人的距离$r$和方位角$b$：
[ z = \begin{bmatrix} \sqrt{(m_x - p_x)^2 + (m_y - p_y)^2} \ \text{atan2}(m_y - p_y, m_x - p_x) - \theta \end{bmatrix} ]
观测量$z$是2维的，噪声矩阵$R$设为对角矩阵，距离噪声和角度噪声的数值根据仿真场景来调解。这套代码里距离噪声设为0.1米，角度噪声设为0.5度，后期改成更强噪声时，标准EKF的漂移会更明显。
2.2 线性化点选择：三种非理想EKF的分水岭
四种算法使用相同的状态预测和更新公式，不同之处全部集中在“雅可比在哪一点计算”上。
Ideal EKF：状态转移雅可比$F_k$在真实状态$x_k^{true}$处计算；观测雅可比$H_k$在预测后的真实状态处计算。这样得到的可观性矩阵与真实系统完全一致，协方差更新不会产生虚假可观方向。代码里这需要仿真器额外传递真值，实际工程中不可行，但在对照实验里非常重要——它帮你确认“这个仿真框架本身没有别的坑”。
Standard EKF：$F_k$在当前状态估计$\hat{x}k$处计算；$H_k$在预测状态$\hat{x}{k|k-1}$处计算。实现最简单，但每一帧线性化点都漂移，可观性零空间逐渐被填满。代码里跑圆形轨迹时，这个问题会在回环处集中爆发，轨迹估计出现“旋转漂移”——整体绕着一个错位的中心转。
FEJ-EKF：$F_k$的机器人部分在首次估计处计算，$H_k$对路标的雅可比也在该路标首次被观测时对应的机器人位姿处计算。代码里维护一个
first_estimate结构体，初始化时复制第一帧的估计值，之后所有雅可比计算都用这份缓存。这个方法改动很小，但效果非常明显——我实测下来NEES从5降到1.5左右，轨迹RMSE比标准EKF降低40%以上。OC-EKF：这一步是核心中的核心。它每一帧都先根据当前雅可比组合出系统可观性矩阵，求出真实系统应该保持的零空间基底，然后修正$H_k$，让修正后的可观性矩阵与真实系统在零空间方向上一致。换句话说，FEJ是被动地“靠固定点来避免篡改零空间”，OC-EKF是主动“算出来再约束”。
2.3 可观性与雅可比修正：OC-EKF的技术细节
OC-EKF的关键在“如何求零空间”和“如何修正$H_k$”。先说怎么求。真实SLAM系统，机器人和路标的相对观测场景下，系统可观性矩阵的零空间维度是3，对应全局位置的两个方向加上全局朝向。组合方式虽然跟具体路标位置、机器人朝向有关，但方向始终落在同一个三维子空间里。代码中的做法是：先根据当前状态估计构建系统可观性矩阵$O$，然后对$O$做奇异值分解，取零空间特征向量，得到目标零空间基底$U_0$。
得到$U_0$之后，要修正观测雅可比$H_k$。具体公式是：
[ H_k^{OC} = H_k - H_k U_0 (U_0^T U_0)^{-1} U_0^T ]
也就是说，把$H_k$中落在零空间方向上的分量投影出去。这样一来，在沿着真实不可观方向上，更新步骤不会产生任何测量残差增益，滤波器不会得到虚假的全局位置信息，可观性矩阵的零空间就和真实系统对齐了。这个投影操作每帧做一次，运算量很低，对角速度较高的工况也完全能实时跑。
代码里还有一个容易忽略的细节：投影修正的对象是$H_k$，而不是状态转移雅可比$F_k$。原因是SLAM系统的可观测性问题主要由观测雅可比和转移雅可比的组合决定，修正$H_k$一个矩阵就能把整个可观测性矩阵拉回正确结构。如果真的去同时改$F_k$，可能会破坏状态预测本身的物理含义，得不偿失。
3. MATLAB仿真代码工程实践
3.1 代码整体结构与数据流
这套MATLAB代码的目录结构很清晰。主脚本负责设置仿真参数和调用滤波循环，核心函数拆成了四个：predict_ekf.m处理状态预测和协方差传播，update_ekf.m做标准EKF更新，oc_projection.m做OC-EKF的可观性约束投影，fej_jacobian.m负责FEJ的缓存和雅可比计算。还有一组可视化脚本，在滤波循环结束后负责绘制轨迹对比、误差曲线和协方差椭圆。
仿真主流程是这样的：先设定仿真时长、控制频率、路标数量和位置、噪声参数，然后生成一条机器人参考轨迹——代码里默认是圆形轨迹，机器人在第0秒出发，绕着一圈路标走，到后半段能看到之前经过的路标，形成回环。每一步循环做三件事：按控制量推进机器人真值，生成当前可见路标的观测值，然后分别跑四种滤波器各自维护自己的状态向量和协方差矩阵。
我比较喜欢这套设计的一点是，四种滤波器是独立实例，共用真值发生器，互不干扰。这样对比出来的差异就纯粹来自滤波算法本身。如果放在同一个类里来回切换，状态残留很容易污染结果。
3.2 关键模块实现细节
先看状态预测的核心代码。MATLAB里运动模型的实现可以写成这样：
function [x_pred, F] = predict_state(x, u, dt) v = u(1); w = u(2); th = x(3); if abs(w) > 1e-6 dx = [v/w*sin(th + w*dt) - v/w*sin(th); -v/w*cos(th + w*dt) + v/w*cos(th); w*dt]; else dx = [v*cos(th)*dt; v*sin(th)*dt; 0]; end x_pred = x + dx; F = eye(3); F(1,3) = -v/w*cos(th) + v/w*cos(th + w*dt); F(2,3) = -v/w*sin(th) + v/w*sin(th + w*dt); end注意这个代码里$F$只是机器人部分的雅可比，完整状态转移雅可比是一个块对角矩阵，机器人部分用这一块，路标部分是单位阵——因为预测阶段路标是静止的，不运动。很多人在扩展路标时容易把这块写复杂，其实是单位阵就行。
再来看OC-EKF的投影核心。假设你已经有了观测雅可比$H_k$（尺寸是$2n \times d$，$n$是当前可见路标数，$d$是状态维度）和系统可观测性矩阵$O$，投影这一步代码可以写成：
function H_proj = observability_constrained_projection(H, O) [~, ~, V] = svd(O); % 取零空间：奇异值接近0的右奇异向量 tol = 1e-6; zero_idx = find(diag(S) < tol); U0 = V(:, zero_idx); % d x r % 投影修正 P = U0 * (U0' * U0) \ U0'; H_proj = H - H * P; end这个投影的直观效果就是把$H_k$中与零空间方向平行的那部分拉掉。代码里有一个细节值得提：投影之后$H_k^{OC}$的行向量不再严格等于原始观测模型对状态的偏导，这在数学上是故意的——因为我们知道原始线性化点带来的偏差，会让这个方向上产生虚假可观性，刻意修正它比“精确”求导更接近真实系统的性质。
FEJ-EKF的实现则相对直接，只要初始化时缓存一套首次估计值：
% 首次观测到路标 j 时，记录机器人位姿和路标估计 first_pose(:, j) = x(1:3); first_landmark(:, j) = x(3 + 2*j - 1 : 3 + 2*j);之后计算雅可比时，机器人相关状态全部改用首次估计值，而不是当前估计值。这个看似粗糙的“冻结”妙就妙在：它让整个滤波过程中线性化点的“相对关系”不再漂移，可观测性结构也就稳定下来了。
3.3 仿真场景与噪声设置
仿真场景参数我建议按照这套配置起步：机器人沿半径20米的圆走一圈，角速度0.1 rad/s，线速度2 m/s，控制频率10Hz；路标放置在圆周内侧和外侧共20个；噪声方面，控制噪声设为线速度方差0.05，角速度方差0.02；观测噪声距离方差0.1米，角度方差0.5度。
这个参数组合跑出来的四种算法差异非常清晰。标准EKF在回环前误差缓慢增长，回环后RMSE会有一个明显的拐点上升，协方差却持续缩小；Ideal EKF全程平滑收敛；FEJ和OC都基本保持一致，但OC的NEES更贴近1。如果想把差异放大，可以把观测距离噪声调到0.2米，角度噪声调到1度——这时候标准EKF甚至可能出现局部发散，而OC-EKF仍然稳定。
我实测下来还有一个经验：路标数量不要太多，20个左右比较合适。路标太少（比如5个），回环约束太弱，四种算法差异不够分明；路标超过40个，状态维度变大，协方差矩阵的数值稳定性成为新问题，容易把注意力从算法对比转移到数值调试上。
4. 实验对比方案与结果判读
4.1 对比指标：RMSE、NEES、一致性
光看轨迹曲线不够，判断滤波器行为是否正确需要量化指标。这套代码里我用了三个指标。第一个是轨迹RMSE——每一时刻机器人位置估计与真值的欧式距离平方和的开方。RMSE直接反映估计精度，但它不能反映滤波器是否“诚实”：一个协方差虚小的滤波器可能RMSE不大，但它的置信区间完全没覆盖真值，这种结果比RMSE稍大但协方差合理的状态更危险。
第二个指标是NEES（Normalized Estimation Error Squared）。定义是：
[ \text{NEES} = (x_{est} - x_{true})^T P^{-1} (x_{est} - x_{true}) ]
在一致滤波器中，NEES的期望值等于状态维度。对于机器人位姿3维状态，理想的NEES是3左右。用多次蒙特卡洛平均后，NEES远大于3说明滤波器过于自信，一致性差；远小于3说明滤波器过度保守，协方差膨胀。我跑完100次蒙特卡洛后，标准EKF的平均NEES在6到8之间，OC-EKF稳定在3左右——这就是一致性改善的直观证据。
第三个指标是路标位置估计误差的均方根误差，衡量建图质量。轨迹好不一定地图好，但两个指标在SLAM里又是强相关的。我在代码里也单独统计了每个路标的估计误差，方便分析是哪个方向的误差主导。
4.2 典型结果与判读方式
跑完仿真，可视化除了一般的轨迹对比外，我建议重点看三个图。第一张是全局轨迹图，四种算法的轨迹叠加在真值轨迹上——这时候你能直观看到标准EKF的轨迹在回环处扭曲，而Ideal、FEJ、OC的轨迹几乎贴在一起。第二张是NEES随时间变化曲线，用对数坐标展示，标准EKF的NEES在中后期持续走高，Ideal和OC都保持平稳。第三张是协方差椭圆图，这个最能看出问题：标准EKF在回环后椭圆迅速缩小，但中心与真值偏离很大，典型的“自信而错误”；OC-EKF的椭圆收敛节奏更平稳，中心也更接近真值。
一些细节经验：只看单次仿真曲线，噪声的随机性可能掩盖算法差异，尤其是轨迹RMSE这种积分量。所以正式对比一定要做蒙特卡洛统计，代码里建议跑50到100次，把每次的指标记录下来求均值。每次仿真单独重置随机数种子。
4.3 Monte Carlo统计的必要性
我自己第一次跑这套代码时偷懒只跑了一次，结果FEJ-EKF的NEES甚至比标准EKF还差，跟理论预期完全相反。后来检查发现是某次随机噪声刚好补偿了标准EKF的漂移方向。把仿真次数提到50次后，FEJ的优势才稳定显现。蒙特卡洛次数太少，噪声样本的偶然性会完全压过算法差异。这也提醒一点：仿真对比结论必须建立在统计意义上，不能拿单次路径对比就说某个算法好。
另外要注意，NEES的95%置信区间和状态维度、蒙特卡洛次数相关。对于3维位姿状态和100次蒙特卡洛实验，NEES在2.4到3.6之间都算正常范围。OC-EKF逐次仿真的NEES方差也更小，这一点可以从多次仿真的NEES曲线带宽度看出来——曲线带越窄，说明算法对不同噪声序列的响应越稳定。
5. 仿真过程中踩过的坑
5.1 滤波器凭空发散的常见原因
我跑这套仿真最早遇到的问题是，某些随机种子下标准EKF在特定时刻协方差变成非正定矩阵，然后直接NaN。检查后发现是协方差传播时没有对称化。EKF更新后，数值误差会让$P$轻微不对称，矩阵快速变大时不对称误差被放大。解决办法非常简单——每次更新完加一行：
P = 0.5 * (P + P');另外，R矩阵过小也会导致发散。你在仿真里看到协方差快速缩小到接近0，然后突然跳变，多半是R矩阵设置过小导致增益过大。把R的数值调到与噪声实际水平一致，而不是“为了效果好看”调小，能从根本上避免这个问题。
5.2 雅可比计算错误导致的可观性漏洞
OC-EKF里有个隐蔽的坑：既然是投影修正，那原始$H_k$必须算对，否则投影目标就错了。我一开始用解析推导的雅可比，路标索引和状态分段的对应关系写错了，导致观测雅可比里路标位置列全错位。症状非常有意思——四种算法结果全都正常，但OC-EKF的NEES反而偏高。原因是投影时把$H_k$当作正确的系统模型偏导，结果它本身就是错的，投影只是把错误合理地“校准”到零空间上，仍然得不到正确更新。
排查方法也很笨但有效：先单独跑标准EKF，对比滤波器估计和真值的差值是否与观测噪声水平一致；再写一个数值差分函数，验证解析雅可比是否正确。这一步过了，再谈OC约束才有意义。
5.3 数值稳定性与病态矩阵
状态维度增大到一定规模（路标超过50个），$3+2N$维的协方差矩阵病情会越来越重。观测更新中要计算$S = HPH^T + R$的逆，$S$本身维度不大，但如果路标间距近，$H$的两行走向量会很接近，$S$接近奇异。代码里用pinv而不是inv是一种懒但稳的做法，不过更推荐的是在逆运算前对$S$加一个小的单位阵扰动，比如(S + 1e-6*eye(size(S)))。这个技巧能让仿真跑大量路标工况时稳定很多。
5.4 把仿真代码扩展到自己场景的小建议
这套代码的框架很好扩展。如果你想把圆形轨迹改成仿真的八轨迹、方形轨迹或者更复杂的路径，只需要替换参考轨迹生成部分，路标位置也可以按场景重新生成。要换成3D SLAM，状态向量和观测模型都要扩展，但可观测性约束的投影思想完全一样——3D SLAM的不可观方向增加到4个（全局3D位置加全局航向），投影公式和零空间求法不变。我建议扩展时保留四种算法共存的对比框架，这样每次改进模型，都能立刻验证是否引入了新的可观测性偏移。
如果你只是想快速对比滤波精度，我还有一个更省事的建议：把路标数量固定在20、R矩阵稍大一点、跑100次蒙特卡洛，得到的结论基本能代表这类问题的普遍性质。别为了追求“惊艳”的曲线效果而故意把噪声调得异常大，那样的结果拿到答辩或评审里，反而容易被追问场景合理性。
仿真代码这个东西，跑通不算本事，跑明白才算。所谓“明白”，就是你能清楚说出四种算法在每一帧雅可比计算上的差异、知道协方差变化背后的可观测性逻辑，并且面对反常识的仿真曲线时能定位出问题出在模型、数值还是算法逻辑。这套OC-EKF对比框架恰好给了你一个最小完备的测试场——算法实现复杂度适中、理论脉络清晰、对比结论可复现，拿来学习SLAM一致性问题和滤波设计，真的合适。
