流形上的优化：SO(3)与SE(3)的广义加减法在FAST-LIO中如何简化状态估计
在FAST-LIO这类紧耦合的激光惯性里程计系统中，状态向量通常包含位置（ℝ³中的平移）和姿态（SO(3)中的旋转）两种不同类型的量。这两类量的加法运算是互不相容的：平移可以用普通的向量加法直接叠加，旋转却需要通过矩阵乘法或四元数乘法组合。这种类型不一致使得状态估计中的线性化和滤波更新变得非常棘手。
FAST-LIO的解决方式是通过定义广义加减法（⊞和⊟）来统一状态向量各组成部分的更新方式，从而将流形上的非线性问题转化为线性空间中的常规卡尔曼滤波问题。
一、从旋转矩阵到流形
旋转的表示方式存在一个根本性的矛盾：为了满足运算的封闭性，旋转矩阵（SO(3)）和四元数需要用过参数化的形式（9维或4维）来描述仅有3个自由度的旋转；为了用最少的参数表示，欧拉角会遭遇万向锁带来的奇异性问题。然而状态估计中的卡尔曼滤波又要求状态量在向量空间上定义良好的加法运算。
流形提供了解决这一矛盾的数学框架。SO(3)是一个三维流形，局部具有欧几里得空间的结构。这意味着，在旋转矩阵附近的一个小范围内，可以用一个三维向量（即李代数）来参数化旋转的“扰动”。这个局部到整体的桥梁，就是指数映射和对数映射。
指数映射将李代数中的旋转向量映射到SO(3)上的旋转矩阵。当旋转向量 φ 的模长为 θ 时，对应的旋转矩阵可由罗德里格斯公式给出。对数映射则是其逆操作，将一个旋转矩阵映射回李代数中的旋转向量。
将旋转矩阵 R 与一个小的旋转向量 r 进行“加法”，实际上是通过指数映射完成的：
text
R ⊞ r = R · Exp(r)
将 r 经由指数映射转变为旋转矩阵，再与原旋转矩阵相乘。这就是将流形上的点“向前移动一小步”的正确方法。
二、广义加减法的精确定义
设 M 是一个 n 维的流形（例如 SO(3)），Rⁿ 是其切空间。FAST-LIO论文中引入了两个操作符：
广义加法（⊞）：
text
⊞ : M × Rⁿ → M
其含义是：在流形上的一个点 x 处，施加一个小的切空间扰动 u，得到一个新的流形上的点 y = x ⊞ u。
广义减法（⊟）：
text
⊟ : M × M → Rⁿ
其含义是：计算流形上两点之间的差异，并用切空间中的一个向量来表述。
当流形取具体的几何对象时，这些运算符的含义变得非常直观。当 M = SO(3)（旋转矩阵）时，R ⊞ r = R · Exp(r)，R1 ⊟ R2 = Log(R2^T · R1)。当 M = Rⁿ（平移向量）时，a ⊞ b = a + b，a ⊟ b = a - b。
对于复合流形，例如 FAST-LIO 的状态向量 x = (R, p, v, bω, ba) ∈ SO(3) × R¹⁵，广义加减法也被相应地定义为在各个子空间上独立执行上述操作，再将结果组合起来。
三、将流形上的误差传递简化为欧几里得形式
广义加减法的一个重要性质，使得流形上的误差传递能够以向量空间中的常规方式来表述，即：
text
(x ⊞ u) ⊟ x = ux ⊞ (y ⊟ x) = y
对任意 x, y ∈ M 和 u ∈ Rⁿ 成立。这一性质表明：在局部切空间中进行加法运算，然后“投影”回流形，再通过广义减法将该点“减”回原点的切空间，得到的扰动向量与最初施加的完全一致。
在FAST-LIO中，每个时刻的状态变量是流形 M 上的点（如位姿），而状态估计的误差量（误差状态）被定义在切空间 Rⁿ 中。真实状态 = 名义状态 ⊞ 误差状态。通过在切空间（线性空间）中完成误差状态的所有线性运算（如卡尔曼滤波的预测和更新），然后在需要时通过广义加法（⊞）将误差叠加到名义状态上，获得流形上新的状态点。
这种方式既避免了直接对旋转矩阵或四元数做加法（这一操作无意义），又充分利用了切空间的优良线性性质来进行状态估计。在ESKF的理论框架中，误差状态变量可以在原点附近取极小的值，线性化近似非常精准，离奇点较远，雅可比矩阵的计算也因此大为简化。
四、源代码实现
FAST-LIO2中的状态估计依赖一个名为IKFoM（Iterative Kalman Filter on Manifold）的工具箱，该工具箱的核心就是基于广义加减法构建了一套完整的流形运算体系。
1. 状态向量定义：use-ikfom.hpp
该文件使用宏构建了一个名为 state_ikfom 的复合流形结构体。该结构体包含位置、姿态（SO3）、速度、IMU零偏以及重力向量等。同时，该宏为该结构体自动生成了 boxplus 和 boxminus 方法，能够正确处理其中每个子流形的运算。
2. boxplus操作实现
在流形运算的头文件中，boxplus函数为实现广义加法运算而实现。对于旋转部分（SO3），它使用指数映射将李代数扰动（三维向量）转换为旋转矩阵，然后与原旋转相乘；对于平移速度等欧氏部分，则直接使用向量加法。一种实现方式的参考代码片段为：
cpp
state_ikfom boxplus(state_ikfom x, Eigen::Matrix<double, 24, 1> f_) {
state_ikfom x_r;
x_r.pos = x.pos + f_.block<3, 1>(0, 0); // 平移：直接向量加法
x_r.rot = x.rot * Sophus::SO3::exp(f_.block<3, 1>(3, 0)); // 旋转：使用指数映射
return x_r;
}
旋转部分的处理体现了广义加法的核心思想——将线性空间中的扰动通过指数映射提升回流形，再进行组合。
3. boxminus操作实现
在状态更新和观测方程线性化中，需要计算流形上两点之间的误差向量。boxminus函数通过对数映射实现这一操作：
text
Vec = point1 ⊟ point2 = Log(point2^{-1} ∘ point1)它返回的 Vec 直接就是定义在流形切空间上的误差状态，可以直接用于滤波器的更新方程。
五、参考资料
FAST-LIO原论文：Xu, Wei, and Fu Zhang. "FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter."IEEE Robotics and Automation Letters, 2021. arXiv版本
《视觉SLAM十四讲》：高翔等著，第3-4讲（李群与李代数）
IKFoM工具包代码解读系列：基于Boost Preprocessor的Manifold Toolkit (MTK) 通用流形类构建的系列博客
知乎专栏《FAST-LIO论文简要介绍》（作者：严国陶）：提供了对论文核心概念的体系化解读
FAST-LIO官方代码仓库：
https://github.com/hku-mars/FAST_LIO和https://github.com/hku-mars/FAST_LIO2
六、推荐学习路径
先回顾《视觉SLAM十四讲》第3-4讲（李群与李代数的基本概念），理解旋转矩阵、四元数、指数映射及对数映射的基本内容。
阅读FAST-LIO论文，重点关注“操作符”的定义及ESKF离散化过程的公式（特别是式（1）、（4）、（5）、（22）等）。
结合官方代码的 use-ikfom.hpp 文件和IKFoM工具箱中与流形相关的头文件，理解 boxplus 和 boxminus 函数在状态预测和更新步骤中如何被调用。
参考CSDN博客《FAST-LIO论文梳理及公式理解》，通过其中的图文对照加深对公式和代码之间映射关系的理解。
初步掌握概念后，在仿真环境中设置不同的传感器噪声水平，观察算法实际工作时的收敛过程，加深对理论的直观认识。