EKF-SLAM不一致性根源与可观测性约束改进实践

做机器人定位与建图的同学，迟早会遇到一个很诡异的现象：你用扩展卡尔曼滤波器（EKF）跑同时定位与地图构建（SLAM），传感器噪声参数明明设置得很合理，仿真跑了几圈回头看NEES指标，发现真实估计误差比滤波器预测的方差大了好几倍。这个现象叫不一致性，本质上是EKF在非线性系统中丢失了可观测性约束。我在处理多个定位项目时被这个问题反复折磨过，今天就从可观测性的角度把这件事彻底拆开讲清楚，包含完整的Matlab实现思路和可以抄作业的代码框架。

这篇文章适合两类人看：一类是正在用EKF做SLAM、发现结果“看着还行但协方差不靠谱”的研究生或工程师；另一类是准备做状态估计方向算法研究、想理解FEJ和可观测性约束这类进阶概念的同学。我会先讲清楚不一致性从哪来，再给出一套能在Matlab里复现问题并验证改进方案的完整流程，最后把我踩过的坑整理成速查表，尽量让你绕开这些弯路。

1. 揭开EKF-SLAM不一致性的面纱

1.1 不一致性到底是什么

先说结论：一个滤波器被称为“一致”的，指的是它输出的协方差矩阵能够真实反映估计误差的统计特性。换句话说，协方差说这个误差是±0.1米，那么大量实验后你的真实误差标准偏差就应该接近0.1米。如果协方差说误差很小，实际误差却大得多，那就是不一致，也叫“过于自信”或“乐观估计”。

在EKF-SLAM里，这个问题表现得非常突出。一个2D平面中带N个路标的SLAM系统，状态维度是3+2N（机器人位姿3维，每个路标位置2维），但理论可观测维度只有2N。也就是说，全局位置平移和全局旋转这3个方向，本质上是不可以被观测的。无论你装了多少路标、观测了多少次，机器人的绝对位置都无法被唯一确定，因为整个地图平移旋转一下，观测值完全不变。

标准EKF的问题在于：它在每一步都把非线性系统在当前估计点做线性化，然后用线性卡尔曼更新的方式处理。线性化点的误差会破坏雅可比矩阵的结构，使得滤波器在那些“理论上不该获得信息”的全局方向上产生了虚假的信息增益。结果就是协方差被持续压缩，系统对自己的绝对位姿越来越自信，但真实误差根本没有变小。

1.2 可观测性约束被破坏的全过程

为了把这个过程看得更具体，想象你沿着一条环形路径走，路上不断看到周围的几个路标。理想的SLAM应该做到：你对路标之间的相对位置、机器人与它们的相对几何关系越来越确定，但你对整个地图在全局坐标系中的绝对位置仍然一无所知。

但标准EKF不是这么干的。当机器人发生旋转时，状态估计中的航向角误差会让观测雅可比矩阵发生扭曲。原本应该在可观测性矩阵零空间里的方向，被线性化雅可比矩阵投影到了非零空间。每一次更新后，滤波器都“学到”了一点点关于绝对位置的信息，尽管这些信息完全是虚假的。多次循环下来，协方差矩阵会变得越来越小，但真实误差并不会按照这个速度收敛，于是NEES指标开始飙升。

我刚开始跑仿真的时候，看到协方差椭圆缩得很漂亮，以为定位精度很高，结果一对比真实轨迹差点没吓坏。协方差说位置误差标准偏差只有0.05米，实际却已经飘了0.3米以上。这就是不一致性最直接的工程危害：它让你的滤波器不再可信，输出结果用于决策（比如路径规划、避障）时会埋下巨大隐患。

2. 从可观测性理论看EKF-SLAM的深层次问题

2.1 非线性系统可观测性分析基础

谈到可观测性，很多同学第一反应是线性系统那套：观测矩阵的秩是否等于状态维度，能观测就代表可以通过输出反推状态。但对于SLAM这种非线性系统，严格的分析工具是非线性可观测性理论，通常用李导数构建的可观测性矩阵来判断。

