Cherry-picked imuFixed differences

.cproject

@@ -592,7 +592,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -600,7 +599,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -648,7 +646,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -656,7 +653,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -664,7 +660,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -680,7 +675,6 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1136,7 +1130,6 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1366,46 +1359,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFMap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testFixedVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1488,6 +1441,7 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1527,6 +1481,7 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1534,6 +1489,7 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1547,6 +1503,46 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testFixedVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1804,7 +1800,6 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1812,7 +1807,6 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1820,7 +1814,6 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1828,7 +1821,6 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2002,6 +1994,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check.navigation" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.navigation</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2683,7 +2683,6 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2691,7 +2690,6 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2699,7 +2697,6 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2809,14 +2806,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBasisDecompositions.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j4</buildArguments>
-				<buildTarget>testBasisDecompositions.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j4</buildArguments>
@@ -3251,6 +3240,7 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>

gtsam.h

@@ -2404,25 +2404,24 @@ class ConstantBias {
 }///\namespace imuBias
 
 #include <gtsam/navigation/ImuFactor.h>
-class PoseVelocity{
+class PoseVelocityBias{
-    PoseVelocity(const gtsam::Pose3& pose, Vector velocity);
+    PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
   };
 class ImuFactorPreintegratedMeasurements {
   // Standard Constructor
   ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
   ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
-  ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
+  // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
 
   // Testable
   void print(string s) const;
   bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
 
-  Matrix measurementCovariance() const;
-  Matrix deltaRij() const;
   double deltaTij() const;
+  Matrix deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
-  Vector biasHat() const;
+  Vector biasHatVector() const;
   Matrix delPdelBiasAcc() const;
   Matrix delPdelBiasOmega() const;
   Matrix delVdelBiasAcc() const;
@@ -2430,10 +2429,11 @@ class ImuFactorPreintegratedMeasurements {
   Matrix delRdelBiasOmega() const;
   Matrix preintMeasCov() const;
 
-
   // Standard Interface
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+  gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
+      Vector gravity, Vector omegaCoriolis) const;
 };
 
 virtual class ImuFactor : gtsam::NonlinearFactor {
@@ -2444,11 +2444,60 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
       const gtsam::Pose3& body_P_sensor);
   // Standard Interface
   gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
+};
-      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
+
+#include <gtsam/navigation/CombinedImuFactor.h>
+class CombinedImuFactorPreintegratedMeasurements {
+  // Standard Constructor
+  CombinedImuFactorPreintegratedMeasurements(
+      const gtsam::imuBias::ConstantBias& bias,
+      Matrix measuredAccCovariance,
+      Matrix measuredOmegaCovariance,
+      Matrix integrationErrorCovariance,
+      Matrix biasAccCovariance,
+      Matrix biasOmegaCovariance,
+      Matrix biasAccOmegaInit);
+  CombinedImuFactorPreintegratedMeasurements(
+      const gtsam::imuBias::ConstantBias& bias,
+      Matrix measuredAccCovariance,
+      Matrix measuredOmegaCovariance,
+      Matrix integrationErrorCovariance,
+      Matrix biasAccCovariance,
+      Matrix biasOmegaCovariance,
+      Matrix biasAccOmegaInit,
+      bool use2ndOrderIntegration);
+  // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
+
+  double deltaTij() const;
+  Matrix deltaRij() const;
+  Vector deltaPij() const;
+  Vector deltaVij() const;
+  Vector biasHatVector() const;
+  Matrix delPdelBiasAcc() const;
+  Matrix delPdelBiasOmega() const;
+  Matrix delVdelBiasAcc() const;
+  Matrix delVdelBiasOmega() const;
+  Matrix delRdelBiasOmega() const;
+  Matrix preintMeasCov() const;
+
+  // Standard Interface
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+  gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
       Vector gravity, Vector omegaCoriolis) const;
 };
 
+virtual class CombinedImuFactor : gtsam::NonlinearFactor {
+  CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
+      const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
+  // Standard Interface
+  gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
+};
+
 #include <gtsam/navigation/AHRSFactor.h>
 class AHRSFactorPreintegratedMeasurements {
   // Standard Constructor
@@ -2461,7 +2510,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;
@@ -2488,64 +2536,6 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
       Vector omegaCoriolis) const;
 };
 
-#include <gtsam/navigation/CombinedImuFactor.h>
-class PoseVelocityBias{
-    PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
-  };
-class CombinedImuFactorPreintegratedMeasurements {
-  // Standard Constructor
-  CombinedImuFactorPreintegratedMeasurements(
-      const gtsam::imuBias::ConstantBias& bias,
-      Matrix measuredAccCovariance,
-      Matrix measuredOmegaCovariance,
-      Matrix integrationErrorCovariance,
-      Matrix biasAccCovariance,
-      Matrix biasOmegaCovariance,
-      Matrix biasAccOmegaInit);
-  CombinedImuFactorPreintegratedMeasurements(
-      const gtsam::imuBias::ConstantBias& bias,
-      Matrix measuredAccCovariance,
-      Matrix measuredOmegaCovariance,
-      Matrix integrationErrorCovariance,
-      Matrix biasAccCovariance,
-      Matrix biasOmegaCovariance,
-      Matrix biasAccOmegaInit,
-      bool use2ndOrderIntegration);
-  CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
-
-  // Standard Interface
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
-
-  Matrix measurementCovariance() const;
-  Matrix deltaRij() const;
-  double deltaTij() const;
-  Vector deltaPij() const;
-  Vector deltaVij() const;
-  Vector biasHat() const;
-  Matrix delPdelBiasAcc() const;
-  Matrix delPdelBiasOmega() const;
-  Matrix delVdelBiasAcc() const;
-  Matrix delVdelBiasOmega() const;
-  Matrix delRdelBiasOmega() const;
-  Matrix PreintMeasCov() const;
-};
-
-virtual class CombinedImuFactor : gtsam::NonlinearFactor {
-  CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
-      const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
-
-  // Standard Interface
-  gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i,
-      const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
-      Vector gravity, Vector omegaCoriolis);
-};
-
 #include <gtsam/navigation/AttitudeFactor.h>
 //virtual class AttitudeFactor : gtsam::NonlinearFactor {
 //  AttitudeFactor(const Unit3& nZ, const Unit3& bRef);

gtsam/navigation/AHRSFactor.cpp

@@ -18,9 +18,9 @@
  **/
 
 #include <gtsam/navigation/AHRSFactor.h>
+#include <iostream>
 
-/* External or standard includes */
+using namespace std;
-#include <ostream>
 
 namespace gtsam {
 
@@ -29,47 +29,35 @@ namespace gtsam {
 //------------------------------------------------------------------------------
 AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
     const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
-    biasHat_(bias), deltaTij_(0.0) {
+    PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias)
-  measurementCovariance_ << measuredOmegaCovariance;
+{
-  delRdelBiasOmega_.setZero();
   preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
 AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
-    biasHat_(Vector3()), deltaTij_(0.0) {
+    PreintegratedRotation(I_3x3), biasHat_(Vector3())
-  measurementCovariance_.setZero();
+{
-  delRdelBiasOmega_.setZero();
-  delRdelBiasOmega_.setZero();
   preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
-void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const {
+void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
-  std::cout << s << std::endl;
+  PreintegratedRotation::print(s);
-  std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl;
+  cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
-  deltaRij_.print(" deltaRij ");
+  cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
-  std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
-      << std::endl;
-  std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl;
 }
 
 //------------------------------------------------------------------------------
 bool AHRSFactor::PreintegratedMeasurements::equals(
     const PreintegratedMeasurements& other, double tol) const {
-  return equal_with_abs_tol(biasHat_, other.biasHat_, tol)
+  return PreintegratedRotation::equals(other, tol)
-      && equal_with_abs_tol(measurementCovariance_,
+      && equal_with_abs_tol(biasHat_, other.biasHat_, tol);
-          other.measurementCovariance_, tol)
-      && deltaRij_.equals(other.deltaRij_, tol)
-      && std::fabs(deltaTij_ - other.deltaTij_) < tol
-      && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
 }
 
 //------------------------------------------------------------------------------
 void AHRSFactor::PreintegratedMeasurements::resetIntegration() {
-  deltaRij_ = Rot3();
+  PreintegratedRotation::resetIntegration();
-  deltaTij_ = 0.0;
-  delRdelBiasOmega_.setZero();
   preintMeasCov_.setZero();
 }
 
@@ -78,7 +66,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
     const Vector3& measuredOmega, double deltaT,
     boost::optional<const Pose3&> body_P_sensor) {
 
-  // NOTE: order is important here because each update uses old values.
   // First we compensate the measurements for the bias
   Vector3 correctedOmega = measuredOmega - biasHat_;
 
@@ -93,64 +80,27 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
   // rotation vector describing rotation increment computed from the
   // current rotation rate measurement
   const Vector3 theta_incr = correctedOmega * deltaT;
+  Matrix3 D_Rincr_integratedOmega;
+  const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !!
 
-  // rotation increment computed from the current rotation rate measurement
+  // Update Jacobian
-  const Rot3 incrR = Rot3::Expmap(theta_incr);
+  update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
-  const Matrix3 incrRt = incrR.transpose();
 
-  // Right Jacobian computed at theta_incr
+  // Update rotation and deltaTij.
-  const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr);
+  Matrix3 Fr; // Jacobian of the update
-
+  updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr);
-  // Update Jacobians
-  // ---------------------------------------------------------------------------
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
-
-  // Update preintegrated measurements covariance
-  // ---------------------------------------------------------------------------
-  const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3)
-  const Matrix3 Jr_theta_i = Rot3::LogmapDerivative(theta_i);
-
-  Rot3 Rot_j = deltaRij_ * incrR;
-  const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3)
-  const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
-
-  // Update preintegrated measurements covariance: as in [2] we consider a first
-  // order propagation that can be seen as a prediction phase in an EKF framework
-  Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i;
-  // analytic expression corresponding to the following numerical derivative
-  // Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>
-  // (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij);
-
-  // overall Jacobian wrpt preintegrated measurements (df/dx)
-  const Matrix3& F = H_angles_angles;
 
   // first order uncertainty propagation
   // the deltaT allows to pass from continuous time noise to discrete time noise
-  preintMeasCov_ = F * preintMeasCov_ * F.transpose()
+  preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose()
-      + measurementCovariance_ * deltaT;
+      + gyroscopeCovariance() * deltaT;
-
-  // Update preintegrated measurements
-  // ---------------------------------------------------------------------------
-  deltaRij_ = deltaRij_ * incrR;
-  deltaTij_ += deltaT;
 }
 
 //------------------------------------------------------------------------------
 Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
     boost::optional<Matrix&> H) const {
   const Vector3 biasOmegaIncr = bias - biasHat_;
-  Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
+  return biascorrectedThetaRij(biasOmegaIncr, H);
-  const Rot3 deltaRij_biascorrected = deltaRij_.retract(
-      delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
-  const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-  if (H) {
-    const Matrix3 Jrinv_theta_bc = //
-        Rot3::LogmapDerivative(theta_biascorrected);
-    const Matrix3 Jr_JbiasOmegaIncr = //
-        Rot3::ExpmapDerivative(delRdelBiasOmega_biasOmegaIncr);
-    (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
-  }
-  return theta_biascorrected;
 }
 //------------------------------------------------------------------------------
 Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
@@ -172,7 +122,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
 // AHRSFactor methods
 //------------------------------------------------------------------------------
 AHRSFactor::AHRSFactor() :
-    preintegratedMeasurements_(Vector3(), Matrix3::Zero()) {
+    _PIM_(Vector3(), Z_3x3) {
 }
 
 AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
@@ -180,7 +130,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
     const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) :
     Base(
         noiseModel::Gaussian::Covariance(
-            preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_(
+            preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_(
         preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
         body_P_sensor) {
 }
@@ -192,13 +142,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
 }
 
 //------------------------------------------------------------------------------
-void AHRSFactor::print(const std::string& s,
+void AHRSFactor::print(const string& s,
     const KeyFormatter& keyFormatter) const {
-  std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
+  cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
       << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
-  preintegratedMeasurements_.print("  preintegrated measurements:");
+  _PIM_.print("  preintegrated measurements:");
-  std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
+  cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
-      << std::endl;
   noiseModel_->print("  noise model: ");
   if (body_P_sensor_)
     body_P_sensor_->print("  sensor pose in body frame: ");
@@ -207,8 +156,7 @@ void AHRSFactor::print(const std::string& s,
 //------------------------------------------------------------------------------
 bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
   const This *e = dynamic_cast<const This*>(&other);
-  return e != NULL && Base::equals(*e, tol)
+  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
-      && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
       && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
       && ((!body_P_sensor_ && !e->body_P_sensor_)
           || (body_P_sensor_ && e->body_P_sensor_
@@ -216,50 +164,49 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
 }
 
 //------------------------------------------------------------------------------
-Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j,
+Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
     const Vector3& bias, boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const {
 
   // Do bias correction, if (H3) will contain 3*3 derivative used below
-  const Vector3 theta_biascorrected = //
+  const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);
-      preintegratedMeasurements_.predict(bias, H3);
 
   // Coriolis term
-  const Vector3 coriolis = //
+  const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_);
-      preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_);
+  const Matrix3 coriolisHat = skewSymmetric(coriolis);
-  const Vector3 theta_corrected = theta_biascorrected - coriolis;
+  const Vector3 correctedOmega = biascorrectedOmega - coriolis;
 
   // Prediction
-  const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected);
+  const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
 
   // Get error between actual and prediction
-  const Rot3 actualRij = rot_i.between(rot_j);
+  const Rot3 actualRij = Ri.between(Rj);
-  const Rot3 fRhat = deltaRij_corrected.between(actualRij);
+  const Rot3 fRrot = correctedDeltaRij.between(actualRij);
-  Vector3 fR = Rot3::Logmap(fRhat);
+  Vector3 fR = Rot3::Logmap(fRrot);
 
   // Terms common to derivatives
-  const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_corrected);
+  const Matrix3 D_cDeltaRij_cOmega = Rot3::ExpmapDerivative(correctedOmega);
-  const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(fR);
+  const Matrix3 D_fR_fRrot = Rot3::LogmapDerivative(fR);
 
   if (H1) {
     // dfR/dRi
     H1->resize(3, 3);
-    Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis);
+    Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat;
     (*H1)
-        << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta);
+        << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis);
   }
 
   if (H2) {
     // dfR/dPosej
     H2->resize(3, 3);
-    (*H2) << Jrinv_fRhat * Matrix3::Identity();
+    (*H2) << D_fR_fRrot * Matrix3::Identity();
   }
 
   if (H3) {
     // dfR/dBias, note H3 contains derivative of predict
-    const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3);
+    const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3);
     H3->resize(3, 3);
