#LyX 2.3 created this file. For more info see http://www.lyx.org/
\lyxformat 544
\begin_document
\begin_header
\save_transient_properties true
\origin unavailable
\textclass article
\use_default_options true
\maintain_unincluded_children false
\language english
\language_package default
\inputencoding auto
\fontencoding global
\font_roman "default" "default"
\font_sans "default" "default"
\font_typewriter "default" "default"
\font_math "auto" "auto"
\font_default_family default
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100 100
\font_tt_scale 100 100
\use_microtype false
\use_dash_ligatures true
\graphics default
\default_output_format default
\output_sync 0
\bibtex_command default
\index_command default
\paperfontsize 11
\spacing single
\use_hyperref false
\papersize default
\use_geometry true
\use_package amsmath 1
\use_package amssymb 1
\use_package cancel 1
\use_package esint 1
\use_package mathdots 1
\use_package mathtools 1
\use_package mhchem 1
\use_package stackrel 1
\use_package stmaryrd 1
\use_package undertilde 1
\cite_engine basic
\cite_engine_type default
\biblio_style plain
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\justification true
\use_refstyle 1
\use_minted 0
\index Index
\shortcut idx
\color #008000
\end_index
\leftmargin 3cm
\topmargin 3cm
\rightmargin 3cm
\bottommargin 3cm
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\paragraph_indentation default
\is_math_indent 0
\math_numbering_side default
\quotes_style english
\dynamic_quotes 0
\papercolumns 1
\papersides 1
\paperpagestyle default
\tracking_changes false
\output_changes false
\html_math_output 0
\html_css_as_file 0
\html_be_strict false
\end_header

\begin_body

\begin_layout Title
The New IMU Factor
\end_layout

\begin_layout Author
Frank Dellaert & Varun Agrawal
\end_layout

\begin_layout Standard
\begin_inset CommandInset include
LatexCommand include
filename "macros.lyx"

\end_inset


\end_layout

\begin_layout Standard
\begin_inset FormulaMacro
\renewcommand{\sothree}{\mathfrak{so(3)}}
{\mathfrak{so(3)}}
\end_inset


\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.
 This is done exactly the same in both variants: 
\begin_inset Formula 
\begin{equation}
e(X_{i},X_{j})=X_{j}\ominus\widehat{X_{j}}\label{eq:imu-factor-error}
\end{equation}

\end_inset

where the predicted NavState 
\begin_inset Formula $\widehat{X_{j}}$
\end_inset

 at time 
\begin_inset Formula $t_{j}$
\end_inset

 is a function of the NavState 
\begin_inset Formula $X_{i}$
\end_inset

 at time 
\begin_inset Formula $t_{i}$
\end_inset

 and the preintegrated measurements 
\begin_inset Formula $PIM$
\end_inset

:
\begin_inset Formula 
\[
\widehat{X_{j}}=f(X_{i},PIM)
\]

\end_inset

The noise model associated with this factor is assumed to be zero-mean Gaussian
 with a 
\begin_inset Formula $9\times9$
\end_inset

 covariance matrix 
\begin_inset Formula $\Sigma_{ij}$
\end_inset

, which is defined in the tangent space 
\begin_inset Formula $T_{X_{j}}\mathcal{N}$
\end_inset

 of the NavState manifold at the NavState 
\begin_inset Formula $X_{j}$
\end_inset

.
 This (discrete-time) covariance matrix is computed in the preintegrated
 measurement class, of which there are two variants as discussed above.
\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 Standard
Since the Combined IMU Factor has a larger state variable due to the inclusion
 of IMU biases, the noise model associated with this factor is assumed to
 be a zero mean Gaussian with a 
\begin_inset Formula $15\times15$
\end_inset

 covariance matrix 
\begin_inset Formula $\Sigma$
\end_inset

, similarly defined on the tangent space of the NavState manifold.
\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
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
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
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

\begin_layout Standard
Let us assume a setup where frames with image and/or laser measurements
 are processed at some fairly low rate, e.g., 10 Hz.
\end_layout

\begin_layout Standard
We define the state of the vehicle at those times as attitude, position,
 and velocity.
 These three quantities are jointly referred to as a NavState 
\begin_inset Formula $X_{b}^{n}\define\left\{ R_{b}^{n},P_{b}^{n},V_{b}^{n}\right\} $
\end_inset

, where the superscript 
\begin_inset Formula $n$
\end_inset

 denotes the 
\emph on
navigation frame
\emph default
, and 
\begin_inset Formula $b$
\end_inset

 the 
\emph on
body frame
\emph default
.
 For simplicity, we drop these indices below where clear from context.
