IMU 离散状态转移模型直观解释

代码校准（2026-08-01）：状态姿态 rot_end/R_wi 表示 IMU→World，采用右乘扰动；正文把 R_wb 写成 World→Body 与后续乘法矛盾时，以 R_wi 为准。IMU_init() 不估计陀螺 bias，而是置零；MAX_INI_COUNT 按 IMU 样本计数；初始协方差不是所有维度统一 0.01；第 11.3 节摘录的是无 IMU Forward_without_imu() 近似，不能代表正常 UndistortPcl() 的 Pose6D 反向去畸变。详见 00-代码基线审查报告.md。

1. 引言

IMU（惯性测量单元）是 FASTLIVO2 系统的核心传感器，提供高频的角速度和线性加速度计测量。理解 IMU 的状态转移模型对于理解整个系统至关重要。

本章将：

从物理直观角度解释 IMU 运动学
推导离散时间下的状态转移方程
解释协方差传播机制
展示代码实现细节

2. IMU 测量模型

2.1 物理测量

IMU 传感器输出两类测量：

测量类型	物理量	符号	单位
角速度	陀螺仪测量	ω_m	rad/s
线性加速度计	加速度计计测量	a_m	m/s²

2.2 真实值与测量值关系

真实值（理想情况）：

ω_true = 机体绕世界坐标系的旋转角速度
a_true = 机体受到的真实加速度计（包括重力和外力）

测量值（含噪声和偏差）：

ω_m = ω_true + b_g + n_g    (角速度测量)
a_m = a_true + b_a + n_a    (加速度计测量)

其中：

b_g: 陀螺仪偏差（缓慢变化的漂移）
b_a: 加速度计计偏差
n_g, n_a: 测量噪声（高斯白噪声）

3. 连续时间运动学方程

3.1 机体坐标系 vs 世界坐标系

世界坐标系 (World Frame, w)
    ↓ (旋转 R_wb)
机体坐标系 (Body Frame, b)

符号约定：

R_wb: 从世界系到机体系的旋转矩阵
p_w: 机体在世界系中的位置
v_w: 机体在世界系中的速度

3.2 姿态更新方程

角速度关系：

ω_wb_b = R_wb^T · ω_wb_w

其中：

ω_wb_b: 机体系中，机体绕世界系的角速度（陀螺仪测量的就是这个）
ω_wb_w: 世界系中，机体绕世界系的角速度

旋转矩阵微分方程：

d(R_wb)/dt = R_wb · [ω_wb_b]×

其中 [ω]× 是角速度的反对称矩阵（叉乘矩阵）：

[ω]× = [  0    -ω_z   ω_y ]
       [  ω_z    0    -ω_x ]
       [ -ω_y   ω_x    0  ]

3.3 速度更新方程

速度微分方程：

dv_w/dt = R_wb · a_true + g

其中：

a_true: 机体系中的真实加速度计（去除重力）
g: 世界系中的重力向量

物理直观：

R_wb · a_true: 将机体系加速度计转换到世界系
g: 重力加速度计（向下）

3.4 位置更新方程

位置微分方程：

dp_w/dt = v_w

3.5 偏差模型

偏差通常建模为随机游走（随机常数）：

db_g/dt = n_bg    (陀螺仪偏差漂移)
db_a/dt = n_ba    (加速度计计偏差漂移)

其中 n_bg, n_ba 是零均值高斯白噪声。

4. 连续时间状态空间方程

4.1 状态向量定义

FASTLIVO2 中的 19 维状态向量：

x = [
  θ   (3)  - 旋转（用旋转矩阵或四元数表示）
  p   (3)  - 位置（世界系）
  τ   (1)  - 曝光时间倒数（视觉模块使用）
  v   (3)  - 速度（世界系）
  b_g (3)  - 陀螺仪偏差
  b_a (3)  - 加速度计计偏差
  g   (3)  - 重力向量
]

4.2 连续时间状态方程

dθ/dt   = R_wb · (ω_m - b_g - n_g)            (旋转更新)
dp/dt    = v                                    (位置更新)
dτ/dt    = 0                                    (曝光时间常数)
dv/dt    = R_wb · (a_m - b_a - n_a) + g       (速度更新)
db_g/dt  = n_bg                                 (陀螺仪偏差)
db_a/dt  = n_ba                                 (加速度计计偏差)
dg/dt    = 0                                    (重力常数)

写成矩阵形式：

dx/dt = f(x, u) + G·n

其中：

x: 状态向量
u = [ω_m, a_m]: IMU 测量
n: 过程噪声
G: 噪声映射矩阵

5. 离散化：从连续到离散

