\documentclass{article}
\usepackage[body={18cm,23cm}, top=3cm]{geometry}
\geometry{papersize={21.59cm,27.94cm}}
\usepackage{xeCJK}
\setmainfont{STSong}
\setCJKmainfont{STSong}
\title{IMU Preintergratoin on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation}
\author{ForgetPast}
\begin{document}
	\maketitle
	这篇文章主要是重新推导IMU Preintergratoin on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation论文的公式，包括所有的细节。
\section*{基本公式}
首先介绍下基本的公式，[\ref{eq:eq_1}]为叉乘的交换律，[\ref{eq:eq_2}]为$SO(3)$的指数映射公式。
\begin{equation}
	a^{\wedge}b=-b^{\wedge}a, \quad \forall a,b\in \mathbf{R}^3
	\label{eq:eq_1}
\end{equation}

\begin{equation}
	\exp(\phi^{\wedge})=\mathbf{I}+\frac{\sin(\Vert \phi \Vert)}{\Vert \phi \Vert }\phi^{\wedge}+
	\frac{1-\cos(\Vert \phi \Vert)}{\Vert \phi \Vert^2}(\phi^{\wedge})^2
	\label{eq:eq_2}
\end{equation}
公式[\ref{eq:eq_2}]的一阶近似为
\begin{equation}
	\exp(\phi^{\wedge})\approx \mathbf{I}+ \phi^{\wedge}
\label{eq:eq_3}
\end{equation}

$SO(3)$的对数映射公式为
\begin{equation}
	\log(\mathcal{R}) = \frac{\phi \cdot (\mathcal{R}-\mathcal{R}^T)}{2\sin(\phi)} \quad \mathrm{with} \quad \phi = \cos^{-1}(\frac{tr(\mathcal{R})-1}{2})
\end{equation}
从而得到$\log(\mathcal{R})=\mathbf{a}\phi$，其中$\mathbf{a}$和$\phi$为旋转轴和旋转的角度。
此外，还需要介绍下$SO(3)$的微分公式
\begin{equation}
	\mathrm{Exp}(\phi + \delta \phi)\approx \mathrm{Exp}(\phi)\mathrm{Exp}(J_r(\phi)\delta \phi)
\end{equation}
从而
\begin{equation}
	\mathrm{Log}(\mathrm{Exp}(\phi)\mathrm{Exp}(\delta \phi)) \approx \phi + J_r^{-1}(\phi)\delta \phi
\end{equation}
以及Adjoint representation
\begin{equation}
	R\mathrm{Exp}(\Phi)R^{\mathrm{T}}=\mathrm{exp}(R\phi^{\wedge}R^{\mathrm{T}})=\mathrm{Exp}(R\phi)
\end{equation}

\section*{IMU模型}
这里假设$W$为惯性系，从而IMU测量模型如公式[\ref{eq:gyro_model}][\ref{eq:acc_model}]所示。
\begin{equation}
	{}_{\mathrm{B}}\tilde{\omega}_{\mathrm{WB}}(t)={}_{\mathrm{B}}\omega_{\mathrm{WB}}(t)+b^g(t)+\eta^g(t) 
	\label{eq:gyro_model}
\end{equation}

\begin{equation}
	{}_{\mathrm{B}}\tilde{a}_{\mathrm{WB}}(t)=\mathbf{R}_{\mathrm{WB}}^{\mathrm{T}}(t) ({}_{\mathrm{W}}a(t) - {}_{\mathrm{W}}g)
	+ b^a(t) + \eta^a(t) 
	\label{eq:acc_model}
\end{equation}
其中带$\tilde{ }$上标的变量表示IMU实际测量得到的结果，不带$\tilde{ }$的为真实的测量值，总是受到零漂和噪声的影响
IMU的运动学模型为($W$系到$B$系)
\begin{equation}
	\dot{R}_{\mathrm{WB}} = R_{\mathrm{WB}}\omega^{\wedge}_{\mathrm{WB}}, \quad 
	{}_{\mathrm{W}}\dot{\mathrm{v}}={}_{\mathrm{W}}a,\quad
	{}_{\mathrm{W}}\dot{\mathrm{p}}={}_{\mathrm{W}}\mathrm{v}
