1711 lines
33 KiB
Plaintext
1711 lines
33 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[W(t,X),V(t,X),A(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[W(X,t),V(t),A(X,t)\right]\label{eq:validField}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
If we know
|
|
\begin_inset Formula $\omega^{b}(t)$
|
|
\end_inset
|
|
|
|
and non-gravity
|
|
\begin_inset Formula $a^{b}(t)$
|
|
\end_inset
|
|
|
|
in the body frame, we know (from Murray84book) that the body angular velocity
|
|
an be written as
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{\omega^{b}(t)}=R(t)^{T}W(X,t)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\Skew{\omega^{b}(t)}\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[W(X,t),V(t),A(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
|
|
|
|
We can 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
|
|
|
|
|
|
\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
|
|
|
|
and 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_{j}}(\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 inrgrated quantities
|
|
\begin_inset Formula $\theta(t)$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $p_{i}(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}\\
|
|
p_{k+1}=p_{k}+\dot{p}_{v}(t_{k})\Delta_{t} & = & p_{k}+v_{k}\Delta_{t}\label{eq:euler_p}\\
|
|
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}
|
|
\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
|
|
|
|
.
|
|
\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},\omega_{k}^{b},a_{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}^{gd}B_{k}+C_{k}\Sigma_{\eta}^{ad}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 $\omega^{b}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $a^{b}$
|
|
\end_inset
|
|
|
|
.
|
|
Noting that
|
|
\begin_inset Formula
|
|
\[
|
|
H(\theta)=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\Skew{\theta}^{k}\approx I-\frac{1}{2}\Skew{\theta}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
for small
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\Skew{\theta}\omega}{\theta}=\deriv{\left(\theta\times\omega\right)}{\theta}=-\deriv{\left(\omega\times\theta\right)}{\theta}=-\deriv{\Skew{\omega}\theta}{\theta}=-\Skew{\omega}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{H(\theta)\omega}{\theta}\approx\frac{1}{2}\Skew{\omega}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Similarly,
|
|
\begin_inset Formula
|
|
\[
|
|
\exp\left(\Skew{\theta}\right)=\sum_{k=0}^{\infty}\frac{1}{k!}\Skew{\theta}^{k}\approx I+\Skew{\theta}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and hence
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\exp\left(\Skew{\theta}\right)a}{\theta}\approx-\Skew a
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
so we finally obtain
|
|
\begin_inset Formula
|
|
\[
|
|
A_{k}\approx\left[\begin{array}{ccc}
|
|
I_{3\times3}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Delta_{t}\\
|
|
& I_{3\times3} & I_{3\times3}\Delta_{t}\\
|
|
-\Skew{a_{k}^{b}}\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}
|
|
H(\theta_{k})^{-1}\Delta^{t}\\
|
|
0_{3\times3}\\
|
|
0_{3\times3}
|
|
\end{array}\right],\,\,\,\, C_{k}=\left[\begin{array}{c}
|
|
0_{3\times3}\\
|
|
0_{3\times3}\\
|
|
\exp\left(\Skew{\theta_{k}}\right)\Delta_{t}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Substituting these expressions into Eq.
|
|
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:prop"
|
|
|
|
\end_inset
|
|
|
|
and dropping terms involving
|
|
\begin_inset Formula $\Delta_{t}^{2}$
|
|
\end_inset
|
|
|
|
, we obtain
|
|
\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
|
|
\[
|
|
\Sigma_{k+1}=\Sigma_{k}+\left[\begin{array}{ccc}
|
|
\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta\theta}-\Sigma_{k}^{\theta\theta}\frac{1}{2}\Skew{\omega_{k}^{b}} & \Sigma_{k}^{\theta v}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta p} & \Sigma_{k}^{\theta\theta}\Skew{a_{k}^{b}}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta v}\\
|
|
. & \Sigma_{k}^{pv}+\Sigma_{k}^{vp} & \Sigma_{k}^{vv}+\Sigma_{k}^{p\theta}\Skew{a_{k}^{b}}\\
|
|
. & . & \Sigma_{k}^{v\theta}\Skew{a_{k}^{b}}-\Skew{a_{k}^{b}}\Sigma_{k}^{\theta v}
|
|
\end{array}\right]\Delta^{t}+\Sigma_{k}^{\eta}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where we only show the upper-triangular part (the matrix is symmetric) and
|
|
where
|
|
\begin_inset Formula
|
|
\[
|
|
\Sigma_{k}^{\eta}=B_{k}\Sigma_{\eta}^{gd}B_{k}+C_{k}\Sigma_{\eta}^{ad}C_{k}=\left[\begin{array}{ccc}
|
|
\sigma^{g}I_{3\times3}\\
|
|
\\
|
|
& & \sigma^{a}I_{3\times3}
|
|
\end{array}\right]\Delta_{t}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The equality in the last line holds in the case of isotropic Gaussian measuremen
|
|
t noise, in which case
|
|
\begin_inset Formula $\Sigma_{\eta}^{gd}$
|
|
\end_inset
|
|
|
|
=
|
|
\begin_inset Formula $\sigma^{g}I_{3\times3}/\Delta_{t}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $\Sigma_{\eta}^{ga}$
|
|
\end_inset
|
|
|
|
=
|
|
\begin_inset Formula $\sigma^{a}I_{3\times3}/\Delta_{t}$
|
|
\end_inset
|
|
|
|
, and used the identities
|
|
\begin_inset Formula $H(\theta)^{-1}H(\theta)^{-T}\approx I_{3\times3}$
|
|
\end_inset
|
|
|
|
for small
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $\exp\left(\Skew{\theta}\right)\exp\left(\Skew{\theta}\right)^{T}=I_{3\times3}$
|
|
\end_inset
|
|
|
|
for all
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Old Stuff:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We only measure
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
at discrete instants of time, and hence we need to make choices about how
|
|
to integrate the equations above numerically.
|
|
For a vehicle such as a quadrotor UAV, it is not a bad assumption to model
|
|
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
as piecewise constant in the body frame, as the actuation is fixed to the
|
|
body.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
The group operation inherited from
|
|
\begin_inset Formula $GL(7)$
|
|
\end_inset
|
|
|
|
yields the following result,
|
|
\begin_inset Formula
|
|
\[
|
|
\left[\begin{array}{ccc}
|
|
R_{1} & & p_{1}\\
|
|
& R_{1} & v_{1}\\
|
|
& & 1
|
|
\end{array}\right]\left[\begin{array}{ccc}
|
|
R_{2} & & p_{2}\\
|
|
& R_{2} & v_{2}\\
|
|
& & 1
|
|
\end{array}\right]=\left[\begin{array}{ccc}
|
|
R_{1}R_{2} & & p_{1}+R_{1}p_{2}\\
|
|
& R_{1}R_{2} & v_{1}+R_{1}v_{2}\\
|
|
& & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
i.e.,
|
|
\begin_inset Formula $R_{2}$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $p_{2}$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $v_{2}$
|
|
\end_inset
|
|
|
|
are to interpreted as quantities in the body frame.
|
|
How can we achieve a constant angular velocity/constant acceleration operation
|
|
with this representation? For an infinitesimal interval
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
, we expect the result to be
|
|
\begin_inset Formula
|
|
\[
|
|
\left[\begin{array}{ccc}
|
|
R+R\hat{\omega}\delta & & p+v\delta\\
|
|
& R+R\hat{\omega}\delta & v+Ra\delta\\
|
|
& & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
This can NOT be achieved by
|
|
\begin_inset Formula
|
|
\[
|
|
\left[\begin{array}{ccc}
|
|
R & & p\\
|
|
& R & v\\
|
|
& & 1
|
|
\end{array}\right]\left[\begin{array}{ccc}
|
|
I+\hat{\omega}\delta & \delta & 0\\
|
|
& I+\hat{\omega}\delta & a\delta\\
|
|
& & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
because it is not closed.
|
|
Hence, the exponential map as defined below does not really do it for us
|
|
\begin_inset Formula
|
|
\[
|
|
\left[\begin{array}{ccc}
|
|
R & & p\\
|
|
& R & v\\
|
|
& & 1
|
|
\end{array}\right]=\lim_{n\rightarrow\infty}\left(I+\left[\begin{array}{ccc}
|
|
\hat{\omega} & & v^{b}\\
|
|
& \hat{\omega} & a\\
|
|
& & 1
|
|
\end{array}\right]\frac{\Delta t}{n}\right)^{n}=\left[\begin{array}{ccc}
|
|
R+R\hat{\omega}\delta & & p+v\delta\\
|
|
& R+R\hat{\omega}\delta & v+Ra\delta\\
|
|
& & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For a quadrotor, forces induced by wind effects and drag are arguably better
|
|
approximated over short intervals as constant in the navigation frame.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Let us examine what we obtain using a constant angular velocity in the body,
|
|
but with a zero-order hold on acceleration in the navigation frame instead.
|
|
While
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
is still measured in the body frame, we rotate it once by
|
|
\begin_inset Formula $R_{0}$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $t=0$
|
|
\end_inset
|
|
|
|
, and we obtain a much simplified picture:
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
R(t) & = & R_{0}\exp\hat{\omega}t\\
|
|
v(t) & = & v_{0}+\left(g+R_{0}a\right)t
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
Plugging this into position now yields a much simpler equation, as well:
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
p(t) & = & p_{0}+v_{0}t+\left(g+R_{0}a\right)\frac{t^{2}}{2}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The goal of the IMU factor is to integrate IMU measurements between two
|
|
successive frames and create a binary factor between two NavStates.
|
|
Integrating successive gyro and accelerometer readings using the above
|
|
equations over each constant interval yields
|
|
\begin_inset Formula
|
|
\begin{eqnarray}
|
|
R_{k+1} & = & R_{k}\exp\hat{\omega}_{k}\Delta t_{k}\label{eq:iter_Rk}\\
|
|
p_{k+1} & = & p_{k}+v_{k}\Delta t_{k}+\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}=p_{k}+\frac{v_{k}+v_{k+1}}{2}\Delta t_{k}\nonumber \\
|
|
v_{k+1} & = & v_{k}+(g+R_{k}a_{k})\Delta t_{k}\nonumber
|
|
\end{eqnarray}
|
|
|
|
\end_inset
|
|
|
|
Starting
|
|
\begin_inset Formula $X_{i}=(R_{i},p_{i},v_{i})$
|
|
\end_inset
|
|
|
|
, we then obtain an expression for
|
|
\begin_inset Formula $X_{j}$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
R_{j} & = & R_{i}\prod_{k}\exp\hat{\omega}_{k}\Delta t_{k}\\
|
|
p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}\\
|
|
v_{j} & = & v_{i}+\sum_{k}(g+R_{k}a_{k})\Delta t_{k}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $R_{k}$
|
|
\end_inset
|
|
|
|
has to be updated as in
|
|
\begin_inset CommandInset ref
|
|
LatexCommand formatted
|
|
reference "eq:iter_Rk"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
Note that
|
|
\begin_inset Formula
|
|
\[
|
|
v_{k}=v_{i}+\sum_{l}(g+R_{l}a_{l})\Delta t_{l}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and hence
|
|
\begin_inset Formula
|
|
\[
|
|
p_{j}=p_{i}+\sum_{k}\left(v_{i}+\sum_{l}(g+R_{l}a_{l})\Delta t_{l}\right)\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
[Is there not a 3/2 power here as in the RSS paper?]
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A crucial problem here is that we depend on a good estimate of
|
|
\begin_inset Formula $R_{k}$
|
|
\end_inset
|
|
|
|
at all times, which includes the potentially wrong estimate for the initial
|
|
attitude
|
|
\begin_inset Formula $R_{i}$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The idea behind the preintegrated IMU factor is two-fold: (a) treat gravity
|
|
separately, in the navigation frame, and (b) instead of integrating in
|
|
absolute coordinates, we want the IMU integration to happen in the frame
|
|
|
|
\begin_inset Formula $(R_{i},p_{i},v_{i})$
|
|
\end_inset
|
|
|
|
.
|
|
The first idea is easily incorporated by separating out all gravity-related
|
|
components:
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
p_{j} & = & p_{i}+\sum_{k}\left(\sum_{l}g\Delta t_{l}\right)\Delta t_{k}+\sum_{k}\left(v_{i}+\sum_{l}R_{l}a_{l}\Delta t_{l}\right)\Delta t_{k}+\sum_{k}g\frac{\left(\Delta t_{k}\right)^{2}}{2}+\sum_{k}R_{k}a_{k}\frac{\left(\Delta t_{k}\right)^{2}}{2}\\
|
|
v_{j} & = & v_{i}+g\sum_{k}\Delta t_{k}+\sum_{k}R_{k}a_{k}\Delta t_{k}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The binary factor is set up as a prediction:
|
|
\begin_inset Formula
|
|
\[
|
|
X_{j}\approx X_{i}\oplus dX_{ij}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $dX_{ij}$
|
|
\end_inset
|
|
|
|
is a tangent vector in the tangent space
|
|
\begin_inset Formula $T_{i}$
|
|
\end_inset
|
|
|
|
to the manifold at
|
|
\begin_inset Formula $X_{i}$
|
|
\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
|