FAST LIO与ESKF：激光雷达-惯性里程计的核心技术解析

1. FAST LIO与ESKF基础概念解析

FAST LIO（Fast LiDAR-Inertial Odometry）是近年来SLAM领域备受关注的一种高效激光雷达-惯性里程计算法。作为一名长期从事SLAM系统开发的工程师，我认为其核心创新点在于巧妙结合了误差状态卡尔曼滤波（ESKF）与紧耦合的激光雷达-惯性测量单元（LiDAR-IMU）融合框架。

1.1 传统KF与ESKF的本质区别

在机器人状态估计领域，卡尔曼滤波（KF）家族算法已有半个多世纪的应用历史。但传统KF在处理三维空间中的姿态估计时存在固有缺陷：

• 参数化问题：三维旋转属于SO(3)流形空间，而KF的误差状态空间是欧式空间，直接使用欧拉角或四元数会导致参数化不一致
• 奇异点问题：欧拉角存在万向节锁现象，在特定姿态下会丢失自由度
• 线性化误差：当姿态变化较大时，一阶泰勒近似的线性化假设不再成立

误差状态卡尔曼滤波（ESKF）通过将状态分解为名义状态（nominal state）和误差状态（error state）来解决这些问题。具体实现上：

cpp复制// 典型ESKF状态变量定义
struct State {
    Eigen::Quaterniond q_GtoI;  // 名义姿态（IMU系到全局系）
    Eigen::Vector3d p_IinG;     // 名义位置
    Eigen::Vector3d v_IinG;     // 名义速度
    Eigen::Vector3d bg;         // 陀螺仪零偏
    Eigen::Vector3d ba;         // 加速度计零偏
    Eigen::Vector3d theta;      // 姿态误差状态（极小旋转向量）
    Eigen::Vector3d dp;         // 位置误差状态
    // ...其他误差状态
};

关键理解：ESKF的核心思想是保持名义状态在流形上自然演化，而误差状态始终在欧式空间中进行卡尔曼滤波更新。这使得算法既保持了数学严谨性，又能利用成熟的线性滤波理论。

1.2 FAST LIO的算法框架特点

FAST LIO的创新之处主要体现在三个方面：

1. **前端