企业官网建设流程全解析

很多人第一次跑通EKF-SLAM的时候，都会有同一个困惑：滤波器的协方差越走越小，看上去自信满满，可实际上机器人早就漂到不知道哪里去了。这个问题不是调参能解决的，它本质上是EKF-SLAM的“不一致性”问题。我当年在做某模拟项目X的可观测性分析时，被这个问题卡了近两周，最后把可观测性矩阵拆开才算看明白。这篇文章就从一个比较硬核但能落地的角度来聊——用可观测性理论研究EKF-SLAM为什么不一致，并且给出完整的Matlab代码实现思路。内容既包含原理推导，也包括仿真环境搭建、评价指标计算和FEJ修复实现，适合正在做SLAM研究或者在工程里被滤波发散折磨的同学。

1. 不一致性问题在EKF-SLAM中是如何表现出来的

1.1 一个反直觉的仿真现象

先抛一个我实测过的现象。假设一个机器人绕一个20米见方的场地走一圈，周围布置了8个路标点，用距离-方位角传感器观测。用标准EKF-SLAM跑50次蒙特卡洛仿真，把真实的均方根误差与滤波器输出的标准差做对比，会发现一个非常刺眼的结果：滤波器认为自己在横纵方向上的定位精度已经到厘米级，而实际误差却有几十厘米，甚至在某些方向上接近1米。

这个现象用一句专业的话概括就是：真实误差远大于协方差所暗示的误差，滤波器过于自信。如果只看单次运行轨迹，你甚至会觉得EKF表现还不错，因为轨迹画出来挺平滑，误差也没有爆炸。但把50次运行的真实误差画成误差椭圆，再叠上滤波器输出的协方差椭圆，两者的形状和大小差距会非常直观——协方差椭圆明显偏小。

这可不是“噪声参数没调好”那么简单。我试过把过程噪声调大、观测噪声调大，滤波器的协方差确实会跟着变大，真实误差却不会成比例改善，两者之间的失配始终存在。这个失配，就是SLAM领域常说的“不一致性”。

1.2 不一致性对建图结果的具体危害

不一致性最直接的危害体现在三个地方：

• 协方差的工程可信度下降。后续的路径规划、回环检测、地图融合都要依赖协方差来评估风险。协方差不准，上层算法的判断就全不可信。
• 长时运行中误差会加速累积。因为滤波器低估不确定性，就会过度信任历史状态，导致漂移在时间上的“刹车”失效。
• 数据关联失败率上升。数据关联经常用马氏距离做门控，而马氏距离依赖正确的协方差。协方差偏小，正确匹配被拒绝的概率就高，错误的匹配反而可能通过。

我在实际仿真里还碰到过一个更隐蔽的问题：滤波器的不一致性会导致地图发生“虚假收缩”。多条轨迹观测同一个路标集合时，EKF倾向于把新路标“拉”到当前估计位姿附近，因为滤波器对自己的位姿太自信了。于是路标点的分布会逐渐向轨迹靠拢，看起来像整个地图在往机器人身上缩。

如果你只是看单次运行的地图，未必能发现这个问题，因为它是统计性的。想要把它定量地抓出来，就需要引入可观测性分析这个工具。

2. 可观测性分析：理论零空间为何在滤波器中被悄悄压缩

2.1 从状态空间谈可观测性

在说可观测性之前，先明确一下SLAM的状态里到底有什么。经典EKF-SLAM的状态向量由机器人的位姿和所有路标位置组成。在2D场景中，机器人位姿是三维的：横向位置、纵向位置、朝向角。如果有N个路标，每个路标有横纵两个坐标，那么状态向量的维度就是3 + 2N。

可观测性研究的是这样一个问题：给定一段时间内的传感器观测序列，能不能唯一确定系统的初始状态。如果一个方向的初始状态无论取什么值，都能产生完全相同的观测，那这个方向就是不可观测的。

对于2D平面SLAM，稍微想一下就能发现：如果把机器人轨迹和所有路标点同时进行一个全局的刚体变换——整体平移(x和y方向)加上整体旋转(θ方向)——所有距离和方位角观测值完全不变。传感器测的是相对关系，它永远无法告诉你“绝对位置在哪”。因此，2D SLAM的不可观测方向有且只有三个：全局x平移、全局y平移、全局旋转。对应地，可观测性矩阵的理论秩应当为 (3+2N) - 3 = 2N。

2.2 EKF线性化如何破坏零空间

