Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

IMU Parameters Documentation #624

Merged
merged 5 commits into from
Dec 2, 2020
Merged
Changes from 3 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion cmake/GtsamTesting.cmake
Original file line number Diff line number Diff line change
@@ -88,7 +88,7 @@ enable_testing()

option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

oops... :-)


# Add option for combining unit tests
if(MSVC OR XCODE_VERSION)
180 changes: 15 additions & 165 deletions doc/ImuFactor.lyx
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#LyX 2.1 created this file. For more info see http://www.lyx.org/
\lyxformat 474
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
\begin_document
\begin_header
\textclass article
@@ -12,13 +12,13 @@
\font_roman default
\font_sans default
\font_typewriter default
\font_math auto
\font_default_family default
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100

\graphics default
\default_output_format default
\output_sync 0
@@ -29,24 +29,15 @@
\use_hyperref false
\papersize default
\use_geometry true
\use_package amsmath 1
\use_package amssymb 1
\use_package cancel 1
\use_package esint 1
\use_package mathdots 1
\use_package mathtools 1
\use_package mhchem 1
\use_package stackrel 1
\use_package stmaryrd 1
\use_package undertilde 1
\use_amsmath 1
\use_esint 1
\use_mhchem 1
\use_mathdots 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
\index Index
\shortcut idx
@@ -253,7 +244,7 @@ X(t)=\left\{ R_{0},P_{0}+V_{0}t,V_{0}\right\}
then the differential equation describing the trajectory is
\begin_inset Formula
\[
\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\,X(0)=\left\{ R_{0},P_{0},V_{0}\right\}
\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\, X(0)=\left\{ R_{0},P_{0},V_{0}\right\}
\]

\end_inset
@@ -611,7 +602,7 @@ key "Iserles00an"
,
\begin_inset Formula
\begin{equation}
\dot{R}(t)=F(R,t),\,\,\,\,R(0)=R_{0}\label{eq:diffSo3}
\dot{R}(t)=F(R,t),\,\,\,\, R(0)=R_{0}\label{eq:diffSo3}
\end{equation}

\end_inset
@@ -956,8 +947,8 @@ Or, as another way to state this, if we solve the differential equations
\begin_inset Formula
\begin{eqnarray*}
\dot{\theta}(t) & = & H(\theta)^{-1}\,\omega^{b}(t)\\
\dot{p}(t) & = & R_{0}^{T}\,V_{0}+v(t)\\
\dot{v}(t) & = & R_{0}^{T}\,g+R_{b}^{0}(t)a^{b}(t)
\dot{p}(t) & = & R_{0}^{T}\, V_{0}+v(t)\\
\dot{v}(t) & = & R_{0}^{T}\, g+R_{b}^{0}(t)a^{b}(t)
\end{eqnarray*}

\end_inset
@@ -1024,7 +1015,7 @@ v(t)=v_{g}(t)+v_{a}(t)
evolving as
\begin_inset Formula
\begin{eqnarray*}
\dot{v}_{g}(t) & = & R_{i}^{T}\,g\\
\dot{v}_{g}(t) & = & R_{i}^{T}\, g\\
\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t)
\end{eqnarray*}

@@ -1050,7 +1041,7 @@ p(t)=p_{i}(t)+p_{g}(t)+p_{v}(t)
evolving as
\begin_inset Formula
\begin{eqnarray*}
\dot{p}_{i}(t) & = & R_{i}^{T}\,V_{i}\\
\dot{p}_{i}(t) & = & R_{i}^{T}\, V_{i}\\
\dot{p}_{g}(t) & = & v_{g}(t)=R_{i}^{T}gt\\
\dot{p}_{v}(t) & = & v_{a}(t)
\end{eqnarray*}
@@ -1105,7 +1096,7 @@ Predict the NavState
from
\begin_inset Formula
\[
X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\,p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\,v_{a}(t_{ij})\right\}
X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\, p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\, v_{a}(t_{ij})\right\}
\]