\end{equation}
根据上面的运动模型，系统从$t$时刻到$t+\Delta t$时刻的状态转移公式为
\begin{equation}
\begin{array}{l}
	R_{\mathrm{WB}}(t+\Delta t) = R_{\mathrm{WB}}(t)\mathrm{Exp}({}_{\mathrm{B}}\omega_{\mathrm{WB}}(t)\Delta t)  \\
	{}_{\mathrm{W}}\mathrm{v}(t+\Delta t) =
	{}_{\mathrm{W}}\mathrm{v}(t)+ {}_{\mathrm{W}} a(t)\Delta t  \\
	{}_{\mathrm{W}}\mathrm{p}(t+\Delta t) =
	{}_{\mathrm{W}}\mathrm{p}(t)+ {}_{\mathrm{W}} \mathrm{v}(t)\Delta t +\frac{1}{2}
	{}_{\mathrm{W}}a(t)\Delta t^2
\end{array}
	\label{eq:state_transient}
\end{equation}
将[\ref{eq:gyro_model}][\ref{eq:acc_model}]代入公式[\ref{eq:state_transient}]并丢掉下标得到
\begin{equation}
\begin{array}{l}
	R(t+\Delta t) = R(t)\mathrm{Exp}((\tilde{\omega}(t) - b^g(t) - \eta^{gd}(t))\Delta t)  \\
	\mathrm{v}(t+\Delta t) =\mathrm{v}(t)+ g\Delta t + R(t)(\tilde{a}(t) - b^a(t) - \eta^{ad}(t))\Delta t  \\
	\mathrm{p}(t+\Delta t) =
	\mathrm{p}(t)+ \mathrm{v}(t)\Delta t +\frac{1}{2}g\Delta t^2 + \frac{1}{2}R(t)(\tilde{a}(t) - b^a(t) - \eta^{ad}(t))\Delta t^2
\end{array}
\label{eq:whole_eq}
\end{equation}
离散时间噪声$\eta^{gd}(t)$是采样率的函数并且和连续时间谱噪声有关$\mathrm{Cov}(\eta^{gd}(t))=\frac{1}{\Delta t}\mathrm{Cov}(\eta^{g}(t))$，加速度计的噪声也是一样的公式。
\section*{流形上的预积分}
到此为止，所有的公式都是平凡的。公式(\ref{eq:whole_eq})是连续时间的，在实际的系统中需要离散化，如下图所示。
\begin{figure}[h]
	\centering
	\includegraphics[width=12cm]{images/clck_sync}
\end{figure}
由于IMU的频率比相机的频率高很多，所以在两帧图像之间$k=i$和$k=j$需要先根据IMU的测量值得到状态转移，如公式()所示，其中IMU的时间间隔为$\Delta t$。
\begin{equation}
\begin{array}{l}
	R_j = R_i\Pi_{k=i}^{j-1}\mathrm{Exp}((\tilde{\omega}_k - b^g_k - \eta^{gd}_k)\Delta t)  \\
	\mathrm{v}_j =\mathrm{v}_i + g\Delta t_{ij} + \sum_{k=i}^{j-1}R_k(\tilde{a}_k - b^a_k - \eta^{ad}_k)\Delta t  \\
	\mathrm{p}_j =
	\mathrm{p}_i+ \sum_{k=i}^{j-1}\mathrm{v}_k\Delta t+ \frac{1}{2}g\Delta t_{ij}^2 +\sum_{k=i}^{j-1}\frac{1}{2}R_k(\tilde{a}_k - b^a_k - \eta^{ad}_k)\Delta t^2