\end_layout

\begin_layout Subsubsection*
Vector Fields and Differential Equations
\end_layout

\begin_layout Standard
We need a way to describe the evolution of a NavState over time.
 The NavState lives in a 9-dimensional manifold 
\begin_inset Formula $M$
\end_inset

, defined by the orthonormality constraints on 
\begin_inset Formula $\Rone$
\end_inset

.
 For a NavState 
\begin_inset Formula $X$
\end_inset

 evolving over time we can write down a differential equation
\begin_inset Formula 
\begin{equation}
\dot{X}(t)=F(t,X)\label{eq:diffeqM}
\end{equation}

\end_inset

where 
\begin_inset Formula $F$
\end_inset

 is a time-varying 
\series bold
vector field
\series default
 on 
\begin_inset Formula $M$
\end_inset

, defined as a mapping from 
\begin_inset Formula $\Rone\times M$
\end_inset

 to tangent vectors at 
\begin_inset Formula $X$
\end_inset

.
 A 
\series bold
tangent vector
\series default
 at 
\begin_inset Formula $X$
\end_inset

 is defined as the derivative of a trajectory at 
\begin_inset Formula $X$
\end_inset

, and for the NavState manifold this will be a triplet 
\begin_inset Formula 
\[
\left[\dot{R}(t,X),\dot{P}(t,X),\dot{V}(t,X)\right]\in\sothree\times\Rthree\times\Rthree
\]

\end_inset

where we use square brackets to indicate a tangent vector.
 The space of all tangent vectors at 
\begin_inset Formula $X$
\end_inset

 is denoted by 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none

\begin_inset Formula $T_{X}M$
\end_inset


\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\uuline default
\uwave default
\noun default
\color inherit
, and hence 
\begin_inset Formula $F(t,X)\in T_{X}M$
\end_inset

.
 For example, if the state evolves along a constant velocity trajectory
\begin_inset Formula 
\[
X(t)=\left\{ R_{0},P_{0}+V_{0}t,V_{0}\right\} 
\]

\end_inset

then the differential equation describing the trajectory is
\begin_inset Formula 
\[
\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\,X(0)=\left\{ R_{0},P_{0},V_{0}\right\} 
\]

\end_inset


\end_layout

\begin_layout Standard
Valid vector fields on a NavState manifold are special, in that the attitude
 and velocity derivatives can be arbitrary functions of X and t, but the
 derivative of position is constrained to be equal to the current velocity
 
\begin_inset Formula $V(t)$
\end_inset

: 
\begin_inset Formula 
\begin{equation}
\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]\label{eq:validField}
\end{equation}

\end_inset

Suppose we are given the 
\series bold
body angular velocity
\series default
 
\begin_inset Formula $\omega^{b}(t)$
\end_inset

 and non-gravity 
\series bold
acceleration
\series default
 
\begin_inset Formula $a^{b}(t)$
\end_inset

 in the body frame.
 We know (from 
\begin_inset CommandInset citation
LatexCommand cite
key "Murray94book"
literal "false"

\end_inset

) that the derivative of 
\begin_inset Formula $R$
\end_inset

 can be written as 
\begin_inset Formula 
\[
\dot{R}(X,t)=R(t)\Skew{\omega^{b}(t)}
\]

\end_inset

where 
\begin_inset Formula $\Skew{\theta}\in so(3)$
\end_inset

 is the skew-symmetric matrix corresponding to 
\begin_inset Formula $\theta$
\end_inset

, and hence the resulting exact vector field is 
\begin_inset Formula 
\begin{equation}
\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]=\left[R(t)\Skew{\omega^{b}(t)},V(t),g+R(t)a^{b}(t)\right]\label{eq:bodyField}
\end{equation}

\end_inset


\end_layout

\begin_layout Subsubsection*
Local Coordinates
\end_layout

\begin_layout Standard
Optimization on manifolds relies crucially on the concept of 
\series bold
local coordinates
\series default
.
 For example, when optimizing over the rotations 
\begin_inset Formula $\SOthree$
\end_inset

 starting from an initial estimate 
\begin_inset Formula $R_{0}$
\end_inset

, we define a local map 
\begin_inset Formula $\Phi_{R_{0}}$
\end_inset

 from 
\begin_inset Formula $\theta\in\Rthree$
\end_inset

 to a neighborhood of 
\begin_inset Formula $\SOthree$
\end_inset

 centered around 
\begin_inset Formula $R_{0}$
\end_inset

, 
\begin_inset Formula 
\[
\Phi_{R_{0}}(\theta)=R_{0}\exp\left(\Skew{\theta}\right)
\]

