IMU预积分误差模型推导：协方差传播、残差构建、雅可比矩阵

1. 预积分目的与中值积分递推

在基于图优化的 VIO（视觉惯性里程计）中，IMU 的采样频率通常高于相机频率。两帧图像之间可能包含数十至数百条 IMU 测量。如果优化器每修改一次状态，都重新遍历这些测量进行积分，会产生大量重复计算。

预积分的核心思想：在关键帧 i 到 j 之间，把高频 IMU 测量压缩为旋转、速度和位置三个相对增量，并同时保存误差协方差及对零偏的雅可比。优化时用这些量构造约束；零偏发生小幅变化时，用一阶修正更新增量，变化过大时再重积分。

本文按"中值递推 → 误差传播与协方差 → 预积分残差 → 偏置雅可比和残差雅可比 → 优化流程"展开。后面的矩阵均从本节的离散递推式推导，不混用欧拉积分、不同旋转扰动方向或不同状态排列。

1.1 坐标系、测量模型与符号

本文讨论传统三维预积分。设世界系为 W，IMU 机体系为 B。旋转矩阵 R_k 把时刻 k 的机体系向量转换到世界系，位置 p_k、速度 v_k 和重力向量 g 均在世界系表达。

为避免"全局姿态"与"预积分旋转"混淆，记：

A k ≡ Δ R i k , ν k ≡ Δ v i k , α k ≡ Δ p i k . \mathbf A_k \equiv \Delta\mathbf R_{ik},\qquad \boldsymbol\nu_k\equiv\Delta\mathbf v_{ik},\qquad \boldsymbol\alpha_k\equiv\Delta\mathbf p_{ik}. Ak≡ΔRik,νk≡Δvik,αk≡Δpik.

A_k 把 B_k 中的向量旋转到积分起点 B_i；ν_k、α_k 也在 B_i 中表达。它们不是全局速度和全局位置。

符号	含义
h_k = t_{k+1} − t_k	相邻 IMU 样本的时间间隔
T = t_j − t_i	整个关键帧积分区间的时长
ω_m,k、a_m,k	IMU 原始角速度和加速度计比力测量，均在 B_k 中表达
b̄_g、b̄_a	本次预积分采用的零偏线性化点，在区间内作为常量
$x$ _×	叉乘矩阵，满足 $x$ _× y = x × y
Exp(x)	exp( $x$ _×)，将三维旋转向量映射到 SO(3)
Log®	log® 的 vee，返回三维旋转向量

IMU 测量模型为：

ω m , k = ω k + b g , k + n g , k , a m , k = R k ⊤ ( v ˙ k − g ) + b a , k + n a , k . \boldsymbol\omega_{m,k} =\boldsymbol\omega_k+\mathbf b_{g,k}+\mathbf n_{g,k}, \qquad \mathbf a_{m,k} =\mathbf R_k^\top(\dot{\mathbf v}k-\mathbf g) +\mathbf b{a,k}+\mathbf n_{a,k}. ωm,k=ωk+bg,k+ng,k,am,k=Rk⊤(v˙k−g)+ba,k+na,k.

加速度计输出的是比力。若 z 轴向上，世界系重力为 g = $0,0,-g₀$ ᵀ，水平静止时理想加速度计输出为 $0,0,g₀$ ᵀ。

1.2 初始化：所有增量相对于区间起点

在关键帧 i 处初始化：

A i = I , ν i = 0 , α i = 0 \boxed{ \mathbf A_i=\mathbf I,\qquad \boldsymbol\nu_i=\mathbf0,\qquad \boldsymbol\alpha_i=\mathbf0 } Ai=I,νi=0,αi=0

随后依次使用相邻样本 k、k+1 进行递推。关键帧时刻若不恰好位于 IMU 采样点，需要插值出精确的起止边界，避免漏掉第一段或多积分一段。

1.3 中值积分：角度、速度和位置怎样递推

以下把 h_k 简写为 h。在一个小步内，先扣除零偏线性化点：

ω ^ k = ω m , k − b ˉ g , a ^ k = a m , k − b ˉ a . \widehat{\boldsymbol\omega}k =\boldsymbol\omega{m,k}-\bar{\mathbf b}_g,\qquad \widehat{\mathbf a}k =\mathbf a{m,k}-\bar{\mathbf b}_a. ω k=ωm,k−bˉg,a k=am,k−bˉa.

第一步：对两端角速度取平均，更新相对旋转。

ϕ k = h 2 ( ω ^ k + ω ^ k + 1 ) , E k = Exp ⁡ ( ϕ k ) , \boldsymbol\phi_k =\frac h2 \left(\widehat{\boldsymbol\omega}k+ \widehat{\boldsymbol\omega}{k+1}\right),\qquad \mathbf E_k=\operatorname{Exp}(\boldsymbol\phi_k), ϕk=2h(ω k+ω k+1),Ek=Exp(ϕk),

A k + 1 = A k E k \boxed{\mathbf A_{k+1}=\mathbf A_k\mathbf E_k} Ak+1=AkEk

第二步：把两端加速度旋转到同一个起点坐标系，再取平均。

c k = 1 2 ( A k a ^ k + A k + 1 a ^ k + 1 ) \boxed{ \mathbf c_k= \frac12\left( \mathbf A_k\widehat{\mathbf a}k+ \mathbf A{k+1}\widehat{\mathbf a}_{k+1} \right) } ck=21(Aka k+Ak+1a k+1)

