More Oct changes in doc
parent
a03924eb85
commit
c2b024055d
|
@ -601,9 +601,154 @@ A crucial detail is that the incremental position
|
|||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The goal of the IMU factor is to integrate IMU measurement between two successiv
|
||||
e frames and create a binary factor between two NavStates.
|
||||
The binary factor is set up as a prediction:
|
||||
The goal of the IMU factor is to integrate IMU measurements between two
|
||||
successive frames and create a binary factor between two NavStates.
|
||||
Integrating successive gyro and accelerometer readings using the above
|
||||
equations over each constant interval yields
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray}
|
||||
R_{k+1} & = & R_{k}\exp\hat{\omega}_{k}\Delta t_{k}\label{eq:iter_Rk}\\
|
||||
p_{k+1} & = & p_{k}+v_{k}\Delta t_{k}+\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}=p_{k}+\frac{v_{k}+v_{k+1}}{2}\Delta t_{k}\nonumber \\
|
||||
v_{k+1} & = & v_{k}+(g+R_{k}a_{k})\Delta t_{k}\nonumber
|
||||
\end{eqnarray}
|
||||
|
||||
\end_inset
|
||||
|
||||
Starting
|
||||
\begin_inset Formula $X_{i}=(R_{i},p_{i},v_{i})$
|
||||
\end_inset
|
||||
|
||||
, we then obtain an expression for
|
||||
\begin_inset Formula $X_{j}$
|
||||
\end_inset
|
||||
|
||||
,
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray*}
|
||||
R_{j} & = & R_{i}\prod_{k}\exp\hat{\omega}_{k}\Delta t_{k}\\
|
||||
p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}\\
|
||||
v_{j} & = & v_{i}+\sum_{k}(g+R_{k}a_{k})\Delta t_{k}
|
||||
\end{eqnarray*}
|
||||
|
||||
\end_inset
|
||||
|
||||
where
|
||||
\begin_inset Formula $R_{k}$
|
||||
\end_inset
|
||||
|
||||
has to be updated as in
|
||||
\begin_inset CommandInset ref
|
||||
LatexCommand formatted
|
||||
reference "eq:iter_Rk"
|
||||
|
||||
\end_inset
|
||||
|
||||
.
|
||||
Note that
|
||||
\begin_inset Formula
|
||||
\[
|
||||
v_{k}=v_{i}+\sum_{l}(g+R_{l}a_{l})\Delta t_{l}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
and hence
|
||||
\begin_inset Formula
|
||||
\[
|
||||
p_{j}=p_{i}+\sum_{k}\left(v_{i}+\sum_{l}(g+R_{l}a_{l})\Delta t_{l}\right)\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
A crucial problem here is that we depend on a good estimate of
|
||||
\begin_inset Formula $R_{k}$
|
||||
\end_inset
|
||||
|
||||
at all times, which includes the potentially wrong estimate for the initial
|
||||
attitude
|
||||
\begin_inset Formula $R_{i}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The idea behind the preintegrated IMU factor is two-fold: (a) treat gravity
|
||||
separately, in the navigation frame, and (b) instead of integrating in
|
||||
absolute coordinates, we want the IMU integration to happen in the frame
|
||||
|
||||
\begin_inset Formula $(R_{i},p_{i},v_{i})$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
The first idea is easily incorporated by separating out all gravity-related
|
||||
components:
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray*}
|
||||
p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}+\sum_{k}v_{k}\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}\\
|
||||
v_{j} & = & v_{i}+g\sum_{k}\Delta t_{k}+\sum_{k}R_{k}a_{k}\Delta t_{k}
|
||||
\end{eqnarray*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
But we need to define what that means: clearly,
|
||||
\begin_inset Formula $SE(3)$
|
||||
\end_inset
|
||||
|
||||
, with its group structure, has a well-defined concept of working
|
||||
\begin_inset Quotes eld
|
||||
\end_inset
|
||||
|
||||
in the frame
|
||||
\begin_inset Quotes erd
|
||||
\end_inset
|
||||
|
||||
.
|
||||
But in this case we have a manifold, not a group.
|
||||
To deal with this, we perform the integration in the tangent space, and
|
||||
hence we need to define a mapping from tangent space to manifold and vice
|
||||
versa.
|
||||
A possible definition of retract, compatible with the semi-direct group
|
||||
structure of
|
||||
\begin_inset Formula $SE(3)$
|
||||
\end_inset
|
||||
|
||||
and treating velocities the same way as positions, is given by
|
||||
\begin_inset Formula
|
||||
\[
|
||||
X\oplus dX=(R,p,v)\oplus(\omega t,\Delta p,\Delta v)=(R\exp\hat{\omega}t,p+R\Delta p,v+R\Delta v)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray*}
|
||||
R_{j} & = & R_{i}\prod_{k}\exp\hat{\omega}_{k}\Delta t_{k}\\
|
||||
p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t_{k}+\frac{1}{2}g\sum_{k}\Delta t_{k}^{2}+\frac{1}{2}\sum_{k}R_{k}a_{k}\Delta t^{2}\\
|
||||
v_{j} & = & v_{i}+g\Delta t_{ij}+\sum_{k}R_{k}a_{k}\Delta t_{k}
|
||||
\end{eqnarray*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Note Note
|
||||
status open
|
||||
|
||||
\begin_layout Plain Layout
|
||||
The binary factor is set up as a prediction:
|
||||
\begin_inset Formula
|
||||
\[
|
||||
X_{j}\approx X_{i}\oplus dX_{ij}
|
||||
|
@ -627,7 +772,7 @@ where
|
|||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_layout Plain Layout
|
||||
Integrating gyro and accelerometer readings, following Forster05rss, and
|
||||
assuming zero bias for now, is done by:
|
||||
\begin_inset Formula
|
||||
|
@ -653,7 +798,7 @@ v_{j} & = & \left\{ v_{i}+g\Delta t_{ij}\right\} +\sum_{k}R_{k}a_{k}\Delta t
|
|||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_layout Plain Layout
|
||||
Let us look at a single interval of the incremental terms:
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray*}
|
||||
|
@ -673,6 +818,11 @@ R_{j}=R_{i}\oplus\left(\omega,R_{i}^{T}v_{i}+\frac{1}{2}R_{i}^{T}g\Delta t+\frac
|
|||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\end_body
|
||||
|
|
Loading…
Reference in New Issue