correct Jacobians for body_P_sensor case, including derivative for centripetal acc

gtsam/navigation/ImuFactor.cpp

@@ -78,8 +78,8 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose();
 #else
   preintMeasCov_ = F * preintMeasCov_ * F.transpose()
-      + G1 * (p().accelerometerCovariance / dt) * G1.transpose()
       + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
+      + G1 * (p().accelerometerCovariance / dt) * G1.transpose()
       + G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
 #endif
 }

gtsam/navigation/PreintegrationBase.cpp

@@ -76,7 +76,8 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
 
 //------------------------------------------------------------------------------
 pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
-    const Vector3& j_measuredAcc, const Vector3& j_measuredOmega) const {
+    const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
+    OptionalJacobian<3, 3> D_correctedAcc_measuredOmega) const {
 
   // Correct for bias in the sensor frame
   Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
@@ -99,6 +100,12 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
       const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
       const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
       j_correctedAcc -= body_Omega_body * b_velocity_bs;
+      if (D_correctedAcc_measuredOmega) {
+        double wdp = j_correctedOmega.dot(b_arm);
+        *D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
+            + j_correctedOmega * b_arm.transpose()) * bRs.matrix()
+            + 2 * b_arm * j_measuredOmega.transpose();
+      }
     }
   }
 
@@ -112,11 +119,20 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
     OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const {
 
   Vector3 j_correctedAcc, j_correctedOmega;
+  Matrix3 D_correctedAcc_measuredOmega;
   boost::tie(j_correctedAcc, j_correctedOmega) =
-      correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
-
+      correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega, D_correctedAcc_measuredOmega);
   // Do update in one fell swoop
-  return deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, F, G1, G2);
+  NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, F, G1, G2);
+  if (G1 && G2 && p().body_P_sensor) {
+    const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
+    *G2 = *G2*bRs;
+    if (!p().body_P_sensor->translation().vector().isZero()) {
+      *G2 += *G1 * D_correctedAcc_measuredOmega;
+    }
+    *G1 = *G1*bRs;
+  }
+  return updated;
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/PreintegrationBase.h

@@ -145,6 +145,10 @@ public:
     return *boost::static_pointer_cast<Params>(p_);
   }
 
+  void set_body_P_sensor(const Pose3& body_P_sensor) {
+    p_->body_P_sensor = body_P_sensor;
+  }
+
   /// getters
   const NavState& deltaXij() const {
     return deltaXij_;
@@ -193,7 +197,8 @@ public:
 
   /// Subtract estimate and correct for sensor pose
   std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
-      const Vector3& j_measuredAcc, const Vector3& j_measuredOmega) const;
+      const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
+      OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none) const;
 
   /// Calculate the updated preintegrated measurement, does not modify
   /// It takes measured quantities in the j frame