#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