SLAM之——卡尔曼滤波算法之带 SO(3) 姿态 + IMU 预积分（IMU preintegration）的误差状态 EKF（Error-State EKF）示例

相关内容地址：SLAM之——卡尔曼滤波算法（ KF）之扩展卡尔曼滤波（EKF, Extended Kalman Filter）深度详解

一、符号与约定

• 使用右侧乘法/右扰动约定：

  • 旋转矩阵表示R ∈ S O ( 3 ) R\in SO(3)R∈SO(3)将机体系向量变换到惯性系：v I = R v B v_I = R v_BvI​=RvB​。
  • 角速度ω \omegaω表示机体系测得的角速度（body frame）。
  • 旋转微扰以右乘小旋转表达：真正的旋转R = R ^ exp ⁡ ( [ δ θ ] × ) R = \hat R \exp([\delta\theta]\times)R=R^exp([δθ]×)，其中δ θ ∈ R 3 \delta\theta\in\mathbb R^3δθ∈R3（切空间的误差向量），[ ⋅ ] × [\cdot]\times[⋅]×是反对称矩阵。

• 四元数也可用，但此处用S O ( 3 ) SO(3)SO(3)矩阵和指数映射表示以便导数清晰。

• 重力向量用g gg（在惯性系中，g = [ 0 , 0 , − 9.81 ] ⊤ g=[0,0,-9.81]^\topg=[0,0,−9.81]⊤）。

• IMU 测量模型（离散时间采样Δ t \Delta tΔt）：

  • 测量的角速度（带偏置与噪声）：ω m = ω + b g + n g \omega_m = \omega + b_g + n_gωm​=ω+bg​+ng​。
  • 测量的线加速度（机体系）：a m = a + b a + n a a_m = a + b_a + n_aam​=a+ba​+na​，其中a aa是机体在机体坐标下去重力的加速度（实际关系见下）。

二、状态与误差定义

1. 全状态（估计值）

时刻k kk的全状态（augmented）可取为：
x k = [ R k p k v k b g , k b a , k ] , x_k = \begin{bmatrix} R_k & p_k & v_k & b_{g,k} & b_{a,k} \end{bmatrix},xk​=[Rk​​pk​​vk​​bg,k​​ba,k​​],
其中：

• R k ∈ S O ( 3 ) R_k\in SO(3)Rk​∈SO(3)：机体到惯性系的旋转；
• p k ∈ R 3 p_k\in\mathbb R^3pk​∈R3：惯性系下位置；
• v k ∈ R 3 v_k\in\mathbb R^3vk​∈R3：惯性系下速度；
• b g , k ∈ R 3 b_{g,k}\in\mathbb R^3bg,k​∈R3：陀螺偏置；
• b a , k ∈ R 3 b_{a,k}\in\mathbb R^3ba,k​∈R3：加速度计偏置。

2. 误差状态（在切空间或者manifold空间）

manifold流行空间参考：SLAM文献之A micro Lie theory for state estimation in robotic（1）

我们定义误差状态（局部）为：
δ x = [ δ θ δ p δ v δ b g δ b a ] ∈ R 15 , \delta x = \begin{bmatrix} \delta\theta \ \delta p \ \delta v \ \delta b_g \ \delta b_a \end{bmatrix}\in\mathbb R^{15},δx=[δθδpδvδbg​δba​​]∈R15,
其中δ θ ∈ R 3 \delta\theta\in\mathbb R^3δθ∈R3是右扰动（so(3) 的向量），使得真旋转R = R ^ exp ⁡ ( [ δ θ ] × ) R = \hat R \exp([\delta\theta]_\times)R=R^exp([δθ]×​)；其余分量为欧氏向量差（位置、速度、偏置）。

状态协方差定义在误差空间上：P = E [ δ x , δ x ⊤ ] P = \mathbb{E}[\delta x,\delta x^\top]P=E[δx,δx⊤]。

三、IMU 预积分：连续到离散与线性化