问题来了：EKF整个算法建立在雅可比矩阵之上，每次预测和更新都要在当前估计值处做一阶泰勒展开。这意味着系统不再严格是原来的非线性系统，而是一个沿着估值轨迹不断变化的线性时变系统。这个线性时变系统的可观测性性质，取决于雅可比矩阵的取值。

理论上的非线性系统具有三个精确的不可观测方向，但线性化系统不一定继承这个结构。当机器人的朝向角估计值不等于真实朝向时，观测模型的雅可比矩阵会把机器人的横向误差和朝向误差耦合在一起，使得“全局旋转”这个本不可观测的方向被拆成了多个方向上稀疏可观测的组合。

这个过程我在仿真里完全复现过。把每一个时刻的观测雅可比H和状态转移雅可比F取出来，用数值方式构造累积可观测性矩阵并计算秩，会得到两种截然不同的结果：

多出来的那一个可观测方向，就是不一致性的根源。因为滤波器认为自己能够观测某个方向，但实际上观测里根本没有这个信息，于是卡尔曼增益会在这个方向上对状态进行错误的修正，同时把协方差压小。

2.3 为什么失调集中在朝向角上

具体到EKF-SLAM，多出来的一维可观测方向通常对应全局航向角。原因在于机器人朝向角估计值参与到了观测雅可比矩阵的计算中。当朝向角存在误差时，观测模型对机器人x、y位置的偏导数与对朝向角的偏导数之间存在虚假的消隐关系，使得原本属于全局旋转零空间的一部分被错误地投影到了可观测空间。

这也可以解释为什么带路标的EKF-SLAM比纯里程计更容易在转角处出现大误差：转弯时朝向角误差最大，虚假可观测性也最强，一次错误的修正就可能让位置的估计被“顺势”拉偏。

严格说，用李导数分析非线性系统的可观测性更加严谨，但在工程仿真中，对线性化系统做可观测性矩阵秩分析已经足以诊断问题。这也是我在Matlab里选择的方向。

3. Matlab仿真环境的搭建与基线EKF-SLAM实现

3.1 仿真数据的生成方式

做可观测性研究，强烈建议先用仿真数据而不是真实数据集。真实数据里数据关联噪声、传感器噪声特性未知，会把不一致性的统计检验搅得一团糟。仿真数据完全可控，所有真相都是已知的，方便做严格对比。

我使用的仿真场景是这样的：机器人沿一条包含直线和圆弧的闭合轨迹运动，速度为0.5 m/s，控制周期0.1 s。地图中随机布置若干个路标，每个路标都能被传感器在5米范围内观测到。观测模型输出相对距离和相对方位角，距离噪声标准差0.05 m，角度噪声标准差0.5度。里程计模型使用距离增量与转角增量，距离噪声标准差0.02 m，转角噪声标准差0.3度。

下面是数据生成的核心代码骨架：

% 参数设置 dt = 0.1; % 时间步长 v = 0.5; % 线速度 w = 0.3; % 角速度 nSteps = 800; % 总步数 % 真实轨迹生成 xTrue = zeros(3, nSteps); for k = 2:nSteps xTrue(:,k) = [xTrue(1,k-1) + v*dt*cos(xTrue(3,k-1)); xTrue(2,k-1) + v*dt*sin(xTrue(3,k-1)); xTrue(3,k-1) + w*dt]; end % 路标真实位置 rng(42); landmarksReal = [ rand(1,8)*20 - 10; rand(1,8)*20 - 10 ];

这里有个小技巧：路标的横纵坐标用固定随机种子生成，保证每次实验的地图一致，方便蒙特卡洛之间做横向对比。随机种子的固定是很多仿真实验容易忽略的一步，不固定种子的话，50次蒙特卡洛结果不可比，也没法判断指标差异是地图差异还是算法差异。

3.2 基线EKF-SLAM的状态增广与初始化

状态向量初始化是整个实现里最需要小心的部分。初识机器人位姿已知，协方差给一个很小的非零值，比如对角线为1e-4。注意不要给零协方差，否则滤波器直接锁死，后续任何信息都无法改变状态，这在可观测性分析中会形成假的一致。

路标状态采用延迟初始化：第一次观测到某路标时，利用当前机器人位姿估计值反算路标位置，并放大协方差以反映位姿不确定性带来的初始化误差。