不能直接对两个时刻的机体系加速度求平均后随意使用同一个旋转，因为机体在两次采样之间可能已经转动。

第三步：用这一步的平均加速度更新速度和位置增量。

ν k + 1 = ν k + h c k , α k + 1 = α k + h ν k + 1 2 h 2 c k . \boxed{ \begin{aligned} \boldsymbol\nu_{k+1} &=\boldsymbol\nu_k+h\mathbf c_k,\\ \boldsymbol\alpha_{k+1} &=\boldsymbol\alpha_k+h\boldsymbol\nu_k +\frac12h^2\mathbf c_k. \end{aligned} } νk+1αk+1=νk+hck,=αk+hνk+21h2ck.

位置更新必须使用更新前的 ν_k。若先覆盖速度，再代入位置公式，会重复计入加速度项。

积分到关键帧 j 时，得到：

Δ R i j = A j , Δ v i j = ν j , Δ p i j = α j . \Delta\mathbf R_{ij}=\mathbf A_j,\qquad \Delta\mathbf v_{ij}=\boldsymbol\nu_j,\qquad \Delta\mathbf p_{ij}=\boldsymbol\alpha_j. ΔRij=Aj,Δvij=νj,Δpij=αj.

这里的"中值积分"具体指：角速度采用端点平均，两个端点旋转后的比力采用端点平均，再把平均比力视为本步常量。它是离散近似，不是任意运动下精确的连续积分。

1.4 预积分量如何连接两端的全局状态

忽略噪声并使用正确零偏时，两端状态近似满足：

R j = R i Δ R i j , v j = v i + g T + R i Δ v i j , p j = p i + v i T + 1 2 g T 2 + R i Δ p i j . \boxed{ \begin{aligned} \mathbf R_j&=\mathbf R_i\Delta\mathbf R_{ij},\\ \mathbf v_j&=\mathbf v_i+\mathbf gT +\mathbf R_i\Delta\mathbf v_{ij},\\ \mathbf p_j&=\mathbf p_i+\mathbf v_iT +\frac12\mathbf gT^2+\mathbf R_i\Delta\mathbf p_{ij}. \end{aligned} } Rjvjpj=RiΔRij,=vi+gT+RiΔvij,=pi+viT+21gT2+RiΔpij.

这也解释了为什么传统三维预积分的增量递推式中没有单独加入重力：增量累计的是旋转到 B_i 的比力，重力在连接世界系状态时显式加入。它不要求先把每条 IMU 测量转换到重力水平系；二维预积分中的倾斜补偿属于额外的降维处理。

1.5 从同一套递推式出发的两条求导路线

固定偏置，研究随机噪声的影响：得到误差递推式，再计算协方差，决定优化中的权重。
固定测量，研究偏置线性化点的变化：得到增量对偏置的雅可比，用于一阶修正。

协方差回答"这一段积分有多可信"；偏置雅可比回答"零偏改一点，积分结果怎样变"。二者来源相同、用途不同，都不能替代预积分增量本身。

2. 第一部分：与中值递推一致的误差状态和协方差传播

2.1 先固定误差定义和状态顺序

使用局部右乘旋转误差，速度、位置和偏置使用加性误差：

A k t r u e ≃ A k Exp ⁡ ( δ θ k ) , ν k t r u e = ν k + δ v k , α k t r u e = α k + δ p k . \mathbf A_k^{\rm true}\simeq \mathbf A_k\operatorname{Exp}(\delta\boldsymbol\theta_k), \qquad \boldsymbol\nu_k^{\rm true}=\boldsymbol\nu_k+\delta\mathbf v_k, \qquad \boldsymbol\alpha_k^{\rm true}=\boldsymbol\alpha_k+\delta\mathbf p_k. Aktrue≃AkExp(δθk),νktrue=νk+δvk,αktrue=αk+δpk.

记真实零偏与名义零偏之差为 δb_g、δb_a。误差状态统一按以下顺序排列：

δ z k = $δ θ k δ v k δ p k δ b g , k δ b a , k$ ∈ R 15 . \delta\mathbf z_k= \begin{bmatrix} \delta\boldsymbol\theta_k\\ \delta\mathbf v_k\\ \delta\mathbf p_k\\ \delta\mathbf b_{g,k}\\ \delta\mathbf b_{a,k} \end{bmatrix}\in\mathbb R^{15}. δzk= δθkδvkδpkδbg,kδba,k ∈R15.

本文的噪声符号采用前述测量模型：真实运动传播使用"测量 − 零偏 − 测量噪声"。因此噪声进入下方误差方程的符号，不能与"含噪测量减去无噪测量"的另一种误差定义混用。

2.2 由旋转递推式得到角度误差递推

SO(3) 的右雅可比记为：

J r ( ϕ ) = I − 1 − cos ⁡ s s 2 $ϕ$ × + s − sin ⁡ s s 3 $ϕ$ × 2 , s = ∥ ϕ ∥ . \mathcal J_r(\boldsymbol\phi) =\mathbf I- \frac{1-\cos s}{s^2} $\\boldsymbol\\phi$ \times+ \frac{s-\sin s}{s^3} $\\boldsymbol\\phi$ \times^2, \qquad s=\|\boldsymbol\phi\|. Jr(ϕ)=I−s21−coss $ϕ$ ×+s3s−sins $ϕ$ ×2,s=∥ϕ∥.

