updated ImuFactor doc with details about CombinedImuFactor

doc/ImuFactor.lyx

@@ -108,6 +108,222 @@ filename "macros.lyx"
 
 \end_layout
 
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
+{\mathfrak{\mathbb{R}^{9}}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rninethree}{\mathfrak{\mathbb{R}^{9\times3}}}
+{\mathfrak{\mathbb{R}^{9\times3}}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rninesix}{\mathfrak{\mathbb{R}^{9\times6}}}
+{\mathfrak{\mathbb{R}^{9\times6}}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rninenine}{\mathfrak{\mathbb{R}^{9\times9}}}
+{\mathfrak{\mathbb{R}^{9\times9}}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsubsection*
+IMU Factor
+\end_layout
+
+\begin_layout Standard
+The IMU factor has 2 variants:
+\end_layout
+
+\begin_layout Enumerate
+ImuFactor is a 5-way factor between the previous pose and velocity, the
+ current pose and velocity, and the current IMU bias.
+\end_layout
+
+\begin_layout Enumerate
+ImuFactor2 is a 3-way factor between the previous NavState, the current
+ NavState and the current IMU bias.
+\end_layout
+
+\begin_layout Standard
+Both variants take a PreintegratedMeasurements object which encodes all
+ the IMU measurements between the previous timestep and the current timestep.
+\end_layout
+
+\begin_layout Standard
+There are also 2 variants of this class:
+\end_layout
+
+\begin_layout Enumerate
+Manifold Preintegration: This version keeps track of the incremental NavState
+ 
+\begin_inset Formula $\Delta X_{ij}$
+\end_inset
+
+ with respect to the previous NavState, on the NavState manifold itself.
+ It also keeps track of the 
+\begin_inset Formula $\Rninesix$
+\end_inset
+
+ Jacobian of 
+\begin_inset Formula $\Delta X_{ij}$
+\end_inset
+
+ w.r.t.
+ the bias.
+ This corresponds to Forster et.
+ al.
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Forster15rss"
+literal "false"
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+Tangent Preintegration: This version keeps track of the incremental NavState
+ in the NavState tangent space instead.
+ This is a 
+\begin_inset Formula $\Rnine$
+\end_inset
+
+ vector 
+\emph on
+preintegrated_
+\emph default
+.
+ It also keeps track of the 
+\begin_inset Formula $\Rninesix$
+\end_inset
+
+ jacobian of the 
+\emph on
+preintegrated_
+\emph default
+ w.r.t.
+ the bias.
+ 
+\end_layout
+
+\begin_layout Standard
+The main function of a factor is to calculate an error.
+ The easiest case to look at is the NavState variant in ImuFactor2, which
+ is given as:
+\begin_inset Formula 
+\begin{equation}
+\Delta X_{ij}=X_{j}-\hat{X_{ij}}\label{eq:imu-factor-error}
+\end{equation}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsubsection*
+Combined IMU Factor
+\end_layout
+
+\begin_layout Standard
+The IMU factor above requires that bias drift over time be modeled as a
+ separate stochastic process (using a BetweenFactor for example), a crucial
+ aspect given that the preintegrated measurements depend on these bias values
+ and are thus correlated.
+ For this reason, we provide another type of IMU factor which we term the
+ Combined IMU Factor.
+ This factor similarly has 2 variants:
+\end_layout
+
+\begin_layout Enumerate
+CombinedImuFactor is a 6-way factor between the previous pose, velocity
+ and IMU bias and the current pose, velocity and IMU bias.
+\end_layout
+
+\begin_layout Enumerate
+CombinedImuFactor2 is a 4-way factor between the previous NavState and IMU
+ bias and the current NavState and IMU bias.
+\end_layout
+
+\begin_layout Subsubsection*
+Covariance Matrices
+\end_layout
+
+\begin_layout Standard
+For IMU preintegration, it is important to propagate the uncertainty accurately
+ as well.
+ As such, we detail the various covariance matrices used in the preintegration
+ step.
+\end_layout
+
+\begin_layout Itemize
+Gyroscope Covariance 
+\begin_inset Formula $Q_{\omega}$
+\end_inset
+
+: Measurement uncertainty of the gyroscope.
+\end_layout
+
+\begin_layout Itemize
+Accelerometer Covariance 
+\begin_inset Formula $Q_{acc}$
+\end_inset
+
+ : Measurement uncertainty of the accelerometer.
+\end_layout
+
+\begin_layout Itemize
+Accelerometer Bias Covariance 
+\begin_inset Formula $Q_{\Delta b^{acc}}$
+\end_inset
+
+ : The covariance associated with the accelerometer bias random walk.
+\end_layout
+
+\begin_layout Itemize
+Gyroscope Bias Covariance 
+\begin_inset Formula $Q_{\Delta b^{\omega}}$
+\end_inset
+
+ : The covariance associated with the gyroscope bias random walk.
+\end_layout
+
+\begin_layout Itemize
+Integration Covariance 
+\begin_inset Formula $Q_{int}$
+\end_inset
+
+ : This is the uncertainty due to modeling errors in the integration from
+ acceleration to velocity and position.
+\end_layout
+
+\begin_layout Itemize
+Initial Bias Estimate Covariance 
+\begin_inset Formula $Q_{init}$
+\end_inset
+
+ : This is the uncertainty associated with the estimation of the bias (since
+ we jointly estimate the bias as well).
+\end_layout
+
 \begin_layout Subsubsection*
 Navigation States
 \end_layout
@@ -725,15 +941,6 @@ In other words, the vector field
 Retractions
 \end_layout
 
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
-{\mathfrak{\mathbb{R}^{9}}}
-\end_inset
-
-
-\end_layout
-
 \begin_layout Standard
 Note that the use of the exponential map in local coordinate mappings is
  not obligatory, even in the context of Lie groups.
@@ -1018,7 +1225,15 @@ In the IMU factor, we need to predict the NavState
 
  needs to be known in order to compensate properly for the initial velocity
  and rotated gravity vector.
- Hence, the idea of Lupton was to split up 
+ Hence, the idea of Lupton
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Lupton12tro"
+literal "false"
+
+\end_inset
+
+ was to split up 
 \begin_inset Formula $v(t)$
 \end_inset
 
@@ -1075,8 +1290,11 @@ p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
 
 \end_inset
 
-The recipe for the IMU factor is then, in summary.
+The recipe for the IMU factor is then, in summary:
- Solve the ordinary differential equations
+\end_layout
+
+\begin_layout Enumerate
+Solve the ordinary differential equations
 \begin_inset Formula 
 \begin{eqnarray*}
 \dot{\theta}(t) & = & H(\theta(t))^{-1}\,\omega^{b}(t)\\
@@ -1095,7 +1313,10 @@ starting from zero, up to time
 \end_inset
 
  at all times.
- Form the local coordinate vector as
+\end_layout
+
+\begin_layout Enumerate
+Form the local coordinate vector as
 \begin_inset Formula 
 \[
 \zeta(t_{ij})=\left[\theta(t_{ij}),p(t_{ij}),v(t_{ij})\right]=\left[\theta(t_{ij}),R_{i}^{T}V_{i}t_{ij}+R_{i}^{T}\frac{gt_{ij}^{2}}{2}+p_{v}(t_{ij}),R_{i}^{T}gt_{ij}+v_{a}(t_{ij})\right]
@@ -1103,6 +1324,10 @@ starting from zero, up to time
 
 \end_inset
 
+
+\end_layout
+
+\begin_layout Enumerate
 Predict the NavState 
 \begin_inset Formula $X_{j}$
 \end_inset
@@ -1197,7 +1422,50 @@ where we defined the rotation matrix
 \end_layout
 
 \begin_layout Subsubsection*
-Noise Propagation
+Noise Modeling
+\end_layout
+
+\begin_layout Standard
+Given the above solutions to the differential equations, we add noise modeling
+ to account for the various sources of error in the system 
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{eqnarray}
+\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,(\omega_{k}^{b}+\epsilon_{k}^{\omega} -b_{k}^{\omega}-\epsilon_{init}^{\omega})\Delta_{t}\nonumber \\
+p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\frac{\Delta_{t}^{2}}{2}+\epsilon_{k}^{int}\label{eq:preintegration}\\
+v_{k+1} & = & v_{k}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\Delta_{t}\nonumber \\
+b_{k+1}^{a} & = & b_{k}^{a}+\epsilon_{k}^{b^{a}}\nonumber \\
+b_{k+1}^{\omega} & = & b_{k}^{\omega}+\epsilon_{k}^{b^{\omega}}\nonumber 
+\end{eqnarray}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+which we can write compactly as,
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{eqnarray}
+\theta_{k+1} & = & f_{\theta}(\theta_{k},b_{k}^{w},\epsilon_{k}^{\omega},\epsilon_{init}^{b^{\omega}})\label{eq:compact-preintegration}\\
+p_{k+1} & = & f_{p}(p_{k},v_{k},\theta_{k},b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a},\epsilon_{k}^{int})\nonumber \\
+v_{k+1} & = & f_{v}(v_{k,}\theta_{k,}b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a})\nonumber \\
+b_{k+1}^{a} & = & f_{b^{a}}(b_{k}^{a},\epsilon_{k}^{b^{a}})\nonumber \\
+b_{k+1}^{\omega} & = & f_{b^{\omega}}(b_{k}^{\omega},\epsilon_{k}^{b^{\omega}})\nonumber 
+\end{eqnarray}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsubsection*
+Noise Propagation in IMU Factor
 \end_layout
 
 \begin_layout Standard