IMU 预积分的思路：在两次关键帧（或滤波更新时刻）i ii和j jj之间，对 IMU 测量进行积分，得到相对增量( Δ R i j , Δ v i j , Δ p i j ) (\Delta R_{ij}, \Delta v_{ij}, \Delta p_{ij})(ΔRij​,Δvij​,Δpij​)，它们在估计偏置的基础上被线性化，并保留关于偏置的一阶雅可比。

1. 连续推进的理想无噪模型（离散化）

在理想无噪无偏条件下（使用去偏测量），从t i t_iti​到t j t_jtj​（时长Δ t i j \Delta t_{ij}Δtij​）连续积分得到：

• 相对旋转：
  R ˙ ( t ) = R ( t ) [ ω ( t ) ] × . \dot R(t) = R(t) [\omega(t)]_\times.R˙(t)=R(t)[ω(t)]×​.
• 速度（惯性系）：
  v ˙ I ( t ) = R ( t ) a ( t ) + g . \dot v_I(t) = R(t) a(t) + g.v˙I​(t)=R(t)a(t)+g.
• 位置：
  p ˙ I ( t ) = v I ( t ) . \dot p_I(t) = v_I(t).p˙​I​(t)=vI​(t).

离散化后的预积分定义（在小步法下）：

• Δ R i j ≜ R i ⊤ R j \Delta R_{ij} \triangleq R_i^\top R_jΔRij​≜Ri⊤​Rj​（估计的相对旋转）
• Δ v i j ≜ R i ⊤ ( v j − v i − g Δ t i j ) \Delta v_{ij} \triangleq R_i^\top (v_j - v_i - g\Delta t_{ij})Δvij​≜Ri⊤​(vj​−vi​−gΔtij​)
• Δ p i j ≜ R i ⊤ ( p j − p i − v i Δ t i j − 1 2 g Δ t i j 2 ) \Delta p_{ij} \triangleq R_i^\top (p_j - p_i - v_i \Delta t_{ij} - \tfrac12 g\Delta t_{ij}^2)Δpij​≜Ri⊤​(pj​−pi​−vi​Δtij​−21​gΔtij2​)

这些量可从原始 IMU 测量逐步累积得到（参见微分推进公式）。

2. 预积分的离散递推（常用 Euler / mid-point）

在每个小时间步δ t \delta tδt用测量( ω m , a m ) (\omega_m, a_m)(ωm​,am​)（已减去当前偏置估计b ^ g , b ^ a \hat b_g,\hat b_ab^g​,b^a​）做更新，例如 mid-point：

• 步k → k + 1 k\to k+1k→k+1（短时）：

  • 计算去偏角速度：ω ~ = ω m − b ^ g \tilde\omega = \omega_m - \hat b_gω~=ωm​−b^g​。
  • δ R ← δ R exp ⁡ ( [ ω ~ δ t ] × ) \delta R \leftarrow \delta R \exp([\tilde\omega \delta t]_\times)δR←δRexp([ω~δt]×​)。
  • 更新速度增量：δ v ← δ v + δ R ( a ~ ) δ t \delta v \leftarrow \delta v + \delta R (\tilde a)\delta tδv←δv+δR(a~)δt，其中a ~ = a m − b ^ a \tilde a = a_m - \hat b_aa~=am​−b^a​。
  • 更新位置增量:δ p ← δ p + δ v δ t + 1 2 δ R a ~ δ t 2 \delta p \leftarrow \delta p + \delta v \delta t + \tfrac12 \delta R\tilde a\delta t^2δp←δp+δvδt+21​δRa~δt2（或更稳定的中点法）。

实际实现里通常使用mid-point 或 RK2/RK4提升精度。

3. 预积分的偏置雅可比（必需）

为了在后续滤波中把偏置不确定性与偏置更新影响纳入，需要保存雅可比矩阵：

• J Δ R b g ∈ R 3 × 3 J_{\Delta Rb_g}\in\mathbb R^{3\times3}JΔRbg​​∈R3×3：∂ Log ⁡ ( Δ R ) / ∂ b g \partial \operatorname{Log}(\Delta R)/\partial b_g∂Log(ΔR)/∂bg​的线性化，
• J Δ v b g , J Δ v b a ∈ R 3 × 3 J_{\Delta vb_g}, J_{\Delta vb_a}\in\mathbb R^{3\times3}JΔvbg​​,JΔvba​​∈R3×3，
• J Δ p b g , J Δ p b a ∈ R 3 × 3 J_{\Delta pb_g}, J_{\Delta pb_a}\in\mathbb R^{3\times3}JΔpbg​​,JΔpba​​∈R3×3。

