diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index b6d2d791df..c2d747f60d 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -193,6 +193,11 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurementsT : public PreintegrationTyp // For backward compatibility: using PreintegratedCombinedMeasurements = PreintegratedCombinedMeasurementsT; +template +inline Eigen::Matrix 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) {} diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index eb90b477b5..594e718fa4 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -173,6 +173,23 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsT: public PreintegrationType { // controls which class PreintegratedImuMeasurements uses): using PreintegratedImuMeasurements = PreintegratedImuMeasurementsT; +/** + * 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 +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 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 diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 43228044ab..6efe5aacec 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/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 diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index dd86b829c5..b08b3f7784 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/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 H1_rot = D_rR_Rpred * D_predict_state_i.topRows<3>(); + Eigen::Matrix 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; } diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h index eaccdb6e04..e13fb3777b 100644 --- a/gtsam/navigation/tests/imuFactorTesting.h +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -19,6 +19,7 @@ #include #include +#include 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 P0_nav9; + + explicit ImuSimConfig(unsigned int seed = 0, bool zero_bw = true) { + std::mt19937 rng(seed); + std::normal_distribution 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 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 MakeRandomImuMeasurements( + const ImuSimConfig& cfg, + double T, + unsigned int seed) { + const int N = static_cast(std::round(T / cfg.dt)); + std::vector 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 { diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 55d806a999..567a3cc15f 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -313,6 +314,286 @@ TEST(CombinedImuFactor, Accelerating) { EXPECT(assert_equal(estimatedCov, expected, 0.1)); } +static inline std::shared_ptr +MakeParamsU_Combined(const ImuSimConfig& cfg) { + // Match MakeSharedU convention: Z-up nav frame => n_gravity = (0,0,-g) + auto p = std::make_shared( + gtsam::Vector3(0.0, 0.0, -cfg.g)); + + const gtsam::Matrix3 I = gtsam::Matrix3::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; + + // Turn off bias random walk so we can compare to 9D EKF (no bias state) + p->biasOmegaCovariance = gtsam::Matrix3::Zero(); + p->biasAccCovariance = gtsam::Matrix3::Zero(); + + p->use2ndOrderCoriolis = false; + return p; +} + +// -------------------------- Maps15Nav + propagation -------------------------- +struct Maps15Nav { + Eigen::Matrix F; // dx_e / dx_s + Eigen::Matrix G; // dx_e / dz (residual/transition def) + Eigen::Matrix covG; // dx_e / dz (covariance def) +}; + +static inline Maps15Nav BuildMaps15Nav_Manifold( + const Eigen::Matrix3d& dR, + const Eigen::Vector3d& dP, + const Eigen::Vector3d& dV, + double dt) { + + Maps15Nav m; + m.F.setZero(); + m.G.setZero(); + m.covG.setZero(); + + const Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); + const Eigen::Matrix3d A = dR.transpose(); + + // ---- F: NAV15 -> NAV15, error order [dtheta, dp, dv, dba, dbg] ---- + m.F.block<3,3>(0,0) = A; + + // dp_e = A*dp_s + dt*A*dv_s - A*skew(dP)*dtheta_s + m.F.block<3,3>(3,0) = -A * gtsam::skewSymmetric(dP); + m.F.block<3,3>(3,3) = A; + m.F.block<3,3>(3,6) = dt * A; + + // dv_e = A*dv_s - A*skew(dV)*dtheta_s + m.F.block<3,3>(6,0) = -A * gtsam::skewSymmetric(dV); + m.F.block<3,3>(6,6) = A; + + // biases constant in this comparison + m.F.block<3,3>(9,9) = I; // dba + m.F.block<3,3>(12,12) = I; // dbg + + // ---- G: residual/transition definition ---- + // z = [dtheta, dp, dv, dba, dbg] + // manifold correction: theta residual uses I (not A) + m.G.block<3,3>(0,0) = I; + m.G.block<3,3>(3,3) = A; + m.G.block<3,3>(6,6) = A; + m.G.block<3,3>(9,9) = -I; + m.G.block<3,3>(12,12) = -I; + + // ---- covG: covariance-error definition ---- + m.covG = m.G; + + return m; +} + +static inline Maps15Nav BuildMaps15Nav_Tangent( + const Eigen::Matrix3d& dR, + const Eigen::Vector3d& dP, + const Eigen::Vector3d& dV, + double dt) { + + Maps15Nav m; + m.F.setZero(); + m.G.setZero(); + m.covG.setZero(); + + const Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); + const Eigen::Matrix3d A = dR.transpose(); + + const gtsam::Vector3 phi = gtsam::Rot3::Logmap(gtsam::Rot3(dR)); + const Eigen::Matrix3d Jr = gtsam::so3::DexpFunctor(phi).rightJacobian(); + + // ---- F ---- + m.F.block<3,3>(0,0) = A; + + m.F.block<3,3>(3,0) = -A * gtsam::skewSymmetric(dP); + m.F.block<3,3>(3,3) = A; + m.F.block<3,3>(3,6) = dt * A; + + m.F.block<3,3>(6,0) = -A * gtsam::skewSymmetric(dV); + m.F.block<3,3>(6,6) = A; + + m.F.block<3,3>(9,9) = I; + m.F.block<3,3>(12,12) = I; + + // ---- G: z = [dphi, dp, dv, dba, dbg] ---- + m.G.block<3,3>(0,0) = Jr; + m.G.block<3,3>(3,3) = A; + m.G.block<3,3>(6,6) = A; + m.G.block<3,3>(9,9) = -I; + m.G.block<3,3>(12,12) = -I; + + // Tangent: covG == G + m.covG = m.G; + + return m; +} + +static inline Eigen::Matrix PropCov15Nav_Combined( + const Maps15Nav& m, + const Eigen::Matrix& Sigma_s, + const Eigen::Matrix& Sigma_z) { + + Eigen::Matrix Sigma_e; + Sigma_e.noalias() = m.F * Sigma_s * m.F.transpose(); + Sigma_e.noalias() += m.covG * Sigma_z * m.covG.transpose(); + return Sigma_e; +} + +template +static void RunCombinedVsEkf9D(TestResult& result_, const std::string& name_, + const ImuSimConfig& cfg, + const std::vector& meas, + BuildMapsFunc buildMaps, + const char* tag) { + using namespace gtsam; + std::cout << "\n--- " << tag << " ---\n"; + + const NavState s0(cfg.Rws0, cfg.pws0, cfg.vws0); + + auto ekfParams = MakeParamsU(cfg); + auto pimParams = MakeParamsU_Combined(cfg); + + // EKF (9D) + NavStateImuEKF ekf(s0, cfg.P0_nav9, ekfParams); + Eigen::Matrix Phi_ekf = Eigen::Matrix::Identity(); + + // Combined PIM (15D) + CombinedPimType pim(pimParams, cfg.bias); + + for (const auto& s : meas) { + // Combined integrates RAW + pim.integrateMeasurement(s.measuredAcc, s.measuredOmega, s.dt); + + // EKF integrates UNBIASED + const Vector3 omega_unb = s.measuredOmega - cfg.bias.gyroscope(); + const Vector3 acc_unb = s.measuredAcc - cfg.bias.accelerometer(); + + NavStateImuEKF::Jacobian A; + ekf.Dynamics(ekfParams->n_gravity, ekf.state(), omega_unb, acc_unb, s.dt, A); + Phi_ekf = A * Phi_ekf; + ekf.predict(omega_unb, acc_unb, s.dt); + } + + const NavState s_ekf = ekf.state(); + const Eigen::Matrix P_ekf = ekf.covariance(); + + // ------------------ (1) state compare ------------------ + const NavState s_pre = pim.predict(s0, cfg.bias); + std::cout << "Compare state (combined preint.predict vs EKF)\n"; + EXPECT_ROT3_NEAR(s_pre.attitude(), s_ekf.attitude(), 1e-3); + EXPECT_MAT_NEAR(s_pre.position(), s_ekf.position(), 1e-4, 1e-2); + EXPECT_MAT_NEAR(s_pre.velocity(), s_ekf.velocity(), 1e-4, 1e-2); + + // Preint increments + const Eigen::Matrix3d dR = pim.deltaRij().matrix(); + const Eigen::Vector3d dP = pim.deltaPij(); + const Eigen::Vector3d dV = pim.deltaVij(); + const double DT = pim.deltaTij(); + + const Maps15Nav maps15 = buildMaps(dR, dP, dV, DT); + + // ------------------ (2) transition compare ------------------ + std::cout << "Compare transition\n"; + const Eigen::Matrix F9 = maps15.F.topLeftCorner<9,9>(); + EXPECT_MAT_NEAR(Phi_ekf, F9, 1e-4, 2e-2); + + // ------------------ (3) covariance mapping sanity check ------------------ + // Qz15: covariance stored in pim + // Qr15: covariance in residual convention (CombinedImuFactor uses this) + const Eigen::Matrix Qz15 = pim.preintMeasCov(); + const Eigen::Matrix Qr15 = combinedImuFactorResidualCov(pim); + + // Compare: G * Qr * G' vs covG * Qz * covG' + const Eigen::Matrix GQrGt = maps15.G * Qr15 * maps15.G.transpose(); + const Eigen::Matrix covGQzGt = maps15.covG * Qz15 * maps15.covG.transpose(); + EXPECT_MAT_NEAR(GQrGt, covGQzGt, 1e-8, 1e-6); + + // ------------------ (4) covariance compare to EKF (top-left 9x9) ------------------ + Eigen::Matrix P0_nav15 = Eigen::Matrix::Zero(); + P0_nav15.topLeftCorner<9,9>() = cfg.P0_nav9; + + const Eigen::Matrix P15 = + PropCov15Nav_Combined(maps15, P0_nav15, Qz15); + + std::cout << "Compare covariance (EKF P vs propagated P15 top-left)\n"; + const Eigen::Matrix P9 = P15.topLeftCorner<9,9>(); + EXPECT_MAT_NEAR(P_ekf, P9, 1e-4, 2e-2); +} + +TEST(CombinedImuFactor, CheckCovTangent) { + const ImuSimConfig cfg(/*seed=*/0, /*zero_bw=*/true); + const double T = 10.0; + const auto meas = MakeRandomImuMeasurements(cfg, T, /*seed=*/1); + + using PimC_Tan = gtsam::PreintegratedCombinedMeasurementsT; + auto buildMaps = [](const Eigen::Matrix3d& dR, + const Eigen::Vector3d& dP, + const Eigen::Vector3d& dV, + double dt) { + return BuildMaps15Nav_Tangent(dR, dP, dV, dt); + }; + + RunCombinedVsEkf9D(result_, name_, cfg, meas, buildMaps, + "Tangent Combined (15D) vs EKF (9D)"); +} + +TEST(CombinedImuFactor, CheckCovManifold) { + const ImuSimConfig cfg(/*seed=*/0, /*zero_bw=*/true); + const double T = 10.0; + const auto meas = MakeRandomImuMeasurements(cfg, T, /*seed=*/1); + + using PimC_Man = gtsam::PreintegratedCombinedMeasurementsT; + auto buildMaps = [](const Eigen::Matrix3d& dR, + const Eigen::Vector3d& dP, + const Eigen::Vector3d& dV, + double dt) { + return BuildMaps15Nav_Manifold(dR, dP, dV, dt); + }; + + RunCombinedVsEkf9D(result_, name_, cfg, meas, buildMaps, + "Manifold Combined (15D) vs EKF (9D)"); +} + +TEST(ImuCombinedVsEKF, CheckResidualCov15D_TangentVsManifold) { + using namespace gtsam; + const unsigned int seed_cfg = 0; + const unsigned int seed_meas = 1; + + ImuSimConfig cfg(seed_cfg); + + auto pimParams = MakeParamsU_Combined(cfg); + + const double T = 10.0; // seconds + const auto meas = MakeRandomImuMeasurements(cfg, T, seed_meas); + + using PimC_Tan = PreintegratedCombinedMeasurementsT; + using PimC_Man = PreintegratedCombinedMeasurementsT; + + PimC_Tan pim_tan(pimParams, cfg.bias); + PimC_Man pim_man(pimParams, cfg.bias); + + for (const auto& s : meas) { + pim_tan.integrateMeasurement(s.measuredAcc, s.measuredOmega, s.dt); + pim_man.integrateMeasurement(s.measuredAcc, s.measuredOmega, s.dt); + } + + // Convert both to the *factor residual* covariance convention + const Eigen::Matrix Qr_tan = combinedImuFactorResidualCov(pim_tan); + const Eigen::Matrix Qr_man = combinedImuFactorResidualCov(pim_man); + + std::cout << "\n[CheckResidualCov15D] Compare tangent vs manifold residual covariance\n"; + // std::cout << "Tangent Qr (15x15):\n" << Qr_tan << "\n"; + // std::cout << "Manifold Qr (15x15):\n" << Qr_man << "\n"; + + EXPECT_MAT_NEAR(Qr_tan, Qr_man, 1e-4, 5e-3); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 4faf0570ea..19a3df6902 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -21,6 +21,7 @@ // #define ENABLE_TIMING // uncomment for timing results #include +#include #include #include #include @@ -877,6 +878,275 @@ TEST_PIM(ImuFactor, CheckCovariance) { EXPECT(assert_equal(expected, actual.preintMeasCov())); } +/* ---------------------------------------------------------------------------- + * Random IMU sim + Maps9 (tangent/manifold) + PIM vs EKF(9D) unit tests + * + * Goals: + * - Random-but-bounded IMU raw measurements + * - Compare: + * 1) state: pim.predict(s0,bias) vs ekf.state() + * 2) transition: jac_ekf vs Maps9.F + * 3) covariance: ekf.covariance vs F*P0*F' + covG*Qz*covG' + */ + +// --------------------------- Maps9 --------------------------- +// Jacobians for X_{e|s} = X_s ⊞ ΔX = f(x_s, z) +// x is NavState error in right-perturbation: [dtheta, dp, dv] +struct Maps9 { + Eigen::Matrix F; // df/dx_s (Nav ordering) + Eigen::Matrix G; // df/dz for "transition-noise mapping" + Eigen::Matrix covG; // df/dz for covariance injection (can differ from G!) + Eigen::Matrix G_inv; + Eigen::Matrix covG_inv; +}; + +// Manifold preintegration maps (NavState right-perturbation coords) +static inline Maps9 BuildMaps9_Manifold( + const Eigen::Matrix3d& /*Rws*/, + const Eigen::Matrix3d& dR, + const Eigen::Vector3d& dP, + const Eigen::Vector3d& dV, + double dt) { + Maps9 m; + m.F.setZero(); m.G.setZero(); m.covG.setZero(); + + const Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); + const Eigen::Matrix3d A = dR.transpose(); + + m.F.block<3,3>(0,0) = A; + m.F.block<3,3>(3,0) = -A * gtsam::skewSymmetric(dP); + m.F.block<3,3>(3,3) = A; + m.F.block<3,3>(3,6) = dt * A; + m.F.block<3,3>(6,0) = -A * gtsam::skewSymmetric(dV); + m.F.block<3,3>(6,6) = A; + + // G (transition-noise mapping; z=[dtheta,dp,dv] in "preint residual noise coords") + m.G.block<3,3>(0,0) = I; + m.G.block<3,3>(3,3) = A; + m.G.block<3,3>(6,6) = A; + + m.covG = m.G; + + m.G_inv = m.G.transpose(); // because blocks are I/A and A is orthonormal + m.covG_inv = m.covG; // Identity + return m; +} + +// Tangent preintegration maps (phi with right Jacobian Jr(phi)) +static inline Maps9 BuildMaps9_Tangent( + const Eigen::Matrix3d& /*Rws*/, + const Eigen::Matrix3d& dR, + const Eigen::Vector3d& dP, + const Eigen::Vector3d& dV, + double dt) { + Maps9 m; + m.F.setZero(); m.G.setZero(); m.covG.setZero(); + m.G_inv.setZero(); m.covG_inv.setZero(); + + const Eigen::Matrix3d A = dR.transpose(); + + // phi = Log(dR) + const gtsam::Vector3 phi = gtsam::Rot3::Logmap(gtsam::Rot3(dR)); + const Eigen::Matrix3d Jr = gtsam::so3::DexpFunctor(phi).rightJacobian(); + const Eigen::Matrix3d Jr_inv = gtsam::so3::DexpFunctor(phi).InvJacobian().right(); + + m.F.block<3,3>(0,0) = A; + m.F.block<3,3>(3,0) = -A * gtsam::skewSymmetric(dP); + m.F.block<3,3>(3,3) = A; + m.F.block<3,3>(3,6) = dt * A; + m.F.block<3,3>(6,0) = -A * gtsam::skewSymmetric(dV); + m.F.block<3,3>(6,6) = A; + + // G (transition-noise mapping uses phi-noise => Jr) + m.G.block<3,3>(0,0) = Jr; + m.G.block<3,3>(3,3) = A; + m.G.block<3,3>(6,6) = A; + + // In tangent case, covariance injection matches G + m.covG = m.G; + + // Inverses + m.G_inv.block<3,3>(0,0) = Jr_inv; + m.G_inv.block<3,3>(3,3) = A.transpose(); + m.G_inv.block<3,3>(6,6) = A.transpose(); + + m.covG_inv = m.G_inv; + return m; +} + +// --------------------------- core integration: PIM + EKF --------------------------- +template +static void IntegratePimAndEkf9D( + const ImuSimConfig& cfg, + const std::shared_ptr& params, + const std::vector& meas, + PimType* pim, + gtsam::NavStateImuEKF* ekf, + Eigen::Matrix* jac_ekf_out) { + + using namespace gtsam; + jac_ekf_out->setIdentity(); + + const Vector3 ba = cfg.bias.accelerometer(); + const Vector3 bg = cfg.bias.gyroscope(); + + for (const auto& s : meas) { + const Vector3 omega_m = s.measuredOmega; + const Vector3 acc_m = s.measuredAcc; + const double dt = s.dt; + + // PIM integrates RAW + pim->integrateMeasurement(acc_m, omega_m, dt); + + // EKF integrates UNBIASED + const Vector3 omega = omega_m - bg; + const Vector3 acc = acc_m - ba; + + NavStateImuEKF::Jacobian A; + ekf->Dynamics(params->n_gravity, ekf->state(), omega, acc, dt, A); + (*jac_ekf_out) = A * (*jac_ekf_out); + ekf->predict(omega, acc, dt); + } +} + +static void RunEkfByPreint9D( + TestResult& result_, + const std::string& name_, + bool use_tangent, + const char* tag, + unsigned int seed_cfg, + unsigned int seed_meas) { + + using namespace gtsam; + std::cout << "\n--- " << tag << " ---\n"; + + ImuSimConfig cfg(seed_cfg); + + const NavState s0(cfg.Rws0, cfg.pws0, cfg.vws0); + auto params = MakeParamsU(cfg); + + using PimTan = PreintegratedImuMeasurementsT; + using PimMan = PreintegratedImuMeasurementsT; + + const double T = 10.0; // seconds + const auto meas = MakeRandomImuMeasurements(cfg, T, seed_meas); + + Eigen::Matrix jac_ekf = Eigen::Matrix::Identity(); + NavStateImuEKF ekf(s0, cfg.P0_nav9, params); + + if (use_tangent) { + PimTan pim(params, cfg.bias); + IntegratePimAndEkf9D(cfg, params, meas, &pim, &ekf, &jac_ekf); + + const NavState s_pre = pim.predict(s0, cfg.bias); + const NavState s_ekf = ekf.state(); + + EXPECT_MAT_NEAR(s_pre.position(), s_ekf.position(), 1e-3, 1e-2); + EXPECT_MAT_NEAR(s_pre.velocity(), s_ekf.velocity(), 1e-3, 1e-2); + EXPECT_ROT3_NEAR(s_pre.attitude(), s_ekf.attitude(), 5e-3); + + const Eigen::Matrix3d dR = pim.deltaRij().matrix(); + const Eigen::Vector3d dP = pim.deltaPij(); + const Eigen::Vector3d dV = pim.deltaVij(); + const double DT = pim.deltaTij(); + const Maps9 maps9 = BuildMaps9_Tangent(cfg.Rws0.matrix(), dR, dP, dV, DT); + + EXPECT_MAT_NEAR(jac_ekf, maps9.F, 1e-3, 2e-2); + + // Cov compare: P = F P0 F' + covG Qz covG' + const Eigen::Matrix Qz = pim.preintMeasCov(); + const Eigen::Matrix Qr = gtsam::imuFactorResidualCov(pim); + Eigen::Matrix cov_pre; + cov_pre.noalias() = maps9.F * cfg.P0_nav9 * maps9.F.transpose(); + Eigen::Matrix GQGt = maps9.G * Qr * maps9.G.transpose(); + Eigen::Matrix covGQGt = maps9.covG * Qz * maps9.covG.transpose(); + EXPECT_MAT_NEAR(GQGt, covGQGt, 1e-8, 1e-6); + cov_pre.noalias() += covGQGt; + + const Eigen::Matrix cov_ekf = ekf.covariance(); + EXPECT_MAT_NEAR(cov_ekf, cov_pre, 1e-3, 2e-2); + + } else { + PimMan pim(params, cfg.bias); + IntegratePimAndEkf9D(cfg, params, meas, &pim, &ekf, &jac_ekf); + + const NavState s_pre = pim.predict(s0, cfg.bias); + const NavState s_ekf = ekf.state(); + + EXPECT_MAT_NEAR(s_pre.position(), s_ekf.position(), 1e-3, 1e-2); + EXPECT_MAT_NEAR(s_pre.velocity(), s_ekf.velocity(), 1e-3, 1e-2); + EXPECT_ROT3_NEAR(s_pre.attitude(), s_ekf.attitude(), 5e-3); + + const Eigen::Matrix3d dR = pim.deltaRij().matrix(); + const Eigen::Vector3d dP = pim.deltaPij(); + const Eigen::Vector3d dV = pim.deltaVij(); + const double DT = pim.deltaTij(); + const Maps9 maps9 = BuildMaps9_Manifold(cfg.Rws0.matrix(), dR, dP, dV, DT); + + EXPECT_MAT_NEAR(jac_ekf, maps9.F, 1e-3, 2e-2); + + const Eigen::Matrix Qz = pim.preintMeasCov(); + const Eigen::Matrix Qr = gtsam::imuFactorResidualCov(pim); + Eigen::Matrix cov_pre; + cov_pre.noalias() = maps9.F * cfg.P0_nav9 * maps9.F.transpose(); + Eigen::Matrix GQGt = maps9.G * Qr * maps9.G.transpose(); + Eigen::Matrix covGQGt = maps9.covG * Qz * maps9.covG.transpose(); + EXPECT_MAT_NEAR(GQGt, covGQGt, 1e-8, 1e-6); + cov_pre.noalias() += covGQGt; + + const Eigen::Matrix cov_ekf = ekf.covariance(); + EXPECT_MAT_NEAR(cov_ekf, cov_pre, 1e-3, 2e-2); + } +} + +TEST(ImuFactor, CheckCovTangent) { + RunEkfByPreint9D(result_, name_, /*use_tangent=*/true, + "Tangent preintegration (9D)", + /*seed_cfg=*/0, + /*seed_meas=*/1); +} + +TEST(ImuFactor, CheckCovManifold) { + RunEkfByPreint9D(result_, name_, /*use_tangent=*/false, + "Manifold preintegration (9D)", + /*seed_cfg=*/0, + /*seed_meas=*/1); +} + +// ----------------------------------------------------------------------------- +// Check that the residual covariance derived from tangent/manifold preint +// is (almost) identical after applying imuFactorResidualCov() mapping. +// ----------------------------------------------------------------------------- +TEST(ImuFactor, CheckCov) { + using namespace gtsam; + const unsigned int seed_cfg = 0; + const unsigned int seed_meas = 1; + ImuSimConfig cfg(seed_cfg); + auto params = MakeParamsU(cfg); + + const double T = 10.0; // seconds + const auto meas = MakeRandomImuMeasurements(cfg, T, seed_meas); + + using PimTan = PreintegratedImuMeasurementsT; + using PimMan = PreintegratedImuMeasurementsT; + + PimTan pim_tan(params, cfg.bias); + PimMan pim_man(params, cfg.bias); + + for (const auto& s : meas) { + pim_tan.integrateMeasurement(s.measuredAcc, s.measuredOmega, s.dt); + pim_man.integrateMeasurement(s.measuredAcc, s.measuredOmega, s.dt); + } + + const Matrix9 Qr_tan = gtsam::imuFactorResidualCov(pim_tan); + const Matrix9 Qr_man = gtsam::imuFactorResidualCov(pim_man); + + // std::cout << "\n[ImuFactor.CheckCov] Qr_tan:\n" << Qr_tan << "\n"; + // std::cout << "\n[ImuFactor.CheckCov] Qr_man:\n" << Qr_man << "\n"; + // They should be extremely close (tune tolerances if needed) + EXPECT_MAT_NEAR(Qr_tan, Qr_man, 1e-4, 5e-3); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 5a3a15aa9c..945b541581 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -155,6 +155,139 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { H2.rightCols<3>())); } +static NavState UpdatePreintegratedSlow( + const Eigen::Vector3d& a_body, + const Eigen::Vector3d& w_body, + double dt, + const NavState& X, + gtsam::OptionalJacobian<9,9> A = {}, + gtsam::OptionalJacobian<9,3> B = {}, + gtsam::OptionalJacobian<9,3> C = {}) { + const Eigen::Matrix3d Rm = X.rotation().matrix(); + + Matrix9 D_dXn_dXt_jm1 = Matrix9::Identity(); + D_dXn_dXt_jm1.block<3,3>(3,3) = Rm.transpose(); + D_dXn_dXt_jm1.block<3,3>(6,6) = Rm.transpose(); + + Matrix9 An; + Matrix93 Bn, Cn; + + NavState Xn = X.update(a_body, w_body, dt, + A ? &An : nullptr, + B ? &Bn : nullptr, + C ? &Cn : nullptr); + + const Eigen::Matrix3d Rnm = Xn.rotation().matrix(); + + Matrix9 D_dXt_dXn_j = Matrix9::Identity(); + D_dXt_dXn_j.block<3,3>(3,3) = Rnm; + D_dXt_dXn_j.block<3,3>(6,6) = Rnm; + + if (A) *A = D_dXt_dXn_j * An * D_dXn_dXt_jm1; + if (B) *B = D_dXt_dXn_j * Bn; + if (C) *C = D_dXt_dXn_j * Cn; + + return Xn; +} + +TEST(ManifoldPreintegration, UpdatePreintegratedFastMatchesSlow) { + using namespace gtsam; + + const unsigned int seed_cfg = 0; + const unsigned int seed_meas = 1; + + ImuSimConfig cfg(seed_cfg); + auto params = MakeParamsU(cfg); + + const double T = 5.0; // seconds + const auto meas = MakeRandomImuMeasurements(cfg, T, seed_meas); + + NavState X_fast; + NavState X_slow; + + Matrix9 P_fast = Matrix9::Zero(); + Matrix9 P_slow = Matrix9::Zero(); + + const Matrix3 aCov = params->accelerometerCovariance; + const Matrix3 wCov = params->gyroscopeCovariance; + const Matrix3 iCov = params->integrationCovariance; + + ManifoldPreintegration mip(params, cfg.bias); + + for (const auto& s : meas) { + Matrix9 A_f; + Matrix93 B_f, C_f; + NavState Xn_fast = mip.UpdatePreintegrated( + s.measuredAcc, s.measuredOmega, s.dt, X_fast, &A_f, &B_f, &C_f); + + Matrix9 A_s; + Matrix93 B_s, C_s; + NavState Xn_slow = UpdatePreintegratedSlow( + s.measuredAcc, s.measuredOmega, s.dt, X_slow, &A_s, &B_s, &C_s); + + EXPECT(assert_equal(Xn_fast, Xn_slow, 1e-9)); + + EXPECT_MAT_NEAR(A_f, A_s, 1e-9, 1e-6); + EXPECT_MAT_NEAR(B_f, B_s, 1e-9, 1e-6); + EXPECT_MAT_NEAR(C_f, C_s, 1e-6, 1e-4); + + P_fast = A_f * P_fast * A_f.transpose(); + P_fast.noalias() += B_f * (aCov / s.dt) * B_f.transpose(); + P_fast.noalias() += C_f * (wCov / s.dt) * C_f.transpose(); + P_fast.block<3,3>(3,3).noalias() += iCov * s.dt; + + P_slow = A_s * P_slow * A_s.transpose(); + P_slow.noalias() += B_s * (aCov / s.dt) * B_s.transpose(); + P_slow.noalias() += C_s * (wCov / s.dt) * C_s.transpose(); + P_slow.block<3,3>(3,3).noalias() += iCov * s.dt; + + EXPECT_MAT_NEAR(P_fast, P_slow, 1e-5, 5e-3); + X_fast = Xn_fast; + X_slow = Xn_slow; + } +} + +// verify that B and C from update() are correct when body_P_sensor +// has both rotation and non-zero translation. The non-zero translation activates +// the cross-term (*C += *B * D_correctedAcc_omega). +TEST(ManifoldPreintegration, UpdateJacobiansWithSensorPose) { + const Pose3 body_P_sensor(Rot3::RzRyRx(0.1, 0.2, -0.15), Point3(0.1, -0.05, 0.2)); + auto params = testing::Params(); + params->body_P_sensor = body_P_sensor; + + const imuBias::ConstantBias zeroBias; + const Vector3 measuredAcc(0.3, -0.2, 9.91); + const Vector3 measuredOmega(0.05, -0.1, 0.15); + const double dt = 0.01; + + // Run one update step from the identity state + auto tangentAfterUpdate = [&](const Vector3& acc, const Vector3& omega) -> Vector9 { + ManifoldPreintegration pim(params, zeroBias); + Matrix9 A; Matrix93 B, C; + pim.update(acc, omega, dt, &A, &B, &C); + const NavState& Xn = pim.deltaXij(); + Vector9 xi; + xi.segment<3>(0) = Rot3::Logmap(Xn.attitude()); + xi.segment<3>(3) = Xn.position(); + xi.segment<3>(6) = Xn.velocity(); + return xi; + }; + + // Analytic Jacobians + ManifoldPreintegration pim(params, zeroBias); + Matrix9 A; Matrix93 B, C; + pim.update(measuredAcc, measuredOmega, dt, &A, &B, &C); + + // Numerical Jacobians + auto f_acc = [&](const Vector3& a) { return tangentAfterUpdate(a, measuredOmega); }; + auto f_omega = [&](const Vector3& w) { return tangentAfterUpdate(measuredAcc, w); }; + const Matrix93 B_num = numericalDerivative11(f_acc, measuredAcc, 1e-5); + const Matrix93 C_num = numericalDerivative11(f_omega, measuredOmega, 1e-5); + + EXPECT_MAT_NEAR(B, B_num, 1e-7, 3e-5); + EXPECT_MAT_NEAR(C, C_num, 1e-7, 3e-5); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/python/gtsam/examples/NavStateImuPimCovarianceComparison.ipynb b/python/gtsam/examples/NavStateImuPimCovarianceComparison.ipynb index f2c899148f..5c16f10080 100644 --- a/python/gtsam/examples/NavStateImuPimCovarianceComparison.ipynb +++ b/python/gtsam/examples/NavStateImuPimCovarianceComparison.ipynb @@ -56,6 +56,78 @@ { "cell_type": "code", "execution_count": 2, + "id": "031c3c82", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import gtsam\n", + "\n", + "def skew(v):\n", + " x, y, z = float(v[0]), float(v[1]), float(v[2])\n", + " return np.array([[0.0, -z, y],\n", + " [z, 0.0, -x],\n", + " [-y, x, 0.0]], dtype=float)\n", + "\n", + "def build_maps9_from_pim(pim, use_manifold):\n", + " \"\"\"\n", + " Build (F, covG) in NavState 9D ordering: [dtheta, dp, dv]\n", + " - F maps start error -> end error in NAV right-perturbation coords.\n", + " - covG maps preint covariance noise -> end error, for covariance propagation.\n", + " \n", + " Notes:\n", + " - For Tangent preintegration: covG == G, rotation uses J_r(phi).\n", + " - For Manifold preintegration: based on our discussion, covG differs;\n", + " here we use covG = I9 (covariance-error definition differs),\n", + " while F remains the same transport Jacobian.\n", + " \"\"\"\n", + " dR = np.array(pim.deltaRij().matrix(), dtype=float) # 3x3\n", + " dP = np.array(pim.deltaPij(), dtype=float).reshape(3) # 3\n", + " dV = np.array(pim.deltaVij(), dtype=float).reshape(3) # 3\n", + " dt = float(pim.deltaTij())\n", + "\n", + " A = dR.T # A = R_e^T R_s = dR^T in NavState formulation\n", + "\n", + " F = np.zeros((9, 9), dtype=float)\n", + " # dtheta_e = A * dtheta_s\n", + " F[0:3, 0:3] = A\n", + "\n", + " # dp_e = A*dp_s + dt*A*dv_s - A*skew(dP)*dtheta_s\n", + " F[3:6, 0:3] = -A @ skew(dP)\n", + " F[3:6, 3:6] = A\n", + " F[3:6, 6:9] = dt * A\n", + "\n", + " # dv_e = A*dv_s - A*skew(dV)*dtheta_s\n", + " F[6:9, 0:3] = -A @ skew(dV)\n", + " F[6:9, 6:9] = A\n", + "\n", + " if use_manifold:\n", + " covG = np.eye(9, dtype=float)\n", + " else:\n", + " # Tangent: z = [dphi, dp, dv], rotation noise maps through Jr(phi)\n", + " phi = np.array(gtsam.Rot3.Logmap(gtsam.Rot3(dR)), dtype=float).reshape(3)\n", + " Jr = np.array(gtsam.so3.DexpFunctor(phi).rightJacobian(), dtype=float)\n", + " covG = np.zeros((9, 9), dtype=float)\n", + " covG[0:3, 0:3] = Jr\n", + " covG[3:6, 3:6] = A\n", + " covG[6:9, 6:9] = A\n", + "\n", + " return F, covG\n", + "\n", + "def predict_cov_from_pim(pim, P0, use_manifold):\n", + " \"\"\"\n", + " Proper predicted covariance for the end-state error:\n", + " P = F P0 F^T + covG Sigma_z covG^T\n", + " where Sigma_z is pim.preintMeasCov().\n", + " \"\"\"\n", + " F, covG = build_maps9_from_pim(pim, use_manifold)\n", + " Sigma_z = np.array(pim.preintMeasCov(), dtype=float)\n", + " return F @ P0 @ F.T + covG @ Sigma_z @ covG.T\n" + ] + }, + { + "cell_type": "code", + "execution_count": 3, "id": "c29fbe5e", "metadata": {}, "outputs": [], @@ -138,18 +210,20 @@ " X_pred_manifold = pim_manifold.predict(X0, bias)\n", " X_pred_ekf = ekf.state()\n", "\n", + " # --- CONSISTENT ERROR for NEES (all in NavState localCoordinates) ---\n", + " e_default = np.array(X_true.localCoordinates(X_pred_default), dtype=float).reshape(9)\n", + " e_manifold = np.array(X_true.localCoordinates(X_pred_manifold), dtype=float).reshape(9)\n", + " e_ekf = np.array(X_true.localCoordinates(X_pred_ekf), dtype=float).reshape(9)\n", + "\n", + " # --- predicted covariance for PIM methods ---\n", + " P_default = predict_cov_from_pim(pim_default, P0, use_manifold=False)\n", + " P_manifold = predict_cov_from_pim(pim_manifold, P0, use_manifold=True)\n", + " P_ekf = np.array(ekf.covariance(), dtype=float)\n", + "\n", " trial = TrialResult(\n", - " default=ErrorCov(\n", - " error=delta_error(X0.localCoordinates(X_pred_default), delta_true, True),\n", - " covariance=P0 + pim_default.preintMeasCov(),\n", - " ),\n", - " manifold=ErrorCov(\n", - " error=delta_error(X0.localCoordinates(X_pred_manifold), delta_true, True),\n", - " covariance=P0 + pim_manifold.preintMeasCov(),\n", - " ),\n", - " ekf=ErrorCov(\n", - " error=X_true.localCoordinates(X_pred_ekf), covariance=ekf.covariance()\n", - " ),\n", + " default=ErrorCov(error=e_default, covariance=P_default),\n", + " manifold=ErrorCov(error=e_manifold, covariance=P_manifold),\n", + " ekf=ErrorCov(error=e_ekf, covariance=P_ekf),\n", " )\n", "\n", " trials.append(trial)\n", @@ -185,9 +259,19 @@ "the exact group error or a simple delta subtraction (controlled by `use_exact_delta_error`).\n" ] }, + { + "cell_type": "markdown", + "id": "e1f3f962", + "metadata": {}, + "source": [ + "I believe \"We transport P0 through the PIM prediction Jacobian H1 so the PIM covariance is expressed in the NavState tangent space before NEES\" \n", + "means to compute the Jac of the predicted state relative to the initial state and use the Jac to relate their covariances?\n", + "\"For PIM NEES we compute the error in **delta space anchored at X_i** \", I think we should compute the NEES as we do for EKF, of the error in the delta state anchored at predicted X_j?" + ] + }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 4, "id": "2455e2f2", "metadata": {}, "outputs": [], @@ -231,64 +315,68 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 5, "id": "a05fd96f", "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "[{'interval_s': np.float64(0.05),\n", - " 'nees_default_mean': 0.732541283415913,\n", - " 'nees_default_std': 0.45064446831420696,\n", - " 'nees_manifold_mean': 0.7325417437759313,\n", - " 'nees_manifold_std': 0.4506453732727786,\n", - " 'nees_ekf_mean': 8.578577369813082,\n", - " 'nees_ekf_std': 3.9847842910628835,\n", - " 'rel_cov_default_vs_manifold': 0.0001334517326373641,\n", - " 'rel_cov_default_vs_ekf': 0.23647551460143135,\n", - " 'rel_cov_manifold_vs_ekf': 0.23646635627849075,\n", - " 'rel_cov_default_vs_emp': 33.32910001244249,\n", - " 'rel_cov_ekf_vs_emp': 41.45994771378634},\n", - " {'interval_s': np.float64(0.1),\n", - " 'nees_default_mean': 2.787998817033879,\n", - " 'nees_default_std': 1.7413000172544384,\n", - " 'nees_manifold_mean': 2.7878594759967203,\n", - " 'nees_manifold_std': 1.7412603227374222,\n", - " 'nees_ekf_mean': 8.81305571909107,\n", - " 'nees_ekf_std': 3.855761694968445,\n", - " 'rel_cov_default_vs_manifold': 0.0024585853420875346,\n", - " 'rel_cov_default_vs_ekf': 0.5603329657944346,\n", - " 'rel_cov_manifold_vs_ekf': 0.5601135667787372,\n", - " 'rel_cov_default_vs_emp': 8.334023084040808,\n", - " 'rel_cov_ekf_vs_emp': 17.162980630477936},\n", - " {'interval_s': np.float64(0.15),\n", - " 'nees_default_mean': 6.682040247014302,\n", - " 'nees_default_std': 5.0592414606991065,\n", - " 'nees_manifold_mean': 6.6800602346612745,\n", - " 'nees_manifold_std': 5.059707972467027,\n", - " 'nees_ekf_mean': 8.927928820615273,\n", - " 'nees_ekf_std': 4.078586754442932,\n", - " 'rel_cov_default_vs_manifold': 0.01253932711487101,\n", - " 'rel_cov_default_vs_ekf': 0.7432679556418527,\n", - " 'rel_cov_manifold_vs_ekf': 0.7422565879812052,\n", - " 'rel_cov_default_vs_emp': 3.2092286129472383,\n", - " 'rel_cov_ekf_vs_emp': 11.234501032873606},\n", - " {'interval_s': np.float64(0.2),\n", - " 'nees_default_mean': 10.781014031215804,\n", - " 'nees_default_std': 6.9768829696487105,\n", - " 'nees_manifold_mean': 10.759646362466919,\n", - " 'nees_manifold_std': 6.966213923765401,\n", - " 'nees_ekf_mean': 8.775491964313076,\n", - " 'nees_ekf_std': 4.022079758983367,\n", - " 'rel_cov_default_vs_manifold': 0.03784550641194231,\n", - " 'rel_cov_default_vs_ekf': 0.8343955193736587,\n", - " 'rel_cov_manifold_vs_ekf': 0.8317971515524428,\n", - " 'rel_cov_default_vs_emp': 1.8216430889121278,\n", - " 'rel_cov_ekf_vs_emp': 10.24874899697862}]" + "[{'interval_s': 0.05,\n", + " 'nees_default_mean': 8.859514145788713,\n", + " 'nees_default_std': 4.178864231857551,\n", + " 'nees_manifold_mean': 8.859517138279198,\n", + " 'nees_manifold_std': 4.17885557992285,\n", + " 'nees_ekf_mean': 8.547189855218901,\n", + " 'nees_ekf_std': 3.9062396270539144,\n", + " 'rel_cov_default_vs_manifold': 4.434728431141062e-08,\n", + " 'rel_cov_default_vs_ekf': 0.003597738168121831,\n", + " 'rel_cov_manifold_vs_ekf': 0.0035977289113661593,\n", + " 'rel_cov_default_vs_emp': 0.17251785142162618,\n", + " 'rel_cov_manifold_vs_emp': 0.17251755916125913,\n", + " 'rel_cov_ekf_vs_emp': 0.17233261245332418},\n", + " {'interval_s': 0.1,\n", + " 'nees_default_mean': 9.48013094344514,\n", + " 'nees_default_std': 4.476872073450289,\n", + " 'nees_manifold_mean': 9.480106731496427,\n", + " 'nees_manifold_std': 4.476852616865027,\n", + " 'nees_ekf_mean': 8.79980626563015,\n", + " 'nees_ekf_std': 3.9491339793618603,\n", + " 'rel_cov_default_vs_manifold': 5.428900372487124e-07,\n", + " 'rel_cov_default_vs_ekf': 0.008730578422165718,\n", + " 'rel_cov_manifold_vs_ekf': 0.00873048372596397,\n", + " 'rel_cov_default_vs_emp': 0.21545137663959799,\n", + " 'rel_cov_manifold_vs_emp': 0.21545025389335284,\n", + " 'rel_cov_ekf_vs_emp': 0.21363458142491304},\n", + " {'interval_s': 0.15,\n", + " 'nees_default_mean': 10.695145626035606,\n", + " 'nees_default_std': 4.821510977447404,\n", + " 'nees_manifold_mean': 10.69509324751408,\n", + " 'nees_manifold_std': 4.8214917930005265,\n", + " 'nees_ekf_mean': 8.794596885664324,\n", + " 'nees_ekf_std': 4.008253905224135,\n", + " 'rel_cov_default_vs_manifold': 1.74908283245806e-06,\n", + " 'rel_cov_default_vs_ekf': 0.011652903892180443,\n", + " 'rel_cov_manifold_vs_ekf': 0.011652614412628135,\n", + " 'rel_cov_default_vs_emp': 0.183921795844851,\n", + " 'rel_cov_manifold_vs_emp': 0.183925435012162,\n", + " 'rel_cov_ekf_vs_emp': 0.18552176595879658},\n", + " {'interval_s': 0.2,\n", + " 'nees_default_mean': 11.488675034384217,\n", + " 'nees_default_std': 5.121658683057512,\n", + " 'nees_manifold_mean': 11.48836890281357,\n", + " 'nees_manifold_std': 5.121527141856117,\n", + " 'nees_ekf_mean': 8.738528556352357,\n", + " 'nees_ekf_std': 3.9523033222813986,\n", + " 'rel_cov_default_vs_manifold': 3.647193392410906e-06,\n", + " 'rel_cov_default_vs_ekf': 0.013128995457628705,\n", + " 'rel_cov_manifold_vs_ekf': 0.013128403837560308,\n", + " 'rel_cov_default_vs_emp': 0.14697249577923546,\n", + " 'rel_cov_manifold_vs_emp': 0.14697260655465696,\n", + " 'rel_cov_ekf_vs_emp': 0.14800616110769185}]" ] }, - "execution_count": 4, + "execution_count": 5, "metadata": {}, "output_type": "execute_result" } @@ -307,10 +395,12 @@ " cov_mean_manifold = np.mean([t.manifold.covariance for t in trials], axis=0)\n", " cov_mean_ekf = np.mean([t.ekf.covariance for t in trials], axis=0)\n", "\n", - " emp_cov = np.cov(np.stack([t.default.error for t in trials], axis=0).T, bias=True)\n", + " emp_cov_default = np.cov(np.stack([t.default.error for t in trials], axis=0).T, bias=True)\n", + " emp_cov_manifold = np.cov(np.stack([t.manifold.error for t in trials], axis=0).T, bias=True)\n", + " emp_cov_ekf = np.cov(np.stack([t.ekf.error for t in trials], axis=0).T, bias=True)\n", "\n", " results.append({\n", - " \"interval_s\": interval_s,\n", + " \"interval_s\": float(interval_s),\n", " \"nees_default_mean\": float(nees_default.mean()),\n", " \"nees_default_std\": float(nees_default.std(ddof=0)),\n", " \"nees_manifold_mean\": float(nees_manifold.mean()),\n", @@ -320,8 +410,9 @@ " \"rel_cov_default_vs_manifold\": rel_frobenius(cov_mean_default, cov_mean_manifold),\n", " \"rel_cov_default_vs_ekf\": rel_frobenius(cov_mean_default, cov_mean_ekf),\n", " \"rel_cov_manifold_vs_ekf\": rel_frobenius(cov_mean_manifold, cov_mean_ekf),\n", - " \"rel_cov_default_vs_emp\": rel_frobenius(cov_mean_default, emp_cov),\n", - " \"rel_cov_ekf_vs_emp\": rel_frobenius(cov_mean_ekf, emp_cov),\n", + " \"rel_cov_default_vs_emp\": rel_frobenius(cov_mean_default, emp_cov_default),\n", + " \"rel_cov_manifold_vs_emp\": rel_frobenius(cov_mean_manifold, emp_cov_manifold),\n", + " \"rel_cov_ekf_vs_emp\": rel_frobenius(cov_mean_ekf, emp_cov_ekf),\n", " })\n", "\n", "results\n" @@ -337,7 +428,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 6, "id": "49441732", "metadata": {}, "outputs": [ @@ -347,10 +438,10 @@ "text": [ "interval_s | nees_default_mean | nees_manifold_mean | nees_ekf_mean | rel_cov_default_vs_manifold | rel_cov_default_vs_ekf\n", "-------------------------------------------------------------------------------------------------------------------------------\n", - "0.0500 | 0.7325 | 0.7325 | 8.5786 | 0.0001 | 0.2365 \n", - "0.1000 | 2.7880 | 2.7879 | 8.8131 | 0.0025 | 0.5603 \n", - "0.1500 | 6.6820 | 6.6801 | 8.9279 | 0.0125 | 0.7433 \n", - "0.2000 | 10.7810 | 10.7596 | 8.7755 | 0.0378 | 0.8344 \n" + "0.0500 | 8.8595 | 8.8595 | 8.5472 | 0.0000 | 0.0036 \n", + "0.1000 | 9.4801 | 9.4801 | 8.7998 | 0.0000 | 0.0087 \n", + "0.1500 | 10.6951 | 10.6951 | 8.7946 | 0.0000 | 0.0117 \n", + "0.2000 | 11.4887 | 11.4884 | 8.7385 | 0.0000 | 0.0131 \n" ] } ], @@ -396,7 +487,7 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 7, "id": "731acc00", "metadata": {}, "outputs": [ @@ -418,10 +509,10 @@ 200 ], "y": [ - 0.732541283415913, - 2.787998817033879, - 6.682040247014302, - 10.781014031215804 + 8.859514145788713, + 9.48013094344514, + 10.695145626035606, + 11.488675034384217 ] }, { @@ -435,10 +526,10 @@ 200 ], "y": [ - 0.7325417437759313, - 2.7878594759967203, - 6.6800602346612745, - 10.759646362466919 + 8.859517138279198, + 9.480106731496427, + 10.69509324751408, + 11.48836890281357 ] }, { @@ -452,10 +543,10 @@ 200 ], "y": [ - 8.578577369813082, - 8.81305571909107, - 8.927928820615273, - 8.775491964313076 + 8.547189855218901, + 8.79980626563015, + 8.794596885664324, + 8.738528556352357 ] }, { @@ -1325,7 +1416,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 8, "id": "c3089888", "metadata": {}, "outputs": [ @@ -1347,10 +1438,10 @@ 200 ], "y": [ - 0.0001334517326373641, - 0.0024585853420875346, - 0.01253932711487101, - 0.03784550641194231 + 4.434728431141062e-08, + 5.428900372487124e-07, + 1.74908283245806e-06, + 3.647193392410906e-06 ] }, { @@ -1364,10 +1455,10 @@ 200 ], "y": [ - 0.23647551460143135, - 0.5603329657944346, - 0.7432679556418527, - 0.8343955193736587 + 0.003597738168121831, + 0.008730578422165718, + 0.011652903892180443, + 0.013128995457628705 ] }, { @@ -1381,10 +1472,10 @@ 200 ], "y": [ - 0.23646635627849075, - 0.5601135667787372, - 0.7422565879812052, - 0.8317971515524428 + 0.0035977289113661593, + 0.00873048372596397, + 0.011652614412628135, + 0.013128403837560308 ] } ],