企业官网建设流程全解析

最近把后端的滤波内容重新翻出来梳理，又认真读了一遍《A Quick Guide for the Iterated Extended Kalman Filter on Manifolds》这篇经典文献。做视觉SLAM、激光SLAM的朋友应该都有体会：建图跑得欢，定位爆得也快，尤其是当你的状态里带着旋转矩阵的时候，标准EKF一个“加法”下去，姿态可能就直接飞了。这篇指南讲的就是怎么在流形上正确地把迭代卡尔曼滤波这件事做对。它篇幅不长，但数学密度不小，推导干净利落，非常适合已经读过《视觉SLAM十四讲》、对滤波和图优化有基本概念，但还没彻底把“李群上的高斯分布”“误差状态更新”吃透的人。

这篇文献解决的核心问题非常具体：传统扩展卡尔曼滤波在更新那一步，默认状态空间是平直的，可以直接做 (x^+=x+Ky) 这种线性加法；但在SLAM里，你的状态里往往有旋转矩阵 (R) 或者完整位姿 (T)，它们生活在一个弯曲的流形上，加法一加就把状态推出流形了。这篇指南把迭代扩展卡尔曼滤波搬到流形上，告诉你每一步应该在哪算误差、在哪线性化、怎么用指数映射把更新“拉回”流形，并且给出了可以直接落地的实现流程。

下面我从原理到实操，把这篇文献掰开揉碎讲一遍，顺便把我自己实现和调试过程中踩过的坑都摆出来。

1. 从一次失败的EKF谈起：流形上的“加法陷阱”

1.1 旋转不是向量：状态“加法”的物理含义失效

先说一个我早期调程序遇到的真实例子。当时做一个小范围视觉定位，状态向量直接用了欧拉角加平移，也就是所谓的“最省事”参数化。EKF预测步还算正常，但一进更新步，当观测噪声稍微大一点，姿态角就会跳变，甚至直接发散。当时我不理解，后来才意识到问题出在哪：欧拉角本来就不是一个线性空间里的坐标，三个角度的组合在空间里是有奇异性的，某些姿态附近，一个微小的角速度增量会对应很大的角度跳变，卡尔曼增益一放大，姿态直接崩。

更本质的问题是，旋转矩阵 (R) 或者位姿变换矩阵 (T) 作为一个整体，是生活在旋转群 (SO(3)) 或刚体运动群 (SE(3)) 上的元素。这个集合的几何结构是弯曲的流形，不满足普通向量空间的加法封闭性。你随手取两个旋转矩阵，把它们对应元素相加，结果几乎肯定不是一个合法的旋转矩阵，甚至连正交性都不满足。这就是所有“直接在矩阵元素上做加减”的滤波器，在SLAM中跑一段时间后协方差矩阵变得不正定、状态失去约束的根本原因。

1.2 一个合格的状态估计器应该长什么样

在流形框架下，状态量本身应该被视为一个流形元素 (x)，而滤波器估计的实际上是“状态的误差”该长什么样。这个误差不是用 (x_{\text{true}} - x_{\text{mean}}) 这种普通减法来定义，而是要借助李代数，把两个流形元素之间的差异映射到切空间上的一个向量。切空间是平直的，误差作为切向量，享受和普通向量一样的加减法、缩放和概率分布定义。这也是为什么SLAM后端滤波的文献最终都走向了“状态在流形上，误差在切空间”这个结论：说白了，就是把几何和概率分开处理，各算各的账。

这个思路在论文里的表述很清晰：先定义一个所谓“retraction”（回撤映射），它把切空间中的扰动向量映射回流形上的元素。最常用的就是指数映射，也就是 (x \cdot \exp(\delta)) 这种形式。滤波器更新的每一步，都是在当前估计点 (x) 附近建立局部坐标系，在这个局部坐标系里做标准EKF的操作，最后再用指数映射把修正量“投射”回流形上。整个过程看起来优雅，但第一遍看的时候很容易被各种符号绕晕。

2. 流形滤波的地基：SO(3)、SE(3)和指数映射