\end_inset

where 
\begin_inset Formula $\exp$
\end_inset

 is the matrix exponential, given by
\begin_inset Formula 
\begin{equation}
\exp\left(\Skew{\theta}\right)=\sum_{k=0}^{\infty}\frac{1}{k!}\Skew{\theta}^{k}\label{eq:expm}
\end{equation}

\end_inset

which for 
\begin_inset Formula $\SOthree$
\end_inset

 can be efficiently computed in closed form.
\end_layout

\begin_layout Standard
The local coordinates 
\begin_inset Formula $\theta$
\end_inset

 are isomorphic to tangent vectors at 
\emph on

\begin_inset Formula $R_{0}$
\end_inset


\emph default
.
 To see this, define 
\begin_inset Formula $\theta=\omega t$
\end_inset

 and note that 
\begin_inset Formula 
\[
\frac{d\Phi_{R_{0}}\left(\omega t\right)}{dt}\biggr\vert_{t=0}=\frac{dR_{0}\exp\left(\Skew{\omega t}\right)}{dt}\biggr\vert_{t=0}=R_{0}\Skew{\omega t}
\]

\end_inset

Hence, the 3-vector 
\begin_inset Formula $\omega$
\end_inset

 defines a direction of travel on the 
\begin_inset Formula $\SOthree$
\end_inset

 manifold, but does so in the local coordinate frame define by 
\begin_inset Formula $R_{0}$
\end_inset

.
\end_layout

\begin_layout Standard
A similar story holds in 
\begin_inset Formula $\SEthree$
\end_inset

: we define local coordinates 
\begin_inset Formula $\xi=\left[\omega t,vt\right]\in\Rsix$
\end_inset

 and a mapping 
\begin_inset Formula 
\[
\Phi_{T_{0}}(\xi)=T_{0}\exp\xihat
\]

\end_inset

where 
\begin_inset Formula $\xihat\in\sethree$
\end_inset

 is defined as 
\begin_inset Formula 
\[
\xihat=\left[\begin{array}{cc}
\Skew{\omega} & v\\
0 & 0
\end{array}\right]t
\]

\end_inset

and the 6-vectors 
\begin_inset Formula $\xi$
\end_inset

 are mapped to tangent vectors 
\begin_inset Formula $T_{0}\xihat$
\end_inset

 at 
\begin_inset Formula $T_{0}$
\end_inset

.
\end_layout

\begin_layout Subsubsection*
Derivative of The Local Coordinate Mapping
\end_layout

\begin_layout Standard
For the local coordinate mapping 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none

\begin_inset Formula $\Phi_{R_{0}}\left(\theta\right)$
\end_inset


\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\uuline default
\uwave default
\noun default
\color inherit
 in 
\begin_inset Formula $\SOthree$
\end_inset

 we can define a 
\begin_inset Formula $3\times3$
\end_inset

 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none
Jacobian 
\begin_inset Formula $H(\theta)$
\end_inset

 that models the effect of an incremental change 
\begin_inset Formula $\delta$
\end_inset

 to the local coordinates:
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\uuline default
\uwave default
\noun default
\color inherit

\begin_inset Formula 
\begin{equation}
\Phi_{R_{0}}\left(\theta+\delta\right)\approx\Phi_{R_{0}}\left(\theta\right)\,\exp\left(\Skew{H(\theta)\delta}\right)=\Phi_{\Phi_{R_{0}}\left(\theta\right)}\left(H(\theta)\delta\right)\label{eq:push_exp}
\end{equation}

\end_inset

This Jacobian depends only on 
\begin_inset Formula $\theta$
\end_inset

 and, for the case of 
\begin_inset Formula $\SOthree$
\end_inset

, is given by a formula similar to the matrix exponential map, 
\begin_inset Formula 
\[
H(\theta)=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\Skew{\theta}^{k}
\]

\end_inset

which can also be computed in closed form.
 In particular, 
\begin_inset Formula $H(0)=I_{3\times3}$
\end_inset

 at the base 
\begin_inset Formula $R_{0}$
\end_inset

.
\end_layout

\begin_layout Subsubsection*
Numerical Integration in Local Coordinates
\end_layout

\begin_layout Standard
Inspired by the paper 
\begin_inset Quotes eld
\end_inset

Lie Group Methods
\begin_inset Quotes erd
\end_inset

 by Iserles et al.
 
\begin_inset CommandInset citation
LatexCommand cite
key "Iserles00an"
literal "true"

\end_inset

, when we have a differential equation on 
\begin_inset Formula $\SOthree$
\end_inset