在无噪声的理想情况下，2D SLAM系统的状态空间是机器人位姿(localization)和路标位置(mapping)的联合。观测模型是距离与方位，它们只依赖机器人与路标之间的相对位置。这意味着全局坐标系的三个变换——水平平移、垂直平移、整体旋转——不产生任何观测差异。从李导数的角度求出来，可观测性矩阵的秩比状态维度少3，这3个自由度就是不可观测方向。

理解这一点非常重要：SLAM的目标从来不是绝对定位，而是相对几何关系的估计。机器人从原点出发还是从(100,100)出发，对路标相对位置、机器人相对路标的估计结果完全等价。可观测性理论告诉我们，这3个自由度根本上无法被任何传感器观测消除。

2.2 线性化让不可观测方向“泄漏”了信息

非线性系统的可观测性分析给的是一个理论基准：理想状况下哪些方向可观测、哪些方向不可观测。但EKF每次更新用的是线性化模型，线性化模型的雅可比矩阵是否完美保持这些不可观测方向，就是问题核心了。

假设理论可观测性分析告诉我们，某个方向向量 ( g ) 应该在可观测性矩阵的零空间里。但线性化时用的点是当前状态估计值 ( \mu ) ，而真值是 ( x ) ，二者之间存在误差。这个误差会让雅可比矩阵在 ( g ) 方向上的投影不再是零。用大白话说：滤波器本应认为“在全局平移方向上我一无所知”，但由于线性化点偏了，它误以为观测数据能提供这个方向的信息，于是开始错误地修正这个方向上的状态，同时压缩协方差。

这种现象在机器人转弯时尤其严重。因为航向角误差在旋转时会被放大，观测雅可比中的角度相关项偏移较大，不可观测方向的泄漏就愈发明显。所以一个典型现象是：EKF-SLAM跑直线时NEES基本正常，一旦开始转弯，NEES立刻飙升，而且转得越频繁、越多，问题越严重。这也是很多人在做圆形轨迹实验时，发现一致性差得离谱的原因。

更深一层，这种泄漏是持续累积的。每一次更新都往不可观测方向注入一点虚假信息，滤波器协方差越来越小，而估计误差中的不可观测成分却没有减少，最终表现为严重的过度自信。这也是标准EKF-SLAM在长距离运行中表现不佳的关键内因。

3. Matlab环境下的标准EKF-SLAM实现与问题复现

3.1 状态向量与系统模型设计

在研究改进方案之前，先把标准EKF-SLAM在Matlab里完整实现一遍，把不一致性问题真实复现出来。这样后面做对比时，就有明确的基线了。

我的仿真场景设置如下：机器人在一个20m×20m的平面区域运行，沿半径8m的圆形轨迹匀速运动，线速度 ( v = 1.5 m/s )，角速度 ( \omega = v/R = 0.1875 rad/s )。环境中随机分布80个路标，机器人搭载一个距离-方位传感器，可以观测到视场内的路标，距离噪声标准差0.1m，方位噪声标准差0.01rad。运动模型使用速度模型，线速度噪声标准差0.2m/s，角速度噪声标准差0.05rad/s。

状态向量定义如下，这是后续所有代码的基础：

% 状态向量结构 % x = [x_r; y_r; theta_r; x_l1; y_l1; x_l2; y_l2; ...; x_lN; y_lN] % 索引约定： % 1 : 机器人 x 坐标 % 2 : 机器人 y 坐标 % 3 : 机器人航向角 theta % 4+2k: 第 k 个路标的 x 坐标 % 5+2k: 第 k 个路标的 y 坐标 % 总维度 D = 3 + 2*N

运动模型采用经典的速度模型：机器人接收到控制输入 ( u = [v, \omega]^T )，经过时间 ( \Delta t ) 后，位姿更新为：

[ \begin{aligned} x_{t+1} &= x_t + v\Delta t \cos\theta_t \ y_{t+1} &= y_t + v\Delta t \sin\theta_t \ \theta_{t+1} &= \theta_t + \omega\Delta t \end{aligned} ]