当 s 很小时使用级数展开，避免数值除零。它满足：

Exp ⁡ ( ϕ + δ ϕ ) ≃ Exp ⁡ ( ϕ ) Exp ⁡ ( J r ( ϕ ) δ ϕ ) . \operatorname{Exp}(\boldsymbol\phi+\delta\boldsymbol\phi) \simeq \operatorname{Exp}(\boldsymbol\phi) \operatorname{Exp}(\mathcal J_r(\boldsymbol\phi)\delta\boldsymbol\phi). Exp(ϕ+δϕ)≃Exp(ϕ)Exp(Jr(ϕ)δϕ).

为使后续矩阵更易读，定义：

K k = E k ⊤ , B k = − h J r ( ϕ k ) , D k = h 2 J r ( ϕ k ) . \mathbf K_k=\mathbf E_k^\top,\qquad \mathbf B_k=-h\mathcal J_r(\boldsymbol\phi_k),\qquad \mathbf D_k=\frac h2\mathcal J_r(\boldsymbol\phi_k). Kk=Ek⊤,Bk=−hJr(ϕk),Dk=2hJr(ϕk).

对第 1.3 节的旋转递推线性化，得到：

δ θ k + 1 = K k δ θ k + B k δ b g , k − D k ( n g , k + n g , k + 1 ) \boxed{ \delta\boldsymbol\theta_{k+1} =\mathbf K_k\delta\boldsymbol\theta_k +\mathbf B_k\delta\mathbf b_{g,k} -\mathbf D_k(\mathbf n_{g,k}+\mathbf n_{g,k+1}) } δθk+1=Kkδθk+Bkδbg,k−Dk(ng,k+ng,k+1)

角度误差不仅受陀螺零偏影响，也受原有角度误差和测量噪声影响。右乘误差下，偏置系数是 B_k；小角度时 B_k ≈ −hI，不能直接写成 −R_k h。

2.3 对平均加速度求导，得到完整状态转移矩阵

为简洁起见，下式中的 A_k、A_{k+1} 和去偏加速度均指名义递推值。利用：

δ ( A a ) ≃ − A $a$ × δ θ + A δ a , \delta(\mathbf A\mathbf a) \simeq-\mathbf A $\\mathbf a$ _\times\delta\boldsymbol\theta +\mathbf A\delta\mathbf a, δ(Aa)≃−A $a$ ×δθ+Aδa,

定义三个 3×3 分块：

U k = − 1 2 ( A k $a \^ k$ × + A k + 1 $a \^ k + 1$ × K k ) , V g , k = − 1 2 A k + 1 $a \^ k + 1$ × B k , V a , k = − 1 2 ( A k + A k + 1 ) . \begin{aligned} \mathbf U_k&=-\frac12 \left(\mathbf A_k $\\widehat{\\mathbf a}_k$ \times +\mathbf A{k+1} $\\widehat{\\mathbf a}_{k+1}$ \times\mathbf K_k\right),\\ \mathbf V{g,k}&=-\frac12 \mathbf A_{k+1} $\\widehat{\\mathbf a}_{k+1}$ \times\mathbf B_k,\\ \mathbf V{a,k}&=-\frac12(\mathbf A_k+\mathbf A_{k+1}). \end{aligned} UkVg,kVa,k=−21(Ak $a k$ ×+Ak+1 $a k+1$ ×Kk),=−21Ak+1 $a k+1$ ×Bk,=−21(Ak+Ak+1).

那么平均加速度误差可以写成：

δ c k = U k δ θ k + V g , k δ b g , k + V a , k δ b a , k + 噪声项 . \delta\mathbf c_k= \mathbf U_k\delta\boldsymbol\theta_k+ \mathbf V_{g,k}\delta\mathbf b_{g,k}+ \mathbf V_{a,k}\delta\mathbf b_{a,k} +\text{噪声项}. δck=Ukδθk+Vg,kδbg,k+Va,kδba,k+噪声项.

继续代入速度、位置递推，得到：

F k = $K k 0 0 B k 0 h U k I 0 h V g , k h V a , k 1 2 h 2 U k h I I 1 2 h 2 V g , k 1 2 h 2 V a , k 0 0 0 I 0 0 0 0 0 I$ \boxed{ \mathbf F_k= \begin{bmatrix} \mathbf K_k&0&0&\mathbf B_k&0\\ h\mathbf U_k&\mathbf I&0&h\mathbf V_{g,k}&h\mathbf V_{a,k}\\ \frac12h^2\mathbf U_k&h\mathbf I&\mathbf I& \frac12h^2\mathbf V_{g,k}&\frac12h^2\mathbf V_{a,k}\\ 0&0&0&\mathbf I&0\\ 0&0&0&0&\mathbf I \end{bmatrix} } Fk= KkhUk21h2Uk000IhI0000I00BkhVg,k21h2Vg,kI00hVa,k21h2Va,k0I

每个分块都是 3×3，F_k 为 15×15。第 3 行说明：本中值位置更新在同一步内就受到姿态和偏置误差影响，不能照搬"位置误差只受速度误差影响"的欧拉简化结论。

2.4 测量噪声和零偏随机游走怎样进入

每个原始采样点的测量噪声记为：

