Fix/covariances

gtsam/navigation/CombinedImuFactor.h

@@ -193,6 +193,11 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurementsT : public PreintegrationTyp
 // For backward compatibility:
 using PreintegratedCombinedMeasurements = PreintegratedCombinedMeasurementsT<DefaultPreintegrationType>;
 
+template <class PIM>
+inline Eigen::Matrix<double, 15, 15> combinedImuFactorResidualCov(const PIM& pim) {
+  return pim.preintMeasCov();
+}
+
 /**
  * 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
@@ -247,7 +252,7 @@ class GTSAM_EXPORT CombinedImuFactorT
   CombinedImuFactorT(
       Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
       const PIM& preintegratedMeasurements)
-      : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov()),
+      : Base(noiseModel::Gaussian::Covariance(combinedImuFactorResidualCov(preintegratedMeasurements)),
              pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
         pim_(preintegratedMeasurements) {}
 

gtsam/navigation/ImuFactor.h

@@ -173,6 +173,23 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsT: public PreintegrationType {
 // controls which class PreintegratedImuMeasurements uses):
 using PreintegratedImuMeasurements = PreintegratedImuMeasurementsT<DefaultPreintegrationType>;
 
+/**
+ * The preint error state dXt = [delta_phi_ij, delta_p_ij, delta_v_ij] is defined as:
+ *   R_ij = R̂_ij Exp(delta_phi_ij)
+ *   p_ij = p̂_ij + delta_p_ij   (additive in the body frame of state_i)
+ *   v_ij = v̂_ij + delta_v_ij   (additive in the body frame of state_i)
+ * and preintMeasCov is the covariance of dXt.
+ *
+ * PreintegrationBase::computeError defines the residual as:
+ *   r = [Log(R_j^{-1} R_pred), R_i^T (p_pred - p_j), R_i^T (v_pred - v_j)]
+ * so the Jacobian of r w.r.t. dXt is identity (up to the rotation right-Jacobian
+ * which is ~I for small angles), and preintMeasCov applies directly.
+ */
+template <class PIM>
+inline gtsam::Matrix9 imuFactorResidualCov(const PIM& pim) {
+  return pim.preintMeasCov();
+}
+
 /**
  * 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
@@ -220,7 +237,7 @@ class GTSAM_EXPORT ImuFactorT: public NoiseModelFactorN<Pose3, Vector3, Pose3, V
    */
   ImuFactorT(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
       const PIM& preintegratedMeasurements)
-      : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov()),
+      : Base(noiseModel::Gaussian::Covariance(imuFactorResidualCov(preintegratedMeasurements)),
              pose_i, vel_i, pose_j, vel_j, bias),
         pim_(preintegratedMeasurements) {}
 
@@ -378,7 +395,7 @@ class GTSAM_EXPORT ImuFactor2T : public NoiseModelFactorN<NavState, NavState, im
    */
   ImuFactor2T(Key state_i, Key state_j, Key bias,
              const PIM& preintegratedMeasurements)
-      : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov()),
+      : Base(noiseModel::Gaussian::Covariance(imuFactorResidualCov(preintegratedMeasurements)),
              state_i, state_j, bias),
         pim_(preintegratedMeasurements) {}
 

gtsam/navigation/ManifoldPreintegration.cpp

@@ -56,6 +56,90 @@ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
       && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
 }
 
