4、IMU运动学模型：离散时间下的IMU运动学方程、四元数更新、位置与速度更新

好，咱们进入正题。IMU的测量值——加速度和角速度——都是连续的物理量。但我们的算法跑在计算机上，计算机只认离散的时间点。所以，必须把连续的运动学方程，改写成离散形式。

说白了，就是要把「微分」变成「差分」。我刚开始做VIO的时候，觉得这一步不就是套个欧拉法嘛，有啥难的？结果真跑起来，发现姿态发散得厉害。嗯，这里面的门道，咱们得仔细捋一捋。

4.1 从连续到离散：核心思想

连续时间下，IMU的状态由下面几个微分方程描述：

四元数导数：    q̇ = 0.5 * q ⊗ ω
速度导数：      v̇ = R * a + g
位置导数：      ṗ = v

其中ω是角速度，a是加速度（都是IMU坐标系下的测量值）。R是旋转矩阵，由q转换而来。

离散化，就是给定一个时间间隔Δt（比如IMU的采样周期10ms），用上一时刻的状态，推算下一时刻的状态。你想想看，这个过程其实就是在做数值积分。

4.2 四元数更新：别再犯我当年的错

四元数更新，是IMU运动学里最容易被忽视的坑。我见过不少新手，直接套用欧拉法：

q_{k+1} = q_k + 0.5 * q_k ⊗ ω_k * Δt

这样做，四元数会很快失去归一化约束。你想想，四元数代表旋转，模长必须是1。一旦偏离，整个旋转就扭曲了。

正确的做法，是利用角速度的「积分」来构造一个旋转增量。我个人习惯用下面的方法：

// 假设角速度 ω_k 在 Δt 内恒定
// 1. 计算角速度的模长
double theta = norm(ω_k) * Δt;

// 2. 如果角速度接近零，直接跳过更新
if (theta < 1e-12) {
    q_{k+1} = q_k;
} else {
    // 3. 构造旋转四元数
    Eigen::Vector3d axis = ω_k / norm(ω_k);
    Eigen::Quaterniond dq(cos(theta/2), 
                          axis.x()*sin(theta/2),
                          axis.y()*sin(theta/2),
                          axis.z()*sin(theta/2));
    // 4. 更新
    q_{k+1} = q_k * dq;
}

这里有个关键假设：角速度在Δt内恒定。实际上IMU采样率够高（200Hz以上），这个假设基本成立。我曾经在低端IMU上试过50Hz采样，结果姿态发散得一塌糊涂。所以，采样率是硬门槛。

4.3 位置与速度更新：小心重力

位置和速度的更新，相对直观一些。但有一个细节：加速度测量值是在IMU坐标系下的，必须旋转到世界坐标系才能积分。

离散形式如下：

// 速度更新
v_{k+1} = v_k + (R_k * a_k + g) * Δt

// 位置更新
p_{k+1} = p_k + v_k * Δt + 0.5 * (R_k * a_k + g) * Δt²

注意这里的R_k，是用上一时刻的姿态q_k构造的旋转矩阵。为什么不用更新后的q_{k+1}？因为位置和速度更新，通常和姿态更新是「串行」的——先更新姿态，再用旧姿态更新位置速度。这样做，在Δt足够小时，误差可以忽略。

但如果你追求极致精度，可以用「并行更新」：先计算中间时刻的姿态，再用它来更新位置速度。嗯，这个我一般在做高精度后端的优化时才用，前端实时跑的话，串行就够了。

4.4 完整的离散时间运动学方程

把上面三块拼起来，就是完整的离散时间IMU运动学模型：

// 输入：上一时刻状态 (q_k, v_k, p_k)，IMU测量值 (ω_k, a_k)，时间间隔 Δt
// 输出：当前时刻状态 (q_{k+1}, v_{k+1}, p_{k+1})

// 1. 四元数更新
double theta = norm(ω_k) * Δt;
if (theta > 1e-12) {
    Eigen::Vector3d axis = ω_k / norm(ω_k);
    Eigen::Quaterniond dq(cos(theta/2), 
                          axis.x()*sin(theta/2),
                          axis.y()*sin(theta/2),
                          axis.z()*sin(theta/2));
    q_{k+1} = q_k * dq;
} else {
    q_{k+1} = q_k;
}

// 2. 速度更新
Eigen::Matrix3d R_k = q_k.toRotationMatrix();
v_{k+1} = v_k + (R_k * a_k + g) * Δt;

// 3. 位置更新
p_{k+1} = p_k + v_k * Δt + 0.5 * (R_k * a_k + g) * Δt * Δt;

这个模型，是VIO前端中IMU预积分的「地基」。你想想看，如果没有这个离散模型，我们根本没法把IMU数据塞进优化框架里。

4.5 知识体系一览

为了让你更直观地理解这一章的核心逻辑，我画了一张流程图：

这张图把离散时间IMU运动学模型的「三步走」讲清楚了。你照着这个流程写代码，基本不会跑偏。

4.6 一些心里话

说实话，IMU运动学模型是整个VIO系统里「最物理」的部分。它不像后端优化那样充满矩阵运算，也不像前端特征匹配那样依赖图像。它就是纯粹的刚体运动学——你给它正确的输入，它就给你正确的输出。

我见过太多人，一上来就调后端优化参数，结果发现前端IMU积分都飘了。其实，先把离散运动学方程写对、测准，VIO系统就成功了一半。剩下的，无非是噪声建模和状态估计的问题。

嗯，这一章的内容就到这里。记住：离散化不是简单的「抄公式」，而是要理解每个假设背后的物理意义。下次你写IMU预积分代码的时候，不妨多想想——这个Δt内，角速度真的恒定吗？

---