n k = $n g , k n a , k$ ∈ R 6 , Q k = Cov ⁡ ( n k ) . \mathbf n_k= \begin{bmatrix}\mathbf n_{g,k}\\\mathbf n_{a,k}\end{bmatrix} \in\mathbb R^6,\qquad \mathbf Q_k=\operatorname{Cov}(\mathbf n_k). nk= $ng,kna,k$ ∈R6,Qk=Cov(nk).

若已知每样本标准差，直接平方构造 Q_k；若给定连续白噪声强度，则须按实际采样模型换算为样本协方差。在均匀采样的区间平均等效模型下，Q_k = Q_c/h。不要把"每样本标准差"和"连续噪声密度"重复换算。

再定义：

M k = 1 2 A k + 1 $a \^ k + 1$ × D k . \mathbf M_k= \frac12\mathbf A_{k+1} $\\widehat{\\mathbf a}_{k+1}$ _\times\mathbf D_k. Mk=21Ak+1 $a k+1$ ×Dk.

两个端点的噪声输入矩阵分别为：

G 0 , k = $- D k 0 h M k - h 2 A k 1 2 h 2 M k - 1 4 h 2 A k 0 0 0 0$ , G 1 , k = $- D k 0 h M k - h 2 A k + 1 1 2 h 2 M k - 1 4 h 2 A k + 1 0 0 0 0$ . \mathbf G_{0,k}= \begin{bmatrix} -\mathbf D_k&0\\ h\mathbf M_k&-\frac h2\mathbf A_k\\ \frac12h^2\mathbf M_k&-\frac14h^2\mathbf A_k\\ 0&0\\ 0&0 \end{bmatrix},\qquad \mathbf G_{1,k}= \begin{bmatrix} -\mathbf D_k&0\\ h\mathbf M_k&-\frac h2\mathbf A_{k+1}\\ \frac12h^2\mathbf M_k&-\frac14h^2\mathbf A_{k+1}\\ 0&0\\ 0&0 \end{bmatrix}. G0,k= −DkhMk21h2Mk000−2hAk−41h2Ak00 ,G1,k= −DkhMk21h2Mk000−2hAk+1−41h2Ak+100 .

G₀、G₁ 均为 15×6。陀螺噪声会改变本步终点旋转，因此同一步也会影响速度、位置，不能只放进角度误差行。

零偏随机游走采用离散近似：

b g , k + 1 = b g , k + w g , k , b a , k + 1 = b a , k + w a , k . \mathbf b_{g,k+1}=\mathbf b_{g,k}+\mathbf w_{g,k}, \qquad \mathbf b_{a,k+1}=\mathbf b_{a,k}+\mathbf w_{a,k}. bg,k+1=bg,k+wg,k,ba,k+1=ba,k+wa,k.

其中 w_k = $w_g,kᵀ,w_a,kᵀ$ ᵀ 是本步零偏增量，独立于测量噪声，其协方差为：

Q w , k = h diag ⁡ ( Q b g , c , Q b a , c ) , H = $0 9 \times 6 I 6$ . \mathbf Q_{w,k}=h\operatorname{diag}(\mathbf Q_{bg,c},\mathbf Q_{ba,c}), \qquad \mathbf H= \begin{bmatrix} \mathbf0_{9\times6}\\ \mathbf I_6 \end{bmatrix}. Qw,k=hdiag(Qbg,c,Qba,c),H= $09\times6I6$ .

这里将零偏在本步内视为常量，在步末加入随机游走增量；更精确的连续随机过程离散化可以进一步细化，但应保持噪声定义一致。

完整的一阶误差递推为：

δ z k + 1 = F k δ z k + G 0 , k n k + G 1 , k n k + 1 + H w k \boxed{ \delta\mathbf z_{k+1} =\mathbf F_k\delta\mathbf z_k+ \mathbf G_{0,k}\mathbf n_k+ \mathbf G_{1,k}\mathbf n_{k+1}+ \mathbf H\mathbf w_k } δzk+1=Fkδzk+G0,knk+G1,knk+1+Hwk

2.5 协方差递推：中值积分要注意共享端点

协方差描述的是上述预积分误差和零偏变化误差的不确定性，不是最终全局位姿的协方差。

设原始 IMU 噪声跨采样点独立。相邻中值步仍然复用同一个样本：第 k−1 步已经使用过 n_k，所以 δz_k 与 n_k 相关。定义：

P k = Cov ⁡ ( δ z k ) , C k = Cov ⁡ ( δ z k , n k ) . \mathbf P_k=\operatorname{Cov}(\delta\mathbf z_k), \qquad \mathbf C_k=\operatorname{Cov}(\delta\mathbf z_k,\mathbf n_k). Pk=Cov(δzk),Ck=Cov(δzk,nk).

由上一节展开得到：

