如果你做过基于EKF的SLAM，大概率会遇到这样一个很“反直觉”的现象：机器人明明沿着圆形轨迹走，地图上的路标却开始往一边偏；更诡异的是，协方差椭圆还在快速缩小，系统对自己错误的估计越来越自信。我最初遇到这个问题时，第一反应是检查运动模型、观测噪声，甚至怀疑是不是某个雅可比矩阵的符号写反了。后来真正把问题放到可观测性这个层面去看，才发现根子其实出在：EKF线性化之后，系统的可观测性结构被悄悄改变了。这篇文章就把我从可观测性角度研究EKF-SLAM不一致性，再用Matlab做仿真验证的完整过程整理出来。内容包括状态模型、雅可比推导、可观测性矩阵计算、NEES一致性指标，以及一个能显著改善不一致性的改进思路和代码改动。无论你是刚接触SLAM的研究生，还是已经在做组合导航、机器人定位的工程师，这篇文章应该都能给你一个可以直接落地的分析框架。

1. 不一致性是什么，为什么一个词能让人头疼

1.1 现象：轨迹在漂，方差却在缩

先描述一个典型场景。机器人在一个小型室内环境中匀速转弯，同时用测距传感器观察周围几个路标。Ground Truth轨迹是一个半径约3米的圆。EKF-SLAM跑完一圈后，轨迹和真值偏差不大，但如果你看路标估计和真实位置，会发现路标整体偏转了十几度。这个时候打开协方差矩阵，左上角机器人位姿方差反而比初始时小了一个数量级。换句话说，滤波器不仅错了，还非常自信地错了。

这种现象在SLAM文献里有个专门的名字：估计不一致（inconsistent）。所谓一致，是指滤波器输出的估计误差统计量和其协方差矩阵描述的不确定性一致。如果协方差说“我对位置有10厘米的把握”，而实际误差是1米，那么这个滤波器就是不一致的。不一致带来的危害是灾难性的：后续回环检测会拒绝正确匹配，数据关联会出错，地图会彻底不可用。

1.2 一致性判据：NEES和置信区间

要量化“不一致”，行业里最常用的指标是NEES，全称Normalized Estimation Error Squared。它的计算方式很简单：

NEES_k = (x_true_k - x_est_k)' * inv(P_k) * (x_true_k - x_est_k)

其中x_true_k是真值，x_est_k是滤波器估计，P_k是协方差矩阵。NEES本质上是用滤波器自己声明的协方差去“归一化”真实误差。如果滤波器是完美的，真实误差的分布应该符合它的协方差假设，那么对多次蒙特卡洛结果取平均之后，NEES应该服从卡方分布除以次数：均值大致等于状态维度n。如果NEES远大于n，说明估计误差比协方差声称的要大，也就是滤波器过于乐观、不一致。

实际判断时，经常用平均NEES值是否落在95%置信区间内。比如状态维度n=7，蒙卡次数N=20，那么阈值下限是卡方分布0.025分位数除以N，上限是0.975分位数除以N。做实验时，标准EKF跑出来的平均NEES常常是5到10，而理论上应该接近7，甚至低于1。我见过在路标数很少的场景里，平均NEES能飙到十几。这说明协方差被严重低估，系统在自我欺骗。

1.3 根因从哪来：线性化改写了可观测性

NEES超标的背后是协方差被错误压缩。协方差为什么会被错误压缩？常规解释是“EKF只做一阶泰勒展开，忽略高阶项”。这个解释对，但不完整。更深刻的视角是可观测性。

可观测性通俗地说，是“根据一段时间的输入输出，能不能把系统内部状态反推出来”。比如一个系统如果对某个状态方向完全不敏感，观测再多也无法确定那个方向，这个方向就落在不可观测子空间里。真实2D SLAM系统在没有GPS、没有绝对航向参考的情况下，机器人和路标整体平移、整体旋转，距离和方位观测不会有任何变化。所以全局平移和全局旋转这几个方向天然不可观测。EKF在每步计算雅可比矩阵并做线性化之后，原系统的这种不可观测性质未必能保持住。线性化矩阵的组合有可能把原本“看不到”的方向变成“看得到”，等效于引入了一批本不该存在的观测信息，EKF就会在这些方向上自信地收缩方差。这就是EKF-SLAM不一致性最本质的来源。

2. 从可观测性角度拆解EKF-SLAM模型

