#LyX 2.0 created this file. For more info see http://www.lyx.org/ \lyxformat 413 \begin_document \begin_header \textclass article \use_default_options true \maintain_unincluded_children false \language english \language_package default \inputencoding auto \fontencoding global \font_roman default \font_sans default \font_typewriter default \font_default_family default \use_non_tex_fonts false \font_sc false \font_osf false \font_sf_scale 100 \font_tt_scale 100 \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_amsmath 1 \use_esint 1 \use_mhchem 1 \use_mathdots 1 \cite_engine basic \use_bibtopic false \use_indices false \paperorientation portrait \suppress_date false \use_refstyle 1 \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 \quotes_language english \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 \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 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 Murray84book) 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} \] \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" \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 \begin_inset FormulaMacro \newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}} {\mathfrak{\mathbb{R}^{9}}} \end_inset \end_layout \begin_layout Standard Note that the use of the exponential map in local coordinate mappings is not obligatory, even in the context of Lie groups. 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 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. 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. 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 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 Propagation \end_layout \begin_layout Standard 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 \begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$ \end_inset 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}+C_{k}\Sigma_{\eta}^{gd}C_{k}\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 . \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_{3x3}+\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_{3x3}-\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}}\\ R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\ R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3} \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 Standard \begin_inset CommandInset bibtex LatexCommand bibtex bibfiles "refs" options "plain" \end_inset \end_layout \end_body \end_document