5.1 为什么需要离散化？

IMU 测量是离散的（例如 200 Hz）
计算机只能处理离散时间
需要将连续方程离散化用于滤波器更新

5.2 离散时间间隔

假设 IMU 采样间隔为 Δt（例如 5 ms，对应 200 Hz）。

5.3 一阶欧拉离散化

旋转（简化版，假设 ω 在 Δt 内不变）：

θ_{k+1} = θ_k + R_wb_k · (ω_m - b_g) · Δt

位置：

p_{k+1} = p_k + v_k · Δt

速度：

v_{k+1} = v_k + (R_wb_k · (a_m - b_a) + g) · Δt

5.4 精确离散化（SO(3) 指数映射）

旋转（使用指数映射）：

R_{k+1} = R_k · Exp[(ω_m - b_g - n_g) · Δt]

其中 Exp[·] 是 SO(3) 的指数映射（罗德里格斯公式）。

指数映射公式（Rodrigues' Rotation Formula）：

Exp(θ · u) = I + sin(θ) · [u]× + (1 - cos(θ)) · [u]×²

其中：

θ = |ω| · Δt: 旋转角度
u = ω / |ω|: 旋转轴单位向量

速度和位置（假设加速度计在 Δt 内不变）：

v_{k+1} = v_k + (R_k · a_true + g) · Δt
p_{k+1} = p_k + v_k · Δt + 0.5 · (R_k · a_true + g) · Δt²

6. FASTLIVO2 中的离散状态转移方程

6.1 代码实现

从 IMU_Processing.cpp 中提取的核心代码：

// IMU 状态传播（简化版）
M3D Exp_f = Exp(angvel_avr, dt);  // 旋转指数映射

// 状态传播
R_imu = R_imu * Exp_f;             // 旋转更新
acc_imu = R_imu * acc_avr + gravity;  // 加速度计转换到世界系
pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt;  // 位置更新
vel_imu = vel_imu + acc_imu * dt;   // 速度更新

6.2 状态转移矩阵 F_x

雅可比矩阵 F_x = ∂f/∂x：

        ┌─────────────────────────────────────────────────────┐
        │  ∂δθ/∂δθ   ∂δθ/∂δp   ∂δθ/∂δτ   ∂δθ/∂δv   ...  │
F_x =   │  ∂δp/∂δθ   ∂δp/∂δp   ∂δp/∂δτ   ∂δp/∂δv   ...  │
        │  ∂δτ/∂δθ   ∂δτ/∂δp   ∂δδτ/∂τ  ∂δτ/∂δv   ...  │
        │  ...                                         │
        └─────────────────────────────────────────────────────┘

具体形式（FASTLIVO2 实现）：

F_x.setIdentity();
F_x.block<3, 3>(0, 0) = Exp(angvel_avr, -dt);  // ∂δθ/∂δθ
F_x.block<3, 3>(0, 10) = -Eye3d * dt;           // ∂δθ/∂b_g
F_x.block<3, 3>(3, 7) = Eye3d * dt;             // ∂δp/∂v
F_x.block<3, 3>(7, 0) = -R_imu * acc_avr_skew * dt;  // ∂v/∂δθ
F_x.block<3, 3>(7, 13) = -R_imu * dt;           // ∂v/∂b_a
F_x.block<3, 3>(7, 16) = Eye3d * dt;            // ∂v/∂g

直观解释：

∂δθ/∂δθ: 旋转误差对旋转误差的影响（旋转本身）
∂δθ/∂b_g: 陀螺仪偏差对旋转的影响（偏差越大，旋转误差越大）
∂δp/∂v: 速度对位置的影响（速度积分成位置）
∂v/∂δθ: 姿态误差对速度的影响（姿态误差会改变加速度计投影方向）
∂v/∂b_a: 加速度计计偏差对速度的影响（偏差直接影响加速度计）
∂v/∂g: 重力对速度的影响（重力直接加速）

6.3 过程噪声协方差矩阵 Q

噪声模型：

n = [n_g, n_a, n_bg, n_ba, n_τ]^T

离散噪声协方差：

cov_w.setZero();

// 陀螺仪噪声（旋转）
cov_w.block<3, 3>(0, 0).diagonal() = cov_gyr * dt * dt;

// 加速度计计噪声（速度）
cov_w.block<3, 3>(7, 7) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt;

// 陀螺仪偏差漂移
cov_w.block<3, 3>(10, 10).diagonal() = cov_bias_gyr * dt * dt;

// 加速度计计偏差漂移
cov_w.block<3, 3>(13, 13).diagonal() = cov_bias_acc * dt * dt;