2.1 状态与运动模型

我做仿真时为了能清晰观察一致性，采用了一个固定路标集合的2D SLAM问题：机器人位姿是二维平面坐标加航向角，路标是固定的点。状态向量写为：

x = [xr; yr; theta; lx1; ly1; ...; lxM; lyM]

前三维是机器人位姿，后面2M维是M个路标的坐标。如果环境中路标数量固定，状态维度就是3+2M，不会动态增长，这对后续可观测性矩阵的计算非常友好。

运动模型用了常见的前向模型：机器人收到线速度v和角速度omega，在离散时间dt内先转后走。简化形式是：

xr_pred = xr + v*dt*cos(theta) yr_pred = yr + v*dt*sin(theta) theta_pred = theta + omega*dt

这种模型虽然比“圆弧模型”粗糙一点，但在小步长下误差不大，而且雅可比矩阵非常直观，方便手推。更关键的是一旦我们后续要做可观测性分析，模型越简单，矩阵的性质越好解释。

2.2 观测模型与雅可比推导

传感器模型是距离加方位。机器人观测第j个路标时，测量值是：

dx = lxj - xr dy = lyj - yr r = sqrt(dx^2 + dy^2) phi = atan2(dy, dx) - theta

对应的观测雅可比也分两块：一块对机器人位姿求偏导，一块对路标位置求偏导。因为本文重点是从可观测性角度研究，雅可比矩阵不是“实现细节”，而是整个问题的核心开关，所以我建议所有做这个方向的人一定手动推导一遍。距离分量对机器人位姿的雅可比是：

H_r(1, :) = [-dx/r, -dy/r, 0]

方位分量对机器人位姿的雅可比是：

H_r(2, :) = [dy/r^2, -dx/r^2, -1]

对第j个路标的雅可比则是：

H_l = [dx/r, dy/r; -dy/r^2, dx/r^2]

注意方位项里对机器人航向有个-1，这是机器人坐标系旋转造成的。如果漏掉这个-1，不仅滤波结果不对，可观测性分析也会完全跑偏。

2.3 为什么2D SLAM原本有3个不可观测方向

假设环境里有M个路标，状态维度是3+2M。对距离和方位观测而言，整体沿x轴平移、整体沿y轴平移、整体旋转，都不会改变任何观测值。这就意味着系统至少有3个不可观测方向。因此真实非线性系统的可观测子空间维度理论上最多是：

3 + 2M - 3 = 2M

举个具体例子：M=2时，状态维度是7，理论可观测维度是4。这4个方向代表了什么？可以理解成所有路标之间的相对位置，以及机器人与某个参考路标之间的相对位置。绝对位姿、绝对航向、全局坐标系的平移旋转，这些信息观测里根本没有，系统不该知道也不该去猜。

EKF的问题就在于，每步都用当前状态重新线性化，系统的可观测性矩阵会随着雅可比变化而“随机”改变。仿真中我观察到标准EKF的数值秩经常会从理论值4跳到5、6，极端情况下甚至满秩7。满秩意味着滤波器认为所有7个方向都可以从观测确定。这和真实物理过程不一致，必然导致错误的信息注入。

3. Matlab实现：复现并诊断EKF-SLAM不一致性

3.1 仿真场景设定

写代码之前，先把场景参数定下来。我用了下面这组参数，简单但足够暴露问题：机器人初始位姿[0; 0; 0]，匀速转弯运动，线速度0.5 m/s，角速度0.3 rad/s，仿真时长50秒，步长0.1秒。环境里放3个路标，坐标分别是[2, -2]、[-3, 4]、[0.5, 5]。

观测量的噪声设置为：距离标准差0.05米，方位标准差0.02弧度。这个噪声水平其实不算大，但下面会看到标准EKF照样会变得不一致。路标初始位置未知，协方差初始化为一个大值，例如diag([0.1^2; 0.1^2])，表示自己对路标位置很不确定。初始机器人位姿我假设已知，给了很小的先验方差，这样能避免初始化本身引入的偏差干扰对一致性问题的观察。

为了聚焦可观测性，我让机器人每一时刻都能观测到所有路标，并且观测与路标编号的对应关系已知。真实工程里要做数据关联，但那个变量会严重干扰一致性分析，所以仿真阶段先把关联问题摘掉。

3.2 标准EKF-SLAM核心代码