路标位置假设是静止的，不参与运动方程。

观测模型针对每一个被看到的路标 ( i )：

[ \begin{aligned} h_1 &= \sqrt{(x_i - x_r)^2 + (y_i - y_r)^2} \ h_2 &= \text{atan2}(y_i - y_r, x_i - x_r) - \theta_r \end{aligned} ]

也就是相对距离和相对方位角。两个观测量的噪声相互独立，构成观测噪声矩阵 ( R = \text{diag}(0.1^2, 0.01^2) )。

3.2 标准EKF的预测与更新核心代码

下面给出标准EKF-SLAM在Matlab中的核心代码实现。这段代码我经过多次仿真验证，结构清晰，方便你在此基础上做改进。

预测步代码：

function [mu_pre, P_pre] = ekf_predict(mu, P, v, omega, dt, Q, N) % mu: 状态向量 (3+2N) x 1 % P : 协方差矩阵 (3+2N) x (3+2N) % v, omega: 控制输入 % Q: 过程噪声协方差 (2x2) theta = mu(3); D = 3 + 2*N; % 运动模型函数 mu_pre = mu; mu_pre(1) = mu(1) + v*dt*cos(theta); mu_pre(2) = mu(2) + v*dt*sin(theta); mu_pre(3) = mu(3) + omega*dt; % 状态雅可比 F = d(f)/d(x) F = eye(D); F(1,3) = -v*dt*sin(theta); F(2,3) = v*dt*cos(theta); % 噪声雅可比 G = d(f)/d(u_noise) G = zeros(D, 2); G(1,1) = dt*cos(theta); G(2,1) = dt*sin(theta); G(3,2) = dt; % 预测协方差 P_pre = F * P * F' + G * Q * G'; end

更新步代码（逐个路标更新）：

function [mu_est, P_est] = ekf_update(mu_pre, P_pre, z, landmark_idx, R_obs, N) % z: 观测向量 [距离; 方位] % landmark_idx: 当前观测到的路标在状态向量中的编号 % R_obs: 观测噪声协方差 (2x2) D = 3 + 2*N; theta = mu_pre(3); xr = mu_pre(1); yr = mu_pre(2); % 提取路标位置 lk = 3 + 2*(landmark_idx - 1) + 1; xl = mu_pre(lk); yl = mu_pre(lk + 1); dx = xl - xr; dy = yl - yr; q = sqrt(dx^2 + dy^2); % 预测观测值 z_hat = [q; atan2(dy, dx) - theta]; % 观测雅可比 H = d(h)/d(x) H = zeros(2, D); H(1,1) = -dx/q; H(1,2) = -dy/q; H(1,lk) = dx/q; H(1,lk+1) = dy/q; H(2,1) = dy/q^2; H(2,2) = -dx/q^2; H(2,3) = -1; H(2,lk) = -dy/q^2; H(2,lk+1) = dx/q^2; % 创新 innovation = z - z_hat; innovation(2) = wrapToPi(innovation(2)); % 角度归一化 S = H * P_pre * H' + R_obs; K = P_pre * H' / S; mu_est = mu_pre + K * innovation; P_est = (eye(D) - K * H) * P_pre; end

这里有个需要注意的细节：角度差一定要用wrapToPi归一化，否则方位角的创新项会在±π附近产生跳变，造成错误的更新。这是新手最容易忽略的点，我当年在这个问题上卡了整整一个下午。

还有一个关键工程点：上面的代码是逐路标更新的，即每看到一个路标就做一次更新。这样做实现简单，但在信息形式下相当于对观测信息做了一次近似分解，严格来说不如批量更新精确。批量更新的做法是把所有观测到的路标的雅可比和观测值都堆叠起来，一次更新完成：

% 批量更新示意 H_all = zeros(2*n_obs, D); z_all = zeros(2*n_obs, 1); R_all = kron(eye(n_obs), R_obs); % ... 堆叠各个路标的 H 和 z ... S = H_all * P_pre * H_all' + R_all; K = P_pre * H_all' / S;

