如果你做过基于EKF的SLAM大概率会遇到这样一个很“反直觉”的现象机器人明明沿着圆形轨迹走地图上的路标却开始往一边偏更诡异的是协方差椭圆还在快速缩小系统对自己错误的估计越来越自信。我最初遇到这个问题时第一反应是检查运动模型、观测噪声甚至怀疑是不是某个雅可比矩阵的符号写反了。后来真正把问题放到可观测性这个层面去看才发现根子其实出在EKF线性化之后系统的可观测性结构被悄悄改变了。这篇文章就把我从可观测性角度研究EKF-SLAM不一致性再用Matlab做仿真验证的完整过程整理出来。内容包括状态模型、雅可比推导、可观测性矩阵计算、NEES一致性指标以及一个能显著改善不一致性的改进思路和代码改动。无论你是刚接触SLAM的研究生还是已经在做组合导航、机器人定位的工程师这篇文章应该都能给你一个可以直接落地的分析框架。1. 不一致性是什么为什么一个词能让人头疼1.1 现象轨迹在漂方差却在缩先描述一个典型场景。机器人在一个小型室内环境中匀速转弯同时用测距传感器观察周围几个路标。Ground Truth轨迹是一个半径约3米的圆。EKF-SLAM跑完一圈后轨迹和真值偏差不大但如果你看路标估计和真实位置会发现路标整体偏转了十几度。这个时候打开协方差矩阵左上角机器人位姿方差反而比初始时小了一个数量级。换句话说滤波器不仅错了还非常自信地错了。这种现象在SLAM文献里有个专门的名字估计不一致inconsistent。所谓一致是指滤波器输出的估计误差统计量和其协方差矩阵描述的不确定性一致。如果协方差说“我对位置有10厘米的把握”而实际误差是1米那么这个滤波器就是不一致的。不一致带来的危害是灾难性的后续回环检测会拒绝正确匹配数据关联会出错地图会彻底不可用。1.2 一致性判据NEES和置信区间要量化“不一致”行业里最常用的指标是NEES全称Normalized Estimation Error Squared。它的计算方式很简单NEES_k (x_true_k - x_est_k) * inv(P_k) * (x_true_k - x_est_k)其中x_true_k是真值x_est_k是滤波器估计P_k是协方差矩阵。NEES本质上是用滤波器自己声明的协方差去“归一化”真实误差。如果滤波器是完美的真实误差的分布应该符合它的协方差假设那么对多次蒙特卡洛结果取平均之后NEES应该服从卡方分布除以次数均值大致等于状态维度n。如果NEES远大于n说明估计误差比协方差声称的要大也就是滤波器过于乐观、不一致。实际判断时经常用平均NEES值是否落在95%置信区间内。比如状态维度n7蒙卡次数N20那么阈值下限是卡方分布0.025分位数除以N上限是0.975分位数除以N。做实验时标准EKF跑出来的平均NEES常常是5到10而理论上应该接近7甚至低于1。我见过在路标数很少的场景里平均NEES能飙到十几。这说明协方差被严重低估系统在自我欺骗。1.3 根因从哪来线性化改写了可观测性NEES超标的背后是协方差被错误压缩。协方差为什么会被错误压缩常规解释是“EKF只做一阶泰勒展开忽略高阶项”。这个解释对但不完整。更深刻的视角是可观测性。可观测性通俗地说是“根据一段时间的输入输出能不能把系统内部状态反推出来”。比如一个系统如果对某个状态方向完全不敏感观测再多也无法确定那个方向这个方向就落在不可观测子空间里。真实2D SLAM系统在没有GPS、没有绝对航向参考的情况下机器人和路标整体平移、整体旋转距离和方位观测不会有任何变化。所以全局平移和全局旋转这几个方向天然不可观测。EKF在每步计算雅可比矩阵并做线性化之后原系统的这种不可观测性质未必能保持住。线性化矩阵的组合有可能把原本“看不到”的方向变成“看得到”等效于引入了一批本不该存在的观测信息EKF就会在这些方向上自信地收缩方差。这就是EKF-SLAM不一致性最本质的来源。2. 从可观测性角度拆解EKF-SLAM模型2.1 状态与运动模型我做仿真时为了能清晰观察一致性采用了一个固定路标集合的2D SLAM问题机器人位姿是二维平面坐标加航向角路标是固定的点。状态向量写为x [xr; yr; theta; lx1; ly1; ...; lxM; lyM]前三维是机器人位姿后面2M维是M个路标的坐标。如果环境中路标数量固定状态维度就是32M不会动态增长这对后续可观测性矩阵的计算非常友好。运动模型用了常见的前向模型机器人收到线速度v和角速度omega在离散时间dt内先转后走。简化形式是xr_pred xr v*dt*cos(theta) yr_pred yr v*dt*sin(theta) theta_pred theta omega*dt这种模型虽然比“圆弧模型”粗糙一点但在小步长下误差不大而且雅可比矩阵非常直观方便手推。更关键的是一旦我们后续要做可观测性分析模型越简单矩阵的性质越好解释。2.2 观测模型与雅可比推导传感器模型是距离加方位。机器人观测第j个路标时测量值是dx lxj - xr dy lyj - yr r sqrt(dx^2 dy^2) phi atan2(dy, dx) - theta对应的观测雅可比也分两块一块对机器人位姿求偏导一块对路标位置求偏导。因为本文重点是从可观测性角度研究雅可比矩阵不是“实现细节”而是整个问题的核心开关所以我建议所有做这个方向的人一定手动推导一遍。距离分量对机器人位姿的雅可比是H_r(1, :) [-dx/r, -dy/r, 0]方位分量对机器人位姿的雅可比是H_r(2, :) [dy/r^2, -dx/r^2, -1]对第j个路标的雅可比则是H_l [dx/r, dy/r; -dy/r^2, dx/r^2]注意方位项里对机器人航向有个-1这是机器人坐标系旋转造成的。如果漏掉这个-1不仅滤波结果不对可观测性分析也会完全跑偏。2.3 为什么2D SLAM原本有3个不可观测方向假设环境里有M个路标状态维度是32M。对距离和方位观测而言整体沿x轴平移、整体沿y轴平移、整体旋转都不会改变任何观测值。这就意味着系统至少有3个不可观测方向。因此真实非线性系统的可观测子空间维度理论上最多是3 2M - 3 2M举个具体例子M2时状态维度是7理论可观测维度是4。这4个方向代表了什么可以理解成所有路标之间的相对位置以及机器人与某个参考路标之间的相对位置。绝对位姿、绝对航向、全局坐标系的平移旋转这些信息观测里根本没有系统不该知道也不该去猜。EKF的问题就在于每步都用当前状态重新线性化系统的可观测性矩阵会随着雅可比变化而“随机”改变。仿真中我观察到标准EKF的数值秩经常会从理论值4跳到5、6极端情况下甚至满秩7。满秩意味着滤波器认为所有7个方向都可以从观测确定。这和真实物理过程不一致必然导致错误的信息注入。3. Matlab实现复现并诊断EKF-SLAM不一致性3.1 仿真场景设定写代码之前先把场景参数定下来。我用了下面这组参数简单但足够暴露问题机器人初始位姿[0; 0; 0]匀速转弯运动线速度0.5 m/s角速度0.3 rad/s仿真时长50秒步长0.1秒。环境里放3个路标坐标分别是[2, -2]、[-3, 4]、[0.5, 5]。观测量的噪声设置为距离标准差0.05米方位标准差0.02弧度。这个噪声水平其实不算大但下面会看到标准EKF照样会变得不一致。路标初始位置未知协方差初始化为一个大值例如diag([0.1^2; 0.1^2])表示自己对路标位置很不确定。初始机器人位姿我假设已知给了很小的先验方差这样能避免初始化本身引入的偏差干扰对一致性问题的观察。为了聚焦可观测性我让机器人每一时刻都能观测到所有路标并且观测与路标编号的对应关系已知。真实工程里要做数据关联但那个变量会严重干扰一致性分析所以仿真阶段先把关联问题摘掉。3.2 标准EKF-SLAM核心代码下面是标准EKF-SLAM仿真里最核心的预测和更新代码片段状态向量组织方式与第2节一致。% x_pred [xr; yr; theta; lx1; ly1; ...; lxM; lyM] function x_pred predictState(x, v, omega, dt) th x(3); x_pred x; x_pred(1) x(1) v*dt*cos(th); x_pred(2) x(2) v*dt*sin(th); x_pred(3) th omega*dt; % 路标不动 end function F jacobianF(x, v, omega, dt) dim length(x); F eye(dim); th x(3); F(1,3) -v*dt*sin(th); F(2,3) v*dt*cos(th); % 对路标部分保持单位阵 end观测预测函数和雅可比函数function z hObs(x, lmIdx) dx x(3 2*lmIdx) - x(1); dy x(4 2*lmIdx) - x(2); z [sqrt(dx^2 dy^2); atan2(dy, dx) - x(3)]; end function H jacobianH(x, lmIdx) dx x(3 2*lmIdx) - x(1); dy x(4 2*lmIdx) - x(2); r2 dx^2 dy^2; r sqrt(r2); H zeros(2, length(x)); H(1,1:3) [-dx/r, -dy/r, 0]; H(1,32*lmIdx:42*lmIdx) [dx/r, dy/r]; H(2,1:3) [dy/r2, -dx/r2, -1]; H(2,32*lmIdx:42*lmIdx) [-dy/r2, dx/r2]; end主循环里EKF的更新是逐路标进行的for k 1:numSteps-1 % 预测 xPred predictState(xEst(:,k), v_k(k), omega_k(k), dt); F jacobianF(xEst(:,k), v_k(k), omega_k(k), dt); PPred F * PEst(:,:,k) * F Q; % 用所有路标观测做一遍序贯更新 xTmp xPred; PTmp PPred; for j 1:M z obsData(:,j,k); % 实际观测量 zHat hObs(xTmp, j); H jacobianH(xTmp, j); S H * PTmp * H R; K PTmp * H / S; xTmp xTmp K * (z - zHat); PTmp (eye(length(xTmp)) - K*H) * PTmp; end xEst(:,k1) xTmp; PEst(:,:,k1) PTmp; end这段代码并不复杂很多人的第一版EKF-SLAM基本长这样。问题在于跑完一圈后去看NEES会发现它远超合理区间。注意我特意没有在代码里做状态增维因为固定路标集合的可观测性分析更清晰。真实问题的核心在这个简化模型里已经存在加了新路标只会更糟。3.3 计算可观测性矩阵和秩要在仿真中观察问题我做了两步一是保存每一步的F矩阵和H矩阵二是用这些矩阵构造离散可观测性矩阵并求秩。离散时变系统的可观测性矩阵定义为从初始时刻开始把所有经过状态转移累积后的观测矩阵堆叠起来。数学写法是O [H0; H1*F0; H2*F1*F0; ...]。代码实现如下function [O, rankObs, singularValues] computeObservability(FHistory, HHistory, N) dim size(FHistory{1}, 1); O []; Phi eye(dim); for i 1:N Phi FHistory{i} * Phi; O [O; HHistory{i} * Phi]; end singularValues svd(O); tol max(size(O)) * eps(max(singularValues)) * 100; rankObs sum(singularValues tol); end注意窗口长度N很关键。取太短系统还没被充分激励秩可能低于理论值取太长线性化累积误差会让雅可比偏离真实但用于观察一致性趋势还是够的。我习惯取N50也就是5秒的滑窗数量和数值实验结果都比较稳定。在标准EKF仿真中我统计每50步窗口的可观测性秩。M3时状态维度9理论可观测维数6。实验结果让我印象很深标准EKF在大多数时刻的数值秩都在7到9之间极少掉到理论值6附近。这说明线性化后的系统虚构出至少1到3个可观测方向。3.4 诊断结果NEES与秩的对应关系把NEES和秩放到一起看对应关系非常直观。标准EKF的平均NEES大概是6.8到8.3之间而理论目标大概是9状态维度看起来似乎没有严重低估这里有个陷阱NEES应该对状态真值和真实误差计算但状态维度较高时某些方向误差被过度压缩、另一些方向误差被放大平均NEES可能“看起来还可以”。所以不能只看平均NEES还要看它是否超过95%置信区间上界。在我做的20次蒙特卡洛仿真里置信上界大约在12.5附近标准EKF并没有每次都超界但局部时间片段会明显超出一旦把状态拆成机器人位姿部分和路标部分分别计算NEES机器人位姿的NEES经常冲到15以上这就是定位子系统的“局部不一致”。更关键的是秩的异常每当可观测性秩从理论6变成8或9时对应时刻NEES就开始飙升。时间上高度重合。反过来如果用后文改进方法把秩固定到理论值NEES就稳定回落。这基本验证了结论EKF-SLAM不一致性很大程度上是线性化把不可观测方向“打开”造成的。4. 从“诊断”到“修复”基于可观测性约束的改进4.1 修复思路AFEJ固定首次估计雅可比既然问题出自每步重新线性化导致可观测性被抬高那最自然的修复就是让雅可比不要那么“多变”。FEJ全称First-Estimate Jacobian思路很直接系统中每个状态首次被使用的时候记录当时的估计值之后计算雅可比时固定使用这个首次估计值而不是当前最新估计。为什么固定首次估计就能恢复可观测性因为EKF-SLAM线性化后的可观测性矩阵本质上是由各步雅可比矩阵组合出来的。如果雅可比在每一步都基于当前状态变化不同时刻的近似方向就很容易“捏造”出额外可观测量。固定首次估计后系统虽然仍然是一个近似线性模型但这个近似模型的不可观测方向会与原系统保持一致。实现上比我预想的简单。在Matlab代码里只要缓存一份首次更新后的状态xFe然后把jacobianH的调用从xTmp改成xFe即可xFe xEst_after_first_full_update; % 第一次完成所有路标更新后固定 % 之后每步 H jacobianH(xFe, j);严格来说运动模型的雅可比F里涉及航向角的部分也应该使用首次估计但实际仿真中F对可观测性秩的影响相对较小主要矛盾在观测雅可比H。为了实验一致性我还是把F里的theta也固定为首次估计的theta。改动量很小效果却非常直接可观测性数值秩基本稳定在理论秩6附近。4.2 修复思路BOC-EKF增益投影FEJ是“治本”的方案但有时我们会碰到系统必须使用最新状态来计算雅可比的应用比如观测模型高度非线性。另一个从可观测性角度出发的修复思路是OC-EKF全称Observability-Constrained EKF可观测性约束EKF。OC-EKF的思想不再去改雅可比而是对卡尔曼增益做投影。每次更新前先根据当前线性化矩阵计算系统的可观测性矩阵并求出其零空间。零空间就是理论上不可观测的方向。既然EKF不应该在这些方向上获得任何新信息那增益在这些方向上的分量就应该被强制置零。实现上就是构造一个零空间投影算子把它乘到标准卡尔曼增益上再进行状态更新和协方差更新。这种方法的理论推导比较绕而且数值实现里有一个关键细节如何正确选择零空间基如何避免把数值噪声当成真实可观测方向。我在实验中用FEJ足够了所以OC-EKF没有作为一个独立模块放进最终脚本但如果你面对的系统不允许固定雅可比可以考虑这条路。4.3 在之前的Matlab脚本里怎么改为了让读者能直接复现我把FEJ版本的循环放一段在这里。和3.2节的代码对比唯一明显区别就是雅可比函数传入的不再是当前状态而是固定状态xFe。% 初始化后第一次完整更新得到 xFe xFe xTmp; % 首次完成路标更新后的状态 % 主循环 for k 1:numSteps-1 xPred predictState(xEst(:,k), v_k(k), omega_k(k), dt); F jacobianF(xFe, v_k(k), omega_k(k), dt); % 固定F PPred F * PEst(:,:,k) * F Q; xTmp xPred; PTmp PPred; for j 1:M z obsData(:, j, k); zHat hObs(xTmp, j); % 预测观测仍用当前状态 H jacobianH(xFe, j); % 固定H关键改动 S H * PTmp * H R; K PTmp * H / S; xTmp xTmp K * (z - zHat); PTmp (eye(length(xTmp)) - K*H) * PTmp; end xEst(:,k1) xTmp; PEst(:,:,k1) PTmp; end这里注意hObs(xTmp, j)还是基于当前状态计算预测量因为这是我们真实认为的观测值只有线性化用的H才是固定值。这种“预测值用最新雅可比用首次”的做法一开始看着别扭但理解它背后的可观测性动机后就会明白这正是为了不让线性化方向随估计漂移而破坏系统结构。5. 实验对比与实际踩坑记录5.1 对比结果我在同一组仿真条件下对比了标准EKF和FEJ-EKF的表现。为了不让单次随机结果主导结论我做了20次蒙特卡洛。结果大致如下指标标准EKFFEJ-EKF平均NEES整体状态8.77.2机器人位姿NEES均值15.33.6位姿RMSE米0.790.31路标位置RMSE米1.260.42可观测性数值秩理论67至9波动稳定在6单步运行时间基准几乎无差别整体状态的平均NEES看似差别不大但机器人位姿NEES差距非常明显。这告诉我不一致性经常先体现在局部状态上只看整体均值会被“平均”掉。可观测性秩从7至9降回6后位姿估计的RMSE也下降了60%以上。这说明从可观测性角度入手不仅能改善系统“自信”程度还能顺带提高真实精度。5.2 哪类场景最容易触发不一致实验过程中我试着改变参数发现下面几个条件会让不一致性迅速放大路标数量少。只有1到2个路标时状态空间里的相对约束变弱线性化更容易构造出虚假可观测方向。M3时秩波动已经很大M2时更明显。转弯剧烈。机器人大角度转弯会让航向角进入雅可比矩阵非线性程度更高H矩阵在不同时刻变化更剧烈可观测性被抬升得更厉害。观测噪声不对称。距离噪声和方位噪声差异过大时EKF对某些方向的压缩会更不均匀加剧NEES异常。初始协方差设置过小。如果一开始就认为机器人位姿非常准后续观测对全局方向的修正权重也会失真可观测性问题被放大。5.3 常见问题排查记录在写这个Matlab研究框架的过程中我踩过不少坑挑几个典型的记录一下。第一个坑是可观测性矩阵窗口长度N的敏感性。如果N取5系统几乎每个时刻秩都小于理论值因为激励还没积累起来如果N取500秩又会因为数值误差虚高。我后来改成先看奇异值谱找一个明显跳变点再用相对阈值统计秩。这样比固定一个绝对阈值要稳健得多。第二个坑是H矩阵的维度匹配。EKF-SLAM状态维度会随路标数量增长如果代码里写成固定维度很容易在变量维度不一致时静默出错。我建议所有关于H和F的函数都通过length(x)获取维度而不是写死数字。这样后面如果要加新路标也不容易出错。第三个坑是NEES计算时用了错误的真值。在做可观测性分析时我会同时保存真值轨迹、滤波器估计和协方差矩阵。如果真值轨迹和估计轨迹的时间轴错位一个采样周期NEES会突然变得很大可能误判滤波器出了问题。还有一个经验每次做完一步EKF就把当时的F和H矩阵存下来。后续做可观测性分析时你会非常庆幸自己存了这些内容。我在最初几版代码里没有存后来为了分析不得不重新跑仿真浪费了不少时间。6. 一点个人经验这个仿真做下来我最大的感受是EKF-SLAM的不一致性问题不是滤波器“算错了”而是滤波器使用了大量虚假信息。从可观测性角度看虚假信息来源于线性化后的系统出现了原系统根本没有的方向。只要你还用EKF这一步近似就无法完全避免但我们可以通过约束雅可比的使用方式来抑制它。FEJ在我这套仿真里几乎零成本地就把NEES拉回了合理范围非常值得在工程里作为默认选项尝试。最后分享一个小技巧把可观测性分析做成一个独立的诊断工具每跑一次EKF就顺带算一次数值秩。做法很简单在主循环外把F_history和H_history两个cell保存下来仿真结束后再调用computeObservability函数。你会直观地看到秩和NEES的变化是同步的。如果你也遇到EKF-SLAM结果“看着在跑实则在飘”的情况不妨先检查可观测性秩是不是比理论值大了。通常做到这一步离解决办法就不远了。