split of measurement covariance into diagonal blocks.

gtsam.h

@@ -2417,7 +2417,6 @@ class ImuFactorPreintegratedMeasurements {
   Matrix delVdelBiasOmega() const;
   Matrix delRdelBiasOmega() const;
   Matrix preintMeasCov() const;
-  Matrix measurementCovariance() const;
 
   // Standard Interface
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
@@ -2473,7 +2472,6 @@ class CombinedImuFactorPreintegratedMeasurements {
   Matrix delVdelBiasOmega() const;
   Matrix delRdelBiasOmega() const;
   Matrix preintMeasCov() const;
-  Matrix measurementCovariance() const;
 
   // Standard Interface
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
@@ -2501,7 +2499,6 @@ class AHRSFactorPreintegratedMeasurements {
   bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
 
   // get Data
-  Matrix measurementCovariance() const;
   Matrix deltaRij() const;
   double deltaTij() const;
   Vector biasHat() const;

gtsam/navigation/AHRSFactor.cpp

@@ -29,15 +29,15 @@ namespace gtsam {
 //------------------------------------------------------------------------------
 AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
     const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
-    biasHat_(bias) {
+    PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias)
-  measurementCovariance_ << measuredOmegaCovariance;
+{
   preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
 AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
-    biasHat_(Vector3()) {
+    PreintegratedRotation(I_3x3), biasHat_(Vector3())
-  measurementCovariance_.setZero();
+{
   preintMeasCov_.setZero();
 }
 
@@ -45,7 +45,6 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
 void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
   PreintegratedRotation::print(s);
   cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
-  cout << " measurementCovariance [" << measurementCovariance_ << " ]" << endl;
   cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
 }
 
@@ -53,9 +52,7 @@ void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
 bool AHRSFactor::PreintegratedMeasurements::equals(
     const PreintegratedMeasurements& other, double tol) const {
   return PreintegratedRotation::equals(other, tol)
-      && equal_with_abs_tol(biasHat_, other.biasHat_, tol)
+      && equal_with_abs_tol(biasHat_, other.biasHat_, tol);
-      && equal_with_abs_tol(measurementCovariance_,
-          other.measurementCovariance_, tol);
 }
 
 //------------------------------------------------------------------------------
@@ -96,7 +93,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
   // first order uncertainty propagation
   // the deltaT allows to pass from continuous time noise to discrete time noise
   preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose()
-      + measurementCovariance_ * deltaT;
+      + gyroscopeCovariance() * deltaT;
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/AHRSFactor.h

@@ -42,8 +42,6 @@ public:
 
   protected:
     Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
-    Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
-
     Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
 
   public:
@@ -59,9 +57,6 @@ public:
     PreintegratedMeasurements(const Vector3& bias,
         const Matrix3& measuredOmegaCovariance);
 
-    const Matrix3& measurementCovariance() const {
-      return measurementCovariance_;
-    }
     Vector3 biasHat() const {
       return biasHat_;
     }
@@ -105,7 +100,6 @@ public:
     void serialize(ARCHIVE & ar, const unsigned int version) {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
       ar & BOOST_SERIALIZATION_NVP(biasHat_);
-      ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
     }
   };
 

gtsam/navigation/CombinedImuFactor.cpp

@@ -36,28 +36,28 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu
     const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
     const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
     const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) :
-        PreintegrationBase(bias, use2ndOrderIntegration)
+        PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
+        integrationErrorCovariance, use2ndOrderIntegration),
+        biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance),
+        biasAccOmegaInit_(biasAccOmegaInit)
 {
-  measurementCovariance_.setZero();
-  measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
-  measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
-  measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
-  measurementCovariance_.block<3,3>(9,9) = biasAccCovariance;
-  measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance;
-  measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit;
   preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
 void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{
   PreintegrationBase::print(s);
-  cout << "  measurementCovariance [ " << measurementCovariance_ << " ]" << endl;
+  cout << "  biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
+  cout << "  biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
+  cout << "  biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl;
   cout << "  preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
 }
 
 //------------------------------------------------------------------------------
 bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{
-  return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+  return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol)
+      && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol)
+      &&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
       && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
       && PreintegrationBase::equals(expected, tol);
 }