% 第一次观测到路标i时的初始化 x_landmark = [ xEst(1) + z(1)*cos(xEst(3) + z(2)); xEst(2) + z(1)*sin(xEst(3) + z(2)) ]; % 对应协方差块 G = [1, 0, -z(1)*sin(xEst(3)+z(2)); 0, 1, z(1)*cos(xEst(3)+z(2))]; P_landmark = G * P_pose * G' + zR * eye(2);

初始化算子是EKF-SLAM里最容易埋雷的地方。如果初始化协方差给得太小，路标位置会被“钉死”在错误的位置上，后续观测量再多也救不回来。业内一般管这个叫信息锁死效应，它在统计上也会表现为不一致性。所以在基线实现里，我刻意把初始化协方差块算得保守一些，留出余量。

3.3 预测与更新循环的实现要点

预测阶段，只有机器人位姿部分发生改变，路标部分保持不变。因此状态转移矩阵是一个分块对角结构，左上角是机器人运动模型的雅可比，右下角是单位阵。

d = sqrt((ux)^2 + (uy)^2); phi = wrapToPi(ut(2)); xPred(3) = wrapToPi(xEst(3) + phi); xPred(1) = xEst(1) + d * cos(xEst(3) + phi/2); xPred(2) = xEst(2) + d * sin(xEst(3) + phi/2); G = [1, 0, -d*sin(xEst(3)+phi/2); 0, 1, d*cos(xEst(3)+phi/2); 0, 0, 1]; F = blkdiag(G, eye(2*maxLandmark)); P = F * P * F' + Q;

更新阶段，观测模型对机器人位姿和对应路标的雅可比矩阵要分别求。距离和方位角对位姿的偏导容易推导，但对路标的偏导要特别注意符号，很多手写EKF结果不对都是错在这一步：

dx = lmx - xEst(1); dy = lmy - xEst(2); q = dx^2 + dy^2; range = sqrt(q); bearing = wrapToPi(atan2(dy, dx) - xEst(3)); H_pose = [-range*dx, -range*dy, 0; dy, -dx, -q]; H_pose = H_pose / q; H_landmark = [range*dx, range*dy; -dy, dx]; H_landmark = H_landmark / q; % 拼接成完整H矩阵，其余位置为0 H = zeros(2, dim); H(:, 1:3) = H_pose; H(:, idx_landmark) = H_landmark; S = H * P * H' + R; K = P * H' / S; xEst = xEst + K * (z - zPred); P = (eye(dim) - K*H) * P;

3.4 数据关联的处理约定

这里必须说明一个约定：本文所有实验都假设数据关联已经给定，即每一步都知道当前观测对应哪个路标。在真实SLAM中，数据关联错误对不一致性的影响往往比滤波算法本身更大，但那是另一层问题。为了把研究焦点集中在可观测性上，先用已知关联把基线跑通，再扩展才是合理的路线。

当然，这也意味着本仿真的结论是在“理想数据关联”条件下得出的。一旦引入数据关联，NEES曲线会普遍上移，但这并不会改变可观测性结构对一致性的主导作用。

4. 把不一致性“测”出来：可观测性矩阵与NEES指标的Matlab实现

4.1 可观测性矩阵的数值构造

可观测性矩阵的构造方法，我推荐直接沿时间轴累积：

记F_k为第k步预测的雅可比矩阵，H_k为第k步观测的雅可比矩阵，则从第0步到第k步的累积可观测性矩阵为：

O = [ H_1; H_2 * F_1; H_3 * F_2 * F_1; ... H_k * F_{k-1} * ... * F_1 ]

在EKF-SLAM中，这一累积过程可以显式实现。Matlab里为了数值稳定性，我不会直接累乘全部F矩阵，而是从后往前递推乘积：

function O = computeObservability(Fs, Hs, k) % Fs: 1到k-1的状态转移雅可比cell数组 % Hs: 1到k的观测雅可比矩阵cell数组 dim = size(Fs{1}, 1); O = zeros(2*k, dim); Phi = eye(dim); for j = k:-1:1 O(2*j-1:2*j, :) = Hs{j} * Phi; if j > 1 Phi = Fs{j-1} * Phi; end end rankO = rank(O, 1e-6); end

用这个函数分别传入“真实轨迹上的雅可比”和“EKF估计轨迹上的雅可比”，就能得到两种秩。我在大量实验中统计到的结果非常稳定：理论秩2N，一个含有N个路标的场景下，真实雅可比给出的秩恰好是2N，而EKF估计雅可比给出的秩则普遍为2N+1。

4.2 NEES与RMS的组合诊断

