2180 lines
48 KiB
Plaintext
2180 lines
48 KiB
Plaintext
#LyX 2.3 created this file. For more info see http://www.lyx.org/
|
|
\lyxformat 544
|
|
\begin_document
|
|
\begin_header
|
|
\save_transient_properties true
|
|
\origin unavailable
|
|
\textclass article
|
|
\use_default_options true
|
|
\maintain_unincluded_children false
|
|
\language english
|
|
\language_package default
|
|
\inputencoding auto
|
|
\fontencoding global
|
|
\font_roman "default" "default"
|
|
\font_sans "default" "default"
|
|
\font_typewriter "default" "default"
|
|
\font_math "auto" "auto"
|
|
\font_default_family default
|
|
\use_non_tex_fonts false
|
|
\font_sc false
|
|
\font_osf false
|
|
\font_sf_scale 100 100
|
|
\font_tt_scale 100 100
|
|
\use_microtype false
|
|
\use_dash_ligatures true
|
|
\graphics default
|
|
\default_output_format default
|
|
\output_sync 0
|
|
\bibtex_command default
|
|
\index_command default
|
|
\paperfontsize 11
|
|
\spacing single
|
|
\use_hyperref false
|
|
\papersize default
|
|
\use_geometry true
|
|
\use_package amsmath 1
|
|
\use_package amssymb 1
|
|
\use_package cancel 1
|
|
\use_package esint 1
|
|
\use_package mathdots 1
|
|
\use_package mathtools 1
|
|
\use_package mhchem 1
|
|
\use_package stackrel 1
|
|
\use_package stmaryrd 1
|
|
\use_package undertilde 1
|
|
\cite_engine basic
|
|
\cite_engine_type default
|
|
\biblio_style plain
|
|
\use_bibtopic false
|
|
\use_indices false
|
|
\paperorientation portrait
|
|
\suppress_date false
|
|
\justification true
|
|
\use_refstyle 1
|
|
\use_minted 0
|
|
\index Index
|
|
\shortcut idx
|
|
\color #008000
|
|
\end_index
|
|
\leftmargin 3cm
|
|
\topmargin 3cm
|
|
\rightmargin 3cm
|
|
\bottommargin 3cm
|
|
\secnumdepth 3
|
|
\tocdepth 3
|
|
\paragraph_separation indent
|
|
\paragraph_indentation default
|
|
\is_math_indent 0
|
|
\math_numbering_side default
|
|
\quotes_style english
|
|
\dynamic_quotes 0
|
|
\papercolumns 1
|
|
\papersides 1
|
|
\paperpagestyle default
|
|
\tracking_changes false
|
|
\output_changes false
|
|
\html_math_output 0
|
|
\html_css_as_file 0
|
|
\html_be_strict false
|
|
\end_header
|
|
|
|
\begin_body
|
|
|
|
\begin_layout Title
|
|
The New IMU Factor
|
|
\end_layout
|
|
|
|
\begin_layout Author
|
|
Frank Dellaert & Varun Agrawal
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset CommandInset include
|
|
LatexCommand include
|
|
filename "macros.lyx"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\renewcommand{\sothree}{\mathfrak{so(3)}}
|
|
{\mathfrak{so(3)}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
|
|
{\mathfrak{\mathbb{R}^{9}}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rninethree}{\mathfrak{\mathbb{R}^{9\times3}}}
|
|
{\mathfrak{\mathbb{R}^{9\times3}}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rninesix}{\mathfrak{\mathbb{R}^{9\times6}}}
|
|
{\mathfrak{\mathbb{R}^{9\times6}}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rninenine}{\mathfrak{\mathbb{R}^{9\times9}}}
|
|
{\mathfrak{\mathbb{R}^{9\times9}}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
IMU Factor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The IMU factor has 2 variants:
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
ImuFactor is a 5-way factor between the previous pose and velocity, the
|
|
current pose and velocity, and the current IMU bias.
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
ImuFactor2 is a 3-way factor between the previous NavState, the current
|
|
NavState and the current IMU bias.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Both variants take a PreintegratedMeasurements object which encodes all
|
|
the IMU measurements between the previous timestep and the current timestep.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
There are also 2 variants of this class:
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Manifold Preintegration: This version keeps track of the incremental NavState
|
|
|
|
\begin_inset Formula $\Delta X_{ij}$
|
|
\end_inset
|
|
|
|
with respect to the previous NavState, on the NavState manifold itself.
|
|
It also keeps track of the
|
|
\begin_inset Formula $\Rninesix$
|
|
\end_inset
|
|
|
|
Jacobian of
|
|
\begin_inset Formula $\Delta X_{ij}$
|
|
\end_inset
|
|
|
|
w.r.t.
|
|
the bias.
|
|
This corresponds to Forster et.
|
|
al.
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Forster15rss"
|
|
literal "false"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Tangent Preintegration: This version keeps track of the incremental NavState
|
|
in the NavState tangent space instead.
|
|
This is a
|
|
\begin_inset Formula $\Rnine$
|
|
\end_inset
|
|
|
|
vector
|
|
\emph on
|
|
preintegrated_
|
|
\emph default
|
|
.
|
|
It also keeps track of the
|
|
\begin_inset Formula $\Rninesix$
|
|
\end_inset
|
|
|
|
jacobian of the
|
|
\emph on
|
|
preintegrated_
|
|
\emph default
|
|
w.r.t.
|
|
the bias.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The main function of a factor is to calculate an error.
|
|
This is done exactly the same in both variants:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
e(X_{i},X_{j})=X_{j}\ominus\widehat{X_{j}}\label{eq:imu-factor-error}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
where the predicted NavState
|
|
\begin_inset Formula $\widehat{X_{j}}$
|
|
\end_inset
|
|
|
|
at time
|
|
\begin_inset Formula $t_{j}$
|
|
\end_inset
|
|
|
|
is a function of the NavState
|
|
\begin_inset Formula $X_{i}$
|
|
\end_inset
|
|
|
|
at time
|
|
\begin_inset Formula $t_{i}$
|
|
\end_inset
|
|
|
|
and the preintegrated measurements
|
|
\begin_inset Formula $PIM$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
\widehat{X_{j}}=f(X_{i},PIM)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The noise model associated with this factor is assumed to be zero-mean Gaussian
|
|
with a
|
|
\begin_inset Formula $9\times9$
|
|
\end_inset
|
|
|
|
covariance matrix
|
|
\begin_inset Formula $\Sigma_{ij}$
|
|
\end_inset
|
|
|
|
, which is defined in the tangent space
|
|
\begin_inset Formula $T_{X_{j}}\mathcal{N}$
|
|
\end_inset
|
|
|
|
of the NavState manifold at the NavState
|
|
\begin_inset Formula $X_{j}$
|
|
\end_inset
|
|
|
|
.
|
|
This (discrete-time) covariance matrix is computed in the preintegrated
|
|
measurement class, of which there are two variants as discussed above.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Combined IMU Factor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The IMU factor above requires that bias drift over time be modeled as a
|
|
separate stochastic process (using a BetweenFactor for example), a crucial
|
|
aspect given that the preintegrated measurements depend on these bias values
|
|
and are thus correlated.
|
|
For this reason, we provide another type of IMU factor which we term the
|
|
Combined IMU Factor.
|
|
This factor similarly has 2 variants:
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
CombinedImuFactor is a 6-way factor between the previous pose, velocity
|
|
and IMU bias and the current pose, velocity and IMU bias.
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
CombinedImuFactor2 is a 4-way factor between the previous NavState and IMU
|
|
bias and the current NavState and IMU bias.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Since the Combined IMU Factor has a larger state variable due to the inclusion
|
|
of IMU biases, the noise model associated with this factor is assumed to
|
|
be a zero mean Gaussian with a
|
|
\begin_inset Formula $15\times15$
|
|
\end_inset
|
|
|
|
covariance matrix
|
|
\begin_inset Formula $\Sigma$
|
|
\end_inset
|
|
|
|
, similarly defined on the tangent space of the NavState manifold.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Covariance Matrices
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For IMU preintegration, it is important to propagate the uncertainty accurately
|
|
as well.
|
|
As such, we detail the various covariance matrices used in the preintegration
|
|
step.
|
|
\end_layout
|
|
|
|
\begin_layout Itemize
|
|
Gyroscope Covariance
|
|
\begin_inset Formula $Q_{\omega}$
|
|
\end_inset
|
|
|
|
: Measurement uncertainty of the gyroscope.
|
|
\end_layout
|
|
|
|
\begin_layout Itemize
|
|
Gyroscope Bias Covariance
|
|
\begin_inset Formula $Q_{\Delta b^{\omega}}$
|
|
\end_inset
|
|
|
|
: The covariance associated with the gyroscope bias random walk.
|
|
\end_layout
|
|
|
|
\begin_layout Itemize
|
|
Accelerometer Covariance
|
|
\begin_inset Formula $Q_{acc}$
|
|
\end_inset
|
|
|
|
: Measurement uncertainty of the accelerometer.
|
|
\end_layout
|
|
|
|
\begin_layout Itemize
|
|
Accelerometer Bias Covariance
|
|
\begin_inset Formula $Q_{\Delta b^{acc}}$
|
|
\end_inset
|
|
|
|
: The covariance associated with the accelerometer bias random walk.
|
|
\end_layout
|
|
|
|
\begin_layout Itemize
|
|
Integration Covariance
|
|
\begin_inset Formula $Q_{int}$
|
|
\end_inset
|
|
|
|
: This is the uncertainty due to modeling errors in the integration from
|
|
acceleration to velocity and position.
|
|
\end_layout
|
|
|
|
\begin_layout Itemize
|
|
Initial Bias Estimate Covariance
|
|
\begin_inset Formula $Q_{init}$
|
|
\end_inset
|
|
|
|
: This is the uncertainty associated with the estimation of the bias (since
|
|
we jointly estimate the bias as well).
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Navigation States
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Let us assume a setup where frames with image and/or laser measurements
|
|
are processed at some fairly low rate, e.g., 10 Hz.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We define the state of the vehicle at those times as attitude, position,
|
|
and velocity.
|
|
These three quantities are jointly referred to as a NavState
|
|
\begin_inset Formula $X_{b}^{n}\define\left\{ R_{b}^{n},P_{b}^{n},V_{b}^{n}\right\} $
|
|
\end_inset
|
|
|
|
, where the superscript
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
denotes the
|
|
\emph on
|
|
navigation frame
|
|
\emph default
|
|
, and
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
the
|
|
\emph on
|
|
body frame
|
|
\emph default
|
|
.
|
|
For simplicity, we drop these indices below where clear from context.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Vector Fields and Differential Equations
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We need a way to describe the evolution of a NavState over time.
|
|
The NavState lives in a 9-dimensional manifold
|
|
\begin_inset Formula $M$
|
|
\end_inset
|
|
|
|
, defined by the orthonormality constraints on
|
|
\begin_inset Formula $\Rone$
|
|
\end_inset
|
|
|
|
.
|
|
For a NavState
|
|
\begin_inset Formula $X$
|
|
\end_inset
|
|
|
|
evolving over time we can write down a differential equation
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\dot{X}(t)=F(t,X)\label{eq:diffeqM}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $F$
|
|
\end_inset
|
|
|
|
is a time-varying
|
|
\series bold
|
|
vector field
|
|
\series default
|
|
on
|
|
\begin_inset Formula $M$
|
|
\end_inset
|
|
|
|
, defined as a mapping from
|
|
\begin_inset Formula $\Rone\times M$
|
|
\end_inset
|
|
|
|
to tangent vectors at
|
|
\begin_inset Formula $X$
|
|
\end_inset
|
|
|
|
.
|
|
A
|
|
\series bold
|
|
tangent vector
|
|
\series default
|
|
at
|
|
\begin_inset Formula $X$
|
|
\end_inset
|
|
|
|
is defined as the derivative of a trajectory at
|
|
\begin_inset Formula $X$
|
|
\end_inset
|
|
|
|
, and for the NavState manifold this will be a triplet
|
|
\begin_inset Formula
|
|
\[
|
|
\left[\dot{R}(t,X),\dot{P}(t,X),\dot{V}(t,X)\right]\in\sothree\times\Rthree\times\Rthree
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where we use square brackets to indicate a tangent vector.
|
|
The space of all tangent vectors at
|
|
\begin_inset Formula $X$
|
|
\end_inset
|
|
|
|
is denoted by
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $T_{X}M$
|
|
\end_inset
|
|
|
|
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
, and hence
|
|
\begin_inset Formula $F(t,X)\in T_{X}M$
|
|
\end_inset
|
|
|
|
.
|
|
For example, if the state evolves along a constant velocity trajectory
|
|
\begin_inset Formula
|
|
\[
|
|
X(t)=\left\{ R_{0},P_{0}+V_{0}t,V_{0}\right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
then the differential equation describing the trajectory is
|
|
\begin_inset Formula
|
|
\[
|
|
\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\,X(0)=\left\{ R_{0},P_{0},V_{0}\right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Valid vector fields on a NavState manifold are special, in that the attitude
|
|
and velocity derivatives can be arbitrary functions of X and t, but the
|
|
derivative of position is constrained to be equal to the current velocity
|
|
|
|
\begin_inset Formula $V(t)$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]\label{eq:validField}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
Suppose we are given the
|
|
\series bold
|
|
body angular velocity
|
|
\series default
|
|
|
|
\begin_inset Formula $\omega^{b}(t)$
|
|
\end_inset
|
|
|
|
and non-gravity
|
|
\series bold
|
|
acceleration
|
|
\series default
|
|
|
|
\begin_inset Formula $a^{b}(t)$
|
|
\end_inset
|
|
|
|
in the body frame.
|
|
We know (from
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Murray94book"
|
|
literal "false"
|
|
|
|
\end_inset
|
|
|
|
) that the derivative of
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
can be written as
|
|
\begin_inset Formula
|
|
\[
|
|
\dot{R}(X,t)=R(t)\Skew{\omega^{b}(t)}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\Skew{\theta}\in so(3)$
|
|
\end_inset
|
|
|
|
is the skew-symmetric matrix corresponding to
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
, and hence the resulting exact vector field is
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]=\left[R(t)\Skew{\omega^{b}(t)},V(t),g+R(t)a^{b}(t)\right]\label{eq:bodyField}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Local Coordinates
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Optimization on manifolds relies crucially on the concept of
|
|
\series bold
|
|
local coordinates
|
|
\series default
|
|
.
|
|
For example, when optimizing over the rotations
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
starting from an initial estimate
|
|
\begin_inset Formula $R_{0}$
|
|
\end_inset
|
|
|
|
, we define a local map
|
|
\begin_inset Formula $\Phi_{R_{0}}$
|
|
\end_inset
|
|
|
|
from
|
|
\begin_inset Formula $\theta\in\Rthree$
|
|
\end_inset
|
|
|
|
to a neighborhood of
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
centered around
|
|
\begin_inset Formula $R_{0}$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula
|
|
\[
|
|
\Phi_{R_{0}}(\theta)=R_{0}\exp\left(\Skew{\theta}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\exp$
|
|
\end_inset
|
|
|
|
is the matrix exponential, given by
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\exp\left(\Skew{\theta}\right)=\sum_{k=0}^{\infty}\frac{1}{k!}\Skew{\theta}^{k}\label{eq:expm}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
which for
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
can be efficiently computed in closed form.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The local coordinates
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
are isomorphic to tangent vectors at
|
|
\emph on
|
|
|
|
\begin_inset Formula $R_{0}$
|
|
\end_inset
|
|
|
|
|
|
\emph default
|
|
.
|
|
To see this, define
|
|
\begin_inset Formula $\theta=\omega t$
|
|
\end_inset
|
|
|
|
and note that
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{d\Phi_{R_{0}}\left(\omega t\right)}{dt}\biggr\vert_{t=0}=\frac{dR_{0}\exp\left(\Skew{\omega t}\right)}{dt}\biggr\vert_{t=0}=R_{0}\Skew{\omega t}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Hence, the 3-vector
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
defines a direction of travel on the
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
manifold, but does so in the local coordinate frame define by
|
|
\begin_inset Formula $R_{0}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A similar story holds in
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
: we define local coordinates
|
|
\begin_inset Formula $\xi=\left[\omega t,vt\right]\in\Rsix$
|
|
\end_inset
|
|
|
|
and a mapping
|
|
\begin_inset Formula
|
|
\[
|
|
\Phi_{T_{0}}(\xi)=T_{0}\exp\xihat
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\xihat\in\sethree$
|
|
\end_inset
|
|
|
|
is defined as
|
|
\begin_inset Formula
|
|
\[
|
|
\xihat=\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0
|
|
\end{array}\right]t
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and the 6-vectors
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
are mapped to tangent vectors
|
|
\begin_inset Formula $T_{0}\xihat$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $T_{0}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Derivative of The Local Coordinate Mapping
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For the local coordinate mapping
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $\Phi_{R_{0}}\left(\theta\right)$
|
|
\end_inset
|
|
|
|
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
in
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
we can define a
|
|
\begin_inset Formula $3\times3$
|
|
\end_inset
|
|
|
|
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
Jacobian
|
|
\begin_inset Formula $H(\theta)$
|
|
\end_inset
|
|
|
|
that models the effect of an incremental change
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
to the local coordinates:
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\Phi_{R_{0}}\left(\theta+\delta\right)\approx\Phi_{R_{0}}\left(\theta\right)\,\exp\left(\Skew{H(\theta)\delta}\right)=\Phi_{\Phi_{R_{0}}\left(\theta\right)}\left(H(\theta)\delta\right)\label{eq:push_exp}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
This Jacobian depends only on
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
and, for the case of
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
, is given by a formula similar to the matrix exponential map,
|
|
\begin_inset Formula
|
|
\[
|
|
H(\theta)=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\Skew{\theta}^{k}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
which can also be computed in closed form.
|
|
In particular,
|
|
\begin_inset Formula $H(0)=I_{3\times3}$
|
|
\end_inset
|
|
|
|
at the base
|
|
\begin_inset Formula $R_{0}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Numerical Integration in Local Coordinates
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Inspired by the paper
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
Lie Group Methods
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
by Iserles et al.
|
|
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Iserles00an"
|
|
literal "true"
|
|
|
|
\end_inset
|
|
|
|
, when we have a differential equation on
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\dot{R}(t)=F(R,t),\,\,\,\,R(0)=R_{0}\label{eq:diffSo3}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
we can transfer it to a differential equation in the 3-dimensional local
|
|
coordinate space.
|
|
To do so, we model the solution to
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:diffSo3"
|
|
|
|
\end_inset
|
|
|
|
as
|
|
\begin_inset Formula
|
|
\[
|
|
R(t)=\Phi_{R_{0}}(\theta(t))
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
To find an expression for
|
|
\begin_inset Formula $\dot{\theta}(t)$
|
|
\end_inset
|
|
|
|
, create a trajectory
|
|
\begin_inset Formula $\gamma(\delta)$
|
|
\end_inset
|
|
|
|
that passes through
|
|
\begin_inset Formula $R(t)$
|
|
\end_inset
|
|
|
|
for
|
|
\begin_inset Formula $\delta=0$
|
|
\end_inset
|
|
|
|
, and moves
|
|
\begin_inset Formula $\theta(t)$
|
|
\end_inset
|
|
|
|
along the direction
|
|
\begin_inset Formula $\dot{\theta}(t)$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
\gamma(\delta)=R(t+\delta)=\Phi_{R_{0}}\left(\theta(t)+\dot{\theta}(t)\delta\right)\approx\Phi_{R(t)}\left(H(\theta)\dot{\theta}(t)\delta\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Taking the derivative for
|
|
\begin_inset Formula $\delta=0$
|
|
\end_inset
|
|
|
|
we obtain
|
|
\begin_inset Formula
|
|
\[
|
|
\dot{R}(t)=\frac{d\gamma(\delta)}{d\delta}\biggr\vert_{\delta=0}=\frac{d\Phi_{R(t)}\left(H(\theta)\dot{\theta}(t)\delta\right)}{d\delta}\biggr\vert_{\delta=0}=R(t)\Skew{H(\theta)\dot{\theta}(t)}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Comparing this to
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:diffSo3"
|
|
|
|
\end_inset
|
|
|
|
we obtain a differential equation for
|
|
\begin_inset Formula $\theta(t)$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
\dot{\theta}(t)=H(\theta)^{-1}\left\{ R(t)^{T}F(R,t)\right\} \check{},\,\,\,\,\theta(0)=0_{3\times1}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
In other words, the vector field
|
|
\begin_inset Formula $F(R,t)$
|
|
\end_inset
|
|
|
|
is rotated to the local frame, the inverse hat operator is applied to get
|
|
a 3-vector, which is then corrected by
|
|
\begin_inset Formula $H(\theta)^{-1}$
|
|
\end_inset
|
|
|
|
away from
|
|
\begin_inset Formula $\theta=0$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Retractions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Note that the use of the exponential map in local coordinate mappings is
|
|
not obligatory, even in the context of Lie groups.
|
|
Often it is computationally expedient to use mappings that are easier to
|
|
compute, but yet induce the same tangent vector at
|
|
\begin_inset Formula $T_{0}.$
|
|
\end_inset
|
|
|
|
Mappings that satisfy this constraint are collectively known as
|
|
\series bold
|
|
retractions
|
|
\series default
|
|
.
|
|
For example, for
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
one could use the retraction
|
|
\begin_inset Formula $\mathcal{R}_{T_{0}}:\Rsix\rightarrow\SEthree$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\mathcal{R}_{T_{0}}\left(\xi\right)=T_{0}\left\{ \exp\left(\Skew{\omega t}\right),vt\right\} =\left\{ \Phi_{R_{0}}\left(\omega t\right),P_{0}+R_{0}vt\right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
This trajectory describes a linear path in position while the frame rotates,
|
|
as opposed to the helical path traced out by the exponential map.
|
|
The tangent vector at
|
|
\begin_inset Formula $T_{0}$
|
|
\end_inset
|
|
|
|
can be computed as
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{d\mathcal{R}_{T_{0}}\left(\xi\right)}{dt}\biggr\vert_{t=0}=\left[R_{0}\Skew{\omega},R_{0}v\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
which is identical to the one induced by
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $\Phi_{T_{0}}(\xi)=T_{0}\exp\xihat$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The NavState manifold is not a Lie group like
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
, but we can easily define a retraction that behaves similarly to the one
|
|
for
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
, while treating velocities the same way as positions:
|
|
\begin_inset Formula
|
|
\[
|
|
\mathcal{R}_{X_{0}}(\zeta)=\left\{ \Phi_{R_{0}}\left(\omega t\right),P_{0}+R_{0}vt,V_{0}+R_{0}at\right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Here
|
|
\begin_inset Formula $\zeta=\left[\omega t,vt,at\right]$
|
|
\end_inset
|
|
|
|
is a 9-vector, with respectively angular, position, and velocity components.
|
|
The tangent vector at
|
|
\begin_inset Formula $X_{0}$
|
|
\end_inset
|
|
|
|
is
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{d\mathcal{R}_{X_{0}}(\zeta)}{dt}\biggr\vert_{t=0}=\left[R_{0}\Skew{\omega},R_{0}v,R_{0}a\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and the isomorphism between
|
|
\begin_inset Formula $\Rnine$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $T_{X_{0}}M$
|
|
\end_inset
|
|
|
|
is
|
|
\begin_inset Formula $\zeta\rightarrow\left[R_{0}\Skew{\omega t},R_{0}vt,R_{0}at\right]$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Integration in Local Coordinates
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We now proceed exactly as before to describe the evolution of the NavState
|
|
in local coordinates.
|
|
Let us model the solution of the differential equation
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:diffeqM"
|
|
|
|
\end_inset
|
|
|
|
as a trajectory
|
|
\begin_inset Formula $\zeta(t)=\left[\theta(t),p(t),v(t)\right]$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $\zeta(0)=0$
|
|
\end_inset
|
|
|
|
, in the local coordinate frame anchored at
|
|
\begin_inset Formula $X_{0}$
|
|
\end_inset
|
|
|
|
.
|
|
Note that this trajectory evolves away from
|
|
\begin_inset Formula $X_{0}$
|
|
\end_inset
|
|
|
|
, and we use the symbols
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $v$
|
|
\end_inset
|
|
|
|
to indicate that these are integrated rather than differential quantities.
|
|
With that, we have
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
X(t)=\mathcal{R}_{X_{0}}(\zeta(t))=\left\{ \Phi_{R_{0}}\left(\theta(t)\right),P_{0}+R_{0}p(t),V_{0}+R_{0}v(t)\right\} \label{eq:scheme1}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
We can create a trajectory
|
|
\begin_inset Formula $\gamma(\delta)$
|
|
\end_inset
|
|
|
|
that passes through
|
|
\begin_inset Formula $X(t)$
|
|
\end_inset
|
|
|
|
for
|
|
\begin_inset Formula $\delta=0$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\gamma(\delta)=X(t+\delta)=\left\{ \Phi_{R_{0}}\left(\theta(t)+\dot{\theta}(t)\delta\right),P_{0}+R_{0}\left\{ p(t)+\dot{p}(t)\delta\right\} ,V_{0}+R_{0}\left\{ v(t)+\dot{v}(t)\delta\right\} \right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and taking the derivative for
|
|
\begin_inset Formula $\delta=0$
|
|
\end_inset
|
|
|
|
we obtain
|
|
\begin_inset Formula
|
|
\[
|
|
\dot{X}(t)=\frac{d\gamma(\delta)}{d\delta}\biggr\vert_{\delta=0}=\left[R(t)\Skew{H(\theta)\dot{\theta}(t)},R_{0}\,\dot{p}(t),R_{0}\,\dot{v}(t)\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Comparing that with the vector field
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:bodyField"
|
|
|
|
\end_inset
|
|
|
|
, we have exact integration iff
|
|
\begin_inset Formula
|
|
\[
|
|
\left[R(t)\Skew{H(\theta)\dot{\theta}(t)},R_{0}\,\dot{p}(t),R_{0}\,\dot{v}(t)\right]=\left[R(t)\Skew{\omega^{b}(t)},V(t),g+R(t)a^{b}(t)\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Or, as another way to state this, if we solve the differential equations
|
|
for
|
|
\begin_inset Formula $\theta(t)$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $p(t)$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $v(t)$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\dot{\theta}(t) & = & H(\theta)^{-1}\,\omega^{b}(t)\\
|
|
\dot{p}(t) & = & R_{0}^{T}\,V_{0}+v(t)\\
|
|
\dot{v}(t) & = & R_{0}^{T}\,g+R_{b}^{0}(t)a^{b}(t)
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $R_{b}^{0}(t)=R_{0}^{T}R(t)$
|
|
\end_inset
|
|
|
|
is the rotation of the body frame with respect to
|
|
\begin_inset Formula $R_{0}$
|
|
\end_inset
|
|
|
|
, and we have used
|
|
\begin_inset Formula $V(t)=V_{0}+R_{0}v(t)$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Application: The New IMU Factor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In the IMU factor, we need to predict the NavState
|
|
\begin_inset Formula $X_{j}$
|
|
\end_inset
|
|
|
|
from the current NavState
|
|
\begin_inset Formula $X_{i}$
|
|
\end_inset
|
|
|
|
and the IMU measurements in-between.
|
|
The above scheme suffers from a problem, which is that
|
|
\begin_inset Formula $X_{i}$
|
|
\end_inset
|
|
|
|
needs to be known in order to compensate properly for the initial velocity
|
|
and rotated gravity vector.
|
|
Hence, the idea of Lupton
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Lupton12tro"
|
|
literal "false"
|
|
|
|
\end_inset
|
|
|
|
was to split up
|
|
\begin_inset Formula $v(t)$
|
|
\end_inset
|
|
|
|
into a gravity-induced part and an accelerometer part
|
|
\begin_inset Formula
|
|
\[
|
|
v(t)=v_{g}(t)+v_{a}(t)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
evolving as
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\dot{v}_{g}(t) & = & R_{i}^{T}\,g\\
|
|
\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t)
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
The solution for the first equation is simply
|
|
\begin_inset Formula $v_{g}(t)=R_{i}^{T}gt$
|
|
\end_inset
|
|
|
|
.
|
|
Similarly, we split the position
|
|
\begin_inset Formula $p(t)$
|
|
\end_inset
|
|
|
|
up in three parts
|
|
\begin_inset Formula
|
|
\[
|
|
p(t)=p_{i}(t)+p_{g}(t)+p_{v}(t)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
evolving as
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\dot{p}_{i}(t) & = & R_{i}^{T}\,V_{i}\\
|
|
\dot{p}_{g}(t) & = & v_{g}(t)=R_{i}^{T}gt\\
|
|
\dot{p}_{v}(t) & = & v_{a}(t)
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
Here the solutions for the two first equations are simply
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
p_{i}(t) & = & R_{i}^{T}V_{i}t\\
|
|
p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
The recipe for the IMU factor is then, in summary:
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Solve the ordinary differential equations
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\dot{\theta}(t) & = & H(\theta(t))^{-1}\,\omega^{b}(t)\\
|
|
\dot{p}_{v}(t) & = & v_{a}(t)\\
|
|
\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t)
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
starting from zero, up to time
|
|
\begin_inset Formula $t_{ij}$
|
|
\end_inset
|
|
|
|
, where
|
|
\begin_inset Formula $R_{b}^{i}(t)=\exp\Skew{\theta(t)}$
|
|
\end_inset
|
|
|
|
at all times.
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Form the local coordinate vector as
|
|
\begin_inset Formula
|
|
\[
|
|
\zeta(t_{ij})=\left[\theta(t_{ij}),p(t_{ij}),v(t_{ij})\right]=\left[\theta(t_{ij}),R_{i}^{T}V_{i}t_{ij}+R_{i}^{T}\frac{gt_{ij}^{2}}{2}+p_{v}(t_{ij}),R_{i}^{T}gt_{ij}+v_{a}(t_{ij})\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Predict the NavState
|
|
\begin_inset Formula $X_{j}$
|
|
\end_inset
|
|
|
|
at time
|
|
\begin_inset Formula $t_{j}$
|
|
\end_inset
|
|
|
|
from
|
|
\begin_inset Formula
|
|
\[
|
|
X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\,p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\,v_{a}(t_{ij})\right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Note that the predicted NavState
|
|
\begin_inset Formula $X_{j}$
|
|
\end_inset
|
|
|
|
depends on
|
|
\begin_inset Formula $X_{i}$
|
|
\end_inset
|
|
|
|
, but the integrated quantities
|
|
\begin_inset Formula $\theta(t)$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $p_{v}(t)$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $v_{a}(t)$
|
|
\end_inset
|
|
|
|
do not.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
A Simple Euler Scheme
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
To solve the differential equation we can use a simple Euler scheme:
|
|
\begin_inset Formula
|
|
\begin{eqnarray}
|
|
\theta_{k+1}=\theta_{k}+\dot{\theta}(t_{k})\Delta_{t} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta-1}\\
|
|
p_{k+1}=p_{k}+\dot{p}_{v}(t_{k})\Delta_{t} & = & p_{k}+v_{k}\Delta_{t}\label{eq:euler_p-1}\\
|
|
v_{k+1}=v_{k}+\dot{v}_{a}(t_{k})\Delta_{t} & = & v_{k}+\exp\left(\Skew{\theta_{k}}\right)a_{k}^{b}\Delta_{t}\label{eq:euler_v-1}
|
|
\end{eqnarray}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\theta_{k}\define\theta(t_{k})$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $p_{k}\define p_{v}(t_{k})$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $v_{k}\define v_{a}(t_{k})$
|
|
\end_inset
|
|
|
|
.
|
|
However, the position propagation can be done more accurately, by using
|
|
exact integration of the zero-order hold acceleration
|
|
\begin_inset Formula $a_{k}^{b}$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\begin{eqnarray}
|
|
\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta}\\
|
|
p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}a_{k}^{b}\frac{\Delta_{t}^{2}}{2}\label{eq:euler_p}\\
|
|
v_{k+1} & = & v_{k}+R_{k}a_{k}^{b}\Delta_{t}\label{eq:euler_v}
|
|
\end{eqnarray}
|
|
|
|
\end_inset
|
|
|
|
where we defined the rotation matrix
|
|
\begin_inset Formula $R_{k}=\exp\left(\Skew{\theta_{k}}\right)$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Noise Modeling
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Given the above solutions to the differential equations, we add noise modeling
|
|
to account for the various sources of error in the system
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\begin{eqnarray}
|
|
\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,(\omega_{k}^{b}+\epsilon_{k}^{\omega}-b_{k}^{\omega}-\epsilon_{init}^{\omega})\Delta_{t}\nonumber \\
|
|
p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\frac{\Delta_{t}^{2}}{2}+\epsilon_{k}^{int}\label{eq:preintegration}\\
|
|
v_{k+1} & = & v_{k}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\Delta_{t}\nonumber \\
|
|
b_{k+1}^{a} & = & b_{k}^{a}+\epsilon_{k}^{b^{a}}\nonumber \\
|
|
b_{k+1}^{\omega} & = & b_{k}^{\omega}+\epsilon_{k}^{b^{\omega}}\nonumber
|
|
\end{eqnarray}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
which we can write compactly as,
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\begin{eqnarray}
|
|
\theta_{k+1} & = & f_{\theta}(\theta_{k},b_{k}^{w},\epsilon_{k}^{\omega},\epsilon_{init}^{b^{\omega}})\label{eq:compact-preintegration}\\
|
|
p_{k+1} & = & f_{p}(p_{k},v_{k},\theta_{k},b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a},\epsilon_{k}^{int})\nonumber \\
|
|
v_{k+1} & = & f_{v}(v_{k,}\theta_{k,}b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a})\nonumber \\
|
|
b_{k+1}^{a} & = & f_{b^{a}}(b_{k}^{a},\epsilon_{k}^{b^{a}})\nonumber \\
|
|
b_{k+1}^{\omega} & = & f_{b^{\omega}}(b_{k}^{\omega},\epsilon_{k}^{b^{\omega}})\nonumber
|
|
\end{eqnarray}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Noise Propagation in IMU Factor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We wish to compute the ImuFactor covariance matrix
|
|
\begin_inset Formula $\Sigma_{ij}$
|
|
\end_inset
|
|
|
|
.
|
|
Even when we assume uncorrelated noise on
|
|
\begin_inset Formula $\omega^{b}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $a^{b}$
|
|
\end_inset
|
|
|
|
, the noise on the final computed quantities will have a non-trivial covariance
|
|
structure, because the intermediate quantities
|
|
\begin_inset Formula $\theta_{k}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $v_{k}$
|
|
\end_inset
|
|
|
|
appear in multiple places.
|
|
To model the noise propagation, let us define the preintegrated navigation
|
|
state
|
|
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$
|
|
\end_inset
|
|
|
|
, as a 9D vector on tangent space at and rewrite Eqns.
|
|
(
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:euler_theta"
|
|
|
|
\end_inset
|
|
|
|
-
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:euler_v"
|
|
|
|
\end_inset
|
|
|
|
) as the non-linear function
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\zeta_{k+1}=f\left(\zeta_{k},a_{k}^{b},\omega_{k}^{b}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Then the noise on
|
|
\begin_inset Formula $\zeta_{k+1}$
|
|
\end_inset
|
|
|
|
propagates as
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}^{T}+C_{k}\Sigma_{\eta}^{gd}C_{k}^{T}\label{eq:prop}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $A_{k}$
|
|
\end_inset
|
|
|
|
is the
|
|
\begin_inset Formula $9\times9$
|
|
\end_inset
|
|
|
|
partial derivative of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
wrpt
|
|
\begin_inset Formula $\zeta$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $B_{k}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $C_{k}$
|
|
\end_inset
|
|
|
|
the respective
|
|
\begin_inset Formula $9\times3$
|
|
\end_inset
|
|
|
|
partial derivatives with respect to the measured quantities
|
|
\begin_inset Formula $a^{b}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $\omega^{b}$
|
|
\end_inset
|
|
|
|
.
|
|
Note that
|
|
\begin_inset Formula $\Sigma_{k},$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\Sigma_{\eta}^{ad}$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $\Sigma_{\eta}^{gd}$
|
|
\end_inset
|
|
|
|
are discrete time covariances with
|
|
\begin_inset Formula $\Sigma_{\eta}^{ad}$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $\Sigma_{\eta}^{gd}$
|
|
\end_inset
|
|
|
|
divided by
|
|
\begin_inset Formula $\Delta_{t}$
|
|
\end_inset
|
|
|
|
.
|
|
Please see the section on Covariance Discretization
|
|
\begin_inset CommandInset ref
|
|
LatexCommand vpageref
|
|
reference "subsec:Covariance-Discretization"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We start with the noise propagation on
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
, which is independent of the other quantities.
|
|
Taking the derivative, we have
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
It can be shown that for small
|
|
\begin_inset Formula $\theta_{k}$
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
For the derivatives of
|
|
\begin_inset Formula $p_{k+1}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $v_{k+1}$
|
|
\end_inset
|
|
|
|
we need the derivative
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{R_{k}a_{k}^{b}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}\deriv{R_{k}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where we used
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\left(Ra\right)}R\approx R\Skew{-a}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and the fact that the dependence of the rotation
|
|
\begin_inset Formula $R_{k}$
|
|
\end_inset
|
|
|
|
on
|
|
\begin_inset Formula $\theta_{k}$
|
|
\end_inset
|
|
|
|
is the already computed
|
|
\begin_inset Formula $H(\theta_{k})$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Putting all this together, we finally obtain
|
|
\begin_inset Formula
|
|
\[
|
|
A_{k}\approx\left[\begin{array}{ccc}
|
|
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3}\\
|
|
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
|
|
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The other partial derivatives are simply
|
|
\begin_inset Formula
|
|
\[
|
|
B_{k}=\left[\begin{array}{c}
|
|
0_{3\times3}\\
|
|
R_{k}\frac{\Delta_{t}}{2}^{2}\\
|
|
R_{k}\Delta_{t}
|
|
\end{array}\right],\,\,\,\,C_{k}=\left[\begin{array}{c}
|
|
H(\theta_{k})^{-1}\Delta_{t}\\
|
|
0_{3\times3}\\
|
|
0_{3\times3}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Noise Propagation in Combined IMU Factor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can similarly account for bias drift over time, as is commonly seen in
|
|
commercial grade IMUs.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We expand the state vector as
|
|
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$
|
|
\end_inset
|
|
|
|
to include the bias terms and define the augmented noise vector
|
|
\begin_inset Formula $\epsilon=[\epsilon_{k}^{\omega},\epsilon_{k}^{a},\epsilon_{k}^{b^{a}},\epsilon_{k}^{b^{\omega}},\epsilon_{k}^{int},\epsilon_{init}^{b^{a}},\epsilon_{init}^{b^{\omega}}]$
|
|
\end_inset
|
|
|
|
.
|
|
This gives the noise propagation equation as
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}Q_{k}G_{k}^{T}\label{eq:prop-combined}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
where
|
|
\begin_inset Formula $F_{k}$
|
|
\end_inset
|
|
|
|
is the
|
|
\begin_inset Formula $15\times15$
|
|
\end_inset
|
|
|
|
derivative of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
wrpt this new
|
|
\begin_inset Formula $\zeta$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $G_{k}$
|
|
\end_inset
|
|
|
|
is the
|
|
\begin_inset Formula $15\times21$
|
|
\end_inset
|
|
|
|
matrix for first order uncertainty propagation.
|
|
|
|
\begin_inset Formula $Q_{k}$
|
|
\end_inset
|
|
|
|
defines the uncertainty of
|
|
\begin_inset Formula $\eta$
|
|
\end_inset
|
|
|
|
.
|
|
The top-left
|
|
\begin_inset Formula $9\times9$
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $F_{k}$
|
|
\end_inset
|
|
|
|
is the same as
|
|
\begin_inset Formula $A_{k}$
|
|
\end_inset
|
|
|
|
, thus we only have the jacobians wrpt the biases left to account for.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Conveniently, the jacobians of the pose and velocity wrpt the biases are
|
|
already computed in the
|
|
\emph on
|
|
ImuFactor
|
|
\emph default
|
|
derivation as matrices
|
|
\begin_inset Formula $B_{k}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $C_{k}$
|
|
\end_inset
|
|
|
|
, while they are identity matrices wrpt the biases themselves.
|
|
Thus, we can easily plug-in the values from the previous section to give
|
|
us the final result
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
F_{k}\approx\left[\begin{array}{ccccc}
|
|
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & H(\theta_{k})^{-1}\Delta_{t}\\
|
|
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2} & 0_{3\times3}\\
|
|
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\
|
|
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3}\\
|
|
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Similarly for
|
|
\begin_inset Formula $Q_{k},$
|
|
\end_inset
|
|
|
|
we get
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
Q_{k}=\left[\begin{array}{ccccccc}
|
|
\Sigma^{\omega}\\
|
|
& \Sigma^{a}\\
|
|
& & \Sigma^{b^{a}}\\
|
|
& & & \Sigma^{b^{\omega}}\\
|
|
& & & & \Sigma^{int}\\
|
|
& & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
|
|
& & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
and for
|
|
\begin_inset Formula $G_{k}$
|
|
\end_inset
|
|
|
|
we get
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
G_{k}=\left[\begin{array}{ccccccc}
|
|
\deriv{\theta}{\epsilon^{\omega}} & \deriv{\theta}{\epsilon^{a}} & \deriv{\theta}{\epsilon^{b^{a}}} & \deriv{\theta}{\epsilon^{b^{\omega}}} & \deriv{\theta}{\epsilon^{int}} & \deriv{\theta}{\epsilon_{init}^{b^{a}}} & \deriv{\theta}{\epsilon_{init}^{b^{\omega}}}\\
|
|
\deriv p{\epsilon^{\omega}} & \deriv p{\epsilon^{a}} & \deriv p{\epsilon^{b^{a}}} & \deriv p{\epsilon^{b^{\omega}}} & \deriv p{\epsilon^{int}} & \deriv p{\epsilon_{init}^{b^{a}}} & \deriv p{\epsilon_{init}^{b^{\omega}}}\\
|
|
\deriv v{\epsilon^{\omega}} & \deriv v{\epsilon^{a}} & \deriv v{\epsilon^{b^{a}}} & \deriv v{\epsilon^{b^{\omega}}} & \deriv v{\epsilon^{int}} & \deriv v{\epsilon_{init}^{b^{a}}} & \deriv v{\epsilon_{init}^{b^{\omega}}}\\
|
|
\deriv{b^{a}}{\epsilon^{\omega}} & \deriv{b^{a}}{\epsilon^{a}} & \deriv{b^{a}}{\epsilon^{b^{a}}} & \deriv{b^{a}}{\epsilon^{b^{\omega}}} & \deriv{b^{a}}{\epsilon^{int}} & \deriv{b^{a}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{a}}{\epsilon_{init}^{b^{\omega}}}\\
|
|
\deriv{b^{\omega}}{\epsilon^{\omega}} & \deriv{b^{\omega}}{\epsilon^{a}} & \deriv{b^{\omega}}{\epsilon^{b^{a}}} & \deriv{b^{\omega}}{\epsilon^{b^{\omega}}} & \deriv{b^{\omega}}{\epsilon^{int}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{\omega}}}
|
|
\end{array}\right]=\left[\begin{array}{ccccccc}
|
|
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
|
|
0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
|
|
0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
|
|
0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can perform the block-wise computation of
|
|
\begin_inset Formula $G_{k}Q_{k}G_{k}^{T}$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
|
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
|
|
0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
|
|
0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
|
|
0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
|
|
\end{array}\right]\left[\begin{array}{ccccccc}
|
|
\Sigma^{\omega}\\
|
|
& \Sigma^{a}\\
|
|
& & \Sigma^{b^{a}}\\
|
|
& & & \Sigma^{b^{\omega}}\\
|
|
& & & & \Sigma^{int}\\
|
|
& & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
|
|
& & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}}
|
|
\end{array}\right]G_{k}^{T}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
|
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
|
|
0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
|
0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
|
0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
|
|
\end{array}\right]G_{k}^{T}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\begin{multline*}
|
|
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
|
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
|
|
0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
|
0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
|
0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
|
|
\end{array}\right]\\
|
|
\left[\begin{array}{ccccc}
|
|
\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
|
|
0 & \deriv p{\epsilon^{a}}^{T} & \deriv v{\epsilon^{a}}^{T} & 0 & 0\\
|
|
0 & 0 & 0 & I_{3\times3} & 0\\
|
|
0 & 0 & 0 & 0 & I_{3\times3}\\
|
|
0 & \deriv p{\epsilon^{int}}^{T} & 0 & 0 & 0\\
|
|
0 & \deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
|
\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0
|
|
\end{array}\right]
|
|
\end{multline*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\begin{multline*}
|
|
=\\
|
|
\left[\begin{array}{ccccc}
|
|
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
|
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}\\
|
|
& +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
|
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
|
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
|
|
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
|
|
\end{array}\right]
|
|
\end{multline*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
which we can break into 3 matrices for clarity, representing the main diagonal
|
|
and off-diagonal elements
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\begin{multline*}
|
|
=\\
|
|
\left[\begin{array}{ccccc}
|
|
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
|
|
0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T} & 0 & 0 & 0\\
|
|
0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T} & 0 & 0\\
|
|
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
|
|
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
|
|
\end{array}\right]+\\
|
|
\left[\begin{array}{ccccc}
|
|
\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0\\
|
|
0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
|
|
0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
|
0 & 0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0 & 0
|
|
\end{array}\right]+\\
|
|
\left[\begin{array}{ccccc}
|
|
0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
|
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
|
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0 & 0
|
|
\end{array}\right]
|
|
\end{multline*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Covariance Discretization
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "subsec:Covariance-Discretization"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
So far, all the covariances are assumed to be continuous since the state
|
|
and measurement models are considered to be continuous-time stochastic
|
|
processes.
|
|
However, we sample measurements in a discrete-time fashion, necessitating
|
|
the need to convert the covariances to their discrete time equivalents.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The IMU is modeled as a first order Gauss-Markov process, with a measurement
|
|
noise and a process noise.
|
|
Following
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
after "Alg. 1 Page 57"
|
|
key "Nikolic16thesis"
|
|
literal "false"
|
|
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
after "Eqns 129-130"
|
|
key "Trawny05report_IndirectKF"
|
|
literal "false"
|
|
|
|
\end_inset
|
|
|
|
, the measurement noises
|
|
\begin_inset Formula $[\epsilon^{a},\epsilon^{\omega},\epsilon_{init}]$
|
|
\end_inset
|
|
|
|
are simply scaled by
|
|
\begin_inset Formula $\frac{1}{\Delta t}$
|
|
\end_inset
|
|
|
|
, and the process noises
|
|
\begin_inset Formula $[\epsilon^{int},\epsilon^{b^{a}},\epsilon^{b^{\omega}}]$
|
|
\end_inset
|
|
|
|
are scaled by
|
|
\begin_inset Formula $\Delta t$
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\Delta t$
|
|
\end_inset
|
|
|
|
is the time interval between 2 consecutive samples.
|
|
For a thorough explanation of the discretization process, please refer
|
|
to
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
after "Section 8.1"
|
|
key "Simon06book"
|
|
literal "false"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset CommandInset bibtex
|
|
LatexCommand bibtex
|
|
btprint "btPrintCited"
|
|
bibfiles "refs"
|
|
options "plain"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_body
|
|
\end_document
|