-    (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega);
+    (*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega);
   }
 
   Vector error(3);
@@ -272,16 +219,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
     const PreintegratedMeasurements preintegratedMeasurements,
     const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) {
 
-  const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias);
+  const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
 
   // Coriolis term
   const Vector3 coriolis = //
       preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis);
 
-  const Vector3 theta_corrected = theta_biascorrected - coriolis;
+  const Vector3 correctedOmega = biascorrectedOmega - coriolis;
-  const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected);
+  const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
 
-  return rot_i.compose(deltaRij_corrected);
+  return rot_i.compose(correctedDeltaRij);
 }
 
 } //namespace gtsam

gtsam/navigation/AHRSFactor.h

@@ -20,6 +20,7 @@
 #pragma once
 
 /* GTSAM includes */
+#include <gtsam/navigation/PreintegratedRotation.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/Pose3.h>
 
@@ -35,17 +36,12 @@ public:
    * Can be built incrementally so as to avoid costly integration at time of
    * factor construction.
    */
-  class PreintegratedMeasurements {
+  class PreintegratedMeasurements : public PreintegratedRotation {
 
     friend class AHRSFactor;
 
   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)
-
-    Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
-    double deltaTij_; ///< Time interval from i to j
-    Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
     Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
 
   public:
@@ -61,31 +57,19 @@ public:
     PreintegratedMeasurements(const Vector3& bias,
         const Matrix3& measuredOmegaCovariance);
 
+    Vector3 biasHat() const {
+      return biasHat_;
+    }
+    const Matrix3& preintMeasCov() const {
+      return preintMeasCov_;
+    }
+
     /// print
     void print(const std::string& s = "Preintegrated Measurements: ") const;
 
     /// equals
     bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const;
 
-    const Matrix3& measurementCovariance() const {
-      return measurementCovariance_;
-    }
-    Matrix3 deltaRij() const {
-      return deltaRij_.matrix();
-    }
-    double deltaTij() const {
-      return deltaTij_;
-    }
-    Vector3 biasHat() const {
-      return biasHat_;
-    }
-    const Matrix3& delRdelBiasOmega() const {
-      return delRdelBiasOmega_;
-    }
-    const Matrix3& preintMeasCov() const {
-      return preintMeasCov_;
-    }
-
     /// TODO: Document
     void resetIntegration();
 
@@ -103,12 +87,6 @@ public:
     Vector3 predict(const Vector3& bias, boost::optional<Matrix&> H =
         boost::none) const;
 
-    /// Integrate coriolis correction in body frame rot_i
-    Vector3 integrateCoriolis(const Rot3& rot_i,
-        const Vector3& omegaCoriolis) const {
-      return rot_i.transpose() * omegaCoriolis * deltaTij_;
-    }
-
     // This function is only used for test purposes
     // (compare numerical derivatives wrt analytic ones)
     static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
@@ -120,11 +98,8 @@ public:
     friend class boost::serialization::access;
     template<class ARCHIVE>
     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_);
-      ar & BOOST_SERIALIZATION_NVP(deltaRij_);
-      ar & BOOST_SERIALIZATION_NVP(deltaTij_);
-      ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
     }
   };
 
@@ -132,7 +107,7 @@ private:
   typedef AHRSFactor This;
   typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
 
-  PreintegratedMeasurements preintegratedMeasurements_;
+  PreintegratedMeasurements _PIM_;
   Vector3 gravity_;
   Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
   boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
@@ -178,7 +153,7 @@ public:
 
   /// Access the preintegrated measurements.
   const PreintegratedMeasurements& preintegratedMeasurements() const {
-    return preintegratedMeasurements_;
+    return _PIM_;
   }
 
   const Vector3& omegaCoriolis() const {
@@ -208,7 +183,7 @@ private:
     ar
         & boost::serialization::make_nvp("NoiseModelFactor3",
             boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
+    ar & BOOST_SERIALIZATION_NVP(_PIM_);
     ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }

gtsam/navigation/CombinedImuFactor.cpp

@@ -36,197 +36,136 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu
     const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
     const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
     const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) :
-            biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
+        PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
-            deltaRij_(Rot3()), deltaTij_(0.0),
+        integrationErrorCovariance, use2ndOrderIntegration),
-            delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
+        biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance),
-            delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
+        biasAccOmegaInit_(biasAccOmegaInit)
-            delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
 {
-  measurementCovariance_.setZero();
+  preintMeasCov_.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{
-  cout << s << endl;
+  PreintegrationBase::print(s);
-  biasHat_.print("  biasHat");
+  cout << "  biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
-  cout << "  deltaTij " << deltaTij_ << endl;
+  cout << "  biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
-  cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
+  cout << "  biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl;
-  cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
+  cout << "  preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
-  deltaRij_.print("  deltaRij ");
-  cout << "  measurementCovariance [ " << measurementCovariance_ << " ]" << endl;
-  cout << "  PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl;
 }
 
 //------------------------------------------------------------------------------
 bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{
-  return biasHat_.equals(expected.biasHat_, tol)
+  return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol)
-  && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+      && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol)
-  && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
+      &&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
-  && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
+      && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
-  && deltaRij_.equals(expected.deltaRij_, tol)
+      && PreintegrationBase::equals(expected, tol);
-  && fabs(deltaTij_ - expected.deltaTij_) < tol
-  && 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(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
 }
 
 //------------------------------------------------------------------------------
 void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
-  deltaPij_ = Vector3::Zero();
+  PreintegrationBase::resetIntegration();
-  deltaVij_ = Vector3::Zero();
+  preintMeasCov_.setZero();
-  deltaRij_ = Rot3();
-  deltaTij_ = 0.0;
-  delPdelBiasAcc_ = Z_3x3;
-  delPdelBiasOmega_ = Z_3x3;
-  delVdelBiasAcc_ = Z_3x3;
-  delVdelBiasOmega_ = Z_3x3;
-  delRdelBiasOmega_ = Z_3x3;
-  PreintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
 void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega,
-    double deltaT, boost::optional<const Pose3&> body_P_sensor) {
+    double deltaT, boost::optional<const Pose3&> body_P_sensor,
+    boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
 
   // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
   // (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
 
-  // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
+  Vector3 correctedAcc, correctedOmega;
-  Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
-  Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
 
-  // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
+  const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-  if(body_P_sensor){
+  Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
-    Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
+  const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
-    correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
-    Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
-    correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
-    // linear acceleration vector in the body frame
-  }
-
-  const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-  const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
-  const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr
 
   // Update Jacobians
   /* ----------------------------------------------------------------------------------------------------------------------- */
-  if(!use2ndOrderIntegration_){
+  updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
-    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
-    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
-  }else{
-    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
-    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
-                                            * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
-  }
-
-  delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
-  delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
-  delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT;
 
   // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
   // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
   // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
   /* ----------------------------------------------------------------------------------------------------------------------- */
-  const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
+  const Matrix3 R_i = deltaRij(); // store this
-  const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i);
+  // Update preintegrated measurements. TODO Frank moved from end of this function !!!
-
+  Matrix9 F_9x9;
-  Rot3 Rot_j = deltaRij_ * Rincr;
+  updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
-  const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
-  const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
 
   // Single Jacobians to propagate covariance
-  Matrix3 H_pos_pos    = I_3x3;
+  Matrix3 H_vel_biasacc = - R_i * deltaT;
-  Matrix3 H_pos_vel    = I_3x3 * deltaT;
+  Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT;
-  Matrix3 H_pos_angles = Z_3x3;
-
-  Matrix3 H_vel_pos    = Z_3x3;
-  Matrix3 H_vel_vel    = I_3x3;
-  Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
-  // analytic expression corresponding to the following numerical derivative
-  // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
-  Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT;
-
-  Matrix3 H_angles_pos   = Z_3x3;
-  Matrix3 H_angles_vel    = Z_3x3;
-  Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
-  Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
-  // analytic expression corresponding to the following numerical derivative
-  // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
 
   // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix F(15,15);
-  F << H_pos_pos,    H_pos_vel,     H_pos_angles,          Z_3x3,                     Z_3x3,
+  // for documentation:
-      H_vel_pos,     H_vel_vel,     H_vel_angles,      H_vel_biasacc,              Z_3x3,
+  //  F << I_3x3,    I_3x3 * deltaT,   Z_3x3,             Z_3x3,            Z_3x3,
-      H_angles_pos,  H_angles_vel,  H_angles_angles,   Z_3x3,                         H_angles_biasomega,
+  //       Z_3x3,    I_3x3,            H_vel_angles,      H_vel_biasacc,    Z_3x3,
-      Z_3x3,         Z_3x3,         Z_3x3,             I_3x3,                         Z_3x3,
+  //       Z_3x3,    Z_3x3,            H_angles_angles,   Z_3x3,            H_angles_biasomega,
-      Z_3x3,         Z_3x3,         Z_3x3,             Z_3x3,                         I_3x3;
+  //       Z_3x3,    Z_3x3,            Z_3x3,             I_3x3,            Z_3x3,
+  //       Z_3x3,    Z_3x3,            Z_3x3,             Z_3x3,            I_3x3;
+  F.setZero();
+  F.block<9,9>(0,0)  = F_9x9;
+  F.block<6,6>(9,9)  = I_6x6;
+  F.block<3,3>(3,9)  = H_vel_biasacc;
+  F.block<3,3>(6,12) = H_angles_biasomega;
 
   // first order uncertainty propagation
   // Optimized matrix multiplication   (1/deltaT) * G * measurementCovariance * G.transpose()
-
   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);
 
+//   BLOCK DIAGONAL TERMS
+  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) * biasAccCovariance_;
-  G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9);
+  G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_;
-
+  // OFF BLOCK DIAGONAL TERMS
-  G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12);
+  Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) *  H_angles_biasomega.transpose();
-
-  // NEW OFF BLOCK DIAGONAL TERMS
-  Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) *  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;
 
