1. IMU丈量值积分
IMU数据包含线加速度 a imu \mathbf{a}_{\text{imu}} aimu 和角速度 ω imu \boldsymbol{\omega}_{\text{imu}} ωimu，函数通过GTSAM库的 integrateMeasurement 方法对IMU丈量值进行积分，公式为：
v k + 1 = v k + ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t , p k + 1 = p k + v k ⋅ Δ t + 1 2 ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t 2 , R k + 1 = R k ⋅ exp ( ( ω imu − b ω ) ⋅ Δ t ) , \begin{aligned} \mathbf{v}_{k+1} &= \mathbf{v}_k + (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t, \\ \mathbf{p}_{k+1} &= \mathbf{p}_k + \mathbf{v}_k \cdot \Delta t + \frac{1}{2} (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t^2, \\ \mathbf{R}_{k+1} &= \mathbf{R}_k \cdot \exp((\boldsymbol{\omega}_{\text{imu}} - \mathbf{b}_\omega) \cdot \Delta t), \end{aligned} vk+1pk+1Rk+1=vk+(Rk⋅(aimu−ba)+g)⋅Δt,=pk+vk⋅Δt+21(Rk⋅(aimu−ba)+g)⋅Δt2,=Rk⋅exp((ωimu−bω)⋅Δt),
此中：
v k \mathbf{v}_k vk 和 p k \mathbf{p}_k pk 分别为速度和位置；
R k \mathbf{R}_k Rk 为姿态的旋转矩阵；
b a \mathbf{b}_a ba 和 b ω \mathbf{b}_\omega bω 分别为加速度和角速度的偏置；
g \mathbf{g} g 为重力加速度；
Δ t \Delta t Δt 为时间隔断。
2. 位姿预测
通过上述积分盘算，预测得到当前状态：
State k + 1 = [ p k + 1 v k + 1 R k + 1 ] . \text{State}_{k+1} = \begin{bmatrix} \mathbf{p}_{k+1} \\ \mathbf{v}_{k+1} \\ \mathbf{R}_{k+1} \end{bmatrix}. Statek+1= pk+1vk+1Rk+1 .
函数中调用 imuIntegratorImu_->predict 实现此预测。 3. 位姿转换（IMU到激光雷达坐标系）
通过以下公式将IMU的位姿 T imu \mathbf{T}_{\text{imu}} Timu 转换为激光雷达位姿 T lidar \mathbf{T}_{\text{lidar}} Tlidar：
T lidar = T imu ⋅ T imu2lidar , \mathbf{T}_{\text{lidar}} = \mathbf{T}_{\text{imu}} \cdot \mathbf{T}_{\text{imu2lidar}}, Tlidar=Timu⋅Timu2lidar,
此中 T imu2lidar \mathbf{T}_{\text{imu2lidar}} Timu2lidar 是IMU到激光雷达的静态外参。
对于位姿 T \mathbf{T} T，其包含旋转 R \mathbf{R} R 宁静移 t \mathbf{t} t，即：
T = [ R t 0 1 ] . \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}. T=[R0t1]. 4. 惯导里程计发布
最终通过以下公式将预测得到的激光雷达位姿和速度发布为惯导里程计数据：
位置：
p odom = T lidar . translation() . \mathbf{p}_{\text{odom}} = \mathbf{T}_{\text{lidar}}.\text{translation()}. podom=Tlidar.translation().
3. IMU积分与因子添加
IMU预积分，将IMU丈量值 a imu , ω imu \mathbf{a}_{\text{imu}}, \boldsymbol{\omega}_{\text{imu}} aimu,ωimu 进行预积分，得到状态更新：
v k + 1 = v k + ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t , p k + 1 = p k + v k ⋅ Δ t + 1 2 ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t 2 , R k + 1 = R k ⋅ exp ( ( ω imu − b ω ) ⋅ Δ t ) . \begin{aligned} \mathbf{v}_{k+1} &= \mathbf{v}_k + (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t, \\ \mathbf{p}_{k+1} &= \mathbf{p}_k + \mathbf{v}_k \cdot \Delta t + \frac{1}{2} (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t^2, \\ \mathbf{R}_{k+1} &= \mathbf{R}_k \cdot \exp((\boldsymbol{\omega}_{\text{imu}} - \mathbf{b}_\omega) \cdot \Delta t). \end{aligned} vk+1pk+1Rk+1=vk+(Rk⋅(aimu−ba)+g)⋅Δt,=pk+vk⋅Δt+21(Rk⋅(aimu−ba)+g)⋅Δt2,=Rk⋅exp((ωimu−bω)⋅Δt).
基于预积分丈量值构建：
ImuFactor ( T k , v k , T k + 1 , v k + 1 , b k , PreintegratedIMU ) . \text{ImuFactor}(\mathbf{T}_{k}, \mathbf{v}_k, \mathbf{T}_{k+1}, \mathbf{v}_{k+1}, \mathbf{b}_k, \text{PreintegratedIMU}). ImuFactor(Tk,vk,Tk+1,vk+1,bk,PreintegratedIMU).
4. 里程计因子添加与优化
位姿因子，激光里程计位姿转换为IMU位姿后，构造先验因子：
PriorFactor ( T k + 1 , T cur , Q pose ) , \text{PriorFactor}(\mathbf{T}_{k+1}, \mathbf{T}_{\text{cur}}, \mathbf{Q}_{\text{pose}}), PriorFactor(Tk+1,Tcur,Qpose),
此中 T cur \mathbf{T}_{\text{cur}} Tcur 为当前里程计位姿。
优化求解，构建优化问题：
x ∗ = arg min x ∑ Factors . \mathbf{x}^* = \arg\min_{\mathbf{x}} \sum \text{Factors}. x∗=argxmin∑Factors.
调用优化器 optimizer.update 进行求解。
// 优化
optimizer.update(graphFactors, graphValues);
optimizer.update();
graphFactors.resize(0);
graphValues.clear();
复制代码
5. 优化后更新状态
优化完成后，从效果中提取最新状态：
T k + 1 = result ( X k + 1 ) , v k + 1 = result ( V k + 1 ) , b k + 1 = result ( B k + 1 ) . \begin{aligned} \mathbf{T}_{k+1} &= \text{result}(X_{k+1}), \\ \mathbf{v}_{k+1} &= \text{result}(V_{k+1}), \\ \mathbf{b}_{k+1} &= \text{result}(B_{k+1}). \end{aligned} Tk+1vk+1bk+1=result(Xk+1),=result(Vk+1),=result(Bk+1).
并重置IMU预积分器：
imuIntegratorOpt − > resetIntegrationAndSetBias ( b k + 1 ) . \text{imuIntegratorOpt}->\text{resetIntegrationAndSetBias}(\mathbf{b}_{k+1}). imuIntegratorOpt−>resetIntegrationAndSetBias(bk+1).
// 为下一步重置预积分
gtsam::Values result = optimizer.calculateEstimate();
6.重新传播
在重新传播之前，需要移除时间戳早于当前校正时间 t currentCorrectionTime − Δ t t_{\text{currentCorrectionTime}} - \Delta t tcurrentCorrectionTime−Δt 的IMU消息：
t imu < t currentCorrectionTime − Δ t . t_{\text{imu}} < t_{\text{currentCorrectionTime}} - \Delta t. timu<tcurrentCorrectionTime−Δt.
通过循环移除队列中满足上述条件的IMU消息，并记录最后一条被移除的IMU时间戳 t lastImuQT t_{\text{lastImuQT}} tlastImuQT：
t lastImuQT = t imu . t_{\text{lastImuQT}} = t_{\text{imu}}. tlastImuQT=timu.
// 首先弹出比当前校正数据旧的IMU消息
double lastImuQT = -1;
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
复制代码
重新传播IMU
在IMU重新传播前，利用最新优化得到的偏置 b odom \mathbf{b}_{\text{odom}} bodom 对IMU积分器重置：
对于队列中剩余的IMU消息，从 t lastImuQT t_{\text{lastImuQT}} tlastImuQT 开始，逐条进行积分：
v k + 1 = v k + ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t , p k + 1 = p k + v k ⋅ Δ t + 1 2 ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t 2 , R k + 1 = R k ⋅ exp ( ( ω imu − b ω ) ⋅ Δ t ) . \begin{aligned} \mathbf{v}_{k+1} &= \mathbf{v}_k + (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t, \\ \mathbf{p}_{k+1} &= \mathbf{p}_k + \mathbf{v}_k \cdot \Delta t + \frac{1}{2} (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t^2, \\ \mathbf{R}_{k+1} &= \mathbf{R}_k \cdot \exp((\boldsymbol{\omega}_{\text{imu}} - \mathbf{b}_\omega) \cdot \Delta t). \end{aligned} vk+1pk+1Rk+1=vk+(Rk⋅(aimu−ba)+g)⋅Δt,=pk+vk⋅Δt+21(Rk⋅(aimu−ba)+g)⋅Δt2,=Rk⋅exp((ωimu−bω)⋅Δt).
此中：
v k \mathbf{v}_k vk 和 p k \mathbf{p}_k pk 分别为速度和位置；
R k \mathbf{R}_k Rk 为姿态旋转矩阵；
a imu \mathbf{a}_{\text{imu}} aimu 和 ω imu \boldsymbol{\omega}_{\text{imu}} ωimu 分别为IMU的线加速度和角速度；
b a \mathbf{b}_a ba 和 b ω \mathbf{b}_\omega bω 分别为加速度和角速度的偏置；
Δ t \Delta t Δt 为时间隔断：
Δ t = { 1 500 , 若 t lastImuQT < 0 ; t imu − t lastImuQT , 否则 . \Delta t = \begin{cases} \frac{1}{500}, & \text{若 } t_{\text{lastImuQT}} < 0; \\ t_{\text{imu}} - t_{\text{lastImuQT}}, & \text{否则}. \end{cases} Δt={5001,timu−tlastImuQT,若 tlastImuQT<0;否则.