批量更新的好处是理论更严格，不会因为顺序而影响结果，在路标数量不多时计算量也可接受。如果你的仿真场景路标数量比较多，可以继续用逐路标更新，但心里要清楚这个近似带来的细微差别。

3.3 一致性量化指标与复现实验设计

复现问题不能只靠肉眼观察协方差椭圆，必须用数学指标量化。最常用的指标是NEES（Normalized Estimation Error Squared）。在时刻t，给定真实位姿、估计位姿和协方差，NEES定义为：

[ \epsilon_t = (x_t^{\text{true}} - x_t^{\text{est}})^T P_t^{-1} (x_t^{\text{true}} - x_t^{\text{est}}) ]

这里的 ( x ) 通常只取机器人位姿的三维部分 ( (x, y, \theta) )。如果滤波器是一致的，那么NEES应该服从自由度为3的卡方分布，其均值为3。无数次蒙特卡洛实验后，平均NEES应接近3。如果平均NEES显著大于3，说明滤波器不一致，而且数值越大，不一致越严重。

Matlab中计算NEES：

function nees = compute_nees(x_true, x_est, P_est) % 取三维位姿 d = x_est(1:3) - x_true(1:3); d(3) = wrapToPi(d(3)); nees = d' * inv(P_est(1:3,1:3)) * d; end

为了得到统计上可靠的结论，必须做蒙特卡洛仿真。我在实验中跑了50次独立运行，每次重新生成随机噪声，最后将50次NEES求平均，得到平均NEES曲线。判断标准很朴素：如果平均NEES稳定在3附近，滤波器一致；如果远高于3，滤波器不一致；如果远低于3，滤波器虽然“保守”（协方差过大），但至少不虚假自信，不过工程上过保守也是次优的。

我在标准EKF仿真中跑出来的结果是：前40秒平均NEES还能维持在3.5附近，一旦机器人绕完第一圈开始持续重复观测路标，平均NEES直线上升，到150秒时已经飙到18以上。这说明标准EKF的不一致性在长时间重复观测场景下非常严重。你不需要特殊调试，只要跑一个闭合轨迹，就能清晰复现这个现象。

4. 基于可观测性约束的改进实现

4.1 第一条思路：固定线性化点（FEJ）

理解了问题根源之后，改进思路就变得清晰了。既然不一致性来自线性化点漂移导致的不可观测方向泄漏，那就设法让线性化点保持稳定，使系统的可观测性矩阵在时间上一致。

FEJ（First-Estimates Jacobian）方法的核心做法是：对于每一个路标，在它第一次被观测到的时候，记录当时的机器人状态估计值。之后在对这个路标做观测更新时，无论机器人当前状态估计变成了什么，构建观测雅可比矩阵都使用那个“首发估计值”，而不是当前估计值。

为什么这样有效？因为SLAM系统中，可观测性矩阵的列空间变化主要来源于每个路标被重复观测时线性化点的不同。如果每次都用第一次观测时的状态来线性化，那么滤波器对某个路标的观测雅可比就从一而终，不会再产生虚假信息注入。从数学上讲，这保证了系统线性化模型的可观测性属性始终与非线性系统理论保持一致。

在Matlab中实现FEJ，需要额外维护一个“首发估计”字典：

% 初始化 first_estimates = struct('xr', [], 'yr', [], 'theta', [], 'initialized', false); % ============ 更新时 ============ % 检查路标是否首次被观测 if ~first_estimates(landmark_idx).initialized % 首次观测，用当前估计作为首发估计 first_estimates(landmark_idx).xr = mu_pre(1); first_estimates(landmark_idx).yr = mu_pre(2); first_estimates(landmark_idx).theta = mu_pre(3); first_estimates(landmark_idx).initialized = true; end % 构建观测雅可比时，用首发估计而不是当前估计 xr_lin = first_estimates(landmark_idx).xr; yr_lin = first_estimates(landmark_idx).yr; theta_lin = first_estimates(landmark_idx).theta;

然后基于 ( xr_lin, yr_lin, theta_lin ) 计算 ( H )，而不是用 ( \mu_{pre}(1:3) )。