\end{array}
\label{eq:dis_eq}
\end{equation}
虽然上面的公式能够得到时间$t_i$和时间$t_j$之间运动的估计，但是缺点就是如果$t_i$时刻的线性点发生改变，即$R_i$发生变化，则需要重新计算$R_j$，所以需要进行预积分，计算两帧之间的相对运动，从而避免重复运算。
\begin{equation}
\begin{array}{l}
	\Delta R_{ij} = R_i^{\mathrm{T}}R_j = \Pi_{k=i}^{j-1}\mathrm{Exp}((\tilde{\omega}_k - b^g_k - \eta^{gd}_k)\Delta t) \\
	\Delta \mathrm{v}_{ij} = R_{i}^{\mathrm{T}}(\mathrm{v}_j - \mathrm{v}_i - g\Delta t_{ij})=
	\sum_{k=i}^{j-1}\Delta R_{ik}(\tilde{a}_k - b^a_k - \eta^{ad}_k)\Delta t \\
\begin{array}{l}
	\Delta p_{ij} = R_i^{\mathrm{T}}(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t^2_{ij})\\	
	=\sum_{k=i}^{j-1}[\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}(\tilde{a}_k - b^a_k - \eta^{ad}_k)\Delta_t^2]\\
	\sum_{k=i}^{j-1}[\frac{3}{2}\Delta R_{ik}(\tilde{a}_k - b^a_k - \eta^{ad}_k)\Delta_t^2]
\end{array}
\end{array}
\label{eq:relate_motion}
\end{equation}
其中$\Delta R_{ik}=R_i^{\mathrm{T}}R_k$，$\Delta \mathrm{v}_{ik}=\mathrm{v}_k -\mathrm{v}_i$。
公式(\ref{eq:relate_motion})仍然是零偏的函数，由于零偏在优化的过程中会发生改变，所以需要采用零偏发生改变时更新相对运动计算量比较小的方式，具体的做法如下所示：（1）假设零偏是已知的；（2）当零偏改变的时候避免重复积分。
这篇文章接下来假设两帧之间的零偏是常数，即$b_i^g=b_{i+1}^g=\cdots = b_{j-1}^g,b_i^a=b_{i+1}^a=\cdots = b_{j-1}^a$
\subsection*{IMU测量预积分}
这一小节假设零偏是已知的，分离出噪声，因此$\Delta R_{ij}$可以写成
\begin{equation}
	\begin{array}{l}
		\Delta R_{ij}\approx \Pi_{k=i}^{j-1}[\mathrm{Exp}(\tilde{\omega}_k - b^g_k)\Delta t]\mathrm{Exp}(-J_r^k \eta_k^{gd}\Delta t) \\
		=\Delta \tilde{R}_{ij}\Pi_{k=i}^{j-1}\mathrm{Exp}(-\Delta \tilde{R}_{k+1j}^{\mathrm{T}}J_r^k \eta_k^{gd}\Delta t) \\
		=\Delta \tilde{R}_{ij}\mathrm{Exp}(-\delta \phi_{ij})
	\end{array}
	\label{eq:orientation_noise}
\end{equation}
其中$J_r^k=J_r^k((\tilde{\omega}_k - b^g_k)\Delta t)$，并定义$\Delta \tilde{R}_{ij}=\Pi_{k=i}^{j-1}\mathrm{Exp}(-\Delta \tilde{R}_{k+1j}^{\mathrm{T}}J_r^k\Delta t)$，噪声为$\delta \phi_{ij}$，后面会具体的分析噪声。
将(\ref{eq:orientation_noise})代入公式(\ref{eq:relate_motion})得到速度的预积分：
\begin{equation}
\begin{array}{l}
	\Delta \mathrm{v}_{ij}\approx \sum_{k=i}^{j-1}\Delta \tilde{R}_{ik}(I-\delta \phi^{\wedge}_{ik})(\tilde{a}_k - b_i^a)\Delta t - \Delta\tilde{R}_{ik}\eta_K^{ad}\Delta t\\
	=\Delta \tilde{\mathrm{v}}_{ij} + \sum_{k=i}^{j-1}[\Delta \tilde{R}_{ik}(\tilde{a}_k - b_i^a)^{\wedge}\delta \phi_{ik}\Delta t - \Delta \tilde{R}_{ik}\eta_k^{ad}\Delta t ] \\
	=\Delta \tilde{\mathrm{v}}_{ij}-\delta \mathrm{v}_{ij}
