$
\def\SOthree{SO(3)}
\def\Exp{\textrm{Exp}}
\def\Log{\textrm{Log}}
\def\dt{\Delta t}
\def\Skew#1{[#1]_{\times}}
\def\MatI#1{\textbf{I}_{ #1\times#1}}
\def\MatZ#1{\textbf{0}_{ #1\times#1}}
$
1 $\SOthree$ Group
关于$\SOthree$的介绍略过，这里只列出几个近似的公式：
$\Exp(\delta\theta)\approx\textbf{I}+\Skew{\theta} \tag{1}$
$\Exp(\theta+\delta\theta)\approx\Exp(\theta)\Exp(J_{r}(\theta)\delta\theta) \tag{2}$
$\Exp(\theta)\Exp(\delta\theta)\approx\Exp(\theta+J_{r}^{-1}(\theta)\delta\theta) \tag{3}$
$\Exp(\delta\phi)\Exp(\delta\theta)\approx\Exp(\delta\phi+J_{r}^{-1}(\delta\phi)\delta\theta)\approx\Exp(\delta\phi+\delta\theta) \tag{4}$
以及Adjoint表示：
$\Exp(\theta)R=R\Exp(R^{T}\theta) \tag{5}$
2 2 IMU Preintegration measurements
2.1 Integration measurements
给定初值，在i和j时刻对imu的角速度和加速度进行积分，可以计算j时刻相对于i时刻的姿态：
$
\begin{aligned}R_{j} & =R_{i}\prod_{k=i}^{j-1}\Exp((\tilde{w}_{k}-b_{i}^{g}-\eta_{k}^{gd})\dt)\\
v_{j} & =v_{i}+\sum_{k=i}^{j-1}(g+R_{k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad}))\dt\\
p_{j} & =p_{i}+\sum_{k=i}^{j-1}v_{k}\Delta t+\frac{1}{2}\sum_{k=i}^{j-1}(g+R_{k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad}))\dt^{2}
\end{aligned}
\tag{6}
$
2.2 Preintegration measurements
在preintegration理论中需要将初值$(R_{i},v_{i},p_{i})$和常数项（包含重力$g$的项）分离出来：
$
\begin{aligned}\Delta R_{ij} & =R_{i}^{T}R_{j}=\prod_{k=i}^{j-1}\textrm{Exp}((\tilde{w}_{k}-b_{i}^{g}-\eta_{k}^{gd})\dt)\\
\Delta v_{ij} & =R_{i}^{T}(v_{j}-v_{i}-g\dt(j-i))=\sum_{k=i}^{j-1}\Delta R_{ik}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt\\
\Delta p_{ij} & =R_{i}^{T}(p_{j}-p_{i}-v_{i}\dt(j-i)-\frac{1}{2}g\dt^{2}(j-i)^{2})=\sum_{k=i}^{j-1}[\Delta v_{ik}\dt+\frac{1}{2}\Delta R_{ik}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt^{2}]
\end{aligned}
\tag{7}
$
$\Delta R_{ij},\Delta v_{ij},\Delta p_{ij}$即为preintegration measurements，即不考虑初值以及重力加速度项的相对测量。注意到这些项包含有噪声$\eta$，我们也需要将它们分离出来。在分离的过程中发现preintegration measurements是近似服从高斯分布的，即：
$
\begin{aligned}\Delta\tilde{R}_{ij} & \approx\Delta R_{ij}\Exp(\delta\phi_{ij})\\
\Delta\tilde{v}_{ij} & \approx\Delta v_{ij}+\delta v_{ij}\\
\Delta\tilde{p}_{ij} & \approx\Delta p_{ij}+\delta p_{ij}
\end{aligned}
\tag{8}
$
其中$\Delta\tilde{R}_{ij},\Delta\tilde{v}_{ij},\Delta\tilde{p}_{ij}$为我们可以计算的测量值，不包含噪声$\eta$。
$
\begin{aligned}\Delta\tilde{R}_{ij} & =\prod_{k=i}^{j-1}\textrm{Exp}((\tilde{w}_{k}-b_{i}^{g})\dt)\\
\Delta\tilde{v}_{ij} & =\sum_{k=i}^{j-1}\Delta\tilde{R}{}_{ik}(\tilde{a}_{k}-b_{i}^{a})\dt\\
\Delta\tilde{p}_{ij} & =\sum_{k=i}^{j-1}[\Delta\tilde{v}{}_{ik}\dt+\frac{1}{2}\Delta\tilde{R}_{ik}(\tilde{a}_{k}-b_{i}^{a})\dt^{2}]
\end{aligned}
\tag{9}
$
定义$\eta_{ij}^{\Delta}=[\delta\phi_{ij}^{T},\delta p_{ij}^{T},\delta v_{ij}^{T}]_{9\times1}^{T}\approx\mathcal{N}(0_{9\times1},\sum_{ij})$为noise preintegration vector，它们是和噪声$\eta$相关的项。这里不会对$\eta_{ij}^{\Delta}$进行求解，因为事实上我们仅需要其递推形式。
2.3 Iterative preintegration measurements
首先给出包含噪声的递推公式：
$
\begin{aligned}\Delta R_{i,k+1} & =\Delta R_{i,k}\Exp((\tilde{w}_{k}-b_{i}^{g}-\eta_{k}^{gd})\dt)\\
\Delta v_{i,k+1} & =\Delta v_{i,k}+\Delta R_{i,k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt\\
\Delta p_{i,k+1} & =\Delta p_{i,k}+\Delta v_{i,k}\Delta t+\frac{1}{2}\Delta R_{i,k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt^{2}
\end{aligned}
\tag{10}
$
接着给出不含噪声的递推公式：
$
\begin{aligned}\Delta\tilde{R}_{i,k+1} & =\Delta\tilde{R}_{i,k}\Exp((\tilde{w}_{k}-b_{i}^{g})\dt)\\
\Delta\tilde{v}_{i,k+1} & =\Delta\tilde{v}_{i,k}+\Delta\tilde{R}_{i,k}(\tilde{a}_{k}-b_{i}^{a})\dt\\
\Delta\tilde{p}_{i,k+1} & =\Delta\tilde{p}_{i,k}+\Delta\tilde{v}_{i,k}\Delta t+\frac{1}{2}\Delta\tilde{R}_{i,k}(\tilde{a}_{k}-b_{i}^{a})\dt^{2}
\end{aligned}
\tag{11}
$
3 IMU Preintegration: Noise Propagation and Bias Updates
3.1 Iterative Noise Propagation
在前面提到noise preintegration vector $\eta_{ij}^{\Delta}=[\delta\phi_{ij}^{T},\delta p_{ij}^{T},\delta v_{ij}^{T}]^{T}\approx\mathcal{N}(0_{9\times1},\sum_{ij})$，这里将证明preintegration measurements近似服从高斯分布，并给出$\eta_{ij}^{\Delta}$的递推计算结果。
Rotation
根据$\SOthree$中不确定性的定义，有$\Delta\tilde{R}_{k,k+1}=\Delta R_{k,k+1}\textrm{Exp}(\delta\phi_{k,k+1})$。
$\Delta R_{k,k+1}$表示包含bias和noise两个相邻时刻的相对旋转，$\Delta\tilde{R}_{k,k+1}$表示不包含noise两个相邻时刻的相对旋转。
$
\begin{aligned}\Delta R_{k,k+1} & =\Exp((\tilde{w_{k}}-b_{i}^{g}-\eta_{k}^{gd})\dt)\\
& \overset{(2)}{\approx}\Exp((\tilde{w_{k}}-b_{i}^{g})\dt)\Exp(-J_{r}((\tilde{w}_{k}-b_{i}^{g})\dt)\eta_{k}^{gd}\dt)\\
& =\Delta\tilde{R}_{k,k+1}\Exp(-J_{r}^{k}\eta^{gd}\dt)\\
& =\Delta\tilde{R}_{k,k+1}\Exp(-\phi_{k,k+1})
\end{aligned}
\tag{12}
$
其中$\Delta\tilde{R}_{k,k+1}=\Exp((\tilde{w_{k}}-b_{i}^{g})\dt)$，$J_{r}^{k}=J_{r}((\tilde{w_{k}}-b_{i}^{g})\dt)$，$\phi_{k,k+1}=J_{r}^{k}\eta^{gd}\dt$。
两个相邻时刻的相对旋转是服从高斯分布的，可以证明在积分后i时刻和j时刻的相对旋转也是近似服从高斯的，即$\Delta\tilde{R}_{ij}\approx\Delta R_{ij}\Exp(\delta\phi_{ij})$，下面进行推导并求出$\delta\phi_{ij}$的递推公式：
设初始时刻$\Delta R_{ii}=\textbf{I}_{3\times3},\delta\phi_{ii}=\textbf{0}_{3\times3}$
$\Delta R_{i,i+1}\approx\Delta R_{ii}\Delta\tilde{R}_{i,i+1}\Exp(-J_{r}^{i}\eta_{i}^{gd}\dt)$
$\implies\delta\phi_{i,i+1}=J_{r}^{i}\eta_{i}^{gd}\dt$
$
\begin{aligned}\Delta R_{i,i+2} & =\Delta R_{i,i+1}\Delta R_{i+1,i+2}\\
& \approx\Delta\tilde{R}_{i,i+1}\Exp(-\delta\phi_{i,i+1})\Delta\tilde{R}_{i+1,i+2}\Exp(-J_{r}^{i+1}\eta_{i+1}^{gd}\dt)\\
& \stackrel{(5)}{=}\Delta\tilde{R}_{i,i+1}\Delta\tilde{R}_{i+1,i+2}\Exp(-\Delta\tilde{R}_{i+1,i+2}^{T}\delta\phi_{i,i+1})\Exp(-J_{r}^{i+1}\eta_{i+1}^{gd}\dt)\\
& \stackrel{(4)}{\approx}\Delta\tilde{R}_{i,i+2}\Exp(-\Delta\tilde{R}_{i+1,i+2}^{T}\delta\phi_{i,i+1}-J_{r}^{i+1}\eta_{i+1}^{gd}\dt)
\end{aligned}
$
$\implies\phi_{i,i+2}=\Delta\tilde{R}_{i+1,i+2}^{T}\delta\phi_{i,i+1}+J_{r}^{i+1}\eta_{i+1}^{gd}\dt$
$
\begin{aligned}\Delta R_{i,i+3} & =\Delta R_{i,i+2}\Delta R_{i+2,i+3}\\
& \approx\Delta\tilde{R}_{i,i+2}\Exp(-\phi_{i,i+2})\Delta\tilde{R}_{i+2,i+3}\Exp(-J_{r}^{i+2}\eta_{i+2}^{gd}\dt)\\
& \stackrel{(5)}{=}\Delta\tilde{R}_{i,i+2}\Delta\tilde{R}_{i+2,i+3}\Exp(-\Delta\tilde{R}_{i+2,i+3}^{T}\phi_{i,i+2})\Exp(-J_{r}^{i+2}\eta_{i+2}^{gd}\dt)\\
& \stackrel{(4)}{\approx}\Delta\tilde{R}_{i,i+3}\Exp(-\Delta\tilde{R}_{i+2,i+3}^{T}\phi_{i,i+2}-J_{r}^{i+2}\eta_{i+2}^{gd}\dt)
\end{aligned}
$
$\implies\phi_{i,i+3}=\Delta\tilde{R}_{i+2,i+3}^{T}\phi_{i,i+2}+J_{r}^{i+2}\eta_{i+2}^{gd}\dt$
因此$\delta\phi_{i,k+1}=\Delta\tilde{R}_{k,k+1}^{T}\delta\phi_{i,k}+J_{r}^{k}\eta_{k}^{gd}\Delta t$。
velocity
对于速度而言，其高斯分布为$\Delta\tilde{v}_{k,k+1}=\Delta v_{k,k+1}+\delta v_{k,k+1}$。
根据公式$(10)$中的$\Delta v_{i,k+1}=\Delta v_{i,k}+\Delta R_{i,k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt$进行推导。
假设初始时刻$\Delta\tilde{v}_{i,k}=\Delta v_{i,k}+\delta v_{i,k}$。
$
\begin{aligned}\Delta v_{i,k+1} & =\Delta v_{i,k}+\Delta R_{i,k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt\\
& =\Delta\tilde{v}_{i,k}-\delta v_{i,k}+\Delta\tilde{R}_{i,k}\Exp(-\delta\phi_{i,k})(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt\\
& \stackrel{(1)}{\approx}\Delta\tilde{v}_{i,k}-\delta v_{i,k}+\Delta\tilde{R}_{i,k}(\textbf{I}_{3\times3}-\Skew{\delta\phi_{i,k}})(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt\\
& \approx\Delta\tilde{v}_{i,k}+\Delta\tilde{R}_{i,k}(\tilde{a}_{k}-b_{i}^{a})\dt-(\delta v_{i,k}-\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt+\Delta\tilde{R}_{i,k}\eta_{k}^{ad}\dt)\\
& =\Delta\tilde{v}_{i,k+1}-(\delta v_{i,k}-\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt+\Delta\tilde{R}_{i,k}\eta_{k}^{ad}\dt)
\end{aligned}
$
因此$\delta v_{i,k+1}=\delta v_{i,k}-\Delta\tilde{R}_{i,k}^{T}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt+\Delta\tilde{R}_{i,k}^{T}\eta_{k}^{ad}\dt$。
Position
对于位移而言，其高斯分布为$\Delta\tilde{p}_{k,k+1}=\Delta p_{k,k+1}+\delta p_{k,k+1}$。
根据公式$(10)$中的$\Delta p_{i,k+1}=\Delta p_{i,k}+\Delta v_{i,k}\Delta t+\frac{1}{2}\Delta R_{i,k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt^{2}$进行推导。
假设$\Delta\tilde{p}_{i,k}=\Delta p_{i,k}+\delta p_{i,k}$
$
\begin{aligned}\Delta p_{i,k+1} & =\Delta p_{i,k}+\Delta v_{i,k}\Delta t+\frac{1}{2}\Delta R_{i,k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt^{2}\\
& =\Delta\tilde{p}_{i,k}-\delta p_{i,k}+(\Delta\tilde{v}_{i,k}-\delta v_{i,k})\dt+\frac{1}{2}\Delta\tilde{R}_{i,k}\Exp(-\delta\phi_{i,k})(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt^{2}\\
& \stackrel{(1))}{\approx}\Delta\tilde{p}_{i,k}+\Delta\tilde{v}_{i,k}\dt-\delta p_{i,k}-\delta v_{i,k}\dt+\frac{1}{2}\Delta\tilde{R}_{i,k}(\textbf{I}_{3\times3}-\Skew{\delta\phi_{i,k}})(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt^{2}\\
& \approx\Delta\tilde{p}_{i,k}+\Delta\tilde{v}_{i,k}\dt+\frac{1}{2}\Delta\tilde{R}_{i,k}(\tilde{a}_{k}-b_{i}^{a})\dt^{2}-\delta p_{i,k}-\delta v_{i,k}\dt+\frac{1}{2}\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt^{2}-\frac{1}{2}\Delta\tilde{R}_{i,k}\eta_{k}^{ad}\dt^{2}\\
& \approx\Delta\tilde{p}_{i,k+1}-(\delta p_{i,k}+\delta v_{i,k}\dt-\frac{1}{2}\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt^{2}+\frac{1}{2}\Delta\tilde{R}_{i,k}\eta_{k}^{ad}\dt^{2})
\end{aligned}
$
因此$\delta p_{i,k+1}=\delta p_{i,k}+\delta v_{i,k}\dt-\frac{1}{2}\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt^{2}+\frac{1}{2}\Delta\tilde{R}_{i,k}\eta_{k}^{ad}\dt^{2}$。
综上，可以得到$\eta_{ij}^{\Delta}$的递推计算公式：
$
\begin{aligned}\delta\phi_{i,k+1} & =\Delta\tilde{R}_{k,k+1}^{T}\delta\phi_{i,k}+J_{r}^{k}\eta_{k}^{gd}\Delta t\\
\delta v_{i,k+1} & =\delta v_{i,k}-\Delta\tilde{R}_{i,k}^{T}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt+\Delta\tilde{R}_{i,k}^{T}\eta_{k}^{ad}\dt\\
\delta p_{i,k+1} & =\delta p_{i,k}+\delta v_{i,k}\dt-\frac{1}{2}\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt^{2}+\frac{1}{2}\Delta\tilde{R}_{i,k}\eta_{k}^{ad}\dt^{2}
\end{aligned}
\tag{13}
$
写成矩阵的形式：
$$
\left[\begin{array}{c}
\delta\phi_{i,k+1}\\
\delta p_{i,k+1}\\
\delta v_{i,k+1}
\end{array}\right]=\left[\begin{array}{ccc}
\Delta\tilde{R}_{k,k+1}^{T} & \MatZ 3 & \MatZ 3\\
-\frac{1}{2}\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\dt^{2} & \MatI 3 & \MatI 3\dt\\
-\Delta\tilde{R}_{i,k}^{T}\Skew{\tilde{a}_{k}-b_{i}^{a}}\dt & \MatZ 3 & \MatI 3
\end{array}\right]_{9\times9}\left[\begin{array}{c}
\delta\phi_{i,k}\\
\delta p_{i,k}\\
\delta v_{i,k}
\end{array}\right]+\left[\begin{array}{c}
\MatZ 3\\
\frac{1}{2}\Delta\tilde{R}_{i,k}\dt^{2}\\
\Delta\tilde{R}_{i,k}^{T}\dt
\end{array}\right]_{9\times3}\eta_{k}^{ad}+\left[\begin{array}{c}
J_{r}^{k}\Delta t\\
\MatZ 3\\
\MatZ 3
\end{array}\right]_{9\times3}\eta_{k}^{gd}
$$
$\eta_{i,k+1}^{\Delta}=A_{k}\eta_{i,k+1}^{\Delta}+B_{k}\eta_{k}^{ad}+C_{k}\eta_{k}^{gd}$
3.2 Bias Correction via First-Order Updates
在前面的推导中，我们假定在时刻i和j之间的偏置是相同的，然而在优化过程中偏置会得到修正。一种简单的方式是将更新后的偏置代入上面的方程中，再对时刻i和j之间的测量进行积分，这样显然不是高效的。设$\hat{b}\leftarrow\bar{b}+\delta b$，其中$\bar{b}$为上一次的估计，$\delta b$ 为微小的增量更新。对公式$(9)$在$\bar{b}$处进行一阶泰勒展开：
$
\begin{aligned}\Delta\tilde{R}_{ij}(\hat{b}_{i}^{g}) & =\prod_{k=i}^{j-1}\textrm{Exp}((\tilde{w}_{k}-\bar{b}_{i}^{g}-\delta b_{i}^{g})\dt)=\Delta\tilde{R}_{ij}(\bar{b}_{i}^{g})\Exp(\frac{\partial\Delta\bar{R}_{ij}}{\partial b^{g}}\delta b_{i}^{g})\\
\Delta\tilde{v}_{ij}(\hat{b}_{i}^{g},\hat{b}_{i}^{a}) & =\sum_{k=i}^{j-1}\Delta\tilde{R}{}_{ik}(\hat{b}_{i}^{g})(\tilde{a}_{k}-\bar{b}_{i}^{a}-\delta b_{i}^{a})\dt=\Delta\tilde{v}_{ij}(\bar{b}_{i}^{g},\bar{b}_{i}^{a})+\frac{\partial\Delta\bar{v}_{ij}}{\partial b^{g}}\delta b_{i}^{g}+\frac{\partial\Delta\bar{v}_{ij}}{\partial b^{a}}\delta b_{i}^{a}\\
\Delta\tilde{p}_{ij}(\hat{b}_{i}^{g},\hat{b}_{i}^{a}) & =\sum_{k=i}^{j-1}[\Delta\tilde{v}{}_{ik}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})\dt+\frac{1}{2}\Delta\tilde{R}{}_{ik}(\hat{b}_{i}^{g})(\tilde{a}_{k}-\bar{b}_{i}^{a}-\delta b_{i}^{a})\dt]=\Delta\tilde{p}_{ij}(\bar{b}_{i}^{g},\bar{b}_{i}^{a})+\frac{\partial\Delta\bar{p}_{ij}}{\partial b^{g}}\delta b_{i}^{g}+\frac{\partial\Delta\bar{p}_{ij}}{\partial b^{a}}\delta b_{i}^{a}
\end{aligned}
\tag{14}
$
因此当偏置更新后，我们只需要更新preintegration measurements中由于微小增量$\delta b$带来的更新。上式与式$(7)$的形式相同，仿照式$(10)$可以写出其迭代形式：
$
\begin{aligned}\Delta\tilde{R}_{i,k+1}(\hat{b}_{i}^{g}) & =\Delta\tilde{R}_{i,k}(\hat{b}_{i}^{g})\Exp((\tilde{w}_{k}-\bar{b}_{i}^{g}-\delta b_{i}^{g})\dt)\\
\Delta\tilde{v}_{i,k+1}(\hat{b}_{i}^{g},\hat{b}_{i}^{a}) & =\Delta\tilde{v}_{i,k}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})+\Delta\tilde{R}_{i,k}(\hat{b}_{i}^{g})(\tilde{a}_{k}-\bar{b}_{i}^{a}-\delta b_{i}^{a})\dt\\
\Delta\tilde{p}_{i,k+1}(\hat{b}_{i}^{g},\hat{b}_{i}^{a}) & =\Delta\tilde{p}_{i,k}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})+\Delta\tilde{v}_{i,k}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})\Delta t+\frac{1}{2}\Delta\tilde{R}_{i,k}(\hat{b}_{i}^{g})(\tilde{a}_{k}-\bar{b}_{i}^{a}-\delta b_{i}^{a})\dt^{2}
\end{aligned}
\tag{15}
$
按照3.1节的推导方法可以写出$\frac{\partial\Delta\bar{R}_{ij}}{\partial b^{g}},\frac{\partial\Delta\bar{v}_{ij}}{\partial b^{g}},\frac{\partial\Delta\bar{v}_{ij}}{\partial b^{a}},\frac{\partial\Delta\bar{p}_{ij}}{\partial b^{g}},\frac{\partial\Delta\bar{p}_{ij}}{\partial b^{a}} $的递推形式，由于式(14)与式 (8)的定义不同，因此递推公式在符号上和式(13)不太一样，但形式是相同的：
$
\begin{aligned}\frac{\partial\Delta\bar{R}_{i,k+1}}{\partial b^{g}} & =\Delta\tilde{R}_{k,k+1}^{T}(\bar{b}_{i}^{g})\frac{\partial\Delta\bar{R}_{i,k}}{\partial b^{g}}-J_{r}^{k}\Delta t\\
\frac{\partial\Delta\bar{v}_{i,k+1}}{\partial b^{g}} & =\frac{\partial\Delta\bar{v}_{i,k}}{\partial b^{g}}-\Delta\tilde{R}_{i,k}^{T}(\bar{b}_{i}^{g})\Skew{\tilde{a}_{k}-\bar{b}_{i}^{a}}\frac{\partial\Delta\bar{R}_{i,k}}{\partial b^{g}}\dt\\
\frac{\partial\Delta\bar{v}_{i,k+1}}{\partial b^{a}} & =\frac{\partial\Delta\bar{v}_{i,k}}{\partial b^{a}}-\Delta\tilde{R}_{i,k}^{T}(\bar{b}_{i}^{g})\dt\\
\frac{\partial\Delta\bar{p}_{i,k+1}}{\partial b^{g}} & =\frac{\partial\Delta\bar{p}_{i,k}}{\partial b^{g}}+\frac{\partial\Delta\bar{v}_{i,k}}{\partial b^{g}}\dt-\frac{1}{2}\Delta\tilde{R}_{i,k}(\bar{b}_{i}^{g})\Skew{\tilde{a}_{k}-\bar{b}_{i}^{a}}\frac{\partial\Delta\bar{R}_{i,k}}{\partial b^{g}}\dt^{2}\\
\frac{\partial\Delta\bar{p}_{i,k+1}}{\partial b^{a}} & =\frac{\partial\Delta\bar{p}_{i,k}}{\partial b^{a}}+\frac{\partial\Delta\bar{v}_{i,k}}{\partial b^{a}}\dt-\frac{1}{2}\Delta\tilde{R}_{i,k}(\bar{b}_{i}^{g})\dt^{2}
\end{aligned}
\tag{16}
$
令$H_{k}^{a}=[\frac{\partial\Delta\bar{R}_{i,k}}{\partial b^{a}}^{T},\frac{\partial\Delta\bar{p}_{i,k}}{\partial b^{a}}^{T},\frac{\partial\Delta\bar{v}_{i,k}}{\partial b^{a}}^{T}]_{9\times3}^{T}$,$H_{k}^{g}=[\frac{\partial\Delta\bar{R}_{i,k}}{\partial b^{g}}^{T},\frac{\partial\Delta\bar{p}_{i,k}}{\partial b^{g}}^{T},\frac{\partial\Delta\bar{v}_{i,k}}{\partial b^{g}}^{T}]_{9\times3}^{T}$,$H_{k}=\left[\begin{array}{cc} H_{k}^{a} & H_{k}^{g}\end{array}\right]_{9\times6}$,结果整理可以得到其矩阵形式：
$
\begin{aligned}H_{k+1}^{a} & =A_{k}H_{k}^{a}-B_{k}\\
H_{k+1}^{g} & =A_{k}H_{k}^{g}-C_{k}
\end{aligned}
\tag{17}
$