-  PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
+  // F_test and G_test are used for testing purposes and are not needed by the factor
-
+  if(F_test){
-  // Update preintegrated measurements
+    F_test->resize(15,15);
-  /* ----------------------------------------------------------------------------------------------------------------------- */
+    (*F_test) << F;
-  if(!use2ndOrderIntegration_){
+  }
-    deltaPij_ += deltaVij_ * deltaT;
+  if(G_test){
-  }else{
+    G_test->resize(15,21);
-    deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+    // This is for testing & documentation
+    ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
+    (*G_test) << I_3x3 * deltaT,  Z_3x3,          Z_3x3,              Z_3x3,              Z_3x3,           Z_3x3,              Z_3x3,
+                 Z_3x3,          -H_vel_biasacc,  Z_3x3,              Z_3x3,              Z_3x3,           H_vel_biasacc,      Z_3x3,
+                 Z_3x3,           Z_3x3,         -H_angles_biasomega, Z_3x3,              Z_3x3,           Z_3x3,              H_angles_biasomega,
+                 Z_3x3,           Z_3x3,          Z_3x3,              I_3x3,              Z_3x3,           Z_3x3,              Z_3x3,
+                 Z_3x3,           Z_3x3,          Z_3x3,              Z_3x3,              I_3x3,           Z_3x3,              Z_3x3;
   }
-  deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
-  deltaRij_ = deltaRij_ * Rincr;
-  deltaTij_ += deltaT;
 }
 
 //------------------------------------------------------------------------------
 // CombinedImuFactor methods
 //------------------------------------------------------------------------------
 CombinedImuFactor::CombinedImuFactor() :
-    preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
+    ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {}
 
 //------------------------------------------------------------------------------
 CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
     const CombinedPreintegratedMeasurements& preintegratedMeasurements,
     const Vector3& gravity, const Vector3& omegaCoriolis,
     boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
-          Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
+          Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
-          preintegratedMeasurements_(preintegratedMeasurements),
+          ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
-          gravity_(gravity),
+          _PIM_(preintegratedMeasurements) {}
-          omegaCoriolis_(omegaCoriolis),
-          body_P_sensor_(body_P_sensor),
-          use2ndOrderCoriolis_(use2ndOrderCoriolis){
-}
 
 //------------------------------------------------------------------------------
 gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@@ -243,22 +182,17 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter)
       << keyFormatter(this->key4()) << ","
       << keyFormatter(this->key5()) << ","
       << keyFormatter(this->key6()) << ")\n";
-  preintegratedMeasurements_.print("  preintegrated measurements:");
+  ImuFactorBase::print("");
-  cout << "  gravity: [ " << gravity_.transpose() << " ]" << endl;
+  _PIM_.print("  preintegrated measurements:");
-  cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
   this->noiseModel_->print("  noise model: ");
-  if(this->body_P_sensor_)
-    this->body_P_sensor_->print("  sensor pose in body frame: ");
 }
 
 //------------------------------------------------------------------------------
 bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const {
   const This *e =  dynamic_cast<const This*> (&expected);
   return e != NULL && Base::equals(*e, tol)
-  && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
+  && _PIM_.equals(e->_PIM_, tol)
-  && equal_with_abs_tol(gravity_, e->gravity_, tol)
+  && ImuFactorBase::equals(*e, tol);
-  && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
-  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
 }
 
 //------------------------------------------------------------------------------
@@ -268,230 +202,69 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
     boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
     boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
 
-  const double& deltaTij = preintegratedMeasurements_.deltaTij_;
+  // if we need the jacobians
-  const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
+  if(H1 || H2 || H3 || H4 || H5 || H6){
-  const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
+    Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R)
 
-  // we give some shorter name to rotations and translations
+    // error wrt preintegrated measurements
-  const Rot3 Rot_i = pose_i.rotation();
+    Vector r_pvR(9);
-  const Rot3 Rot_j = pose_j.rotation();
+    r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
-  const Vector3 pos_i = pose_i.translation().vector();
+        gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
-  const Vector3 pos_j = pose_j.translation().vector();
+        H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR);
 
-  // We compute factor's Jacobians, according to [3]
+    // error wrt bias evolution model (random walk)
-  /* ---------------------------------------------------------------------------------------------------- */
+    Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-
-  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-      Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
-
-  const Rot3 deltaRij_biascorrected_corioliscorrected =
-      Rot3::Expmap( theta_biascorrected_corioliscorrected );
-
-  const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
-
-  const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected);
-
-  const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
-
-  const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat));
 
     if(H1) {
       H1->resize(15,6);
-
+      H1->block<9,6>(0,0) = H1_pvR;
-    Matrix3 dfPdPi;
+      // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
-    Matrix3 dfVdPi;
+      H1->block<6,6>(9,0) = Z_6x6;
-    if(use2ndOrderCoriolis_){
-      dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-      dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
     }
-    else{
-      dfPdPi = - Rot_i.matrix();
-      dfVdPi = Z_3x3;
-    }
-
-    (*H1) <<
-        // dfP/dRi
-        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
-        + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
-        // dfP/dPi
-        dfPdPi,
-        // dfV/dRi
-        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
-        + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
-        // dfV/dPi
-        dfVdPi,
-        // dfR/dRi
-        Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-        // dfR/dPi
-        Z_3x3,
-        //dBiasAcc/dPi
-        Z_3x3, Z_3x3,
-        //dBiasOmega/dPi
-        Z_3x3, Z_3x3;
-  }
-
     if(H2) {
       H2->resize(15,3);
-    (*H2) <<
+      H2->block<9,3>(0,0) = H2_pvR;
-        // dfP/dVi
+      // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
-        - I_3x3 * deltaTij
+      H2->block<6,3>(9,0) = Matrix::Zero(6,3);
-        + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
-        // dfV/dVi
-        - I_3x3
-        + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-        // dfR/dVi
-        Z_3x3,
-        //dBiasAcc/dVi
-        Z_3x3,
-        //dBiasOmega/dVi
-        Z_3x3;
     }
-
     if(H3) {
       H3->resize(15,6);
-    (*H3) <<
+      H3->block<9,6>(0,0) = H3_pvR;
-        // dfP/dPosej
+      // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
-        Z_3x3, Rot_j.matrix(),
+      H3->block<6,6>(9,0) = Z_6x6;
-        // dfV/dPosej
-        Matrix::Zero(3,6),
-        // dfR/dPosej
-        Jrinv_fRhat *  ( I_3x3 ), Z_3x3,
-        //dBiasAcc/dPosej
-        Z_3x3, Z_3x3,
-        //dBiasOmega/dPosej
-        Z_3x3, Z_3x3;
     }
-
     if(H4) {
       H4->resize(15,3);
-    (*H4) <<
+      H4->block<9,3>(0,0) = H4_pvR;
-        // dfP/dVj
+      // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
-        Z_3x3,
+      H4->block<6,3>(9,0) = Matrix::Zero(6,3);
-        // dfV/dVj
-        I_3x3,
-        // dfR/dVj
-        Z_3x3,
-        //dBiasAcc/dVj
-        Z_3x3,
-        //dBiasOmega/dVj
-        Z_3x3;
     }
-
     if(H5) {
-    const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected);
-    const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
-    const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
-
       H5->resize(15,6);
-    (*H5) <<
+      H5->block<9,6>(0,0) = H5_pvR;
-        // dfP/dBias_i
+      // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
-        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
+      H5->block<6,6>(9,0) = Hbias_i;
-        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
-        // dfV/dBias_i
-        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
-        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
-        // dfR/dBias_i
-        Matrix::Zero(3,3),
-        Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
-        //dBiasAcc/dBias_i
-        -I_3x3, Z_3x3,
-        //dBiasOmega/dBias_i
-        Z_3x3, -I_3x3;
     }
-
     if(H6) {
       H6->resize(15,6);
-    (*H6) <<
+      H6->block<9,6>(0,0) = Matrix::Zero(9,6);
-        // dfP/dBias_j
+      // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
-        Z_3x3, Z_3x3,
+      H6->block<6,6>(9,0) = Hbias_j;
-        // dfV/dBias_j
-        Z_3x3, Z_3x3,
-        // dfR/dBias_j
-        Z_3x3, Z_3x3,
-        //dBiasAcc/dBias_j
-        I_3x3, Z_3x3,
-        //dBiasOmega/dBias_j
-        Z_3x3, I_3x3;
     }
-
+    Vector r(15); r << r_pvR, fbias; // vector of size 15
-  // Evaluate residual error, according to [3]
+    return r;
-  /* ---------------------------------------------------------------------------------------------------- */
+  }
-  const Vector3 fp =
+  // else, only compute the error vector:
-      pos_j - pos_i
+  // error wrt preintegrated measurements
-      - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
+  Vector r_pvR(9);
-          + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
+  r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
-          + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
+      gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
-          - vel_i * deltaTij
+      boost::none, boost::none, boost::none, boost::none, boost::none);
-          + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+  // error wrt bias evolution model (random walk)
-          - 0.5 * gravity_ * deltaTij*deltaTij;
+  Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
-
+  // overall error
-  const Vector3 fv =
+  Vector r(15); r << r_pvR, fbias; // vector of size 15
-      vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
-          + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
-          + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
-          + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
-          - gravity_ * deltaTij;
-
-  const Vector3 fR = Rot3::Logmap(fRhat);
-
-  const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
-
-  const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
-
-  Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
-
   return r;
 }
 
-//------------------------------------------------------------------------------
-PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
-    const imuBias::ConstantBias& bias_i,
-    const CombinedPreintegratedMeasurements& preintegratedMeasurements,
-    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
-
-  const double& deltaTij = preintegratedMeasurements.deltaTij_;
-  const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
-  const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
-
-  const Rot3 Rot_i = pose_i.rotation();
-  const Vector3 pos_i = pose_i.translation().vector();
-
-  // Predict state at time j
-  /* ---------------------------------------------------------------------------------------------------- */
-  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
-      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
-      + vel_i * deltaTij
-      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-      + 0.5 * gravity * deltaTij*deltaTij;
-
-  Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
-      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
-      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-      + gravity * deltaTij);
-
-  if(use2ndOrderCoriolis){
-    pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-    vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
-  }
-
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-      Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-  const Rot3 deltaRij_biascorrected_corioliscorrected =
-      Rot3::Expmap( theta_biascorrected_corioliscorrected );
-  const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-  Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-
-  return PoseVelocityBias(pose_j, vel_j, bias_i);
-}
-
 } /// namespace gtsam

gtsam/navigation/CombinedImuFactor.h

@@ -23,7 +23,8 @@
 
 /* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/PreintegrationBase.h>
+#include <gtsam/navigation/ImuFactorBase.h>
 #include <gtsam/base/debug.h>
 
 namespace gtsam {
@@ -33,78 +34,63 @@ namespace gtsam {
  * @addtogroup SLAM
  *
  * If you are using the factor, please cite:
- * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
+ * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
- * independent sets in factor graphs: a unifying perspective based on smart factors,
+ * conditionally independent sets in factor graphs: a unifying perspective based
- * Int. Conf. on Robotics and Automation (ICRA), 2014.
+ * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
  *
- * REFERENCES:
+ ** REFERENCES:
- * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
- * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
+ *     Volume 2, 2008.
- * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
+ * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
- * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
+ *     High-Dynamic Motion in Built Environments Without Initial Conditions",
+ *     TRO, 28(1):61-76, 2012.
+ * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
+ *     Computation of the Jacobian Matrices", Tech. Report, 2013.
  */
 
 /**
- * Struct to hold all state variables of CombinedImuFactor returned by Predict function
+ * CombinedImuFactor is a 6-ways factor involving previous state (pose and
+ * velocity of the vehicle, as well as bias at previous time step), and current
+ * state (pose, velocity, bias at current time step). Following the pre-
+ * integration scheme proposed in [2], the CombinedImuFactor includes many IMU
+ * measurements, which are "summarized" using the CombinedPreintegratedMeasurements
+ * class. There are 3 main differences wrpt the ImuFactor class:
+ * 1) The factor is 6-ways, meaning that it also involves both biases (previous
+ *    and current time step).Therefore, the factor internally imposes the biases
+ *    to be slowly varying; in particular, the matrices "biasAccCovariance" and
+ *    "biasOmegaCovariance" described the random walk that models bias evolution.
+ * 2) The preintegration covariance takes into account the noise in the bias
+ *    estimate used for integration.
+ * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves
+ *    the correlation between the bias uncertainty and the preintegrated
+ *    measurements uncertainty.
  */
-struct PoseVelocityBias {
+class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuFactorBase{
-  Pose3 pose;
-  Vector3 velocity;
-  imuBias::ConstantBias bias;
-
-  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
-      const imuBias::ConstantBias _bias) :
-        pose(_pose), velocity(_velocity), bias(_bias) {
-  }
-};
-
-/**
- * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias
- * at previous time step), and current state (pose, velocity, bias at current time step). According to the
- * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are
- * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor:
- * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step).
- * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices
- * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution.
- * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration.
- * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty
- * and the preintegrated measurements uncertainty.
- */
-class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
 public:
 
