BIG: switch to NavState delta pose

gtsam/navigation/ImuFactor.cpp

@@ -61,16 +61,17 @@ void PreintegratedImuMeasurements::resetIntegration() {
 #define D_v_v(H) (H)->block<3,3>(6,6)
 //------------------------------------------------------------------------------
 void PreintegratedImuMeasurements::integrateMeasurement(
-    const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
+    const Vector3& measuredAcc, const Vector3& measuredOmega, double dt,
-    OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
+    OptionalJacobian<9, 9> outF, OptionalJacobian<9, 9> G) {
 
-  const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test
+  static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
 
   // Update preintegrated measurements (also get Jacobian)
   Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
   Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
-  updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT,
+  Matrix93 G1, G2;
-      &D_incrR_integratedOmega, &F);
+  updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt,
+      &D_incrR_integratedOmega, &F, &G1, &G2);
 
   // first order covariance propagation:
   // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
@@ -78,35 +79,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G'
   // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
   // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
-  // NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise,
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose()
-  // as G and measurementCovariance are block-diagonal matrices
+      + G1 * (p().accelerometerCovariance / dt) * G1.transpose()
-  preintMeasCov_ = F * preintMeasCov_ * F.transpose();
+      + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
-  D_R_R(&preintMeasCov_) += D_incrR_integratedOmega * p().gyroscopeCovariance
+      + G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
-      * D_incrR_integratedOmega.transpose() * deltaT;
-  D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT;
-  D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT;
 