// 曝光时间噪声（视觉模块）
if (exposure_estimate_en) cov_w(6, 6) = cov_inv_expo * dt * dt;

物理直观：

旋转噪声随时间平方增长（dt²）
速度噪声随时间平方增长（加速度计积分）
偏差漂移随时间平方增长（随机游走）
噪声需从机体系转换到世界系（R_imu * cov_acc * R_imu^T）

7. 协方差传播方程

7.1 为什么需要协方差传播？

估计状态的不确定性
卡尔曼滤波器的核心
平衡测量更新的权重

7.2 连续时间协方差方程

李雅普诺夫方程：

dP/dt = F · P + P · F^T + G · Q · G^T

其中：

P: 状态协方差矩阵
F: 状态转移矩阵的雅可比
Q: 过程噪声协方差
G: 噪声映射矩阵

7.3 离散时间协方差传播

离散李雅普诺夫方程：

P_{k+1} = F_x · P_k · F_x^T + Q

代码实现：

state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w;

7.4 协方差传播的物理意义

不确定性演化：

初始不确定性 ──[F_x·P·F_x^T]──> 传播的不确定性
                            +
                            ──[Q]──> 过程噪声增加的不确定性
                            =
                        总不确定性（下一时刻）

直观解释：

F_x · P · F_x^T: 将当前不确定性通过状态转移传播
Q: 加上过程噪声带来的新不确定性
协方差会随时间增长（不确定性累积）

8. 状态误差 vs. 名义状态

8.1 误差状态卡尔曼滤波（ESKF）

FASTLIVO2 使用误差状态卡尔曼滤波，而非标准卡尔曼滤波。

区别：

标准 KF: 估计状态本身（如旋转矩阵）
ESKF: 估计状态误差（如旋转误差 δθ）

优势：

旋转误差是小量（线性化更准确）
误差空间是线性的（避免非线性）
数值更稳定

8.2 状态表示

名义状态（非线性）：

θ_nominal (旋转矩阵或四元数)
p_nominal (位置)
v_nominal (速度)
...

误差状态（线性）：

δθ (旋转误差，李代数表示)
δp (位置误差)
δv (速度误差)
...

总状态：

θ = θ_nominal ⊕ δθ    (李群李代数运算)
p = p_nominal + δp
v = v_nominal + δv
...

8.3 误差状态更新

测量更新后：

// 更新误差状态
δx = K · (z - h(x_nominal))

// 更新名义状态
θ_nominal = θ_nominal ⊕ δθ
p_nominal = p_nominal + δp
...

// 重置误差状态
δx = 0

代码实现（从 common_lib.h）：

StatesGroup operator+(const Matrix<double, DIM_STATE, 1> &state_add)
{
  StatesGroup a;
  a.rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0));
  a.pos_end = this->pos_end + state_add.block<3, 1>(3, 0);
  a.inv_expo_time = this->inv_expo_time + state_add(6, 0);
  a.vel_end = this->vel_end + state_add.block<3, 1>(7, 0);
  a.bias_g = this->bias_g + state_add.block<3, 1>(10, 0);
  a.bias_a = this->bias_a + state_add.block<3, 1>(13, 0);
  a.gravity = this->gravity + state_add.block<3, 1>(16, 0);
  return a;
};

9. IMU 初始化

9.1 静态初始化

目的：

估计重力方向
估计初始陀螺仪偏差

假设：

机器人静止
只有重力作用于加速度计计

代码实现（从 IMU_Processing.cpp）：

void ImuProcess::IMU_init(const MeasureGroup &meas, StatesGroup &state_inout, int &N)
{
  // 1. 计算平均加速度计
  for (const auto &imu : meas.imu)
  {
    cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
    mean_acc += (cur_acc - mean_acc) / N;
    N++;
  }

  // 2. 估计重力方向（加速度计平均值的负方向）
  state_inout.gravity = -mean_acc / mean_acc.norm() * G_m_s2;

  // 3. 初始姿态设为单位矩阵（世界系与机体系对齐）
  state_inout.rot_end = Eye3d;

  // 4. 初始陀螺仪偏差设为 0
  state_inout.bias_g = Zero3d;
}

9.2 初始化参数

参数	默认值	说明
`MAX_INI_COUNT`	20	初始化所需的 IMU 帧数
`INIT_COV`	0.01	初始协方差
`G_m_s2`	9.81	重力加速度计（中国广东）

10. 完整离散状态转移流程

10.1 流程图

输入: 状态 x_k, 协方差 P_k, IMU 测量 u_k

Step 1: 去偏差
┌─────────────────────────────────────────┐
│ ω_true = ω_m - b_g                     │
│ a_true = a_m - b_a                     │
└─────────────────────────────────────────┘

