CombinedImuFactor: Add bias effect on position jacobian

doc/ImuFactor.lyx

@@ -1,24 +1,28 @@
-#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
 \language english
 \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
@@ -69,7 +86,7 @@ The new IMU Factor
 \end_layout
 
 \begin_layout Author
-Frank Dellaert
+Frank Dellaert & Varun Agrawal
 \end_layout
 
 \begin_layout Standard
@@ -244,7 +261,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
@@ -592,6 +609,7 @@ Lie Group Methods
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Iserles00an"
+literal "true"
 
 \end_inset
 
@@ -602,7 +620,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
@@ -947,8 +965,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
@@ -1015,7 +1033,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*}
 
@@ -1041,7 +1059,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*}
@@ -1096,7 +1114,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
@@ -1372,7 +1390,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}
@@ -1382,6 +1400,99 @@ H(\theta_{k})^{-1}\Delta_{t}\\
 \end_inset
 
 
+\end_layout
+
+\begin_layout Subsubsection*
+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.
+ This is accomplished via the 
+\emph on
+CombinedImuFactor
+\emph default
+ which is a 6-way factor between the previous 
+\emph on
+pose/velocity/bias
+\emph default
+ and the 
+\emph on
+pose/velocity/bias
+\emph default
+ at the next timestep.
+
+\end_layout
+
+\begin_layout Standard
+We expand the state vector as 
+\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},a_{k}^{b}, \omega_{k}^{b}]$
+\end_inset
+
+.
+ For the jacobian 
+\begin_inset Formula $F_{k}$
+\end_inset
+
+ of 
+\begin_inset Formula $f$
+\end_inset
+
+ wrpt this new 
+\begin_inset Formula $\zeta$
+\end_inset
+
+, we get a 
+\begin_inset Formula $15\times15$
+\end_inset
+
+ matrix.
+ The top-left 
+\begin_inset Formula $9\times9$
+\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}=\left[\begin{array}{ccccc}
+I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} &  &  &  & 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}\\
+R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} &  & I_{3\times3} & R_{k}\Delta_{t}\\
+ &  &  & I_{3\times3}\\
+ &  &  &  & I_{3\times3}
+\end{array}\right]
+\]
+
+\end_inset
+
+
 \end_layout
 
 \begin_layout Standard

examples/CombinedImuFactorsExample.cpp

@@ -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",
-      po::value<string>()->default_value("imuFactorExampleResults.csv"),
-      "path to the result file to use")("use_isam", po::bool_switch(),
-                                        "use ISAM as the optimizer");
+  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")  // 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);

gtsam/navigation/CombinedImuFactor.cpp

@@ -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,15 +110,16 @@ 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>();
 
   // 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;
 
@@ -124,19 +130,24 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   const Matrix3& iCov = p().integrationCovariance;
 
   // 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);
 
+  Matrix3 aCov_updated = (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0));
+
   // BLOCK DIAGONAL TERMS
-  D_t_t(&G_measCov_Gt) = dt * iCov;
+  D_t_t(&G_measCov_Gt) = ((1 / dt) * pos_H_biasAcc
+      * aCov_updated
+      * (pos_H_biasAcc.transpose())) + (dt * iCov);
   D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
-      * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
+      * aCov_updated
       * (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_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
   D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
 
@@ -145,6 +156,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
       * 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;
 }
 

gtsam/navigation/CombinedImuFactor.h

@@ -42,6 +42,8 @@ typedef ManifoldPreintegration PreintegrationType;
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
  * conditionally independent sets in factor graphs: a unifying perspective based
  * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
+ * 
+ * [3] is available in this repo as "PreintegratedIMUJacobians.pdf".
  *
  * REFERENCES:
  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",

gtsam/navigation/ImuFactor.cpp

@@ -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;
 }
 

gtsam/navigation/ImuFactor.h

@@ -44,6 +44,8 @@ typedef ManifoldPreintegration PreintegrationType;
  * 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.
+ * 
+ * [3] is available in this repo as "PreintegratedIMUJacobians.pdf".
  *
  * REFERENCES:
  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",