12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #874 from borglab/fix/368 

CombinedImuFactor: Add bias effect on position jacobian
release/4.3a0
Varun Agrawal 2022-08-21 16:33:53 -04:00 committed by GitHub
parent fb8d6abb21 103c78b0de
commit 587678e0b7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 1198 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

850
doc/ImuFactor.lyx
View File

@ -1,7 +1,9 @@
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
#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
@ -9,16 +11,18 @@
\language_package default
\inputencoding auto
\fontencoding global
\font_roman default
\font_sans default
\font_typewriter default
\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
\font_tt_scale 100
\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
@ -29,16 +33,26 @@
\use_hyperref false
\papersize default
\use_geometry true
\use_amsmath 1
\use_esint 1
\use_mhchem 1
\use_mathdots 1
\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
@ -51,7 +65,10 @@
\tocdepth 3
\paragraph_separation indent
\paragraph_indentation default
\quotes_language english
\is_math_indent 0
\math_numbering_side default
\quotes_style english
\dynamic_quotes 0
\papercolumns 1
\papersides 1
\paperpagestyle default
@ -65,11 +82,11 @@
\begin_body
\begin_layout Title
The new IMU Factor
The New IMU Factor
\end_layout
\begin_layout Author
Frank Dellaert
Frank Dellaert & Varun Agrawal
\end_layout
\begin_layout Standard
@ -91,6 +108,282 @@ filename "macros.lyx"
\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
@ -285,7 +578,15 @@ acceleration
\end_inset
in the body frame.
We know (from Murray84book) that the derivative of
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
@ -592,6 +893,7 @@ Lie Group Methods
\begin_inset CommandInset citation
LatexCommand cite
key "Iserles00an"
literal "true"
\end_inset
@ -707,15 +1009,6 @@ In other words, the vector field
Retractions
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
{\mathfrak{\mathbb{R}^{9}}}
\end_inset
\end_layout
\begin_layout Standard
Note that the use of the exponential map in local coordinate mappings is
not obligatory, even in the context of Lie groups.
@ -1000,7 +1293,15 @@ In the IMU factor, we need to predict the NavState
needs to be known in order to compensate properly for the initial velocity
and rotated gravity vector.
Hence, the idea of Lupton was to split up
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
@ -1057,7 +1358,10 @@ p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
\end_inset
The recipe for the IMU factor is then, in summary.
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*}
@ -1077,6 +1381,9 @@ starting from zero, up to time
\end_inset
at all times.
\end_layout
\begin_layout Enumerate
Form the local coordinate vector as
\begin_inset Formula
\[
@ -1085,6 +1392,10 @@ starting from zero, up to time
\end_inset
\end_layout
\begin_layout Enumerate
Predict the NavState
\begin_inset Formula $X_{j}$
\end_inset
@ -1179,10 +1490,58 @@ where we defined the rotation matrix
\end_layout
\begin_layout Subsubsection*
Noise Propagation
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
@ -1201,11 +1560,12 @@ Even when we assume uncorrelated noise on
\end_inset
appear in multiple places.
To model the noise propagation, let us define
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
and rewrite Eqns.
, as a 9D vector on tangent space at and rewrite Eqns.
(
\begin_inset CommandInset ref
LatexCommand ref
@ -1239,7 +1599,7 @@ Then the noise on
propagates as
\begin_inset Formula
\begin{equation}
\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop}
\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
@ -1280,6 +1640,42 @@ where
\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
@ -1295,7 +1691,7 @@ We start with the noise propagation on
\begin_layout Standard
\begin_inset Formula
\[
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
\]
\end_inset
@ -1307,7 +1703,7 @@ It can be shown that for small
we have
\begin_inset Formula
\[
\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}}
\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
@ -1357,9 +1753,9 @@ 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}}\\
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} & & I_{3\times3}
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3}
\end{array}\right]
\]
@ -1384,9 +1780,393 @@ H(\theta_{k})^{-1}\Delta_{t}\\
\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"

BIN
doc/ImuFactor.pdf
View File

Binary file not shown.

BIN
doc/PreintegratedIMUJacobians.pdf Normal file
View File

Binary file not shown.

76
doc/refs.bib
View File