P k + 1 = F k P k F k ⊤ + G 0 , k Q k G 0 , k ⊤ + G 1 , k Q k + 1 G 1 , k ⊤ + H Q w , k H ⊤ + F k C k G 0 , k ⊤ + G 0 , k C k ⊤ F k ⊤ , C k + 1 = G 1 , k Q k + 1 . \boxed{ \begin{aligned} \mathbf P_{k+1}={}& \mathbf F_k\mathbf P_k\mathbf F_k^\top +\mathbf G_{0,k}\mathbf Q_k\mathbf G_{0,k}^\top +\mathbf G_{1,k}\mathbf Q_{k+1}\mathbf G_{1,k}^\top\\ &+\mathbf H\mathbf Q_{w,k}\mathbf H^\top +\mathbf F_k\mathbf C_k\mathbf G_{0,k}^\top +\mathbf G_{0,k}\mathbf C_k^\top\mathbf F_k^\top,\\ \mathbf C_{k+1}={}&\mathbf G_{1,k}\mathbf Q_{k+1}. \end{aligned} } Pk+1=Ck+1=FkPkFk⊤+G0,kQkG0,k⊤+G1,kQk+1G1,k⊤+HQw,kH⊤+FkCkG0,k⊤+G0,kCk⊤Fk⊤,G1,kQk+1.

在给定起点状态、起点零偏和预处理的条件下，初值取 P_i = 0、C_i = 0。起点零偏本身的不确定性由优化变量及其先验处理，不再重复塞进同一条件测量协方差。

常见简写 P_{k+1} = F_k P_k F_kᵀ + G_k Q_k G_kᵀ，要求本步新噪声与已有误差独立。若直接对共享端点的中值积分套用这个简式，就省略了相关项；也可以通过增广噪声状态等方式显式处理相关性。

实际传感器滤波、端点插值会带来额外相关性，需要按具体测量模型处理。以上公式严格对应本文声明的离散噪声假设。

2.6 9 维与 15 维协方差、信息矩阵和白化

只累计角度、速度、位置误差，且把区间偏置视为固定时，使用 9 维运动误差协方差。
本文同时传播零偏随机游走，使用 15 维联合误差协方差，与后文的 15 维残差对应。

不能用 9×9 协方差直接乘 15 维残差。采用"9 维运动因子 + 独立 bias 因子"的分拆时，也应说明是否忽略了运动误差与 bias 变化的相关性。

在小误差线性化下，取最终 P_j 作为对应残差坐标下的 Σ_ij，信息矩阵为 Σ_ij 的逆：

E I = r I ⊤ Σ i j − 1 r I . E_{\mathcal I}=\mathbf r_{\mathcal I}^\top \boldsymbol\Sigma_{ij}^{-1}\mathbf r_{\mathcal I}. EI=rI⊤Σij−1rI.

协方差越大，对应约束越弱；协方差越小，对应约束越强。非对角项保留不同误差分量之间的相关性。

若 Σ_ij 正定，令 Σ_ij = LLᵀ，则：

W = L − 1 , W ⊤ W = Σ i j − 1 , ∥ W r I ∥ 2 = E I . \mathbf W=\mathbf L^{-1},\qquad \mathbf W^\top\mathbf W=\boldsymbol\Sigma_{ij}^{-1}, \qquad \|\mathbf W\mathbf r_{\mathcal I}\|^2=E_{\mathcal I}. W=L−1,W⊤W=Σij−1,∥WrI∥2=EI.

在 Ceres 中通常返回白化后的残差 Wr。很短区间、缺少样本或噪声维度不足时，协方差可能奇异，不能直接求逆；简单给对角线加下限也不等同于完成了噪声标定。

3. 第二部分：偏置修正后的预积分残差

3.1 区分零偏线性化点与优化变量

预积分原始结果以 b̄_g、b̄_a 为线性化点计算。优化变量是关键帧 i、j 的位姿、速度和零偏：

X i = $R i , v i , p i , b g , i , b a , i$ , \mathbf X_i= $\\mathbf R_i,\\mathbf v_i,\\mathbf p_i,\\mathbf b_{g,i},\\mathbf b_{a,i}$ , Xi= $Ri,vi,pi,bg,i,ba,i$ ,

第 j 帧同理。定义：

δ b g = b g , i − b ˉ g , δ b a = b a , i − b ˉ a . \delta\mathbf b_g=\mathbf b_{g,i}-\bar{\mathbf b}g,\qquad \delta\mathbf b_a=\mathbf b{a,i}-\bar{\mathbf b}_a. δbg=bg,i−bˉg,δba=ba,i−bˉa.

使用第 4.1 节给出的偏置雅可比，得到修正后的测量：

Δ R i j c = Δ R ‾ i j Exp ⁡ ( J R g δ b g ) , Δ v i j c = Δ v ‾ i j + J v g δ b g + J v a δ b a , Δ p i j c = Δ p ‾ i j + J p g δ b g + J p a δ b a . \begin{aligned} \Delta\mathbf R^c_{ij} &=\overline{\Delta\mathbf R}{ij} \operatorname{Exp}(\mathbf J{Rg}\delta\mathbf b_g),\\ \Delta\mathbf v^c_{ij} &=\overline{\Delta\mathbf v}{ij} +\mathbf J{vg}\delta\mathbf b_g+\mathbf J_{va}\delta\mathbf b_a,\\ \Delta\mathbf p^c_{ij} &=\overline{\Delta\mathbf p}{ij} +\mathbf J{pg}\delta\mathbf b_g+\mathbf J_{pa}\delta\mathbf b_a. \end{aligned} ΔRijcΔvijcΔpijc=ΔRijExp(JRgδbg),=Δvij+Jvgδbg+Jvaδba,=Δpij+Jpgδbg+Jpaδba.

这些式子每次都从保存的原始预积分结果和同一个线性化点计算，避免把多次修正重复叠加。它们是局部一阶模型，偏置变化过大时需要重新积分。

