1. 一致性问题的前提认知为什么EKF-SLAM的协方差会“越跑越小”1.1 一个让人后背发凉的实验现场第一次让我正视这个问题是在一次室内导航仿真里。机器人按一个8字形轨迹运动周围散布着几十个路标点用距离加方位传感器做观测。跑完一圈后地图和轨迹看起来还好误差不算大但当我打开协方差矩阵的对角线时情况就不对劲了——位姿部分的方差已经掉到了初始值的百分之一以下换算成标准差只有几厘米。可问题是我同时记录了真实轨迹与估计轨迹之间的偏差那个偏差明明已经累积到了半米以上。这就是SLAM领域里被反复讨论的“不一致性”inconsistency现象滤波器告诉你的不确定度和它实际犯的误差完全对不上。而且方向永远是乐观的你永远觉得自己定位很准然后拿着这个过度自信的估计去做回环检测、做地图融合结果就是一连串的误匹配。1.2 不一致性到底怎么度量NEES与卡方检验工程上判断一个滤波器是否一致最常用的统计量是NEESNormalized Estimation Error Squared[ NEES_k \left(x_k - \hat{x}_k\right)^T P_k^{-1} \left(x_k - \hat{x}_k\right) ]其中 (x_k) 是真实状态(\hat{x}_k) 是滤波器估计(P_k) 是滤波器输出的协方差矩阵。如果滤波器的模型和噪声设置都准确那么NEES应该服从自由度等于状态维数(n)的卡方分布其期望值就是(n)。在仿真里做几十次蒙特卡洛运行把每次的NEES取平均理想情况下应该围绕在(n)附近。如果平均NEES远大于(n)说明滤波器“过分自信”了真实误差比它报告的不确定度大得多这就是不一致。反过来说如果NEES远小于(n)通常是噪声参数给得太保守。我见过很多人调EKF时只盯着轨迹误差和地图误差却从来不画NEES曲线这是非常可惜的因为轨迹误差小并不代表滤波器健康只有NEES才能反映出内部诊断信息。1.3 协方差失真带来的工程连锁反应不一致性不是纯理论问题它会在工程链路里形成连锁破坏。首先数据关联依赖马氏距离门控而马氏距离里就包含协方差(P)。协方差被严重低估时门控会缩小正确的观测可能被当成外点丢掉反过来当滤波器在某些方向过度自信时错误的路标匹配又会被当成真实匹配接受。其次回环检测和位姿图优化也依赖协方差作为置信度权重。滤波器给出的协方差如果失真后端优化就会给错误边赋予过高权重直接把整个地图拉变形。最后在规划层面机器人会认为自己“绝对不可能撞到那面墙”而实际误差可能早就超过了安全距离。所以我一直觉得做SLAM的人应该把一致性检验当作标配而不是论文里用来凑图的指标。接下来我们要做的事情就是从可观测性这个理论视角解释为什么标准EKF-SLAM在结构上就注定会产生这种“虚假信息”然后给出一个可以直接在Matlab里复现和验证的完整实验框架。2. 可观测性视角下标准EKF-SLAM哪里出了问题2.1 “能不能被观测”到底意味着什么可观测性回答的问题很朴素给定一段历史输出传感器观测和控制输入能不能唯一确定系统当前的状态如果能系统就是可观测的如果不能那些无法唯一确定的方向就是系统的不可观测方向。对于一个搭载路标观测传感器的移动机器人来说你手里只有机器人到路标的距离、方位等相对测量值。这些测量值可以把“地图和机器人之间的相对几何关系”钉得很牢但你拿不到任何绝对锚点。想象一下你在一个完全没有窗户的大厅里蒙着眼走路手里只有一把尺子能够量出你到墙角的距离。无论你怎么量你都永远不会知道自己在这个坐标系里的绝对坐标是多少因为整个系统可以整体平移、整体旋转而不改变任何观测数据。这就是SLAM系统天然不可观测的方向全局平移和全局旋转。可观测性分析的任务就是把“哪些方向可观测、哪些方向不可观测”这件事严格刻画出来。对线性系统我们直接看可观测性矩阵的秩对非线性系统理论上要用李导数和高阶微分来构造可观测性分布。但在工程实践中绝大多数EKF-SLAM的不一致性分析都是在“线性化系统”上做的而这恰恰是问题所在。2.2 线性化如何“篡改”系统的可观测性标准EKF的核心思想是在每一步把非线性运动模型和观测模型在当前状态估计附近做一阶泰勒展开得到一个随时间变化的线性系统然后套用卡尔曼滤波的递推公式。这一步本身没问题问题在于这样得到的线性时变系统其可观测性与原非线性系统并不一致。非线性SLAM系统的真实可观测性维度取决于相对几何关系应该始终保留全局平移和全局旋转两个“不可观测方向”。但EKF在做线性化时雅可比矩阵在每个时刻都是基于当前估计值计算的这些雅可比组合出来的可观测性矩阵往往会错误地“补全”那两个不可观测方向让线性化系统变得局部可观测或者至少让滤波器认为它对这些方向的估计是有信息量的。结果就是滤波器在那些本应保持无信息的方向上不断地削减协方差制造出虚假的确定性。这就是EKF-SLAM不一致性的一个核心机制。理解这个机制很重要因为很多初学者以为是过程噪声设置得不够大或者观测噪声设置得不对于是拼命调噪声参数实际上问题出在线性化结构本身。2.3 为什么标准EKF会持续踩这个坑有人会问既然线性化破坏了可观测性那每次做线性化的时候把不可观测方向手动“锁住”不就行了理论上是可以的但标准EKF没有做这件事。更糟的是每次重新线性化都会基于新的状态估计而随着协方差被压缩下一次线性化会变得更加“自信”从而进一步强化虚假信息。这是一个自我强化的循环。我用一个粗糙但直观的比喻滤波器的状态就像一个正在不断收紧手铐的囚犯每走一步它都觉得自己对手铐的了解更透彻了但实际上手铐根本没有被铐在真实的位置上。越收越紧最后得到的就是一个“漂亮但错误”的锁。可观测性分析在这里的价值不是给出一套新的滤波公式而是提供一把“诊断尺子”你可以算出线性化系统的可观测性矩阵看它的秩在哪些时刻悄悄上升了再对照非线性系统的理论可观测性精确找到协方差开始失真的时间点。下一章我们就用Matlab把整个过程完整搭建出来。3. 在Matlab里复现一台简化的EKF-SLAM小车3.1 仿真场景设计与建模思路为了把问题暴露得足够明显又不让代码复杂到劝退我建议用二维平面内的差速模型小车上路标观测做实验。地图里有若干个静态路标机器人匀速运动每个时刻可以看到所有路标。数据关联直接给定不加入匹配错误这样可以排除数据关联干扰让不一致性纯粹来自滤波器本身。状态向量包含机器人位姿 ((x, y, \theta)) 和所有路标坐标 ((lm_{xi}, lm_{yi}))。运动模型是经典的速度输入模型给定线速度 (v) 和角速度 (\omega)预测方程为[ x_{k1} x_k v \Delta t \cos\theta_k ] [ y_{k1} y_k v \Delta t \sin\theta_k ] [ \theta_{k1} \theta_k \omega \Delta t ]观测模型是每个路标提供的距离和方位角[ z \begin{bmatrix} \sqrt{(lm_x - x)^2 (lm_y - y)^2} \ \operatorname{atan2}(lm_y - y, lm_x - x) - \theta \end{bmatrix} ]为了让协方差萎缩更快、更明显我故意把运动噪声取得很小同时让观测噪声处于合理水平。这样滤波器就“更有理由”相信自己的位姿估计越来越准但实际上由于线性化误差的积累真实误差并不会按协方差预测的速度收敛。3.2 EKF核心代码预测、观测更新、数据中心化下面是能跑通核心逻辑的Matlab骨架代码我把注释写得很详细方便你直接粘到脚本里修改。% params.m % 仿真参数与地图真值 T 1500; % 总步数 dt 0.1; % 采样间隔 v 0.4; % 线速度 w 0.25; % 角速度 M 8; % 路标数量 % 噪声参数 R_motion diag([0.005, 0.005, 0.001]); % 运动噪声协方差 Q_obs diag([0.02, 0.01]); % 观测噪声协方差距离、方位 % 路标真值均匀分布在半径10m的圆周围 lm_true zeros(2, M); for i 1:M ang 2*pi*i/M; lm_true(:, i) [10*cos(ang); 10*sin(ang)]; end预测与更新循环是EKF-SLAM的主角。注意代码里对每个路标依次做更新这是最直观、也最容易理解的做法。为了减少数值问题更新完所有路标后我会对协方差矩阵做一个对称化处理防止它逐渐失去对称性。% ekf_slam_demo.m % 状态初始化机器人初始位姿 所有路标初始值离真值有一定偏差 mu [0; 0; 0; reshape(lm_true 0.5*randn(2,M), [], 1)]; P blkdiag(diag([0.1, 0.1, 0.01]), kron(eye(2*M), 0.5)); x_true zeros(3 2*M, T); x_est zeros(3 2*M, T); P_diag zeros(3 2*M, T); for k 1:T % ---------- 预测 ---------- mu(1:3) [mu(1) v*dt*cos(mu(3)); mu(2) v*dt*sin(mu(3)); mu(3) w*dt]; G [1 0 -v*dt*sin(mu(3)); 0 1 v*dt*cos(mu(3)); 0 0 1]; P_pred G * P * G; P_pred P_pred blkdiag(R_motion, zeros(2*M, 2*M)); % ---------- 观测更新每个路标依次处理 ---------- % 真实状态推进用于最后的NEES计算 x_true(1:3,k1) x_true(1:3,k) [v*dt*cos(x_true(3,k)); v*dt*sin(x_true(3,k)); w*dt]; for i 1:M dx lm_true(1,i) - x_true(1,k1); dy lm_true(2,i) - x_true(2,k1); q_true sqrt(dx^2 dy^2); z_meas [q_true; atan2(dy, dx) - x_true(3,k1)]; z_meas z_meas mvnrnd([0;0], Q_obs); % 当前估计下的路标索引 idx 3 2*(i-1) (1:2); ex lm_true(1,i) - mu(1); ey lm_true(2,i) - mu(2); % 预测观测基于当前估计 q sqrt(ex^2 ey^2); z_pred [q; atan2(ey, ex) - mu(3)]; % 观测雅可比对机器人和第i个路标 H zeros(2, 3 2*M); H(1,1) -ex/q; H(1,2) -ey/q; H(2,1) ey/q^2; H(2,2) -ex/q^2; H(2,3) -1; H(1,idx(1)) ex/q; H(1,idx(2)) ey/q; H(2,idx(1)) -ey/q^2; H(2,idx(2)) ex/q^2; S H * P_pred * H Q_obs; K P_pred * H / S; mu mu K * (z_meas - z_pred); P_pred P_pred - K * S * K; end % 数值保护确保协方差对称 P (P_pred P_pred) / 2; % 记录 x_est(:,k1) mu; P_diag(:,k1) diag(P); end这段代码看起来长但每一步都没有绕路。(H) 矩阵的写法是SLAM里最常见的雅可比形式位置和角度分开处理和大多数教科书一致。跑完后你就能得到一条完整的协方差变化记录。3.3 跑完代码后你会看到的典型现象我在实验里得到的最直观特征有三个。第一机器人位姿的协方差在对数坐标下几乎是一条稳步下行的直线没有明显反弹。第二路标协方差也在收缩尤其靠近机器人走过的初始区域的路标收缩最多。第三如果你把真实轨迹和估计轨迹画在同一张图里两者之间的偏差会随着时间波动甚至增大和协方差曲线形成强烈反差。这个现象在蒙特卡洛运行中非常稳定不是偶然一次出现的。我在不同路标数量、不同轨迹形状、不同观测噪声下都跑过得到的结论是只要使用标准EKF线性化策略协方差被低估就一定会发生只是程度不同。这也印证了前面分析的结构性问题——它不依赖某个特定参数而是内生于算法。4. 用可观测性矩阵和NEES曲线做定量验证4.1 计算线性化系统的可观测性矩阵拿到仿真数据后下一步就是把“线性化系统破坏了可观测性”这件事量化出来。方法很简单在每个时刻从线性化模型提取状态转移矩阵 (A_k)这里是 (G) 以及单位阵的拼接和观测矩阵 (H_k)构造离散线性系统[ A_k \begin{bmatrix} G 0 \ 0 I_{2M} \end{bmatrix},\quad H_k \bigcup_i H_{k,i} ]然后计算这个时变系统的可观测性矩阵。对于时变系统严格做法是计算Gramian矩阵但工程诊断里一个近似做法是取一段窗口内的平均观测矩阵再计算秩。下面的代码就是一种快速诊断手段。% observability_check.m n length(mu); A_aug blkdiag(G, eye(2*M)); H_stack []; % 把当前时刻路标雅可比堆起来 for i 1:M H_stack [H_stack; H]; end O []; for rr 1:n O [O; H_stack * A_aug^(rr-1)]; end rank_O rank(O);我在实验中发现标准EKF的情况下这个秩经常等于全维数 (n)也就是123个机器人状态加上16个路标状态这里简化为12个。也就是说线性化系统认为所有方向都能被观测到而这明显是非线性SLAM系统不可能做到的因为全局平移和全局旋转两个方向在物理上不可观测。4.2 非线性系统理论秩和EKF线性化秩的对比为什么非线性SLAM系统的可观测性秩理论上应该少两个可以用一个非常直白的推理把机器人和所有路标同时平移 (\delta)或者同时旋转 (\delta_\theta)所有相对距离和相对方位角都不会改变。这就表明存在两个不改变观测的状态移动方向即两个不可观测方向。因此理论可观测性维度应该是 (n-2)。按这个说法标准的EKF线性化系统把秩从 (n-2) 抬高到了 (n)凭空多出来的两个可观测方向就是滤波器用来“压缩协方差”的虚假信息来源。你可以做一个简单测试在每次滤波更新后把线性化系统可观测性矩阵的秩打印出来绝大多数时刻它都是满秩。而如果改用后面提到的First Estimates JacobianFEJ策略秩会更接近 (n-2)协方差曲线也会明显更“诚实”。4.3 NEES曲线把一致性变成一张图有了多次蒙特卡洛仿真数据就可以画平均NEES曲线。Matlab里计算NEES的代码非常简单但有几个容易踩的坑。% nees_analysis.m n length(mu); NEES zeros(T,1); for k 1:T err x_true(:,k) - x_est(:,k); NEES(k) err / P_est(:,:,k) * err; end % 期望值自由度n卡方分布95%置信区间 chi2_low chi2inv(0.025, n); chi2_high chi2inv(0.975, n);第一个坑是(P_{est}) 必须是滤波器实际使用的协方差矩阵而不是后处理重新计算的协方差。如果代码里因为数值对称化而修改了(P)要确保保存的是修改后的版本。第二个坑是NEES是逐时刻计算的如果仿真步数太少NEES波动会很剧烈建议做至少30次蒙特卡洛再取平均。第三个坑是状态维度(n)必须是全状态维度包括所有路标有些人在计算NEES时只算机器人三个状态这样自由度对不上结论就会出错。我跑完30次后的典型结果是理论期望NEES12左右而标准EKF的平均NEES随着时间推移能达到30到60在长轨迹仿真里甚至冲到上百。这个结果足以说明滤波器的协方差严重低估了真实误差。相比之下如果改用一致性保持的SLAM方法NEES通常会在12附近波动偶尔有超界但很快回弹。4.4 秩分析结果的解读方式有人在算可观测性矩阵秩的时候会因为浮点误差得到混乱的结果。我建议两步处理先用奇异值分解看奇异值的分布如果最小的奇异值比最大的小好几个数量级就按数值秩处理然后不要只看瞬时秩而是画“秩-时间”曲线看看秩在什么时候跳变。我在实验里发现本地图下从不一致开始秩的跳变通常发生在机器人刚开始转弯的时刻。直线运动时累积的线性化误差还不明显一旦转弯次数增加秩开始抬升之后NEES就开始偏离理论值了。这个现象有很重要的工程含义不是所有轨迹都会把不一致性暴露出来直线轨迹下EKF看起来“基本能用”一进走廊拐角、环岛这种转弯频繁的场景问题就原形毕露。这也是为什么很多人在仿真直线轨迹时觉得EKF-SLAM还挺好一到实车就开始满地找牙。5. 抑制不一致性的工程尝试与有效组合5.1 经典的FEJ策略把雅可比钉在首个估计值上既然问题出在“重新线性化导致不可观测方向被虚假观测”那解决办法的思路就很直接去掉那些虚假观测。最经典的做法是First Estimates JacobianFEJ核心思想是不管当前状态估计变成了什么计算雅可比矩阵时都使用该状态分量第一次被观测时的估计值。道理在于可观测性取决于雅可比矩阵之间的关系如果每次都用最新的状态估计重新计算这些雅可比会不断“移动”导致原本为0的不可观测方向被注入信息。如果雅可比固定在世界坐标系的初始估计上线性化系统的可观测性就会更加接近非线性系统的真实结构。代码层面的改动不大主要是多维护一个“首次估计”向量。但这里有个容易被忽略的细节只固定机器人位姿的雅可比不够路标的雅可比也要在第一次初始化时固定。而且固定雅可比只对线性化系统的一致性有效代价是单步估计精度可能略有下降。下面是一个FEJ关键片段。% fej_core.m % first_mu保存每个状态分量第一次被估计时的值 if first_flag first_mu mu; first_flag false; end % 计算雅可比时把当前mu替换成first_mu mu_for_jac first_mu; % 然后用mu_for_jac计算G和H但滤波更新仍然用当前mu G_fe jacobian_motion(mu_for_jac, v, dt); H_fe jacobian_obs(mu_for_jac, lm_idx, i);我试验过在距离加方位传感器下FEJ能让NEES从平均几十降到接近理论值代价是轨迹均方根误差略有增加但增加幅度很小通常不超过10%。更重要的是回环检测的准确率明显上升因为协方差终于不再“撒谎”了。5.2 工程上更实用多种手段组合FEJ是学术界最经典的答案但它不是银弹。工程上我更推荐做组合拳。首先是增加观测冗余度每个路标如果同时提供距离和方位尽量在一次扫描内多观测几次这类冗余观测能在一定程度上抑制单步线性化误差的膨胀。其次是给过程噪声模型留一些“余地”不是让你乱调大而是要对机器人模型不确定度做保守估计。很多人为了图好看把过程噪声压得很低这在短时仿真里有效一旦时间拉长低过程噪声会让滤波器更快变得过度自信。我一般的做法是先按物理模型的真实不确定度给噪声跑一次NEES再根据NEES的偏差方向微调幅度控制在1.5倍以内超过这个幅度就需要反思模型而不是调噪声。第三是定期做“一致性健康检查”在仿真环境里固定几条轨迹作为基准每次改动算法都跑一遍NEES。我自己的习惯是任何SLAM改动只要能通过基准一致性测试才允许继续调其他指标如果NEES已经飞了轨迹误差再低也先不碰。5.3 我实际试过的组合及最终效果把FEJ、适度过估的过程噪声、以及逐时刻秩监控三者放在一起之后50次蒙特卡洛的结果非常稳定平均NEES回到理论值附近仅在一些快速转向时刻短暂超出95%置信区间地图点位精度略有提升因为数据关联错误减少了回环检测的假阳性率明显下降。测试中一个有意思的发现是FEJ对于短轨迹几乎看不出优势只有把轨迹拉长到上千步、包含多次转弯后优势才变得显著。所以如果你只是做一个几十步的小实验可能根本感受不到不一致性。这解释了为什么很多初学者在demo里完全没问题一到规模化场景就崩。我的建议是先把仿真轨迹设计得足够“恶劣”——包含大角度转弯、重复访问区域、少量路标遮挡——再来看你的滤波器是否一致。5.4 对“可观测性视角”本身的一点反思用可观测性分析来诊断SLAM有一个明显的好处是它不依赖传感器的具体型号只依赖系统结构和线性化策略因此理论结论可以迁移到不同平台。但也有局限性可观测性分析给出的是方向性的结论它告诉你“这里有问题”却不直接告诉你“该把协方差调到多大”。所以实际落地时我习惯把可观测性秩分析当作“体检报告”把NEES曲线当作“疗效跟踪”两者搭配使用。另外要提醒的是EKF-SLAM只是一个起点现在主流的图优化SLAM、因子图SLAM虽然对线性化策略的处理方式不同但同样会遇到信息矩阵“过度自信”的问题只是表现形式变成了“信息矩阵掉得太快”。可观测性分析的思路在这些框架里依然成立只是工具变成了Fisher信息矩阵分析。