企业官网建设流程全解析

搞了一段时间的移动机器人定位建图，手里积累了一批“看着没问题、细看全是问题”的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个路标，传感器是距离-方位模型，数据关联假设已知。具体参数如下：

选择这些参数有讲究。过程噪声不能调得太小，否则滤波器天然过度自信，会混淆“参数问题”和“结构问题”；测量噪声选相对干净的传感器，也是为了让问题聚焦到线性化结构上。这组参数让标准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_l：

dx = 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(b+x(3)), -r*sin(b+x(3)); sin(b+x(3)), r*cos(b+x(3))]; Gx = [1, 0, -r*sin(b+x(3)); 0, 1, r*cos(b+x(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_O+1: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]区间内。这组参数下d=3、N=50，95%接受区间大约是[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的代码算一下，结果非常清晰：

在我们的仿真设定里，状态维度是3+2×16=35，真实系统不可观维度是3；而标准EKF累积观测矩阵算出来的不可观维度稳定在1。这跟文献中“标准EKF-SLAM线性化系统比真实系统多两个可观测自由度”的结论完全对上。多出来的那两个自由度，恰恰就是全局位置和全局朝向——虚假信息的注入通道。

数值上的一致性非常好：NEES出界的时机，和可观测性维度出现偏差的时机高度相关。这也是我做这个项目的核心收获——有了可观测性这个工具，不一致性问题就不再看不清摸不着了。

4.3 FEJ-EKF修正思路与实现要点

修正思路很直接：既然问题出在线性化点随估计变化、破坏了对称性，那就把线性化点钉死。FEJ（First 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初始化。前面说过，交叉协方差不能填零。填零会让新路标和机器人之间失去正确相关，后续观测更新会做出错误的信息融合。

我把这些问题整理成一张速查表：

5.2 实验设计层面的坑

蒙特卡洛次数太少，NEES曲线噪声很大，看不出趋势；太多又跑得慢。50次是个不错的平衡点。我习惯先跑20次看趋势，确认问题出现的位置，再跑满50次出正式图。

过程噪声的设置也要小心。我一开始把σ_v和σ_ω调得很小，结果NEES没等线性化问题完全暴露就已经冲出上界，根本分不清是参数问题还是结构问题。正确做法是先把参数调到中等合理，让标准EKF在最初四五十步内保持“看似一致”的假象，再让它慢慢暴露问题，这样才有对照意义。

数据关联必须要固定为已知。一旦数据关联出错，NEES会瞬间爆炸，但那属于另一类问题，会掩盖我们要研究的可观测性缺陷。做一致性实验时，仿真里直接给正确对应关系，别用最近邻匹配，否则结论很难干净。

需要说明的是，这些坑大多来自通用工程实践和本项目调试过程中的多次排查，不一定覆盖所有代码写法，但按这个流程排查能省下大量时间。

5.3 从仿真走向真机要注意什么

仿真里我们能控制一切，真机就不行了。真实传感器噪声往往不是高斯、不是白噪声，里程计有系统漂移，路标检测有漏检和误检。仿真里好用的FEJ参数，到真机上通常需要重新调，而且数据关联问题会变成主要矛盾。

但可观测性这个分析框架依然有效。它告诉我们某个方向的信息从物理上拿不到，任何滤波结果都不该在那些方向过度自信。真机调试时，我依然会先离线采集一段数据，用同样的方法算一遍线性化系统的不可观维度，再决定要不要上FEJ修正。这一步几乎不花时间，却能提前暴露很多“看起来玄学”的滤波器问题。

以后再看某个SLAM系统结果异常，我的第一反应不是急着调噪声，而是先算一算它的线性化模型到底可观测到什么程度。很多时候，问题根本不在参数，而在模型结构本身。建议直接把3.3的可观测性计算代码抄回去，先算不可观维度，再谈调参——这个顺序能帮你省下大量反复试错的时间，也更容易看清问题的本质。