这些雅可比可以在预积分递推中逐步更新（有标准递推公式，见下）。原则上，预积分把Δ R , Δ v , Δ p \Delta R,\Delta v,\Delta pΔR,Δv,Δp用当前偏置线性化为：
Δ R ( b g ) ≈ Δ R ( b ^ g ) exp ⁡ ( J Δ R , b g ( b g − b ^ g ) ) , \Delta R(b_g) \approx \Delta R(\hat b_g)\exp\big(J_{\Delta R,b_g} (b_g-\hat b_g)\big),ΔR(bg​)≈ΔR(b^g​)exp(JΔR,bg​​(bg​−b^g​)),
Δ v ( b g , b a ) ≈ Δ v ( b ^ ) + J Δ v b g ( b g − b ^ g ) + J Δ v b a ( b a − b ^ a ) , \Delta v(b_g,b_a) \approx \Delta v(\hat b) + J_{\Delta vb_g}(b_g-\hat b_g) + J_{\Delta vb_a}(b_a-\hat b_a),Δv(bg​,ba​)≈Δv(b^)+JΔvbg​​(bg​−b^g​)+JΔvba​​(ba​−b^a​),
Δ p ( b g , b a ) ≈ Δ p ( b ^ ) + J Δ p b g ( b g − b ^ g ) + J Δ p b a ( b a − b ^ a ) . \Delta p(b_g,b_a) \approx \Delta p(\hat b) + J_{\Delta pb_g}(b_g-\hat b_g) + J_{\Delta pb_a}(b_a-\hat b_a).Δp(bg​,ba​)≈Δp(b^)+JΔpbg​​(bg​−b^g​)+JΔpba​​(ba​−b^a​).

（实现上将这些雅可比以递推形式累积，参见 Forster 等人的 Preintegration 论文或 IMU library。）

4. 预积分噪声协方差

预积分还要累积测量噪声（IMU 白噪声、随机游走等）到一个3 × 3 3\times33×3或9 × 9 9\times99×9的预积分协方差矩阵：
Σ Δ = Cov ( [ Log ⁡ ( Δ R ) , Δ v , Δ p ] ⊤ ) . \Sigma_{\Delta} = \text{Cov}\big([\operatorname{Log}(\Delta R), \Delta v, \Delta p]^\top\big).ΣΔ​=Cov([Log(ΔR),Δv,Δp]⊤).
该矩阵在递推中通过线性化映射G GG累积 IMU 噪声。

四、误差状态的预测（利用预积分）

当我们用预积分从关键时刻i ii传播到j jj，并且用当前估计偏置（b ^ g i , b ^ a i \hat b_{gi},\hat b_{ai}b^gi​,b^ai​）进行预积分，预测模型（观测模型形式）为：

州态预测（基于估计）：
R ^ j = R ^ i Δ R i j ( b ^ i ) \hat R_j = \hat R_i \Delta R_{ij}(\hat b_i) \R^j​=R^i​ΔRij​(b^i​)
v ^ j = v ^ i + g Δ t i j + R ^ i Δ v i j ( b ^ i , b ^ a i ) \hat v_j = \hat v_i + g \Delta t_{ij} + \hat R_i \Delta v_{ij}(\hat b_i,\hat b_{ai})v^j​=v^i​+gΔtij​+R^i​Δvij​(b^i​,b^ai​)
p ^ j = p ^ i + v ^ i Δ t i j + 1 2 g Δ t i j 2 + R ^ i Δ p i j ( b ^ i , b ^ a i ) \hat p_j = \hat p_i + \hat v_i \Delta t_{ij} + \tfrac12 g \Delta t_{ij}^2 + \hat R_i \Delta p_{ij}(\hat b_i,\hat b_{ai})p^​j​=p^​i​+v^i​Δtij​+21​gΔtij2​+R^i​Δpij​(b^i​,b^ai​)
偏置预测（若在无模型假设下常用随机游走）：
b ^ g j = b ^ g i , b ^ a j = b ^ a i \hat b_{gj} = \hat b_{gi},\qquad \hat b_{aj} = \hat b_{ai}b^gj​=b^gi​,b^aj​=b^ai​

