1 Star 1 Fork 0

jian-li / imupreintergration

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Clone or download
document.tex 14.89 KB
Copy Edit Web IDE Raw Blame History
jian-li authored 2016-07-23 20:40 . add noise propogation
\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}

Comment ( 0 )

Sign in for post a comment

1
https://gitee.com/JaneLee/imupreintergration.git
git@gitee.com:JaneLee/imupreintergration.git
JaneLee
imupreintergration
imupreintergration
master

Search