-  /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
+  /**
-   * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor).
+   * CombinedPreintegratedMeasurements integrates the IMU measurements
-   * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received
+   * (rotation rates and accelerations) and the corresponding covariance matrix.
-   * from the IMU) so as to avoid costly integration at time of factor construction.
+   * The measurements are then used to build the CombinedImuFactor. Integration
+   * is done incrementally (ideally, one integrates the measurement as soon as
+   * it is received from the IMU) so as to avoid costly integration at time of
+   * factor construction.
    */
-  class CombinedPreintegratedMeasurements {
+  class CombinedPreintegratedMeasurements: public PreintegrationBase {
 
     friend class CombinedImuFactor;
 
   protected:
-    imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
-    Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
-    ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
 
-    Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+    Matrix3 biasAccCovariance_;   ///< continuous-time "Covariance" describing accelerometer bias random walk
-    Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
+    Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
-    Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
+    Matrix6 biasAccOmegaInit_;    ///< covariance of bias used for pre-integration
-    double deltaTij_;  ///< Time interval from i to j
 
-    Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
+    Eigen::Matrix<double,15,15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
-    Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
-    Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
-    Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
-    Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
-
-    Eigen::Matrix<double,15,15> PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements
     ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
     ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
     ///< between the preintegrated measurements and the biases
 
-    bool use2ndOrderIntegration_; ///< Controls the order of integration
-
   public:
 
     /**
@@ -141,60 +127,20 @@ public:
      * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
      */
     void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-        boost::optional<const Pose3&> body_P_sensor = boost::none);
+        boost::optional<const Pose3&> body_P_sensor = boost::none,
+        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_;}
-    Matrix deltaRij() const {return deltaRij_.matrix();}
-    double deltaTij() const{return deltaTij_;}
-    Vector deltaPij() const {return deltaPij_;}
-    Vector deltaVij() const {return deltaVij_;}
-    Vector biasHat() const { return biasHat_.vector();}
-    Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
-    Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
-    Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
-    Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
-    Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
-    Matrix PreintMeasCov() const { return PreintMeasCov_;}
-
-    /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-    // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-    static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-        const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-      // Note: all delta terms refer to an IMU\sensor system at t0
-      Vector body_t_a_body = msr_acc_t;
-      Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-      return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
-    }
-
-    // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-    static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-        const Vector3& delta_angles){
-      // Note: all delta terms refer to an IMU\sensor system at t0
-      // Calculate the corrected measurements using the Bias object
-      Vector body_t_omega_body= msr_gyro_t;
-      Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-      R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-      return Rot3::Logmap(R_t_to_t0);
-    }
-    /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
 
   private:
-    /** Serialization function */
+
+    /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & BOOST_SERIALIZATION_NVP(biasHat_);
+      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
-      ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
+      ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
-      ar & BOOST_SERIALIZATION_NVP(deltaPij_);
-      ar & BOOST_SERIALIZATION_NVP(deltaVij_);
-      ar & BOOST_SERIALIZATION_NVP(deltaRij_);
-      ar & BOOST_SERIALIZATION_NVP(deltaTij_);
-      ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
-      ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
-      ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
-      ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
-      ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
     }
   };
 
@@ -203,12 +149,7 @@ private:
   typedef CombinedImuFactor This;
   typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
 
-  CombinedPreintegratedMeasurements preintegratedMeasurements_;
+  CombinedPreintegratedMeasurements _PIM_;
-  Vector3 gravity_;
-  Vector3 omegaCoriolis_;
-  boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
-
-  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 
 public:
 
@@ -257,11 +198,7 @@ public:
   /** Access the preintegrated measurements. */
 
   const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
-    return preintegratedMeasurements_; }
+    return _PIM_; }
-
-  const Vector3& gravity() const { return gravity_; }
-
-  const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
 
   /** implement functions needed to derive from Factor */
 
@@ -275,12 +212,6 @@ public:
       boost::optional<Matrix&> H5 = boost::none,
       boost::optional<Matrix&> H6 = boost::none) const;
 
-  /// predicted states from IMU
-  static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias_i,
-      const CombinedPreintegratedMeasurements& preintegratedMeasurements,
-      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
-
 private:
 
   /** Serialization function */
@@ -289,7 +220,7 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & boost::serialization::make_nvp("NoiseModelFactor6",
         boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
+    ar & BOOST_SERIALIZATION_NVP(_PIM_);
     ar & BOOST_SERIALIZATION_NVP(gravity_);
     ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);

gtsam/navigation/ImuFactor.cpp

@@ -35,165 +35,82 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
     const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
     const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
     const bool use2ndOrderIntegration) :
-                  biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
+            PreintegrationBase(bias,
-                  deltaRij_(Rot3()), deltaTij_(0.0),
+            measuredAccCovariance, measuredOmegaCovariance,
-                  delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
+            integrationErrorCovariance, use2ndOrderIntegration)
-                  delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
-                  delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
 {
-  measurementCovariance_.setZero();
+  preintMeasCov_.setZero();
-  measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
-  measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
-  measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
-  PreintMeasCov_.setZero(9,9);
 }
 
 //------------------------------------------------------------------------------
 void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
-  cout << s << endl;
+  PreintegrationBase::print(s);
-  biasHat_.print("  biasHat");
+  cout << "  preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl;
-  cout << "  deltaTij " << deltaTij_ << endl;
-  cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
-  cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
-  deltaRij_.print("  deltaRij ");
-  cout << "  measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl;
-  cout << "  PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl;
 }
 
 //------------------------------------------------------------------------------
 bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
-  return biasHat_.equals(expected.biasHat_, tol)
+  return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
-  && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+  && PreintegrationBase::equals(expected, tol);
-  && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
-  && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
-  && deltaRij_.equals(expected.deltaRij_, tol)
-  && fabs(deltaTij_ - expected.deltaTij_) < tol
-  && 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(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
 }
 
 //------------------------------------------------------------------------------
 void ImuFactor::PreintegratedMeasurements::resetIntegration(){
-  deltaPij_ = Vector3::Zero();
+  PreintegrationBase::resetIntegration();
-  deltaVij_ = Vector3::Zero();
+  preintMeasCov_.setZero();
-  deltaRij_ = Rot3();
-  deltaTij_ = 0.0;
-  delPdelBiasAcc_ = Z_3x3;
-  delPdelBiasOmega_ = Z_3x3;
-  delVdelBiasAcc_ = Z_3x3;
-  delVdelBiasOmega_ = Z_3x3;
-  delRdelBiasOmega_ = Z_3x3;
-  PreintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
 void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-    boost::optional<const Pose3&> body_P_sensor) {
+    boost::optional<const Pose3&> body_P_sensor,
+    OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
 
-  // NOTE: order is important here because each update uses old values (i.e., we have to update
+  Vector3 correctedAcc, correctedOmega;
-  // jacobians and covariances before updating preintegrated measurements).
+  correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
 
-  // First we compensate the measurements for the bias
+  const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-  Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
-  Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+  const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
-
-  // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
-  if(body_P_sensor){
-    Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
-    correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
-    Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
-    correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
-    // linear acceleration vector in the body frame
-  }
-
-  const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-  const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
-
-  const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr
 
   // Update Jacobians
-  /* ----------------------------------------------------------------------------------------------------------------------- */
+  updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
-  if(!use2ndOrderIntegration_){
-    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
-    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
-  }else{
-    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
-    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
-                                            * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
-  }
-  delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
-  delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
-  delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT;
 
-  // Update preintegrated measurements covariance
+  // Update preintegrated measurements (also get Jacobian)
+  const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
+  Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
+  updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
+
+  // first order covariance propagation:
   // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
   /* ----------------------------------------------------------------------------------------------------------------------- */
-  const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
+  // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
-  const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i);
+  // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
-
-  Rot3 Rot_j = deltaRij_ * Rincr;
-  const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
-  const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
-
-  Matrix H_pos_pos    = I_3x3;
-  Matrix H_pos_vel    = I_3x3 * deltaT;
-  Matrix H_pos_angles = Z_3x3;
-
-  Matrix H_vel_pos    = Z_3x3;
-  Matrix H_vel_vel    = I_3x3;
-  Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
-  // analytic expression corresponding to the following numerical derivative
-  // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
-
-  Matrix H_angles_pos   = Z_3x3;
-  Matrix H_angles_vel    = Z_3x3;
-  Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
-  // analytic expression corresponding to the following numerical derivative
-  // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
-
-  // overall Jacobian wrt preintegrated measurements (df/dx)
-  Matrix F(9,9);
-  F << H_pos_pos, H_pos_vel,  H_pos_angles,
-      H_vel_pos, H_vel_vel, H_vel_angles,
-      H_angles_pos, H_angles_vel, H_angles_angles;
-
-  // first order uncertainty propagation:
-  // the deltaT allows to pass from continuous time noise to discrete time noise
   // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
-  // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
+  // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise,
-  PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
+  // as G and measurementCovariance are blockdiagonal matrices
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose();
+  preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT;
+  preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
+  preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
 
-  // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
+  // F_test and G_test are given as output for testing purposes and are not needed by the factor
-  // This in only kept for documentation.
+  if(F_test){ // This in only for testing
-  //
+    (*F_test) << F;
-  // Matrix G(9,9);
+  }
-  // G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
+  if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
-  //      Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
+    //           intNoise         accNoise      omegaNoise
-  //      Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
+    (*G_test) << I_3x3 * deltaT,   Z_3x3,        Z_3x3,                                 // pos
-  //
+                 Z_3x3,            R_i * deltaT, Z_3x3,                                 // vel
-  // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
+                 Z_3x3,            Z_3x3,        D_Rincr_integratedOmega * deltaT;      // angle
-
-  // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
-  /* ----------------------------------------------------------------------------------------------------------------------- */
-  if(!use2ndOrderIntegration_){
-    deltaPij_ += deltaVij_ * deltaT;
-  }else{
-    deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
   }
-  deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
-  deltaRij_ = deltaRij_ * Rincr;
-  deltaTij_ += deltaT;
 }
 
 //------------------------------------------------------------------------------
 // ImuFactor methods
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor() :
-    preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
+    ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
 
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor(
@@ -202,13 +119,10 @@ ImuFactor::ImuFactor(
     const Vector3& gravity, const Vector3& omegaCoriolis,
     boost::optional<const Pose3&> body_P_sensor,
     const bool use2ndOrderCoriolis) :
-        Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
+        Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_),
-        preintegratedMeasurements_(preintegratedMeasurements),
+            pose_i, vel_i, pose_j, vel_j, bias),
-        gravity_(gravity),
+        ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
-        omegaCoriolis_(omegaCoriolis),
+        _PIM_(preintegratedMeasurements) {}
-        body_P_sensor_(body_P_sensor),
-        use2ndOrderCoriolis_(use2ndOrderCoriolis){
-}
 
 //------------------------------------------------------------------------------
 gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
@@ -224,215 +138,28 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
       << keyFormatter(this->key3()) << ","
       << keyFormatter(this->key4()) << ","
       << keyFormatter(this->key5()) << ")\n";
-  preintegratedMeasurements_.print("  preintegrated measurements:");
+  ImuFactorBase::print("");
-  cout << "  gravity: [ " << gravity_.transpose() << " ]" << endl;
+  _PIM_.print("  preintegrated measurements:");
-  cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
   this->noiseModel_->print("  noise model: ");
-  if(this->body_P_sensor_)
-    this->body_P_sensor_->print("  sensor pose in body frame: ");
 }
 
 //------------------------------------------------------------------------------
 bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
   const This *e =  dynamic_cast<const This*> (&expected);
   return e != NULL && Base::equals(*e, tol)
-  && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
+  && _PIM_.equals(e->_PIM_, tol)
-  && equal_with_abs_tol(gravity_, e->gravity_, tol)
+  && ImuFactorBase::equals(*e, tol);
-  && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
-  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
 }
 
 //------------------------------------------------------------------------------
-Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
+Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
-    const imuBias::ConstantBias& bias,
+    const Pose3& pose_j, const Vector3& vel_j,
-    boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2,
+    const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H3,  boost::optional<Matrix&> H4,
+    boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
-    boost::optional<Matrix&> H5) const
+    boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
-{
 
-  const double& deltaTij = preintegratedMeasurements_.deltaTij_;
+  return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
-  const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
+      gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5);
-  const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
-
-  // we give some shorter name to rotations and translations
-  const Rot3 Rot_i = pose_i.rotation();
-  const Rot3 Rot_j = pose_j.rotation();
-  const Vector3 pos_i = pose_i.translation().vector();
-  const Vector3 pos_j = pose_j.translation().vector();
-
-  // We compute factor's Jacobians
-  /* ---------------------------------------------------------------------------------------------------- */
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-
-  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-      Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
-
-  const Rot3 deltaRij_biascorrected_corioliscorrected =
-      Rot3::Expmap( theta_biascorrected_corioliscorrected );
-
-  const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
-
-  const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected);
-
-  const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
-
-  const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat));
-
-  if(H1) {
-    H1->resize(9,6);
-
-    Matrix3 dfPdPi;
-    Matrix3 dfVdPi;
-    if(use2ndOrderCoriolis_){
-      dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-      dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
-    }
-    else{
-      dfPdPi = - Rot_i.matrix();
-      dfVdPi = Z_3x3;
-    }
-
-    (*H1) <<
-        // dfP/dRi
-        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
-        + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
-        // dfP/dPi
-        dfPdPi,
-        // dfV/dRi
-        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
-        + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
-        // dfV/dPi
-        dfVdPi,
-        // dfR/dRi
-        Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-        // dfR/dPi
-        Z_3x3;
-  }
-
-  if(H2) {
-    H2->resize(9,3);
-    (*H2) <<
-        // dfP/dVi
-        - I_3x3 * deltaTij
-        + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
-        // dfV/dVi
-        - I_3x3
-        + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-        // dfR/dVi
-        Z_3x3;
-  }
-
-  if(H3) {
-    H3->resize(9,6);
-    (*H3) <<
-        // dfP/dPosej
-        Z_3x3, Rot_j.matrix(),
-        // dfV/dPosej
-        Matrix::Zero(3,6),
-        // dfR/dPosej
-        Jrinv_fRhat *  ( I_3x3 ), Z_3x3;
-  }
-
-  if(H4) {
-    H4->resize(9,3);
-    (*H4) <<
-        // dfP/dVj
-        Z_3x3,
-        // dfV/dVj
-        I_3x3,
-        // dfR/dVj
-        Z_3x3;
-  }
-
-  if(H5) {
-    const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected);
-    const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
-    const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
-
-    H5->resize(9,6);
-    (*H5) <<
-        // dfP/dBias
-        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
-        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
-        // dfV/dBias
-        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
-        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
-        // dfR/dBias
-        Matrix::Zero(3,3),
-        Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
-  }
-
-  // Evaluate residual error, according to [3]
-  /* ---------------------------------------------------------------------------------------------------- */
-  const Vector3 fp =
-      pos_j - pos_i
-      - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
-          + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
-          + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
-          - vel_i * deltaTij
-          + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-          - 0.5 * gravity_ * deltaTij*deltaTij;
-
-  const Vector3 fv =
-      vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
-          + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
-          + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
-          + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
-          - gravity_ * deltaTij;
-
-  const Vector3 fR = Rot3::Logmap(fRhat);
-
-  Vector r(9); r << fp, fv, fR;
-  return r;
-}
-
-//------------------------------------------------------------------------------
-PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
-    const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
-    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis)
-{
-
-  const double& deltaTij = preintegratedMeasurements.deltaTij_;
-  const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
-  const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
-
-  const Rot3 Rot_i = pose_i.rotation();
-  const Vector3 pos_i = pose_i.translation().vector();
-
-  // Predict state at time j
-  /* ---------------------------------------------------------------------------------------------------- */
-  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
-      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
-      + vel_i * deltaTij
-      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-      + 0.5 * gravity * deltaTij*deltaTij;
-
-  Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
-      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
-      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-      + gravity * deltaTij);
-
-  if(use2ndOrderCoriolis){
-    pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-    vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
-  }
-
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-      Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-  const Rot3 deltaRij_biascorrected_corioliscorrected =
-      Rot3::Expmap( theta_biascorrected_corioliscorrected );
-  const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-  Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-  return PoseVelocity(pose_j, vel_j);
 }
 
 } /// namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -23,7 +23,8 @@
 
 /* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/PreintegrationBase.h>
+#include <gtsam/navigation/ImuFactorBase.h>
 #include <gtsam/base/debug.h>
 
 namespace gtsam {
@@ -38,65 +39,45 @@ namespace gtsam {
  * Int. Conf. on Robotics and Automation (ICRA), 2014.
  *
  ** REFERENCES:
- * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
- * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
+ *     Volume 2, 2008.
- * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
+ * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
- * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
+ *     High-Dynamic Motion in Built Environments Without Initial Conditions",
+ *     TRO, 28(1):61-76, 2012.
+ * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
+ *     Computation of the Jacobian Matrices", Tech. Report, 2013.
  */
 
 /**
- * Struct to hold return variables by the Predict Function
+ * ImuFactor is a 5-ways factor involving previous state (pose and velocity of
+ * the vehicle at previous time step), current state (pose and velocity at
+ * current time step), and the bias estimate. Following the preintegration
+ * scheme proposed in [2], the ImuFactor includes many IMU measurements, which
+ * are "summarized" using the PreintegratedMeasurements class.
+ * Note that this factor does not model "temporal consistency" of the biases
+ * (which are usually slowly varying quantities), which is up to the caller.
+ * See also CombinedImuFactor for a class that does this for you.
  */
-struct PoseVelocity {
+class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase {
-  Pose3 pose;
-  Vector3 velocity;
-  PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
-    pose(_pose), velocity(_velocity) {
-  }
-};
-
-/**
- * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step),
- * current state (pose and velocity at current time step), and the bias estimate. According to the
- * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are
- * "summarized" using the PreintegratedMeasurements class.
- * Note that this factor does not force "temporal consistency" of the biases (which are usually
- * slowly varying quantities), see also CombinedImuFactor for more details.
- */
-class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
 public:
 
   /**
    * PreintegratedMeasurements accumulates (integrates) the IMU measurements
    * (rotation rates and accelerations) and the corresponding covariance matrix.
-   * The measurements are then used to build the Preintegrated IMU factor (ImuFactor).
+   * The measurements are then used to build the Preintegrated IMU factor.
-   * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received
+   * Integration is done incrementally (ideally, one integrates the measurement
-   * from the IMU) so as to avoid costly integration at time of factor construction.
+   * as soon as it is received from the IMU) so as to avoid costly integration
+   * at time of factor construction.
    */
-  class PreintegratedMeasurements {
+  class PreintegratedMeasurements: public PreintegrationBase {
 
     friend class ImuFactor;
 
   protected:
-    imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
-    Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
 
-    Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+    Eigen::Matrix<double,9,9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
-    Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
-    Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
-    double deltaTij_;  ///< Time interval from i to j
-
-    Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
-    Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
-    Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
-    Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
-    Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
-
-    Eigen::Matrix<double,9,9> PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
     ///< (first-order propagation from *measurementCovariance*).
 
-    bool use2ndOrderIntegration_; ///< Controls the order of integration
-
   public:
 
     /**
@@ -127,62 +108,23 @@ public:
      * @param measuredOmega Measured angular velocity (as given by the sensor)
      * @param deltaT Time interval between two consecutive IMU measurements
      * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
+     * @param Fout, Gout Jacobians used internally (only needed for testing)
      */
     void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-        boost::optional<const Pose3&> body_P_sensor = boost::none);
+        boost::optional<const Pose3&> body_P_sensor = boost::none,
+        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_;}
-    Matrix deltaRij() const {return deltaRij_.matrix();}
-    double deltaTij() const{return deltaTij_;}
-    Vector deltaPij() const {return deltaPij_;}
-    Vector deltaVij() const {return deltaVij_;}
-    Vector biasHat() const { return biasHat_.vector();}
-    Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
-    Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
-    Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
-    Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
-    Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
-    Matrix preintMeasCov() const { return PreintMeasCov_;}
-
-    /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-    // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-    static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-        const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-      // Note: all delta terms refer to an IMU\sensor system at t0
-      Vector body_t_a_body = msr_acc_t;
-      Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-      return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
-    }
-
-    // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-    static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-        const Vector3& delta_angles){
-      // Note: all delta terms refer to an IMU\sensor system at t0
-      // Calculate the corrected measurements using the Bias object
-      Vector body_t_omega_body= msr_gyro_t;
-      Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-      R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-      return Rot3::Logmap(R_t_to_t0);
-    }
-    /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
 
   private:
-      /** Serialization function */
+
+    /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
-        ar & BOOST_SERIALIZATION_NVP(biasHat_);
+      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
-        ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
+      ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
-        ar & BOOST_SERIALIZATION_NVP(deltaPij_);
-        ar & BOOST_SERIALIZATION_NVP(deltaVij_);
-        ar & BOOST_SERIALIZATION_NVP(deltaRij_);
-        ar & BOOST_SERIALIZATION_NVP(deltaTij_);
-        ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
-        ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
-        ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
-        ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
-        ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
     }
   };
 
@@ -191,12 +133,7 @@ public:
   typedef ImuFactor This;
   typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
 
-    PreintegratedMeasurements preintegratedMeasurements_;
+  PreintegratedMeasurements _PIM_;
-    Vector3 gravity_;
-    Vector3 omegaCoriolis_;
-    boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
-
-    bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 
   public:
 
@@ -244,11 +181,7 @@ public:
   /** Access the preintegrated measurements. */
 
   const PreintegratedMeasurements& preintegratedMeasurements() const {
-      return preintegratedMeasurements_; }
+    return _PIM_; }
-
-    const Vector3& gravity() const { return gravity_; }
-
-    const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
 
   /** implement functions needed to derive from Factor */
 
@@ -261,11 +194,6 @@ public:
       boost::optional<Matrix&> H4 = boost::none,
       boost::optional<Matrix&> H5 = boost::none) const;
 
-    /// predicted states from IMU
-    static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
-        const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
-        const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
-
   private:
 
   /** Serialization function */
@@ -274,13 +202,13 @@ public:
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & boost::serialization::make_nvp("NoiseModelFactor5",
         boost::serialization::base_object<Base>(*this));
-      ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
+    ar & BOOST_SERIALIZATION_NVP(_PIM_);
     ar & BOOST_SERIALIZATION_NVP(gravity_);
     ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
-  }; // class ImuFactor
+}; // class ImuFactor
 
-  typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
+typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
 
 } /// namespace gtsam

gtsam/navigation/ImuFactorBase.h

@@ -0,0 +1,84 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  PreintegrationBase.h
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+/* GTSAM includes */
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/PreintegrationBase.h>
+
+namespace gtsam {
+
+class ImuFactorBase {
+
+protected:
+
+  Vector3 gravity_;
+  Vector3 omegaCoriolis_;
+  boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
+  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
+
+public:
+
+  /** Default constructor - only use for serialization */
+  ImuFactorBase() :
+    gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
+    body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
+
+  /**
+   *  Default constructor, stores basic quantities required by the Imu factors
+   * @param gravity Gravity vector expressed in the global frame
+   * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
+   * @param body_P_sensor Optional pose of the sensor frame in the body frame
+   * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
+   */
+  ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis,
+      boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
+        gravity_(gravity), omegaCoriolis_(omegaCoriolis),
+        body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
+
+  /// Methods to access class variables
+  const Vector3& gravity() const { return gravity_; }
+  const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
+
+  /// Needed for testable
+  //------------------------------------------------------------------------------
+  void print(const std::string& s) const {
+    std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
+    std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
+    std::cout << "  use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl;
+    if(this->body_P_sensor_)
+      this->body_P_sensor_->print("  sensor pose in body frame: ");
+  }
+
+  /// Needed for testable
+  //------------------------------------------------------------------------------
+  bool equals(const ImuFactorBase& expected, double tol) const {
+    return equal_with_abs_tol(gravity_, expected.gravity_, tol)
+    && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol)
+    && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_)
+    && ((!body_P_sensor_ && !expected.body_P_sensor_) ||
+        (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_)));
+  }
+
+};
+
+} /// namespace gtsam

gtsam/navigation/PreintegratedRotation.h

