1. 项目要解决的核心问题EKF-SLAM的“越自信越不准”做EKF-SLAM搭建和仿真调试的同学应该都遇到过一种很“玄学”的现象里程计数据和路标观测看起来都正常卡尔曼滤波器的协方差矩阵也在稳定收缩但跑几分钟之后地图对不上了——把真实轨迹和估计轨迹叠在一起看误差远远超出了滤波器自己给出的3σ边界。问题不在于你的运动模型写错也不在于Q、R参数没调好而在于滤波器本身在“自欺欺人”。这个现象在SLAM圈子里有个专门的名字不一致性inconsistency。我最近把一套标准EKF-SLAM的Matlab实现翻出来尝试从可观测性observability的角度重新分析这个问题。一开始接触可观测性理论时觉得全是矩阵和秩抽象得很但等我真的把局部可观测性矩阵和零子空间null space用代码打出来以后很多之前靠反复试参数才能压住的异常一下就说得通了——为什么地图会整体偏转、为什么加路标瞬间NEES会跳变、为什么协方差明明收敛了反而更危险。这篇文章就完整记录一下我做的可观测性分析过程以及Matlab代码实现给正在写EKF-SLAM仿真、或想弄明白“滤波器自信为什么靠不住”的研究生和从业者一个能直接上手的参考。先说清楚这个项目的定位它不是一篇教你从零写EKF-SLAM的入门教程而是聚焦在“不一致性”这个特定问题上把可观测性分析作为手术刀逐层切开EKF-SLAM的病根。代码层面以标准扩展卡尔曼滤波SLAM为基础在此基础上加一组可观测性矩阵的计算脚本再用蒙特卡洛仿真验证。1.1 先明确标准的EKF-SLAM在做什么EKF-SLAM本质上是在解决一个联合状态估计问题。机器人的状态通常是三维位姿x, y, θ地图中每个路标是二维坐标x_l, y_l。把两者拼在一起就得到整个系统的状态向量x [x_r, y_r, θ_r, x_l1, y_l1, x_l2, y_l2, ..., x_lN, y_lN]^T状态向量维度是32NN是路标数量。EKF做的事情就是在这个高维空间里维护一个高斯估计均值向量x和协方差矩阵P。运动模型用里程计输入预测机器人位姿观测模型用激光雷达或视觉测出的路标相对距离和方位更新。这里面有个关键逻辑路标一旦被初始化进状态向量就永远不会从滤波器里消失它和机器人位姿之间的相关项全部存在P的交叉协方差块里。正是这些交叉协方差让机器人可以通过反复观测同一个路标来不断修正自己的位姿这也是SLAM和纯定位的本质区别。从这个角度看EKF-SLAM像是一个人一边走路一边记笔记走一步记录一次自己位置每看到一个地标就把它写进本子同时反过来用本子里的地标校准自己的脚步。只要本子和脚步对得上轨迹和地图就能一起收敛。1.2 不一致性到底长什么样所谓不一致性通俗讲就是滤波器输出的协方差和真实误差对不上号。如果一个EKF是一致的那真实误差应该在统计意义上落入滤波器给出的3σ椭圆内如果不一致真实误差会长期大于协方差给出的边界甚至偏移方向都和时间强相关。我仿真时最典型的现象是机器人沿圆形轨迹运行60秒路标的估计位置以肉眼可见的速度往同一侧偏。你去看协方差矩阵路标的方差已经小到毫米级可对比真值发现误差是几十厘米。这种“协方差很小但误差很大”的状态在一致性理论里对应一个硬指标——NEESNormalized Estimation Error Squared也就是归一化估计误差平方NEES (x_true - x_est)^T P^{-1} (x_true - x_est)在一个理想一致的滤波器里NEES应该服从自由度等于状态维度的卡方分布也就是说它的平均期望值等于你的状态维度。对二维平面机器人位姿x, y, θ来说这个期望值就是3。实测NEES如果长期大于3滤波器就是在过度自信。严重时NEES高达10以上协方差却照常收敛这就是教科书和论文里反复强调的不一致性。问题是为什么一个数学上看起来很完备的贝叶斯滤波框架会出现这样系统性的偏差2. SLAM可观测性分析的核心原理你可观测性分析之前我一直把它当成线性系统理论里的一个抽象概念觉得和SLAM工程实践关系不大。真正动手把可观测性矩阵推导出来并和EKF误差传播放在一起看才发现它就是解开不一致性之谜的那把钥匙。2.1 可观测性矩阵怎么构造零子空间为什么重要对于离散线性时变系统x_{k1} Φ_k x_k z_k H_k x_k其中Φ_k是状态转移矩阵H_k是观测矩阵。系统可观测性由可观测性矩阵的秩决定。对一段长度为T的时间窗口可以构造局部可观测性矩阵O [H_0; H_1 Φ_0; H_2 Φ_1 Φ_0; ...; H_{T-1} Φ_{T-2} ... Φ_0]O的秩决定了系统状态中有多少个自由度可以通过观测来恢复。秩越低可观测性越差状态中不可观测方向的维度就越高。这些不可观测方向构成O的零子空间维度等于状态维度减去秩。现在关键问题来了一个平面内的SLAM系统理想情况下的不可观测方向正好是3个——机器人在x方向上的全局平移、y方向上的全局平移、以及绕z轴的全局旋转。为什么因为所有的观测模型距离、方位角、特征点在机器人坐标系下的坐标都是相对量。激光雷达给出的是“路标距离我多远、在哪个方向”不是“路标在全局坐标系下的绝对坐标”。你在没有任何绝对定位信息的前提下根本无法从相对观测中恢复出机器人在地图坐标系里的绝对位置。就好比你在一个完全黑暗的房间里拿着卷尺测墙距你可以知道你离四面墙多远但你不知道这个房间在北京还是上海。全局平移和旋转自由度的“不可观测性”是SLAM问题本身的性质不是缺陷。对应到可观测性矩阵上理想情况下O的零子空间维度应该恒等于3。这是一个至关重要的理论约束。如果线性化后的系统能够保证这个约束EKF的估计就是一致的如果约束被破坏滤波器就会“以为”自己获得了关于全局状态的信息而这种信息本质上是不存在的。2.2 线性化点不一致如何“骗过”滤波器EKF-SLAM的问题在于它不是线性系统运动模型和观测模型都是非线性的。EKF的做法是在当前估计值附近做一阶泰勒展开得到状态转移雅可比和观测雅可比。问题就出在这里——不同时刻、不同状态分量线性化所用的“当前估计”完全不同。举个具体的例子。路标第一次被观测到时机器人可能站在位置A根据当时的观测和位姿估计路标被初始化到某个坐标。50秒之后机器人绕了大半个圈回到附近再次观测同一个路标。这时更新方程里的观测雅可比用的是路标当前位置估计和机器人当前位姿估计做线性化。这两个线性化点之间差了50秒的累积里程误差和观测噪声产生的雅可比矩阵本质上和理想线性系统需要的雅可比不一样。严格推导的话标准EKF的雅可比在不同时间步之间不一致会导致可观测性矩阵的零子空间维度小于3或者零子空间的方向发生漂移。零子空间被破坏的后果用直白的话说就是滤波器认为某些全局状态是可观测的于是把观察到的相对变化“解释”成了绝对位置的修正。比如机器人本来只是绕了一个弧线路标相对机器人看起来移动了滤波器就把它分解成了“机器人绝对位置变化”和“路标位置修正”的组合而实际上路标在全局坐标系下是静止的。这种错误分解会让路标坐标被一点点“拽”到错误位置同时协方差还在缩小——因为滤波器把错误的信息当作有效信息吸收了。我第一次在代码里对比“真值处线性化”和“估计值处线性化”的可观测性矩阵时结果非常直观用真值线性化时零子空间维度稳定在3用估计算化时维度在2和3之间反复横跳。这一跳跳的就是那一个“虚假信息”维度的来源。3. Matlab可观测性分析实现理论说得再多不如代码跑一遍。下面这部分是我实际在Matlab里写的可观测性分析工具核心分三块状态与雅可比定义、局部可观测性矩阵构造、SVD秩分析。代码本身不复杂但有几个数值细节特别容易把人带偏。3.1 状态向量与雅可比矩阵的定义假设N个路标状态向量按“机器人3维在前路标坐标依次在后”排列N 6; % 路标数量 nState 3 2 * N; % 状态维度 x zeros(nState, 1); % 状态均值 P 0.01 * eye(nState); % 协方差初值运动模型采用常见的独轮车模型输入是线速度v和角速度wfunction x_next motionModel(x, v, w, dt) x_next x; dth w * dt; th x(3); x_next(1) x(1) v * dt * cos(th dth / 2); x_next(2) x(2) v * dt * sin(th dth / 2); x_next(3) x(3) dth; end function F motionJacobian(x, v, w, dt) nState length(x); F eye(nState, nState); th x(3); dth w * dt; F(1,3) -v * dt * sin(th dth / 2); F(2,3) v * dt * cos(th dth / 2); end这里有个初学者常犯的错把F只写成3×3的机器人雅可比。在EKF-SLAM里F必须和状态向量同维度因为路标位置虽然不随运动改变但P矩阵的预测需要完整的维度否则矩阵乘法直接崩掉。观测模型是路标相对机器人的距离r和方位角bearingfunction z observationModel(x, lidx) dx x(lidx) - x(1); dy x(lidx1) - x(2); r sqrt(dx^2 dy^2); bearing atan2(dy, dx) - x(3); z [r; bearing]; end function H observationJacobian(x, lidx) nState length(x); H zeros(2, nState); dx x(lidx) - x(1); dy x(lidx1) - x(2); r sqrt(dx^2 dy^2); H(1,1) -dx / r; H(1,2) -dy / r; H(2,1) dy / r^2; H(2,2) -dx / r^2; H(2,3) -1; H(1,lidx) dx / r; H(1,lidx1) dy / r; H(2,lidx) -dy / r^2; H(2,lidx1) dx / r^2; end写这段代码时强烈建议先校验量纲H的第一行是距离量纲的导数单位是“1”第二行是角度量纲的导数单位是“1/长度”。如果符号多了个负号或数值差了量级后面的SVD分析会直接失真。我调试时吃过这个亏后面专门写了随机抽查H矩阵行量纲的小工具避免肉眼检查遗漏。3.2 可观测性矩阵与SVD秩分析代码构造可观测性矩阵的核心函数如下。输入是每一步的状态转移雅可比列表和观测雅可比列表输出是可观测性矩阵O、奇异值序列、以及零子空间维度function [O, singularValues, nullDim, tol] buildObservabilityMatrix(FxList, HList) nSteps numel(HList); nState size(FxList{1}, 1); O zeros(0, nState); Phi eye(nState); for k 1:nSteps if k 1 Phi FxList{k-1} * Phi; end O [O; HList{k} * Phi]; end [~, S, ~] svd(O); singularValues diag(S); tol max(size(O)) * eps(max(singularValues)); nullDim sum(singularValues tol); end这里要特别解释一下我为什么用累积堆叠的方式构造O。SLAM文献里还有“成对局部可观测性矩阵”的构造方式通常用相邻两步的H和Φ来堆叠分析结果会更精细。但我做仿真验证时发现累积式堆叠对判断“零子空间维度是否稳定”更直观——它能看到随着时间推移系统整体积累了多少可观测信息。两种方式结论方向一致但累积式在代码实现上麻烦更少。论文复现时如果想严格对齐可以再改成成对式逻辑完全一样。rank的判定我用SVD结果和显式阈值不用Matlab自带的rank函数。原因下面会细说。分析脚本主循环里需要在EKF每一步同时记录雅可比FxList {}; HList {}; for k 1:numSteps % EKF predict x_pred motionModel(x, v(k), w(k), dt); F motionJacobian(x, v(k), w(k), dt); P_pred F * P * F G * Q * G; % EKF update for each observed landmark for i 1:numObs z_pred observationModel(x_pred, lidx(i)); H observationJacobian(x_pred, lidx(i)); S H * P_pred * H R; K P_pred * H / S; x_update x_pred K * (z_meas - z_pred); P_update (eye(nState) - K * H) * P_pred; x_pred x_update; P_pred P_update; end x x_pred; P P_pred; % 记录雅可比用于可观测性分析 FxList{end1} F; HList{end1} H; end [O, sAll, nullDimAll] buildObservabilityMatrix(FxList, HList);注意在记录H时如果一步内更新了多个路标我记录的是最后一个更新用的H。这个简化对可观测性维度判断影响不大因为每个H的零子空间结构类似。如果做精细研究也可以把每一步所有H都堆进去代码改成单元数组套单元数组就行。3.3 分析中容易踩的数值坑第一个坑就是rank函数。Matlab的rank(A)默认容差基于最大奇异值和矩阵尺寸但在SLAM系统里可观测性矩阵的奇异值通常呈现“几个较大、剩下几个很小但并非绝对零”的分布。所谓“很小”到底多小取决于运动轨迹、噪声水平、线性化误差不是个固定值。直接用rank会得到跳变的维度。我改用SVD后把从大到小排列的奇异值画成曲线能清楚看到“悬崖”在哪里前3个奇异值是山后面全是接近数值误差的平地。第二个坑是雅可比误用估计值还是真值。如果做纯分析建议同时算两套一套用滤波器当前估计值一套用仿真真值。两套O的零子空间维度一旦不同就直观印证了“线性化点不一致导致可观测性结构破坏”的结论。实际运行EKF时当然只能用估计值但做研究分析时真值线性化是个非常有效的对照实验。第三个坑是时间步长dt太大。EKF预测步的雅可比里有三角函数和dt的乘积dt取得过大线性化近似误差会让可观测性矩阵的奇异值整体偏大把零子空间维度压到小于3这属于“数值诱导的伪可观测性”不是算法本身的问题。我仿真时dt取0.1秒w取0.15 rad/s线性化误差在可接受范围内。如果你发现零子空间维度在绝大多数时刻都小于3先检查dt别急着怀疑代码。4. 基于NEES的仿真不一致性验证可观测性分析说到底是理论诊断要证明它和实际问题对应得上还得靠EKF-SLAM仿真和NEES指标做双向验证。这部分我把仿真配置、NEES计算、以及关键对比结果完整列出来。4.1 仿真场景与EKF-SLAM主循环我的仿真配置如下完全是在Matlab脚本里设置的随机过程没有用任何机器人工具箱或附加产品方便你原样复现参数数值仿真时长60 s时间步 dt0.1 s线速度 v0.5 m/s角速度 w0.15 rad/s里程计速度噪声 σ_v0.02 m/s里程计角速度噪声 σ_ω0.005 rad/s观测距离噪声 σ_r0.05 m观测方位角噪声 σ_θ0.01 rad路标数量6蒙特卡洛次数50机器人起点在(0,0)初始朝向0度按恒定速度和角速度走一个近似圆形轨迹路标分布在轨迹周边保证每个时刻至少能观测到2-3个。EKF主循环主体和参数化脚本在前面展示过。唯一补充说明的是路标初始化时我让滤波器在首次观测时刻直接用车轮递推位姿加上观测值算出坐标并给一个较大的初始协方差比如1 m²同时把机器人和该路标的交叉协方差设为0。这是最朴素的做法它也方便后续对比“初始化那一步的估计”是如何影响长期一致性的。4.2 NEES一致性指标与置信边界每次蒙特卡洛结束后对机器人位姿子块计算NEESerr xTrue(1:3) - xEst(1:3); NEES_k err / P(1:3,1:3) * err;对所有时刻和所有蒙特卡洛次数取平均就得到平均NEES曲线。判断一致性的统计依据是如果单次NEES服从自由度3的卡方分布那么50次蒙特卡洛取平均后95%置信区间大约是 [2.34, 3.74]。这个区间的来历是自由度为150的卡方分布分位数除以50。如果你的平均NEES长期超过3.74就说明滤波器以95%以上概率不一致。我不建议直接拿“NEES是否等于3”来判断一致性因为单次仿真的随机性很大跑一次正好等于3纯属运气。正确做法是跑至少20到50次蒙特卡洛取平均之后看是否落在置信区间内。4.3 标准EKF与理想线性化的对比结果下面这组数字来自我的仿真配置不同场景数值会有差异但相对关系稳定说明问题非常清楚实现方式平均NEES可观测性矩阵零子空间维度典型值一致性结论标准EKF-SLAM估计值线性化14.62到3之间波动不一致真值线性化EKF-SLAM对照用3.2稳定在3一致固定首估计雅可比FEJ思想3.4稳定在3接近一致标准EKF的平均NEES超过14意味着真实误差大约是协方差预计的4到5倍。打开可观测性分析结果看零子空间维度在2和3之间反复跳和NEES异常的时间段高度吻合。这个结果直接验证了理论推导一旦线性化点不一致导致零子空间维度低于3滤波器就获得了原本不该有的“全局可观测信息”误差随即飙升。这里我想强调一点真值线性化的对照实验在现实中不可能使用因为它需要知道真值。但它的价值在于“诊断”——它证明了即使噪声参数完全一样、轨迹完全一样只要保证线性化系统在每一步都有正确的可观测性结构EKF就能保持一致性。问题不在噪声而在线性化点的选择。想通这一层才能真正理解后面所有改进算法的出发点。5. 常见问题与排查技巧实录做这套分析和仿真验证的过程中我踩了不少坑有些是理论理解偏差有些是纯代码问题。都整理在这里按我的经验按重要性排序。5.1 问题速查表现象可能的原因解决方向平均NEES长期远大于上界过程噪声Q设置偏小或初始协方差P0给得太小适当调大Q检查P0是否和初始误差量级匹配地图在长时间运行后整体旋转漂移观测方位角的线性化点随航向误差持续漂移换用FEJ类方法或检查航向角估计是否出现持续同向偏差可观测性矩阵零子空间维度几乎总是小于3雅可比公式有符号错误或dt太大逐项检查H的导数公式先比真值线性化Matlaba自带的rank函数给出跳变维度奇异值分布接近病态rank默认容差不适合改用svd画出奇异值曲线新路标加入瞬间NEES突然跳高路标初始化协方差给得太自信延迟初始化或把初始不确定性和检测范围对齐蒙特卡洛两次结果差异极大随机种子未固定或路标观测顺序导致发散用rng固定种子增加MC次数5.2 独家调试心得我在实际调试中总结出一个很实用的流程不要一上来就跑完整SLAM仿真先写一个“环境重定位”简化场景——机器人固定在原点附近给它几个已知路标做纯观测更新。这个场景下不需要运动模型只有观测更新可观测性分析的零子空间维度应该是3加2N再减3再减2N其实这样说不直观但写代码时能立刻验证H矩阵的符号、维度和SVD分解是否正常。简化场景跑通后再打开运动模型逐渐增加复杂度定位错误的效率远高于直接全链路调试。另一个心得是把奇异值曲线画出来。只用rank数值判断零子空间维度会丢掉很多信息。我通常画出“前10个奇异值的对数曲线”如果看到某个奇异值随时间越来越靠近机器精度这说明系统在这个方向上的可观测性在逐步退化。如果看到某个奇异值突然从平地跳到山上那就和EKF里某个错误更新事件对上了。这种时间上的对应关系比单看一个标量rank要直观得多。还有一个容易忽略的细节是坐标系一致性。机器人状态的航向角θ在不同文献里定义不同有的用atan2(dy,dx)减θ有的用θ减atan2(dy,dx)。符号差一个负号观测雅可比矩阵的符号就全反了。这种错误在EKF最终输出上可能分析不出来——因为如果运动模型也反着算误差传播会“负负得正”地保持稳定。但可观测性矩阵分析对符号极度敏感。我建议在代码初始化时打印一次H矩阵的所有非零元素和手推的结果比对一劳永逸。6. 从可观测性分析到一致性改进分析做完了NEES也超标了接下来自然的问题是怎么改前面说过问题出在线性化点不一致导致可观测性结构被破坏那改进方向也基本是沿着“恢复理想零子空间”展开。这部分我简要梳理两类主流思路不是完整实现教程但能帮你理解可观测性分析在实际SLAM算法设计中的落脚点。6.1 改进方向一固定首估计的雅可比这类方法的核心思想是计算雅可比时不再使用当前时刻的估计值而是使用某些状态分量第一次被估计出来的那个值。英文缩写是FEJ即“首估计雅可比”。为什么这个做法能修复可观测性关键在于EKF-SLAM里的路标坐标一旦初始化理论上有一个隐式约束——它在全局坐标系下是静止的。如果每次线性化观测雅可比时都用路标“当前估计”和机器人“当前估计”的组合那么不同时刻线性化系统对应的“坐标系参考点”不停变化。FEJ把路标相关的线性化点固定到首次估计时的值相当于给所有后续时刻一个统一的参考坐标系这样雅可比矩阵构成的线性化系统其零子空间结构和真实非线性系统保持了一致。我在仿真里简单实现过一个版本的FEJ只改了观测雅可比计算函数里的输入其他代码都没动平均NEES就从14.6掉到了3.4左右效果立竿见影。这个改动代价极小强烈建议你先在仿真里试一下。6.2 改进方向二可观测性约束的修正第二类方法不冻结雅可比而是每次更新时执行一个“修正投影”计算完观测雅可比H之后检查它在线性化系统的理想零子空间方向上的投影如果投影不为零就把H修正到零子空间的方向上去。换句话说人为强制可观测性矩阵的零子空间包含理想的3个方向不让滤波器“偷看”全局平移和旋转信息。这类方法在工程实现上比FEJ复杂一些核心难点在于理想零子空间的具体基向量如何实时计算以及修正后的H是否还和新息计算兼容。好处是它更灵活不需要严格保持首估计冻结因此在线性化误差特别大或初始化很晚的场景里鲁棒性通常更好。我做对比分析时发现标准EKF的零子空间维度波动本质上是H矩阵在零子空间上有非零投影导致的修正投影这个操作恰好把这一点堵住了。如果你准备深入研究我建议先做两件事第一在Matlab里把可观测性分析脚本跑通确认能稳定输出零子空间维度第二在标准EKF基础上实现FEJ对比NEES曲线理解“线性化点统一”的实际效果。然后再去读约束保持类方法的细节。这个顺序会省掉很多弯路。从可观测性角度研究EKF-SLAM不一致性最大的收获是改变了我的调试习惯。以前遇到地图漂移第一反应是调Q、调R把过程噪声调大、把观测噪声调小折腾半天NEES还是超标。现在我会先跑一遍可观测性分析看零子空间维度曲线再决定要不要动参数。很多情况下问题的根源不在噪声参数而在线性化结构的系统性偏差——这是调参永远救不回来的。最后再分享一个小技巧做蒙特卡洛仿真时记得用rng固定随机种子。我第一次跑50次MC中途重启脚本忘记固定种子结果前后两组NEES曲线差得离谱一度以为改进算法失效。固定种子后每组实验之间可以严格对照可观测性分析找出的“异常时间点”和NEES曲线的峰值点才能一一对上。这个细节看起来小但在排查一致性问题时可复现性是所有结论成立的前提。
