从代码到公式：手把手拆解FAST-LIO状态预测模块（附C++/Eigen实现）

FAST-LIO状态预测模块的数学与代码全景解析在SLAM系统的开发中状态预测模块的精度直接影响整个系统的定位效果。FAST-LIO作为基于紧耦合迭代卡尔曼滤波的激光-惯性里程计其状态预测模块融合了IMU的高频测量与激光雷达的点云数据实现了精准的状态估计。本文将深入剖析FAST-LIO状态预测模块的数学原理与工程实现帮助开发者跨越理论到实践的鸿沟。1. 状态空间建模与变量定义FAST-LIO的状态空间设计是其核心创新之一。系统状态x包含位置、姿态、速度、传感器偏差等关键信息完整描述了机器人的运动状态。1.1 状态变量结构在C实现中状态通过state_ikfom结构体定义struct state_ikfom { Eigen::Vector3d pos Eigen::Vector3d(0,0,0); // 位置 (x,y,z) Sophus::SO3 rot Sophus::SO3(Eigen::Matrix3d::Identity()); // 旋转矩阵 Sophus::SO3 offset_R_L_I Sophus::SO3(Eigen::Matrix3d::Identity()); // LiDAR-IMU旋转偏移 Eigen::Vector3d offset_T_L_I Eigen::Vector3d(0,0,0); // LiDAR-IMU平移偏移 Eigen::Vector3d vel Eigen::Vector3d(0,0,0); // 速度 Eigen::Vector3d bg Eigen::Vector3d(0,0,0); // 陀螺仪偏置 Eigen::Vector3d ba Eigen::Vector3d(0,0,0); // 加速度计偏置 Eigen::Vector3d grav Eigen::Vector3d(0,0,-G_m_s2); // 重力向量 };数学上状态变量可表示为x {p, R, ΔR, ΔT, v, bg, ba, g}其中p ∈ ℝ³全局坐标系中的位置R ∈ SO(3)局部到全局的旋转矩阵ΔR ∈ SO(3), ΔT ∈ ℝ³LiDAR到IMU的外参v ∈ ℝ³线速度bg, ba ∈ ℝ³IMU偏差g ∈ S²重力向量单位球面1.2 IMU输入定义IMU输入数据通过input_ikfom结构体表示struct input_ikfom { Eigen::Vector3d acc Eigen::Vector3d(0,0,0); // 加速度计测量(m/s²) Eigen::Vector3d gyro Eigen::Vector3d(0,0,0); // 陀螺仪测量(rad/s) };数学表示为 u {a, ω}2. 状态转移函数的实现剖析状态转移函数f描述了系统状态随时间演化的规律是预测模块的核心。2.1 状态转移方程数学上状态转移函数定义为f(x,u) [v, ω-bg, 0, 0, R(a-ba)g, 0, 0, 0]ᵀC实现中的get_f函数对应这一数学表达Eigen::Matrixdouble, 24, 1 get_f(state_ikfom s, input_ikfom in) { Eigen::Matrixdouble, 24, 1 res Eigen::Matrixdouble, 24, 1::Zero(); Eigen::Vector3d omega in.gyro - s.bg; // 角速度减去偏差 Eigen::Vector3d a_inertial s.rot.matrix() * (in.acc - s.ba); // 加速度转到世界系 for(int i0; i3; i) { res(i) s.vel[i]; // 速度 res(i3) omega[i]; // 角速度 res(i12) a_inertial[i] s.grav[i]; // 加速度重力 } return res; }关键点解析第1-3维线速度v第4-6维角速度ω减去陀螺仪偏差bg第13-15维加速度在世界坐标系的投影加上重力2.2 离散时间积分在实际实现中状态预测采用前向欧拉积分x_{k1} x_k ⊞ (Δt·f(x_k,u_k))其中⊞为广义加法运算后文详述Δt为IMU采样间隔。3. 雅可比矩阵的计算与实现雅可比矩阵在误差状态传播中起关键作用直接影响卡尔曼滤波的精度。3.1 状态雅可比矩阵Fx数学上Fx ∂f/∂x描述了状态转移函数对状态的敏感性。FAST-LIO中对应的公式为Fx [Exp(-ω̂Δt) 0 0 -A(ω̂Δt)ᵀΔt 0 0 0 I IΔt 0 0 0 -GR̂[â]∧Δt 0 I 0 -GR̂Δt IΔt 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I]C实现通过df_dx函数计算Eigen::Matrixdouble,24,24 df_dx(state_ikfom s, input_ikfom in) { Eigen::Matrixdouble,24,24 cov Eigen::Matrixdouble,24,24::Zero(); cov.block3,3(0,12) Eigen::Matrix3d::Identity(); // ∂v/∂v Eigen::Vector3d acc_ in.acc - s.ba; cov.block3,3(12,3) -s.rot.matrix() * Sophus::SO3::hat(acc_); // ∂a/∂R cov.block3,3(12,18) -s.rot.matrix(); // ∂a/∂ba cov.block3,3(12,21) Eigen::Matrix3d::Identity(); // ∂a/∂g cov.block3,3(3,15) -Eigen::Matrix3d::Identity(); // ∂ω/∂bg return cov; }关键块解析cov.block3,3(0,12)速度对速度的导数单位矩阵cov.block3,3(12,3)加速度对旋转的导数涉及叉乘矩阵cov.block3,3(12,18)加速度对加速度偏差的导数3.2 噪声雅可比矩阵FwFw ∂f/∂w描述了状态转移对过程噪声的敏感性Fw [-A(ω̂Δt)ᵀΔt 0 0 0 0 0 0 0 0 0 -GR̂Δt 0 0 0 0 IΔt 0 0 0 0 0 0 0 0]C实现Eigen::Matrixdouble,24,12 df_dw(state_ikfom s, input_ikfom in) { Eigen::Matrixdouble,24,12 cov Eigen::Matrixdouble,24,12::Zero(); cov.block3,3(12,3) -s.rot.matrix(); // ∂a/∂wa cov.block3,3(3,0) -Eigen::Matrix3d::Identity(); // ∂ω/∂wω cov.block3,3(15,6) Eigen::Matrix3d::Identity(); // ∂bg/∂wbg cov.block3,3(18,9) Eigen::Matrix3d::Identity(); // ∂ba/∂wba return cov; }4. 流形上的状态更新FAST-LIO采用流形上的广义加法(⊞)和减法(⊟)来处理SO(3)旋转群的特殊性质。4.1 广义加法实现广义加法boxplus实现了状态增量的更新state_ikfom boxplus(state_ikfom x, Eigen::Matrixdouble,24,1 f_) { state_ikfom x_r; x_r.pos x.pos f_.block3,1(0,0); x_r.rot x.rot * Sophus::SO3::exp(f_.block3,1(3,0)); x_r.offset_R_L_I x.offset_R_L_I * Sophus::SO3::exp(f_.block3,1(6,0)); x_r.offset_T_L_I x.offset_T_L_I f_.block3,1(9,0); x_r.vel x.vel f_.block3,1(12,0); x_r.bg x.bg f_.block3,1(15,0); x_r.ba x.ba f_.block3,1(18,0); x_r.grav x.grav f_.block3,1(21,0); return x_r; }数学上对于旋转部分的更新采用指数映射 R R·exp(ξ) 其中ξ∈so(3)为李代数元素4.2 广义减法实现广义减法boxminus用于计算状态差异vectorized_state boxminus(state_ikfom x1, state_ikfom x2) { vectorized_state x_r vectorized_state::Zero(); x_r.block3,1(0,0) x1.pos - x2.pos; x_r.block3,1(3,0) Sophus::SO3(x2.rot.matrix().transpose() * x1.rot.matrix()).log(); // 其他分量类似... return x_r; }旋转差异通过李代数对数映射计算 ξ log(R₂ᵀR₁)5. 前向传播与协方差预测完整的前向传播过程包括状态预测和协方差矩阵更新。5.1 预测步骤实现predict函数实现了完整的预测流程void predict(double dt, Eigen::Matrixdouble,12,12 Q, const input_ikfom i_in) { Eigen::Matrixdouble,24,1 f_ get_f(x_, i_in); // 状态转移 Eigen::Matrixdouble,24,24 f_x_ df_dx(x_, i_in); // 状态雅可比 Eigen::Matrixdouble,24,12 f_w_ df_dw(x_, i_in); // 噪声雅可比 x_ boxplus(x_, f_ * dt); // 状态预测 f_x_ Matrixdouble,24,24::Identity() f_x_ * dt; // 离散化 P_ (f_x_)*P_*(f_x_).transpose() (dt*f_w_)*Q*(dt*f_w_).transpose(); // 协方差预测 }5.2 协方差传播分析协方差传播公式为 P_{k1} Fx·P_k·Fxᵀ Fw·Q·Fwᵀ其中Fx I ∂f/∂x·Δt 离散化后的状态转移矩阵Fw ∂f/∂w·Δt 离散化后的噪声转移矩阵Q 为过程噪声协方差矩阵在实际部署中矩阵运算的数值稳定性是关键考量。FAST-LIO通过精心设计的矩阵块操作保证了计算效率。