## jian-li / imupreintergration .gitee-modal { width: 500px !important; }

Explore and code with more than 6 million developers，Free private repositories ！：）
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
document.tex 14.89 KB
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*{基本公式}

\label{eq:eq_1}

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

\exp(\phi^{\wedge})\approx \mathbf{I}+ \phi^{\wedge}
\label{eq:eq_3}

$SO(3)$的对数映射公式为

\mathrm{Exp}(\phi + \delta \phi)\approx \mathrm{Exp}(\phi)\mathrm{Exp}(J_r(\phi)\delta \phi)

\mathrm{Log}(\mathrm{Exp}(\phi)\mathrm{Exp}(\delta \phi)) \approx \phi + J_r^{-1}(\phi)\delta \phi

R\mathrm{Exp}(\Phi)R^{\mathrm{T}}=\mathrm{exp}(R\phi^{\wedge}R^{\mathrm{T}})=\mathrm{Exp}(R\phi)

\section*{IMU模型}

{}_{\mathrm{B}}\tilde{\omega}_{\mathrm{WB}}(t)={}_{\mathrm{B}}\omega_{\mathrm{WB}}(t)+b^g(t)+\eta^g(t)
\label{eq:gyro_model}

{}_{\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}

IMU的运动学模型为($W$系到$B$系)

{}_{\mathrm{W}}\dot{\mathrm{p}}={}_{\mathrm{W}}\mathrm{v}

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

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

\section*{流形上的预积分}

\begin{figure}[h]
\centering
\includegraphics[width=12cm]{images/clck_sync}
\end{figure}

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

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

\subsection*{IMU测量预积分}

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

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

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

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

\subsection*{噪声传播}

\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)

\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))

\delta \phi_{ij} \approx \sum_{k=i}^{j-1}\Delta \tilde{R}_{k+1j}^{\mathrm{T}}J_r^k \eta_k^{gd} \Delta t

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

\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} \\
\end{array}
\right]_{6\times 1}

\eta_{ik+1}^{\Delta} = A\eta_{ik}^{\Delta} + B\eta_{k}^{d}

\Sigma_{ik+1}=A\Sigma_{ik}A^{\mathrm{T}}+B\Sigma_{\eta}B^{\mathrm{T}}

\subsection*{融合零偏更新}

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

\subsection*{预融合因子}

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

\subsection*{零偏模型}

\dot{b}^g(t) = \eta^{bg}, \quad \dot{b}^a(t) = \eta^{ba}
\label{eq:integrated_whit_noise}

\label{eq:two_frame}

\end{document}