+NavState ManifoldPreintegration::UpdatePreintegrated(
+    const Eigen::Vector3d& a_body,
+    const Eigen::Vector3d& w_body,
+    double dt,
+    const NavState& X,                       // (R,p,v)
+    gtsam::OptionalJacobian<9,9> A_t,        // dXt_{k+1} / dXt_k
+    gtsam::OptionalJacobian<9,3> B_t,        // dXt_{k+1} / d a_body
+    gtsam::OptionalJacobian<9,3> C_t) {      // dXt_{k+1} / d w_body
+
+  // NavState right perturbation dXn on the NavState X_ij's manifold:
+  //   dXn = [delta_phi_ij, delta_p_ij, delta_v_ij] where
+  //     R_ij  = R̂_ij * Exp(delta_phi_ij)
+  //     p_ij  = p̂_ij + R_ij * delta_p_ij    (body-frame additive)
+  //     v_ij  = v̂_ij + R_ij * delta_v_ij    (body-frame additive)
+  // The preintegration error state dXt uses navigation-frame additive p_ij, v_ij:
+  //   dXt = [delta_phi_ij, delta_p_ij, delta_v_ij] where
+  //     R_ij  = R̂_ij * Exp(delta_phi_ij)
+  //     p_ij  = p̂_ij + delta_p_ij            (navigation-frame additive)
+  //     v_ij  = v̂_ij + delta_v_ij            (navigation-frame additive)
+  // To propagate X_ij's covariance we need A_t in the dXt convention, not A_n
+  // from NavState.update() in the dXn convention.  The relation is:
+  //   A_t = (dXt_j/dXn_j) * A_n * (dXn_{j-1}/dXt_{j-1})
+  // A_t, B_t, C_t below are computed directly in the dXt convention.
+
+  const double dt2  = dt * dt;
+  const double dt22 = 0.5 * dt2;
+
+  const gtsam::Rot3& R = X.rotation();
+  const Eigen::Matrix3d Rm = R.matrix();
+
+  // Rotation increment
+  const Eigen::Vector3d phi = w_body * dt;
+
+  Eigen::Matrix3d Dexp;  // not used directly here, but cheap to compute
+  const gtsam::Rot3 dR = gtsam::Rot3::Expmap(phi, (A_t || C_t) ? &Dexp : nullptr);
+  const gtsam::Rot3 Rn = R.compose(dR);
+
+  // a_nav uses OLD R (matching typical preintegration / NavState.update structure)
+  const Eigen::Vector3d a_nav = R.rotate(a_body);
+
+  const gtsam::Point3  pn = X.position() + X.v() * dt + a_nav * dt22;
+  const Eigen::Vector3d vn = X.v() + a_nav * dt;
+
+  NavState Xn(Rn, pn, vn);
+
+  if (A_t) {
+    A_t->setZero();
+
+    // dphi+ = dR^T * dphi  (right-perturbation transport)
+    A_t->block<3,3>(0,0) = dR.transpose().matrix();
+
+    // dp+ = dp + dt dv + (d/dphi)(0.5*R*a*dt^2)*dphi
+    // dv+ = dv            + (d/dphi)(R*a*dt)*dphi
+    //
+    // Right perturbation: R Exp(dphi) a ≈ R (I + [dphi]x) a
+    // so δ(R a) = R (dphi x a) = - R [a]x dphi
+    const Eigen::Matrix3d a_skew = skewSymmetric(a_body);
+    A_t->block<3,3>(3,0) = -Rm * a_skew * dt22;
+    A_t->block<3,3>(6,0) = -Rm * a_skew * dt;
+
+    // world-additive p,v parts
+    A_t->block<3,3>(3,3) = Eigen::Matrix3d::Identity();
+    A_t->block<3,3>(3,6) = Eigen::Matrix3d::Identity() * dt;
+    A_t->block<3,3>(6,6) = Eigen::Matrix3d::Identity();
+  }
+
+  if (B_t) {
+    B_t->setZero();
+    // dp+ / da = 0.5 * R * dt^2, dv+ / da = R * dt
+    B_t->block<3,3>(3,0) = Rm * dt22;
+    B_t->block<3,3>(6,0) = Rm * dt;
+  }
+
+  if (C_t) {
+    C_t->setZero();
+    // dphi+ / dw = invJr(phi) * dt
+    so3::DexpFunctor local(phi);
+    const Eigen::Matrix3d invJr = local.InvJacobian().right();
+    C_t->block<3,3>(0,0) = invJr * dt;
+  }
+
+  return Xn;
+}
+
 //------------------------------------------------------------------------------
 void ManifoldPreintegration::update(const Vector3& measuredAcc,
     const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
@@ -78,7 +162,7 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc,
 
   // Do update
   deltaTij_ += dt;
-  deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
+  deltaXij_ = UpdatePreintegrated(acc, omega, dt, deltaXij_, A, B, C);
 
   if (p().body_P_sensor) {
     // More complicated derivatives in case of non-trivial sensor pose

gtsam/navigation/ManifoldPreintegration.h

@@ -92,6 +92,19 @@ class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
   /// @name Main functionality
   /// @{
 
+  // Error chart dXt:
+  //   R = Rhat * Exp(dphi)     (right perturbation)
+  //   p = phat + dp            (world additive)
+  //   v = vhat + dv            (world additive)
+  static NavState UpdatePreintegrated(
+      const Eigen::Vector3d& a_body,
+      const Eigen::Vector3d& w_body,
+      double dt,
+      const NavState& X,                          // (R,p,v)
+      gtsam::OptionalJacobian<9,9> A = {},        // dXt_{k+1} / dXt_k
+      gtsam::OptionalJacobian<9,3> B = {},        // dXt_{k+1} / d a_body
+      gtsam::OptionalJacobian<9,3> C = {});       // dXt_{k+1} / d w_body
+
   /// Update preintegrated measurements and get derivatives
   /// It takes measured quantities in the j frame
   /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose

gtsam/navigation/PreintegrationBase.cpp

@@ -152,15 +152,64 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i,
   NavState predictedState_j = predict(
       state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0);
 
-  // Calculate error
-  Matrix9 D_error_state_j, D_error_predict;
-  Vector9 error =
-      state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
-                               H1 || H3 ? &D_error_predict : 0);
-
-  if (H1) *H1 << D_error_predict * D_predict_state_i;
-  if (H2) *H2 << D_error_state_j;
-  if (H3) *H3 << D_error_predict * D_predict_bias_i;
+  // Rotation residual: r_R = Log(R_j^{-1} R_pred).
+  Matrix3 D_Rrel_Rj, D_Rrel_Rpred, D_rR_Rrel;
+  const Rot3 Rrel = state_j.attitude().between(predictedState_j.attitude(),
+      H2 ? &D_Rrel_Rj : nullptr, (H1 || H3) ? &D_Rrel_Rpred : nullptr);
+  const Vector3 r_R = Rot3::Logmap(Rrel, (H1 || H2 || H3) ? &D_rR_Rrel : nullptr);
+
+  // Position and velocity residuals in body frame of state_i.
+  // Using R_i^T makes the Jacobian of [r_p; r_v] w.r.t. preint error state dXt
+  // equal to identity, so preintMeasCov_ applies directly as residual covariance.
+  const Matrix3 Ri_T = state_i.R().transpose();
+  const Vector3 dp = predictedState_j.t() - state_j.t();
+  const Vector3 dv = predictedState_j.v() - state_j.v();
+  const Vector3 r_p = Ri_T * dp;
+  const Vector3 r_v = Ri_T * dv;
+
+  Vector9 error;
+  error << r_R, r_p, r_v;
+
+  // NavState retract uses the body-frame (dXn) perturbation convention:
+  //   dXn = [delta_phi, delta_p, delta_v] where
+  //     R_i'  = R_i * Exp(delta_phi)
+  //     p_i'  = p_i + R_i * delta_p     (body-frame additive)
+  //     v_i'  = v_i + R_i * delta_v     (body-frame additive)
+  // Therefore D_predict_state_i and D_predict_bias_i have their position/velocity
+  // rows expressed in the body frame of predictedState_j.
+  if (H1) {
+    // r_R depends on R_pred only through D_predict_state_i.topRows<3>().
+    // D_predict_state_i's position/velocity rows are in the body frame of
+    // predictedState_j; convert to navigation frame via R_i^T * R_pred.
+    const Matrix3 D_rR_Rpred = D_rR_Rrel * D_Rrel_Rpred;
+    const Matrix3 RiT_Rpred = Ri_T * predictedState_j.R();
+    const Eigen::Matrix<double,3,9> H1_rot = D_rR_Rpred * D_predict_state_i.topRows<3>();
+    Eigen::Matrix<double,3,9> H1_pos, H1_vel;
+    H1_pos << skewSymmetric(r_p) + RiT_Rpred * D_predict_state_i.block<3,3>(3,0),
+               RiT_Rpred * D_predict_state_i.block<3,3>(3,3),
+               RiT_Rpred * D_predict_state_i.block<3,3>(3,6);
+    H1_vel << skewSymmetric(r_v) + RiT_Rpred * D_predict_state_i.block<3,3>(6,0),
+               RiT_Rpred * D_predict_state_i.block<3,3>(6,3),
+               RiT_Rpred * D_predict_state_i.block<3,3>(6,6);
+    *H1 << H1_rot, H1_pos, H1_vel;
+  }
+
+  if (H2) {
+    // r_R = Log(R_j^{-1} R_pred) depends on R_j; r_p, r_v depend on p_j, v_j via -R_i^T R_j.
+    const Matrix3 D_rR_Rj = D_rR_Rrel * D_Rrel_Rj;
+    const Matrix3 neg_RiT_Rj = -(Ri_T * state_j.R());
+    *H2 << D_rR_Rj,      Z_3x3,       Z_3x3,
+           Z_3x3,        neg_RiT_Rj,  Z_3x3,
+           Z_3x3,        Z_3x3,       neg_RiT_Rj;
+  }
+
+  if (H3) {
+    const Matrix3 D_rR_Rpred = D_rR_Rrel * D_Rrel_Rpred;
+    const Matrix3 RiT_Rpred = Ri_T * predictedState_j.R();
+    *H3 << D_rR_Rpred * D_predict_bias_i.topRows<3>(),
+           RiT_Rpred * D_predict_bias_i.middleRows<3>(3),
+           RiT_Rpred * D_predict_bias_i.middleRows<3>(6);
+  }
 
   return error;
 }

