From eacdf1c7fa6326ac7b98f5b96b02d13e7742dff2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 00:25:35 -0400 Subject: [PATCH] Now handle dynamic types --- gtsam/navigation/InvariantEKF.h | 31 ++-- gtsam/navigation/LieGroupEKF.h | 27 +-- gtsam/navigation/ManifoldEKF.h | 192 +++++++++++++++------ gtsam/navigation/tests/testManifoldEKF.cpp | 109 ++++++++++++ 4 files changed, 282 insertions(+), 77 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 82a3f8fec..d2c6c2cb8 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -25,6 +25,8 @@ #pragma once #include // Include the base class +#include // For traits (needed for AdjointMap, Expmap) + namespace gtsam { @@ -41,32 +43,41 @@ namespace gtsam { * * The state-dependent prediction methods from LieGroupEKF are hidden. * The update step remains the same as in ManifoldEKF/LieGroupEKF. + * Covariances (P, Q) are `Matrix`. */ template class InvariantEKF : public LieGroupEKF { public: using Base = LieGroupEKF; ///< Base class type using TangentVector = typename Base::TangentVector; ///< Tangent vector type - using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc. - using Jacobian = typename Base::Jacobian; ///< Jacobian matrix type specific to the group G + // Jacobian for group-specific operations like AdjointMap. + // Becomes Matrix if G has dynamic dimension. + using Jacobian = typename Base::Jacobian; - /// Constructor: forwards to LieGroupEKF constructor - InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {} + /** + * Constructor: forwards to LieGroupEKF constructor. + * @param X0 Initial state on Lie group G. + * @param P0 Initial covariance in the tangent space at X0 (must be Matrix). + */ + InvariantEKF(const G& X0, const Matrix& P0) : Base(X0, P0) {} + + // We hide state-dependent predict methods from LieGroupEKF by only providing the + // invariant predict methods below. /** * Predict step via group composition (Left-Invariant): * X_{k+1} = X_k * U * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q - * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to - * F = Ad_{U^{-1}} in the base class predict method. + * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. * * @param U Lie group element representing the motion increment. - * @param Q Process noise covariance in the tangent space (size nxn). + * @param Q Process noise covariance in the tangent space (must be Matrix, size n_ x n_). */ void predict(const G& U, const Matrix& Q) { this->X_ = this->X_.compose(U); // TODO(dellaert): traits::AdjointMap should exist - Jacobian A = traits::Inverse(U).AdjointMap(); + const Jacobian A = traits::Inverse(U).AdjointMap(); + // P_ is Matrix. A is Eigen::Matrix. Q is Matrix. this->P_ = A * this->P_ * A.transpose() + Q; } @@ -77,10 +88,10 @@ namespace gtsam { * * @param u Tangent space control vector. * @param dt Time interval. - * @param Q Process noise covariance matrix (size nxn). + * @param Q Process noise covariance matrix (Matrix, size n_ x n_). */ void predict(const TangentVector& u, double dt, const Matrix& Q) { - G U = traits::Expmap(u * dt); + const G U = traits::Expmap(u * dt); predict(U, Q); // Call the group composition predict } diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index fe16eaefe..47c715531 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -38,7 +38,7 @@ namespace gtsam { * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF * * @tparam G Lie group type providing group operations and Expmap/AdjointMap. - * Must satisfy LieGroup concept (`gtsam::IsLieGroup::value`). + * Must satisfy LieGroup concept (`IsLieGroup::value`). * * This filter specializes ManifoldEKF for Lie groups, offering predict methods * with state-dependent dynamics functions. @@ -48,13 +48,19 @@ namespace gtsam { class LieGroupEKF : public ManifoldEKF { public: using Base = ManifoldEKF; ///< Base class type - static constexpr int n = Base::n; ///< Group dimension (tangent space dimension) + // n is the tangent space dimension of G. If G::dimension is Eigen::Dynamic, n is Eigen::Dynamic. + static constexpr int n = traits::dimension; using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G - using MatrixN = typename Base::MatrixN; ///< Square matrix of size n for covariance and Jacobians - using Jacobian = Eigen::Matrix; ///< Jacobian matrix type specific to the group G + // Jacobian for group-specific operations like Adjoint, Expmap derivatives. + // Becomes Matrix if n is Eigen::Dynamic. + using Jacobian = Eigen::Matrix; - /// Constructor: initialize with state and covariance - LieGroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) { + /** + * Constructor: initialize with state and covariance. + * @param X0 Initial state on Lie group G. + * @param P0 Initial covariance in the tangent space at X0 (must be Matrix). + */ + LieGroupEKF(const G& X0, const Matrix& P0) : Base(X0, P0) { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); } @@ -62,6 +68,7 @@ namespace gtsam { * SFINAE check for correctly typed state-dependent dynamics function. * Signature: TangentVector f(const G& X, OptionalJacobian Df) * Df = d(xi)/d(local(X)) + * If n is Eigen::Dynamic, OptionalJacobian is OptionalJacobian. */ template using enable_if_dynamics = std::enable_if_t< @@ -79,7 +86,7 @@ namespace gtsam { * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor returning tangent vector xi and its Jacobian Df w.r.t. local(X). * @param dt Time step. - * @param A Optional pointer to store the computed state transition Jacobian A. + * @param A OptionalJacobian to store the computed state transition Jacobian A. * @return Predicted state X_{k+1}. */ template > @@ -99,14 +106,14 @@ namespace gtsam { /** * Predict step with state-dependent dynamics: - * Uses predictMean to compute X_{k+1} and F, then calls base predict. + * Uses predictMean to compute X_{k+1} and F, then updates covariance. * X_{k+1}, F = predictMean(f, dt) * P_{k+1} = F P_k F^T + Q * * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor. * @param dt Time step. - * @param Q Process noise covariance (size nxn). + * @param Q Process noise covariance (Matrix, size n_ x n_). */ template > void predict(Dynamics&& f, double dt, const Matrix& Q) { @@ -153,7 +160,7 @@ namespace gtsam { * @param f Dynamics functor. * @param u Control input. * @param dt Time step. - * @param Q Process noise covariance (size nxn). + * @param Q Process noise covariance (must be Matrix, size n_ x n_). */ template > void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index d5acbe0c3..68eaaa793 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -18,6 +18,9 @@ * differentiable manifold. It relies on the manifold's retract and * localCoordinates operations. * + * Works with manifolds M that may have fixed or dynamic tangent space dimensions. + * Covariances and Jacobians are handled as `Matrix` (dynamic-size Eigen matrices). + * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ @@ -27,9 +30,11 @@ #include #include #include -#include // Include for traits +#include // Include for traits and IsManifold #include +#include +#include #include namespace gtsam { @@ -39,41 +44,59 @@ namespace gtsam { * @brief Extended Kalman Filter on a generic manifold M * * @tparam M Manifold type providing: - * - static int dimension = tangent dimension - * - using TangentVector = Eigen::Vector... - * - A `retract(const TangentVector&)` method (member or static) - * - A `localCoordinates(const M&)` method (member or static) - * - `gtsam::traits` specialization must exist. + * - `traits` specialization must exist, defining + * `dimension` (compile-time or Eigen::Dynamic), + * `TangentVector`, `Retract`, and `LocalCoordinates`. + * If `dimension` is Eigen::Dynamic, `GetDimension(const M&)` + * must be provided by traits. * * This filter maintains a state X in the manifold M and covariance P in the - * tangent space at X. Prediction requires providing the predicted next state - * and the state transition Jacobian F. Updates apply a measurement function h - * and correct the state using the tangent space error. + * tangent space at X. The covariance P is always stored as a gtsam::Matrix. + * Prediction requires providing the predicted next state and the state transition Jacobian F. + * Updates apply a measurement function h and correct the state using the tangent space error. */ template class ManifoldEKF { public: - /// Manifold dimension (tangent space dimension) - static constexpr int n = traits::dimension; - - /// Tangent vector type for the manifold M + /// Tangent vector type for the manifold M, as defined by its traits. using TangentVector = typename traits::TangentVector; - /// Square matrix of size n for covariance and Jacobians - using MatrixN = Eigen::Matrix; + /** + * Constructor: initialize with state and covariance. + * @param X0 Initial state on manifold M. + * @param P0 Initial covariance in the tangent space at X0. Must be a square + * Matrix whose dimensions match the tangent space dimension of X0. + */ + ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0), P_(P0) { + static_assert(IsManifold::value, + "Template parameter M must be a GTSAM Manifold."); - /// Constructor: initialize with state and covariance - ManifoldEKF(const M& X0, const MatrixN& P0) : X_(X0), P_(P0) { - static_assert(IsManifold::value, "Template parameter M must be a GTSAM Manifold"); + // Determine tangent space dimension n_ at runtime. + if constexpr (traits::dimension == Eigen::Dynamic) { + // If M::dimension is dynamic, traits::GetDimension(M) must exist. + n_ = traits::GetDimension(X0); + } + else { + n_ = traits::dimension; + } + + // Validate dimensions of initial covariance P0. + if (P0.rows() != n_ || P0.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF: Initial covariance P0 dimensions (" + + std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) + + ") do not match state's tangent space dimension (" + + std::to_string(n_) + ")."); + } } - virtual ~ManifoldEKF() = default; // Add virtual destructor for base class + virtual ~ManifoldEKF() = default; - /// @return current state estimate + /// @return current state estimate on manifold M. const M& state() const { return X_; } - /// @return current covariance estimate - const MatrixN& covariance() const { return P_; } + /// @return current covariance estimate in the tangent space (always a Matrix). + const Matrix& covariance() const { return P_; } /** * Basic predict step: Updates state and covariance given the predicted @@ -83,11 +106,23 @@ namespace gtsam { * where F = d(local(X_{k+1})) / d(local(X_k)) is the Jacobian of the * state transition in local coordinates around X_k. * - * @param X_next The predicted state at time k+1. + * @param X_next The predicted state at time k+1 on manifold M. * @param F The state transition Jacobian (size nxn). * @param Q Process noise covariance matrix in the tangent space (size nxn). */ - void predict(const M& X_next, const MatrixN& F, const Matrix& Q) { + void predict(const M& X_next, const Matrix& F, const Matrix& Q) { + if (F.rows() != n_ || F.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::predict: Jacobian F dimensions (" + + std::to_string(F.rows()) + "x" + std::to_string(F.cols()) + + ") must be " + std::to_string(n_) + "x" + std::to_string(n_) + "."); + } + if (Q.rows() != n_ || Q.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::predict: Noise Q dimensions (" + + std::to_string(Q.rows()) + "x" + std::to_string(Q.cols()) + + ") must be " + std::to_string(n_) + "x" + std::to_string(n_) + "."); + } X_ = X_next; P_ = F * P_ * F.transpose() + Q; } @@ -96,7 +131,7 @@ namespace gtsam { * Measurement update: Corrects the state and covariance using a pre-calculated * predicted measurement and its Jacobian. * - * @tparam Measurement Type of the measurement vector (e.g., VectorN). + * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). * @param prediction Predicted measurement. * @param H Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X). * Its dimensions must be m x n. @@ -105,56 +140,99 @@ namespace gtsam { */ template void update(const Measurement& prediction, - const Eigen::Matrix::dimension, n>& H, - const Measurement& z, const Matrix& R) { - // Innovation z \ominus prediction - Vector innovation = traits::Local(z, prediction); + const Matrix& H, + const Measurement& z, + const Matrix& R) { - // Innovation covariance and Kalman Gain - auto S = H * P_ * H.transpose() + R; // S is m x m - Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m) + static_assert(IsManifold::value, + "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates."); - // Correction vector in tangent space - TangentVector delta_xi = -K * innovation; // delta_xi is n x 1 + int m; // Measurement dimension + if constexpr (traits::dimension == Eigen::Dynamic) { + m = traits::GetDimension(z); + if (traits::GetDimension(prediction) != m) { + throw std::invalid_argument( + "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions."); + } + } + else { + m = traits::dimension; + } - // Update state using retract + if (H.rows() != m || H.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::update: Jacobian H dimensions (" + + std::to_string(H.rows()) + "x" + std::to_string(H.cols()) + + ") must be " + std::to_string(m) + "x" + std::to_string(n_) + "."); + } + if (R.rows() != m || R.cols() != m) { + throw std::invalid_argument( + "ManifoldEKF::update: Noise R dimensions (" + + std::to_string(R.rows()) + "x" + std::to_string(R.cols()) + + ") must be " + std::to_string(m) + "x" + std::to_string(m) + "."); + } + + // Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z) + // This is `log(prediction.inverse() * z)` if Measurement is a Lie group. + typename traits::TangentVector innovation = + traits::Local(prediction, z); + + // Innovation covariance: S = H P H^T + R + const Matrix S = H * P_ * H.transpose() + R; // S is m x m + + // Kalman Gain: K = P H^T S^-1 + const Matrix K = P_ * H.transpose() * S.inverse(); // K is n_ x m + + // Correction vector in tangent space of M: delta_xi = K * innovation + const TangentVector delta_xi = K * innovation; // delta_xi is n_ x 1 + + // Update state using retract: X_new = retract(X_old, delta_xi) X_ = traits::Retract(X_, delta_xi); // Update covariance using Joseph form for numerical stability - MatrixN I_KH = I_n - K * H; // I_KH is n x n + const auto I_n = Matrix::Identity(n_, n_); + const Matrix I_KH = I_n - K * H; // I_KH is n x n P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); } /** - * Measurement update: Corrects the state and covariance using a measurement. + * Measurement update: Corrects the state and covariance using a measurement + * model function. * - * @tparam Measurement Type of the measurement vector (e.g., VectorN) - * @tparam Prediction Functor signature: Measurement h(const M&, OptionalJacobian&) - * @param h Measurement model functor returning predicted measurement prediction - * and its Jacobian H = d(h)/d(local(X)). + * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). + * @tparam MeasurementFunction Functor/lambda with signature compatible with: + * `Measurement h(const M& x, Jac& H_jacobian)` + * where `Jac` can be `Matrix&` or `OptionalJacobian&`. + * The Jacobian H should be d(h)/d(local(X)). + * @param h Measurement model function. * @param z Observed measurement. - * @param R Measurement noise covariance (size m x m). + * @param R Measurement noise covariance (must be an m x m Matrix). */ - template - void update(Prediction&& h, const Measurement& z, const Matrix& R) { - constexpr int m = traits::dimension; + template + void update(MeasurementFunction&& h, const Measurement& z, const Matrix& R) { + static_assert(IsManifold::value, + "Template parameter Measurement must be a GTSAM Manifold."); + + int m; // Measurement dimension + if constexpr (traits::dimension == Eigen::Dynamic) { + m = traits::GetDimension(z); + } + else { + m = traits::dimension; + } + // Predict measurement and get Jacobian H = dh/dlocal(X) - Eigen::Matrix H; + Matrix H(m, n_); Measurement prediction = h(X_, H); - update(prediction, H, z, R); // Call the other update function + + // Call the other update function + update(prediction, H, z, R); } protected: - M X_; ///< manifold state estimate - MatrixN P_; ///< covariance in tangent space at X_ - - private: - /// Identity matrix of size n - static const MatrixN I_n; + M X_; ///< Manifold state estimate. + Matrix P_; ///< Covariance in tangent space at X_ (always a dynamic Matrix). + int n_; ///< Tangent space dimension of M, determined at construction. }; - // Define static identity I_n - template - const typename ManifoldEKF::MatrixN ManifoldEKF::I_n = ManifoldEKF::MatrixN::Identity(); - } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 1d74bb704..50730a8d4 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -147,6 +147,115 @@ TEST(ManifoldEKF_Unit3, Update) { EXPECT(assert_equal(P_updated_expected, ekf.covariance(), 1e-8)); } +// Define simple dynamics and measurement for a 2x2 Matrix state +namespace exampleDynamicMatrix { + + // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. + Matrix predictNextState(const Matrix& p, const Vector& vTangent, double dt) { + return traits::Retract(p, vTangent * dt); + } + + // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) + Vector1 measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + if (H) { + // p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11] + // trace = p(0,0) + p(1,1) + // H = d(trace)/d(p_flat) = [1, 0, 0, 1] + // The Jacobian H will be 1x4 for a 2x2 matrix. + H->resize(1, 4); + *H << 1.0, 0.0, 0.0, 1.0; + } + return Vector1(p(0, 0) + p(1, 1)); + } + +} // namespace exampleDynamicMatrix + +// Test fixture for ManifoldEKF with a 2x2 Matrix state +struct DynamicMatrixEKFTest { + Matrix p0Matrix; // Initial state (as 2x2 Matrix) + Matrix p0Covariance; // Initial covariance (dynamic Matrix, 4x4) + Vector velocityTangent; // Control input in tangent space (Vector4 for 2x2 matrix) + double dt; + Matrix processNoiseCovariance; // Process noise covariance (dynamic Matrix, 4x4) + Matrix measurementNoiseCovariance; // Measurement noise covariance (dynamic Matrix, 1x1) + + DynamicMatrixEKFTest() : + p0Matrix((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()), + p0Covariance(I_4x4 * 0.01), + velocityTangent((Vector(4) << 0.5, 0.1, -0.1, -0.5).finished()), // [dp00, dp10, dp01, dp11]/sec + dt(0.1), + processNoiseCovariance(I_4x4 * 0.001), + measurementNoiseCovariance(Matrix::Identity(1, 1) * 0.005) + { + } +}; + + +TEST(ManifoldEKF_DynamicMatrix, Predict) { + DynamicMatrixEKFTest data; + + ManifoldEKF ekf(data.p0Matrix, data.p0Covariance); + // For a 2x2 Matrix, tangent space dimension is 2*2=4. + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(data.p0Matrix.rows() * data.p0Matrix.cols(), ekf.state().size()); + + // --- Prepare inputs for ManifoldEKF::predict --- + Matrix pNextExpected = exampleDynamicMatrix::predictNextState(data.p0Matrix, data.velocityTangent, data.dt); + + // For this linear prediction model (p_next = p_current + V*dt in tangent space), F is Identity. + Matrix jacobianF = I_4x4; // Jacobian of the state transition function + + // --- Perform EKF prediction --- + ekf.predict(pNextExpected, jacobianF, data.processNoiseCovariance); + + // --- Verification --- + EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); + Matrix pCovarianceExpected = jacobianF * data.p0Covariance * jacobianF.transpose() + data.processNoiseCovariance; + EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); +} + +TEST(ManifoldEKF_DynamicMatrix, Update) { + DynamicMatrixEKFTest data; + + Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); + Matrix pStartCovariance = I_4x4 * 0.02; + ManifoldEKF ekf(pStartMatrix, pStartCovariance); + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + + // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) + Vector1 zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here + Vector1 zObserved = zTrue - Vector1(0.03); // Add some "error" + + // --- Perform EKF update --- + ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); + + // --- Verification (Manual Kalman Update Steps) --- + // 1. Predict measurement and get Jacobian H + Matrix H(1, 4); // This will be 1x4 for a 2x2 matrix measurement + Vector1 zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H); + + // 2. Innovation and Innovation Covariance + // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) + // For Vector1 (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + Vector1 innovationY = zObserved - zPredictionManual; + Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance; + + // 3. Kalman Gain K + Matrix kalmanGainK = pStartCovariance * H.transpose() * innovationCovarianceS.inverse(); // K is 4x1 + + // 4. State Correction (in tangent space of Matrix) + Vector deltaXiTangent = kalmanGainK * innovationY; // deltaXi is 4x1 Vector + + // 5. Expected Updated State and Covariance + Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); + + // Covariance update: P_new = (I - K H) P_old + Matrix pUpdatedCovarianceExpected = (I_4x4 - kalmanGainK * H) * pStartCovariance; + + // --- Compare EKF result with manual calculation --- + EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); +} int main() { TestResult tr;