1400 lines
26 KiB
Plaintext
1400 lines
26 KiB
Plaintext
#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
|