@@ -120,17 +120,17 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
   Matrix G_measCov_Gt = Matrix::Zero(15,15);
 
   // BLOCK DIAGONAL TERMS
-  G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0);
+  G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance();
   G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc)  *
-      (measurementCovariance_.block<3,3>(3,3)  +  measurementCovariance_.block<3,3>(15,15) ) *
+      (accelerometerCovariance()  +  biasAccOmegaInit_.block<3,3>(0,0) ) *
       (H_vel_biasacc.transpose());
   G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) *  (H_angles_biasomega) *
-      (measurementCovariance_.block<3,3>(6,6)  +  measurementCovariance_.block<3,3>(18,18) ) *
+      (gyroscopeCovariance()  +  biasAccOmegaInit_.block<3,3>(3,3) ) *
       (H_angles_biasomega.transpose());
-  G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * measurementCovariance_.block<3,3>(9,9);
+  G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_;
-  G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * measurementCovariance_.block<3,3>(12,12);
+  G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_;
   // OFF BLOCK DIAGONAL TERMS
-  Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) *  H_angles_biasomega.transpose();
+  Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) *  H_angles_biasomega.transpose();
   G_measCov_Gt.block<3,3>(3,6) = block23;
   G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
   preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;

gtsam/navigation/CombinedImuFactor.h

@@ -82,8 +82,9 @@ public:
 
   protected:
 
-    Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
+    Matrix3 biasAccCovariance_;   ///< continuous-time "Covariance" describing accelerometer bias random walk
-    ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
+    Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
+    Matrix6 biasAccOmegaInit_;    ///< covariance of bias used for pre-integration
 
     Eigen::Matrix<double,15,15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
     ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
@@ -130,7 +131,6 @@ public:
         boost::optional<Matrix&> F_test = boost::none, boost::optional<Matrix&> G_test = boost::none);
 
     /// methods to access class variables
-    Matrix measurementCovariance() const {return measurementCovariance_;}
     Matrix preintMeasCov() const { return preintMeasCov_;}
 
   private:
@@ -140,7 +140,6 @@ public:
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
-      ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
       ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
     }
   };

gtsam/navigation/ImuFactor.cpp

@@ -35,26 +35,22 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
     const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
     const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
     const bool use2ndOrderIntegration) :
-        PreintegrationBase(bias, use2ndOrderIntegration)
+            PreintegrationBase(bias,
+            measuredAccCovariance, measuredOmegaCovariance,
+            integrationErrorCovariance, use2ndOrderIntegration)
 {
-  measurementCovariance_.setZero();
-  measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
-  measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
-  measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
   preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
 void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
   PreintegrationBase::print(s);
-  cout << "  measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl;
   cout << "  preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl;
 }
 
 //------------------------------------------------------------------------------
 bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
-  return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+  return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
-  && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
   && PreintegrationBase::equals(expected, tol);
 }
 
@@ -94,9 +90,9 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
   // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise,
   // as G and measurementCovariance are blockdiagonal matrices
   preintMeasCov_ = F * preintMeasCov_ * F.transpose();