\end_inset
@@ -1381,7 +1372,7 @@ B_{k}=\left[\begin{array}{c}
0_{3\times3}\\
R_{k}\frac{\Delta_{t}}{2}^{2}\\
R_{k}\Delta_{t}
\end{array}\right],\,\,\,\,C_{k}=\left[\begin{array}{c}
\end{array}\right],\,\,\,\, C_{k}=\left[\begin{array}{c}
H(\theta_{k})^{-1}\Delta_{t}\\
0_{3\times3}\\
0_{3\times3}
@@ -1391,147 +1382,6 @@ H(\theta_{k})^{-1}\Delta_{t}\\
\end_inset


\end_layout

\begin_layout Subsubsection*
Units
\end_layout

\begin_layout Standard
The units of the IMU are as follows:
\end_layout

\begin_layout Standard
\begin_inset Tabular
<lyxtabular version="3" rows="5" columns="2">
<features rotate="0" tabularvalignment="middle">
<column alignment="center" valignment="top">
<column alignment="center" valignment="top">
<row>
<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
Parameter
\end_layout

\end_inset
</cell>
<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
Units
\end_layout

\end_inset
</cell>
</row>
<row>
<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
gyro_noise_sigma
\end_layout

\end_inset
</cell>
<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
\begin_inset Formula $rad/s/\sqrt{Hz}$
\end_inset


\end_layout

\end_inset
</cell>
</row>
<row>
<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
accel_noise_sigma
\end_layout

\end_inset
</cell>
<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
\begin_inset Formula $m/s^{2}/\sqrt{Hz}$
\end_inset


\end_layout

\end_inset
</cell>
</row>
<row>
<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
gyro_bias_rw_sigma
\end_layout

\end_inset
</cell>
<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
\begin_inset Formula $rad/s$
\end_inset

or
\begin_inset Formula $rad\sqrt{Hz}/s$
\end_inset


\end_layout

\end_inset
</cell>
</row>
<row>
<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
accel_bias_rw_sigma
\end_layout

\end_inset
</cell>
<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
\begin_inset Text

\begin_layout Plain Layout
\begin_inset Formula $m/s^{2}$
\end_inset

or
\begin_inset Formula $m\sqrt{Hz}/s^{2}$
\end_inset


\end_layout

\end_inset
</cell>
</row>
</lyxtabular>

\end_inset


\end_layout

\begin_layout Standard
Binary file modified doc/ImuFactor.pdf
Binary file not shown.
4 changes: 2 additions & 2 deletions gtsam/navigation/ImuBias.h
Original file line number Diff line number Diff line change
@@ -29,8 +29,8 @@ namespace imuBias {

class GTSAM_EXPORT ConstantBias {
private:
Vector3 biasAcc_;
Vector3 biasGyro_;
Vector3 biasAcc_; ///< The units for stddev are σ = m/s² or m √Hz/s²
Vector3 biasGyro_; ///< The units for stddev are σ = rad/s or rad √Hz/s

public:
/// dimension of the variable - used to autodetect sizes
4 changes: 3 additions & 1 deletion gtsam/navigation/PreintegratedRotation.h
Original file line number Diff line number Diff line change
@@ -29,7 +29,9 @@ namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct GTSAM_EXPORT PreintegratedRotationParams {
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
/// Continuous-time "Covariance" of gyroscope measurements
/// The units for stddev are σ = rad/s/√Hz
Matrix3 gyroscopeCovariance;
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame

4 changes: 3 additions & 1 deletion gtsam/navigation/PreintegrationParams.h
Original file line number Diff line number Diff line change
@@ -24,7 +24,9 @@ namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
/// Continuous-time "Covariance" of accelerometer
/// The units for stddev are σ = m/s²/√Hz
Matrix3 accelerometerCovariance;
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 n_gravity; ///< Gravity vector in nav frame