@ -1,26 +1,72 @@
%% This BibTeX bibliography file was created using BibDesk.
%% https://bibdesk.sourceforge.io/
%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400
%% Saved with string encoding Unicode (UTF-8)
@article{Lupton12tro,
author = {Lupton, Todd and Sukkarieh, Salah},
date-added = {2021-09-27 17:38:56 -0400},
date-modified = {2021-09-27 17:39:09 -0400},
doi = {10.1109/TRO.2011.2170332},
journal = {IEEE Transactions on Robotics},
number = {1},
pages = {61-76},
title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
volume = {28},
year = {2012},
Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
@inproceedings{Forster15rss,
author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
booktitle = {Robotics: Science and Systems},
date-added = {2021-09-26 20:44:41 -0400},
date-modified = {2021-09-26 20:45:03 -0400},
title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
year = {2015}}
@article{Iserles00an,
title = {Lie-group methods},
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and
N{\o}rsett, Syvert P and Zanna, Antonella},
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
journal = {Acta Numerica 2000},
volume = {9},
pages = {215--365},
year = {2000},
publisher = {Cambridge Univ Press}
}
publisher = {Cambridge Univ Press},
title = {Lie-group methods},
volume = {9},
year = {2000}}
@book{Murray94book,
author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
publisher = {CRC press},
title = {A mathematical introduction to robotic manipulation},
author = {Murray, Richard M and Li, Zexiang and Sastry, S
Shankar and Sastry, S Shankara},
year = {1994},
publisher = {CRC press}
}
year = {1994}}
@book{Spivak65book,
title = {Calculus on manifolds},
author = {Spivak, Michael},
publisher = {WA Benjamin New York},
title = {Calculus on manifolds},
volume = {1},
year = {1965},
publisher = {WA Benjamin New York}
year = {1965}}
@phdthesis{Nikolic16thesis,
title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation},
author={Nikolic, Janosch},
year={2016},
school={ETH Zurich}
}
@book{Simon06book,
title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches},
author={Simon, Dan},
year={2006},
publisher={John Wiley \& Sons}
}
@inproceedings{Trawny05report_IndirectKF,
title={Indirect Kalman Filter for 3 D Attitude Estimation},
author={Nikolas Trawny and Stergios I. Roumeliotis},
year={2005}
}

17
examples/CombinedImuFactorsExample.cpp
View File

@ -60,13 +60,14 @@ namespace po = boost::program_options;
po::variables_map parseOptions(int argc, char* argv[]) {
po::options_description desc;
desc.add_options()("help,h", "produce help message")(
"data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
"path to the CSV file with the IMU data")(
"output_filename",
desc.add_options()("help,h", "produce help message") // help message
("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
"path to the CSV file with the IMU data") // path to the data file
("output_filename",
po::value<string>()->default_value("imuFactorExampleResults.csv"),
"path to the result file to use")("use_isam", po::bool_switch(),
"use ISAM as the optimizer");
"path to the result file to use") // filename to save results to
("use_isam", po::bool_switch(),
"use ISAM as the optimizer"); // flag for ISAM optimizer
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
@ -106,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
I_3x3 * 1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
Matrix66 bias_acc_omega_int =
Matrix66 bias_acc_omega_init =
I_6x6 * 1e-5; // error in the bias used for preintegration
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@ -122,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
p->biasAccOmegaInt = bias_acc_omega_init;
return p;
}

4
examples/ImuFactorsExample.cpp
View File

@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
I_3x3 * 1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
Matrix66 bias_acc_omega_int =
Matrix66 bias_acc_omega_init =
I_6x6 * 1e-5; // error in the bias used for preintegration
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
p->biasAccOmegaInt = bias_acc_omega_init;
return p;
}

1
gtsam/base/Vector.h
View File

@ -60,6 +60,7 @@ GTSAM_MAKE_VECTOR_DEFS(9)
GTSAM_MAKE_VECTOR_DEFS(10)
GTSAM_MAKE_VECTOR_DEFS(11)
GTSAM_MAKE_VECTOR_DEFS(12)
GTSAM_MAKE_VECTOR_DEFS(15)
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;

