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