预积分最容易让人困惑的地方，是把三件事混在了一起：这一段 IMU 算出了什么、这个结果有多可信、零偏改了以后结果怎么改。
先把这三件事分开，公式就有了用处。
本文独立组织讲解，使用自设例子和标准运动学公式。延伸阅读见文末半闲居士的《简明预积分推导》和 Forster 等人的原始论文。
1. 先看一个能算明白的例子
暂时只考虑水平方向，不转弯；这一轴上的重力投影为零，且已经扣除加速度计零偏。假设接下来 1 秒内，加速度恒为 2 m / s 2 2\,\mathrm{m/s^2} 2m/s2。
无论开始时在什么位置、以多大速度运动，这段加速度带来的贡献都是：
Δ v = 2 m / s , Δ p = 1 m . \Delta v=2\,\mathrm{m/s},\qquad \Delta p=1\,\mathrm m. Δv=2m/s,Δp=1m.
注意， Δ p \Delta p Δp 只是加速度带来的位移贡献 。如果开始时速度为 3 m / s 3\,\mathrm{m/s} 3m/s，实际位移是 3 × 1 + 1 = 4 m 3\times1+1=4\,\mathrm m 3×1+1=4m。
这就抓住了预积分的用途：先把这段 IMU 数据的贡献算好，起点位置、速度和姿态留到优化时再代入。
例如，IMU 为 200 Hz、关键帧为 10 Hz，两帧之间约有 20 个 IMU 采样区间。可以把它们积成一个连接两帧的约束，不必给每个 IMU 时刻都设一个优化节点。
2. 三维情况下，要先说清坐标系
约定：
| 符号 | 含义 |
|---|---|
| i , j i,j i,j | 这段预积分的起点和终点 |
| k k k | 中间某个 IMU 时刻 |
| Δ t k = t k + 1 − t k \Delta t_k=t_{k+1}-t_k Δtk=tk+1−tk | 一个 IMU 采样区间的时长 |
| T = t j − t i T=t_j-t_i T=tj−ti | 两个关键帧之间的总时长 |
| R k \mathbf R_k Rk | 把 k k k 时刻机体系向量转到世界系的旋转矩阵 |
| p k , v k \mathbf p_k,\mathbf v_k pk,vk | 世界系中的位置和速度 |
| g \mathbf g g | 世界系中的重力向量；若世界系 z 轴向上，则为 ( 0 , 0 , − g ) ⊤ (0,0,-g)^\top (0,0,−g)⊤ |
| ω m , k , a m , k \boldsymbol\omega_{m,k},\mathbf a_{m,k} ωm,k,am,k | IMU 输出的角速度和比力，均在机体系中 |
| b ˉ g , b ˉ a \bar{\mathbf b}_g,\bar{\mathbf b}_a bˉg,bˉa | 本段积分使用的陀螺仪、加速度计零偏估计 |
加速度计输出的是比力，不能直接当成世界系中的运动加速度。 扣除零偏后，还要旋转到世界系并加上重力：
a w o r l d , k ≈ R k ( a m , k − b ˉ a ) + g . \mathbf a_{\mathrm{world},k} \approx \mathbf R_k(\mathbf a_{m,k}-\bar{\mathbf b}_a)+\mathbf g. aworld,k≈Rk(am,k−bˉa)+g.
静止且姿态为单位阵时，加速度计读数约为 − g -\mathbf g −g，所以上式得到的运动加速度为零。
预积分把整段数据都表达到起点 i i i 的机体系。定义三个相对量：
Δ R i j = R i ⊤ R j , Δ v i j = R i ⊤ ( v j − v i − g T ) , Δ p i j = R i ⊤ ( p j − p i − v i T − 1 2 g T 2 ) . \begin{aligned} \Delta\mathbf R_{ij}&=\mathbf R_i^\top\mathbf R_j,\\ \Delta\mathbf v_{ij}&=\mathbf R_i^\top(\mathbf v_j-\mathbf v_i-\mathbf gT),\\ \Delta\mathbf p_{ij}&=\mathbf R_i^\top \left(\mathbf p_j-\mathbf p_i-\mathbf v_iT-\tfrac12\mathbf gT^2\right). \end{aligned} ΔRijΔvijΔpij=Ri⊤Rj,=Ri⊤(vj−vi−gT),=Ri⊤(pj−pi−viT−21gT2).
读这三个式子时，只看两步：先扣掉起点状态和重力贡献，再转回起点坐标系。 因此 Δ v \Delta\mathbf v Δv 不是世界系里的速度直接相减， Δ p \Delta\mathbf p Δp 也不是实际总位移。
这里的重力处理体现在模型中，不要求先把每个 IMU 采样转到重力水平面。三维预积分与依赖重力对齐的简化二维模型，需要分开理解。
3. 程序到底怎样把 IMU 积起来
本节先用前向离散积分：每个小区间使用区间起点的姿态和读数。第 8 节再说明中值积分的替换方法。
先扣除零偏：
ω k = ω m , k − b ˉ g , f k = a m , k − b ˉ a . \boldsymbol\omega_k=\boldsymbol\omega_{m,k}-\bar{\mathbf b}g, \qquad \mathbf f_k=\mathbf a{m,k}-\bar{\mathbf b}_a. ωk=ωm,k−bˉg,fk=am,k−bˉa.
用 Δ R ^ , Δ v ^ , Δ p ^ \widehat{\Delta\mathbf R},\widehat{\Delta\mathbf v},\widehat{\Delta\mathbf p} ΔR ,Δv ,Δp 表示实际用带噪声读数算出的结果。帽子只表示计算值，不表示它已经没有误差。
起点初始化为：
Δ R ^ i i = I , Δ v ^ i i = 0 , Δ p ^ i i = 0. \widehat{\Delta\mathbf R}{ii}=\mathbf I, \qquad \widehat{\Delta\mathbf v}{ii}=\mathbf0, \qquad \widehat{\Delta\mathbf p}_{ii}=\mathbf0. ΔR ii=I,Δv ii=0,Δp ii=0.
旋转初值是单位矩阵，表示"还没有旋转"。
每来一个采样区间，更新一次：
Δ R ^ i , k + 1 = Δ R ^ i k Exp ( ω k Δ t k ) , Δ v ^ i , k + 1 = Δ v ^ i k + Δ R ^ i k f k Δ t k , Δ p ^ i , k + 1 = Δ p ^ i k + Δ v ^ i k Δ t k + 1 2 Δ R ^ i k f k Δ t k 2 . \boxed{ \begin{aligned} \widehat{\Delta\mathbf R}{i,k+1} &=\widehat{\Delta\mathbf R}{ik} \operatorname{Exp}(\boldsymbol\omega_k\Delta t_k),\\ \widehat{\Delta\mathbf v}{i,k+1} &=\widehat{\Delta\mathbf v}{ik} +\widehat{\Delta\mathbf R}{ik}\mathbf f_k\Delta t_k,\\ \widehat{\Delta\mathbf p}{i,k+1} &=\widehat{\Delta\mathbf p}{ik} +\widehat{\Delta\mathbf v}{ik}\Delta t_k +\tfrac12\widehat{\Delta\mathbf R}_{ik}\mathbf f_k\Delta t_k^2. \end{aligned}} ΔR i,k+1Δv i,k+1Δp i,k+1=ΔR ikExp(ωkΔtk),=Δv ik+ΔR ikfkΔtk,=Δp ik+Δv ikΔtk+21ΔR ikfkΔtk2.
Exp ( ϕ ) \operatorname{Exp}(\boldsymbol\phi) Exp(ϕ) 把三维旋转向量变成旋转矩阵：向量方向是转轴，长度是转角，单位为弧度。
三行分别是：
- 把这一步的转动接到已有转动后面。
- 把当前比力转到起点坐标系，累加速度贡献。
- 累加已有速度带来的位移，以及这一步比力带来的位移。
三行右侧都使用更新前的量。 写程序时应先保存旧值，避免速度刚更新就被位置公式误用。
这组递推不含世界系中的 R i , p i , v i \mathbf R_i,\mathbf p_i,\mathbf v_i Ri,pi,vi。优化改了这些起点状态，不需要重新遍历 IMU 数据。它仍依赖零偏，这个问题留到第 5 节处理。
4. 协方差：这段积分结果有多可信
4.1 先看一维，别急着看矩阵
假设某一步加速度读数中有噪声 n k n_k nk，方差为 q k q_k qk。速度计算误差记作 e v , k e_{v,k} ev,k，则：
e v , k + 1 = e v , k + n k Δ t k . e_{v,k+1}=e_{v,k}+n_k\Delta t_k. ev,k+1=ev,k+nkΔtk.
若新噪声与已有误差独立，方差就是：
P v , k + 1 = P v , k + q k Δ t k 2 . P_{v,k+1}=P_{v,k}+q_k\Delta t_k^2. Pv,k+1=Pv,k+qkΔtk2.
原因很直接：旧误差会留下，新读数又带来一份误差；乘了 Δ t k \Delta t_k Δtk，方差就乘 Δ t k 2 \Delta t_k^2 Δtk2。
同一份加速度噪声还会影响位置，造成速度误差和位置误差相关。因此三维情况下要联合保存它们的协方差，不能只记几个互不相关的方差。
4.2 三维多了"方向算错，会把加速度转错"
先约定误差的正负号。以理想预积分量和计算值的关系定义：
Δ R i k = Δ R ^ i k Exp ( − δ θ k ) , Δ v i k = Δ v ^ i k − δ v k , Δ p i k = Δ p ^ i k − δ p k . \begin{aligned} \Delta\mathbf R_{ik} &=\widehat{\Delta\mathbf R}{ik}\operatorname{Exp}(-\delta\boldsymbol\theta_k),\\ \Delta\mathbf v{ik} &=\widehat{\Delta\mathbf v}{ik}-\delta\mathbf v_k,\\ \Delta\mathbf p{ik} &=\widehat{\Delta\mathbf p}_{ik}-\delta\mathbf p_k. \end{aligned} ΔRikΔvikΔpik=ΔR ikExp(−δθk),=Δv ik−δvk,=Δp ik−δpk.
δ θ k \delta\boldsymbol\theta_k δθk 是三维小转角误差， δ v k , δ p k \delta\mathbf v_k,\delta\mathbf p_k δvk,δpk 是速度和位置预积分误差。这里只分析测量噪声造成的误差，暂时固定零偏。
为让公式短一点，记：
D k = Δ R ^ i k , A k = Exp ( ω k Δ t k ) . \mathbf D_k=\widehat{\Delta\mathbf R}_{ik},\qquad \mathbf A_k=\operatorname{Exp}(\boldsymbol\omega_k\Delta t_k). Dk=ΔR ik,Ak=Exp(ωkΔtk).
D k \mathbf D_k Dk 是已经累计的旋转； A k \mathbf A_k Ak 只是当前这一步的旋转。
还有两个运算需要先说明：
- f × u = f × u \\mathbf f_\times\mathbf u=\mathbf f\times\mathbf u f×u=f×u：方括号只是把叉乘写成矩阵乘法。
- J r , k = J r ( ω k Δ t k ) \mathbf J_{r,k}=\mathbf J_r(\boldsymbol\omega_k\Delta t_k) Jr,k=Jr(ωkΔtk)：旋转右雅可比，用于把"旋转向量的微小变化"换成"接在旋转矩阵右侧的小转动"。小转角时约为单位矩阵，精确定义见文末。
把原始积分递推中的误差代进去，舍掉误差与噪声相乘等二阶小量，得到：
δ θ k + 1 ≈ A k ⊤ δ θ k + J r , k n g , k Δ t k , δ v k + 1 ≈ δ v k − D k f k × δ θ k Δ t k + D k n a , k Δ t k , δ p k + 1 ≈ δ p k + δ v k Δ t k − 1 2 D k f k × δ θ k Δ t k 2 + 1 2 D k n a , k Δ t k 2 . \begin{aligned} \delta\boldsymbol\theta_{k+1} &\approx\mathbf A_k^\top\delta\boldsymbol\theta_k +\mathbf J_{r,k}\mathbf n_{g,k}\Delta t_k,\\ \delta\mathbf v_{k+1} &\approx\delta\mathbf v_k -\mathbf D_k\\mathbf f_k\times\delta\boldsymbol\theta_k\Delta t_k +\mathbf D_k\mathbf n{a,k}\Delta t_k,\\ \delta\mathbf p_{k+1} &\approx\delta\mathbf p_k+\delta\mathbf v_k\Delta t_k -\tfrac12\mathbf D_k\\mathbf f_k\times\delta\boldsymbol\theta_k\Delta t_k^2 +\tfrac12\mathbf D_k\mathbf n{a,k}\Delta t_k^2. \end{aligned} δθk+1δvk+1δpk+1≈Ak⊤δθk+Jr,kng,kΔtk,≈δvk−Dkfk×δθkΔtk+Dkna,kΔtk,≈δpk+δvkΔtk−21Dkfk×δθkΔtk2+21Dkna,kΔtk2.
n g , k , n a , k \mathbf n_{g,k},\mathbf n_{a,k} ng,k,na,k 分别是当前陀螺仪和加速度计的离散读数噪声。
其中最值得看懂的是速度里的第二项。它来自：
D k Exp ( − δ θ k ) ( f k − n a , k ) ≈ D k f k + D k f k × δ θ k − D k n a , k . \begin{aligned} \mathbf D_k\operatorname{Exp}(-\delta\boldsymbol\theta_k) (\mathbf f_k-\mathbf n_{a,k}) &\approx\mathbf D_k\mathbf f_k +\mathbf D_k\\mathbf f_k\times\delta\boldsymbol\theta_k -\mathbf D_k\mathbf n{a,k}. \end{aligned} DkExp(−δθk)(fk−na,k)≈Dkfk+Dkfk×δθk−Dkna,k.
这里用了 Exp ( − δ θ ) ≈ I − δ θ × \operatorname{Exp}(-\delta\boldsymbol\theta)\approx\mathbf I-\\delta\\boldsymbol\\theta_\times Exp(−δθ)≈I−δθ×，以及 − δ θ × f = f × δ θ -\delta\boldsymbol\theta\times\mathbf f=\mathbf f\times\delta\boldsymbol\theta −δθ×f=f×δθ。再用"计算值减理想值"的误差定义，速度误差里的姿态项前面就出现负号。
它说的就是：即使加速度读数准确，转它的方向有误，积分出来的速度也会错。 陀螺仪噪声因此也会影响速度和位置。
4.3 雅可比矩阵，就是把上面的系数排成表
把三个误差叠成一个九维向量，把两类噪声叠成一个六维向量：
e k = δ θ k δ v k δ p k , n k = n g , k n a , k . \mathbf e_k= \begin{bmatrix}\delta\boldsymbol\theta_k\\\delta\mathbf v_k\\\delta\mathbf p_k\end{bmatrix}, \qquad \mathbf n_k=\begin{bmatrix}\mathbf n_{g,k}\\\mathbf n_{a,k}\end{bmatrix}. ek= δθkδvkδpk ,nk=ng,kna,k.
刚才的三行误差递推，就可以缩写为：
e k + 1 ≈ F k e k + G k n k , \mathbf e_{k+1}\approx\mathbf F_k\mathbf e_k+\mathbf G_k\mathbf n_k, ek+1≈Fkek+Gknk,
其中：
F k = A k ⊤ 0 0 − D k \[ f k × Δ t k I 0 − 1 2 D k f k × Δ t k 2 I Δ t k I ] , G k = J r , k Δ t k 0 0 D k Δ t k 0 1 2 D k Δ t k 2 . \mathbf F_k= \begin{bmatrix} \mathbf A_k^\top&\mathbf0&\mathbf0\\ -\mathbf D_k\\mathbf f_k\times\Delta t_k&\mathbf I&\mathbf0\\ -\tfrac12\mathbf D_k\\mathbf f_k\times\Delta t_k^2&\mathbf I\Delta t_k&\mathbf I \end{bmatrix}, \quad \mathbf G_k= \begin{bmatrix} \mathbf J_{r,k}\Delta t_k&\mathbf0\\ \mathbf0&\mathbf D_k\Delta t_k\\ \mathbf0&\tfrac12\mathbf D_k\Delta t_k^2 \end{bmatrix}. Fk= Ak⊤−Dkfk×Δtk−21Dkfk×Δtk20IIΔtk00I ,Gk= Jr,kΔtk000DkΔtk21DkΔtk2 .
每个小块都是 3 × 3 3\times3 3×3。 F k \mathbf F_k Fk 的三列对应旧的角度、速度、位置误差； G k \mathbf G_k Gk 的两列对应新的陀螺仪、加速度计噪声。
例如 F k \mathbf F_k Fk 的第三行第二列是 I Δ t k \mathbf I\Delta t_k IΔtk，只是把"旧速度误差乘时间，会变成位置误差"写进矩阵。它没有另起一套理论。
定义：
P k = Cov ( e k ) , Q k = Cov ( n k ) . \mathbf P_k=\operatorname{Cov}(\mathbf e_k),\qquad \mathbf Q_k=\operatorname{Cov}(\mathbf n_k). Pk=Cov(ek),Qk=Cov(nk).
在当前噪声与历史误差独立的假设下：
P k + 1 = F k P k F k ⊤ + G k Q k G k ⊤ , P i = 0. \boxed{\mathbf P_{k+1} =\mathbf F_k\mathbf P_k\mathbf F_k^\top +\mathbf G_k\mathbf Q_k\mathbf G_k^\top},\qquad\mathbf P_i=\mathbf0. Pk+1=FkPkFk⊤+GkQkGk⊤,Pi=0.
第一项传播已有的不确定性，第二项加入当前读数带来的不确定性。这就是前面一维方差递推的矩阵版本。
这里的 P \mathbf P P 为 9 × 9 9\times9 9×9，是这段旋转、速度、位置预积分误差的协方差。如果还把零偏随机游走放进同一个误差状态，常见维数会扩展到 15；那是扩展后的模型。
5. 零偏雅可比：零偏改一点，结果该改多少
再回到一维例子。假设持续 T T T 秒，加速度计读数恒为 a m a_m am，使用的零偏估计为 b ˉ a \bar b_a bˉa：
Δ v ^ = ( a m − b ˉ a ) T , Δ p ^ = 1 2 ( a m − b ˉ a ) T 2 . \widehat{\Delta v}=(a_m-\bar b_a)T,\qquad \widehat{\Delta p}=\tfrac12(a_m-\bar b_a)T^2. Δv =(am−bˉa)T,Δp =21(am−bˉa)T2.
于是：
∂ Δ v ^ ∂ b ˉ a = − T , ∂ Δ p ^ ∂ b ˉ a = − 1 2 T 2 . \frac{\partial\widehat{\Delta v}}{\partial\bar b_a}=-T, \qquad \frac{\partial\widehat{\Delta p}}{\partial\bar b_a}=-\tfrac12T^2. ∂bˉa∂Δv =−T,∂bˉa∂Δp =−21T2.
这两个导数，就是最简单的零偏雅可比。例如积分时长为 1 秒，后来把零偏估计增加 0.1 m / s 2 0.1\,\mathrm{m/s^2} 0.1m/s2，速度预积分就减少 0.1 m / s 0.1\,\mathrm{m/s} 0.1m/s，位置预积分减少 0.05 m 0.05\,\mathrm m 0.05m。
雅可比记录的是灵敏度：输入改一点，输出跟着改多少。
三维需要保存五个 3 × 3 3\times3 3×3 灵敏度矩阵：
| 矩阵 | 描述的影响 |
|---|---|
| J R g \mathbf J_{Rg} JRg | 陀螺仪零偏对旋转预积分的影响 |
| J v g , J v a \mathbf J_{vg},\mathbf J_{va} Jvg,Jva | 两种零偏对速度预积分的影响 |
| J p g , J p a \mathbf J_{pg},\mathbf J_{pa} Jpg,Jpa | 两种零偏对位置预积分的影响 |
其中旋转的导数是在三维小转角中定义的，不是对旋转矩阵九个元素分别求导。
优化后的零偏相对积分时的零偏，改变了 δ b g , δ b a \delta\mathbf b_g,\delta\mathbf b_a δbg,δba。一阶修正为：
Δ R ^ c ≈ Δ R ^ Exp ( J R g δ b g ) , Δ v ^ c ≈ Δ v ^ + J v g δ b g + J v a δ b a , Δ p ^ c ≈ Δ p ^ + J p g δ b g + J p a δ b a . \begin{aligned} \widehat{\Delta\mathbf R}^{\,c} &\approx\widehat{\Delta\mathbf R} \operatorname{Exp}(\mathbf J_{Rg}\delta\mathbf b_g),\\ \widehat{\Delta\mathbf v}^{\,c} &\approx\widehat{\Delta\mathbf v} +\mathbf J_{vg}\delta\mathbf b_g+\mathbf J_{va}\delta\mathbf b_a,\\ \widehat{\Delta\mathbf p}^{\,c} &\approx\widehat{\Delta\mathbf p} +\mathbf J_{pg}\delta\mathbf b_g+\mathbf J_{pa}\delta\mathbf b_a. \end{aligned} ΔR cΔv cΔp c≈ΔR Exp(JRgδbg),≈Δv +Jvgδbg+Jvaδba,≈Δp +Jpgδbg+Jpaδba.
上标 c c c 表示修正后的值； δ b \delta\mathbf b δb 是"当前零偏估计减去积分时使用的零偏估计"。零偏变化较小时，这样修正通常比重做整段积分省事；变化较大时应重新积分。
这些灵敏度从哪里来
还是对第 3 节的积分递推求导。例如旋转和速度的陀螺仪零偏雅可比满足：
J R g , k + 1 = A k ⊤ J R g , k − J r , k Δ t k , J v g , k + 1 = J v g , k − D k f k × J R g , k Δ t k . \begin{aligned} \mathbf J_{Rg,k+1} &=\mathbf A_k^\top\mathbf J_{Rg,k}-\mathbf J_{r,k}\Delta t_k,\\ \mathbf J_{vg,k+1} &=\mathbf J_{vg,k} -\mathbf D_k\\mathbf f_k\times\mathbf J{Rg,k}\Delta t_k. \end{aligned} JRg,k+1Jvg,k+1=Ak⊤JRg,k−Jr,kΔtk,=Jvg,k−Dkfk×JRg,kΔtk.
第一行有两部分：之前积累的旋转灵敏度要传到下一步；当前扣除的陀螺仪零偏也会改变这一步转角。
第二行说明：陀螺仪零偏先影响旋转，再影响加速度的旋转方向，最后影响速度。
完整五个矩阵可以合并成一个 9 × 6 9\times6 9×6 矩阵：
S k = J R g , k 0 J v g , k J v a , k J p g , k J p a , k . \mathbf S_k= \begin{bmatrix} \mathbf J_{Rg,k}&\mathbf0\\ \mathbf J_{vg,k}&\mathbf J_{va,k}\\ \mathbf J_{pg,k}&\mathbf J_{pa,k} \end{bmatrix}. Sk= JRg,kJvg,kJpg,k0Jva,kJpa,k .
对于本文这套前向积分，它们可以直接用第 4 节的矩阵一起递推：
S k + 1 = F k S k − G k , S i = 0. \boxed{\mathbf S_{k+1}=\mathbf F_k\mathbf S_k-\mathbf G_k}, \qquad\mathbf S_i=\mathbf0. Sk+1=FkSk−Gk,Si=0.
这里的减号来自读数中"减去零偏"的操作。噪声与零偏都从同一处进入积分公式，但用途不同：噪声用于统计不确定性，零偏改变量用于修正计算值。
6. 优化怎样使用这段结果
每个关键帧通常估计姿态、位置、速度和两种零偏：
x k = ( R k , p k , v k , b g , k , b a , k ) . \mathbf x_k=(\mathbf R_k,\mathbf p_k,\mathbf v_k, \mathbf b_{g,k},\mathbf b_{a,k}). xk=(Rk,pk,vk,bg,k,ba,k).
优化器要比较两份结果：关键帧状态算出的相对运动，是否符合 IMU 给出的预积分测量。
用第 5 节修正后的预积分值构造残差：
r R = Log ( Δ R \^ c ) ⊤ R i ⊤ R j , r v = R i ⊤ ( v j − v i − g T ) − Δ v ^ c , r p = R i ⊤ ( p j − p i − v i T − 1 2 g T 2 ) − Δ p ^ c . \begin{aligned} \mathbf r_R &=\operatorname{Log}\!\left (\\widehat{\\Delta\\mathbf R}\^{\\,c})\^\\top\\mathbf R_i\^\\top\\mathbf R_j \\right,\\ \mathbf r_v &=\mathbf R_i^\top(\mathbf v_j-\mathbf v_i-\mathbf gT) -\widehat{\Delta\mathbf v}^{\,c},\\ \mathbf r_p &=\mathbf R_i^\top \left(\mathbf p_j-\mathbf p_i-\mathbf v_iT-\tfrac12\mathbf gT^2\right) -\widehat{\Delta\mathbf p}^{\,c}. \end{aligned} rRrvrp=Log(ΔR c)⊤Ri⊤Rj,=Ri⊤(vj−vi−gT)−Δv c,=Ri⊤(pj−pi−viT−21gT2)−Δp c.
Log \operatorname{Log} Log 把旋转矩阵变回三维旋转向量。因此三项各三维，合起来是九维残差 r \mathbf r r。残差越接近零，两种结果越一致。
第 4 节的协方差用在这里：
E I M U = r ⊤ P i j − 1 r . \boxed{E_{\mathrm{IMU}}=\mathbf r^\top\mathbf P_{ij}^{-1}\mathbf r}. EIMU=r⊤Pij−1r.
这是局部线性化、高斯误差模型下的加权误差。直观上，越确定的误差方向，权重越大；越不确定的方向，权重越小。P \mathbf P P 给出可信程度， P − 1 \mathbf P^{-1} P−1 才是信息权重。
实际实现通常使用 Cholesky 分解白化残差，避免直接求逆。还需要为相邻关键帧的零偏建立随机游走约束，并与相机、激光等约束共同优化。
7. 三种雅可比，分别服务于什么事
| 名称 | 对什么求导 | 用来做什么 |
|---|---|---|
| 误差传播雅可比 F , G \mathbf F,\mathbf G F,G | 下一步误差对旧误差、新噪声 | 递推协方差 |
| 零偏雅可比 S \mathbf S S | 预积分结果对积分所用零偏 | 零偏变化后快速修正预积分 |
| 残差雅可比 H \mathbf H H | 优化残差对关键帧状态的小改变量 | 告诉优化器怎样调整状态 |
例如速度残差中：
∂ r v ∂ v i = − R i ⊤ , ∂ r v ∂ v j = R i ⊤ . \frac{\partial\mathbf r_v}{\partial\mathbf v_i}=-\mathbf R_i^\top, \qquad \frac{\partial\mathbf r_v}{\partial\mathbf v_j}=\mathbf R_i^\top. ∂vi∂rv=−Ri⊤,∂vj∂rv=Ri⊤.
因为 v i \mathbf v_i vi 前面本来就是负号， v j \mathbf v_j vj 前面就是正号。
而速度残差对加速度计零偏的导数为 − J v a -\mathbf J_{va} −Jva，因为残差里减去了修正后的预积分速度。它们与零偏雅可比有关，但求导的对象已经变成了残差。
程序可以在每个 IMU 区间同时更新三样东西：积分值、协方差、零偏雅可比。 到关键帧时，再用它们计算残差和优化所需的导数。
8. 换成中值积分，要改哪里
前文每步只使用区间起点读数。中值积分会利用区间两端的数据。
先取两端扣除零偏后的角速度平均值，更新旋转：
ω ˉ k = 1 2 ( ω m , k + ω m , k + 1 ) − b ˉ g , Δ R ^ i , k + 1 = Δ R ^ i k Exp ( ω ˉ k Δ t k ) . \begin{aligned} \bar{\boldsymbol\omega}k &=\tfrac12(\boldsymbol\omega{m,k}+\boldsymbol\omega_{m,k+1}) -\bar{\mathbf b}g,\\ \widehat{\Delta\mathbf R}{i,k+1} &=\widehat{\Delta\mathbf R}_{ik} \operatorname{Exp}(\bar{\boldsymbol\omega}_k\Delta t_k). \end{aligned} ωˉkΔR i,k+1=21(ωm,k+ωm,k+1)−bˉg,=ΔR ikExp(ωˉkΔtk).
然后把两端比力分别转到起点坐标系，再求平均：
f ˉ k i = 1 2 Δ R \^ i k ( a m , k − b ˉ a ) + Δ R \^ i , k + 1 ( a m , k + 1 − b ˉ a ) . \bar{\mathbf f}^{\,i}_k=\tfrac12\left \\widehat{\\Delta\\mathbf R}_{ik}(\\mathbf a_{m,k}-\\bar{\\mathbf b}_a) +\\widehat{\\Delta\\mathbf R}_{i,k+1}(\\mathbf a_{m,k+1}-\\bar{\\mathbf b}_a) \\right. fˉki=21ΔR ik(am,k−bˉa)+ΔR i,k+1(am,k+1−bˉa).
上标 i i i 表示这个平均比力已经在起点坐标系中。再更新：
Δ v ^ i , k + 1 = Δ v ^ i k + f ˉ k i Δ t k , Δ p ^ i , k + 1 = Δ p ^ i k + Δ v ^ i k Δ t k + 1 2 f ˉ k i Δ t k 2 . \begin{aligned} \widehat{\Delta\mathbf v}{i,k+1} &=\widehat{\Delta\mathbf v}{ik}+\bar{\mathbf f}^{\,i}k\Delta t_k,\\ \widehat{\Delta\mathbf p}{i,k+1} &=\widehat{\Delta\mathbf p}{ik} +\widehat{\Delta\mathbf v}{ik}\Delta t_k +\tfrac12\bar{\mathbf f}^{\,i}_k\Delta t_k^2. \end{aligned} Δv i,k+1Δp i,k+1=Δv ik+fˉkiΔtk,=Δp ik+Δv ikΔtk+21fˉkiΔtk2.
中值积分改变了递推关系，误差传播和零偏雅可比也必须按这组新公式重新求导。 尤其是区间末端的加速度使用了更新后的旋转，当前陀螺仪误差会在同一区间影响速度和位置，不能照搬第 4、5 节的矩阵。
此外，相邻区间会共用一个端点采样。若把端点读数噪声建模为独立随机变量，严格传播时需要处理共用采样引起的相关性；把各区间噪声视为独立则是一种需要明确的近似。
查公式时需要的两个细节
旋转右雅可比的精确形式。 令 ϕ \boldsymbol\phi ϕ 为旋转向量、 θ = ∥ ϕ ∥ \theta=\|\boldsymbol\phi\| θ=∥ϕ∥：
J r ( ϕ ) = I − 1 − cos θ θ 2 ϕ × + θ − sin θ θ 3 ϕ × 2 . \mathbf J_r(\boldsymbol\phi) =\mathbf I -\frac{1-\cos\theta}{\theta^2}\\boldsymbol\\phi\times +\frac{\theta-\sin\theta}{\theta^3}\\boldsymbol\\phi\times^2. Jr(ϕ)=I−θ21−cosθϕ×+θ3θ−sinθϕ×2.
它满足一阶关系：
Exp ( ϕ + δ ϕ ) ≈ Exp ( ϕ ) Exp ( J r ( ϕ ) δ ϕ ) . \operatorname{Exp}(\boldsymbol\phi+\delta\boldsymbol\phi) \approx\operatorname{Exp}(\boldsymbol\phi) \operatorname{Exp}\!\left(\mathbf J_r(\boldsymbol\phi)\delta\boldsymbol\phi\right). Exp(ϕ+δϕ)≈Exp(ϕ)Exp(Jr(ϕ)δϕ).
接近零时用极限或级数计算，避免直接除以零。
离散噪声方差和连续噪声密度。 第 4 节的 Q k \mathbf Q_k Qk 是离散采样噪声协方差。若手里的是连续白噪声谱密度矩阵 Q c \mathbf Q_c Qc，在区间平均采样模型下有 Q k = Q c / Δ t k \mathbf Q_k=\mathbf Q_c/\Delta t_k Qk=Qc/Δtk。若给的是噪声密度标准差，应先平方。不要把已经是离散读数方差的量再除一次时间。
参考阅读
- 半闲居士：简明预积分推导。建议结合原文查看完整李群推导。
- Christian Forster, Luca Carlone, Frank Dellaert, Davide Scaramuzza：On-Manifold Preintegration for Real-Time Visual-Inertial Odometry。完整理论及误差传播、零偏修正、残差雅可比见论文及附录。