@@ -1257,7 +1525,7 @@ Then the noise on
  propagates as
 \begin_inset Formula 
 \begin{equation}
-\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop}
+\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}^{T}+C_{k}\Sigma_{\eta}^{gd}C_{k}^{T}\label{eq:prop}
 \end{equation}
 
 \end_inset
@@ -1313,7 +1581,7 @@ We start with the noise propagation on
 \begin_layout Standard
 \begin_inset Formula 
 \[
-\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
+\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
 \]
 
 \end_inset
@@ -1325,7 +1593,7 @@ It can be shown that for small
  we have 
 \begin_inset Formula 
 \[
-\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}}
+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}}
 \]
 
 \end_inset
@@ -1375,9 +1643,9 @@ Putting all this together, we finally obtain
 \begin_inset Formula 
 \[
 A_{k}\approx\left[\begin{array}{ccc}
-I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\
+I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3}\\
 R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
-R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} &  & I_{3\times3}
+R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3}
 \end{array}\right]
 \]
 
@@ -1403,26 +1671,12 @@ H(\theta_{k})^{-1}\Delta_{t}\\
 \end_layout
 
 \begin_layout Subsubsection*
-Combined IMU Factor
+Noise Propagation in Combined IMU Factor
 \end_layout
 
 \begin_layout Standard
 We can similarly account for bias drift over time, as is commonly seen in
  commercial grade IMUs.