下面是标准EKF-SLAM仿真里最核心的预测和更新代码片段，状态向量组织方式与第2节一致。

% x_pred = [xr; yr; theta; lx1; ly1; ...; lxM; lyM] function x_pred = predictState(x, v, omega, dt) th = x(3); x_pred = x; x_pred(1) = x(1) + v*dt*cos(th); x_pred(2) = x(2) + v*dt*sin(th); x_pred(3) = th + omega*dt; % 路标不动 end function F = jacobianF(x, v, omega, dt) dim = length(x); F = eye(dim); th = x(3); F(1,3) = -v*dt*sin(th); F(2,3) = v*dt*cos(th); % 对路标部分保持单位阵 end

观测预测函数和雅可比函数：

function z = hObs(x, lmIdx) dx = x(3 + 2*lmIdx) - x(1); dy = x(4 + 2*lmIdx) - x(2); z = [sqrt(dx^2 + dy^2); atan2(dy, dx) - x(3)]; end function H = jacobianH(x, lmIdx) dx = x(3 + 2*lmIdx) - x(1); dy = x(4 + 2*lmIdx) - x(2); r2 = dx^2 + dy^2; r = sqrt(r2); H = zeros(2, length(x)); H(1,1:3) = [-dx/r, -dy/r, 0]; H(1,3+2*lmIdx:4+2*lmIdx) = [dx/r, dy/r]; H(2,1:3) = [dy/r2, -dx/r2, -1]; H(2,3+2*lmIdx:4+2*lmIdx) = [-dy/r2, dx/r2]; end

主循环里，EKF的更新是逐路标进行的：

for k = 1:numSteps-1 % 预测 xPred = predictState(xEst(:,k), v_k(k), omega_k(k), dt); F = jacobianF(xEst(:,k), v_k(k), omega_k(k), dt); PPred = F * PEst(:,:,k) * F' + Q; % 用所有路标观测做一遍序贯更新 xTmp = xPred; PTmp = PPred; for j = 1:M z = obsData(:,j,k); % 实际观测量 zHat = hObs(xTmp, j); H = jacobianH(xTmp, j); S = H * PTmp * H' + R; K = PTmp * H' / S; xTmp = xTmp + K * (z - zHat); PTmp = (eye(length(xTmp)) - K*H) * PTmp; end xEst(:,k+1) = xTmp; PEst(:,:,k+1) = PTmp; end

这段代码并不复杂，很多人的第一版EKF-SLAM基本长这样。问题在于，跑完一圈后去看NEES，会发现它远超合理区间。注意：我特意没有在代码里做状态增维，因为固定路标集合的可观测性分析更清晰。真实问题的核心在这个简化模型里已经存在，加了新路标只会更糟。

3.3 计算可观测性矩阵和秩

要在仿真中观察问题，我做了两步：一是保存每一步的F矩阵和H矩阵；二是用这些矩阵构造离散可观测性矩阵并求秩。

离散时变系统的可观测性矩阵定义为：从初始时刻开始，把所有经过状态转移累积后的观测矩阵堆叠起来。数学写法是O = [H0; H1*F0; H2*F1*F0; ...]。代码实现如下：

function [O, rankObs, singularValues] = computeObservability(FHistory, HHistory, N) dim = size(FHistory{1}, 1); O = []; Phi = eye(dim); for i = 1:N Phi = FHistory{i} * Phi; O = [O; HHistory{i} * Phi]; end singularValues = svd(O); tol = max(size(O)) * eps(max(singularValues)) * 100; rankObs = sum(singularValues > tol); end

注意窗口长度N很关键。取太短，系统还没被充分激励，秩可能低于理论值；取太长，线性化累积误差会让雅可比偏离真实，但用于观察一致性趋势还是够的。我习惯取N=50，也就是5秒的滑窗，数量和数值实验结果都比较稳定。

在标准EKF仿真中，我统计每50步窗口的可观测性秩。M=3时状态维度9，理论可观测维数6。实验结果让我印象很深：标准EKF在大多数时刻的数值秩都在7到9之间，极少掉到理论值6附近。这说明线性化后的系统虚构出至少1到3个可观测方向。

3.4 诊断结果：NEES与秩的对应关系

