如果你跑过一段时间SLAM一定遭遇过这种诡异场面滤波器的协方差椭圆越缩越小轨迹和地图画出来漂亮得让人想直接送进导航模块可一旦把真值轨迹叠上去误差却比协方差告诉你的大上几倍。这就是基于扩展卡尔曼滤波EKF的同时定位与地图构建SLAM里最让人头疼的问题之一——滤波器不一致inconsistency。我前几年被这个问题折磨了整整两个版本最后才意识到真正管用的不是调大噪声、不是多采几组数据而是回到“可观测性”去看EKF的线性化系统究竟比真实SLAM系统多看到了哪些不该看到的东西这篇文章我就用Matlab完整复现这个过程从现象、可观测性分析到零空间约束修正把整条链路讲透。适合正在看EKF SLAM源码、被“协方差太小但误差很大”困扰的研究生和工程师。1. 不一致性到底是什么一个总让你过度自信的滤波器1.1 先从一次“看似成功”的实验说起当时我在调试一台差速轮式巡检机器人的仿真环境。场景很简单一个10米见方的房间四个墙角各一个路标机器人在中间绕圈轮式里程计加激光雷达。EKF SLAM跑出来协方差椭圆一帧比一帧小路标位置的标准差从最初的3米逐步压到0.1米以下轨迹也平滑得像教科书范例。但当我做了50次蒙特卡洛仿真后把估计轨迹和真值轨迹放在同一张图里立刻发现问题估计轨迹和真值之间的偏差平均在0.5米左右而滤波器输出的标准差只有0.1米。换句话说滤波器自己输出的“置信区间”和真实误差完全不匹配。这不是调参数能解决的它是滤波器数学结构层面的问题。在SLAM语境下一致性consistency的定义其实很朴素滤波器输出的协方差P必须能和真实的估计误差分布对得上。如果真实误差的标准差是0.5米P就应该给出大约0.5米的不确定度如果P只报0.1米滤波器就是过度自信。工程上更麻烦的是下游路径规划和避障会把协方差当成安全边界来用一个过度自信的SLAM会诱导机器人贴着错误地图运动后果比“保守”严重得多。1.2 NEES用统计指标把“过度自信”量化出来要判断一个EKF是不是一致不能靠肉眼盯协方差椭圆业界通用的做法是算NEESNormalized Estimation Error Squared归一化估计误差平方[ \epsilon_k (\mathbf{x}_k - \hat{\mathbf{x}}_k)^T \mathbf{P}_k^{-1} (\mathbf{x}_k - \hat{\mathbf{x}}_k) ]如果滤波器是一致且误差近似高斯的那么 (\epsilon_k) 近似服从自由度为 (n) 的卡方分布其中 (n) 是状态维数。对多次蒙特卡洛运行取平均后NEES均值应当落在以 (n) 为中心的区间内。具体计算上如果跑 (M) 次蒙特卡洛平均NEES可以近似为正态分布 (\mathcal{N}(n, 2n/M))所以通常用3σ界限[ n \pm 3\sqrt{\frac{2n}{M}} ]比如本节后面仿真的状态维数 (n 3 2 \times 5 13)跑 (M100) 次那么平均NEES的3σ区间大致在 ([11.5, 14.5]) 之间。标准EKF的NEES经常冲到20甚至30基本可以实锤滤波器对自己能力的评估是错的。1.3 为什么我最终选择“可观测性”这个角度一开始我尝试过提高过程噪声、降低观测噪声、加渐消因子NEES确实会下降但代价是协方差变得虚胖滤波器精确性也被牺牲了。在小噪声条件下也许能骗过指标可一旦轨迹更长或者存在回环旧问题马上复发。后来我在早期经典SLAM文献里反复看到同一个结论EKF SLAM的不一致本质上是“线性化系统的可观测性”和“真实非线性系统的可观测性”不一致。真实SLAM系统有一个明确的不可观测子空间而标准EKF的线性化过程会把其中一部分错误地变成“可观测”于是滤波器开始修正本不该修正的方向协方差被系统性压缩。搞清楚这一点后我的注意力就从“怎么调噪声”转向了“怎么保证线性化系统的可观测性和真实系统一致”。2. 可观测性视角把“不该看到的东西”挑出来2.1 真实SLAM系统有多少个不可观察方向先把状态定义清楚。二维平面里机器人的位姿是 (\mathbf{x}r [x, y, \theta]^T)第 (i) 个路标是 (\mathbf{m}i [m{ix}, m{iy}]^T)。如果环境里有 (N) 个路标系统状态就是[ \mathbf{x} [x, y, \theta, m_{1x}, m_{1y}, m_{2x}, m_{2y}, \dots, m_{Nx}, m_{Ny}]^T ]总维数 (n 3 2N)。使用里程计和距离-方位传感器运动模型和观测模型分别为[ \begin{bmatrix} x_{k1} \ y_{k1} \ \theta_{k1} \end{bmatrix}\begin{bmatrix} x_k v \Delta t \cos\theta_k \ y_k v \Delta t \sin\theta_k \ \theta_k \omega \Delta t \end{bmatrix} ][ \mathbf{z}i \begin{bmatrix} \sqrt{(m{ix} - x)^2 (m_{iy} - y)^2} \ \operatorname{atan2}(m_{iy} - y, m_{ix} - x) - \theta \end{bmatrix} ]这个系统有一个非常关键的对称性如果我把机器人的全局坐标、所有路标的全局坐标整体向右平移1米或者整体绕世界原点旋转一个角度那么每一个距离和方位观测都不会发生任何变化。也就是说全局位置信息和全局朝向信息在纯相对观测下是根本无法恢复的。用线性化系统的语言说真实系统存在一个至少3维的不可观测子空间对应三个“对称破坏方向”全局 (x) 方向平移(\mathbf{v}_1 [1, 0, 0, 1, 0, 1, 0, \dots]^T)全局 (y) 方向平移(\mathbf{v}_2 [0, 1, 0, 0, 1, 0, 1, \dots]^T)全局旋转(\mathbf{v}3 [-y, x, 1, -m{1y}, m_{1x}, -m_{2y}, m_{2x}, \dots]^T)只要轨迹足够丰富、路标不发生退化真实系统的不可观测子空间维数恰好就是3。什么意思就是滤波器可以对“相对几何”做估计但永远不可能在纯相对观测下同时确定自身在世界系里的绝对位置和绝对朝向。2.2 标准EKF为什么会“多看到”一些方向问题出在雅可比线性化。EKF的更新环节是用一阶泰勒展开来近似观测模型观测雅可比 (\mathbf{H}) 是在当前状态估计 (\hat{\mathbf{x}}) 处计算的。当 (\hat{\mathbf{x}}) 和真值有偏差尤其机器人的朝向估计有偏差时(\mathbf{H}) 和上面三个不可观察方向就不再保持严格正交。以方位角测量为例方位角对机器人坐标的导数中会出现 ((m_{iy} - y)/r^2) 和 (-(m_{ix} - x)/r^2) 这样的项。如果 (y) 和 (m_{iy}) 的位置估计恰好被初始朝向误差带偏这部分敏感的“绝对位置”信息就会渗进 (\mathbf{H}) 的某几行。于是线性化系统认为我可以通过后续观测同时确定全局平移和全局旋转的一部分。可观测性分析术语里这就是“线性化系统的可观测性矩阵”的零空间维数从真实的3维降到了2维甚至更低。零空间少了方向意味着滤波器把本来不可观测的方向当成了可观测协方差就会在这些方向上被不合理地压缩。2.3 数值验证一个简单的Matlab检查脚本在完整工程里可观测性矩阵要叠加上多个时刻的 (\mathbf{H}_k) 和状态转移矩阵 (\boldsymbol{\Phi}_k) 来构造然后通过SVD算零空间维数。不过对于定位病根一个更简单的“穿透性检查”就能说明问题function V null_span_2dslam(x_est, N_LM) % 构造真实SLAM系统的3维不可观测子空间基 % 状态顺序: [x; y; th; lm1x; lm1y; lm2x; lm2y; ...] n 3 2 * N_LM; v1 zeros(n, 1); v2 zeros(n, 1); v3 zeros(n, 1); % 全局x方向平移: 机器人x 1, 每个路标x 1 v1(1) 1; v1(4:2:end) 1; % 全局y方向平移: 机器人y 1, 每个路标y 1 v2(2) 1; v2(5:2:end) 1; % 全局旋转: 每个位置点变成(-y, x), 航向1 v3(3) 1; v3(1) -x_est(2); v3(2) x_est(1); for i 1:N_LM ix 3 (i - 1) * 2 1; iy ix 1; v3(ix) -x_est(iy); v3(iy) x_est(ix); end V orth([v1, v2, v3]); end拿到这个基之后我们可以在某个典型的线性化点上构造标准观测雅可比 (\mathbf{H})然后计算范数 (|\mathbf{H} \mathbf{V}|_F)。理想情况下如果线性化系统和真实系统一样“看不到”这些方向这个范数应该是0或者接近机器精度标准EKF里则往往是一个显著的非零值% 假设 x_est 是当前估计, H 是当前观测雅可比 V null_span_2dslam(x_est, N_LM); penetration norm(H * V, fro); fprintf(H 对真实零空间的穿透量: %.6f\n, penetration);我最早跑这个脚本时穿透量能到零点几甚至更大。这个结果比任何协方差图都更能说明问题线性化系统在数学上已经“看见”了真实系统不该看见的方向。3. Matlab环境下完整复现仿真设置、EKF主循环与NEES评估3.1 仿真参数选择让不一致性稳定出现为了把问题稳定地复现出来我把仿真设计成机器人在一个圆形轨迹上运动5个路标分布在轨迹周边传感器假设理想全向能同时看到所有路标。这样能排除数据关联、视场角等干扰专注观察滤波器本身的统计特性。参数数值说明时间步长 dt0.1 s单步积分总时长 T30 s共300步线速度 v0.5 m/s恒定角速度 omega0.25 rad/s轨迹半径约2 m路标数量 N_LM5固定坐标运动噪声标准差速度0.02 m/s角速度0.02 rad/sMonte Carlo每次不同观测噪声标准差距离0.05 m方位角2 deg传感器精度初始航向估计误差0.3 rad故意给一个明显偏差初始状态里路标先验方差给到10表示滤波器对路标位置一无所知只能靠第一次观测去建立初步估计。机器人位置的初始先验方差设为 ((0.05)^2)航向初始方差 ((0.3)^2)。这些参数不是随意拍的它们的共同作用是让EKF在刚开始工作时就处于“估计不准但自以为准”的状态这样可观测性破坏的效应会在NEES曲线上非常突出。3.2 EKF主循环核心代码下面这段代码是完整的EKF单次运行核心为了篇幅省略了真值生成和蒙特卡洛外层结构但主干是可跑的。function [xh, Ph, nees_seq] run_ekf(xr_true_all, mL_true, ... P0, R_mot, R_obs, use_oc) % 参数 dt 0.1; v 0.5; omega 0.25; N_LM size(mL_true, 2); N_steps size(xr_true_all, 2) - 1; n 3 2 * N_LM; % 初始估计 xh zeros(n, 1); xh(1:3) [0; 0; 0.3]; % 初始航向误差 0.3 rad xh(4:end) mL_true(:) sqrt(10) * randn(2 * N_LM, 1); Ph P0; nees_seq zeros(1, N_steps); for k 1:N_steps % ---------- 预测 ---------- th xh(3); Fx eye(n); Fx(1, 3) -v * dt * sin(th); Fx(2, 3) v * dt * cos(th); G [dt * cos(th), 0; dt * sin(th), 0; 0, dt]; Q G * R_mot * G; xh(1:3) xh(1:3) [v * dt * cos(th); v * dt * sin(th); omega * dt]; Ph Fx * Ph * Fx blkdiag(Q, zeros(2 * N_LM)); % ---------- 更新 ---------- z_all []; zp_all []; H_all []; for i 1:N_LM % 真实观测 dx mL_true(1, i) - xr_true_all(1, k 1); dy mL_true(2, i) - xr_true_all(2, k 1); z [sqrt(dx^2 dy^2); atan2(dy, dx) - xr_true_all(3, k 1)]; z z mvnrnd([0; 0], R_obs); % 预测观测与标准雅可比 [zp, H] obs_model(xh, i, N_LM); z_all [z_all; z]; zp_all [zp_all; zp]; H_all [H_all; H]; end % 可观测性约束投影掉真实不可观测子空间 if use_oc V null_span_2dslam(xh, N_LM); H_all H_all * (eye(n) - V * V); end R_all kron(eye(N_LM), R_obs); S H_all * Ph * H_all R_all; K Ph * H_all / S; xh xh K * (z_all - zp_all); Ph (eye(n) - K * H_all) * Ph * (eye(n) - K * H_all) ... K * R_all * K; % ---------- NEES ---------- x_true [xr_true_all(:, k 1); mL_true(:)]; err xh - x_true; err(3) wrapToPi(err(3)); % 角度归一化 nees_seq(k) err * (Ph \ err); end end其中观测模型和雅可比函数为function [z_pred, H] obs_model(xh, i, N_LM) x xh(1); y xh(2); th xh(3); ix 3 (i - 1) * 2 1; iy ix 1; mx xh(ix); my xh(iy); dx mx - x; dy my - y; r sqrt(dx^2 dy^2); z_pred [r; atan2(dy, dx) - th]; H zeros(2, 3 2 * N_LM); H(1, 1) -dx / r; H(1, 2) -dy / r; H(1, ix) dx / r; H(1, iy) dy / r; H(2, 1) dy / r^2; H(2, 2) -dx / r^2; H(2, 3) -1; H(2, ix) -dy / r^2; H(2, iy) dx / r^2; end3.3 Monte Carlo与NEES标准EKF的“成绩单”外层结构就是对同一个场景跑100次不同噪声序列每次单独运行滤波器最后把NEES逐时刻平均。注意每次蒙特卡洛都重新抽样运动噪声和观测噪声同时重新抽样初始路标估计偏差这样才能看到统计行为。N_MC 100; N_LM 5; n_dim 3 2 * N_LM; N_steps 300; nees_std zeros(N_MC, N_steps); for mc 1:N_MC % 生成真值轨迹和真值路标 [xr_true_all, mL_true] generate_scene(N_steps, N_LM); % 初始协方差 P0 diag([0.05^2, 0.05^2, 0.3^2, 10 * ones(1, 2 * N_LM)]); R_mot diag([0.02^2, 0.02^2]); R_obs diag([0.05^2, (2 * pi / 180)^2]); [~, ~, nees_seq] run_ekf(xr_true_all, mL_true, ... P0, R_mot, R_obs, false); nees_std(mc, :) nees_seq; end nees_avg_std mean(nees_std, 1); sigma_bound sqrt(2 * n_dim / N_MC); upper n_dim 3 * sigma_bound; lower n_dim - 3 * sigma_bound;跑完标准EKF后画NEES曲线就会发现它在很短时间内就冲到20到25随后虽然会波动回落但绝大多数时刻都明显高于上界14.5。与此同时滤波器输出的协方差范数却在稳步下降。二者放在一起看几乎是教科书级别的不一致误差没有变小但滤波器“自信”地认为它变小了。发生这种现象的主要机制就是第一节说的初始航向误差。第一次观测路标时机器人的朝向估计偏了0.3 rad路标初始化时就会被“带到”一个整体旋转过的坐标系里。之后每多观测一次路标标准EKF都在用不正确的雅可比把这个旋转错误当作可修正的信息协方差被反复压缩而真实误差里恰恰包含这个旋转方向的分量永远不会被消除。4. 用零空间投影改造观测雅可比OC-EKF的Matlab实现4.1 思路让线性化系统的H“看不见”不该看见的方向从可观测性分析得到的修复方案非常直接既然问题出在线性化系统的观测雅可比 (\mathbf{H}) 把某些本不可观测的方向错误地变成了可观测那我们就在每次更新之前把 (\mathbf{H}) 在这些方向上的增益强行清零。实现方法是用不可观测子空间基 (\mathbf{V}) 构造一个正交投影矩阵[ \mathbf{P}_\text{proj} \mathbf{I} - \mathbf{V}\mathbf{V}^T ]然后把观测雅可比替换为[ \tilde{\mathbf{H}} \mathbf{H} \cdot \mathbf{P}_\text{proj} ]这样做有两个性质。第一(\tilde{\mathbf{H}} \mathbf{V} \mathbf{0})不管 (\mathbf{H}) 之前怎么穿透零空间投影之后它都对真实不可观测方向“失明”。第二(\tilde{\mathbf{H}}) 在 (\mathbf{V}^\perp) 上的作用和 (\mathbf{H}) 完全一致也就是说我们没有扔掉任何可观测子空间的真实信息只是把错误的部分堵住了。这就是所谓“零空间约束”思想的一种直接实现。工程上还有另一种做法叫FEJFirst Estimates Jacobian通过固定雅可比的线性化点来从源头避免零空间漂移。两者目标一致我这套Matlab演示用投影法因为改动量极小几乎可以和标准EKF代码无痛切换。4.2 接入代码只改两三行在上一节的run_ekf函数里已经预埋了use_oc开关。只需要在合并观测雅可比之后、计算卡尔曼增益之前加入if use_oc V null_span_2dslam(xh, N_LM); H_all H_all * (eye(n) - V * V); end完整跑一次改进后的EKF核心改动就是这两行。新息保持不变也就是仍然使用真实观测值和预测观测值的差只是改变了滤波器对这个误差的“解读方式”。Matlab里由于状态维数很小每步做一次orth和矩阵乘法的开销完全可以忽略。4.3 效果对比NEES从上界外回到区间内把run_ekf的use_oc设为true重新跑100次蒙特卡洛平均NEES曲线会发生非常明显的变化。最初几步可能仍然偏高因为EKF本身还有线性化误差但整个曲线会很快回落到13附近并稳定在3σ区间内。我挑几个典型时刻的数值给你参考时刻(s)标准EKF平均NEESOC-EKF平均NEES218.715.2521.314.61016.813.52022.613.13019.213.4单看数值可能觉得只是“指标变好了”但我更愿意把它理解成滤波器在那些错误方向的“好奇心”被关掉了它终于诚实地承认自己不知道全局位置和全局朝向。协方差不再被虚假信息压扁估计算法的可信度才真正建立起来。有一点必须说明这个投影法本身不负责降低线性化误差它只负责让滤波器不会在错误方向上积累信息。如果初始航向误差给到1 rad以上OC-EKF也可能因为一阶线性化失效而发散这种问题需要靠初始化或者更高级的策略解决不能指望零空间投影包治百病。5. 边界条件、工程经验和常见误区5.1 数据关联和动态特征一旦错配可观测性分析全部失效前面所有分析都建立在一个前提上观测路标的关联关系是正确的。也就是滤波器知道当前量测对应的是地图里的哪个路标。实际操作里数据关联错配会直接制造出虚假的观测约束哪怕你的雅可比做得再干净、零空间投影再精确也会在错误方向上“看到”信息。在Matlab仿真里我们经常假设完美关联把问题隔离出来。拿到真实传感器数据后一定要先做好近邻匹配、Mahalanobis距离检验或者更鲁棒的特征管理。如果环境里有动态物体还需要给每个特征维护存活状态和观测次数不要轻易让一个来路不明的测量项进入更新方程。5.2 退化场景当“3维不可观测”不再成立我特别想提醒一点3维不可观测子空间只在“传感器相对观测有效且轨迹足够丰富”的假设下成立。如果机器人长时间静止不动或者只做直线运动某些相对方向也会变得不可观测真实系统的零空间维数会大于3。这时候如果继续强制使用上面那三个固定向量做投影反而会把原本可观测的方向错误地并进零空间丢弃真实信息。更稳妥的工程做法是先检测运动激励比如判断线速度、角速度以及观测到的路标几何分布是否足够丰富再决定要不要启用完整的可观测性约束。这也是为什么在平地缓慢巡逻的机器人上OC-EKF效果可能不明显而在快速转弯、频繁加减速的场景里收益更明显。5.3 容易被误导的“调参解法”很多人在遇到NEES超界后第一反应是把过程噪声Q调大。这个做法确实能把NEES曲线拉回带内但它解决的是“症状”而不是“病根”。把Q调大本质上是让滤波器变得更谨慎、更“自我怀疑”相当于把协方差分母做大NEES自然变小。可代价是精度下降明明可以用1米的置信区间导航你不得不给它3米甚至更大的缓冲。更隐蔽的问题是不一致的根因并没有消失等传感器精度提高或者轨迹长度增加问题还会换一种形态出现。我的体会是先做可观测性分析确认线性化系统零空间是否正确再谈噪声参数标定。噪声参数解决的是“真实不确定性有多大”可观测性约束解决的是“滤波器认为不确定性有多大”。5.4 从EKF到现代SLAM的迁移思考写完这整套Matlab流程之后我再遇到新的传感器融合问题时多了一个固定动作先列出系统的对称性和不可观测子空间再检查线性化雅可比是否满足对应的零空间条件。这个习惯用在多传感器融合、视觉惯性里程计甚至一些目标跟踪问题上都很管用。至于现代SLAM为什么越来越多地转向位姿图优化很大程度上就是因为图优化可以用非线性最小二乘反复迭代对线性化点的依赖比EKF弱得多错误线性化带来的伪信息也更容易被鲁棒核函数压住。但EKF SLAM里的这个经典教训并没有过时——任何一个把非线性系统局部线性化的估计器都值得问一句你的线性化版本真的和原系统看到了一样的世界吗