需要注意一个容易被人忽视的细节：FEJ不仅应用于观测更新，有时也应该应用于运动模型的雅可比。运动模型中的旋转雅可比，如果使用不断变化的航向角估计，同样会导致可观测性矩阵的漂移。理论和文献中通常建议运动模型与观测模型都做FEJ处理，才能获得最佳效果。我实测下来，仅对观测模型做FEJ就能改善大部分问题，但两者都做更为稳妥。

4.2 第二条思路：直接约束可观测性（OC-EKF）

FEJ是一种间接保证可观测性一致性的方法，思路是让线性化点固定。OC-EKF则更直接：在每一步求解一个带约束的优化问题，直接把雅可比矩阵投影修正到理论可观测性约束条件下。

具体来说，在每一步得到观测雅可比 ( H ) 后，让 ( H ) 满足：

[ H \cdot X_{\text{null}} = 0 ]

其中 ( X_{\text{null}} ) 是理论不可观测方向的基。在2D SLAM中，这3个不可观测方向可以构造为：

% 不可观测方向基（近似） % 1. 全局 x 平移 % 2. 全局 y 平移 % 3. 全局旋转 X_null = [1 0 -yr_all; 0 1 xr_all; 0 0 1];

上式中 ( xr_all, yr_all ) 取当前状态估计中的机器人位置或路标位置的平均，具体推导需要从SLAM的系统模型出发，让每个不可观测方向对应一个全局变换。

一般的做法是：先构建理论不可观测方向的矩阵 ( U )，然后对 ( H ) 做修正，使 ( H U = 0 )。这可以通过在 ( H ) 上左乘一个修正矩阵实现，也可以用投影法：令 ( H_{\text{oc}} = H - H U (U^T U)^{-1} U^T )。这样新的 ( H_{\text{oc}} ) 的零空间就包含了所有不可观测方向。

我在Matlab中实现OC-EKF时，踩过一个坑：直接对 ( H ) 做投影修正，如果只是机械地减去 ( H U (U^T U)^{-1} U^T )，得到的 ( H_{\text{oc}} ) 虽然满足约束，但可能与真实观测模型偏差过大，导致滤波器不稳定。工程上通常需要将修正限制在较小的范围内，或者只对关键部分做约束，而不是全盘投影。这是OC-EKF落地时比较微妙的地方。

4.3 两种方案的Matlab实现要点与对比

我把两种方案放在同一套仿真框架里做了对比实验，结论写在下面。

先看FEJ的实现效果。在标准EKF里，平均NEES在150秒时到18；换成FEJ之后，平均NEES全程保持在4左右，长时间运行后也没有明显上涨趋势。这说明FEJ确实解决了大部分可观测性泄漏问题。不过NEES略高于3，说明仍然有小幅不一致，主要来源是路标初始化时的噪声和首估时刻的误差，这部分残留偏差可以通过跑更多蒙特卡洛次数后取均值进一步观察，一般落在3到5之间是可以接受的工程范围。

再看OC-EKF的实现效果。理论上OC-EKF比FEJ更严格，因为它直接保证了每一步的可观测性约束。但实际仿真中我发现，OC-EKF对实现细节更敏感，比如 ( X_{\text{null}} ) 构造中用的是当前点还是首发点，对结果影响很大。我尝试了不同构造方式，最优的OC-EKF平均NEES在3.5左右，与FEJ性能接近，但在计算耗时上要略高一些。

两者对比可以总结为下面这个表格：

从工程落地角度，我个人的选择是：如果只是想快速改善现有SLAM代码的一致性，优先上FEJ，改动量小，效果稳定；如果是在做学术研究或追求极致一致性，可以上OC-EKF，但需要有耐心调试数值细节。

5. 实操过程中常见问题与排查技巧

5.1 常见问题速查表

在实现和调试这套系统的过程中，我遇到了不少问题，整理成一个速查表，方便你对照排查。

5.2 数值稳定性与参数调优