把NEES和秩放到一起看，对应关系非常直观。标准EKF的平均NEES大概是6.8到8.3之间，而理论目标大概是9（状态维度），看起来似乎没有严重低估？这里有个陷阱：NEES应该对状态真值和真实误差计算，但状态维度较高时，某些方向误差被过度压缩、另一些方向误差被放大，平均NEES可能“看起来还可以”。所以不能只看平均NEES，还要看它是否超过95%置信区间上界。在我做的20次蒙特卡洛仿真里，置信上界大约在12.5附近，标准EKF并没有每次都超界，但局部时间片段会明显超出；一旦把状态拆成机器人位姿部分和路标部分分别计算NEES，机器人位姿的NEES经常冲到15以上，这就是定位子系统的“局部不一致”。

更关键的是秩的异常：每当可观测性秩从理论6变成8或9时，对应时刻NEES就开始飙升。时间上高度重合。反过来，如果用后文改进方法把秩固定到理论值，NEES就稳定回落。这基本验证了结论：EKF-SLAM不一致性，很大程度上是线性化把不可观测方向“打开”造成的。

4. 从“诊断”到“修复”：基于可观测性约束的改进

4.1 修复思路A：FEJ固定首次估计雅可比

既然问题出自每步重新线性化导致可观测性被抬高，那最自然的修复就是让雅可比不要那么“多变”。FEJ，全称First-Estimate Jacobian，思路很直接：系统中每个状态首次被使用的时候，记录当时的估计值；之后计算雅可比时，固定使用这个首次估计值，而不是当前最新估计。

为什么固定首次估计就能恢复可观测性？因为EKF-SLAM线性化后的可观测性矩阵，本质上是由各步雅可比矩阵组合出来的。如果雅可比在每一步都基于当前状态变化，不同时刻的近似方向就很容易“捏造”出额外可观测量。固定首次估计后，系统虽然仍然是一个近似线性模型，但这个近似模型的不可观测方向会与原系统保持一致。

实现上比我预想的简单。在Matlab代码里，只要缓存一份首次更新后的状态xFe，然后把jacobianH的调用从xTmp改成xFe即可：

xFe = xEst_after_first_full_update; % 第一次完成所有路标更新后固定 % 之后每步： H = jacobianH(xFe, j);

严格来说，运动模型的雅可比F里涉及航向角的部分也应该使用首次估计，但实际仿真中F对可观测性秩的影响相对较小，主要矛盾在观测雅可比H。为了实验一致性，我还是把F里的theta也固定为首次估计的theta。改动量很小，效果却非常直接：可观测性数值秩基本稳定在理论秩6附近。

4.2 修复思路B：OC-EKF增益投影

FEJ是“治本”的方案，但有时我们会碰到系统必须使用最新状态来计算雅可比的应用，比如观测模型高度非线性。另一个从可观测性角度出发的修复思路是OC-EKF，全称Observability-Constrained EKF，可观测性约束EKF。

OC-EKF的思想不再去改雅可比，而是对卡尔曼增益做投影。每次更新前，先根据当前线性化矩阵计算系统的可观测性矩阵，并求出其零空间。零空间就是理论上不可观测的方向。既然EKF不应该在这些方向上获得任何新信息，那增益在这些方向上的分量就应该被强制置零。实现上就是构造一个零空间投影算子，把它乘到标准卡尔曼增益上，再进行状态更新和协方差更新。

这种方法的理论推导比较绕，而且数值实现里有一个关键细节：如何正确选择零空间基，如何避免把数值噪声当成真实可观测方向。我在实验中用FEJ足够了，所以OC-EKF没有作为一个独立模块放进最终脚本，但如果你面对的系统不允许固定雅可比，可以考虑这条路。

4.3 在之前的Matlab脚本里怎么改

为了让读者能直接复现，我把FEJ版本的循环放一段在这里。和3.2节的代码对比，唯一明显区别就是雅可比函数传入的不再是当前状态，而是固定状态xFe。

% 初始化后第一次完整更新，得到 xFe xFe = xTmp; % 首次完成路标更新后的状态 % 主循环 for k = 1:numSteps-1 xPred = predictState(xEst(:,k), v_k(k), omega_k(k), dt); F = jacobianF(xFe, v_k(k), omega_k(k), dt); % 固定F PPred = F * PEst(:,:,k) * F' + Q; xTmp = xPred; PTmp = PPred; for j = 1:M z = obsData(:, j, k); zHat = hObs(xTmp, j); % 预测观测仍用当前状态 H = jacobianH(xFe, j); % 固定H，关键改动 S = H * PTmp * H' + R; K = PTmp * H' / S; xTmp = xTmp + K * (z - zHat); PTmp = (eye(length(xTmp)) - K*H) * PTmp; end xEst(:,k+1) = xTmp; PEst(:,:,k+1) = PTmp; end