77
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -93,9 +93,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
if (dt <= 0) {
throw std::runtime_error(
"PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
}
// Update preintegrated measurements.
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx)
Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively.
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// Update preintegrated measurements covariance: as in [2] we consider a first
@ -105,47 +110,78 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// and preintegrated measurements
// Single Jacobians to propagate covariance
// TODO(frank): should we not also account for bias on position?
Matrix3 theta_H_biasOmega = -C.topRows<3>();
Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
Matrix3 theta_H_biasOmega = C.topRows<3>();
Matrix3 pos_H_biasAcc = B.middleRows<3>(3);
Matrix3 vel_H_biasAcc = B.bottomRows<3>();
Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega;
Matrix3 pos_H_biasAccInit = -pos_H_biasAcc;
Matrix3 vel_H_biasAccInit = -vel_H_biasAcc;
// overall Jacobian wrt preintegrated measurements (df/dx)
Eigen::Matrix<double, 15, 15> F;
F.setZero();
F.block<9, 9>(0, 0) = A;
F.block<3, 3>(0, 12) = theta_H_biasOmega;
F.block<3, 3>(3, 9) = pos_H_biasAcc;
F.block<3, 3>(6, 9) = vel_H_biasAcc;
F.block<6, 6>(9, 9) = I_6x6;
// Update the uncertainty on the state (matrix F in [4]).
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
// propagate uncertainty
// TODO(frank): use noiseModel routine so we can have arbitrary noise models.
const Matrix3& aCov = p().accelerometerCovariance;
const Matrix3& wCov = p().gyroscopeCovariance;
const Matrix3& iCov = p().integrationCovariance;
const Matrix6& bInitCov = p().biasAccOmegaInt;
// first order uncertainty propagation
// Optimized matrix multiplication (1/dt) * G * measurementCovariance *
// G.transpose()
// Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
Eigen::Matrix<double, 15, 15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15);
const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt;
const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt;
const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt;
const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt;
// BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = dt * iCov;
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
* (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
* (vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
* (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
* (theta_H_biasOmega.transpose());
D_R_R(&G_measCov_Gt) =
(theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose()) //
+
(theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
D_t_t(&G_measCov_Gt) =
(pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) //
+ (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) //
+ (dt * iCov);
D_v_v(&G_measCov_Gt) =
(vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) //
+ (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
* theta_H_biasOmega.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
D_R_t(&G_measCov_Gt) =
theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
D_R_v(&G_measCov_Gt) =
theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
D_t_R(&G_measCov_Gt) =
pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
D_t_v(&G_measCov_Gt) =
(pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) +
(pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
D_v_R(&G_measCov_Gt) =
vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
D_v_t(&G_measCov_Gt) =
(vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) +
(vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
preintMeasCov_.noalias() += G_measCov_Gt;
}
//------------------------------------------------------------------------------
@ -253,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os;
}
}
/// namespace gtsam
} // namespace gtsam

7
gtsam/navigation/CombinedImuFactor.h
View File

@ -51,6 +51,7 @@ typedef ManifoldPreintegration PreintegrationType;
* TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
* Computation of the Jacobian Matrices", Tech. Report, 2013.
* Available in this repo as "PreintegratedIMUJacobians.pdf".
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
* Robotics: Science and Systems (RSS), 2015.
@ -61,7 +62,7 @@ typedef ManifoldPreintegration PreintegrationType;
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate.
/// Default constructor makes uninitialized params struct.
/// Used for serialization.
@ -92,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
private:

6
gtsam/navigation/ImuFactor.cpp
View File

@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
// Update preintegrated measurements (also get Jacobian)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively.
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// first order covariance propagation:
@ -73,11 +73,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
const Matrix3& iCov = p().integrationCovariance;
// (1/dt) allows to pass from continuous time noise to discrete time noise
// Update the uncertainty on the state (matrix A in [4]).
preintMeasCov_ = A * preintMeasCov_ * A.transpose();
// These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]).
preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix)
preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
}

1
gtsam/navigation/ImuFactor.h
View File

@ -53,6 +53,7 @@ typedef ManifoldPreintegration PreintegrationType;
* TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
* Computation of the Jacobian Matrices", Tech. Report, 2013.
* Available in this repo as "PreintegratedIMUJacobians.pdf".
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
* Robotics: Science and Systems (RSS), 2015.

60
gtsam/navigation/ScenarioRunner.cpp
View File

@ -15,8 +15,8 @@
* @author Frank Dellaert
*/
#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/base/timing.h>
#include <gtsam/navigation/ScenarioRunner.h>
#include <boost/assign.hpp>
#include <cmath>
@ -105,4 +105,62 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
return Q / (N - 1);
}
PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
double T, const Bias& estimatedBias, bool corrupted) const {
gttic_(integrate);
PreintegratedCombinedMeasurements pim(p_, estimatedBias);
const double dt = imuSampleTime();
const size_t nrSteps = T / dt;
double t = 0;
for (size_t k = 0; k < nrSteps; k++, t += dt) {
Vector3 measuredOmega =
corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
Vector3 measuredAcc =
corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
}
return pim;
}
NavState CombinedScenarioRunner::predict(
const PreintegratedCombinedMeasurements& pim,
const Bias& estimatedBias) const {
const NavState state_i(scenario().pose(0), scenario().velocity_n(0));
return pim.predict(state_i, estimatedBias);
}
Eigen::Matrix<double, 15, 15> CombinedScenarioRunner::estimateCovariance(
double T, size_t N, const Bias& estimatedBias) const {
gttic_(estimateCovariance);
// Get predict prediction from ground truth measurements
NavState prediction = predict(integrate(T));
// Sample !
Matrix samples(15, N);
Vector15 sum = Vector15::Zero();
for (size_t i = 0; i < N; i++) {
auto pim = integrate(T, estimatedBias, true);
NavState sampled = predict(pim);
Vector15 xi = Vector15::Zero();
xi << sampled.localCoordinates(prediction),
(estimatedBias_.vector() - estimatedBias.vector());
samples.col(i) = xi;
sum += xi;
}
// Compute MC covariance
Vector15 sampleMean = sum / N;
Eigen::Matrix<double, 15, 15> Q;
Q.setZero();
for (size_t i = 0; i < N; i++) {
Vector15 xi = samples.col(i) - sampleMean;
Q += xi * xi.transpose();
}
return Q / (N - 1);
}
} // namespace gtsam