,
\begin_inset Formula 
\begin{equation}
\dot{R}(t)=F(R,t),\,\,\,\,R(0)=R_{0}\label{eq:diffSo3}
\end{equation}

\end_inset

we can transfer it to a differential equation in the 3-dimensional local
 coordinate space.
 To do so, we model the solution to 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:diffSo3"

\end_inset

 as
\begin_inset Formula 
\[
R(t)=\Phi_{R_{0}}(\theta(t))
\]

\end_inset

To find an expression for 
\begin_inset Formula $\dot{\theta}(t)$
\end_inset

, create a trajectory 
\begin_inset Formula $\gamma(\delta)$
\end_inset

 that passes through 
\begin_inset Formula $R(t)$
\end_inset

 for 
\begin_inset Formula $\delta=0$
\end_inset

, and moves 
\begin_inset Formula $\theta(t)$
\end_inset

 along the direction 
\begin_inset Formula $\dot{\theta}(t)$
\end_inset

: 
\begin_inset Formula 
\[
\gamma(\delta)=R(t+\delta)=\Phi_{R_{0}}\left(\theta(t)+\dot{\theta}(t)\delta\right)\approx\Phi_{R(t)}\left(H(\theta)\dot{\theta}(t)\delta\right)
\]

\end_inset

Taking the derivative for 
\begin_inset Formula $\delta=0$
\end_inset

 we obtain
\begin_inset Formula 
\[
\dot{R}(t)=\frac{d\gamma(\delta)}{d\delta}\biggr\vert_{\delta=0}=\frac{d\Phi_{R(t)}\left(H(\theta)\dot{\theta}(t)\delta\right)}{d\delta}\biggr\vert_{\delta=0}=R(t)\Skew{H(\theta)\dot{\theta}(t)}
\]

\end_inset

Comparing this to 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:diffSo3"

\end_inset

 we obtain a differential equation for 
\begin_inset Formula $\theta(t)$
\end_inset

:
\begin_inset Formula 
\[
\dot{\theta}(t)=H(\theta)^{-1}\left\{ R(t)^{T}F(R,t)\right\} \check{},\,\,\,\,\theta(0)=0_{3\times1}
\]

\end_inset

In other words, the vector field 
\begin_inset Formula $F(R,t)$
\end_inset

 is rotated to the local frame, the inverse hat operator is applied to get
 a 3-vector, which is then corrected by 
\begin_inset Formula $H(\theta)^{-1}$
\end_inset

 away from 
\begin_inset Formula $\theta=0$
\end_inset

.
\end_layout

\begin_layout Subsubsection*
Retractions
\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.
 Often it is computationally expedient to use mappings that are easier to
 compute, but yet induce the same tangent vector at 
\begin_inset Formula $T_{0}.$
\end_inset

 Mappings that satisfy this constraint are collectively known as 
\series bold
retractions
\series default
.
 For example, for 
\begin_inset Formula $\SEthree$
\end_inset

 one could use the retraction 
\begin_inset Formula $\mathcal{R}_{T_{0}}:\Rsix\rightarrow\SEthree$
\end_inset

 
\begin_inset Formula 
\[
\mathcal{R}_{T_{0}}\left(\xi\right)=T_{0}\left\{ \exp\left(\Skew{\omega t}\right),vt\right\} =\left\{ \Phi_{R_{0}}\left(\omega t\right),P_{0}+R_{0}vt\right\} 
\]

\end_inset

This trajectory describes a linear path in position while the frame rotates,
 as opposed to the helical path traced out by the exponential map.
 The tangent vector at 
\begin_inset Formula $T_{0}$
\end_inset

 can be computed as
\end_layout

\begin_layout Standard
\begin_inset Formula 
\[
\frac{d\mathcal{R}_{T_{0}}\left(\xi\right)}{dt}\biggr\vert_{t=0}=\left[R_{0}\Skew{\omega},R_{0}v\right]
\]

\end_inset

which is identical to the one induced by 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none

\begin_inset Formula $\Phi_{T_{0}}(\xi)=T_{0}\exp\xihat$
\end_inset

.
\end_layout

\begin_layout Standard
The NavState manifold is not a Lie group like 
\begin_inset Formula $\SEthree$
\end_inset

, but we can easily define a retraction that behaves similarly to the one
 for 
\begin_inset Formula $\SEthree$
\end_inset

, while treating velocities the same way as positions:
\begin_inset Formula 
\[
\mathcal{R}_{X_{0}}(\zeta)=\left\{ \Phi_{R_{0}}\left(\omega t\right),P_{0}+R_{0}vt,V_{0}+R_{0}at\right\} 
\]

\end_inset