-  Matrix3 F_pos_noiseacc;
+  if (outF) *outF = F;
-  F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
+  if (G) *G << G2, Gi*dt, G1; // NOTE(frank): order here is R,P,V
-  D_t_t(&preintMeasCov_) += (1 / deltaT) * F_pos_noiseacc
-      * p().accelerometerCovariance * F_pos_noiseacc.transpose();
-  Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance
-      * dRij.transpose(); // has 1/deltaT
-  D_t_v(&preintMeasCov_) += temp;
-  D_v_t(&preintMeasCov_) += temp.transpose();
-
-  // F_test and G_test are given as output for testing purposes and are not needed by the factor
-  if (F_test) {
-    (*F_test) << F;
-  }
-  if (G_test) {
-    // This in only for testing & documentation, while the actual computation is done block-wise
-    //           omegaNoise               intNoise        accNoise
-    (*G_test) <<
-        D_incrR_integratedOmega * deltaT, Z_3x3,          Z_3x3, // angle
-        Z_3x3,                            I_3x3 * deltaT, F_pos_noiseacc, // pos
-        Z_3x3,                            Z_3x3,          dRij * deltaT; // vel
-  }
 }
 //------------------------------------------------------------------------------
 PreintegratedImuMeasurements::PreintegratedImuMeasurements(

gtsam/navigation/ImuFactor.h

@@ -96,13 +96,13 @@ public:
    * Add a single IMU measurement to the preintegration.
    * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
    * @param measuredOmega Measured angular velocity (as given by the sensor)
-   * @param deltaT Time interval between this and the last IMU measurement
+   * @param dt Time interval between this and the last IMU measurement
-   * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
+   * @param F, F Jacobians used internally (only needed for testing)
-   * @param Fout, Gout Jacobians used internally (only needed for testing)
    */
   void integrateMeasurement(const Vector3& measuredAcc,
-      const Vector3& measuredOmega, double deltaT,
+      const Vector3& measuredOmega, double dt, //
-      OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
+      OptionalJacobian<9, 9> F = boost::none, //
+      OptionalJacobian<9, 9> G = boost::none);
 
   /// Return pre-integrated measurement covariance
   Matrix preintMeasCov() const { return preintMeasCov_; }

gtsam/navigation/NavState.cpp

@@ -322,14 +322,14 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt,
     OptionalJacobian<9, 9> H2) const {
   const Rot3& nRb = R_;
   const Velocity3& n_v = v_; // derivative is Ri !
-  const double dt2 = dt * dt;
+  const double dt22 = 0.5 * dt * dt;
 
   Vector9 xi;
   Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
   dR(xi) = dR(pim);
   dP(xi) = dP(pim)
       + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
-      + (0.5 * dt2) * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
+      + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
   dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
 
   if (omegaCoriolis) {
@@ -342,7 +342,7 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt,
     if (H1) {
       if (!omegaCoriolis)
         H1->setZero(); // if coriolis H1 is already initialized
-      D_t_R(H1) += dt * D_dP_Ri1 + (0.5 * dt2) * D_dP_Ri2;
+      D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2;
       D_t_v(H1) += dt * D_dP_nv * Ri;
       D_v_R(H1) += dt * D_dV_Ri;
     }

gtsam/navigation/PreintegrationBase.cpp

@@ -29,10 +29,8 @@ namespace gtsam {
 /// Re-initialize PreintegratedMeasurements
 void PreintegrationBase::resetIntegration() {
   deltaTij_ = 0.0;
-  deltaRij_ = Rot3();
+  deltaXij_ = NavState();
   delRdelBiasOmega_ = Z_3x3;
-  deltaPij_ = Vector3::Zero();
-  deltaVij_ = Vector3::Zero();
   delPdelBiasAcc_ = Z_3x3;
   delPdelBiasOmega_ = Z_3x3;
   delVdelBiasAcc_ = Z_3x3;
@@ -43,21 +41,19 @@ void PreintegrationBase::resetIntegration() {
 void PreintegrationBase::print(const string& s) const {
   cout << s << endl;
   cout << "    deltaTij [" << deltaTij_ << "]" << endl;
-  cout << "    deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
+  cout << "    deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl;
-  cout << "    deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
+  cout << "    deltaPij [ " << deltaPij().transpose() << " ]" << endl;
-  cout << "    deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
+  cout << "    deltaVij [ " << deltaVij().transpose() << " ]" << endl;
   biasHat_.print("    biasHat");
 }
 
 /// Needed for testable
 bool PreintegrationBase::equals(const PreintegrationBase& other,
     double tol) const {
-  return deltaRij_.equals(other.deltaRij_, tol)
+  return fabs(deltaTij_ - other.deltaTij_) < tol
-      && fabs(deltaTij_ - other.deltaTij_) < tol
+      && deltaXij_.equals(other.deltaXij_, tol)
-      && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
       && biasHat_.equals(other.biasHat_, tol)
-      && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
+      && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
-      && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
       && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
       && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
       && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
@@ -65,8 +61,8 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
 }
 
 void PreintegrationBase::updatePreintegratedMeasurements(
-    const Vector3& measuredAcc, const Vector3& measuredOmega,
+    const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
-    const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F) {
+    Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) {
 
   // 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).
@@ -91,43 +87,31 @@ void PreintegrationBase::updatePreintegratedMeasurements(
     correctedAcc = bRs * correctedAcc - correctedOmega.cross(b_velocity_bs);
   }
 
-  // Calculate acceleration in *current* i frame, i.e., before rotation update below
+  // Save current rotation for updating Jacobians
+  const Rot3 oldRij = deltaXij_.attitude();
+
+  // Do update in one fell swoop
+  deltaTij_ += dt;
+  deltaXij_ = deltaXij_.update(correctedAcc, correctedOmega, dt, F, G1, G2);
+
+  // Update Jacobians
+  // TODO(frank): we are repeating some computation here: accessible in F ?
   Matrix3 D_acc_R;
-  const Matrix3 dRij = deltaRij_.matrix(); // expensive
+  oldRij.rotate(correctedAcc, D_acc_R);
-  const Vector3 i_acc = deltaRij_.rotate(correctedAcc, D_acc_R);
   const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
 
-//  NavState iTj(deltaRij_, deltaPij_, deltaVij_);
+  const Vector3 integratedOmega = correctedOmega * dt;
-//  iTj = iTj.update();
+  const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
-
-  // rotation vector describing rotation increment computed from the
-  // current rotation rate measurement
-  const Vector3 integratedOmega = correctedOmega * deltaT;
-  const Rot3 incrR =  Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
-
-  // Update deltaTij and rotation
-  deltaTij_ += deltaT;
-  Matrix3 D_Rij_incrR;
-  deltaRij_ = deltaRij_.compose(incrR, D_Rij_incrR);
-
-  // Update Jacobian
   const Matrix3 incrRt = incrR.transpose();
   delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
-      - *D_incrR_integratedOmega * deltaT;
+      - *D_incrR_integratedOmega * dt;
 
-  double dt22 = 0.5 * deltaT * deltaT;
+  double dt22 = 0.5 * dt * dt;
-  deltaPij_ += dt22 * i_acc + deltaT * deltaVij_;
+  const Matrix3 dRij = oldRij.matrix(); // expensive
-  deltaVij_ += deltaT * i_acc;
+  delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
-
+  delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
-  *F << // angle        pos    vel
+  delVdelBiasAcc_ += -dRij * dt;
-      D_Rij_incrR, Z_3x3, Z_3x3, // angle
+  delVdelBiasOmega_ += D_acc_biasOmega * dt;
-  dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos
-  deltaT * D_acc_R, Z_3x3, I_3x3; // vel
-
-  delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - dt22 * dRij;
-  delPdelBiasOmega_ += deltaT * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
-  delVdelBiasAcc_ += -dRij * deltaT;
-  delVdelBiasOmega_ += D_acc_biasOmega * deltaT;
 }
 
 //------------------------------------------------------------------------------
@@ -135,24 +119,26 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
   // Correct deltaRij, derivative is delRdelBiasOmega_
   const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
-  Matrix3 D_deltaRij_bias;
+  Matrix3 D_correctedRij_bias;
   const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
-  const Rot3 deltaRij = deltaRij_.expmap(biasInducedOmega, boost::none, H ? &D_deltaRij_bias : 0);
+  const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
-  if (H) D_deltaRij_bias *= delRdelBiasOmega_;
+      H ? &D_correctedRij_bias : 0);
+  if (H)
+    D_correctedRij_bias *= delRdelBiasOmega_;
 
   Vector9 xi;
-  Matrix3 D_dR_deltaRij;
+  Matrix3 D_dR_correctedRij;
   // TODO(frank): could line below be simplified? It is equivalent to
   //   LogMap(deltaRij_.compose(Expmap(delRdelBiasOmega_ * biasIncr.gyroscope())))
-  NavState::dR(xi) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0);
+  NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
-  NavState::dP(xi) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
+  NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
       + delPdelBiasOmega_ * biasIncr.gyroscope();
-  NavState::dV(xi) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
+  NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
       + delVdelBiasOmega_ * biasIncr.gyroscope();
 
   if (H) {
     Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
-    D_dR_bias << Z_3x3, D_dR_deltaRij * D_deltaRij_bias;
+    D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
     D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
     D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
     (*H) << D_dR_bias, D_dP_bias, D_dV_bias;

gtsam/navigation/PreintegrationBase.h

@@ -100,9 +100,14 @@ public:
 
 protected:
 
-  double deltaTij_;  ///< Time interval from i to j
+  double deltaTij_;   ///< Time interval from i to j
-  Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
+
-  Matrix3 delRdelBiasOmega_;  ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
+  /**
+   * Preintegrated navigation state, from frame i to frame j
+   * Note: relative position does not take into account velocity at time i, see deltap+, in [2]
+   * Note: velocity is now also in frame i, as opposed to deltaVij in [2]
+   */
+  NavState deltaXij_;
 
   /// Parameters
   boost::shared_ptr<Params> p_;
@@ -110,12 +115,10 @@ protected:
   /// Acceleration and gyro bias used for preintegration
   imuBias::ConstantBias biasHat_;
 
-  Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+  Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
-  Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
+  Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
-
-  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 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
   Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
 
   /// Default constructor for serialization
@@ -147,19 +150,19 @@ public:
     return deltaTij_;
   }
   const Rot3& deltaRij() const {
-    return deltaRij_;
+    return deltaXij_.attitude();
   }
-  const Matrix3& delRdelBiasOmega() const {
+  Vector3 deltaPij() const {
-    return delRdelBiasOmega_;
+    return deltaXij_.position().vector();
+  }
+  Vector3 deltaVij() const {
+    return deltaXij_.velocity();
   }
   const imuBias::ConstantBias& biasHat() const {
     return biasHat_;
   }
-  const Vector3& deltaPij() const {
+  const Matrix3& delRdelBiasOmega() const {
-    return deltaPij_;
+    return delRdelBiasOmega_;
-  }
-  const Vector3& deltaVij() const {
-    return deltaVij_;
   }
   const Matrix3& delPdelBiasAcc() const {
     return delPdelBiasAcc_;
@@ -188,7 +191,7 @@ public:
   /// Update preintegrated measurements
   void updatePreintegratedMeasurements(const Vector3& measuredAcc,
       const Vector3& measuredOmega, const double deltaT,
-      Matrix3* D_incrR_integratedOmega, Matrix9* F);
+      Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2);
 
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far
@@ -220,11 +223,9 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_NVP(p_);
     ar & BOOST_SERIALIZATION_NVP(deltaTij_);
-    ar & BOOST_SERIALIZATION_NVP(deltaRij_);
+    ar & BOOST_SERIALIZATION_NVP(deltaXij_);
-    ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
     ar & BOOST_SERIALIZATION_NVP(biasHat_);
-    ar & BOOST_SERIALIZATION_NVP(deltaPij_);
+    ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
-    ar & BOOST_SERIALIZATION_NVP(deltaVij_);
     ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
     ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
     ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);

gtsam/navigation/tests/testImuFactor.cpp

@@ -40,6 +40,7 @@ using symbol_shorthand::B;
 static const Vector3 kGravityAlongNavZDown(0, 0, 9.81);
 static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
 static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
+static const imuBias::ConstantBias kZeroBiasHat, kZeroBias;
 
 /* ************************************************************************* */
 namespace {
@@ -139,46 +140,51 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
 /* ************************************************************************* */
 TEST(ImuFactor, StraightLine) {
   // Set up IMU measurements
-  static const double g = 10; // make this easy to check
+  static const double g = 10; // make gravity 10 to make this easy to check
-  static const double a = 0.2, dt = 3.0;
+  static const double v = 5.0, a = 0.2, dt = 3.0;
-  const double exact = dt * dt / 2;
+  const double dt22 = dt * dt / 2;
-  Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0);
 
-  // Set up body pointing towards y axis, and start at 10,20,0 with zero velocity
+  // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
-  // TODO(frank): clean up Rot3 mess
+  // The body itself has Z axis pointing down
   static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
   static const Point3 initial_position(10, 20, 0);
-  static const NavState state1(nRb, initial_position, Velocity3(0, 0, 0));
+  static const Vector3 initial_velocity(v,0,0);
+  static const NavState state1(nRb, initial_position, initial_velocity);
 
-  imuBias::ConstantBias biasHat, bias;
+  // set up acceleration in X direction, no angular velocity.
+  // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z
+  Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0);
+
+  // Create parameters assuming nav frame has Z up
   boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
-      PreintegratedImuMeasurements::Params::MakeSharedU(g); // Up convention!
+      PreintegratedImuMeasurements::Params::MakeSharedU(g);
 
-  PreintegratedImuMeasurements pim(p, biasHat);
+  // Now, preintegrate for 3 seconds, in 10 steps
+  PreintegratedImuMeasurements pim(p, kZeroBiasHat);
   for (size_t i = 0; i < 10; i++)
     pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10);
 
   // Check integrated quantities in body frame: gravity measured by IMU is integrated!
+  Vector3 b_deltaP(a * dt22, 0, -g * dt22);
+  Vector3 b_deltaV(a * dt, 0, -g * dt);
   EXPECT(assert_equal(Rot3(), pim.deltaRij()));
-  EXPECT(assert_equal(Vector3(a * exact, 0, -g * exact), pim.deltaPij()));
+  EXPECT(assert_equal(b_deltaP, pim.deltaPij()));
-  EXPECT(assert_equal(Vector3(a * dt, 0, -g * dt), pim.deltaVij()));
+  EXPECT(assert_equal(b_deltaV, pim.deltaVij()));
 
   // Check bias-corrected delta: also in body frame
   Vector9 expectedBC;
-  expectedBC << 0, 0, 0, a * exact, 0, -g * exact, a * dt, 0, -g * dt;
+  expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV;
-  EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(bias)));
+  EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias)));
 