3.2 三个运动残差与两个零偏变化残差

r R = Log ⁡ ( ( Δ R i j c ) ⊤ R i ⊤ R j ) , r v = R i ⊤ ( v j − v i − g T ) − Δ v i j c , r p = R i ⊤ ( p j − p i − v i T − 1 2 g T 2 ) − Δ p i j c , r b g = b g , j − b g , i , r b a = b a , j − b a , i . \boxed{ \begin{aligned} \mathbf r_R&= \operatorname{Log}\left((\Delta\mathbf R^c_{ij})^\top \mathbf R_i^\top\mathbf R_j\right),\\ \mathbf r_v&= \mathbf R_i^\top(\mathbf v_j-\mathbf v_i-\mathbf gT) -\Delta\mathbf v^c_{ij},\\ \mathbf r_p&= \mathbf R_i^\top \left(\mathbf p_j-\mathbf p_i-\mathbf v_iT-\frac12\mathbf gT^2\right) -\Delta\mathbf p^c_{ij},\\ \mathbf r_{bg}&=\mathbf b_{g,j}-\mathbf b_{g,i},\\ \mathbf r_{ba}&=\mathbf b_{a,j}-\mathbf b_{a,i}. \end{aligned} } rRrvrprbgrba=Log((ΔRijc)⊤Ri⊤Rj),=Ri⊤(vj−vi−gT)−Δvijc,=Ri⊤(pj−pi−viT−21gT2)−Δpijc,=bg,j−bg,i,=ba,j−ba,i.

前三行分别比较"状态推算出的相对旋转、速度变化、位置变化"和"偏置修正后的预积分测量"。后两行约束两端零偏变化，其允许幅度由随机游走协方差决定，不是强制两端 bias 完全相等。

残差顺序与第 2 节误差状态保持一致：

r I = $r R ⊤ , r v ⊤ , r p ⊤ , r b g ⊤ , r b a ⊤$ ⊤ ∈ R 15 . \mathbf r_{\mathcal I} = $\\mathbf r_R\^\\top,\\mathbf r_v\^\\top,\\mathbf r_p\^\\top, \\mathbf r_{bg}\^\\top,\\mathbf r_{ba}\^\\top$ ^\top \in\mathbb R^{15}. rI= $rR⊤,rv⊤,rp⊤,rbg⊤,rba⊤$ ⊤∈R15.

量	在优化中的身份
保存的原始增量、线性化点、偏置雅可比	本次线性化下的已知测量数据
修正后的三个增量	通过起点 bias 依赖优化变量
两端的姿态、速度、位置、bias	待优化状态
g、T	本文设为已知常量
Σ_ij	与残差维度、顺序和误差坐标一致的协方差权重

传统 IMU 因子本身仍有全局参考自由度，实际系统还需视觉、激光、先验等约束，才能估计相应状态。

4. 第三部分：偏置雅可比与残差雅可比

4.1 预积分对偏置的雅可比：从第 1.3 节逐步求导

先区分两类雅可比：

本节 J_Rg、J_vg 等描述预积分增量对偏置的变化率，用于修正测量。
第 4.2 节描述残差对所有优化状态的变化率，用于构造求解器的线性方程。

定义旋转偏置雅可比的右乘形式：

A k ( b ˉ g + δ b g ) ≃ A k ( b ˉ g ) Exp ⁡ ( J R g , k δ b g ) . \mathbf A_k(\bar{\mathbf b}_g+\delta\mathbf b_g) \simeq\mathbf A_k(\bar{\mathbf b}g) \operatorname{Exp}(\mathbf J{Rg,k}\delta\mathbf b_g). Ak(bˉg+δbg)≃Ak(bˉg)Exp(JRg,kδbg).

速度、位置偏置雅可比分别为 J_vg、J_va、J_pg、J_pa。区间起点处，这五个 3×3 矩阵均为零。

使用第 2 节的 K、B、U、V_g、V_a，直接对中值递推应用链式法则，得到：

J R g , k + 1 = K k J R g , k + B k , J v g , k + 1 = J v g , k + h ( U k J R g , k + V g , k ) , J v a , k + 1 = J v a , k + h V a , k , J p g , k + 1 = J p g , k + h J v g , k + 1 2 h 2 ( U k J R g , k + V g , k ) , J p a , k + 1 = J p a , k + h J v a , k + 1 2 h 2 V a , k . \boxed{ \begin{aligned} \mathbf J_{Rg,k+1} &=\mathbf K_k\mathbf J_{Rg,k}+\mathbf B_k,\\ \mathbf J_{vg,k+1} &=\mathbf J_{vg,k} +h(\mathbf U_k\mathbf J_{Rg,k}+\mathbf V_{g,k}),\\ \mathbf J_{va,k+1} &=\mathbf J_{va,k}+h\mathbf V_{a,k},\\ \mathbf J_{pg,k+1} &=\mathbf J_{pg,k}+h\mathbf J_{vg,k} +\frac12h^2(\mathbf U_k\mathbf J_{Rg,k}+\mathbf V_{g,k}),\\ \mathbf J_{pa,k+1} &=\mathbf J_{pa,k}+h\mathbf J_{va,k} +\frac12h^2\mathbf V_{a,k}. \end{aligned} } JRg,k+1Jvg,k+1Jva,k+1Jpg,k+1Jpa,k+1=KkJRg,k+Bk,=Jvg,k+h(UkJRg,k+Vg,k),=Jva,k+hVa,k,=Jpg,k+hJvg,k+21h2(UkJRg,k+Vg,k),=Jpa,k+hJva,k+21h2Va,k.