Here 
\begin_inset Formula $\zeta=\left[\omega t,vt,at\right]$
\end_inset

 is a 9-vector, with respectively angular, position, and velocity components.
 The tangent vector at 
\begin_inset Formula $X_{0}$
\end_inset

 is
\begin_inset Formula 
\[
\frac{d\mathcal{R}_{X_{0}}(\zeta)}{dt}\biggr\vert_{t=0}=\left[R_{0}\Skew{\omega},R_{0}v,R_{0}a\right]
\]

\end_inset

and the isomorphism between 
\begin_inset Formula $\Rnine$
\end_inset

 and 
\begin_inset Formula $T_{X_{0}}M$
\end_inset

 is 
\begin_inset Formula $\zeta\rightarrow\left[R_{0}\Skew{\omega t},R_{0}vt,R_{0}at\right]$
\end_inset

.
\end_layout

\begin_layout Subsubsection*
Integration in Local Coordinates
\end_layout

\begin_layout Standard
We now proceed exactly as before to describe the evolution of the NavState
 in local coordinates.
 Let us model the solution of the differential equation 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:diffeqM"

\end_inset

 as a trajectory 
\begin_inset Formula $\zeta(t)=\left[\theta(t),p(t),v(t)\right]$
\end_inset

, with 
\begin_inset Formula $\zeta(0)=0$
\end_inset

, in the local coordinate frame anchored at 
\begin_inset Formula $X_{0}$
\end_inset

.
 Note that this trajectory evolves away from 
\begin_inset Formula $X_{0}$
\end_inset

, and we use the symbols 
\begin_inset Formula $\theta$
\end_inset

, 
\begin_inset Formula $p$
\end_inset

, and 
\begin_inset Formula $v$
\end_inset

 to indicate that these are integrated rather than differential quantities.
 With that, we have 
\begin_inset Formula 
\begin{equation}
X(t)=\mathcal{R}_{X_{0}}(\zeta(t))=\left\{ \Phi_{R_{0}}\left(\theta(t)\right),P_{0}+R_{0}p(t),V_{0}+R_{0}v(t)\right\} \label{eq:scheme1}
\end{equation}

\end_inset

We can create a trajectory 
\begin_inset Formula $\gamma(\delta)$
\end_inset

 that passes through 
\begin_inset Formula $X(t)$
\end_inset

 for 
\begin_inset Formula $\delta=0$
\end_inset

 
\begin_inset Formula 
\[
\gamma(\delta)=X(t+\delta)=\left\{ \Phi_{R_{0}}\left(\theta(t)+\dot{\theta}(t)\delta\right),P_{0}+R_{0}\left\{ p(t)+\dot{p}(t)\delta\right\} ,V_{0}+R_{0}\left\{ v(t)+\dot{v}(t)\delta\right\} \right\} 
\]

\end_inset

and taking the derivative for 
\begin_inset Formula $\delta=0$
\end_inset

 we obtain
\begin_inset Formula 
\[
\dot{X}(t)=\frac{d\gamma(\delta)}{d\delta}\biggr\vert_{\delta=0}=\left[R(t)\Skew{H(\theta)\dot{\theta}(t)},R_{0}\,\dot{p}(t),R_{0}\,\dot{v}(t)\right]
\]

\end_inset

Comparing that with the vector field 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:bodyField"

\end_inset

, we have exact integration iff 
\begin_inset Formula 
\[
\left[R(t)\Skew{H(\theta)\dot{\theta}(t)},R_{0}\,\dot{p}(t),R_{0}\,\dot{v}(t)\right]=\left[R(t)\Skew{\omega^{b}(t)},V(t),g+R(t)a^{b}(t)\right]
\]

\end_inset

Or, as another way to state this, if we solve the differential equations
 for 
\begin_inset Formula $\theta(t)$
\end_inset

, 
\begin_inset Formula $p(t)$
\end_inset

, and 
\begin_inset Formula $v(t)$
\end_inset

 such that
\begin_inset Formula 
\begin{eqnarray*}
\dot{\theta}(t) & = & H(\theta)^{-1}\,\omega^{b}(t)\\
\dot{p}(t) & = & R_{0}^{T}\,V_{0}+v(t)\\
\dot{v}(t) & = & R_{0}^{T}\,g+R_{b}^{0}(t)a^{b}(t)
\end{eqnarray*}

\end_inset

where 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none

\begin_inset Formula $R_{b}^{0}(t)=R_{0}^{T}R(t)$
\end_inset

 is the rotation of the body frame with respect to 
\begin_inset Formula $R_{0}$
\end_inset

