Merge pull request #879 from borglab/fix/combined-imu-cov

Fix Preintegration Covariance of CombinedImuFactor

doc/refs.bib

@@ -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},
-  journal =	 {Acta Numerica 2000},
-  volume =	 {9},
-  pages =	 {215--365},
-  year =	 {2000},
-  publisher =	 {Cambridge Univ Press}
-}
+	author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
+	journal = {Acta Numerica 2000},
+	pages = {215--365},
+	publisher = {Cambridge Univ Press},
+	title = {Lie-group methods},
+	volume = {9},
+	year = {2000}}
 
 @book{Murray94book,
-  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}
-}
+	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},
+	year = {1994}}
 
 @book{Spivak65book,
-  title =	 {Calculus on manifolds},
-  author =	 {Spivak, Michael},
-  volume =	 {1},
-  year =	 {1965},
-  publisher =	 {WA Benjamin New York}
-}
+	author = {Spivak, Michael},
+	publisher = {WA Benjamin New York},
+	title = {Calculus on manifolds},
+	volume = {1},
+	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}
+}

examples/CombinedImuFactorsExample.cpp

@@ -107,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);
@@ -123,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->biasAccOmegaInit = bias_acc_omega_init;
 
   return p;
 }

examples/ImuFactorsExample.cpp

@@ -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->biasAccOmegaInit = bias_acc_omega_init;
 
   return p;
 }

gtsam/base/Vector.h

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

gtsam/navigation/CombinedImuFactor.cpp

@@ -39,7 +39,7 @@ void PreintegrationCombinedParams::print(const string& s) const {
        << endl;
   cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
        << endl;
-  cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
+  cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]"
        << endl;
 }
 
@@ -52,7 +52,7 @@ bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& oth
                             tol) &&
          equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
                             tol) &&
-         equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
+         equal_with_abs_tol(biasAccOmegaInit, e->biasAccOmegaInit, tol);
 }
 
 //------------------------------------------------------------------------------
@@ -114,6 +114,10 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   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();
@@ -123,41 +127,61 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   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().biasAccOmegaInit;
 
   // first order uncertainty propagation
   // 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));
+  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) = ((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_updated
-      * (vel_H_biasAcc.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_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
-      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
-      * (theta_H_biasOmega.transpose());
+  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;
 }
 
 //------------------------------------------------------------------------------
@@ -265,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

gtsam/navigation/CombinedImuFactor.h

@@ -62,19 +62,19 @@ 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 biasAccOmegaInit;     ///< covariance of bias used as initial estimate.
 
   /// Default constructor makes uninitialized params struct.
   /// Used for serialization.
   PreintegrationCombinedParams()
       : biasAccCovariance(I_3x3),
         biasOmegaCovariance(I_3x3),
-        biasAccOmegaInt(I_6x6) {}
+        biasAccOmegaInit(I_6x6) {}
 
   /// See two named constructors below for good values of n_gravity in body frame
   PreintegrationCombinedParams(const Vector3& n_gravity) :
     PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
-    biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
+    biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {
 
   }
 
@@ -93,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 setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInit=cov; }
 
   const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
   const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
-  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
+  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInit; }
 
 private:
 
@@ -109,7 +109,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
     ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
     ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
-    ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
+    ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
   }
 
 public:

gtsam/navigation/PreintegrationBase.cpp

@@ -157,9 +157,9 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i,
       state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
                                H1 || H3 ? &D_error_predict : 0);
 
-  if (H1) *H1 << D_error_predict* D_predict_state_i;
+  if (H1) *H1 << D_error_predict * D_predict_state_i;
   if (H2) *H2 << D_error_state_j;
-  if (H3) *H3 << D_error_predict* D_predict_bias_i;
+  if (H3) *H3 << D_error_predict * D_predict_bias_i;
 
   return error;
 }

gtsam/navigation/ScenarioRunner.cpp

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