-  // Check prediction, note we move along x in body, along y in nav
+  // Check prediction in nav, note we move along x in body, along y in nav
-  NavState expected(nRb, Point3(10, 20 + a * exact, 0),
+  Point3 expected_position(10 + v*dt, 20 + a * dt22, 0);
-      Velocity3(0, a * dt, 0));
+  Velocity3 expected_velocity(v, a * dt, 0);
-  EXPECT(assert_equal(expected, pim.predict(state1, bias)));
+  NavState expected(nRb, expected_position, expected_velocity);
+  EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias)));
 }
 
 /* ************************************************************************* */
 TEST(ImuFactor, PreintegratedMeasurements) {
-  // Linearization point
-  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
-
   // Measurements
   Vector3 measuredAcc(0.1, 0.0, 0.0);
   Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
@@ -192,7 +198,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
   double expectedDeltaT1(0.5);
 
   // Actual preintegrated values
-  PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance,
+  PreintegratedImuMeasurements actual1(kZeroBiasHat, kMeasuredAccCovariance,
       kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
   actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
@@ -222,7 +228,6 @@ TEST(ImuFactor, PreintegratedMeasurements) {
 /* ************************************************************************* */
 // Common linearization point and measurements for tests
 namespace common {
-static const imuBias::ConstantBias biasHat, bias; // Bias
 static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
     Point3(5.0, 1.0, 0));
 static const Vector3 v1(Vector3(0.5, 0.0, 0.0));
@@ -252,28 +257,28 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
   p->integrationCovariance = kIntegrationErrorCovariance;
   p->use2ndOrderCoriolis = true;
 
-  PreintegratedImuMeasurements pim(p, bias);
+  PreintegratedImuMeasurements pim(p, kZeroBiasHat);
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // biasCorrectedDelta
   Matrix96 actualH;
-  pim.biasCorrectedDelta(bias, actualH);
+  pim.biasCorrectedDelta(kZeroBias, actualH);
   Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
       boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
-          boost::none), bias);
+          boost::none), kZeroBias);
   EXPECT(assert_equal(expectedH, actualH));
 
   Matrix9 aH1;
   Matrix96 aH2;