误差变换线性化（构造F FF）

把误差δ x i \delta x_iδxi​在时刻i ii线性传播到时刻j jj，可写成：

δ x j ≈ F δ x i + G n \delta x_j \approx F\delta x_i + Gnδxj​≈Fδxi​+Gn

其中n nn包括 IMU 测量噪声（角速噪声n g n_gng​、加速度噪声n a n_ana​）和偏置随机游走噪声（如果建模的话）。矩阵F ∈ R 15 × 15 F\in\mathbb R^{15\times15}F∈R15×15与G ∈ R 15 × m G\in\mathbb R^{15\times m}G∈R15×m（m mm为噪声维度）来自对上述非线性传播方程关于误差的线性化；一个常见的块结构（近似）如下：

写误差顺序为[ δ θ , δ p , δ v , δ b g , δ b a ] [\delta\theta,\ \delta p,\ \delta v,\ \delta b_g,\ \delta b_a][δθ,δp,δv,δbg​,δba​]，则线性化近似（示意）：

F ≈ [ I 3 0 0 − I 3 Δ t ω → θ 0 F p θ I 3 F p v F p b g F p b a F v θ 0 I 3 F v b g F v b a 0 0 0 I 3 0 0 0 0 0 I 3 ] . F \approx \begin{bmatrix} I_3 & 0 & 0 & -I_3 \Delta t_{\omega\to\theta} & 0 \\ F_{p\theta} & I_3 & F_{pv} & F_{pb_g} & F_{pb_a} \\ F_{v\theta} & 0 & I_3 & F_{vb_g} & F_{vb_a} \\ 0 & 0 & 0 & I_3 & 0 \\ 0 & 0 & 0 & 0 & I_3 \end{bmatrix}.F≈​I3​Fpθ​Fvθ​00​0I3​000​0Fpv​I3​00​−I3​Δtω→θ​Fpbg​​Fvbg​​I3​0​0Fpba​​Fvba​​0I3​​​.

更具体的每块（在离散预积分上下文）常见写法（Forster 等，Lupton 等）：

• F v θ = − R ^ i [ Δ v i j ] × F_{v\theta} = -\hat R_i [\Delta v_{ij}]_\timesFvθ​=−R^i​[Δvij​]×​（或相似项，取决于约定）
• F p θ = − R ^ i [ Δ p i j ] × F_{p\theta} = -\hat R_i [\Delta p_{ij}]_\timesFpθ​=−R^i​[Δpij​]×​
• F v b g = − R ^ i J Δ v , b g F_{vb_g} = -\hat R_i J_{\Delta v,b_g}Fvbg​​=−R^i​JΔv,bg​​
• F p b g = − R ^ i J Δ p , b g F_{pb_g} = -\hat R_i J_{\Delta p,b_g}Fpbg​​=−R^i​JΔp,bg​​
• F v b a = − R ^ i J Δ v , b a F_{vb_a} = -\hat R_iJ_{\Delta v,b_a}Fvba​​=−R^i​JΔv,ba​​等

注：准确的F FF元素要看用的预积分累积方程（midpoint / first-order）及扰动约定；常用参考为 Forster et al., “IMU Preintegration on Manifold” (RSS 2015) —— 该文给出了详细递推式。

协方差传播

用线性化结果更新协方差：
P j = F P i F ⊤ + G Q n G ⊤ , P_j = F P_i F^\top + G Q_n G^\top,Pj​=FPi​F⊤+GQn​G⊤,
其中Q n Q_nQn​是 IMU 噪声（角速噪声、加速度噪声、偏置随机游走等）在离散步上的协方差，G GG将这些噪声映射到误差状态。

五、测量更新（以外部位姿测量为例）

假设在时刻j jj有一个外部位姿测量（例如来自视觉 or LiDAR 构建的位姿约束），测量模型可写成（以i ii为参考）：