\end{array}
\label{eq:velocity_noise}
\end{equation}
其中预积分速度测量$\Delta \tilde{\mathrm{v}}_{ij}=\sum_{k=i}^{j-1}\Delta \tilde{R}_{ik}(\tilde{a}_k - b_i^a)$，噪声为$\delta \mathrm{v}_{ij}$。
将(\ref{eq:orientation_noise})代入公式(\ref{eq:relate_motion})得到位置的预积分：
\begin{equation}
\begin{array}{l}
	\Delta \mathrm{p}_{ij}\approx \sum_{k=i}^{j-1}\frac{3}{2}\Delta \tilde{R}_{ik}(I-\delta \phi^{\wedge}_{ik})(\tilde{a}_k - b_i^a)\Delta t^2 - \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{R}_{ik}\eta_K^{ad}\Delta t^2\\
	=\Delta \tilde{\mathrm{p}}_{ij} + \sum_{k=i}^{j-1}[\frac{3}{2}\Delta \tilde{R}_{ik}(\tilde{a}_k - b_i^a)^{\wedge}\delta \phi_{ik}\Delta t^2 - \frac{3}{2}\Delta \tilde{R}_{ik}\eta_k^{ad}\Delta t^2 ] \\
	=\Delta \tilde{\mathrm{p}}_{ij}-\delta \mathrm{p}_{ij}
\end{array}
\label{eq:position_noise}
\end{equation}
将$\Delta R_{ij},\Delta \mathrm{v}_{ij}, \Delta p_{ij}$代入(\ref{eq:orientation_noise})，(\ref{eq:velocity_noise})，(\ref{eq:position_noise})得到式()
\begin{equation}
	\begin{array}{l}
		\Delta \tilde{R}_{ij}=R^{\mathrm{T}}_iR_j\mathrm{Exp}(\delta \phi_{ij}) \\
		\Delta \title{\mathrm{v}}_{ij} = R_i^{\mathrm{T}}(\mathrm{v}_j - \mathrm{v}_i - g\Delta t_{ij}) + \delta \mathrm{v}_{ij} \\
		\Delta \title{\mathrm{p}}_{ij} = R_i^{\mathrm{T}}(\mathrm{p}_j - \mathrm{p}_i - \mathrm{v}_i\Delta t_{ij}  - \frac{1}{2}g\Delta t_{ij}^2) + \delta \mathrm{p}_{ij}
	\end{array}
	\label{eq:imu_measure_eq}
\end{equation}
因此，含噪声的测量可以写成待估计值的函数加上随机噪声，随机向量为$[\delta \phi_{ij}^{\mathrm{T}}, \delta v_{ij}^{\mathrm{T}}, \delta p_{ij}^{\mathrm{T}}]$。下面的一小节将会介绍噪声的性质。
\subsection*{噪声传播}
首先介绍方向噪声
\begin{equation}
	\mathrm{Exp}(-\delta \phi_{ij}) = \Pi_{k=i}^{j-1}\mathrm{Exp}(-\Delta \tilde{R}_{k+1j}^{\mathrm{T}}J_r^k \eta_k^{gd} \Delta t)
\end{equation}
取对数得到
\begin{equation}
	\delta \phi_{ij} = -\mathrm{Log}(\Pi_{k=i}^{j-1}\mathrm{Exp}(-\Delta \tilde{R}_{k+1j}^{\mathrm{T}}J_r^k \eta_k^{gd} \Delta t))
\end{equation}
使用一阶近似(由于右雅克比近似为单位阵)得到
\begin{equation}
	\delta \phi_{ij} \approx \sum_{k=i}^{j-1}\Delta \tilde{R}_{k+1j}^{\mathrm{T}}J_r^k \eta_k^{gd} \Delta t
