diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index a2eea3719..d33e0829b 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -97,6 +97,7 @@ namespace gtsam { void predict(const TangentVector& u, double dt, const Covariance& Q) { G U; if constexpr (std::is_same_v) { + // Specialize to Matrix case as its Expmap is not defined. const Matrix& X = static_cast(this->X_); U.resize(X.rows(), X.cols()); Eigen::Map(static_cast(U).data(), U.size()) = u * dt; diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index 4421ab660..007ed3ad0 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -93,21 +93,16 @@ namespace gtsam { Jacobian Df, Dexp; // Eigen::Matrix if constexpr (std::is_same_v) { - std::cout << "We are here" << std::endl; - Jacobian Df; // Eigen::Matrix - TangentVector xi = f(this->X_, Df); - const Matrix& X = static_cast(this->X_); - G U = Matrix(X.rows(), X.cols()); - Eigen::Map(static_cast(U).data(), U.size()) = xi * dt; + // Specialize to Matrix case as its Expmap is not defined. + TangentVector xi = f(this->X_, A ? &Df : nullptr); + const Matrix nextX = traits::Retract(this->X_, xi * dt, A ? &Dexp : nullptr); // just addition if (A) { const Matrix I_n = Matrix::Identity(this->n_, this->n_); - const Matrix& Dexp = I_n; *A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix } - return this->X_ + U; // Matrix addition + return nextX; } else { - Jacobian Df, Dexp; // Eigen::Matrix TangentVector xi = f(this->X_, A ? &Df : nullptr); // xi and Df = d(xi)/d(localX) G U = traits::Expmap(xi * dt, A ? &Dexp : nullptr); if (A) { diff --git a/gtsam/navigation/tests/testLieGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp index 62a35473d..f70511293 100644 --- a/gtsam/navigation/tests/testLieGroupEKF.cpp +++ b/gtsam/navigation/tests/testLieGroupEKF.cpp @@ -108,6 +108,94 @@ TEST(GroupeEKF, StateAndControl) { EXPECT(assert_equal(expectedH, actualH)); } +// Namespace for dynamic Matrix LieGroupEKF test +namespace exampleLieGroupDynamicMatrix { + // Constant tangent vector for dynamics (same as "velocityTangent" in IEKF test) + const Vector kFixedVelocityTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); + + // Dynamics function: xi = f(X, H_X) + // Returns a constant tangent vector, so Df_DX = 0. + // H_X is D(xi)/D(X_local), where X_local is the tangent space perturbation of X. + Vector f(const Matrix& X, OptionalJacobian H_X = {}) { + if (H_X) { + size_t state_dim = X.size(); + size_t tangent_dim = kFixedVelocityTangent.size(); + // Ensure Jacobian dimensions are consistent even if state or tangent is 0-dim + H_X->setZero(tangent_dim, state_dim); + } + return kFixedVelocityTangent; + } + + // Measurement function h(X, H_p) + // H_p is D(h)/D(X_local) + double h(const Matrix& p, OptionalJacobian<-1, -1> H_p = {}) { + if (p.size() == 0) { + if (H_p) H_p->resize(1, 0); // Jacobian dh/dX_local is 1x0 + return 0.0; // Define trace of empty matrix as 0 for consistency + } + // This test specifically uses 2x2 matrices (size 4) + if (H_p) { + H_p->resize(1, p.size()); // p.size() is 4 + (*H_p) << 1.0, 0.0, 0.0, 1.0; // d(trace)/d(p_flat) for p_flat = [p00,p10,p01,p11] + } + return p(0, 0) + p(1, 1); // trace + } +} // namespace exampleLieGroupDynamicMatrix + +TEST(LieGroupEKF_DynamicMatrix, PredictAndUpdate) { + // --- Setup --- + Matrix p0Matrix = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix p0Covariance = I_4x4 * 0.01; + double dt = 0.1; + Matrix process_noise_Q = I_4x4 * 0.001; + Matrix measurement_noise_R = Matrix::Identity(1, 1) * 0.005; + + LieGroupEKF ekf(p0Matrix, p0Covariance); + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + + // --- Predict --- + // ekf.predict takes f(X, H_X), dt, process_noise_Q + ekf.predict(exampleLieGroupDynamicMatrix::f, dt, process_noise_Q); + + // Verification for Predict + // For f, Df_DXk = 0 (Jacobian of xi w.r.t X_local is Zero). + // State transition Jacobian A = Ad_Uinv + Dexp * Df_DXk * dt. + // For Matrix (VectorSpace): Ad_Uinv = I, Dexp = I. + // So, A = I + I * 0 * dt = I. + // Covariance update: P_next = A * P_current * A.transpose() + Q = I * P_current * I + Q = P_current + Q. + Matrix pPredictedExpected = traits::Retract(p0Matrix, exampleLieGroupDynamicMatrix::kFixedVelocityTangent * dt); + Matrix pCovariancePredictedExpected = p0Covariance + process_noise_Q; + + EXPECT(assert_equal(pPredictedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pCovariancePredictedExpected, ekf.covariance(), 1e-9)); + + // --- Update --- + Matrix pStateBeforeUpdate = ekf.state(); + Matrix pCovarianceBeforeUpdate = ekf.covariance(); + + double zTrue = exampleLieGroupDynamicMatrix::h(pStateBeforeUpdate); + double zObserved = zTrue - 0.03; // Simulated measurement with some error + + ekf.update(exampleLieGroupDynamicMatrix::h, zObserved, measurement_noise_R); + + // Verification for Update (Manual Kalman Steps) + Matrix H_update(1, 4); // Measurement Jacobian: 1x4 for 2x2 matrix, trace measurement + double zPredictionManual = exampleLieGroupDynamicMatrix::h(pStateBeforeUpdate, H_update); + // Innovation: y_tangent = traits::Local(prediction, observation) + // For double (scalar), Local(A,B) is B-A. + double innovationY_tangent = zObserved - zPredictionManual; + Matrix S_innovation_cov = H_update * pCovarianceBeforeUpdate * H_update.transpose() + measurement_noise_R; + Matrix K_gain = pCovarianceBeforeUpdate * H_update.transpose() * S_innovation_cov.inverse(); + Vector deltaXiTangent = K_gain * innovationY_tangent; // Tangent space correction for Matrix state + Matrix pUpdatedExpected = traits::Retract(pStateBeforeUpdate, deltaXiTangent); + Matrix I_KH = I_4x4 - K_gain * H_update; // I_4x4 because state dimension is 4 + Matrix pUpdatedCovarianceExpected = I_KH * pCovarianceBeforeUpdate * I_KH.transpose() + K_gain * measurement_noise_R * K_gain.transpose(); + + EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);