@@ -0,0 +1,141 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  PreintegratedRotation.h
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+#include <gtsam/geometry/Rot3.h>
+
+namespace gtsam {
+
+/**
+ * PreintegratedRotation is the base class for all PreintegratedMeasurements
+ * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
+ * It includes the definitions of the preintegrated rotation.
+ */
+class PreintegratedRotation {
+
+  Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
+  double deltaTij_;  ///< Time interval from i to j
+
+  /// 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(const Matrix3& measuredOmegaCovariance) :
+    deltaRij_(Rot3()), deltaTij_(0.0),
+    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(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol);
+  }
+
+  /// Re-initialize PreintegratedMeasurements
+  void resetIntegration(){
+    deltaRij_ = Rot3();
+    deltaTij_ = 0.0;
+    delRdelBiasOmega_ = Z_3x3;
+  }
+
+  /// Update preintegrated measurements
+  void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
+      OptionalJacobian<3, 3> H = boost::none){
+    deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
+    deltaTij_ += deltaT;
+  }
+
+  /**
+   *  Update Jacobians to be used during preintegration
+   *  TODO: explain arguments
+   */
+  void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
+      double deltaT) {
+    const Matrix3 incrRt = incrR.transpose();
+    delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT;
+  }
+
+  /// Return a bias corrected version of the integrated rotation - expensive
+  Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const {
+    return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr);
+  }
+
+  /// Get so<3> version of bias corrected rotation, with optional Jacobian
+  // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
+  Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
+      OptionalJacobian<3, 3> H = boost::none) const {
+    // First, we correct deltaRij using the biasOmegaIncr, rotated
+    const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
+
+    if (H) {
+      Matrix3 Jrinv_theta_bc;
+      // This was done via an expmap, now we go *back* to so<3>
+      const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc);
+      const Matrix3 Jr_JbiasOmegaIncr = //
+          Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
+      (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
+      return biascorrectedOmega;
+    }
+    //else
+    return Rot3::Logmap(deltaRij_biascorrected);
+  }
+
+  /// Integrate coriolis correction in body frame rot_i
+  Vector3 integrateCoriolis(const Rot3& rot_i,
+      const Vector3& omegaCoriolis) const {
+    return rot_i.transpose() * omegaCoriolis * deltaTij();
+  }
+
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(deltaRij_);
+    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
+    ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
+  }
+};
+
+} /// namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -0,0 +1,425 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  PreintegrationBase.h
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+#include <gtsam/navigation/PreintegratedRotation.h>
+#include <gtsam/navigation/ImuBias.h>
+
+namespace gtsam {
+
+/**
+ * Struct to hold all state variables of returned by Predict function
+ */
+struct PoseVelocityBias {
+  Pose3 pose;
+  Vector3 velocity;
+  imuBias::ConstantBias bias;
+
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
+      const imuBias::ConstantBias _bias) :
+        pose(_pose), velocity(_velocity), bias(_bias) {
+  }
+};
+
+/**
+ * PreintegrationBase is the base class for PreintegratedMeasurements
+ * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor).
+ * It includes the definitions of the preintegrated variables and the methods
+ * to access, print, and compare them.
+ */
+class PreintegrationBase : public PreintegratedRotation {
+
+  imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
+  bool use2ndOrderIntegration_; ///< Controls the order of integration
+
+  Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+  Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
+
+  Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
+  Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
+  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:
+
+  /**
+   *  Default constructor, initializes the variables in the base class
+   *  @param bias Current estimate of acceleration and rotation rate biases
+   *  @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 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),
+    accelerometerCovariance_(measuredAccCovariance),
+    integrationCovariance_(integrationErrorCovariance) {}
+
+  /// methods to access class variables
+  const Vector3& deltaPij() const {return deltaPij_;}
+  const Vector3& deltaVij() const {return deltaVij_;}
+  const imuBias::ConstantBias& biasHat() const { return biasHat_;}
+  Vector6 biasHatVector() const { return biasHat_.vector();} // expensive
+  const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;}
+  const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;}
+  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");
+  }
+
+  /// Needed for testable
+  bool equals(const PreintegrationBase& expected, double tol) const {
+    return PreintegratedRotation::equals(expected, tol)
+    && biasHat_.equals(expected.biasHat_, tol)
+    && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
+    && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
+    && 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(accelerometerCovariance_, expected.accelerometerCovariance_, tol)
+    && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol);
+  }
+
+  /// Re-initialize PreintegratedMeasurements
+  void resetIntegration(){
+    PreintegratedRotation::resetIntegration();
+    deltaPij_ = Vector3::Zero();
+    deltaVij_ = Vector3::Zero();
+    delPdelBiasAcc_ = Z_3x3;
+    delPdelBiasOmega_ = Z_3x3;
+    delVdelBiasAcc_ = Z_3x3;
+    delVdelBiasOmega_ = Z_3x3;
+  }
+
+  /// Update preintegrated measurements
+  void updatePreintegratedMeasurements(const Vector3& correctedAcc,
+      const Rot3& incrR, const double deltaT,  OptionalJacobian<9, 9> F) {
+
+    Matrix3 dRij = deltaRij(); // expensive
+    Vector3 temp = dRij * correctedAcc * deltaT;
+    if(!use2ndOrderIntegration_){
+      deltaPij_ += deltaVij_ * deltaT;
+    }else{
+      deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
+    }
+    deltaVij_ += temp;
+
+    Matrix3 R_i, F_angles_angles;
+    if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
+    updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0);
+
+    if(F){
+      Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
+      //    pos          vel              angle
+      *F << I_3x3,       I_3x3 * deltaT,  Z_3x3,          // pos
+            Z_3x3,       I_3x3,           F_vel_angles,   // vel
+            Z_3x3,       Z_3x3,           F_angles_angles;// angle
+    }
+  }
+
+  /// Update Jacobians to be used during preintegration
+  void updatePreintegratedJacobians(const Vector3& correctedAcc,
+      const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){
+    Matrix3 dRij = deltaRij(); // expensive
+    Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
+    if (!use2ndOrderIntegration_) {
+      delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
+      delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
+    } else {
+      delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
+      delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5);
+    }
+    delVdelBiasAcc_ += -dRij * deltaT;
+    delVdelBiasOmega_ += temp;
+    update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT);
+  }
+
+  void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
+      const Vector3& measuredOmega, Vector3& correctedAcc,
+      Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
+    correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+    correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+
+    // Then compensate for sensor-body displacement: we express the quantities
+    // (originally in the IMU frame) into the body frame
+    if(body_P_sensor){
+      Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
+      correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
+      Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
+      correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
+      // linear acceleration vector in the body frame
+    }
+  }
+
+  /// Predict state at time j
+  //------------------------------------------------------------------------------
+  PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
+      const imuBias::ConstantBias& bias_i, const Vector3& gravity,
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
+      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
+
+    const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
+    const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
+
+    const Rot3& Rot_i = pose_i.rotation();
+    const Vector3& pos_i = pose_i.translation().vector();
+
+    // Predict state at time j
+    /* ---------------------------------------------------------------------------------------------------- */
+    Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr;
+    if(deltaPij_biascorrected_out)// if desired, store this
+      *deltaPij_biascorrected_out = deltaPij_biascorrected;
+
+    Vector3 pos_j =  pos_i + Rot_i.matrix() * deltaPij_biascorrected
+        + vel_i * deltaTij()
+        - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij()  // Coriolis term - we got rid of the 2 wrt ins paper
+        + 0.5 * gravity * deltaTij()*deltaTij();
+
+    Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr;
+    if(deltaVij_biascorrected_out)// if desired, store this
+      *deltaVij_biascorrected_out = deltaVij_biascorrected;
+
+    Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected
+        - 2 * omegaCoriolis.cross(vel_i) * deltaTij()  // Coriolis term
+        + gravity * deltaTij());
+
+    if(use2ndOrderCoriolis){
+      pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij();  // 2nd order coriolis term for position
+      vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
+    }
+
+    const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
+    // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
+
+    Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
+    Vector3 correctedOmega = biascorrectedOmega  -
+        Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
+    const Rot3 correctedDeltaRij =
+        Rot3::Expmap( correctedOmega );
+    const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij  );
+
+    Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
+    return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
+  }
+
+  /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
+  //------------------------------------------------------------------------------
+  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
+      const Pose3& pose_j, const Vector3& vel_j,
+      const imuBias::ConstantBias& bias_i, const Vector3& gravity,
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
+      OptionalJacobian<9, 6> H1 = boost::none,
+      OptionalJacobian<9, 3> H2 = boost::none,
+      OptionalJacobian<9, 6> H3 = boost::none,
+      OptionalJacobian<9, 3> H4 = boost::none,
+      OptionalJacobian<9, 6> H5 = boost::none) const {
+
+    // We need the mismatch w.r.t. the biases used for preintegration
+    // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary
+    const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
+
+    // we give some shorter name to rotations and translations
+    const Rot3& Ri = pose_i.rotation();
+    const Rot3& Rj = pose_j.rotation();
+    const Vector3& pos_j = pose_j.translation().vector();
+
+    // Evaluate residual error, according to [3]
+    /* ---------------------------------------------------------------------------------------------------- */
+    Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
+    PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
+        omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected);
+
+    // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
+    const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() );
+
+    // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
+    const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity );
+
+    // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
+
+    // Jacobian computation
+    /* ---------------------------------------------------------------------------------------------------- */
+    // Get Get so<3> version of bias corrected rotation
+    // If H5 is asked for, we will need the Jacobian, which we store in H5
+    // H5 will then be corrected below to take into account the Coriolis effect
+    Matrix3 D_cThetaRij_biasOmegaIncr;
+    Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0);
+
+    // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
+    const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
+    const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
+    Vector3 correctedOmega = biascorrectedOmega  - coriolis;
+
+    Rot3 correctedDeltaRij, fRrot;
+    Vector3 fR;
+
+    // Accessory matrix, used to build the jacobians
+    Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot;
+
+    // This is done to save computation: we ask for the jacobians only when they are needed
+    if(H1 || H2 || H3 || H4 || H5){
+      correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega);
+      // Residual rotation error
+      fRrot = correctedDeltaRij.between(Ri.between(Rj));
+      fR = Rot3::Logmap(fRrot, D_fR_fRrot);
+      D_coriolis = -D_cDeltaRij_cOmega  * skewSymmetric(coriolis);
+    }else{
+      correctedDeltaRij = Rot3::Expmap( correctedOmega);
+      // Residual rotation error
+      fRrot = correctedDeltaRij.between(Ri.between(Rj));
+      fR = Rot3::Logmap(fRrot);
+    }
+    if(H1) {
+      H1->resize(9,6);
+      Matrix3 dfPdPi = -I_3x3;
+      Matrix3 dfVdPi = Z_3x3;
+      if(use2ndOrderCoriolis){
+        // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
+        Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose());
+        dfPdPi += 0.5 * temp * deltaTij()*deltaTij();
+        dfVdPi += temp * deltaTij();
+      }
+      (*H1) <<
+          // dfP/dRi
+          skewSymmetric(fp + deltaPij_biascorrected),
+          // dfP/dPi
+          dfPdPi,
+          // dfV/dRi
+          skewSymmetric(fv + deltaVij_biascorrected),
+          // dfV/dPi
+          dfVdPi,
+          // dfR/dRi
+          D_fR_fRrot *  (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
+          // dfR/dPi
+          Z_3x3;
+    }
+    if(H2) {
+      H2->resize(9,3);
+      (*H2) <<
+          // dfP/dVi
+          - Ri.transpose() * deltaTij()
+          + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(),  // Coriolis term - we got rid of the 2 wrt ins paper
+          // dfV/dVi
+          - Ri.transpose()
+          + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
+          // dfR/dVi
+          Z_3x3;
+    }
+    if(H3) {
+      H3->resize(9,6);
+      (*H3) <<
+          // dfP/dPosej
+          Z_3x3, Ri.transpose() * Rj.matrix(),
+          // dfV/dPosej
+          Matrix::Zero(3,6),
+          // dfR/dPosej
+          D_fR_fRrot *  ( I_3x3 ), Z_3x3;
+    }
+    if(H4) {
+      H4->resize(9,3);
+      (*H4) <<
+          // dfP/dVj
+          Z_3x3,
+          // dfV/dVj
+          Ri.transpose(),
+          // dfR/dVj
+          Z_3x3;
+    }
+    if(H5) {
+      // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
+      const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
+      H5->resize(9,6);
+      (*H5) <<
+          // dfP/dBias
+          - delPdelBiasAcc(),   - delPdelBiasOmega(),
+          // dfV/dBias
+          - delVdelBiasAcc(),   - delVdelBiasOmega(),
+          // dfR/dBias
+          Z_3x3,               D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
+    }
+    Vector9 r; r << fp, fv, fR;
+    return r;
+  }
+
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
+    ar & BOOST_SERIALIZATION_NVP(biasHat_);
+    ar & BOOST_SERIALIZATION_NVP(deltaPij_);
+    ar & BOOST_SERIALIZATION_NVP(deltaVij_);
+    ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
+    ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
+    ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
+    ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
+  }
+};
+
+class ImuBase {
+
+protected:
+
+  Vector3 gravity_;
+  Vector3 omegaCoriolis_;
+  boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
+  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
+
+public:
+
+  ImuBase() :
+    gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
+    body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
+
+  ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
+      boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
+        gravity_(gravity), omegaCoriolis_(omegaCoriolis),
+        body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
+
+  const Vector3& gravity() const { return gravity_; }
+  const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
+
+};
+
+} /// namespace gtsam

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -116,7 +116,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
 /* ************************************************************************* */
 TEST(AHRSFactor, Error) {
   // Linearization point
-  Vector3 bias; // Bias
+  Vector3 bias(0.,0.,0.); // Bias
   Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
   Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
 

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -39,19 +39,55 @@ using symbol_shorthand::X;
 using symbol_shorthand::V;
 using symbol_shorthand::B;
 
-/* ************************************************************************* */
 namespace {
+/* ************************************************************************* */
+// Auxiliary functions to test Jacobians F and G used for
+// covariance propagation during preintegration
+/* ************************************************************************* */
+Vector updatePreintegratedMeasurementsTest(
+    const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
+    const imuBias::ConstantBias& bias_old,
+    const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
+    const bool use2ndOrderIntegration) {
 
-ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
+  Matrix3 dRij = deltaRij_old.matrix();
+  Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
+  Vector3 deltaPij_new;
+  if(!use2ndOrderIntegration){
+    deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
+  }else{
+    deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
+  }
+  Vector3 deltaVij_new = deltaVij_old + temp;
+  Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT);
+  Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
+  imuBias::ConstantBias bias_new(bias_old);
+  Vector result(15);  result << deltaPij_new,  deltaVij_new, logDeltaRij_new, bias_new.vector();
+  return result;
+}
+
+Rot3 updatePreintegratedMeasurementsRot(
+    const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
+    const imuBias::ConstantBias& bias_old,
+    const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
+    const bool use2ndOrderIntegration){
+
+  Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old,
+      bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration);
+
+  return Rot3::Expmap(result.segment<3>(6));
+}
+
+// Auxiliary functions to test preintegrated Jacobians
+// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
+/* ************************************************************************* */
+CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
     const imuBias::ConstantBias& bias,
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
-    const list<double>& deltaTs,
+    const list<double>& deltaTs){
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
+  CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(),
-    )
+      Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false);
-{
-  ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
-      Matrix3::Identity(), Matrix3::Identity());
 
   list<Vector3>::const_iterator itAcc = measuredAccs.begin();
   list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@@ -59,7 +95,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
   for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
     result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
   }