44
gtsam/navigation/ScenarioRunner.h
View File

@ -16,9 +16,10 @@
*/
#pragma once
#include <gtsam/linear/Sampler.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <gtsam/linear/Sampler.h>
namespace gtsam {
@ -66,10 +67,10 @@ class GTSAM_EXPORT ScenarioRunner {
// also, uses g=10 for easy debugging
const Vector3& gravity_n() const { return p_->n_gravity; }
const Scenario& scenario() const { return scenario_; }
// A gyro simply measures angular velocity in body frame
Vector3 actualAngularVelocity(double t) const {
return scenario_.omega_b(t);
}
Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
// An accelerometer measures acceleration in body, but not gravity
Vector3 actualSpecificForce(double t) const {
@ -106,4 +107,39 @@ class GTSAM_EXPORT ScenarioRunner {
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
};
/*
* Simple class to test navigation scenarios with CombinedImuMeasurements.
* Takes a trajectory scenario as input, and can generate IMU measurements
*/
class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
public:
typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
private:
const SharedParams p_;
const Bias estimatedBias_;
public:
CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0,
const Bias& bias = Bias())
: ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
imuSampleTime, bias),
p_(p),
estimatedBias_(bias) {}
/// Integrate measurements for T seconds into a PIM
PreintegratedCombinedMeasurements integrate(
double T, const Bias& estimatedBias = Bias(),
bool corrupted = false) const;
/// Predict predict given a PIM
NavState predict(const PreintegratedCombinedMeasurements& pim,
const Bias& estimatedBias = Bias()) const;
/// Compute a Monte Carlo estimate of the predict covariance using N samples
Eigen::Matrix<double, 15, 15> estimateCovariance(
double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
};
} // namespace gtsam

4
gtsam/navigation/navigation.i
View File