- This is accomplished via the 
-\emph on
-CombinedImuFactor
-\emph default
- which is a 6-way factor between the previous 
-\emph on
-pose/velocity/bias
-\emph default
- and the 
-\emph on
-pose/velocity/bias
-\emph default
- at the next timestep.
- 
 \end_layout
 
 \begin_layout Standard
@@ -1430,14 +1684,18 @@ We expand the state vector as
 \begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$
 \end_inset
 
- to include the bias terms.
+ to include the bias terms and define the augmented noise vector 
+\begin_inset Formula $\epsilon=[\epsilon_{k}^{\omega},\epsilon_{k}^{a},\epsilon_{k}^{b^{a}},\epsilon_{k}^{b^{\omega}},\epsilon_{k}^{int},\epsilon_{init}^{b^{a}},\epsilon_{init}^{b^{\omega}}]$
+\end_inset
+
+.
  This gives the noise propagation equation as
 \end_layout
 
 \begin_layout Standard
 \begin_inset Formula 
 \begin{equation}
-\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}\Sigma_{k}G_{k}\label{eq:prop-combined}
+\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}Q_{k}G_{k}^{T}\label{eq:prop-combined}
 \end{equation}
 
 \end_inset
@@ -1467,10 +1725,19 @@ where
 \end_inset
 
  is the 
-\begin_inset Formula $15\times15$
+\begin_inset Formula $15\times21$
 \end_inset
 
  matrix for first order uncertainty propagation.
+ 
+\begin_inset Formula $Q_{k}$
+\end_inset
+
+ defines the uncertainty of 
+\begin_inset Formula $\eta$
+\end_inset
+
+.
  The top-left 
 \begin_inset Formula $9\times9$
 \end_inset
