More Oct changes in doc

doc/ImuFactor.lyx

@@ -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
+The goal of the IMU factor is to integrate IMU measurements between two
-e frames and create a binary factor between two NavStates.
+ successive frames and create a binary factor between two NavStates.
- The binary factor is set up as a prediction:
+ 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