@ -165,11 +165,11 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
void setBiasAccCovariance(Matrix cov);
void setBiasOmegaCovariance(Matrix cov);
void setBiasAccOmegaInt(Matrix cov);
void setBiasAccOmegaInit(Matrix cov);
Matrix getBiasAccCovariance() const ;
Matrix getBiasOmegaCovariance() const ;
Matrix getBiasAccOmegaInt() const;
Matrix getBiasAccOmegaInit() const;
};

131
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -16,18 +16,19 @@
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
* @author Varun Agrawal
*/
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/nonlinear/Values.h>
#include <list>
@ -40,9 +41,12 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
p->integrationCovariance = 0.0001 * I_3x3;
p->biasAccCovariance = Z_3x3;
p->biasOmegaCovariance = Z_3x3;
p->biasAccOmegaInt = Z_6x6;
return p;
}
}
} // namespace testing
/* ************************************************************************* */
TEST(CombinedImuFactor, PreintegratedMeasurements ) {
@ -71,6 +75,7 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
}
/* ************************************************************************* */
TEST(CombinedImuFactor, ErrorWithBiases ) {
Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
@ -203,6 +208,114 @@ TEST(CombinedImuFactor, PredictRotation) {
EXPECT(assert_equal(expectedPose, actual.pose(), tol));
}
/* ************************************************************************* */
// Testing covariance to check if all the jacobians are accounted for.
TEST(CombinedImuFactor, CheckCovariance) {
auto params = PreintegrationCombinedParams::MakeSharedU(9.81);
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
params->setIntegrationCovariance(pow(0.0, 2) * I_3x3);
params->setOmegaCoriolis(Vector3::Zero());
imuBias::ConstantBias currentBias;
PreintegratedCombinedMeasurements actual(params, currentBias);
// Measurements
Vector3 measuredAcc(0.1577, -0.8251, 9.6111);
Vector3 measuredOmega(-0.0210, 0.0311, 0.0145);
double deltaT = 0.01;
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Eigen::Matrix<double, 15, 15> expected;
expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
// regression
EXPECT(assert_equal(expected, actual.preintMeasCov()));
}
// Test that the covariance values for the ImuFactor and the CombinedImuFactor
// (top-left 9x9) are the same
TEST(CombinedImuFactor, SameCovariance) {
// IMU measurements and time delta
Vector3 accMeas(0.1577, -0.8251, 9.6111);
Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
double deltaT = 0.01;
// Assume zero bias
imuBias::ConstantBias currentBias;
// Define params for ImuFactor
auto params = PreintegrationParams::MakeSharedU();
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
params->setIntegrationCovariance(pow(0, 2) * I_3x3);
params->setOmegaCoriolis(Vector3::Zero());
// The IMU preintegration object for ImuFactor
PreintegratedImuMeasurements pim(params, currentBias);
pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
// Define params for CombinedImuFactor
auto combined_params = PreintegrationCombinedParams::MakeSharedU();
combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
// Set bias integration covariance explicitly to zero
combined_params->setIntegrationCovariance(Z_3x3);
combined_params->setOmegaCoriolis(Z_3x1);
// Set bias initial covariance explicitly to zero
combined_params->setBiasAccOmegaInit(Z_6x6);
// The IMU preintegration object for CombinedImuFactor
PreintegratedCombinedMeasurements cpim(combined_params, currentBias);
cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
// Assert if the noise covariance
EXPECT(assert_equal(pim.preintMeasCov(),
cpim.preintMeasCov().block(0, 0, 9, 9)));
}
/* ************************************************************************* */
TEST(CombinedImuFactor, Accelerating) {
const double a = 0.2, v = 50;
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
// going in X The body itself has Z axis pointing down
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
const Point3 initial_position(10, 20, 0);
const Vector3 initial_velocity(v, 0, 0);
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
Vector3(a, 0, 0));
const double T = 3.0; // seconds
CombinedScenarioRunner runner(scenario, testing::Params(), T / 10);
PreintegratedCombinedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
auto estimatedCov = runner.estimateCovariance(T, 100);
Eigen::Matrix<double, 15, 15> expected = pim.preintMeasCov();
EXPECT(assert_equal(estimatedCov, expected, 0.1));
}
/* ************************************************************************* */
int main() {
TestResult tr;