搞了一段时间的移动机器人定位建图手里积累了一批“看着没问题、细看全是问题”的EKF-SLAM工程。这次这个项目比较典型从可观测性的角度研究EKF-SLAM的不一致性并用Matlab代码把整套逻辑验证出来。说简单点就是两件事——第一用可观测性理论解释为什么标准EKF-SLAM会“迷之自信”第二用一套可复现的仿真代码把现象、原因、修正方案全部摆到桌面上。如果你正在写课程设计、准备算法岗面试或者已经被滤波器一致性坑到怀疑人生这篇内容应该能帮你省下不少弯路。1. 先把问题拆清楚EKF-SLAM到底哪里“不一致”1.1 EKF-SLAM的算法骨架EKF-SLAM就是用一个扩展卡尔曼滤波器同时估计机器人位姿和环境中路标的位置。状态向量一般长这样x [x_r, y_r, θ_r, lx_1, ly_1, lx_2, ly_2, ...]ᵀ机器人位姿3维每个路标2维。预测步用运动模型里程计或者速度控制推进机器人位姿更新步用距离-方位传感器对路标的观测做修正。这个框架在教科书上很成熟但注意它天生包含两个近似运动模型和观测模型都被线性化了而且线性化点用的是当前的状态估计值。这两个近似就是后面所有问题的源头。网上随便一搜就能找到大量EKF-SLAM的代码跑起来也能画出像模像样的轨迹和地图。但我劝你一句画图好看不代表滤波正确。真正要判断一个滤波器靠不靠谱必须做一致性评估尤其是跑蒙特卡洛之后看误差统计否则你看到的只是“滤镜下的美好”。1.2 什么是不一致性滤波器一致性通俗讲就是滤波器的协方差P要“说话算话”。真实估计误差需要满足E[(x̂ - x_true)(x̂ - x_true)ᵀ] ≈ P如果真实误差比P大得多那就说明滤波器过度自信这就是不一致。工程上最常用的量化指标是NEES归一化估计误差平方。对机器人位姿子状态做几十次蒙特卡洛仿真逐时刻计算平均NEES如果它长期落在χ²检验的置信区间之外这个滤波器就是不一致的。标准EKF-SLAM的典型表现是NEES一路上穿远高于上界。换句话说滤波器自己觉得“我很确定”但实际误差远大于它的自信。这个现象在激光SLAM、视觉SLAM、水下SLAM里普遍存在属于EKF体系的结构性缺陷不是简单调调噪声参数就能消除的。1.3 为什么说这个问题重要很多人觉得地图“画出来像样子”就行了协方差爱咋咋地。但SLAM的不确定性是要被下游模块消费的路径规划要用走廊的置信度判断安不安全闭环检测要用协方差判断匹配靠不靠谱多张子图拼接要看重投影误差的分布。一个过度自信的滤波器会让机器人在狭窄环境中“自信地撞墙”也会让闭环把错误匹配当成正确匹配。所以一致性不是一个学术洁癖而是实际安全性的底线。这也是我从可观测性角度去研究它的原因不一致性虽然表现为协方差爆炸或收缩根子却在于滤波器对“哪些信息物理上可得”这件事理解错了。2. 可观测性视角SLAM“病根”的数学解释2.1 可观测性滤波器的“信息天花板”可观测性说的是给定一段控制输入和观测序列系统的初始状态能不能被唯一确定。对线性时不变系统看观测矩阵的秩O [Hᵀ, (HF)ᵀ, (HF²)ᵀ, ...]ᵀ如果矩阵缺秩就存在某个方向上的初始状态无法被区分。对时变系统则要累积一段时间内所有观测和状态转移矩阵。可以把可观测性理解成“外界信息的天花板”天花板以下是物理上得不到的信息任何算法宣称能得到都是在说谎。EKF在每次更新里让协方差减小本质上就是在“宣称”它获得了信息。如果某个方向本来不可观测协方差却持续收缩那这个信息一定是编出来的。我们的做法就是把这个天花板算出来而且算的是“滤波器实际使用的线性化模型”的天花板不是理想非线性模型的天花板。2.2 真实SLAM的不可观空间是三维的先看理想情况。把机器人和所有路标放在同一个二维平面里传感器提供的是相对观测——距离r和方位角φ。这类观测只和“机器人与路标的相对几何”有关。现在把整张地图、整条轨迹一起平移一个量再旋转一个角度所有观测值完全不变。这说明什么说明全局位置x、全局位置y、全局朝向θ这3个自由度在真实非线性SLAM系统里是不可观测的。能观测到的只是相对几何关系这一点是SLAM本身的物理限制不是算法bug。所以一个正确的SLAM滤波器协方差在全局平移和全局旋转方向上的不确定性应该只增不减或者说至少不应该被“观测”缩小。理想情况下状态协方差矩阵在这3个方向上的信息增量应该是零。2.3 线性化模型“偷看”了不可观方向问题恰恰出在这里。EKF把非线性系统线性化之后可观测性结构被破坏了。我们仿真里用真实的雅可比矩阵搭出来的线性化系统运动一段时间后观测矩阵的不可观维度只剩1维。原本应该不可观测的3个方向里有2个方向被“看”到了。为什么线性化会改变可观测性因为雅可比矩阵每一步都是对着当前状态估计值求的而估计值本身随着观测不断移动线性化点一直在变。这个变化破坏了系统原本的旋转和平移对称性。于是每一次更新都通过交叉协方差悄悄注入一点“绝对朝向”和“绝对位置”的信息长期累积下来滤波器就在物理上不可能观测到的方向上不断收缩协方差。这就是虚假信息的来源也就是不一致性的直接机制。提示这是理解整个项目的关键。不要只把EKF-SLAM的不一致归咎于“噪声参数调小了”。很多实验都表明即使过程噪声、测量噪声都调得很合理标准EKF-SLAM依然不一致就是因为线性化模型的不可观维度错了。打个比方一个盲人拿着探路杖走在一张没有绝对标记的大平原上。他只知道自己和树、石头的相对方位永远不可能知道“北”在哪边。但标准EKF相当于每一步都基于“当前猜测的绝对朝向”重新做线性化结果在数学上慢慢把绝对朝向当成可观测的了。本来不存在的信息源被算法硬生生“造”了出来。3. Matlab仿真从参数到代码一步一步搭3.1 仿真场景与参数设定我搭的场景很常规但足以把问题暴露出来二维平面上一个机器人沿折线/螺旋路径运动周围随机撒16个路标传感器是距离-方位模型数据关联假设已知。具体参数如下参数取值说明仿真时长60 sdt0.1 s共600个控制周期机器人路径先直行后转弯的折线保证对路标的多角度观测控制输入v1.0 m/sω0.2 rad/s匀速转弯过程噪声σ_v0.1 m/sσ_ω0.02 rad/s每个控制周期的噪声标准差观测模型距离方位最大量程30 m观测噪声σ_r0.1 mσ_b0.01 rad相对干净的传感器路标数量16个分布在工作区附近蒙特卡洛次数50用于一致性统计选择这些参数有讲究。过程噪声不能调得太小否则滤波器天然过度自信会混淆“参数问题”和“结构问题”测量噪声选相对干净的传感器也是为了让问题聚焦到线性化结构上。这组参数让标准EKF在前几十步看起来还算正常随后才慢慢暴露不一致对照效果最好。3.2 工程结构与EKF核心实现代码按功能拆成几个文件主脚本、仿真数据生成、标准EKF滤波、可观测性分析、一致性评估、以及一个FEJ修正的对比实现。主脚本里设置参数、循环调用预测和更新。预测步核心代码% 速度运动模型预测机器人位姿部分 dt 0.1; if abs(w) 1e-6 xPred x(1:3) [ -(v/w)*sin(x(3)) (v/w)*sin(x(3)w*dt); (v/w)*cos(x(3)) - (v/w)*cos(x(3)w*dt); w*dt ]; else xPred x(1:3) [ v*cos(x(3))*dt; v*sin(x(3))*dt; 0 ]; end预测步还要生成运动模型对位姿的雅可比G_k这是最容易出错的地方。推导时要对着运动模型逐项求导ω趋近于0时的极限形式也要单独处理。完整状态只推进机器人部分所以状态协方差更新用分块形式P_pred blkdiag(G_k, eye(2N_landmarks)) * P * blkdiag(G_k, eye(2N_landmarks)) Q更新步对每个观测到的路标组装观测雅可比H_r和H_ldx lx - x(1); dy ly - x(2); r2 dx^2 dy^2; r sqrt(r2); Hr [ -dx/r, -dy/r, 0 ; dy/r2, -dx/r2, -1 ]; Hl [ dx/r, dy/r ; -dy/r2, dx/r2 ];注意正负号取决于观测模型的定义。我的模型里方位角是“相对机器人朝向的方位角”所以H_r对θ的偏导是-1。如果你用的是绝对方位角这里就是0。改模型后建议先用符号工具推一遍雅可比再手工核对数值点别凭感觉写。新路标初始化是另一个重灾区。很多人直接把新增路标对应的协方差块填零这是错的。新路标的不确定性和它与当前机器人位姿的相关性必须用观测模型关于位姿的雅可比推出来% 新路标均值 lm [x(1) r*cos(b x(3)); x(2) r*sin(b x(3))]; % 对观测z的雅可比和对机器人位姿x的雅可比 Gz [cos(bx(3)), -r*sin(bx(3)); sin(bx(3)), r*cos(bx(3))]; Gx [1, 0, -r*sin(bx(3)); 0, 1, r*cos(bx(3))]; x [x; lm]; P blkdiag(P, Gz*R*Gz); P(1:3, end-1:end) P(1:3, 1:3) * Gx; P(end-1:end, 1:3) P(1:3, end-1:end);这段代码把新路标自身的协方差以及它和机器人位姿的交叉协方差正确接入大矩阵。填零的做法表面能跑但会给系统注入虚假的“相对约束”可观测性分析和一致性评估都会受到影响。3.3 可观测性矩阵的计算方法要分析“滤波器实际使用的”线性化系统就得把预测和更新时用到的矩阵全部保存下来然后累积成观测矩阵% Phi累积转移矩阵O累积观测矩阵 Phi eye(n); O []; for k 1:K Fcurr F_list{k}; % 预测步雅可比填充后的完整F Hcurr H_list{k}; % 当前时刻全部观测的雅可比 O [O; Hcurr * Phi]; Phi Fcurr * Phi; end [~, S, V] svd(O); sigma diag(S); tol sigma(1) * 1e-8; % 相对阈值 rank_O sum(sigma tol); unobservable_dim n - rank_O; unobservable_dirs V(:, rank_O1:end);这里有两个关键点。第一阈值别直接用rank()函数的默认值。状态里既有米又有弧度尺度差异很大默认阈值很容易给出错误的高秩。我习惯把所有状态方向按典型尺度归一化之后再算SVD并用相对阈值σ/σ_max 1e-8判定有效奇异值。第二累积方式。可观测性矩阵可以用全历程累积也可以用滑动窗口。对EKF-SLAM的不一致性分析一定要用全历程累积矩阵因为它对应“滤波器从头到尾以为自己能获得的信息总量”。只算单步更新的话看不到虚假信息随时间的累积效应。3.4 NEES一致性评估指标一致性用机器人位姿子状态的NEES来量化代码很短d 3; % 只考察位姿子状态 N 50; % 蒙特卡洛次数 alpha 0.05; chi_low chi2inv(alpha/2, d*N)/N; chi_high chi2inv(1-alpha/2, d*N)/N; for k 1:K err x_true(1:3,k) - x_hat(1:3,k); nees_k err * (P_hat(1:3,1:3,k) \ err); store_nees(k, run) nees_k; end ANEES mean(store_nees, 2); % 对所有蒙特卡洛跑求平均判断标准是平均NEES是否落在[chi_low, chi_high]区间内。这组参数下d3、N5095%接受区间大约是[2.35, 3.73]。平均NEES长期大于3.73说明滤波器实际误差被它严重低估这就是过度自信长期小于2.35则过于保守。4. 实验结果与修正方案4.1 标准EKF的“迷之自信”实测跑完50次蒙特卡洛画出来的平均NEES曲线非常典型前几秒还在置信区间附近徘徊随着机器人开始转弯NEES迅速蹿升最终稳定在10到20之间稳稳压在上界之上。也就是说滤波器报告的不确定度比实际误差小了一个数量级。再看二维位置子状态的协方差椭圆把50次蒙特卡洛的真实位姿误差散点画出来再用滤波器的P(1:2,1:2)画一个3σ椭圆两者叠在一起误差散点明显“溢出”椭圆。这是可视化理解不一致性最直观的一张图也是答辩和汇报时最容易被问到的图。这个结果说明一件事哪怕数据关联已知、运动模型和传感器模型都精确、噪声参数都合理标准EKF-SLAM依然会不一致。它的病根在结构上不在参数上。4.2 可观测性维度的数值对比用3.3的代码算一下结果非常清晰系统状态维度不可观维度含义真实非线性SLAM系统3 2N3全局x、y、θ不可观标准EKF线性化模型3 2N1实测2个本不可观方向被“虚假观测”FEJ-EKF修正模型3 2N3恢复正确的信息天花板在我们的仿真设定里状态维度是32×1635真实系统不可观维度是3而标准EKF累积观测矩阵算出来的不可观维度稳定在1。这跟文献中“标准EKF-SLAM线性化系统比真实系统多两个可观测自由度”的结论完全对上。多出来的那两个自由度恰恰就是全局位置和全局朝向——虚假信息的注入通道。数值上的一致性非常好NEES出界的时机和可观测性维度出现偏差的时机高度相关。这也是我做这个项目的核心收获——有了可观测性这个工具不一致性问题就不再看不清摸不着了。4.3 FEJ-EKF修正思路与实现要点修正思路很直接既然问题出在线性化点随估计变化、破坏了对称性那就把线性化点钉死。FEJFirst Estimates Jacobian的做法是某个路标第一次被观测时记录当时的机器人位姿估计此后每次计算该路标的观测雅可比H_r和H_l都用这个“第一次估计”而不是用当前位姿。% 路标首次被观测时记录第一次估计 if ~first_pose_initialized(id) first_pose(:, id) x_hat(1:3); first_pose_initialized(id) true; end % 更新该路标时用first_pose(:, id)代替当前x_hat(1:3) xr_f first_pose(:, id); dx lx - xr_f(1); dy ly - xr_f(2); % ... 用dx, dy和xr_f(3)计算Hr、Hl就这么一个小改动跑出来的平均NEES会从10~20回落到置信区间附近不可观维度也从1恢复到3。原理不复杂FEJ让线性化点在整个过程中保持“前后一致”线性化模型因此保留了和真实系统类似的平移旋转对称结构虚假信息通道被堵上了。OC-EKF是另一种更系统的做法不修改线性化点而是显式地把雅可比投影到与真实系统不可观方向正交的补空间上。它更通用尤其适合视觉SLAM、多机器人协同这类复杂系统但实现复杂度高不少。在小规模2D仿真里FEJ已经能非常直观地说明问题建议先把它跑通再考虑OC-EKF。5. 实操中反复踩过的坑5.1 数值与代码层面的坑这一节基本是血泪教训。第一rank和SVD的阈值真的会骗人。状态里既有米又有弧度直接对原始观测矩阵做SVD主奇异值和次奇异值可能差好几个数量级阈值稍微不对不可观维度就飘。先把每个状态分量按典型尺度归一化再用相对阈值。第二F矩阵怎么组装。状态转移只推进机器人位姿部分路标是静态的所以完整F矩阵应该是左上角3×3为运动雅可比G_k、路标对应块为单位阵的分块矩阵。如果把整个状态都用同一个运动雅可比去乘路标就被“推着跑”了后面所有分析全废。第三观测雅可比的正负号。这个坑我至少踩过两次。距离-方位模型里只要方位角定义成“相对机器人朝向”H_r对θ的偏导就必须是-1如果你用绝对方位角就是0。差一个符号NEES可能看起来很“正常”但地图和轨迹其实已经悄悄偏了。第四landmark初始化。前面说过交叉协方差不能填零。填零会让新路标和机器人之间失去正确相关后续观测更新会做出错误的信息融合。我把这些问题整理成一张速查表现象排查方向解决办法NEES全程低于下界过程噪声过大或测量噪声过小适当减小Q或增大RNEES前期正常后期爆发线性化模型不可观维度不足计算不可观维度尝试FEJ/OC-EKF加入新路标后P非正定交叉协方差初始化错误用Gx、Gz正确扩展P可观测维度数值不稳定SVD阈值或状态量纲问题归一化状态用相对阈值σ/σ_max1e-8地图整体旋转漂移真实系统固有不可观性不是bug是物理限制需全局参考或回环5.2 实验设计层面的坑蒙特卡洛次数太少NEES曲线噪声很大看不出趋势太多又跑得慢。50次是个不错的平衡点。我习惯先跑20次看趋势确认问题出现的位置再跑满50次出正式图。过程噪声的设置也要小心。我一开始把σ_v和σ_ω调得很小结果NEES没等线性化问题完全暴露就已经冲出上界根本分不清是参数问题还是结构问题。正确做法是先把参数调到中等合理让标准EKF在最初四五十步内保持“看似一致”的假象再让它慢慢暴露问题这样才有对照意义。数据关联必须要固定为已知。一旦数据关联出错NEES会瞬间爆炸但那属于另一类问题会掩盖我们要研究的可观测性缺陷。做一致性实验时仿真里直接给正确对应关系别用最近邻匹配否则结论很难干净。需要说明的是这些坑大多来自通用工程实践和本项目调试过程中的多次排查不一定覆盖所有代码写法但按这个流程排查能省下大量时间。5.3 从仿真走向真机要注意什么仿真里我们能控制一切真机就不行了。真实传感器噪声往往不是高斯、不是白噪声里程计有系统漂移路标检测有漏检和误检。仿真里好用的FEJ参数到真机上通常需要重新调而且数据关联问题会变成主要矛盾。但可观测性这个分析框架依然有效。它告诉我们某个方向的信息从物理上拿不到任何滤波结果都不该在那些方向过度自信。真机调试时我依然会先离线采集一段数据用同样的方法算一遍线性化系统的不可观维度再决定要不要上FEJ修正。这一步几乎不花时间却能提前暴露很多“看起来玄学”的滤波器问题。以后再看某个SLAM系统结果异常我的第一反应不是急着调噪声而是先算一算它的线性化模型到底可观测到什么程度。很多时候问题根本不在参数而在模型结构本身。建议直接把3.3的可观测性计算代码抄回去先算不可观维度再谈调参——这个顺序能帮你省下大量反复试错的时间也更容易看清问题的本质。
