env/gtsam-4.1.1/doc/ImuFactor.lyx

#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 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"

\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