2021-09-18 02:21:39 +08:00
#LyX 2.3 created this file. For more info see http://www.lyx.org/
\lyxformat 544
2015-10-11 04:48:52 +08:00
\begin_document
\begin_header
2021-09-18 02:21:39 +08:00
\save_transient_properties true
\origin unavailable
2015-10-11 04:48:52 +08:00
\textclass article
\use_default_options true
\maintain_unincluded_children false
\language english
\language_package default
\inputencoding auto
\fontencoding global
2021-09-18 02:21:39 +08:00
\font_roman "default" "default"
\font_sans "default" "default"
\font_typewriter "default" "default"
\font_math "auto" "auto"
2015-10-11 04:48:52 +08:00
\font_default_family default
\use_non_tex_fonts false
\font_sc false
\font_osf false
2021-09-18 02:21:39 +08:00
\font_sf_scale 100 100
\font_tt_scale 100 100
\use_microtype false
\use_dash_ligatures true
2015-10-11 04:48:52 +08:00
\graphics default
\default_output_format default
\output_sync 0
\bibtex_command default
\index_command default
2015-10-13 00:24:07 +08:00
\paperfontsize 11
\spacing single
2015-10-11 04:48:52 +08:00
\use_hyperref false
\papersize default
2015-10-13 00:24:07 +08:00
\use_geometry true
2021-09-18 02:21:39 +08:00
\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
2015-10-11 04:48:52 +08:00
\cite_engine basic
2021-09-18 02:21:39 +08:00
\cite_engine_type default
\biblio_style plain
2015-10-11 04:48:52 +08:00
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
2021-09-18 02:21:39 +08:00
\justification true
2015-10-11 04:48:52 +08:00
\use_refstyle 1
2021-09-18 02:21:39 +08:00
\use_minted 0
2015-10-11 04:48:52 +08:00
\index Index
\shortcut idx
\color #008000
\end_index
2015-10-13 00:24:07 +08:00
\leftmargin 3cm
\topmargin 3cm
\rightmargin 3cm
\bottommargin 3cm
2015-10-11 04:48:52 +08:00
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\paragraph_indentation default
2021-09-18 02:21:39 +08:00
\is_math_indent 0
\math_numbering_side default
\quotes_style english
\dynamic_quotes 0
2015-10-11 04:48:52 +08:00
\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
2021-10-10 10:52:01 +08:00
The New IMU Factor
2015-10-11 04:48:52 +08:00
\end_layout
\begin_layout Author
2021-09-18 02:21:39 +08:00
Frank Dellaert & Varun Agrawal
2015-10-11 04:48:52 +08:00
\end_layout
2015-12-21 09:00:05 +08:00
\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
2021-09-28 23:45:31 +08:00
\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.
2021-10-10 10:51:48 +08:00
This is done exactly the same in both variants:
2021-09-28 23:45:31 +08:00
\begin_inset Formula
\begin{equation}
2021-10-10 10:51:48 +08:00
e(X_{i},X_{j})=X_{j}\ominus\widehat{X_{j}}\label{eq:imu-factor-error}
2021-09-28 23:45:31 +08:00
\end{equation}
\end_inset
2021-10-10 10:51:48 +08:00
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
2021-09-28 23:45:31 +08:00
2021-10-10 10:51:48 +08:00
, 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
.
2022-07-04 03:57:54 +08:00
This (discrete-time) covariance matrix is computed in the preintegrated
measurement class, of which there are two variants as discussed above.
2021-09-28 23:45:31 +08:00
\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
2022-07-04 03:57:54 +08:00
\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
2021-09-28 23:45:31 +08:00
\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
2021-10-10 10:51:48 +08:00
\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
2021-09-28 23:45:31 +08:00
\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
2015-12-21 09:00:05 +08:00
\begin_layout Subsubsection*
Navigation States
\end_layout
2015-10-11 04:48:52 +08:00
\begin_layout Standard
2015-10-13 00:24:07 +08:00
Let us assume a setup where frames with image and/or laser measurements
are processed at some fairly low rate, e.g., 10 Hz.
2015-12-21 09:00:05 +08:00
\end_layout
\begin_layout Standard
We define the state of the vehicle at those times as attitude, position,
2015-10-11 04:48:52 +08:00
and velocity.
2015-10-13 00:24:07 +08:00
These three quantities are jointly referred to as a NavState
2015-12-21 09:00:05 +08:00
\begin_inset Formula $X_{b}^{n}\define\left\{ R_{b}^{n},P_{b}^{n},V_{b}^{n}\right\} $
2015-10-13 00:24:07 +08:00
\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.
2015-10-11 04:48:52 +08:00
\end_layout
2015-12-21 09:00:05 +08:00
\begin_layout Subsubsection*
Vector Fields and Differential Equations
\end_layout
2015-10-11 04:48:52 +08:00
\begin_layout Standard
2015-12-21 09:00:05 +08:00
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$
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
, defined by the orthonormality constraints on
\begin_inset Formula $\Rone$
\end_inset
.
For a NavState
\begin_inset Formula $X$
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
evolving over time we can write down a differential equation
2015-10-13 00:24:07 +08:00
\begin_inset Formula
2015-12-21 09:00:05 +08:00
\begin{equation}
\dot{X}(t)=F(t,X)\label{eq:diffeqM}
\end{equation}
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
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
2015-12-22 13:28:04 +08:00
\begin_inset Formula $\Rone\times M$
2015-12-21 09:00:05 +08:00
\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
\[
2016-01-17 10:34:16 +08:00
\left[\dot{R}(t,X),\dot{P}(t,X),\dot{V}(t,X)\right]\in\sothree\times\Rthree\times\Rthree
2015-12-21 09:00:05 +08:00
\]
\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
\[
2021-09-18 02:21:39 +08:00
\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\,X(0)=\left\{ R_{0},P_{0},V_{0}\right\}
2015-12-21 09:00:05 +08:00
\]
\end_inset
2015-10-13 00:24:07 +08:00
\end_layout
\begin_layout Standard
2015-12-21 09:00:05 +08:00
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}
2016-01-17 10:34:16 +08:00
\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]\label{eq:validField}
2015-12-21 09:00:05 +08:00
\end{equation}
\end_inset
2016-01-17 10:34:16 +08:00
Suppose we are given the
\series bold
body angular velocity
\series default
2015-12-21 09:00:05 +08:00
\begin_inset Formula $\omega^{b}(t)$
\end_inset
and non-gravity
2016-01-17 10:34:16 +08:00
\series bold
acceleration
\series default
2015-12-21 09:00:05 +08:00
\begin_inset Formula $a^{b}(t)$
\end_inset
2016-01-17 10:34:16 +08:00
in the body frame.
2022-07-04 03:57:54 +08:00
We know (from
\begin_inset CommandInset citation
LatexCommand cite
key "Murray94book"
literal "false"
\end_inset
) that the derivative of
2016-01-17 10:34:16 +08:00
\begin_inset Formula $R$
\end_inset
can be written as
2015-12-21 09:00:05 +08:00
\begin_inset Formula
\[
2016-01-17 10:34:16 +08:00
\dot{R}(X,t)=R(t)\Skew{\omega^{b}(t)}
2015-12-21 09:00:05 +08:00
\]
\end_inset
where
2016-01-17 10:34:16 +08:00
\begin_inset Formula $\Skew{\theta}\in so(3)$
2015-12-21 09:00:05 +08:00
\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}
2016-01-17 10:34:16 +08:00
\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}
2015-12-21 09:00:05 +08:00
\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
\[
2019-09-07 00:02:15 +08:00
\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}
2015-12-21 09:00:05 +08:00
\]
\end_inset
Hence, the 3-vector
2015-10-13 00:24:07 +08:00
\begin_inset Formula $\omega$
\end_inset
2015-12-21 09:00:05 +08:00
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*
2015-12-25 06:26:32 +08:00
Derivative of The Local Coordinate Mapping
2015-12-21 09:00:05 +08:00
\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$
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
\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"
2021-09-18 02:21:39 +08:00
literal "true"
2015-12-21 09:00:05 +08:00
\end_inset
, when we have a differential equation on
\begin_inset Formula $\SOthree$
\end_inset
,
\begin_inset Formula
\begin{equation}
2021-09-18 02:21:39 +08:00
\dot{R}(t)=F(R,t),\,\,\,\,R(0)=R_{0}\label{eq:diffSo3}
2015-12-21 09:00:05 +08:00
\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
2016-01-17 10:34:16 +08:00
To find an expression for
\begin_inset Formula $\dot{\theta}(t)$
\end_inset
, create a trajectory
2015-12-21 09:00:05 +08:00
\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
2016-01-17 10:34:16 +08:00
, and moves
\begin_inset Formula $\theta(t)$
\end_inset
along the direction
\begin_inset Formula $\dot{\theta}(t)$
\end_inset
:
2015-12-21 09:00:05 +08:00
\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
2016-01-17 10:34:16 +08:00
Taking the derivative for
2015-12-21 09:00:05 +08:00
\begin_inset Formula $\delta=0$
\end_inset
we obtain
\begin_inset Formula
\[
2015-12-22 13:28:04 +08:00
\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)}
2015-12-21 09:00:05 +08:00
\]
\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
2015-12-22 13:28:04 +08:00
\begin_inset Formula $X_{0}$
2015-12-21 09:00:05 +08:00
\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$
2015-10-13 00:24:07 +08:00
\end_inset
and
2015-12-21 09:00:05 +08:00
\begin_inset Formula $T_{X_{0}}M$
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
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
\[
2015-12-22 13:28:04 +08:00
\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\}
2015-12-21 09:00:05 +08:00
\]
\end_inset
and taking the derivative for
\begin_inset Formula $\delta=0$
\end_inset
we obtain
\begin_inset Formula
\[
2015-12-22 13:28:04 +08:00
\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]
2015-12-21 09:00:05 +08:00
\]
\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
\[
2015-12-23 11:39:03 +08:00
\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]
2015-12-21 09:00:05 +08:00
\]
\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*}
2015-12-22 04:55:01 +08:00
\dot{\theta}(t) & = & H(\theta)^{-1}\,\omega^{b}(t)\\
2021-09-18 02:21:39 +08:00
\dot{p}(t) & = & R_{0}^{T}\,V_{0}+v(t)\\
\dot{v}(t) & = & R_{0}^{T}\,g+R_{b}^{0}(t)a^{b}(t)
2015-12-21 09:00:05 +08:00
\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
.
2015-10-13 00:24:07 +08:00
\end_layout
2015-12-21 09:00:05 +08:00
\begin_layout Subsubsection*
Application: The New IMU Factor
\end_layout
\begin_layout Standard
2015-12-24 09:59:37 +08:00
In the IMU factor, we need to predict the NavState
\begin_inset Formula $X_{j}$
2015-12-21 09:00:05 +08:00
\end_inset
2015-12-24 09:59:37 +08:00
from the current NavState
\begin_inset Formula $X_{i}$
2015-12-21 09:00:05 +08:00
\end_inset
2015-12-24 09:59:37 +08:00
and the IMU measurements in-between.
The above scheme suffers from a problem, which is that
\begin_inset Formula $X_{i}$
2015-12-21 09:00:05 +08:00
\end_inset
2015-12-24 09:59:37 +08:00
needs to be known in order to compensate properly for the initial velocity
and rotated gravity vector.
2021-09-28 23:45:31 +08:00
Hence, the idea of Lupton
\begin_inset CommandInset citation
LatexCommand cite
key "Lupton12tro"
literal "false"
\end_inset
was to split up
2015-12-21 09:00:05 +08:00
\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*}
2021-09-18 02:21:39 +08:00
\dot{v}_{g}(t) & = & R_{i}^{T}\,g\\
2015-12-24 09:59:37 +08:00
\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t)
2015-12-21 09:00:05 +08:00
\end{eqnarray*}
\end_inset
The solution for the first equation is simply
2015-12-24 09:59:37 +08:00
\begin_inset Formula $v_{g}(t)=R_{i}^{T}gt$
2015-12-21 09:00:05 +08:00
\end_inset
.
Similarly, we split the position
\begin_inset Formula $p(t)$
\end_inset
up in three parts
\begin_inset Formula
\[
2015-12-24 09:59:37 +08:00
p(t)=p_{i}(t)+p_{g}(t)+p_{v}(t)
2015-12-21 09:00:05 +08:00
\]
\end_inset
2015-10-13 00:24:07 +08:00
2015-12-21 09:00:05 +08:00
evolving as
2015-10-13 00:24:07 +08:00
\begin_inset Formula
\begin{eqnarray*}
2021-09-18 02:21:39 +08:00
\dot{p}_{i}(t) & = & R_{i}^{T}\,V_{i}\\
2015-12-24 09:59:37 +08:00
\dot{p}_{g}(t) & = & v_{g}(t)=R_{i}^{T}gt\\
2015-12-21 09:00:05 +08:00
\dot{p}_{v}(t) & = & v_{a}(t)
2015-10-13 00:24:07 +08:00
\end{eqnarray*}
\end_inset
2015-12-21 09:00:05 +08:00
Here the solutions for the two first equations are simply
\begin_inset Formula
\begin{eqnarray*}
2015-12-24 09:59:37 +08:00
p_{i}(t) & = & R_{i}^{T}V_{i}t\\
p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
2015-12-21 09:00:05 +08:00
\end{eqnarray*}
2015-10-13 00:24:07 +08:00
\end_inset
2021-09-28 23:45:31 +08:00
The recipe for the IMU factor is then, in summary:
\end_layout
\begin_layout Enumerate
Solve the ordinary differential equations
2015-12-21 09:00:05 +08:00
\begin_inset Formula
\begin{eqnarray*}
2015-12-22 13:28:04 +08:00
\dot{\theta}(t) & = & H(\theta(t))^{-1}\,\omega^{b}(t)\\
2015-12-21 09:00:05 +08:00
\dot{p}_{v}(t) & = & v_{a}(t)\\
2015-12-24 09:59:37 +08:00
\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t)
2015-12-21 09:00:05 +08:00
\end{eqnarray*}
2015-10-13 00:24:07 +08:00
2015-12-21 09:00:05 +08:00
\end_inset
2015-10-13 00:24:07 +08:00
2015-12-21 09:00:05 +08:00
starting from zero, up to time
\begin_inset Formula $t_{ij}$
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
, where
2015-12-24 09:59:37 +08:00
\begin_inset Formula $R_{b}^{i}(t)=\exp\Skew{\theta(t)}$
2015-12-21 09:00:05 +08:00
\end_inset
at all times.
2021-09-28 23:45:31 +08:00
\end_layout
\begin_layout Enumerate
Form the local coordinate vector as
2015-10-13 00:24:07 +08:00
\begin_inset Formula
\[
2015-12-24 09:59:37 +08:00
\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]
2015-10-13 00:24:07 +08:00
\]
\end_inset
2021-09-28 23:45:31 +08:00
\end_layout
\begin_layout Enumerate
2015-12-21 09:00:05 +08:00
Predict the NavState
\begin_inset Formula $X_{j}$
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
at time
\begin_inset Formula $t_{j}$
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
from
\begin_inset Formula
\[
2021-09-18 02:21:39 +08:00
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\}
2015-12-21 09:00:05 +08:00
\]
2015-10-13 00:24:07 +08:00
\end_inset
2015-12-21 09:00:05 +08:00
2015-10-13 00:24:07 +08:00
\end_layout
2015-12-24 09:59:37 +08:00
\begin_layout Standard
Note that the predicted NavState
\begin_inset Formula $X_{j}$
\end_inset
depends on
\begin_inset Formula $X_{i}$
\end_inset
2016-01-17 10:34:16 +08:00
, but the integrated quantities
2015-12-24 09:59:37 +08:00
\begin_inset Formula $\theta(t)$
\end_inset
,
2016-01-17 10:34:16 +08:00
\begin_inset Formula $p_{v}(t)$
2015-12-24 09:59:37 +08:00
\end_inset
, and
\begin_inset Formula $v_{a}(t)$
\end_inset
do not.
\end_layout
2015-12-22 04:55:01 +08:00
\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
2015-12-25 06:26:32 +08:00
\begin{eqnarray}
2016-01-17 10:34:16 +08:00
\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}
2015-12-25 06:26:32 +08:00
\end{eqnarray}
2015-12-22 04:55:01 +08:00
\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
2016-01-17 10:34:16 +08:00
.
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
2015-12-22 04:55:01 +08:00
.
\end_layout
2015-12-25 06:26:32 +08:00
\begin_layout Subsubsection*
2021-09-28 23:45:31 +08:00
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}
2021-09-29 06:26:03 +08:00
\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,(\omega_{k}^{b}+\epsilon_{k}^{\omega}-b_{k}^{\omega}-\epsilon_{init}^{\omega})\Delta_{t}\nonumber \\
2021-09-28 23:45:31 +08:00
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
2015-12-25 06:26:32 +08:00
\end_layout
2015-12-24 09:59:37 +08:00
\begin_layout Standard
2021-10-10 10:51:48 +08:00
We wish to compute the ImuFactor covariance matrix
\begin_inset Formula $\Sigma_{ij}$
\end_inset
.
Even when we assume uncorrelated noise on
2015-12-25 06:26:32 +08:00
\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
2016-01-18 11:18:20 +08:00
and
2015-12-25 06:26:32 +08:00
\begin_inset Formula $v_{k}$
\end_inset
appear in multiple places.
2021-10-10 10:51:48 +08:00
To model the noise propagation, let us define the preintegrated navigation
state
2015-12-25 06:26:32 +08:00
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$
2015-12-24 09:59:37 +08:00
\end_inset
2021-10-10 10:51:48 +08:00
, as a 9D vector on tangent space at and rewrite Eqns.
2015-12-25 06:26:32 +08:00
(
\begin_inset CommandInset ref
LatexCommand ref
reference "eq:euler_theta"
\end_inset
2016-01-18 11:18:20 +08:00
-
2015-12-25 06:26:32 +08:00
\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
\[
2016-01-18 11:18:20 +08:00
\zeta_{k+1}=f\left(\zeta_{k},a_{k}^{b},\omega_{k}^{b}\right)
2015-12-25 06:26:32 +08:00
\]
\end_inset
Then the noise on
\begin_inset Formula $\zeta_{k+1}$
\end_inset
propagates as
\begin_inset Formula
\begin{equation}
2021-09-28 23:45:31 +08:00
\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}
2015-12-25 06:26:32 +08:00
\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
2016-01-17 10:34:16 +08:00
\begin_inset Formula $a^{b}$
2015-12-25 06:26:32 +08:00
\end_inset
and
2016-01-17 10:34:16 +08:00
\begin_inset Formula $\omega^{b}$
2015-12-24 09:59:37 +08:00
\end_inset
2022-07-04 03:57:54 +08:00
.
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
2015-12-24 09:59:37 +08:00
.
2016-01-17 10:34:16 +08:00
\end_layout
2016-01-18 11:18:20 +08:00
\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
2016-01-17 10:34:16 +08:00
\begin_layout Standard
2015-12-25 06:26:32 +08:00
\begin_inset Formula
\[
2021-09-28 23:45:31 +08:00
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
2015-12-25 06:26:32 +08:00
\]
\end_inset
2016-01-17 10:34:16 +08:00
It can be shown that for small
\begin_inset Formula $\theta_{k}$
2015-12-25 06:26:32 +08:00
\end_inset
2016-01-17 10:34:16 +08:00
we have
2015-12-25 06:26:32 +08:00
\begin_inset Formula
\[
2022-07-04 03:57:54 +08:00
\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}}
2015-12-25 06:26:32 +08:00
\]
\end_inset
2016-01-17 10:34:16 +08:00
For the derivatives of
\begin_inset Formula $p_{k+1}$
2015-12-25 06:26:32 +08:00
\end_inset
2016-01-17 10:34:16 +08:00
and
\begin_inset Formula $v_{k+1}$
2015-12-25 06:26:32 +08:00
\end_inset
2016-01-17 10:34:16 +08:00
we need the derivative
2015-12-25 06:26:32 +08:00
\begin_inset Formula
\[
2016-01-18 11:18:20 +08:00
\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})
2015-12-25 06:26:32 +08:00
\]
\end_inset
2016-01-17 10:34:16 +08:00
where we used
2015-12-25 06:26:32 +08:00
\begin_inset Formula
\[
2016-01-18 11:18:20 +08:00
\deriv{\left(Ra\right)}R\approx R\Skew{-a}
2015-12-25 06:26:32 +08:00
\]
\end_inset
2016-01-17 10:34:16 +08:00
and the fact that the dependence of the rotation
\begin_inset Formula $R_{k}$
2015-12-25 06:26:32 +08:00
\end_inset
2016-01-17 10:34:16 +08:00
on
\begin_inset Formula $\theta_{k}$
2015-12-25 06:26:32 +08:00
\end_inset
2016-01-17 10:34:16 +08:00
is the already computed
\begin_inset Formula $H(\theta_{k})$
2015-12-25 06:26:32 +08:00
\end_inset
2016-01-17 10:34:16 +08:00
.
\end_layout
2015-12-25 06:26:32 +08:00
2016-01-17 10:34:16 +08:00
\begin_layout Standard
Putting all this together, we finally obtain
2015-12-25 06:26:32 +08:00
\begin_inset Formula
\[
2016-01-17 10:34:16 +08:00
A_{k}\approx\left[\begin{array}{ccc}
2021-09-28 23:45:31 +08:00
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3}\\
2016-01-18 11:18:20 +08:00
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
2021-09-28 23:45:31 +08:00
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3}
2016-01-17 10:34:16 +08:00
\end{array}\right]
2015-12-25 06:26:32 +08:00
\]
\end_inset
2016-01-17 10:34:16 +08:00
The other partial derivatives are simply
2015-12-25 06:26:32 +08:00
\begin_inset Formula
\[
2016-01-17 10:34:16 +08:00
B_{k}=\left[\begin{array}{c}
0_{3\times3}\\
R_{k}\frac{\Delta_{t}}{2}^{2}\\
R_{k}\Delta_{t}
2021-09-18 02:21:39 +08:00
\end{array}\right],\,\,\,\,C_{k}=\left[\begin{array}{c}
2016-01-17 10:34:16 +08:00
H(\theta_{k})^{-1}\Delta_{t}\\
0_{3\times3}\\
0_{3\times3}
\end{array}\right]
2015-12-25 06:26:32 +08:00
\]
\end_inset
2021-09-18 02:21:39 +08:00
\end_layout
\begin_layout Subsubsection*
2021-09-28 23:45:31 +08:00
Noise Propagation in Combined IMU Factor
2021-09-18 02:21:39 +08:00
\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
2021-09-22 00:29:36 +08:00
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$
2021-09-18 02:21:39 +08:00
\end_inset
2021-09-28 23:45:31 +08:00
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
.
2021-09-22 00:29:36 +08:00
This gives the noise propagation equation as
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{equation}
2021-09-28 23:45:31 +08:00
\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}Q_{k}G_{k}^{T}\label{eq:prop-combined}
2021-09-22 00:29:36 +08:00
\end{equation}
\end_inset
\end_layout
\begin_layout Standard
where
2021-09-18 02:21:39 +08:00
\begin_inset Formula $F_{k}$
\end_inset
2021-09-22 00:29:36 +08:00
is the
\begin_inset Formula $15\times15$
\end_inset
derivative of
2021-09-18 02:21:39 +08:00
\begin_inset Formula $f$
\end_inset
wrpt this new
\begin_inset Formula $\zeta$
\end_inset
2021-09-22 00:29:36 +08:00
, and
\begin_inset Formula $G_{k}$
\end_inset
is the
2021-09-28 23:45:31 +08:00
\begin_inset Formula $15\times21$
2021-09-18 02:21:39 +08:00
\end_inset
2021-09-22 00:29:36 +08:00
matrix for first order uncertainty propagation.
2021-09-28 23:45:31 +08:00
\begin_inset Formula $Q_{k}$
\end_inset
defines the uncertainty of
\begin_inset Formula $\eta$
\end_inset
.
2021-09-18 02:21:39 +08:00
The top-left
\begin_inset Formula $9\times9$
2021-09-22 00:29:36 +08:00
\end_inset
of
\begin_inset Formula $F_{k}$
2021-09-18 02:21:39 +08:00
\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
\[
2021-09-22 00:29:36 +08:00
F_{k}\approx\left[\begin{array}{ccccc}
2021-09-28 23:45:31 +08:00
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}
2021-09-18 02:21:39 +08:00
\end{array}\right]
\]
\end_inset
2021-09-28 23:45:31 +08:00
\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}
2021-09-29 06:26:03 +08:00
\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
0 & \deriv p{\epsilon^{a}}^{T} & \deriv v{\epsilon^{a}}^{T} & 0 & 0\\
2021-09-28 23:45:31 +08:00
0 & 0 & 0 & I_{3\times3} & 0\\
0 & 0 & 0 & 0 & I_{3\times3}\\
2021-09-29 06:26:03 +08:00
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
2021-09-28 23:45:31 +08:00
\end{array}\right]
\end{multline*}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{multline*}
=\\
\left[\begin{array}{ccccc}
2021-09-29 06:26:03 +08:00
\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\\
2021-09-28 23:45:31 +08:00
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}
2021-09-29 06:26:03 +08:00
\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\\
2021-09-28 23:45:31 +08:00
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
\end{array}\right]+\\
\left[\begin{array}{ccccc}
2021-09-29 06:26:03 +08:00
\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\\
2021-09-28 23:45:31 +08:00
0 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0
\end{array}\right]+\\
\left[\begin{array}{ccccc}
2021-09-29 06:26:03 +08:00
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\\
2021-09-28 23:45:31 +08:00
0 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0
\end{array}\right]
\end{multline*}
\end_inset
2015-10-13 00:24:07 +08:00
\end_layout
2021-09-29 06:26:03 +08:00
\begin_layout Subsubsection*
Covariance Discretization
2022-07-04 03:57:54 +08:00
\begin_inset CommandInset label
LatexCommand label
name "subsec:Covariance-Discretization"
\end_inset
2021-09-29 06:26:03 +08:00
\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
2015-12-21 09:00:05 +08:00
\begin_layout Standard
\begin_inset CommandInset bibtex
LatexCommand bibtex
2021-09-28 23:45:31 +08:00
btprint "btPrintCited"
2015-12-21 09:00:05 +08:00
bibfiles "refs"
options "plain"
2015-12-16 04:44:22 +08:00
\end_inset
2015-10-11 04:48:52 +08:00
\end_layout
\end_body
\end_document