\end{equation}
由于$\delta \mathrm{v}_{ij}$和$\delta p_{ij}$为加速度噪声$\eta_k^{ad}$和预积分方向噪声$\delta \phi_{ij}$，因此也是零均值、高斯的。下面具体的分析预融合噪声向量$\eta_{ij}^{\Delta} = [\delta \phi_{ij}^{\mathrm{T}}, \delta \mathrm{v}^{\mathrm{T}}, \delta p_{ij}^{\mathrm{T}}]^{\mathrm{T}}\sim \mathcal{N}(0_{9\times 1}, \Sigma_{ij})$的传播。
首先具体的给出预融合噪声的表达式：
\begin{equation}
	\begin{array}{l}
		\delta \phi_{ij} \approx \sum_{k=i}^{j-1}\Delta \tilde{R}_{k+1}^{\mathrm{T}}J_r^k\eta_k^{gd}\Delta t \\
		\delta \mathrm{v}_{ij} \approx \sum_{k=i}^{j-1}[-\Delta \tilde{R}_{ik}(\tilde{a}_k - b_i^a)^{\wedge}\delta \phi_{ik}\Delta t + \Delta \tilde{R}_{ik}\eta_k^{ad}\Delta t ] \\
		\delta p_{ij} \approx \sum_{k=i}^{j-1}[-\frac{3}{2}\Delta \tilde{R}_{ik}(\tilde{a}_k - b_i^a)^{\wedge}\delta \phi_{ik}\Delta t^2 + \frac{3}{2}\Delta \tilde{R}_{ik}\eta_k^{ad}\Delta t^2 ]
	\end{array}
\end{equation}
将三项噪声写成迭代的形式
\begin{equation}
	\left[
	\begin{array}{c}
		\delta \phi_{ik+1} \\
		\delta \mathrm{v}_{ik+1} \\
		\delta p_{ik+1} 
	\end{array}
	\right]_{9\times 1}
	=
	\left[
	\begin{array}{ccc}
		\Delta \tilde{R}_{kk+1}^{\mathrm{T}} & 0_{3\times 3} & 0_{3\times 3} \\
		-\Delta\tilde{R}_{ik}(\tilde{a}_k - b_i^a)^{\wedge}\Delta t & I_{3\times 3} & 0_{3\times 3}\\
		-\frac{1}{2}\Delta\tilde{R}_{ik}(\tilde{a}_k - b_i^a)^{\wedge}\Delta t^2 & I_{3\times 3}\Delta t & I_{3\times 3}
	\end{array}
	\right]_{9 \times 9}
	\left[
	\begin{array}{c}
		\delta \phi_{ik} \\
		\delta \mathrm{v}_{ik} \\
		\delta p_{ik} 
	\end{array}
	\right]_{9\times 1}
	+
	\left[
	\begin{array}{cc}
		J_r^k\Delta t & 0_{3\times 3} \\
		0_{3\times 3} & \Delta \tilde{R}_{ik}\Delta t \\
		0_{3\times 3} & \frac{1}{2}\tilde{R}_{ik}\Delta t^2
	\end{array}
	\right]_{9\times 6}
	\left[
	\begin{array}{c}
		\eta_k^{gd} \\
		\eta_k^{ad} 
	\end{array}
	\right]_{6\times 1}
\end{equation}
可以简记成
\begin{equation}
	\eta_{ik+1}^{\Delta} = A\eta_{ik}^{\Delta} + B\eta_{k}^{d}
\end{equation}
因此，噪声的传播方程为
\begin{equation}
	\Sigma_{ik+1}=A\Sigma_{ik}A^{\mathrm{T}}+B\Sigma_{\eta}B^{\mathrm{T}}
\end{equation}
其中初始值为$\Sigma_{ii}=0_{9\times 9}$。