, and we have used 
\begin_inset Formula $V(t)=V_{0}+R_{0}v(t)$
\end_inset

.
\end_layout

\begin_layout Subsubsection*
Application: The New IMU Factor
\end_layout

\begin_layout Standard
In the IMU factor, we need to predict the NavState 
\begin_inset Formula $X_{j}$
\end_inset

 from the current NavState 
\begin_inset Formula $X_{i}$
\end_inset

 and the IMU measurements in-between.
 The above scheme suffers from a problem, which is that 
\begin_inset Formula $X_{i}$
\end_inset

 needs to be known in order to compensate properly for the initial velocity
 and rotated gravity vector.
 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

 into a gravity-induced part and an accelerometer part
\begin_inset Formula 
\[
v(t)=v_{g}(t)+v_{a}(t)
\]

\end_inset

evolving as
\begin_inset Formula 
\begin{eqnarray*}
\dot{v}_{g}(t) & = & R_{i}^{T}\,g\\
\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t)
\end{eqnarray*}

\end_inset

The solution for the first equation is simply 
\begin_inset Formula $v_{g}(t)=R_{i}^{T}gt$
\end_inset

.
 Similarly, we split the position 
\begin_inset Formula $p(t)$
\end_inset

 up in three parts 
\begin_inset Formula 
\[
p(t)=p_{i}(t)+p_{g}(t)+p_{v}(t)
\]

\end_inset

evolving as
\begin_inset Formula 
\begin{eqnarray*}
\dot{p}_{i}(t) & = & R_{i}^{T}\,V_{i}\\
\dot{p}_{g}(t) & = & v_{g}(t)=R_{i}^{T}gt\\
\dot{p}_{v}(t) & = & v_{a}(t)
\end{eqnarray*}

\end_inset

Here the solutions for the two first equations are simply 
\begin_inset Formula 
\begin{eqnarray*}
p_{i}(t) & = & R_{i}^{T}V_{i}t\\
p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
\end{eqnarray*}

\end_inset

The recipe for the IMU factor is then, in summary:
\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)\\
\dot{p}_{v}(t) & = & v_{a}(t)\\
\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t)
\end{eqnarray*}

\end_inset

starting from zero, up to time 
\begin_inset Formula $t_{ij}$
\end_inset

, where 
\begin_inset Formula $R_{b}^{i}(t)=\exp\Skew{\theta(t)}$
\end_inset

 at all times.
\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]
\]

\end_inset


\end_layout

\begin_layout Enumerate
Predict the NavState 
\begin_inset Formula $X_{j}$
\end_inset

 at time 
\begin_inset Formula $t_{j}$
\end_inset

 from
\begin_inset Formula 
\[
X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\,p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\,v_{a}(t_{ij})\right\} 
\]

\end_inset


\end_layout

\begin_layout Standard
Note that the predicted NavState 
\begin_inset Formula $X_{j}$
\end_inset

 depends on 
\begin_inset Formula $X_{i}$
\end_inset

, but the integrated quantities 
\begin_inset Formula $\theta(t)$
\end_inset

,
\begin_inset Formula $p_{v}(t)$
\end_inset

, and 
\begin_inset Formula $v_{a}(t)$
\end_inset

 do not.
\end_layout

\begin_layout Subsubsection*
A Simple Euler Scheme
\end_layout

\begin_layout Standard
To solve the differential equation we can use a simple Euler scheme:
\begin_inset Formula 
\begin{eqnarray}
\theta_{k+1}=\theta_{k}+\dot{\theta}(t_{k})\Delta_{t} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta-1}\\
p_{k+1}=p_{k}+\dot{p}_{v}(t_{k})\Delta_{t} & = & p_{k}+v_{k}\Delta_{t}\label{eq:euler_p-1}\\
v_{k+1}=v_{k}+\dot{v}_{a}(t_{k})\Delta_{t} & = & v_{k}+\exp\left(\Skew{\theta_{k}}\right)a_{k}^{b}\Delta_{t}\label{eq:euler_v-1}
\end{eqnarray}

\end_inset

where 
\begin_inset Formula $\theta_{k}\define\theta(t_{k})$
\end_inset

, 
\begin_inset Formula $p_{k}\define p_{v}(t_{k})$
\end_inset

, and 
\begin_inset Formula $v_{k}\define v_{a}(t_{k})$
\end_inset

.
 However, the position propagation can be done more accurately, by using
 exact integration of the zero-order hold acceleration 
\begin_inset Formula $a_{k}^{b}$
\end_inset

