IMU Preintegration On Manifold(1)

$\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 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}$

接着给出不含噪声的递推公式：
ΔR̃ i,k+1Δṽ i,k+1Δp̃ i,k+1=ΔR̃ i,kExp((w̃ k−b