整体流程：从 IMU 样本到状态估计

知识库 › 定位仓库学习 › 核心算法 › 前端 › IMU预积分 › 整体流程：从 IMU 样本到状态估计

一句话记住：预积分是「算」，不是「估」。它把 100 个 IMU 样本压缩成一个约束；真正的估计发生在窗口优化层面，不在预积分里。

一、三层分工

层	做什么	频率	有没有优化
① 缓存层	IMU 样本入队（$\texttt{pushSample()}$）	1000 Hz	❌ 纯缓存，连加法都没有
② 预积分层	批量数值积分 + 敏感度累积 + 协方差传播	每个图像帧一次	❌ 纯计算
③ 优化层	用所有边的残差解位姿 / 速度 / 零偏	每个图像帧一次	✅ 估计只在这里

二、三条解耦的时间线

时间 ────────────────────────────────────────────────────────────►

【① IMU 样本流】1000 Hz
  ▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪
  └──── pushSample()：只往 pending 队列里塞，不算任何东西 ────┘

【② 图像帧流】10 Hz
  ●──────────────────────────────●──────────────────────────────●──
  帧k                            帧k+1                          帧k+2
                                 │
                                 └─ appendFrame() 被调用
                                    ① 从队列切出 [帧k, 帧k+1] 的 100 个样本
                                    ② new Preintegrator(锁定的零偏)  ← 零偏在这冻结
                                    ③ for 循环 100 次 push()          ← ★ 批量积分
                                    ④ 得到 pre.p / v / q / J / C / S
                                    ⑤ 这条「边」挂到窗口上

【③ 优化流】每帧一次
                                 │
                                 └─► 窗口求解
                                      用所有边的残差 → 估计【位姿 / 速度 / 零偏】
                                                          ↑
                                                    ★ 这才是「估计」！
                                                    零偏在这里才被更新
                                                          │
                                                          └─► 下一帧建区间时，
                                                              用新零偏当线性化点

容易误解的地方：「持续」的是 IMU 样本的缓存，不是积分本身。预积分是图像帧到达时一次性批量完成的，不是每来一个样本积一点。

三、逐句核对：常见的五个误解

常见说法	判定	说明
按滑动窗口有若干帧，任务是根据视觉+IMU 优化估计位姿	✅	完全正确
在两帧之间进行持续性的离散预积分	⚠️	持续的是样本缓存；积分是帧到达时批量做的
预积分和估计	❌	预积分不含任何估计
预积分实现 IMU 零漂的动态估计	❌	零偏估计在窗口优化里，预积分只是「用」它并记录敏感度
根据每次 push 进来的数据累加预积分数据，并估计偏差	❌	累加正确；但只记录对偏差的敏感度 $\mathbf J$，不估计
移测量也满足某种分布	✅	直觉很准！这就是协方差 $\mathbf C$（见第七节）
一个区间用完得到两帧之间的位姿变化，和画面信息一起参与整体状态估计	✅	这句完全正确，抓住了本质

四、预积分做的三件事（没有一件是「估计」）

void Preintegrator::push(double dt, const V3& a1, const V3& w1) {
    // ① 积分标称状态（中值法）
    p += vdt + .5a*dt*dt;
    v += a*dt;
    q  = qt.normalized();

    // ② 累积「对初始状态的敏感度」（雅可比）
    J = F*J;

    // ③ 传播「不确定性」（协方差）
    C = F*C*F.transpose() + V*effective*V.transpose();
}

做的东西	输出	是不是「估计」
① 积分	$\Delta\mathbf p,\ \Delta\mathbf v,\ \Delta\mathbf R$	❌ 纯数值计算
② 敏感度累积	$\mathbf J$（15×15）、$\mathbf S$（15×6）	❌ 纯数值计算
③ 不确定性传播	$\mathbf C$（15×15）	❌ 纯数值计算

五、估计发生在哪里

窗口求解是一个迭代循环，步骤 ④ 才是「估计」：

组装：每条边用 $\texttt{pre.p/v/q}$ 和 $\texttt{pre.J}$ 造出 15 维残差 $\mathbf r$ 和雅可比 $\mathbf J_r$
构造：$\mathbf H=\sum\mathbf J_r^\top\mathbf J_r$，$\mathbf b=\sum\mathbf J_r^\top\mathbf r$
求解：Dogleg 解出增量 $\Delta\mathcal X$（含位姿、速度、零偏）
更新：$\mathcal X\leftarrow\mathcal X+\Delta\mathcal X$ ← ★ 零偏在这里才变！
回到步骤 1，继续迭代