仅仅看秩还不够，因为秩反映的是结构性问题，要判断一次具体运行是否不一致，还要用统计指标。NEES（归一化估计误差平方）是标准的滤波器一致性指标：

epsilon_k = (x_true,k - x_est,k)^T P_k^{-1} (x_true,k - x_est,k)

对于维度为d的高斯系统，NEES的理想均值应等于d。EKF-SLAM中状态维度很高，直接对全状态计算NEES会受路标初始化的巨大协方差影响。所以我在实践中分两部分看：一部分只统计机器人位姿的NEES，另一部分统计路标部分的NEES。

function [poseNees, mapNees] = computeNEES(xTrue, xEst, P, nPose) e = xTrue - xEst; e(3) = wrapToPi(e(3)); poseNees = e(1:nPose)' / P(1:nPose,1:nPose) * e(1:nPose); eMap = e(nPose+1:end); mapNees = eMap' / P(nPose+1:end,nPose+1:end) * eMap; end

做50次蒙特卡洛后，取每个时刻的均值并与卡方分布的置信区间比较。以机器人位姿为例，维度d=3，95%置信区间是均值落在3.0附近的范围内。50次样本时阈值大概在1.75到4.50之间。真实EKF的位姿NEES均值通常会冲到10以上，甚至到20，远超置信区间上界。

4.3 基线仿真结果：一致性的“血压计”读数

以我的仿真配置为例，基线EKF-SLAM跑完800步，位姿NEES均值的典型走势如下：前100步还比较克制，在5左右徘徊；200步之后开始爬升，到300步已经超过10；后半程基本在12到18之间波动，几乎不可能回到置信区间内。

路标部分的NEES更夸张，因为路标估计的协方差在初始化后快速缩小，但真实误差并没有同步缩小。某几个路标的NEES在400步后可以到30以上。

RMS误差与平均标准差的对比也很说明问题。把横纵方向的RMS位置误差和协方差开方后的平均标准差放在一起：

比值越来越大，说明滤波器输出的不确定性估计越来越脱离真实误差，这就是不一致性的统计画像。

5. 基于可观测性约束的修复：FEJ-EKF的代码级实现

5.1 FEJ的核心思路：固定线性化点

知道了问题根源，修复就有了明确方向：让线性化系统的可观测性矩阵保持与理论非线性系统一致的秩。最经典的实现方法叫First-Estimates Jacobian，即第一估计雅可比法。它的思想是：对于每个路标状态，在其第一次被初始化时记录当时的线性化点，之后所有与该路标相关的雅可比矩阵都在这个固定线性化点上计算，而不是每次使用当前最新的状态估计。

为什么固定线性化点会有效？因为可观测性矩阵的秩被破坏，根源在于机器人的朝向估计值和路标位置估计值使用了不同时刻的线性化点，导致雅可比矩阵之间出现虚假的相关性。FEJ强制所有变量在首次估计时取点，相当于恢复了理论系统中“零空间保持”的条件。

5.2 Matthews代码实现细节

以第i个路标为例。初始化阶段，记录路标的第一估计位置lmFirst_i和机器人当时的第一估计位姿poseFirst_i。之后在计算观测雅可比矩阵H时，不再使用xEst中的路标位置和位姿，而使用这两个被冻结的量：

% 首次初始化时记录线性化点 lmFirst_i = xEst(idx_i); poseFirst_i = xEst(1:3); % 之后每次计算H时使用固定线性化点 dx = lmFirst_i(1) - poseFirst_i(1); dy = lmFirst_i(2) - poseFirst_i(2); q = dx^2 + dy^2; range = sqrt(q); H_fej = [ -range*dx/q, -range*dy/q, 0, range*dx/q, range*dy/q ];

有一点要特别注意：预测阶段机器人位姿的状态值依然要用最新的估计值更新，FLFEJ冻结的只是用于计算雅可比矩阵的线性化点，不是滤波器状态本身，否则整个滤波器就变成开环了。

5.3 实现时容易踩的三个坑

第一个坑是朝向角的wrap问题。记录的第一估计朝向角与当前估计之间相差可能超过π，不要直接做常规减法，要用wrapToPi统一。

第二个坑是线性化点的尺度一致性。FEJ冻结的线性化点一旦确立，后续所有关联该路标的观测H都要指向它，如果代码里不小心用了某个中间时刻的估计值，可观测性修正就彻底失效了。我给每个路标单独维护一个结构体数组，字段包括firstLandmarkPos、firstRobotPose，并且只允许在初始化分支里写入。