-
   return result;
 }
 
@@ -67,20 +102,16 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
     const imuBias::ConstantBias& bias,
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
-    const list<double>& deltaTs,
+    const list<double>& deltaTs){
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
-{
   return evaluatePreintegratedMeasurements(bias,
-      measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij();
+      measuredAccs, measuredOmegas, deltaTs).deltaPij();
 }
 
 Vector3 evaluatePreintegratedMeasurementsVelocity(
     const imuBias::ConstantBias& bias,
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
-    const list<double>& deltaTs,
+    const list<double>& deltaTs){
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
-{
   return evaluatePreintegratedMeasurements(bias,
       measuredAccs, measuredOmegas, deltaTs).deltaVij();
 }
@@ -89,9 +120,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
     const imuBias::ConstantBias& bias,
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
-    const list<double>& deltaTs,
+    const list<double>& deltaTs){
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
-{
   return Rot3(evaluatePreintegratedMeasurements(bias,
       measuredAccs, measuredOmegas, deltaTs).deltaRij());
 }
@@ -101,7 +130,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
 /* ************************************************************************* */
 TEST( CombinedImuFactor, PreintegratedMeasurements )
 {
-  //cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
   // Linearization point
   imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
 
@@ -120,28 +148,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
       Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
 
-//           const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
-//           const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
-//           const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
-//           const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
-//           const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
-//           const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
-//           const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
-
   actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol));
-//  EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol));
+  EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol));
-//  EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol));
+  EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol));
-//  DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol);
+  DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
 }
 
-
 /* ************************************************************************* */
 TEST( CombinedImuFactor, ErrorWithBiases )
 {
-  //cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl;
-
   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
   imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
@@ -157,18 +174,9 @@ TEST( CombinedImuFactor, ErrorWithBiases )
   double deltaT = 1.0;
   double tol = 1e-6;
 
-  //           const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
-  //           const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
-  //           const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
-  //           const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
-  //           const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
-  //           const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
-  //           const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
-
   Matrix I6x6(6,6);
   I6x6 = Matrix::Identity(6,6);
 
-
   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
       Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
 
@@ -180,26 +188,22 @@ TEST( CombinedImuFactor, ErrorWithBiases )
 
   Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-
   // Create factor
   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
 
-    noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov());
+  noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
   CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
 
-
   Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
 
   Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);
 
-
   EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
 
   // Expected Jacobians
   Matrix H1e, H2e, H3e, H4e, H5e;
   (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
 
-
   // Actual Jacobians
   Matrix H1a, H2a, H3a, H4a, H5a, H6a;
   (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
@@ -214,7 +218,6 @@ TEST( CombinedImuFactor, ErrorWithBiases )
 /* ************************************************************************* */
 TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
 {
-  //cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
   // Linearization point
   imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
 
@@ -237,22 +240,22 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
   }
 
   // Actual preintegrated values
-  ImuFactor::PreintegratedMeasurements preintegrated =
+  CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
-      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
+      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
 
   // Compute numerical derivatives
   Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
-      boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
+      boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias);
   Matrix expectedDelPdelBiasAcc   = expectedDelPdelBias.leftCols(3);
   Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
 
   Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
-      boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
+      boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias);
   Matrix expectedDelVdelBiasAcc   = expectedDelVdelBias.leftCols(3);
   Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
 
   Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
-      boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
+      boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias);
   Matrix expectedDelRdelBiasAcc   = expectedDelRdelBias.leftCols(3);
   Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
 
@@ -265,6 +268,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
   EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
+/* ************************************************************************* */
 TEST(CombinedImuFactor, PredictPositionAndVelocity){
   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
 
@@ -284,21 +288,20 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){
   for (int i = 0; i<1000; ++i)   Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
-    noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov());
+  noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
   CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
 
   // Predict
   Pose3 x1;
   Vector3 v1(0, 0.0, 0.0);
-    PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis);
+  PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
   Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
   Vector3 expectedVelocity; expectedVelocity<<0,1,0;
   EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
   EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
-
-
 }
 
+/* ************************************************************************* */
 TEST(CombinedImuFactor, PredictRotation) {
   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
   Matrix I6x6(6,6);
@@ -319,14 +322,152 @@ TEST(CombinedImuFactor, PredictRotation) {
   // Predict
   Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0));
   Vector3 v(0,0,0);
-  PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis);
+  PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis);
   Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0));
   EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol));
 }
 
-#include <gtsam/linear/GaussianFactorGraph.h>
+/* ************************************************************************* */
+TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
+{
+  // Linearization point
+  imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
+  Pose3 body_P_sensor = Pose3();
 
+  // Measurements
+  list<Vector3> measuredAccs, measuredOmegas;
+  list<double> deltaTs;
+  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
+  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
+  deltaTs.push_back(0.01);
+  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
+  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
+  deltaTs.push_back(0.01);
+  for(int i=1;i<100;i++)
+  {
+    measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
+    measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
+    deltaTs.push_back(0.01);
+  }
+  // Actual preintegrated values
+  CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
+      evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs);
+
+  // so far we only created a nontrivial linearization point for the preintegrated measurements
+  // Now we add a new measurement and ask for Jacobians
+  const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
+  const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
+  const double newDeltaT = 0.01;
+  const Rot3    deltaRij_old = preintegrated.deltaRij();    // before adding new measurement
+  const Vector3 deltaVij_old = preintegrated.deltaVij();    // before adding new measurement
+  const Vector3 deltaPij_old = preintegrated.deltaPij();    // before adding new measurement
+
+  Matrix oldPreintCovariance = preintegrated.preintMeasCov();
+
+  Matrix Factual, Gactual;
+  preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
+      body_P_sensor, Factual, Gactual);
+
+  bool use2ndOrderIntegration = false;
+
+  //////////////////////////////////////////////////////////////////////////////////////////////
+  // COMPUTE NUMERICAL DERIVATIVES FOR F
+  //////////////////////////////////////////////////////////////////////////////////////////////
+  // Compute expected F wrt positions (15,3)
+  Matrix df_dpos =
+      numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
+          _1, deltaVij_old, deltaRij_old, bias_old,
+          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
+  // rotation part has to be done properly, on manifold
+  df_dpos.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
+          _1, deltaVij_old, deltaRij_old, bias_old,
+           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
+
+  // Compute expected F wrt velocities (15,3)
+  Matrix df_dvel =
+      numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
+          deltaPij_old, _1, deltaRij_old, bias_old,
+          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
+  // rotation part has to be done properly, on manifold
+  df_dvel.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
+            deltaPij_old, _1, deltaRij_old, bias_old,
+            newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
+
+  // Compute expected F wrt angles (15,3)
+  Matrix df_dangle = numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedMeasurementsTest,
+          deltaPij_old, deltaVij_old, _1, bias_old,
+          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
+  // rotation part has to be done properly, on manifold
+  df_dangle.block<3,3>(6,0) = numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedMeasurementsRot,
+            deltaPij_old, deltaVij_old, _1, bias_old,
+            newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
+
+  // Compute expected F wrt biases (15,6)
+  Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest,
+          deltaPij_old, deltaVij_old, deltaRij_old, _1,
+          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
+  // rotation part has to be done properly, on manifold
+  df_dbias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot,
+            deltaPij_old, deltaVij_old, deltaRij_old, _1,
+            newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
+
+  Matrix Fexpected(15,15);
+  Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
+  EXPECT(assert_equal(Fexpected, Factual));
+
+  //////////////////////////////////////////////////////////////////////////////////////////////
+  // COMPUTE NUMERICAL DERIVATIVES FOR G
+  //////////////////////////////////////////////////////////////////////////////////////////////
+  // Compute expected G wrt integration noise
+  Matrix df_dintNoise(15,3);
+  df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3;
+
+  // Compute expected G wrt acc noise (15,3)
+  Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
+          deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
+          _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
+  // rotation part has to be done properly, on manifold
+  df_daccNoise.block<3,3>(6,0) =  numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
+            deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
+            _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
+
+  // Compute expected G wrt gyro noise (15,3)
+  Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
+          deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
+          newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
+  // rotation part has to be done properly, on manifold
+  df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
+            deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
+            newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
+
+  // Compute expected G wrt bias random walk noise (15,6)
+  Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries
+  df_rwBias.setZero();
+  df_rwBias.block<6,6>(9,0) = eye(6);
+
+  // Compute expected G wrt gyro noise (15,6)
+  Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest,
+          deltaPij_old, deltaVij_old, deltaRij_old, _1,
+          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
+  // rotation part has to be done properly, on manifold
+  df_dinitBias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot,
+            deltaPij_old, deltaVij_old, deltaRij_old, _1,
+            newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
+  df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows
+
+  Matrix Gexpected(15,21);
+  Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
+
+  EXPECT(assert_equal(Gexpected, Gactual));
+
+  // Check covariance propagation
+  Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
+      (1/newDeltaT) * Gactual * Gactual.transpose();
+
+  Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
+  EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
+}
 
 /* ************************************************************************* */
-  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

gtsam/navigation/tests/testImuFactor.cpp

@@ -37,6 +37,8 @@ using symbol_shorthand::B;
 
 /* ************************************************************************* */
 namespace {
+// Auxiliary functions to test evaluate error in ImuFactor
+/* ************************************************************************* */
 Vector callEvaluateError(const ImuFactor& factor,
     const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
     const imuBias::ConstantBias& bias){
@@ -49,14 +51,48 @@ Rot3 evaluateRotationError(const ImuFactor& factor,
   return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
 }
 
+// Auxiliary functions to test Jacobians F and G used for
+// covariance propagation during preintegration
+/* ************************************************************************* */
+Vector updatePreintegratedPosVel(
+    const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
+    const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
+    const bool use2ndOrderIntegration_) {
+
+  Matrix3 dRij = deltaRij_old.matrix();
+  Vector3 temp = dRij * correctedAcc * deltaT;
+  Vector3 deltaPij_new;
+  if(!use2ndOrderIntegration_){
+    deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
+  }else{
+    deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
+  }
+  Vector3 deltaVij_new = deltaVij_old + temp;
+
+  Vector result(6);  result << deltaPij_new,  deltaVij_new;
+  return result;
+}
+
+Rot3 updatePreintegratedRot(const Rot3& deltaRij_old,
+    const Vector3& correctedOmega, const double deltaT) {
+  Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
+  return deltaRij_new;
+}
+
+// Auxiliary functions to test preintegrated Jacobians
+// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
+/* ************************************************************************* */
+double accNoiseVar = 0.01;
+double omegaNoiseVar = 0.03;
+double intNoiseVar = 0.0001;
 ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
     const imuBias::ConstantBias& bias,
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
     const list<double>& deltaTs,
     const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)  ){
-  ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
+  ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(),
-      Matrix3::Identity(), Matrix3::Identity());
+      omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity());
 
   list<Vector3>::const_iterator itAcc = measuredAccs.begin();
   list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@@ -152,7 +188,7 @@ TEST( ImuFactor, PreintegratedMeasurements )
 }
 
 /* ************************************************************************* */