2.1 为什么最小参数化总是出问题

很多人会问：既然直接用旋转矩阵元素不行，那我用四元数行不行？四元数确实是SLAM里常用的旋转表达，但四元数本身也是一个约束流形——单位范数约束。做EKF时你必须处理四维状态里的冗余维度，再加上四元数双覆盖的问题，(q) 和 (-q) 表示同一个旋转，这在滤波更新的过程中非常容易引发符号翻转。很多新手在这里头大，说到底是因为没有真正把“流形元素”和“切空间坐标”分开。

论文里的做法是干脆把状态直接建模在 (SO(3)) 或 (SE(3)) 上，不引入多余的自由度，每个误差变量都落在切空间，维度刚好等于流形维度。这样既没有约束问题，也没有参数化的奇异问题。我第一次用这套框架重写代码时，感觉就是过滤器从“走钢丝”变成了“走路”：“误差空间”里的协方差矩阵始终保持正定，状态始终合法，调起参来非常安心。

2.2 左扰动还是右扰动：协方差住在哪

理解流形滤波的时候，第一个绕不开的选择题就是左扰动还是右扰动。左扰动的意思是，真实状态可以表示为 (x = \exp(\delta^{\wedge}) \cdot \bar{x})，也就是在估计值左边乘一个小扰动；右扰动则是 (x = \bar{x} \cdot \exp(\delta^{\wedge}))。这个选择会影响你雅可比矩阵的形式、协方差的定义，以及最后协方差是活在“世界系”还是“载体系”里。

在SLAM的实际场景里，传感器观测通常描述的是“载体坐标到世界坐标”的关系，而IMU角速度、轮速计增量这些又天然定义在载体坐标系。如果你选择的扰动方向和观测模型的方向不匹配，推导雅可比的时候就会多出一堆链式法则，而且很容易推错。文献里主要采用的是右扰动，但更重要的是，它告诉你：只要你的误差状态定义清楚，在哪个“方向”扰动并不影响最终结果，影响的是公式的简洁性和数值稳定性。我自己实现时选的是右扰动，因为视觉重投影误差对位姿的雅可比，在右扰动模型下反而更好求，这个大家可以根据自己的传感器模型灵活决定。

2.3 高斯分布其实定义在切空间

另一个关键的认知颠覆是，流形上并没有真正意义上的“高斯分布”，严格说没有一个既保持流形几何又等价于欧氏高斯分布的分布。工程上的做法是：把高斯分布定义在某个点（通常是均值）的切空间上。也就是说，我们估计状态 (x) 的均值 (m)，并且假设误差 (\delta) 服从切空间里的高斯分布 (\mathcal{N}(0, P))。这样P就还是那个熟悉的协方差矩阵，计算上完全复用标准卡尔曼框架，只是每次要用指数映射把误差“叠加”到均值上去，从而得到流形上的真实状态。

这一点想通了之后，整个IEKF的推导就顺畅很多了。预测步里，你先在切空间里做误差传播；更新步里，你在切空间里计算卡尔曼增益；最后把增益和残差的乘积当成切空间里的修正向量，用指数映射更新流形状态。整个滤波过程从数学上就变成了：流形状态在“宏观”层面做非线性运动，切空间误差在“微观”层面做标准高斯推断。这种看问题的方式，是我读这篇文献最大的收获之一。

3. 一步步拆解流形上的IEKF迭代更新

3.1 从标准EKF的线性化痛点说起

标准EKF的更新步很简单：在当前估计点处把观测模型做一阶泰勒展开，算出雅可比 (H)，然后更新增益 (K) 和状态。这个做法的前提是，当前估计点离真实点足够近，泰勒展开的一次项能很好近似非线性函数。但在SLAM里，这个前提经常不成立。最典型的场景：机器人位姿不确定度较大，或者观测值本身有强非线性，比如鱼眼相机、激光点云配准、大视角变化下的视觉重投影。这种情况下，标准EKF用一次线性化得到的增益 (K) 往往是“歪”的，更新后状态反而更差，甚至有发散风险。