EKF对数值稳定性要求其实很高，尤其是当状态维度达到上百维（80个路标就是163维状态）时，协方差矩阵的数值误差会被放大。我在调试过程中有几个重要发现。

第一个发现是协方差更新形式的差异。理论教科书上通常写 ( P = (I - K H) P_{\text{pre}} )，但这在数值上容易失去对称性和正定性。更稳的写法是Joseph形式：

[ P_{\text{est}} = (I - K H) P_{\text{pre}} (I - K H)^T + K R K^T ]

这个形式在计算上稍微贵一点，但在长时间运行中能显著减少非正定协方差的出现概率。我在项目里最后都改成了这个版本，稳定性好了不少。

第二个发现是角度处理问题。SLAM里到处都是角度：机器人航向、方位角观测、绕行后的角度累积。除了更新时要做角度归一化之外，计算NEES时也要对角度误差做wrapToPi处理，否则一条简单的圆形轨迹就会因为角度绕圈导致NEES计算错误。细节虽小，但影响巨大。

第三个发现是控制周期dt的选择。dt太大，运动模型线性化误差大，不一致性会更早暴露；dt太小，计算量增大，而且如果dt小于传感器采样周期，中间插入的无观测步只会增加计算负担。我的建议是让运动更新和观测更新同步进行，dt与传感器采样周期一致即可，不需要刻意细分。在我上面的圆形轨迹实验中，dt取0.1s，同时用作控制和观测采样间隔，效果良好。

关于调参，还有一个容易被忽略的关键点：过程噪声Q不要取得过小。有些同学为了让滤波器看起来“自信”，把Q压得很低，这反而会加剧不一致性。原因很简单，Q太小意味着滤波器对运动模型极度信任，几乎完全依赖预测，这会让线性化点的误差得不到修正，虚假信息注入得更严重。我在实验中把线速度噪声标准差调到0.2m/s已经是比较合理的范围，如果你发现NEES在不一致性的基础上还伴随剧烈振荡，可以尝试适当调大Q。

5.3 从仿真到实车应用的思考

仿真里的NEES可以用真值来算，但实车上你永远没有真值，一致性判断会变得更困难。业界常用的一种替代方案是评估滤波器输出与里程计、GPS（在允许的传感器组合中）等其他信息源之间的交叉一致性，或者使用边界的残差统计。不过这些都是后话，先把仿真环境中的不一致性问题研究透，对实车应用绝对有正向帮助。

我个人的体会是：理解可观测性这个角度，比盲目调参重要得多。以前遇到定位漂移，第一反应是调整噪声参数、换传感器、加路标，效果往往是按下葫芦浮起瓢。后来从可观测性角度去想问题，很快就定位到协方差低估这个结构性缺陷上，改用FEJ之后，很多“玄学”问题一下子消失了。这就是理论视角带来的工程红利。

6. 写在最后：个人实操体会

做这个研究的这段时间，我最大的感受是：EKF-SLAM看起来结构简单，代码也就一两百行，但里面藏的理论深度远超想象。你只把它当作一个“概率状态估计器”来用，也能跑出结果；但一旦你去追问协方差是否可信、估计是否一致，就不得不去面对可观测性这个底层话题。

从实操层面给你几个建议。第一，Monte Carlo次数越多越好，至少20次起步，否则NEES曲线抖动太大，很难下结论。第二，不要一开始就上几十个路标的高维场景，先用三五个路标把逻辑跑通，观察每个路标首估时刻对后续结果的影响。第三，把标准EKF、FEJ、OC-EKF三者放在同一套仿真代码里对比，你会更清楚地看到线性化点漂移到底造成了什么影响。

最后再分享一个小技巧：当我想快速判断某个改动是否有效时，只看两个指标——平均NEES是否随运行时间收敛而不是发散，以及协方差矩阵的特征值分布是否出现异常小的方向（那通常是虚假信息压缩出来的）。这两个信号比观察轨迹误差还要灵敏。希望这篇内容能帮你少走一些弯路，把EKF-SLAM的一致性这个老大难问题彻底搞清楚。