:
\begin_inset Formula 
\begin{eqnarray}
\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta}\\
p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}a_{k}^{b}\frac{\Delta_{t}^{2}}{2}\label{eq:euler_p}\\
v_{k+1} & = & v_{k}+R_{k}a_{k}^{b}\Delta_{t}\label{eq:euler_v}
\end{eqnarray}

\end_inset

where we defined the rotation matrix 
\begin_inset Formula $R_{k}=\exp\left(\Skew{\theta_{k}}\right)$
\end_inset

.
\end_layout

\begin_layout Subsubsection*
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
We wish to compute the ImuFactor covariance matrix 
\begin_inset Formula $\Sigma_{ij}$
\end_inset

.
 Even when we assume uncorrelated noise on 
\begin_inset Formula $\omega^{b}$
\end_inset

 and 
\begin_inset Formula $a^{b}$
\end_inset

, the noise on the final computed quantities will have a non-trivial covariance
 structure, because the intermediate quantities 
\begin_inset Formula $\theta_{k}$
\end_inset

 and 
\begin_inset Formula $v_{k}$
\end_inset

 appear in multiple places.
 To model the noise propagation, let us define the preintegrated navigation
 state 
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$
\end_inset

, as a 9D vector on tangent space at and rewrite Eqns.
 (
\begin_inset CommandInset ref
LatexCommand ref
reference "eq:euler_theta"

\end_inset

-
\begin_inset CommandInset ref
LatexCommand ref
reference "eq:euler_v"

\end_inset

) as the non-linear function 
\begin_inset Formula $f$
\end_inset


\begin_inset Formula 
\[
\zeta_{k+1}=f\left(\zeta_{k},a_{k}^{b},\omega_{k}^{b}\right)
\]

\end_inset

Then the noise on 
\begin_inset Formula $\zeta_{k+1}$
\end_inset

 propagates as
\begin_inset Formula 
\begin{equation}
\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

where 
\begin_inset Formula $A_{k}$
\end_inset

 is the 
\begin_inset Formula $9\times9$
\end_inset

 partial derivative of 
\begin_inset Formula $f$
\end_inset

 wrpt 
\begin_inset Formula $\zeta$
\end_inset

, and 
\begin_inset Formula $B_{k}$
\end_inset

 and 
\begin_inset Formula $C_{k}$
\end_inset

 the respective 
\begin_inset Formula $9\times3$
\end_inset

 partial derivatives with respect to the measured quantities 
\begin_inset Formula $a^{b}$
\end_inset

 and 
\begin_inset Formula $\omega^{b}$
\end_inset

.
 Note that 
\begin_inset Formula $\Sigma_{k},$
\end_inset


\begin_inset Formula $\Sigma_{\eta}^{ad}$
\end_inset

, and 
\begin_inset Formula $\Sigma_{\eta}^{gd}$
\end_inset

 are discrete time covariances with 
\begin_inset Formula $\Sigma_{\eta}^{ad}$
\end_inset

, and 
\begin_inset Formula $\Sigma_{\eta}^{gd}$
\end_inset

divided by 
\begin_inset Formula $\Delta_{t}$
\end_inset

.
 Please see the section on Covariance Discretization
\begin_inset CommandInset ref
LatexCommand vpageref
reference "subsec:Covariance-Discretization"
plural "false"
caps "false"
noprefix "false"

\end_inset

.
\end_layout

\begin_layout Standard
We start with the noise propagation on 
\begin_inset Formula $\theta$
\end_inset

, which is independent of the other quantities.
 Taking the derivative, we have
\end_layout

\begin_layout Standard
\begin_inset Formula 
\[
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
\]

\end_inset

It can be shown that for small 
\begin_inset Formula $\theta_{k}$
\end_inset

 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_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}
\]

\end_inset

For the derivatives of 
\begin_inset Formula $p_{k+1}$
\end_inset

 and 
\begin_inset Formula $v_{k+1}$
\end_inset

 we need the derivative
\begin_inset Formula 
\[
\deriv{R_{k}a_{k}^{b}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}\deriv{R_{k}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})
\]

\end_inset

where we used 
\begin_inset Formula 
\[
\deriv{\left(Ra\right)}R\approx R\Skew{-a}
\]

\end_inset

and the fact that the dependence of the rotation 
\begin_inset Formula $R_{k}$
\end_inset

 on 
\begin_inset Formula $\theta_{k}$
\end_inset

 is the already computed 
\begin_inset Formula $H(\theta_{k})$
\end_inset

.
 
\end_layout

\begin_layout Standard
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}} & 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} & 0_{3\times3} & I_{3\times3}
\end{array}\right]
\]

\end_inset