-  NavState predictedState = pim.predict(state1, bias, aH1, aH2);
+  NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
   Matrix eH1 = numericalDerivative11<NavState, NavState>(
-      boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none,
+      boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none,
           boost::none), state1);
   EXPECT(assert_equal(eH1, aH1));
   Matrix eH2 = numericalDerivative11<NavState, imuBias::ConstantBias>(
       boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
-          boost::none), bias);
+          boost::none), kZeroBias);
   EXPECT(assert_equal(eH2, aH2));
   return;
 
@@ -282,11 +287,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
 /* ************************************************************************* */
 TEST(ImuFactor, ErrorAndJacobians) {
   using namespace common;
-  PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance,
+  PreintegratedImuMeasurements pim(kZeroBiasHat, kMeasuredAccCovariance,
       kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
 
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-  EXPECT(assert_equal(state2, pim.predict(state1, bias)));
+  EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias)));
 
   // Create factor
   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
@@ -296,14 +301,14 @@ TEST(ImuFactor, ErrorAndJacobians) {
   Vector expectedError(9);
   expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
   EXPECT(
-      assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, bias)));
+      assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias)));
 
   Values values;
   values.insert(X(1), x1);
   values.insert(V(1), v1);
   values.insert(X(2), x2);
   values.insert(V(2), v2);