关键：步骤 ④ 零偏变了之后，预积分量并不重新算 —— 靠 $\texttt{pre.J}$ 做一阶修正：
$\text{残差}=\text{旧零偏下的预积分量}+\mathbf J\cdot(\text{新零偏}-\text{旧零偏})-\text{状态推算}$

六、零偏的特殊地位：建区间时锁定

// src/swf/imu_processor.h
// 区间预积分的线性化点：建区间时锁定，之后的偏置变化由预积分的一阶 Jacobian 补偿。
struct ImuLinearizationPoint {
    V3D accel_bias = V3D::Zero();
    V3D gyro_bias  = V3D::Zero();
    V3D accel_scale = V3D::Ones();
    V3D gyro_scale  = V3D::Ones();
};

「建区间时锁定」这五个字就是答案：预积分时用的零偏是那一刻的估计值，冻结住，整个区间不再变。零偏会变，但那是在窗口优化里变的。

七、为什么预积分量「也满足某种分布」

这是一个很好的直觉。预积分量确实服从高斯分布：

$$\Delta\mathbf p_{ij}\sim\mathcal N\big(\hat{\Delta\mathbf p}_{ij},\ \mathbf C_{\Delta p}\big)$$

但这个分布不是「自然就有」的，是算出来的 —— 通过误差传播：

$$\mathbf C_{k+1}=\underbrace{\mathbf F_k\mathbf C_k\mathbf F_k^\top}_{\text{旧不确定性被搬过去}}+\underbrace{\mathbf V_k\mathbf Q_k\mathbf V_k^\top}_{\text{这一步新注入的噪声}}$$

注意：协方差 $\mathbf C$ 是由 状态转移矩阵 $\mathbf F$ 和噪声映射 $\mathbf V$ 决定的，不是由「对偏差的雅可比 $\mathbf J$」决定的。
$\mathbf F$ 和 $\mathbf J$ 是同一个矩阵的两种用途：$\mathbf F$ 单步传播协方差，$\mathbf J$ 连乘累积敏感度（$\mathbf J_N=\mathbf F_{N-1}\cdots\mathbf F_0$）。

最后 $\mathbf C$ 变成白化矩阵：

$$\texttt{sqrt\_info}=\mathbf C^{-1/2},\qquad \mathbf r_{\text{白化}}=\mathbf C^{-1/2}\mathbf r$$

含义：「这个预积分量不确定性大，那它说的话就别太当真。」

八、预积分产出清单

变量	类型	作用
$\texttt{p, v, q}$	3+3+4	三个预积分量，残差的「预测值」
$\texttt{duration}$	标量	区间时长，重力项和时延项要用
$\texttt{J}$	15×15	对初始误差状态的敏感度 → 零偏一阶修正 + 残差雅可比的原料
$\texttt{S}$	15×6	对刻度 $[\mathbf s_a,\mathbf s_g]$ 的敏感度
$\texttt{C}$	15×15	协方差 → 白化矩阵
$\texttt{gyro\_first/last}$	3+3	区间两端角速度 → 时延修正
$\texttt{acc\_prev/gyro\_prev}$	3+3	上一个样本 → 下一步中点法

九、代码对照

// ① 缓存：什么都不算
void IMUProcessor::pushSample(const ImuSample &sample) {
    pending.push_back(sample);          // 只是入队
    prediction.push_back(sample);
}

// ② 图像帧到达：一次性批量积分
bool IMUProcessor::appendFrame(double image_time, const ImuLinearizationPoint &point) {
    std::vector<ImuSample> samples;
    extractSamples(frame_times.back(), image_time, samples);   // 切出区间

    interval.preintegrator = std::make_unique<Preintegrator>(
        samples.front().accel, samples.front().gyro,
        point.accel_bias, point.gyro_bias,        // 用「锁定的」零偏
        point.accel_scale, point.gyro_scale, noise);

    for (std::size_t i = 1; i < samples.size(); ++i) {          // 批量循环
        const double dt = samples[i].timestamp - samples[i-1].timestamp;
        interval.preintegrator->push(dt, samples[i].accel, samples[i].gyro);
    }
    ...
}

十、三个 takeaway

预积分是「算」，不是「估」。它是把 100 个 IMU 样本压缩成一个约束的纯数值过程。零偏在预积分阶段是冻结的，只是被记录下敏感度。
「持续」的是样本缓存，不是积分。$\texttt{pushSample}$ 只入队；真正的积分发生在图像帧到达、$\texttt{appendFrame}$ 被调用时，一次性批量完成。
估计在窗口层。零偏、速度、位姿都在那里被解出来；零偏更新后不重新积分 —— 靠 $\texttt{pre.J}$ 做一阶修正。这就是「预」积分能成立的前提，也是雅可比矩阵存在的理由。