右侧全部使用第 k 步更新前的矩阵。陀螺 bias 不仅改变旋转，还会改变加速度的旋转方向，所以 J_vg、J_pg 一般不为零。

也可以从完整状态转移矩阵统一累计：

S i = $0 9 \times 6 I 6$ , S k + 1 = F k S k . \mathbf S_i= \begin{bmatrix}\mathbf0_{9\times6}\\\mathbf I_6\end{bmatrix}, \qquad \mathbf S_{k+1}=\mathbf F_k\mathbf S_k. Si= $09\times6I6$ ,Sk+1=FkSk.

S 的前 9 行即上述运动增量对两类 bias 的雅可比。注意这里累计的是确定性敏感度；协方差传播还要另外考虑随机噪声及共享端点相关性。

4.2 残差对两端状态的雅可比

优化时，对姿态采用右乘扰动：

R i ← R i Exp ⁡ ( δ θ i ) , R j ← R j Exp ⁡ ( δ θ j ) . \mathbf R_i\leftarrow \mathbf R_i\operatorname{Exp}(\delta\boldsymbol\theta_i), \qquad \mathbf R_j\leftarrow \mathbf R_j\operatorname{Exp}(\delta\boldsymbol\theta_j). Ri←RiExp(δθi),Rj←RjExp(δθj).

位置、速度和 bias 采用各自坐标系中的加性更新。定义：

M = R i ⊤ R j , E = ( Δ R i j c ) ⊤ M , u = R i ⊤ ( v j − v i − g T ) , w = R i ⊤ ( p j − p i − v i T − 1 2 g T 2 ) , η = J R g δ b g . \begin{aligned} \mathbf M&=\mathbf R_i^\top\mathbf R_j,\\ \mathbf E&=(\Delta\mathbf R^c_{ij})^\top\mathbf M,\\ \mathbf u&=\mathbf R_i^\top(\mathbf v_j-\mathbf v_i-\mathbf gT),\\ \mathbf w&=\mathbf R_i^\top (\mathbf p_j-\mathbf p_i-\mathbf v_iT-\tfrac12\mathbf gT^2),\\ \boldsymbol\eta&=\mathbf J_{Rg}\delta\mathbf b_g. \end{aligned} MEuwη=Ri⊤Rj,=(ΔRijc)⊤M,=Ri⊤(vj−vi−gT),=Ri⊤(pj−pi−viT−21gT2),=JRgδbg.

下文的右雅可比逆始终在旋转残差 r_R = Log(E) 处求值。

旋转残差的三个非零分块为：

∂ r R ∂ δ θ i = − J r − 1 ( r R ) M ⊤ , ∂ r R ∂ δ θ j = J r − 1 ( r R ) , ∂ r R ∂ b g , i = − J r − 1 ( r R ) E ⊤ J r ( η ) J R g . \boxed{ \begin{aligned} \frac{\partial\mathbf r_R}{\partial\delta\boldsymbol\theta_i} &=-\mathcal J_r^{-1}(\mathbf r_R)\mathbf M^\top,\\ \frac{\partial\mathbf r_R}{\partial\delta\boldsymbol\theta_j} &=\mathcal J_r^{-1}(\mathbf r_R),\\ \frac{\partial\mathbf r_R}{\partial\mathbf b_{g,i}} &=-\mathcal J_r^{-1}(\mathbf r_R)\mathbf E^\top \mathcal J_r(\boldsymbol\eta)\mathbf J_{Rg}. \end{aligned} } ∂δθi∂rR∂δθj∂rR∂bg,i∂rR=−Jr−1(rR)M⊤,=Jr−1(rR),=−Jr−1(rR)E⊤Jr(η)JRg.

最后一项是对"第 3.1 节一阶 bias 修正模型"的链式求导；η = 0 时，其中的 J_r(η) = I。

残差	对 δθ_i	对 v_i	对 p_i	对 b_g,i	对 b_a,i
r_R	见上式	0	0	见上式	0
r_v	$u$ _×	−R_iᵀ	0	−J_vg	−J_va
r_p	$w$ _×	−R_iᵀ T	−R_iᵀ	−J_pg	−J_pa
r_bg	0	0	0	−I	0
r_ba	0	0	0	0	−I

残差	对 δθ_j	对 v_j	对 p_j	对 b_g,j	对 b_a,j
r_R	J_r⁻¹(r_R)	0	0	0	0
r_v	0	R_iᵀ	0	0	0
r_p	0	0	R_iᵀ	0	0
r_bg	0	0	0	I	0
r_ba	0	0	0	0	I

速度、位置残差中，预积分测量前有负号，因此其 bias 导数也是相应预积分偏置雅可比的负值，不能把两类雅可比直接写成相同符号。

旋转残差接近零，只能推出 J_r⁻¹(r_R) ≈ I；不能因此把对 θ_i 的导数直接简化成 −I，因为 Mᵀ 是真实相对旋转，未必接近单位阵。也不能把本 SO(3) 对数残差的导数与某个工程中的四元数近似残差逐项等同。

4.3 白化后再进入求解器