图优化派的做法是用高斯牛顿迭代优化，每次重新线性化直到收敛。IEKF本质上就是把这种迭代思想搬进了滤波框架。文献里的核心洞察就是：更新步本质上是一个以预测值为初值的优化问题，优化目标是让后验概率最大；与其像标准EKF那样只做一步线性化，不如在这个优化里多迭代几次，每次都重新在当前状态点计算雅可比和增益。迭代收敛后的结果，精度更接近非线性高斯牛顿的解，同时保住了滤波的递推结构。

3.2 IEKF的迭代机制：把一次更新变成N次优化

读这篇文献的时候最爽的部分，就是把IEKF的核心循环看懂。标准EKF里的更新公式可以看作只执行了一次的高斯牛顿迭代，而IEKF只是在内部加了一个循环：每轮迭代里，先基于当前状态 (x_i) 重新算雅可比 (H)，再算增益 (K_i)，得到切空间修正量 (\delta_i)，然后用指数映射把它作用到当前状态 (x_{i+1} = x_i \cdot \exp(\delta_i))。需要特别注意的是，协方差矩阵 (P) 在迭代过程中始终保持不变，因为它描述的是预测状态自身的置信度，不是随着均值点移动而重新估计的。

为什么这么设计？因为预测协方差代表的是“我从运动模型那里得到的对状态的认识”，这个认识在观测到来时已经确定了，迭代只是在调整均值，让它与观测更一致。从实现角度看，这意味着增益可以不断变化，但不需要反复做协方差的传播，省掉了很多计算，也避免了迭代过程中协方差被重复“挤压”的数值问题。最后等收敛到最优状态 (x^*) 之后，再用那一次的增益对协方差做标准更新 (P = (I - KH)P)。这个细节很多资料不讲，但实际上非常关键。

3.3 收敛判断与终止条件：别让迭代白跑

既然有了迭代，自然就要有收敛判据。文献里通常用的是切空间修正量 (\delta) 的范数：当 (|\delta|) 小于某个阈值时，认为修正量已经很小，状态变化不再显著，就停止迭代。工程上我一般会同时加两个保护条件：一个是最大迭代次数上限，防止在病态条件下死循环；另一个是如果连续几次迭代的代价函数值（比如加权残差平方和）没有明显下降，就提前终止。

这里有个很容易踩的坑：阈值不能设得太死。切空间修正量的尺度取决于状态量的单位，姿态误差的量级大概在0.001以下才算收敛，但位置误差可能0.01米量级也很正常，所以最好对旋转和平移分别设阈值，或者把误差向量归一化后再比较。我在实现里是把 (\delta) 按旋转部分和平移部分拆开，分别判断，效果比单独用一个总范数稳定得多。

4. 从推导到代码：实现流形IEKF的关键细节

4.1 状态定义与误差状态法

动手写代码前，先要把状态表示定下来。我的建议是总状态 (x) 包含流形上的位姿部分 (T \in SE(3)) 和普通欧氏空间部分（比如速度、零偏、地图点坐标），切空间误差向量则为： (\delta = [\delta_\theta, \delta_t, \delta_v, \delta_b]^T)，其中 (\delta_\theta) 和 (\delta_t) 是位姿的旋转和平移误差，后面是速度、零偏等。位姿更新用指数映射，普通状态用加法。换句话说，你的代码里至少要有两个“加法”函数：

• boxplus(x, delta)：流形状态 x 加切空间扰动 delta，位姿部分走指数映射，其他部分直接加法。
• boxminus(x1, x2)：求 x1 相对 x2 的切空间误差，用于残差计算。

论文里的符号体系差不多也是这个意思。把这两个函数写好，后面推雅可比和写迭代循环都会顺很多。千万不要把它们混成一个函数，否则早晚出bug。

4.2 雅可比计算最容易踩的坑

IEKF对雅可比的依赖比标准EKF更重，因为每轮迭代都要重算一次雅可比，写错一个符号，迭代不但不收敛，还可能直接飞出天际。我踩过的坑主要有两个。