Step 2: 计算状态转移矩阵 F_x
┌─────────────────────────────────────────┐
│ F_x(0:3, 0:3)   = Exp(-ω_true · Δt)  │
│ F_x(0:3, 10:13)  = -I · Δt            │
│ F_x(3:6, 7:10)   = I · Δt             │
│ F_x(7:10, 0:3)   = -R · [a_true]× · Δt│
│ F_x(7:10, 13:16) = -R · Δt            │
│ F_x(7:10, 16:19) = I · Δt             │
└─────────────────────────────────────────┘

Step 3: 计算噪声协方差 Q
┌─────────────────────────────────────────┐
│ Q(0:3, 0:3)    = σ_g² · Δt²          │
│ Q(7:10, 7:10)  = R · σ_a² · R^T · Δt² │
│ Q(10:13, 10:13) = σ_bg² · Δt²        │
│ Q(13:16, 13:16) = σ_ba² · Δt²        │
│ Q(6, 6)        = σ_τ² · Δt² (可选)   │
└─────────────────────────────────────────┘

Step 4: 状态传播
┌─────────────────────────────────────────┐
│ R_{k+1} = R_k · Exp(ω_true · Δt)    │
│ p_{k+1} = p_k + v_k · Δt             │
│ v_{k+1} = v_k + (R_k · a_true + g) · Δt │
└─────────────────────────────────────────┘

Step 5: 协方差传播
┌─────────────────────────────────────────┐
│ P_{k+1} = F_x · P_k · F_x^T + Q     │
└─────────────────────────────────────────┘

输出: 状态 x_{k+1}, 协方差 P_{k+1}

10.2 代码实现细节

从 IMU_Processing.cpp 提取的完整传播代码：

// 1. 计算平均角速度和加速度计
angvel_avr = 0.5 * (head->angular_velocity + tail->angular_velocity);
acc_avr = 0.5 * (head->linear_acceleration + tail->linear_acceleration);

// 2. 去偏差
angvel_avr -= state_inout.bias_g;
acc_avr = acc_avr * G_m_s2 / mean_acc.norm() - state_inout.bias_a;

// 3. 计算时间间隔
dt = tail->header.stamp.toSec() - head->header.stamp.toSec();

// 4. 计算状态转移矩阵
M3D Exp_f = Exp(angvel_avr, dt);
M3D acc_avr_skew;
acc_avr_skew << SKEW_SYM_MATRX(acc_avr);

F_x.setIdentity();
F_x.block<3, 3>(0, 0) = Exp(angvel_avr, -dt);
if (ba_bg_est_en) F_x.block<3, 3>(0, 10) = -Eye3d * dt;
F_x.block<3, 3>(3, 7) = Eye3d * dt;
F_x.block<3, 3>(7, 0) = -R_imu * acc_avr_skew * dt;
if (ba_bg_est_en) F_x.block<3, 3>(7, 13) = -R_imu * dt;
if (gravity_est_en) F_x.block<3, 3>(7, 16) = Eye3d * dt;

// 5. 计算噪声协方差
cov_w.setZero();
cov_w.block<3, 3>(0, 0).diagonal() = cov_gyr * dt * dt;
cov_w.block<3, 3>(7, 7) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt;
cov_w.block<3, 3>(10, 10).diagonal() = cov_bias_gyr * dt * dt;
cov_w.block<3, 3>(13, 13).diagonal() = cov_bias_acc * dt * dt;
if (exposure_estimate_en) cov_w(6, 6) = cov_inv_expo * dt * dt;

// 6. 协方差传播
state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w;

// 7. 状态传播
R_imu = R_imu * Exp_f;  // 旋转
acc_imu = R_imu * acc_avr + state_inout.gravity;  // 世界系加速度计
pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt;  // 位置
vel_imu = vel_imu + acc_imu * dt;  // 速度

11. 实际应用中的关键问题

11.1 时间同步

问题：IMU、激光雷达、相机时间戳不一致

解决：

硬件同步（外部触发）
软件同步（时间戳对齐）
FASTLIVO2 使用 imu_time_offset, lidar_time_offset, img_time_offset 参数

11.2 坐标系转换

传感器外参：

T_li: 激光雷达到 IMU 的变换
T_ci: 相机到 IMU 的变换

坐标转换：

p_imu = R_li · p_lidar + t_li
p_camera = R_ci · p_imu + t_ci

代码实现：

// 激光雷达到 IMU
Lid_rot_to_IMU = extR;  // 旋转
Lid_offset_to_IMU = extT;  // 平移