-TEST( ImuFactor, Error )
+TEST( ImuFactor, ErrorAndJacobians )
 {
   // Linearization point
   imuBias::ConstantBias bias; // Bias
@@ -180,62 +216,52 @@ TEST( ImuFactor, Error )
   Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
   EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
 
-  // Expected Jacobians
-  Matrix H1e = numericalDerivative11<Vector,Pose3>(
-      boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
-  Matrix H2e = numericalDerivative11<Vector,Vector3>(
-      boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
-  Matrix H3e = numericalDerivative11<Vector,Pose3>(
-      boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
-  Matrix H4e = numericalDerivative11<Vector,Vector3>(
-      boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
-  Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
-      boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
-
-  // Check rotation Jacobians
-  Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
-      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
-  Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
-      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
-
   // Actual Jacobians
   Matrix H1a, H2a, H3a, H4a, H5a;
   (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
 
-  // positions and velocities
+  // Expected Jacobians
-  Matrix H1etop6 =  H1e.topRows(6);
+  /////////////////// H1 ///////////////////////////
-  Matrix H1atop6 =  H1a.topRows(6);
+  Matrix H1e = numericalDerivative11<Vector,Pose3>(
-  EXPECT(assert_equal(H1etop6, H1atop6));
+      boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
-  // rotations
+  // Jacobians are around zero, so the rotation part is the same as:
-  EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations
+  Matrix H1Rot3 = numericalDerivative11<Rot3,Pose3>(
+      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
+  EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3)));
+  EXPECT(assert_equal(H1e, H1a));
 
+  /////////////////// H2 ///////////////////////////
+  Matrix H2e = numericalDerivative11<Vector,Vector3>(
+      boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
   EXPECT(assert_equal(H2e, H2a));
 
-  // positions and velocities
+  /////////////////// H3 ///////////////////////////
-  Matrix H3etop6 =  H3e.topRows(6);
+  Matrix H3e = numericalDerivative11<Vector,Pose3>(
-  Matrix H3atop6 =  H3a.topRows(6);
+      boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
-  EXPECT(assert_equal(H3etop6, H3atop6));
+  // Jacobians are around zero, so the rotation part is the same as:
-  // rotations
+  Matrix H3Rot3 = numericalDerivative11<Rot3,Pose3>(
-  EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations
+      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
+  EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3)));
+  EXPECT(assert_equal(H3e, H3a));
 
+  /////////////////// H4 ///////////////////////////
+  Matrix H4e = numericalDerivative11<Vector,Vector3>(
+      boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
   EXPECT(assert_equal(H4e, H4a));
-  // EXPECT(assert_equal(H5e, H5a));
+
+  /////////////////// H5 ///////////////////////////
+  Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
+      boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
+  EXPECT(assert_equal(H5e, H5a));
 }
 
 /* ************************************************************************* */
-TEST( ImuFactor, ErrorWithBiases )
+TEST( ImuFactor, ErrorAndJacobianWithBiases )
 {
-  // Linearization point
+  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
-//  Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
+  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0));
-//  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
-//  Vector3 v1(Vector3(0.5, 0.0, 0.0));
-//  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
-//  Vector3 v2(Vector3(0.5, 0.0, 0.0));
-
-  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
-  Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
   Vector3 v1(Vector3(0.5, 0.0, 0.0));
-  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
+  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
   Vector3 v2(Vector3(0.5, 0.0, 0.0));
 
   // Measurements
@@ -245,12 +271,10 @@ TEST( ImuFactor, ErrorWithBiases )
   Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
   double deltaT = 1.0;
 
-  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
+  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
+      Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-  //  ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
-  //    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-
   // Create factor
   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
 
@@ -258,8 +282,9 @@ TEST( ImuFactor, ErrorWithBiases )
   Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
   SETDEBUG("ImuFactor evaluateError", false);
 
-    // Expected error
+  // Expected error (should not be zero in this test, as we want to evaluate Jacobians
-    Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
+  // at a nontrivial linearization point)
+  // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
   // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
 
   // Expected Jacobians
@@ -294,7 +319,72 @@ TEST( ImuFactor, ErrorWithBiases )
 }
 
 /* ************************************************************************* */
-TEST( ImuFactor, PartialDerivativeExpmap )
+TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis )
+{
+  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
+  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0));
+  Vector3 v1(Vector3(0.5, 0.0, 0.0));
+  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
+  Vector3 v2(Vector3(0.5, 0.0, 0.0));
+
+  // Measurements
+  Vector3 gravity; gravity << 0, 0, 9.81;
+  Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
+  Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
+  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
+  double deltaT = 1.0;
+
+  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
+      Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
+  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+
+  // Create factor
+  Pose3 bodyPsensor = Pose3();
+  bool use2ndOrderCoriolis = true;
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
+
+  SETDEBUG("ImuFactor evaluateError", false);
+  Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
+  SETDEBUG("ImuFactor evaluateError", false);
+
+  // Expected error (should not be zero in this test, as we want to evaluate Jacobians
+  // at a nontrivial linearization point)
+  // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
+  // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
+
+  // Expected Jacobians
+  Matrix H1e = numericalDerivative11<Vector,Pose3>(
+      boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
+  Matrix H2e = numericalDerivative11<Vector,Vector3>(
+      boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
+  Matrix H3e = numericalDerivative11<Vector,Pose3>(
+      boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
+  Matrix H4e = numericalDerivative11<Vector,Vector3>(
+      boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
+  Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
+      boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
+
+  // Check rotation Jacobians
+  Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
+      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
+  Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
+      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
+  Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
+      boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);
+
+  // Actual Jacobians
+  Matrix H1a, H2a, H3a, H4a, H5a;
+  (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
+
+  EXPECT(assert_equal(H1e, H1a));
+  EXPECT(assert_equal(H2e, H2a));
+  EXPECT(assert_equal(H3e, H3a));
+  EXPECT(assert_equal(H4e, H4a));
+  EXPECT(assert_equal(H5e, H5a));
+}
+
+/* ************************************************************************* */
+TEST( ImuFactor, PartialDerivative_wrt_Bias )
 {
   // Linearization point
   Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
@@ -324,20 +414,14 @@ TEST( ImuFactor, PartialDerivativeLogmap )
   // Measurements
   Vector3 deltatheta; deltatheta << 0, 0, 0;
 
-
   // Compute numerical derivatives
   Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind(
       &evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
 
-  const Vector3 x = thetahat; // parametrization of so(3)
+  Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);
-  const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-  double normx = norm_2(x);
-  const Matrix3  actualDelFdeltheta = Matrix3::Identity() +
-       0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
 
   // Compare Jacobians
   EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
-
 }
 
 /* ************************************************************************* */
@@ -354,7 +438,6 @@ TEST( ImuFactor, fistOrderExponential )
   double alpha = 0.0;
   Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
 
-
   const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
 
   Matrix3  delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
@@ -366,7 +449,7 @@ TEST( ImuFactor, fistOrderExponential )
       hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
   //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
 
-  // Compare Jacobians
+  // This is a first order expansion so the equality is only an approximation
   EXPECT(assert_equal(expectedRot, actualRot));
 }
 
@@ -423,6 +506,128 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
   EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
+/* ************************************************************************* */
+TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
+{
+  // Linearization point
+  imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
+  Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
+
+  // Measurements
+  list<Vector3> measuredAccs, measuredOmegas;
+  list<double> deltaTs;
+  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
+  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
+  deltaTs.push_back(0.01);
+  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
+  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
+  deltaTs.push_back(0.01);
+  for(int i=1;i<100;i++)
+  {
+    measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
+    measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
+    deltaTs.push_back(0.01);
+  }
+  // Actual preintegrated values
+  ImuFactor::PreintegratedMeasurements preintegrated =
+      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
+
+  // so far we only created a nontrivial linearization point for the preintegrated measurements
+  // Now we add a new measurement and ask for Jacobians
+  const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
+  const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
+  const double newDeltaT = 0.01;
+  const Rot3    deltaRij_old = preintegrated.deltaRij();    // before adding new measurement
+  const Vector3 deltaVij_old = preintegrated.deltaVij();    // before adding new measurement
+  const Vector3 deltaPij_old = preintegrated.deltaPij();    // before adding new measurement
+
+  Matrix oldPreintCovariance = preintegrated.preintMeasCov();
+
+  Matrix Factual, Gactual;
+  preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
+      body_P_sensor, Factual, Gactual);
+
+  bool use2ndOrderIntegration = false;
+
+  //////////////////////////////////////////////////////////////////////////////////////////////
+  // COMPUTE NUMERICAL DERIVATIVES FOR F
+  //////////////////////////////////////////////////////////////////////////////////////////////
+  // Compute expected f_pos_vel wrt positions
+  Matrix dfpv_dpos =
+      numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
+          _1, deltaVij_old, deltaRij_old,
+          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
+
+  // Compute expected f_pos_vel wrt velocities
+  Matrix dfpv_dvel =
+      numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
+          deltaPij_old, _1, deltaRij_old,
+          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
+
+  // Compute expected f_pos_vel wrt angles
+  Matrix dfpv_dangle =
+      numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
+          deltaPij_old, deltaVij_old, _1,
+          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
+
+  Matrix FexpectedTop6(6,9);   FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
+
+  // Compute expected f_rot wrt angles
+  Matrix dfr_dangle =
+      numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
+          _1, newMeasuredOmega, newDeltaT), deltaRij_old);
+
+  Matrix FexpectedBottom3(3,9);
+  FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
+  Matrix Fexpected(9,9);  Fexpected << FexpectedTop6, FexpectedBottom3;
+
+  EXPECT(assert_equal(Fexpected, Factual));
+
+  //////////////////////////////////////////////////////////////////////////////////////////////
+  // COMPUTE NUMERICAL DERIVATIVES FOR G
+  //////////////////////////////////////////////////////////////////////////////////////////////
+  // Compute jacobian wrt integration noise
+  Matrix dgpv_dintNoise(6,3);
+  dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
+
+  // Compute jacobian wrt acc noise
+  Matrix dgpv_daccNoise =
+      numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
+          deltaPij_old, deltaVij_old, deltaRij_old,
+          _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
+
+  // Compute expected F wrt gyro noise
+  Matrix dgpv_domegaNoise =
+      numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
+          deltaPij_old, deltaVij_old, deltaRij_old,
+          newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
+  Matrix GexpectedTop6(6,9);
+  GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
+
+  // Compute expected f_rot wrt gyro noise
+  Matrix dgr_dangle =
+      numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot,
+          deltaRij_old, _1, newDeltaT), newMeasuredOmega);
+
+  Matrix GexpectedBottom3(3,9);
+  GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
+  Matrix Gexpected(9,9);  Gexpected << GexpectedTop6, GexpectedBottom3;
+
+  EXPECT(assert_equal(Gexpected, Gactual));
+
+  // Check covariance propagation
+  Matrix9 measurementCovariance;
+  measurementCovariance << intNoiseVar*I_3x3, Z_3x3,             Z_3x3,
+                           Z_3x3,             accNoiseVar*I_3x3, Z_3x3,
+                           Z_3x3,             Z_3x3,             omegaNoiseVar*I_3x3;
+
+  Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
+      (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
+
+  Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
+  EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
+}
+
 //#include <gtsam/linear/GaussianFactorGraph.h>
 ///* ************************************************************************* */
 //TEST( ImuFactor, LinearizeTiming)
@@ -561,13 +766,11 @@ TEST(ImuFactor, PredictPositionAndVelocity){
     // Predict
     Pose3 x1;
     Vector3 v1(0, 0.0, 0.0);
-    PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
+    PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
     Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
     Vector3 expectedVelocity; expectedVelocity<<0,1,0;
     EXPECT(assert_equal(expectedPose, poseVelocity.pose));
     EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
-
-
 }
 
 /* ************************************************************************* */
@@ -595,7 +798,7 @@ TEST(ImuFactor, PredictRotation) {
     // Predict
     Pose3 x1;
     Vector3 v1(0, 0.0, 0.0);
-    PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
+    PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
     Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
     Vector3 expectedVelocity; expectedVelocity<<0,0,0;
     EXPECT(assert_equal(expectedPose, poseVelocity.pose));