The other partial derivatives are simply
\begin_inset Formula 
\[
B_{k}=\left[\begin{array}{c}
0_{3\times3}\\
R_{k}\frac{\Delta_{t}}{2}^{2}\\
R_{k}\Delta_{t}
\end{array}\right],\,\,\,\,C_{k}=\left[\begin{array}{c}
H(\theta_{k})^{-1}\Delta_{t}\\
0_{3\times3}\\
0_{3\times3}
\end{array}\right]
\]

\end_inset


\end_layout

\begin_layout Subsubsection*
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.
\end_layout

\begin_layout Standard
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 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}Q_{k}G_{k}^{T}\label{eq:prop-combined}
\end{equation}

\end_inset


\end_layout

\begin_layout Standard
where 
\begin_inset Formula $F_{k}$
\end_inset

 is the 
\begin_inset Formula $15\times15$
\end_inset

 derivative of 
\begin_inset Formula $f$
\end_inset

 wrpt this new 
\begin_inset Formula $\zeta$
\end_inset

, and 
\begin_inset Formula $G_{k}$
\end_inset

 is the 
\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

 of 
\begin_inset Formula $F_{k}$
\end_inset

 is the same as 
\begin_inset Formula $A_{k}$
\end_inset

, thus we only have the jacobians wrpt the biases left to account for.
\end_layout

\begin_layout Standard
Conveniently, the jacobians of the pose and velocity wrpt the biases are
 already computed in the 
\emph on
ImuFactor 
\emph default
derivation as matrices 
\begin_inset Formula $B_{k}$
\end_inset

 and 
\begin_inset Formula $C_{k}$
\end_inset

, while they are identity matrices wrpt the biases themselves.
 Thus, we can easily plug-in the values from the previous section to give
 us the final result
\end_layout

\begin_layout Standard
\begin_inset Formula 
\[
F_{k}\approx\left[\begin{array}{ccccc}
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} & 0_{3\times3}\\
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{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}}^{T} & 0 & 0 & 0 & 0\\
0 & \deriv p{\epsilon^{a}}^{T} & \deriv v{\epsilon^{a}}^{T} & 0 & 0\\
0 & 0 & 0 & I_{3\times3} & 0\\
0 & 0 & 0 & 0 & I_{3\times3}\\
0 & \deriv p{\epsilon^{int}}^{T} & 0 & 0 & 0\\
0 & \deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 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}}^{T}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}\\
 & +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 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}}^{T} & 0 & 0 & 0 & 0\\
0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T} & 0 & 0 & 0\\
0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T} & 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}}}^{T} & 0 & 0 & 0 & 0\\
0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 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}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0
\end{array}\right]
\end{multline*}

\end_inset


\end_layout

\begin_layout Subsubsection*
Covariance Discretization
\begin_inset CommandInset label
LatexCommand label
name "subsec:Covariance-Discretization"

\end_inset


\end_layout

\begin_layout Standard
So far, all the covariances are assumed to be continuous since the state
 and measurement models are considered to be continuous-time stochastic
 processes.
 However, we sample measurements in a discrete-time fashion, necessitating
 the need to convert the covariances to their discrete time equivalents.
\end_layout

\begin_layout Standard
The IMU is modeled as a first order Gauss-Markov process, with a measurement
 noise and a process noise.
 Following 
\begin_inset CommandInset citation
LatexCommand cite
after "Alg. 1 Page 57"
key "Nikolic16thesis"
literal "false"

\end_inset

 and 
\begin_inset CommandInset citation
LatexCommand cite
after "Eqns 129-130"
key "Trawny05report_IndirectKF"
literal "false"

\end_inset

, the measurement noises 
\begin_inset Formula $[\epsilon^{a},\epsilon^{\omega},\epsilon_{init}]$
\end_inset

 are simply scaled by 
\begin_inset Formula $\frac{1}{\Delta t}$
\end_inset

, and the process noises 
\begin_inset Formula $[\epsilon^{int},\epsilon^{b^{a}},\epsilon^{b^{\omega}}]$
\end_inset

 are scaled by 
\begin_inset Formula $\Delta t$
\end_inset

 where 
\begin_inset Formula $\Delta t$
\end_inset

 is the time interval between 2 consecutive samples.
 For a thorough explanation of the discretization process, please refer
 to 
\begin_inset CommandInset citation
LatexCommand cite
after "Section 8.1"
key "Simon06book"
literal "false"

\end_inset

.
\end_layout

\begin_layout Standard
\begin_inset CommandInset bibtex
LatexCommand bibtex
btprint "btPrintCited"
bibfiles "refs"
options "plain"

\end_inset


\end_layout

\end_body
\end_document