-  preintMeasCov_.block<3,3>(0,0) += measurementCovariance_.block<3,3>(0,0) * deltaT;
+  preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT;
-  preintMeasCov_.block<3,3>(3,3) += R_i * measurementCovariance_.block<3,3>(3,3) * R_i.transpose() * deltaT;
+  preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
-  preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * measurementCovariance_.block<3,3>(6,6) * D_Rincr_integratedOmega.transpose() * deltaT;
+  preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
 
   // F_test and G_test are given as output for testing purposes and are not needed by the factor
   if(F_test){ // This in only for testing

gtsam/navigation/ImuFactor.h

@@ -75,9 +75,6 @@ public:
 
   protected:
 
-    Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of
-    ///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
-
     Eigen::Matrix<double,9,9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
     ///< (first-order propagation from *measurementCovariance*).
 
@@ -118,7 +115,6 @@ public:
         OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
 
     /// methods to access class variables
-    Matrix measurementCovariance() const {return measurementCovariance_;}
     Matrix preintMeasCov() const { return preintMeasCov_;}
 
   private:
@@ -128,7 +124,6 @@ public:
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
-      ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
       ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
     }
   };

gtsam/navigation/PreintegratedRotation.h

@@ -37,33 +37,39 @@ class PreintegratedRotation {
 
   /// Jacobian of preintegrated rotation w.r.t. angular rate bias
   Matrix3 delRdelBiasOmega_;
+  Matrix3 gyroscopeCovariance_;     ///< continuous-time "Covariance" of gyroscope measurements
 
 public:
 
   /**
    *  Default constructor, initializes the variables in the base class
    */
-  PreintegratedRotation() :
+  PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
     deltaRij_(Rot3()), deltaTij_(0.0),
-    delRdelBiasOmega_(Z_3x3) {}
+    delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {}
 
   /// methods to access class variables
   Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive
   Vector3 thetaRij(boost::optional<Matrix3&> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive
   const double& deltaTij() const{return deltaTij_;}
   const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;}
+  const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;}
 
   /// Needed for testable
   void print(const std::string& s) const {
     std::cout << s << std::endl;
+    std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl;
     deltaRij_.print("  deltaRij ");
+    std::cout << "delRdelBiasOmega ["    << delRdelBiasOmega_ << "]" << std::endl;
+    std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl;
   }
 
   /// Needed for testable
   bool equals(const PreintegratedRotation& expected, double tol) const {
     return deltaRij_.equals(expected.deltaRij_, tol)
     && fabs(deltaTij_ - expected.deltaTij_) < tol
-    && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
+    && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol)
+    && equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol);
   }
 
   /// Re-initialize PreintegratedMeasurements

gtsam/navigation/PreintegrationBase.h

@@ -59,6 +59,10 @@ class PreintegrationBase : public PreintegratedRotation {
   Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
   Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
 
+  Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements
+  Matrix3 integrationCovariance_;   ///< continuous-time "Covariance" describing integration uncertainty
+  /// (to compensate errors in Euler integration)
+
 public:
 
   /**
@@ -67,11 +71,16 @@ public:
    *  @param use2ndOrderIntegration     Controls the order of integration
    *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
    */
-  PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) :
+  PreintegrationBase(const imuBias::ConstantBias& bias,
+      const Matrix3&  measuredAccCovariance, const Matrix3& measuredOmegaCovariance,
+      const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) :
+        PreintegratedRotation(measuredOmegaCovariance),
     biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration),
     deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
     delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
-    delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3) {}
+    delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
+    accelerometerCovariance_(measuredAccCovariance),
+    integrationCovariance_(integrationErrorCovariance) {}
 
   /// methods to access class variables
   const Vector3& deltaPij() const {return deltaPij_;}
@@ -83,9 +92,14 @@ public:
   const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;}
   const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;}
 
+  const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;}
+  const Matrix3& integrationCovariance() const { return integrationCovariance_;}
+
   /// Needed for testable
   void print(const std::string& s) const {
     PreintegratedRotation::print(s);
+    std::cout << "  accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl;
+    std::cout << "  integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl;
     std::cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
     std::cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
     biasHat_.print("  biasHat");
@@ -100,7 +114,9 @@ public:
     && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
     && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
     && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
-    && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol);
+    && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
+    && equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol)
+    && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol);
   }
 
   /// Re-initialize PreintegratedMeasurements