• 测量提供相对或绝对位姿。以绝对位姿量测( R ˉ j , p ˉ j ) (\bar R_j,\bar p_j)(Rˉj​,pˉ​j​)为例，其观测残差在误差空间为：

  • 旋转残差（右扰动）：
    r R = Log ⁡ ( R ˉ j ⊤ R ^ j ) ≈ R ^ j ⊤ ( R ^ j exp ⁡ ( [ δ θ j ] × ) ) ≈ δ θ j ( 线性近似 ) r_R = \operatorname{Log}\big(\bar R_j^\top \hat R_j\big)\approx \hat R_j^\top(\hat R_j \exp([\delta\theta_j]_\times)) \approx \delta\theta_j \quad (\text{线性近似})rR​=Log(Rˉj⊤​R^j​)≈R^j⊤​(R^j​exp([δθj​]×​))≈δθj​(线性近似)
    更常用写法，量测模型给出
    r R ≈ δ θ j + n R r_R \approx \delta\theta_j + n_RrR​≈δθj​+nR​
  • 位置残差：
    r p = p ˉ j − p ^ j ≈ − δ p j + n p r_p = \bar p_j - \hat p_j \approx -\delta p_j + n_prp​=pˉ​j​−p^​j​≈−δpj​+np​

合并观测矩阵H HH（将误差态映射到观测残差）例如：
r = [ r R r p ] ≈ H δ x j + n z , r = \begin{bmatrix} r_R \ r_p \end{bmatrix} \approx H\delta x_j + n_z,r=[rR​rp​​]≈Hδxj​+nz​,
其中H HH取形如下（若量测只与R , p R,pR,p相关）：
H = [ I 3 0 0 0 0 0 − I 3 0 0 0 ] . H = \begin{bmatrix} I_3 & 0 & 0 & 0 & 0 \\ 0 & -I_3 & 0 & 0 & 0 \end{bmatrix}.H=[I3​0​0−I3​​00​00​00​].

然后 EKF 标准更新：

• 卡尔曼增益：
  K = P j H ⊤ ( H P j H ⊤ + R z ) − 1 K = P_j H^\top (H P_j H^\top + R_z)^{-1}K=Pj​H⊤(HPj​H⊤+Rz​)−1
• 误差态更新：
  δ x ^ = K r \delta \hat x = K rδx^=Kr
• 协方差更新（Joseph form 更数值稳定）：
  P j ← ( I − K H ) P j ( I − K H ) ⊤ + K R z K ⊤ P_j \leftarrow (I - K H) P_j (I - K H)^\top + K R_z K^\topPj​←(I−KH)Pj​(I−KH)⊤+KRz​K⊤

将误差注入到全状态（状态修正）

• 位置/速度/偏置直接加法修正：
  p ^ j ← p ^ j + δ p ^ , v ^ j ← v ^ j + δ v ^ , b ^ g ← b ^ g + δ b ^ g , b ^ a ← b ^ a + δ b ^ a . \hat p_j \leftarrow \hat p_j + \delta \hat p,\quad \hat v_j \leftarrow \hat v_j + \delta \hat v,\quad \hat b_g \leftarrow \hat b_g + \delta \hat b_g,\quad \hat b_a \leftarrow \hat b_a + \delta \hat b_a.p^​j​←p^​j​+δp^​,v^j​←v^j​+δv^,b^g​←b^g​+δb^g​,b^a​←b^a​+δb^a​.
• 旋转修正（右扰动）：
  R ^ j ← R ^ j exp ⁡ ( [ δ θ ^ ] × ) . \hat R_j \leftarrow \hat R_j \exp([\delta\hat\theta]_\times).R^j​←R^j​exp([δθ^]×​).

重要：修正后需把误差态置零（reset）：
δ x ← 0 , \delta x \leftarrow 0,δx←0,
但注意协方差仍保留为更新后的P j P_jPj​（因为P j P_jPj​表示误差的不确定度）。

重线性化 / 重新预积分（如果有偏置更改）

如果在注入后偏置估计发生了变化（δ b ^ ≠ 0 \delta \hat b \neq 0δb^=0），原先基于b ^ i \hat b_ib^i​做的预积分需要重约束 / 重线性化：