// IMU 到相机
vio_manager->setImuToLidarExtrinsic(extT, extR);
vio_manager->setLidarToCameraExtrinsic(cameraextrinR, cameraextrinT);

11.3 去畸变

问题：激光雷达扫描时间长，期间机器人运动导致点云畸变

解决：使用 IMU 估计的运动去畸变

代码实现：

// 对每个点进行反向传播
for (auto it_pcl = pcl_wait_proc.points.end() - 1; it_pcl != pcl_wait_proc.points.begin(); it_pcl--)
{
    dt_j = pcl_end_offset_time - it_pcl->curvature / double(1000);
    M3D R_jk(Exp(state_inout.bias_g, -dt_j));
    V3D P_j(it_pcl->x, it_pcl->y, it_pcl->z);

    // 去畸变
    V3D p_jk = -state_inout.rot_end.transpose() * state_inout.vel_end * dt_j;
    V3D P_compensate = R_jk * P_j + p_jk;

    it_pcl->x = P_compensate(0);
    it_pcl->y = P_compensate(1);
    it_pcl->z = P_compensate(2);
}

12. 总结

12.1 核心要点

IMU 提供高频位姿预测：通过积分角速度和加速度计
离散化是必要的：因为 IMU 测量和计算都是离散的
状态转移矩阵 F_x：描述状态如何随时间演化
协方差传播：维护估计的不确定性
ESKF 架构：估计误差状态而非状态本身，提高线性化精度

12.2 与其他模块的关系

IMU 模块
    │
    ├── 状态传播（预测）
    │       │
    │       ├── 激光雷达模块（LIO）
    │       │       │
    │       │       ├── 点面残差
    │       │       └── 状态更新
    │       │
    │       └── 视觉模块（VIO）
    │               │
    │               ├── 光度残差
    │               └── 状态更新
    │
    └── 协方差传播（不确定性）

12.3 代码文件映射

文件	功能
`include/IMU_Processing.h`	IMU 处理类定义
`src/IMU_Processing.cpp`	IMU 状态传播、去畸变实现
`include/utils/so3_math.h`	SO(3) 指数映射、对数映射
`include/common_lib.h`	状态向量定义
`src/LIVMapper.cpp`	IMU 初始化、调用

13. 数学符号汇总

符号	含义	维度
`x`	状态向量	19×1
`P`	状态协方差矩阵	19×19
`F_x`	状态转移矩阵	19×19
`Q`	过程噪声协方差	19×19
`R_wb`	世界系到机体系的旋转矩阵	3×3
`ω_m`	IMU 角速度测量	3×1
`a_m`	IMU 加速度计测量	3×1
`b_g`	陀螺仪偏差	3×1
`b_a`	加速度计计偏差	3×1
`g`	重力向量	3×1
`n_g`, `n_a`, `n_bg`, `n_ba`	测量噪声	3×1
`Δt`	时间间隔	标量

下一章节：第 5 章 - 激光雷达点扫描重组的原因和方法

FilesExpand file tree

04-IMU离散状态转移模型直观解释.md

Latest commit

History

04-IMU离散状态转移模型直观解释.md

File metadata and controls

IMU 离散状态转移模型直观解释

1. 引言

2. IMU 测量模型

2.1 物理测量

2.2 真实值与测量值关系

3. 连续时间运动学方程

3.1 机体坐标系 vs 世界坐标系

3.2 姿态更新方程

3.3 速度更新方程

3.4 位置更新方程

3.5 偏差模型

4. 连续时间状态空间方程

4.1 状态向量定义

4.2 连续时间状态方程

5. 离散化：从连续到离散

5.1 为什么需要离散化？

5.2 离散时间间隔

5.3 一阶欧拉离散化

5.4 精确离散化（SO(3) 指数映射）

6. FASTLIVO2 中的离散状态转移方程

6.1 代码实现

6.2 状态转移矩阵 F_x

6.3 过程噪声协方差矩阵 Q

7. 协方差传播方程

7.1 为什么需要协方差传播？

7.2 连续时间协方差方程

7.3 离散时间协方差传播

7.4 协方差传播的物理意义

8. 状态误差 vs. 名义状态

8.1 误差状态卡尔曼滤波（ESKF）

8.2 状态表示

8.3 误差状态更新

9. IMU 初始化

9.1 静态初始化

9.2 初始化参数

10. 完整离散状态转移流程

10.1 流程图

10.2 代码实现细节

11. 实际应用中的关键问题

11.1 时间同步

11.2 坐标系转换

11.3 去畸变

12. 总结

12.1 核心要点

12.2 与其他模块的关系

12.3 代码文件映射

13. 数学符号汇总