-  values.insert(B(1), bias);
+  values.insert(B(1), kZeroBias);
   EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));
 
   // Make sure linearization is correct
@@ -312,16 +317,16 @@ TEST(ImuFactor, ErrorAndJacobians) {
 
   // Actual Jacobians
   Matrix H1a, H2a, H3a, H4a, H5a;
-  (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
+  (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a);
 
   // Make sure rotation part is correct when error is interpreted as axis-angle
   // Jacobians are around zero, so the rotation part is the same as:
   Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
+      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1);
   EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
 
   Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
+      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2);
   EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
 
   // Evaluate error with wrong values
@@ -330,7 +335,7 @@ TEST(ImuFactor, ErrorAndJacobians) {
   expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901;
   EXPECT(
       assert_equal(expectedError,
-          factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-2));
+          factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2));
   EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2));
 
   // Make sure the whitening is done correctly
@@ -503,9 +508,6 @@ TEST(ImuFactor, fistOrderExponential) {
 
 /* ************************************************************************* */
 TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
-  // Linearization point
-  imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
-
   Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
 
   // Measurements
@@ -526,28 +528,28 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
 
   // Actual preintegrated values
   PreintegratedImuMeasurements preintegrated =
-      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
+      evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
           deltaTs);
 
   // Compute numerical derivatives
   Matrix expectedDelPdelBias = numericalDerivative11<Vector,
       imuBias::ConstantBias>(
       boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
-          measuredOmegas, deltaTs), bias);
+          measuredOmegas, deltaTs), kZeroBias);
   Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
   Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
 
   Matrix expectedDelVdelBias = numericalDerivative11<Vector,
       imuBias::ConstantBias>(
       boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
-          measuredOmegas, deltaTs), bias);
+          measuredOmegas, deltaTs), kZeroBias);
   Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
   Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
 
   Matrix expectedDelRdelBias =
       numericalDerivative11<Rot3, imuBias::ConstantBias>(
           boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
-              measuredAccs, measuredOmegas, deltaTs), bias);
+              measuredAccs, measuredOmegas, deltaTs), kZeroBias);
   Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
   Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
 