• 常见做法：保留原始 IMU 测量序列或增量信息，使用新的偏置b ^ i + δ b ^ \hat b_i+\delta \hat bb^i​+δb^重新计算Δ R , Δ v , Δ p \Delta R,\Delta v,\Delta pΔR,Δv,Δp；或者用预积分的偏置雅可比对当前预积分做一阶校正（效率更高）：
  Δ v ← Δ v + J Δ v b g δ b ^ g + J Δ v b a δ b ^ a , \Delta v \leftarrow \Delta v + J_{\Delta vb_g}\delta \hat b_g + J_{\Delta vb_a}\delta \hat b_a,Δv←Δv+JΔvbg​​δb^g​+JΔvba​​δb^a​,
  Δ p ← Δ p + J Δ p b g δ b ^ g + J Δ p b a δ b ^ a , \Delta p \leftarrow \Delta p + J_{\Delta pb_g}\delta \hat b_g + J_{\Delta pb_a}\delta \hat b_a,Δp←Δp+JΔpbg​​δb^g​+JΔpba​​δb^a​,
  Δ R ← Δ R exp ⁡ ( J Δ R b g δ b ^ g ) . \Delta R \leftarrow \Delta R \exp\big(J_{\Delta Rb_g}\delta \hat b_g\big).ΔR←ΔRexp(JΔRbg​​δb^g​).

以上校正通常足够（因为预积分已线性化到偏置），比重积分原始 IMU 更高效。

六、算法伪代码

初始化: \hat R_0, \hat p_0, \hat v_0, \hat b_{g,0}, \hat b_{a,0}, P_0$ 每到新的关键时刻 j: 1) 用 IMU 测量从 i -> j 做预积分，得到: ΔR_{ij}, Δv_{ij}, Δp_{ij}, J_{ΔR,bg}, J_{Δv,bg}, J_{Δv,ba}, J_{Δp,bg}, J_{Δp,ba}, Σ_Δ (预积分噪声协方差) 2) 预测: \hat R_j = \hat R_i ΔR_{ij} \hat v_j = \hat v_i + g Δt + \hat R_i Δv_{ij} \hat p_j = \hat p_i + \hat v_i Δt + 0.5 g Δt^2 + \hat R_i Δp_{ij} \hat b_{* j} = \hat b_{* i} 3) 线性化计算 F, G（由 Δ 与 J 确定） 4) 协方差传播: P_j = F P_i F^T + G Q_n G^T 5) 若有外部观测（pose / pos / landmarks）: 构造残差 r 和 H K = P_j H^T (H P_j H^T + R_z)^{-1} δx = K r 注入误差: \hat R_j ← \hat R_j exp([δθ]_×) \hat p_j ← \hat p_j + δp ... 协方差更新: P_j ← (I-KH) P_j (I-KH)^T + K R_z K^T 将误差状态清零 6) 如果偏置更新 δb ≠ 0: 根据 J 的一阶修正 ΔR, Δv, Δp（不用重积分）

七、实现要点与常见陷阱

1. 约定一致性：务必在代码/文档里明确“扰动是左乘还是右乘”“角速度在哪一坐标系表示”“R RR定义（body->inertial or inverse）”。不同约定会改变雅可比符号与F FF的具体块项。

2. 重投影/重线性化 vs 预积分雅可比修正：

   • 采用雅可比一阶修正通常足够且高效；
   • 如果偏置变化很大或预积分步数很少，考虑重积分原始 IMU（稳健但慢）。

3. 数值稳定性：

   • 在做exp ⁡ ( [ δ θ ] × ) \exp([\delta\theta]_\times)exp([δθ]×​)或log ⁡ ( R ) \log(R)log(R)时使用稳定的 Rodrigues / series 展开；
   • 当角度很小时用小角近似避免数值问题。

4. 协方差对称性与正定性：

   • 用 Joseph 形式更新协方差以保障数值稳定性；
   • 若出现非正定（数值误差），可做微正则（P ← ( P + P ⊤ ) / 2 P \leftarrow (P+P^\top)/2P←(P+P⊤)/2，或加小对角量）。

