从代码到实践：手把手带你理解FAST-LIO中的状态传播与雅可比计算（附C++代码逐行解析）

在机器人定位与建图领域，FAST-LIO因其高效的激光-惯性里程计融合算法而备受关注。但真正让开发者头疼的，往往是那些隐藏在数学公式背后的工程实现细节。本文将带您深入FAST-LIO的核心代码层，揭示状态传播与雅可比计算的实现奥秘。

1. FAST-LIO状态模型解析

FAST-LIO的状态变量构成了整个系统的基础框架。让我们先看代码中state_ikfom结构体的定义：

cpp复制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); // 重力向量
};

这个24维状态向量可以分解为以下几个关键部分：

• 位姿信息：位置pos和旋转rot表示机器人在全局坐标系中的位姿
• 传感器标定：offset_R_L_I和offset_T_L_I描述LiDAR与IMU之间的外参
• 运动状态：速度vel反映当前运动状态
• 传感器误差：bg和ba分别补偿陀螺仪和加速度计的测量偏差
• 环境参数：grav表示重力加速度向量

注意：在实际应用中，重力向量通常初始化为(0,0,-9.81)，但会被作为状态变量参与优化。

2. 状态转移函数的代码实现

状态转移函数f(x,u)描述了系统状态如何随时间演化。以下是get_f函数的完整实现：

cpp复制Eigen::Matrix<double, 24, 1> get_f(state_ikfom s, input_ikfom in) {
    Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
    Eigen::Vector3d omega = in.gyro - s.bg;  // 补偿偏置后的角速度
    Eigen::Vector3d a_inertial = s.rot.matrix() * (in.acc - s.ba); // 世界坐标系下的加速度
    
    for (int i = 0; i < 3; i++) {
        res(i) = s.vel[i];          // 速度分量
        res(i + 3) = omega[i];      // 角速度分量
        res(i + 12) = a_inertial[i] + s.grav[i]; // 加速度分量
    }
    return res;
}

这个函数对应着以下数学表达：

f(x,u) =
\begin{bmatrix}
v \
\omega - b_g \
0 \
0 \
R(a - b_a) + g \
0 \
0 \
0
\end

关键实现细节：

1. 角速度处理：omega = in.gyro - s.bg实现了对陀螺仪测量值的偏置补偿
2. 加速度转换：a_inertial将加速度从IMU坐标系转换到世界坐标系
3. 重力补偿：a_inertial[i] + s.grav[i]将重力加速度纳入状态方程

3. 雅可比矩阵计算详解

雅可比矩阵在误差状态传播中起着关键作用。FAST-LIO需要计算两个雅可比矩阵：∂f/∂x和∂f/∂w。

3.1 状态雅可比矩阵(df/dx)

cpp复制Eigen::Matrix<double, 24, 24> df_dx(state_ikfom s, input_ikfom in) {
    Eigen::Matrix<double, 24, 24> cov = Eigen::Matrix<double, 24, 24>::Zero();
    cov.block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();  // ∂v/∂v
    Eigen::Vector3d acc_ = in.acc - s.ba;  // 补偿偏置后的加速度
    
    cov.block<3, 3>(12, 3) = -s.rot.matrix() * Sophus::SO3::hat(acc_); // ∂a/∂θ
    cov.block<3, 3>(12, 18) = -s.rot.matrix();  // ∂a/∂ba
    cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // ∂a/∂g
    cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); // ∂ω/∂bg
    
    return cov;
}

这个实现对应论文中的公式(7)，主要包含以下几个关键块：

3.2 噪声雅可比矩阵(df/dw)

cpp复制Eigen::Matrix<double, 24, 12> df_dw(state_ikfom s, input_ikfom in) {
    Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero();
    cov.block<3, 3>(12, 3) = -s.rot.matrix();  // ∂a/∂wa
    cov.block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); // ∂ω/∂wω
    cov.block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); // ∂bg/∂wbg
    cov.block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); // ∂ba/∂wba
    return cov;
}

噪声雅可比矩阵描述了过程噪声对状态的影响：

1. 角速度噪声：直接影响角速度测量
2. 加速度噪声：通过旋转矩阵影响世界坐标系下的加速度
3. 偏置噪声：直接作用于陀螺和加速度计偏置

4. 前向传播的实现细节

前向传播predict函数整合了前述所有组件：

cpp复制void predict(double &dt, Eigen::Matrix<double, 12, 12> &Q, const input_ikfom &i_in) {
    Eigen::Matrix<double, 24, 1> f_ = get_f(x_, i_in);  // 计算f(x,u)
    Eigen::Matrix<double, 24, 24> f_x_ = df_dx(x_, i_in); // 计算Fx
    Eigen::Matrix<double, 24, 12> f_w_ = df_dw(x_, i_in); // 计算Fw
    
    x_ = boxplus(x_, f_ * dt);  // 状态传播
    f_x_ = Matrix<double, 24, 24>::Identity() + f_x_ * dt; // 离散化
    
    // 协方差传播
    P_ = (f_x_)*P_ * (f_x_).transpose() + (dt * f_w_) * Q * (dt * f_w_).transpose();
}

前向传播过程分为三个关键步骤：

1. 状态预测：使用boxplus操作实现状态更新
2. 雅可比矩阵离散化：将连续时间雅可比转换为离散形式
3. 协方差传播：根据误差状态模型更新状态协方差

提示：boxplus操作实现了流形上的状态更新，正确处理了SO(3)旋转矩阵的特殊性质。

5. 广义加法与减法的实现

FAST-LIO使用boxplus和boxminus操作来处理流形上的状态更新：

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));
    x_r.offset_R_L_I = x.offset_R_L_I * Sophus::SO3::exp(f_.block<3, 1>(6, 0));
    x_r.offset_T_L_I = x.offset_T_L_I + f_.block<3, 1>(9, 0);
    x_r.vel = x.vel + f_.block<3, 1>(12, 0);
    x_r.bg = x.bg + f_.block<3, 1>(15, 0);
    x_r.ba = x.ba + f_.block<3, 1>(18, 0);
    x_r.grav = x.grav + f_.block<3, 1>(21, 0);
    return x_r;
}

这个实现有几个值得注意的细节：

1. 旋转更新：使用SO3的指数映射处理旋转增量
2. 平移更新：简单的向量加法
3. 偏置更新：直接相加实现随机游走模型

对应的boxminus操作实现了状态差的计算：

cpp复制vectorized_state boxminus(state_ikfom x1, state_ikfom x2) {
    vectorized_state x_r = vectorized_state::Zero();
    x_r.block<3, 1>(0, 0) = x1.pos - x2.pos;
    x_r.block<3, 1>(3, 0) = Sophus::SO3(x2.rot.matrix().transpose() * x1.rot.matrix()).log();
    x_r.block<3, 1>(6, 0) = Sophus::SO3(x2.offset_R_L_I.matrix().transpose() * x1.offset_R_L_I.matrix()).log();
    x_r.block<3, 1>(9, 0) = x1.offset_T_L_I - x2.offset_T_L_I;
    x_r.block<3, 1>(12, 0) = x1.vel - x2.vel;
    x_r.block<3, 1>(15, 0) = x1.bg - x2.bg;
    x_r.block<3, 1>(18, 0) = x1.ba - x2.ba;
    x_r.block<3, 1>(21, 0) = x1.grav - x2.grav;
    return x_r;
}

关键点在于旋转差的计算使用了SO3的对数映射，这保证了旋转差值始终在切空间(so(3))中表示。