设上表组成原始残差雅可比 J。协方差权重在一次线性化中固定，则：

r w = W r , J w = W J . \mathbf r_w=\mathbf W\mathbf r,\qquad \mathbf J_w=\mathbf W\mathbf J. rw=Wr,Jw=WJ.

高斯---牛顿方程写成：

J w ⊤ J w δ x = − J w ⊤ r w . \mathbf J_w^\top\mathbf J_w\,\delta\mathbf x =-\mathbf J_w^\top\mathbf r_w. Jw⊤Jwδx=−Jw⊤rw.

使用 Ceres 自动求导时，仍需在残差函数内实现正确的 bias 修正和白化。自动求导不会修复错误的模型、符号、坐标约定或协方差。

5. 总代价函数与完整优化流程

实际 VIO/LIO 系统一般联合使用 IMU、视觉或激光等约束：

min ⁡ X $\sum ( i , j ) \in I ∥ W i j r I , i j ∥ 2 + E v i s u a l / l i d a r ( X ) + E p r i o r ( X )$ . \min_{\mathbf X} \left $\\sum_{(i,j)\\in\\mathcal I} \\\|\\mathbf W_{ij}\\mathbf r_{\\mathcal I,ij}\\\|\^2 +E_{\\rm visual/lidar}(\\mathbf X)+E_{\\rm prior}(\\mathbf X) \\right$ . Xmin (i,j)∈I∑∥WijrI,ij∥2+Evisual/lidar(X)+Eprior(X) .

实现时可以按以下顺序组织：

区间初始化：确定起止时刻、bias 线性化点、噪声参数；初始化 A = I、ν = 0、α = 0、P = 0、偏置雅可比为零。
逐样本预积分：按第 1.3 节更新旋转、速度和位置增量，同时按第 2 节传播协方差、按第 4.1 节累计偏置雅可比。各式中的"旧值"必须在覆盖前保存。
结算关键帧区间：保存三个原始增量、积分时长、bias 线性化点、偏置雅可比和误差协方差。保留下一段所需的边界 IMU 样本。
构造优化因子：用当前 bias 从原始增量计算修正量，构造第 3 节残差，按相同的维度和顺序白化。
求解并更新状态：联合其他传感器约束优化位姿、速度和 bias；bias 小幅变化使用一阶修正，过大则按新的线性化点重积分。

**递推的是增量，不是只递推 F、G 就会自动得到增量。**F、G 是从增量递推式求导得到的辅助矩阵；它们分别服务于敏感度和不确定性传播。

6. 自检与容易混淆的地方

6.1 三个基础自检

水平静止：若 R_i = I、g = $0,0,-g₀$ ᵀ，且已去偏的 IMU 比力为 $0,0,g₀$ ᵀ，则 Δv = $0,0,g₀T$ ᵀ、Δp = $0,0,g₀T²/2$ ᵀ。代入第 1.4 节后，真实速度和位置仍不变。预积分比力增量非零，并不表示机器人在运动。
无转动、常值比力：应得到 Δv = aT、Δp = aT²/2。此时对加速度 bias 的雅可比为 J_va = −TI、J_pa = −T²I/2。
纯转动：核对旋转乘法顺序、右扰动定义和残差正负号。对解析雅可比做中心有限差分时，旋转差应使用 Log(R_nomᵀ R_perturbed)，不能对旋转矩阵元素直接相减代替角度误差。

6.2 两类雅可比和协方差分别负责什么

对象	计算什么	在优化中做什么
增量 ΔR、Δv、Δp	这段 IMU 给出的相对运动测量	构造运动残差
增量对 bias 的雅可比	偏置改变后增量怎样改变	一阶 bias 修正
预积分误差协方差	测量噪声和所建模 bias 漂移怎样累积	构造信息矩阵、白化残差
残差对状态的雅可比	调整位姿、速度、bias 会怎样改变残差	求解状态增量

协方差传播依赖明确的传感器噪声模型。由递推式算出了一个矩阵，不等于真实传感器已经完成标定；没有把倾斜、外参、同步等误差纳入模型时，也不能把它理解为包含所有误差来源的置信度。

6.3 本文公式的验证范围

本文新增的中值状态转移矩阵、两个端点噪声矩阵、整段偏置雅可比及旋转残差雅可比，均做了中心有限差分检查；最大绝对误差小于 7×10⁻¹⁰。共享端点协方差递推也与"对全部原始噪声构造总敏感度矩阵后直接传播"的结果一致，最大绝对差小于 9×10⁻²⁰。

这些检查验证的是所声明离散模型的一阶代数一致性，不替代真实 IMU 噪声标定、连续模型误差评估或数据集精度验证。

7. 参考资料与实现约定

Forster 等：On-Manifold Preintegration for Real-Time Visual-Inertial Odometry：预积分、噪声传播、bias 一阶修正及因子构建的理论依据。
VINS-Mono：integration_base.h：端点平均预积分的工程参考。
VINS-Mono：imu_factor.h：残差、白化和优化接口的工程参考。

本文采用 SO(3) 指数映射、局部右乘误差、θ/v/p/bg/ba 的状态顺序，以及显式共享端点相关性处理。VINS-Mono 源码的状态排列、四元数更新、线性化近似和噪声处理并不与本文逐项相同；对照代码时应先统一约定，不能只按矩阵行列位置照抄。