gtsam/navigation/tests/imuFactorTesting.h

@@ -19,6 +19,7 @@
 
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/ImuBias.h>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;
@@ -45,6 +46,128 @@ auto radians = [](double t) { return t * M_PI / 180; };
 [[maybe_unused]] static const double kGyroSigma =
     radians(0.5) / 60;                                        // 0.5 degree ARW
 [[maybe_unused]] static const double kAccelSigma = 0.1 / 60;  // 10 cm VRW
+
+// --------------------------- small helpers ---------------------------
+// Rotation near: angle of R0^{-1} R1 should be <= tol_rad.
+#define EXPECT_ROT3_NEAR(R0, R1, tol_rad)                                         \
+  do {                                                                            \
+    const gtsam::Vector3 _w = gtsam::Rot3::Logmap((R0).between((R1)));            \
+    const double _w_norm = _w.norm();                                             \
+    if (_w_norm > (tol_rad)) {                                                    \
+      std::cout << "EXPECT_ROT3_NEAR failed\n"                                    \
+                << "  w: [" << _w.transpose() << "]\n"                            \
+                << "  w_norm: " << _w_norm << " tol: " << (tol_rad) << "\n"       \
+                << "  R0:\n" << (R0).matrix() << "\n"                             \
+                << "  R1:\n" << (R1).matrix() << "\n";                            \
+    }                                                                             \
+    EXPECT(_w_norm <= (tol_rad));                                                 \
+  } while (0)
+
+// Robust-ish numeric compare for matrices, with abs+rel tolerance on Fro norm.
+// Passes if ||A-B||_F <= abs_tol + rel_tol * max(1, ||B||_F)
+#define EXPECT_MAT_NEAR(A, B, abs_tol, rel_tol)                                   \
+  do {                                                                            \
+    const auto& _A = (A);                                                         \
+    const auto& _B = (B);                                                         \
+    const double _bnorm = _B.norm();                                              \
+    const double _denom = std::max(1.0, _bnorm);                                  \
+    const double _err   = (_A - _B).norm();                                       \
+    const double _tol = (abs_tol) + (rel_tol) * _denom;                           \
+    if (_err > _tol) {                                                            \
+      std::cout << "EXPECT_MAT_NEAR failed\n"                                     \
+                << "  err: " << _err << " tol: " << _tol                          \
+                << " (abs: " << (abs_tol) << ", rel: " << (rel_tol) << ", bnorm: "\
+                << _bnorm << ")\n"                                                \
+                << "  A:\n" << _A << "\n"                                         \
+                << "  B:\n" << _B << "\n";                                        \
+    }                                                                             \
+    EXPECT(_err <= _tol);                                                         \
+  } while (0)
+
+struct ImuSimConfig {
+  double g = 9.81;
+  double dt = 0.005;
+
+  // Noise model used inside preintegration/ekf (white noise)
+  double sigma_g_c = 5e-2;
+  double sigma_a_c = 3e-2;
+  double sigma_gw_c = 7e-3;
+  double sigma_aw_c = 2e-3;
+
+  gtsam::Rot3  Rws0;
+  gtsam::Point3 pws0;
+  gtsam::Vector3 vws0;
+  gtsam::imuBias::ConstantBias bias;  // (acc, gyro)
+
+  // NAV9 covariance (order: [dtheta, dp, dv])
+  Eigen::Matrix<double,9,9> P0_nav9;
+
+  explicit ImuSimConfig(unsigned int seed = 0, bool zero_bw = true) {
+    std::mt19937 rng(seed);
+    std::normal_distribution<double> N01(0.0, 1.0);
+    auto n = [&](){ return N01(rng); };
+
+    const gtsam::Vector3 w0(0.2*n(), 0.2*n(), 0.2*n());
+    Rws0 = gtsam::Rot3::Expmap(w0);
+
+    pws0 = gtsam::Point3(n(), n(), n());
+    vws0 = gtsam::Vector3(n(), n(), n());
+
+    const gtsam::Vector3 ba0(0.05*n(), 0.05*n(), 0.05*n());
+    const gtsam::Vector3 bg0(0.01*n(), 0.01*n(), 0.01*n());
+    bias = gtsam::imuBias::ConstantBias(ba0, bg0);
+
+    P0_nav9.setZero();
+    P0_nav9.diagonal().segment<3>(0).setConstant(1.0);     // rot
+    P0_nav9.diagonal().segment<3>(3).setConstant(100.0);   // pos
+    P0_nav9.diagonal().segment<3>(6).setConstant(10.0);    // vel
+    if (zero_bw) {
+      sigma_gw_c = 0;
+      sigma_aw_c = 0;
+    }
+  }
+};
+
+inline std::shared_ptr<gtsam::PreintegrationParams> MakeParamsU(const ImuSimConfig& cfg) {
+  auto p = gtsam::PreintegrationParams::MakeSharedU(cfg.g);
+  const Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
+  double cov_g_c = cfg.sigma_g_c * cfg.sigma_g_c;
+  double cov_a_c = cfg.sigma_a_c * cfg.sigma_a_c;
+  p->gyroscopeCovariance     = cov_g_c * I;
+  p->accelerometerCovariance = cov_a_c * I;
+  p->gyroscopeCovariance(0, 0) *= 10000;
+  p->gyroscopeCovariance(1, 1) *= 100;
+  p->accelerometerCovariance(0, 0) *= 10000;
+  p->accelerometerCovariance(1, 1) *= 100;
+  p->integrationCovariance   = 1e-12 * I;
+  p->use2ndOrderCoriolis     = false;
+  return p;
+}
+
+struct ImuRawSample {
+  gtsam::Vector3 measuredOmega;
+  gtsam::Vector3 measuredAcc;
+  double dt;
+};
+
+inline std::vector<ImuRawSample> MakeRandomImuMeasurements(
+    const ImuSimConfig& cfg,
+    double T,
+    unsigned int seed) {
+  const int N = static_cast<int>(std::round(T / cfg.dt));
+  std::vector<ImuRawSample> out;
+  if (N <= 0) return out;
+  out.reserve(N);
+
+  const Vector3 acc_mean = Vector3(0.0, 0.0, cfg.g);
+  for (int k = 0; k < N; ++k) {
+    Eigen::Vector3d omega = Eigen::Vector3d::Random();  // range from [-1, 1]
+    Eigen::Vector3d acc = Eigen::Vector3d::Random();
+    acc += acc_mean;
+    out.push_back(ImuRawSample{omega, acc, cfg.dt});
+  }
+  return out;
+}
 }  // namespace
 
 namespace testing {