\subsection*{融合零偏更新}
在上一节中，将零偏值认为是常数，然而实际优化中零偏估计会改变，而重新计算测量的增量计算量会很大，可以使用一阶近似更新，公式如下
\begin{equation}
	\begin{array}{l}
		\Delta \tilde{R}_{ij}(b_i^g)\approx \Delta \tilde{R}_{ij}(b_i^g)\mathrm{Exp}(\frac{\partial \Delta \tilde{R}_{ij}}{\partial{b^g}}\delta b^g) \\
		\Delta \tilde{\mathrm{v}}_{ij}(b_i^g, b_i^a)\approx \Delta \tilde{\mathrm{v}}_{ij}(\tilde{b}_i^g, \tilde{b}_i^a) + \frac{\Delta \tilde{\mathrm{v}}_{ij}}{\partial{b^g}}\delta b_i^g + \frac{\Delta \tilde{\mathrm{v}}_{ij}}{\partial{b^a}}\delta b_i^a \\
		\Delta \tilde{\mathrm{p}}_{ij}(b_i^g, b_i^a)\approx \Delta \tilde{\mathrm{p}}_{ij}(\tilde{b}_i^g, \tilde{b}_i^a) + \frac{\Delta \tilde{\mathrm{p}}_{ij}}{\partial{b^g}}\delta b_i^g + \frac{\Delta \tilde{\mathrm{p}}_{ij}}{\partial{b^a}}\delta b_i^a
	\end{array}
\end{equation}

\subsection*{预融合因子}
在(\ref{eq:imu_measure_eq})中测量噪声是一阶零均值、高斯的，因此很容易得到残差:
\begin{equation}
	\begin{array}{l}
		r_{\Delta R_{ij}} = \mathrm{Log}((\Delta \tilde{R}_{ij}(\tilde{b}_i^g)\mathrm{Exp}(\frac{\partial{\Delta \tilde{R}_{ij}}}{\partial{b^g}}\delta b^g))^{\mathrm{T}}R_i^{\mathrm{T}}R_j) \\
		r_{\Delta \mathrm{v}_{ij}} = R_i^{\mathrm{T}}(\mathrm{v}_j - \mathrm{v}_i - g\Delta t_{ij}) - [\Delta \tilde{\mathrm{v}}_{ij}(\hat{b}_i^g, \hat{b}_i^a)+\frac{\partial{\Delta \tilde{\mathrm{v}}_{ij}}}{\partial{b^g}}\delta b^g+\frac{\partial{\Delta \tilde{\mathrm{v}}_{ij}}}{\partial{b^g}}\delta b^g] \\
		r_{\Delta \mathrm{p}_{ij}} = R_i^{\mathrm{T}}(\mathrm{p}_j - \mathrm{p}_i - \mathrm{v}_i\Delta t_{ij} - \frac{1}{2}g\Delta t_{ij}^2) - [\Delta \tilde{\mathrm{p}}_{ij}(\hat{b}_i^g, \hat{b}_i^a)+\frac{\partial{\Delta \tilde{p}_{ij}}}{\partial{b^g}}\delta b^g+\frac{\partial{\Delta \tilde{p}_{ij}}}{\partial{b^g}}\delta b^g] \\
	\end{array}
\end{equation}
上面的残差可以进行G-N迭代，从而得到$R,\mathrm{v},\mathrm{p}$的更新，下面加入零偏$b$的更新。
\subsection*{零偏模型}
在(\ref{eq:gyro_model},\ref{eq:acc_model})介绍IMU测量模型的时候，其中包含零偏项$b^g,b^a$，它们是缓变的。因此，可以将它们建模为布朗运动，如式所示：
\begin{equation}
	\dot{b}^g(t) = \eta^{bg}, \quad \dot{b}^a(t) = \eta^{ba}
	\label{eq:integrated_whit_noise}
\end{equation}
在时间段$[t_i, t_j]$积分，得到两个相邻帧$i$和$j$的零偏：
\begin{equation}
	b_j^g= b_i^g + \eta^{bgd}, \quad b_j^a=b_i^a + \eta^{bad}
	\label{eq:two_frame}
\end{equation}
其中，零散时间噪声$\eta^{bgd},\eta^{bad}$是零均值的，并且协方差为$\Sigma^{bgd}=\Delta t_{ij}\mathrm{Cov}(\eta^{bg}),\Sigma^{bad}=\Delta t_{ij}\mathrm{Cov}(\eta^{ba})$。
\end{document}