第三个坑是地图扩展时矩阵索引的错位。新增路标后，状态向量维度变了，如果用固定数字索引写死雅可比的位置，一旦中间有路标从未被观测过，索引就会错乱。更好的做法是每个路标保存一个全局索引ID，H矩阵按ID动态拼接。

5.4 其他修复思路简析

FEJ并不是唯一的修复方式，但它是理解和实现成本最均衡的一个。另一个常见的思路是观测约束EKF，它对状态协方差施加可观测性约束，利用投影矩阵强制协方差的不可观测方向与理论一致。这个方法的数值实现比FEJ复杂，需要做SVD分解和低秩投影，而且对线性化点的处理如果不到位，与FEJ结合的变体更容易引入数值波动。

还有一种思路是使用UKF或粒子滤波，从根源上减少线性化误差，但计算量大幅上升，且在高维状态空间下仍然存在采样稀疏问题。

我的判断是：如果是做研究复现，首选FEJ；如果是工程应用，建议在FEJ基础上再叠加回环检测缓解累积漂移。FEJ不是万能的，它主要解决一致性，不解决数据关联和全局优化的问题。

6. 仿真对比结果与工程落地建议

6.1 无约束EKF与FEJ-EKF的指标对比

把FEJ-EKF放进同样的仿真环境里跑50次蒙特卡洛，效果非常明显。

位姿NEES均值从无约束EKF的10到18，一路降到2到5之间，绝大部分时刻落在95%置信区间内。虽然不能说全程完美一致，但相比基线已经有了质的改善。路标NEES的下降幅度更大，多个路标的NEES从30以上降到8以内。

RMS误差方面，由于可观测性结构修复了，滤波器在长时运行中的漂移速度明显变慢。在800步仿真结束时，FEJ-EKF的RMS位置误差约0.12米，标准差约0.09米，比值从基线EKF的4.3降到1.4左右。这个结果与我看到的公开文献中的趋势一致。

可观测性矩阵的秩也回到了理论值2N。我特意在Matlab里加入了对秩的在线监测，FEJ模式下整条轨迹上都没有再出现过秩+1的情况。

6.2 不同噪声水平下的稳定性实验

我又做了一组噪声敏感性实验：把距离观测噪声标准差从0.05米依次调到0.1、0.2米，角度噪声从0.5度调到1度、2度。无约束EKF的NEES在这些参数上调高后普遍恶化，均值和波动范围都在变大；FEJ-EKF对噪声参数变化则相对稳健，NEES均值在噪声放大4倍后也只从3左右升到6左右。

这组实验的结论很重要：FEJ的修复不是“恰好治好了某种噪声下的病”，而是从算法结构上堵住了不一致性的源头。噪声参数的变化只会影响滤波器的绝对精度，但不会重新引入结构性失真。

6.3 从仿真走向实车之前的三点经验

先说线性化点冻结的长度。FEJ在长时间、大规模地图上的性能会受线性化点过旧的影响，极端情况下会降低定位精度。工程上常见做法是每隔一段时间对协方差做一次“重参数化”，或者配合回环检测在确认闭环后重置部分线性化点。

再说数据关联。实测数据中数据关联错误会掩盖FEJ带来的收益，所以实车实验前一定要先把外点剔除和数据关联门控调好。我见过不少人把FEJ接上真实数据集后发现NEES还是高，最后排查下来发现八成是数据关联错误。

最后说雅可比的计算方式。做可观测性分析时，强烈建议解析雅可比而不是数值雅可比。Matlab的有限差分在状态维度很高时容易受到round-off误差影响，差分会放大微小扰动，导致秩的计算不稳定。解析雅可比初写麻烦，但一旦写好，后续做FEJ和可观测性秩分析都要稳稳得多。

6.4 给后续要写代码的同学的几句掏心话

这块代码说难不难，但细节极多。我建议不要一上来就追求在真实数据集上跑通，先把仿真环境做扎实，把NEES和可观测性秩的脚本都写好，确认各个环节的数值结果是稳定可复现的，再往上叠加功能。仿真阶段每加一个功能就要重新跑一次蒙特卡洛，别看单次运行很快，50次全跑下来数据量的波动会暴露许多一次运行看不出的小问题。

我现在做项目时，会把可观测性秩分析直接集成到仿真框架的每个里程碑节点作为CI检查项，一旦出现秩异常立刻报警。这个方法看似简单，但在排查SLAM问题时比看轨迹图高效得多。希望你也能从这篇文章里拿到这个思路，少走我刚入坑时走过的弯路。