@@ -1509,22 +1776,213 @@ derivation as matrices
 \begin_inset Formula 
 \[
 F_{k}\approx\left[\begin{array}{ccccc}
-I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} &  &  &  & H(\theta_{k})^{-1}\Delta_{t}\\
+I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & H(\theta_{k})^{-1}\Delta_{t}\\
-R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2}\\
+R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2} & 0_{3\times3}\\
-R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} &  & I_{3\times3} & R_{k}\Delta_{t}\\
+R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\
- &  &  & I_{3\times3}\\
+0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3}\\
- &  &  &  & I_{3\times3}
+0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3}
 \end{array}\right]
 \]
 
 \end_inset
 
 
+\end_layout
+
+\begin_layout Standard
+Similarly for 
+\begin_inset Formula $Q_{k},$
+\end_inset
+
+we get
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+Q_{k}=\left[\begin{array}{ccccccc}
+\Sigma^{\omega}\\
+ & \Sigma^{a}\\
+ &  & \Sigma^{b^{a}}\\
+ &  &  & \Sigma^{b^{\omega}}\\
+ &  &  &  & \Sigma^{int}\\
+ &  &  &  &  & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
+ &  &  &  &  & \Sigma^{init_{21}} & \Sigma^{init_{22}}
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+and for 
+\begin_inset Formula $G_{k}$
+\end_inset
+
+ we get
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+G_{k}=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}} & \deriv{\theta}{\epsilon^{a}} & \deriv{\theta}{\epsilon^{b^{a}}} & \deriv{\theta}{\epsilon^{b^{\omega}}} & \deriv{\theta}{\epsilon^{int}} & \deriv{\theta}{\epsilon_{init}^{b^{a}}} & \deriv{\theta}{\epsilon_{init}^{b^{\omega}}}\\
+\deriv p{\epsilon^{\omega}} & \deriv p{\epsilon^{a}} & \deriv p{\epsilon^{b^{a}}} & \deriv p{\epsilon^{b^{\omega}}} & \deriv p{\epsilon^{int}} & \deriv p{\epsilon_{init}^{b^{a}}} & \deriv p{\epsilon_{init}^{b^{\omega}}}\\
+\deriv v{\epsilon^{\omega}} & \deriv v{\epsilon^{a}} & \deriv v{\epsilon^{b^{a}}} & \deriv v{\epsilon^{b^{\omega}}} & \deriv v{\epsilon^{int}} & \deriv v{\epsilon_{init}^{b^{a}}} & \deriv v{\epsilon_{init}^{b^{\omega}}}\\
+\deriv{b^{a}}{\epsilon^{\omega}} & \deriv{b^{a}}{\epsilon^{a}} & \deriv{b^{a}}{\epsilon^{b^{a}}} & \deriv{b^{a}}{\epsilon^{b^{\omega}}} & \deriv{b^{a}}{\epsilon^{int}} & \deriv{b^{a}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{a}}{\epsilon_{init}^{b^{\omega}}}\\
+\deriv{b^{\omega}}{\epsilon^{\omega}} & \deriv{b^{\omega}}{\epsilon^{a}} & \deriv{b^{\omega}}{\epsilon^{b^{a}}} & \deriv{b^{\omega}}{\epsilon^{b^{\omega}}} & \deriv{b^{\omega}}{\epsilon^{int}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{\omega}}}
+\end{array}\right]=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
+0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
+0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
+0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+We can perform the block-wise computation of 
+\begin_inset Formula $G_{k}Q_{k}G_{k}^{T}$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
+0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
+0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
+0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
+\end{array}\right]\left[\begin{array}{ccccccc}
+\Sigma^{\omega}\\
+ & \Sigma^{a}\\
+ &  & \Sigma^{b^{a}}\\
+ &  &  & \Sigma^{b^{\omega}}\\
+ &  &  &  & \Sigma^{int}\\
+ &  &  &  &  & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
+ &  &  &  &  & \Sigma^{init_{21}} & \Sigma^{init_{22}}
+\end{array}\right]G_{k}^{T}
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
+0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
+0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
+0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
+\end{array}\right]G_{k}^{T}
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{multline*}
+G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
+0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
+0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
+0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
+\end{array}\right]\\
+\left[\begin{array}{ccccc}
+\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0\\
+0 & \deriv p{\epsilon^{a}} & \deriv v{\epsilon^{a}} & 0 & 0\\
+0 & 0 & 0 & I_{3\times3} & 0\\
+0 & 0 & 0 & 0 & I_{3\times3}\\
+0 & \deriv p{\epsilon^{int}} & 0 & 0 & 0\\
+0 & \deriv p{\eta_{init}^{b^{a}}} & \deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
+\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & 0 & 0 & 0
+\end{array}\right]
+\end{multline*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{multline*}
+=\\
+\left[\begin{array}{ccccc}
+\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}\\
+ & +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
+0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
+0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
+\end{array}\right]
+\end{multline*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+which we can break into 3 matrices for clarity, representing the main diagonal
+ and off-diagonal elements
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{multline*}
+=\\
+\left[\begin{array}{ccccc}
+\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0\\
+0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}} & 0 & 0 & 0\\
+0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}} & 0 & 0\\
+0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
+0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
+\end{array}\right]+\\
+\left[\begin{array}{ccccc}
+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & 0 & 0 & 0\\
+0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & 0 & 0 & 0\\
+0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
+0 & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & 0 & 0
+\end{array}\right]+\\
+\left[\begin{array}{ccccc}
+0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & 0 & 0 & 0\\
+0 & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & 0 & 0
+\end{array}\right]
+\end{multline*}
+
+\end_inset
+
+
 \end_layout
 
 \begin_layout Standard
 \begin_inset CommandInset bibtex
 LatexCommand bibtex