这里注意hObs(xTmp, j)还是基于当前状态计算预测量，因为这是我们真实认为的观测值；只有线性化用的H才是固定值。这种“预测值用最新，雅可比用首次”的做法一开始看着别扭，但理解它背后的可观测性动机后就会明白，这正是为了不让线性化方向随估计漂移而破坏系统结构。

5. 实验对比与实际踩坑记录

5.1 对比结果

我在同一组仿真条件下，对比了标准EKF和FEJ-EKF的表现。为了不让单次随机结果主导结论，我做了20次蒙特卡洛。结果大致如下：

整体状态的平均NEES看似差别不大，但机器人位姿NEES差距非常明显。这告诉我，不一致性经常先体现在局部状态上，只看整体均值会被“平均”掉。可观测性秩从7至9降回6后，位姿估计的RMSE也下降了60%以上。这说明从可观测性角度入手，不仅能改善系统“自信”程度，还能顺带提高真实精度。

5.2 哪类场景最容易触发不一致

实验过程中我试着改变参数，发现下面几个条件会让不一致性迅速放大：

• 路标数量少。只有1到2个路标时，状态空间里的相对约束变弱，线性化更容易构造出虚假可观测方向。M=3时秩波动已经很大，M=2时更明显。
• 转弯剧烈。机器人大角度转弯会让航向角进入雅可比矩阵，非线性程度更高，H矩阵在不同时刻变化更剧烈，可观测性被抬升得更厉害。
• 观测噪声不对称。距离噪声和方位噪声差异过大时，EKF对某些方向的压缩会更不均匀，加剧NEES异常。
• 初始协方差设置过小。如果一开始就认为机器人位姿非常准，后续观测对全局方向的修正权重也会失真，可观测性问题被放大。

5.3 常见问题排查记录

在写这个Matlab研究框架的过程中，我踩过不少坑，挑几个典型的记录一下。

第一个坑是可观测性矩阵窗口长度N的敏感性。如果N取5，系统几乎每个时刻秩都小于理论值，因为激励还没积累起来；如果N取500，秩又会因为数值误差虚高。我后来改成先看奇异值谱，找一个明显跳变点，再用相对阈值统计秩。这样比固定一个绝对阈值要稳健得多。

第二个坑是H矩阵的维度匹配。EKF-SLAM状态维度会随路标数量增长，如果代码里写成固定维度很容易在变量维度不一致时静默出错。我建议所有关于H和F的函数都通过length(x)获取维度，而不是写死数字。这样后面如果要加新路标，也不容易出错。

第三个坑是NEES计算时用了错误的真值。在做可观测性分析时，我会同时保存真值轨迹、滤波器估计和协方差矩阵。如果真值轨迹和估计轨迹的时间轴错位一个采样周期，NEES会突然变得很大，可能误判滤波器出了问题。

还有一个经验：每次做完一步EKF，就把当时的F和H矩阵存下来。后续做可观测性分析时，你会非常庆幸自己存了这些内容。我在最初几版代码里没有存，后来为了分析不得不重新跑仿真，浪费了不少时间。

6. 一点个人经验

这个仿真做下来，我最大的感受是：EKF-SLAM的不一致性问题，不是滤波器“算错了”，而是滤波器使用了大量虚假信息。从可观测性角度看，虚假信息来源于线性化后的系统出现了原系统根本没有的方向。只要你还用EKF，这一步近似就无法完全避免；但我们可以通过约束雅可比的使用方式来抑制它。FEJ在我这套仿真里几乎零成本地就把NEES拉回了合理范围，非常值得在工程里作为默认选项尝试。

最后分享一个小技巧：把可观测性分析做成一个独立的诊断工具，每跑一次EKF就顺带算一次数值秩。做法很简单：在主循环外把F_history和H_history两个cell保存下来，仿真结束后再调用computeObservability函数。你会直观地看到秩和NEES的变化是同步的。如果你也遇到EKF-SLAM结果“看着在跑，实则在飘”的情况，不妨先检查可观测性秩是不是比理论值大了。通常做到这一步，离解决办法就不远了。