第一个坑是扰动方向搞反。比如你用右扰动，那 (H) 的列方向就应该对应“在状态右侧乘指数扰动导致观测变化”的方向，如果你推的时候不小心用了左扰动公式，看起来挺合理，实际上每一维的偏导数符号全反，迭代就会在真实解附近震荡或者直接发散。一个简单的验证方法是：数值雅可比对比解析雅可比，在迭代的第一步随机扰动几个方向，对比一下，如果误差在1e-5量级以内，说明符号和方向基本对了。

第二个坑是链式法则里漏了变换矩阵和观测帧的关系。视觉重投影时，残差对相机位姿的雅可比往往要考虑把3D点从世界系变换到相机系，然后再投影到像素平面。很多人纸上推导没问题，一写代码就忘了每一步之间的坐标系变换，导致雅可比矩阵的各元素量纲都正确、但数值不匹配。建议实现的时候分段验证：先验证纯旋转部分对残差的数值导数，再验证平移部分，最后合并。

4.3 一段可直接参考的伪代码

这篇文献最让人喜欢的地方，就是它给出了足够清晰的算法框架。我这里给一段我用过的伪代码结构，你们可以直接照着搭：

// 输入: 预测状态 x_pred, 预测协方差 P_pred // 观测值 z, 观测噪声协方差 R // 输出: 更新后的状态 x, 协方差 P x = x_pred; P = P_pred; for iter = 1 : max_iter // 当前状态计算残差、雅可比 H = compute_jacobian(x); // 观测模型对切空间误差的雅可比 S = H * P * H^T + R; // 新息协方差 K = P * H^T * inverse(S); // 卡尔曼增益 e = z - h(x); // 观测残差 delta = K * e; // 切空间里的修正向量 // 收敛判断 if (norm_rot(delta) < eps_rot && norm_trans(delta) < eps_trans) break; // 用指数映射更新流形状态 x = boxplus(x, delta); end // 收敛后更新协方差（只用最后一次增益） P = (I - K * H) * P;

这段代码简洁得惊人，但每行背后都是前两节的数学。值得注意的是compute_jacobian必须在每一次迭代都重新算，因为它的线性化点一直在变。还有，最后协方差更新用的是循环结束时的K和H，不是某一次中间值。如果你发现协方差更新后数值跳动很大，回头检查一下是不是用了旧H。

4.4 几个工程向的数值注意点

用这种迭代滤波，数值稳定性是第一位的。首先，求逆inverse(S)尽量不要显式做，而是用Cholesky分解或者QR分解直接解线性方程组(H P H^T + R) delta = ...。实际中S的维度可能到几百甚至几千，显式求逆既浪费计算，数值上也不如分解法稳。

其次，指数映射的实现要小心。尤其是旋转向量接近180度时，对数映射的奇异性会导致误差向量数值爆炸。虽然SLAM里常规状态下不会出现这么大的姿态差，但在初始化、传感器失效、大外点的情况下确实可能碰到。较好的做法是在指数映射实现里加入“小角度近似”分支，当旋转角度小于某个阈值时直接用一阶近似展开，避免sin(θ)/θ出现除零。这个细节能让你在极端场景下少掉很多头发。

最后，每次迭代完x = boxplus(x, delta)之后，如果状态里有旋转矩阵，记得重新正交化，避免长时间的数值累积让矩阵逐渐偏离流形。这个开销不大，但效果立竿见影，纸面上的流形数学，落到浮点运算上永远要记得“数据并不天然活在流形上”。

5. 实际跑SLAM时的选型与排查经验

5.1 IEKF比标准EKF更优的典型场景

很多人问：我现在的系统用的标准EKF，有必要换成IEKF吗？我的经验是：如果你的观测模型接近线性、噪声不大、初始化做得好，标准EKF和IEKF的差距其实很小，因为一次线性化就已经足够逼近。但如果你遇到以下情况，IEKF的价值会非常明显。