+btprint "btPrintCited"
 bibfiles "refs"
 options "plain"
 

doc/refs.bib

@@ -1,26 +1,52 @@
+%% This BibTeX bibliography file was created using BibDesk.
+%% https://bibdesk.sourceforge.io/
+
+%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400 
+
+
+%% Saved with string encoding Unicode (UTF-8) 
+
+
+
+@article{Lupton12tro,
+	author = {Lupton, Todd and Sukkarieh, Salah},
+	date-added = {2021-09-27 17:38:56 -0400},
+	date-modified = {2021-09-27 17:39:09 -0400},
+	doi = {10.1109/TRO.2011.2170332},
+	journal = {IEEE Transactions on Robotics},
+	number = {1},
+	pages = {61-76},
+	title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
+	volume = {28},
+	year = {2012},
+	Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
+
+@inproceedings{Forster15rss,
+	author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
+	booktitle = {Robotics: Science and Systems},
+	date-added = {2021-09-26 20:44:41 -0400},
+	date-modified = {2021-09-26 20:45:03 -0400},
+	title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
+	year = {2015}}
+
 @article{Iserles00an,
-  title =	 {Lie-group methods},
+	author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
-  author =	 {Iserles, Arieh and Munthe-Kaas, Hans Z and
+	journal = {Acta Numerica 2000},
-                  N{\o}rsett, Syvert P and Zanna, Antonella},
+	pages = {215--365},
-  journal =	 {Acta Numerica 2000},
+	publisher = {Cambridge Univ Press},
-  volume =	 {9},
+	title = {Lie-group methods},
-  pages =	 {215--365},
+	volume = {9},
-  year =	 {2000},
+	year = {2000}}
-  publisher =	 {Cambridge Univ Press}
-}
 
 @book{Murray94book,
-  title =	 {A mathematical introduction to robotic manipulation},
+	author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
-  author =	 {Murray, Richard M and Li, Zexiang and Sastry, S
+	publisher = {CRC press},
-                  Shankar and Sastry, S Shankara},
+	title = {A mathematical introduction to robotic manipulation},
-  year =	 {1994},
+	year = {1994}}
-  publisher =	 {CRC press}
-}
 
 @book{Spivak65book,
-  title =	 {Calculus on manifolds},
+	author = {Spivak, Michael},
-  author =	 {Spivak, Michael},
+	publisher = {WA Benjamin New York},
-  volume =	 {1},
+	title = {Calculus on manifolds},
-  year =	 {1965},
+	volume = {1},
-  publisher =	 {WA Benjamin New York}
+	year = {1965}}
-}