5. 观测信息的参考系：输入的外部位姿需转换到滤波器的惯性/世界基准。

6. 偏置随机游走建模：若把偏置建模为随机游走，Q n Q_nQn​要包含偏置驱动项，从而G Q n G ⊤ G Q_n G^\topGQn​G⊤中包含偏置过程噪声对P PP的贡献。

7. 观测延迟与时序：视觉或 LiDAR 常有延迟，需做时序对齐或滑动窗口（或基于关键帧 + 预积分的滑动窗口优化）。

八、参考实现点

• 用 Eigen 等库实现S O ( 3 ) SO(3)SO(3)运算（[ ⋅ ] × , exp ⁡ , log ⁡ [\cdot]_\times,\exp,\log[⋅]×​,exp,log），并封装稳定的小角实现。

• 预积分模块应支持：

  • 累积Δ R , Δ v , Δ p \Delta R,\Delta v,\Delta pΔR,Δv,Δp；
  • 同步更新并导出雅可比J ⋅ , b J_{\cdot,b}J⋅,b​；
  • 导出预积分噪声协方差Σ Δ \Sigma_\DeltaΣΔ​；
  • 支持在偏置修正下做一阶校正。

• 使用 double 精度以避免长时间积分误差累积。

• 单元测试：对合成数据验证无偏场景下残差期望为零；对已知旋转/偏置合成场景验证雅可比数值一致性（数值差分检验）。

九、关键公式总结

预测（利用预积分）：
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 . \hat R_j = \hat R_i \Delta R_{ij},\quad \\ \hat v_j = \hat v_i + g\Delta t + \hat R_i \Delta v_{ij},\quad\\ \hat p_j = \hat p_i + \hat v_i \Delta t + \tfrac12 g\Delta t^2 + \hat R_i \Delta p_{ij}.R^j​=R^i​ΔRij​,v^j​=v^i​+gΔt+R^i​Δvij​,p^​j​=p^​i​+v^i​Δt+21​gΔt2+R^i​Δpij​.

偏置一阶修正（注入后）：
Δ v ← Δ v + J Δ v , b g , δ b g + J Δ v , b a , δ b a , \Delta v \leftarrow \Delta v + J_{\Delta v,b_g},\delta b_g + J_{\Delta v,b_a},\delta b_a,Δv←Δv+JΔv,bg​​,δbg​+JΔv,ba​​,δba​,
Δ p ← Δ p + J Δ p , b g , δ b g + J Δ p , b a , δ b a , \Delta p \leftarrow \Delta p + J_{\Delta p,b_g},\delta b_g + J_{\Delta p,b_a},\delta b_a,Δp←Δp+JΔp,bg​​,δbg​+JΔp,ba​​,δba​,
Δ R ← Δ R exp ⁡ ( J Δ R , b g , δ b g ) . \Delta R \leftarrow \Delta R \exp\big(J_{\Delta R,b_g},\delta b_g\big).ΔR←ΔRexp(JΔR,bg​​,δbg​).

协方差传播：
P j = F P i F ⊤ + G Q n G ⊤ . P_j = F P_i F^\top + G Q_n G^\top.Pj​=FPi​F⊤+GQn​G⊤.

观测更新（位姿）：
K = P j H ⊤ ( H P j H ⊤ + R z ) − 1 , δ x = K r , K = P_j H^\top (H P_j H^\top + R_z)^{-1},\quad \delta x = K r,K=Pj​H⊤(HPj​H⊤+Rz​)−1,δx=Kr,
R ^ j ← R ^ j exp ⁡ ( [ δ θ ] × ) , p ^ j ← p ^ j + δ p , P j ← ( I − K H ) P j ( I − K H ) ⊤ + K R z K ⊤ . \hat R_j \leftarrow \hat R_j \exp([\delta\theta]_\times),\quad \\ \hat p_j \leftarrow \hat p_j + \delta p,\\ \quad P_j \leftarrow (I-KH)P_j(I-KH)^\top + K R_z K^\top.R^j​←R^j​exp([δθ]×​),p^​j​←p^​j​+δp,Pj​←(I−KH)Pj​(I−KH)⊤+KRz​K⊤.