第一个是高非线性观测：视觉重投影、激光点云面配准、超广角或鱼眼相机的投影模型。这些模型的雅可比在不同状态点上变化非常剧烈，单步线性化很容易给出一个偏斜的增益。实测来看，改用IEKF后，同样一个前端VIO系统，在快速运动和大视角变化场景下的姿态漂移明显变小，因为每次更新实际上都在沿着观测面做更精确的优化运动。

第二个是初始误差较大的场景：比如系统启动时姿态粗略初始化，或者中途传感器掉线恢复。这时预测状态离真实状态较远，单步线性化的近似很差，而IEKF可以通过内部迭代一步步把状态“拉近”真实解。我做过一个测试，人为把初始偏航角误差加大到15度，标准EKF直接发散了，IEKF照样收敛了回来，这个差别是很直观的。

第三个是状态约束强的场景：视觉惯性导航里，重力和零偏耦合很强，如果一步更新吃得不准，后面好几秒的状态都会被带歪。IEKF的迭代本质是在每帧上做了一次小型优化，约束满足得更好，长期下来系统整体的稳定性会好不少。

5.2 与图优化后端的取舍：滤波不是万能钥匙

但也要说一句公道话：IEKF不是银弹。它本质上仍然是滤波，历史误差会被压缩在协方差矩阵里，无法像图优化那样通过边缘化、重新线性化、回环检测等手段修正旧轨迹。如果项目以建图为主，需要处理大量回环和漂移修正，图优化后端的优势是实实在在的。我的经验是拿IEKF做实时里程计，定期把关键帧状态交给后端图优化做全局修正，两个配合起来用才是效率最高的方案。

从实现复杂度上说，IEKF的代码量比标准EKF大不了多少，但调参难度有一些差别：增益不再是一把下去定生死，可调空间更大，但相应的也要小心振荡。如果你发现系统更新后位置突然抖动了一下，先检查迭代收敛阈值是不是太松，其次检查延迟是否太大，最后再怀疑雅可比。

5.3 常见问题速查表

这里把我在实现和调试过程中碰到的高频问题整理一个速查表，方便大家对照排查。

5.4 调试经验：可视化与参数调优

调试这种滤波，只看数值日志远远不够。我的习惯是把位姿协方差投影成三倍标准差椭球，和估计位姿一起显示在可视化界面里。这样你一眼就能看见，更新步发生的时候，椭球是不是真的变小了，方向有没有不对劲。调IEKF阈值和迭代次数时，开着这个可视化看真实数据回放，比盯着终端里的数字高效得多。

一个比较神奇的现象：当迭代次数从1增加到5的时候，系统精度提升非常明显，但从5增加到10，收益就不大了。所以我最终的实现里，最大迭代次数设在5到10之间，收敛阈值设在1e-4左右。不要盲目加大迭代次数，因为每一次迭代都要重建雅可比和新息矩阵，计算量并不小。

另外，如果你用的状态不仅有位姿还有地图点，IEKF还有个额外好处：地图点坐标的更新也走同样的迭代流程，相当于每次观测都在做一次稀疏的local bundle adjustment，这比标准EKF对地图点的“每次只推一步”要精确得多。地图点收敛更快，后续的匹配和重投影成功率也更高，整个建图质量会随之提升。

回到这篇文献本身。它给我的最大启发其实不是IEKF这个算法本身，而是一种看待状态估计的视角：把几何结构当作硬约束，把不确定性放在平直空间里，两者的交界处用指数映射这个“桥梁”来精确连接。读这篇文献之前，我在SLAM滤波上一直是东拼西凑的状态；读完之后，整个滤波器的基础思路变得非常统一：流形状态做运动，切空间误差做概率。这套思想后来也帮我在调视觉惯性里程计、处理点云配准初值这些具体问题上少走了很多弯路。

如果你正准备从一个标准EKF的模块往更鲁棒的方案迁移，或者只是想把流形滤波的数学搞得更通透，这篇文献是真的值得抽出一下午细读一遍。读的时候建议手里备着纸笔，跟着它的公式推一遍，再回到这里对照伪代码实现一次。等你真正把IEKF的循环跑通，你会明显感觉到：SLAM后端这件事，从此又清楚了一截。