@@ -565,9 +567,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
 
 /* ************************************************************************* */
 TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
-  // Linearization point
-  imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
-
   // Measurements
   const Vector3 measuredAcc(0.1, 0.0, 0.0);
   const Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
@@ -587,20 +586,19 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
     deltaTs.push_back(deltaT);
   }
   // Actual preintegrated values
-  PreintegratedImuMeasurements preintegrated =
+  PreintegratedImuMeasurements pim = evaluatePreintegratedMeasurements(
-      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
+      kZeroBias, measuredAccs, measuredOmegas, deltaTs);
-          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 Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
+  const Rot3 deltaRij_old = pim.deltaRij(); // before adding new measurement
-  const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
+  const Vector3 deltaVij_old = pim.deltaVij(); // before adding new measurement
-  const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
+  const Vector3 deltaPij_old = pim.deltaPij(); // before adding new measurement
 
-  Matrix oldPreintCovariance = preintegrated.preintMeasCov();
+  Matrix oldPreintCovariance = pim.preintMeasCov();
 
   Matrix Factual, Gactual;
-  preintegrated.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
+  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
       Factual, Gactual);
 
   //////////////////////////////////////////////////////////////////////////////////////////////
@@ -673,15 +671,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
       * Factual.transpose()
       + (1 / deltaT) * Gactual * measurementCovariance * Gactual.transpose();
 
-  Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
+  Matrix newPreintCovarianceActual = pim.preintMeasCov();
   EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
 }
 
 /* ************************************************************************* */
 TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
-  // Linearization point
-  imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
-
   // Measurements
   list<Vector3> measuredAccs, measuredOmegas;
   list<double> deltaTs;
@@ -699,7 +694,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
   }
   // Actual preintegrated values
   PreintegratedImuMeasurements preintegrated =
-      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
+      evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
           deltaTs);
 
   // so far we only created a nontrivial linearization point for the preintegrated measurements