From ac295ebcaca91ae673bdc0a1d46847a330d8df9e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 8 May 2025 23:33:14 -0400 Subject: [PATCH 01/10] Add explicit update --- gtsam/navigation/ManifoldEKF.h | 71 ++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 33 deletions(-) diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index 7951b88cb..d5acbe0c3 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -92,22 +92,45 @@ namespace gtsam { P_ = F * P_ * F.transpose() + Q; } + /** + * 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). + * @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. + * @param z Observed measurement. + * @param R Measurement noise covariance (size m x m). + */ + 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); + + // 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) + + // Correction vector in tangent space + TangentVector delta_xi = -K * innovation; // delta_xi is n x 1 + + // Update state using retract + 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 + P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); + } + /** * Measurement update: Corrects the state and covariance using a measurement. - * z_pred, H = h(X) - * y = z - z_pred (innovation, or z_pred - z depending on convention) - * S = H P H^T + R (innovation covariance) - * K = P H^T S^{-1} (Kalman gain) - * delta_xi = -K * y (correction in tangent space) - * X <- X.retract(delta_xi) - * P <- (I - K H) P * * @tparam Measurement Type of the measurement vector (e.g., VectorN) - * @tparam Prediction Functor signature: Measurement h(const M&, - * OptionalJacobian&) - * where m is the measurement dimension. - * - * @param h Measurement model functor returning predicted measurement z_pred + * @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)). * @param z Observed measurement. * @param R Measurement noise covariance (size m x m). @@ -115,28 +138,10 @@ namespace gtsam { template void update(Prediction&& h, const Measurement& z, const Matrix& R) { constexpr int m = traits::dimension; - Eigen::Matrix H; - // Predict measurement and get Jacobian H = dh/dlocal(X) - Measurement z_pred = h(X_, H); - - // Innovation - // Ensure consistent subtraction for manifold types if Measurement is one - Vector innovation = traits::Local(z, z_pred); // y = z_pred (-) z (in tangent space) - - // Innovation covariance and Kalman Gain - auto S = H * P_ * H.transpose() + R; - Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m) - - // Correction vector in tangent space - TangentVector delta_xi = -K * innovation; // delta_xi = - K * y - - // Update state using retract - X_ = traits::Retract(X_, delta_xi); // X <- X.retract(delta_xi) - - // Update covariance using Joseph form: - MatrixN I_KH = I_n - K * H; - P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); + Eigen::Matrix H; + Measurement prediction = h(X_, H); + update(prediction, H, z, R); // Call the other update function } protected: From eacdf1c7fa6326ac7b98f5b96b02d13e7742dff2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 00:25:35 -0400 Subject: [PATCH 02/10] 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; From 16f650c16c91d2923cab26ead7a9dc95de8b55ce Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 00:48:07 -0400 Subject: [PATCH 03/10] Retain efficiency in static case --- gtsam/navigation/InvariantEKF.h | 29 ++-- gtsam/navigation/LieGroupEKF.h | 70 +++++---- gtsam/navigation/ManifoldEKF.h | 165 ++++++++++++--------- gtsam/navigation/tests/testManifoldEKF.cpp | 31 ++-- 4 files changed, 167 insertions(+), 128 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index d2c6c2cb8..c0d95db84 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -38,30 +38,32 @@ namespace gtsam { * * This filter inherits from LieGroupEKF but restricts the prediction interface * to only the left-invariant prediction methods: - * 1. Prediction via group composition: `predict(const G& U, const Matrix& Q)` - * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Matrix& Q)` + * 1. Prediction via group composition: `predict(const G& U, const Covariance& Q)` + * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Covariance& Q)` * * The state-dependent prediction methods from LieGroupEKF are hidden. * The update step remains the same as in ManifoldEKF/LieGroupEKF. - * Covariances (P, Q) are `Matrix`. + * Covariances (P, Q) are `Eigen::Matrix`. */ template class InvariantEKF : public LieGroupEKF { public: using Base = LieGroupEKF; ///< Base class type using TangentVector = typename Base::TangentVector; ///< Tangent vector type - // Jacobian for group-specific operations like AdjointMap. - // Becomes Matrix if G has dynamic dimension. + /// Jacobian for group-specific operations like AdjointMap. Eigen::Matrix. using Jacobian = typename Base::Jacobian; + /// Covariance matrix type. Eigen::Matrix. + using Covariance = typename Base::Covariance; + /** * 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). + * @param P0_input Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix). */ - InvariantEKF(const G& X0, const Matrix& P0) : Base(X0, P0) {} + InvariantEKF(const G& X0, const Matrix& P0_input) : Base(X0, P0_input) {} - // We hide state-dependent predict methods from LieGroupEKF by only providing the + // We hide state-dependent predict methods from LieGroupEKF by only providing the // invariant predict methods below. /** @@ -71,13 +73,14 @@ namespace gtsam { * 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 (must be Matrix, size n_ x n_). + * @param Q Process noise covariance (Eigen::Matrix). */ - void predict(const G& U, const Matrix& Q) { + void predict(const G& U, const Covariance& Q) { this->X_ = this->X_.compose(U); // TODO(dellaert): traits::AdjointMap should exist const Jacobian A = traits::Inverse(U).AdjointMap(); - // P_ is Matrix. A is Eigen::Matrix. Q is Matrix. + // P_ is Covariance. A is Jacobian. Q is Covariance. + // All are Eigen::Matrix. this->P_ = A * this->P_ * A.transpose() + Q; } @@ -88,9 +91,9 @@ namespace gtsam { * * @param u Tangent space control vector. * @param dt Time interval. - * @param Q Process noise covariance matrix (Matrix, size n_ x n_). + * @param Q Process noise covariance matrix (Eigen::Matrix). */ - void predict(const TangentVector& u, double dt, const Matrix& Q) { + void predict(const TangentVector& u, double dt, const Covariance& Q) { 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 47c715531..da41a1e3d 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -48,17 +48,16 @@ namespace gtsam { class LieGroupEKF : public ManifoldEKF { public: using Base = ManifoldEKF; ///< Base class type - // 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 - // Jacobian for group-specific operations like Adjoint, Expmap derivatives. - // Becomes Matrix if n is Eigen::Dynamic. - using Jacobian = Eigen::Matrix; + static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G. + using TangentVector = typename Base::TangentVector; ///< Tangent vector type for G. + /// Jacobian for group operations (Adjoint, Expmap derivatives, F). + using Jacobian = typename Base::Jacobian; // Eigen::Matrix + using Covariance = typename Base::Covariance; // Eigen::Matrix /** * 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). + * @param P0 Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix). */ LieGroupEKF(const G& X0, const Matrix& P0) : Base(X0, P0) { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); @@ -66,38 +65,39 @@ namespace gtsam { /** * SFINAE check for correctly typed state-dependent dynamics function. - * Signature: TangentVector f(const G& X, OptionalJacobian Df) + * 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< !std::is_convertible_v&& std::is_invocable_r_v&>>; + OptionalJacobian&>>; /** * Predict mean and Jacobian A with state-dependent dynamics: - * xi = f(X_k, Df) (Compute tangent vector dynamics and Jacobian Df) + * xi = f(X_k, Df) (Compute tangent vector dynamics and Jacobian Df) * U = Expmap(xi * dt, Dexp) (Compute motion increment U and Expmap Jacobian Dexp) - * X_{k+1} = X_k * U (Predict next state) - * F = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian) + * X_{k+1} = X_k * U (Predict next state) + * A = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian) * - * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) + * @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 OptionalJacobian to store the computed state transition Jacobian A. * @return Predicted state X_{k+1}. */ template > - G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { - Jacobian Df, Dexp; + G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { + Jacobian Df, Dexp; // Eigen::Matrix + TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX) G U = traits::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt G X_next = this->X_.compose(U); if (A) { // Full state transition Jacobian for left-invariant EKF: + // AdjointMap returns Jacobian (Eigen::Matrix) *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; } return X_next; @@ -106,29 +106,32 @@ namespace gtsam { /** * Predict step with state-dependent dynamics: - * 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 + * Uses predictMean to compute X_{k+1} and A, then updates covariance. + * X_{k+1}, A = predictMean(f, dt) + * P_{k+1} = A P_k A^T + Q * - * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor. * @param dt Time step. - * @param Q Process noise covariance (Matrix, size n_ x n_). + * @param Q Process noise covariance (Eigen::Matrix). */ template > - void predict(Dynamics&& f, double dt, const Matrix& Q) { - Jacobian A; + void predict(Dynamics&& f, double dt, const Covariance& Q) { + Jacobian A; // Eigen::Matrix + if constexpr (Dim == Eigen::Dynamic) { + A.resize(this->n_, this->n_); + } this->X_ = predictMean(std::forward(f), dt, A); this->P_ = A * this->P_ * A.transpose() + Q; } /** * SFINAE check for state- and control-dependent dynamics function. - * Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian Df) + * Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian Df) */ template using enable_if_full_dynamics = std::enable_if_t< - std::is_invocable_r_v&> + std::is_invocable_r_v&> >; /** @@ -137,7 +140,7 @@ namespace gtsam { * xi = f(X_k, u, Df) * * @tparam Control Control input type. - * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) * @param f Dynamics functor. * @param u Control input. * @param dt Time step. @@ -145,8 +148,8 @@ namespace gtsam { * @return Predicted state X_{k+1}. */ template > - G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian A = {}) const { - return predictMean([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, A); + G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian A = {}) const { + return predictMean([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, A); } @@ -156,15 +159,18 @@ namespace gtsam { * xi = f(X_k, u, Df) * * @tparam Control Control input type. - * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) * @param f Dynamics functor. * @param u Control input. * @param dt Time step. - * @param Q Process noise covariance (must be Matrix, size n_ x n_). + * @param Q Process noise covariance (Eigen::Matrix). */ template > - void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { - return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); + void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) { + // Note: The lambda below captures f by reference. Ensure f's lifetime. + // If f is an rvalue, std::forward or std::move might be needed if the lambda is stored. + // Here, the lambda is temporary, so [&] is fine. + return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); } }; // LieGroupEKF diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index 68eaaa793..0f405c3ae 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -19,7 +19,7 @@ * 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). + * Covariances and Jacobians leverage Eigen's static or dynamic matrices for efficiency. * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert @@ -51,33 +51,41 @@ namespace gtsam { * must be provided by traits. * * This filter maintains a state X in the manifold M and covariance P in the - * tangent space at X. The covariance P is always stored as a gtsam::Matrix. + * tangent space at X. The covariance P is `Eigen::Matrix`, + * where Dim is the compile-time dimension of M (or Eigen::Dynamic). * 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: - /// Tangent vector type for the manifold M, as defined by its traits. + /// Compile-time dimension of the manifold M. + static constexpr int Dim = traits::dimension; + + /// Tangent vector type for the manifold M. using TangentVector = typename traits::TangentVector; + /// Covariance matrix type (P, Q). + using Covariance = Eigen::Matrix; + /// State transition Jacobian type (F). + using Jacobian = 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. + * @param P0 Initial covariance in the tangent space at X0. + * Provided as a dynamic gtsam::Matrix for flexibility, + * but will be stored internally as Covariance. */ - ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0), P_(P0) { + ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0) { 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); + if constexpr (Dim == Eigen::Dynamic) { + n_ = traits::GetDimension(X0); // Runtime dimension for dynamic case } else { - n_ = traits::dimension; + n_ = Dim; // Runtime dimension is fixed compile-time dimension } // Validate dimensions of initial covariance P0. @@ -88,6 +96,7 @@ namespace gtsam { ") do not match state's tangent space dimension (" + std::to_string(n_) + ")."); } + P_ = P0; // Assigns MatrixXd to Eigen::Matrix } virtual ~ManifoldEKF() = default; @@ -95,8 +104,11 @@ namespace gtsam { /// @return current state estimate on manifold M. const M& state() const { return X_; } - /// @return current covariance estimate in the tangent space (always a Matrix). - const Matrix& covariance() const { return P_; } + /// @return current covariance estimate (Eigen::Matrix). + const Covariance& covariance() const { return P_; } + + /// @return runtime dimension of the tangent space. + int dimension() const { return n_; } /** * Basic predict step: Updates state and covariance given the predicted @@ -107,22 +119,19 @@ namespace gtsam { * state transition in local coordinates around X_k. * * @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). + * @param F The state transition Jacobian (Eigen::Matrix). + * @param Q Process noise covariance matrix (Eigen::Matrix). */ - 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_) + "."); + void predict(const M& X_next, const Jacobian& F, const Covariance& Q) { + if constexpr (Dim == Eigen::Dynamic) { + if (F.rows() != n_ || F.cols() != n_ || Q.rows() != n_ || Q.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::predict: Dynamic F/Q dimensions must match state dimension " + + std::to_string(n_) + "."); + } } + // For fixed Dim, types enforce dimensions. + X_ = X_next; P_ = F * P_ * F.transpose() + Q; } @@ -134,64 +143,75 @@ namespace gtsam { * @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. + * Type: Eigen::Matrix. * @param z Observed measurement. - * @param R Measurement noise covariance (size m x m). + * @param R Measurement noise covariance (Eigen::Matrix). */ template void update(const Measurement& prediction, - const Matrix& H, + const Eigen::Matrix::dimension, Dim>& H, const Measurement& z, - const Matrix& R) { + const Eigen::Matrix::dimension, traits::dimension>& R) { static_assert(IsManifold::value, "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates."); - int m; // Measurement dimension - if constexpr (traits::dimension == Eigen::Dynamic) { - m = traits::GetDimension(z); - if (traits::GetDimension(prediction) != m) { + static constexpr int MeasDim = traits::dimension; + + int m_runtime; // Measurement dimension at runtime + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); + if (traits::GetDimension(prediction) != m_runtime) { throw std::invalid_argument( "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions."); } + // Runtime check for H and R if they involve dynamic dimensions + if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || R.cols() != m_runtime) { + throw std::invalid_argument( + "ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch for dynamic measurement."); + } } else { - m = traits::dimension; - } - - 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) + "."); + m_runtime = MeasDim; + // Types enforce dimensions for H and R if MeasDim and Dim are fixed. + // If Dim is dynamic but MeasDim is fixed, H.cols() needs check. + if constexpr (Dim == Eigen::Dynamic) { + if (H.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::update: Jacobian H columns must match state dimension " + std::to_string(n_) + "."); + } + } } // 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 + // S will be Eigen::Matrix + Eigen::Matrix S = H * P_ * H.transpose() + R; // Kalman Gain: K = P H^T S^-1 - const Matrix K = P_ * H.transpose() * S.inverse(); // K is n_ x m + // K will be Eigen::Matrix + Eigen::Matrix K = P_ * H.transpose() * S.inverse(); // Correction vector in tangent space of M: delta_xi = K * innovation - const TangentVector delta_xi = K * innovation; // delta_xi is n_ x 1 + const TangentVector delta_xi = K * innovation; // delta_xi is Dim x 1 (or n_ x 1 if dynamic) // 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 - const auto I_n = Matrix::Identity(n_, n_); - const Matrix I_KH = I_n - K * H; // I_KH is n x n + Jacobian I_n; // Eigen::Matrix + if constexpr (Dim == Eigen::Dynamic) { + I_n = Jacobian::Identity(n_, n_); + } + else { + I_n = Jacobian::Identity(); + } + + // I_KH will be Eigen::Matrix + Jacobian I_KH = I_n - K * H; P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); } @@ -202,37 +222,48 @@ namespace gtsam { * @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&`. + * where `Jac` can be `Eigen::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 (must be an m x m Matrix). + * @param R Measurement noise covariance (Eigen::Matrix). */ template - void update(MeasurementFunction&& h, const Measurement& z, const Matrix& R) { + void update(MeasurementFunction&& h_func, const Measurement& z, + const Eigen::Matrix::dimension, traits::dimension>& 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); + static constexpr int MeasDim = traits::dimension; + + int m_runtime; // Measurement dimension at runtime + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); } else { - m = traits::dimension; + m_runtime = MeasDim; } + // Prepare Jacobian H for the measurement function + Matrix H(m_runtime, n_); + if constexpr (MeasDim == Eigen::Dynamic || Dim == Eigen::Dynamic) { + // If H involves any dynamic dimension, it needs explicit resizing. + H.resize(m_runtime, n_); + } + // If H is fully fixed-size, its dimensions are set at compile time. + // Predict measurement and get Jacobian H = dh/dlocal(X) - Matrix H(m, n_); - Measurement prediction = h(X_, H); + Measurement prediction = h_func(X_, H); // Call the other update function update(prediction, H, z, R); } protected: - 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. + M X_; ///< Manifold state estimate. + Covariance P_; ///< Covariance (Eigen::Matrix). + int n_; ///< Runtime tangent space dimension of M. }; } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 50730a8d4..7516cd1cc 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -36,14 +36,14 @@ namespace exampleUnit3 { // Define a measurement model: measure the z-component of the Unit3 direction // H is the Jacobian dh/d(local(p)) - Vector1 measureZ(const Unit3& p, OptionalJacobian<1, 2> H) { + double measureZ(const Unit3& p, OptionalJacobian<1, 2> H) { if (H) { // H = d(p.point3().z()) / d(local(p)) // Calculate numerically for simplicity in test - auto h = [](const Unit3& p_) { return Vector1(p_.point3().z()); }; - *H = numericalDerivative11(h, p); + auto h = [](const Unit3& p_) { return p_.point3().z(); }; + *H = numericalDerivative11(h, p); } - return Vector1(p.point3().z()); + return p.point3().z(); } } // namespace exampleUnit3 @@ -116,8 +116,8 @@ TEST(ManifoldEKF_Unit3, Update) { ManifoldEKF ekf(p_start, P_start); // Simulate a measurement (e.g., true value + noise) - Vector1 z_true = exampleUnit3::measureZ(p_start, {}); - Vector1 z_observed = z_true + Vector1(0.02); // Add some noise + double z_true = exampleUnit3::measureZ(p_start, {}); + double z_observed = z_true + 0.02; // Add some noise // --- Perform EKF update --- ekf.update(exampleUnit3::measureZ, z_observed, data.R); @@ -125,10 +125,10 @@ TEST(ManifoldEKF_Unit3, Update) { // --- Verification (Manual Kalman Update Steps) --- // 1. Predict measurement and get Jacobian H Matrix12 H; // Note: Jacobian is 1x2 for Unit3 - Vector1 z_pred = exampleUnit3::measureZ(p_start, H); + double z_pred = exampleUnit3::measureZ(p_start, H); // 2. Innovation and Covariance - Vector1 y = z_pred - z_observed; // Innovation (using vector subtraction for z) + double y = z_pred - z_observed; // Innovation (using vector subtraction for z) Matrix1 S = H * P_start * H.transpose() + data.R; // 1x1 matrix // 3. Kalman Gain K @@ -156,16 +156,15 @@ namespace exampleDynamicMatrix { } // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) - Vector1 measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + double 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)); + return p(0, 0) + p(1, 1); } } // namespace exampleDynamicMatrix @@ -223,8 +222,8 @@ TEST(ManifoldEKF_DynamicMatrix, Update) { 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" + double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here + double zObserved = zTrue - 0.03; // Add some "error" // --- Perform EKF update --- ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); @@ -232,12 +231,12 @@ TEST(ManifoldEKF_DynamicMatrix, Update) { // --- 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); + double 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; + // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + double innovationY = zObserved - zPredictionManual; Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance; // 3. Kalman Gain K From ea783b0edb9a1698962ed1a542bcb7c2d536918d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 10:40:20 -0400 Subject: [PATCH 04/10] Reduce clutter --- gtsam/navigation/InvariantEKF.h | 11 +-- gtsam/navigation/LieGroupEKF.h | 21 +++--- gtsam/navigation/ManifoldEKF.h | 115 +++++++++++++++----------------- 3 files changed, 67 insertions(+), 80 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index c0d95db84..08ba24def 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -43,7 +43,8 @@ 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 `Eigen::Matrix`. + * For details on how static and dynamic dimensions are handled, please refer to + * the `ManifoldEKF` class documentation. */ template class InvariantEKF : public LieGroupEKF { @@ -59,9 +60,9 @@ namespace gtsam { /** * Constructor: forwards to LieGroupEKF constructor. * @param X0 Initial state on Lie group G. - * @param P0_input Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix). + * @param P0 Initial covariance in the tangent space at X0. */ - InvariantEKF(const G& X0, const Matrix& P0_input) : Base(X0, P0_input) {} + InvariantEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {} // We hide state-dependent predict methods from LieGroupEKF by only providing the // invariant predict methods below. @@ -73,7 +74,7 @@ namespace gtsam { * 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 (Eigen::Matrix). + * @param Q Process noise covariance. */ void predict(const G& U, const Covariance& Q) { this->X_ = this->X_.compose(U); @@ -91,7 +92,7 @@ namespace gtsam { * * @param u Tangent space control vector. * @param dt Time interval. - * @param Q Process noise covariance matrix (Eigen::Matrix). + * @param Q Process noise covariance matrix. */ void predict(const TangentVector& u, double dt, const Covariance& Q) { const G U = traits::Expmap(u * dt); diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index da41a1e3d..07efa0b0b 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -37,12 +37,13 @@ namespace gtsam { * @class LieGroupEKF * @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 (`IsLieGroup::value`). + * @tparam G Lie group type (must satisfy LieGroup concept). * * This filter specializes ManifoldEKF for Lie groups, offering predict methods * with state-dependent dynamics functions. * Use the InvariantEKF class for prediction via group composition. + * For details on how static and dynamic dimensions are handled, please refer to + * the `ManifoldEKF` class documentation. */ template class LieGroupEKF : public ManifoldEKF { @@ -57,9 +58,9 @@ namespace gtsam { /** * Constructor: initialize with state and covariance. * @param X0 Initial state on Lie group G. - * @param P0 Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix). + * @param P0 Initial covariance in the tangent space at X0. */ - LieGroupEKF(const G& X0, const Matrix& P0) : Base(X0, P0) { + LieGroupEKF(const G& X0, const Covariance& P0) : Base(X0, P0) { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); } @@ -96,8 +97,7 @@ namespace gtsam { G X_next = this->X_.compose(U); if (A) { - // Full state transition Jacobian for left-invariant EKF: - // AdjointMap returns Jacobian (Eigen::Matrix) + // State transition Jacobian for left-invariant EKF: *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; } return X_next; @@ -113,11 +113,11 @@ namespace gtsam { * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor. * @param dt Time step. - * @param Q Process noise covariance (Eigen::Matrix). + * @param Q Process noise covariance. */ template > void predict(Dynamics&& f, double dt, const Covariance& Q) { - Jacobian A; // Eigen::Matrix + Jacobian A; if constexpr (Dim == Eigen::Dynamic) { A.resize(this->n_, this->n_); } @@ -163,13 +163,10 @@ namespace gtsam { * @param f Dynamics functor. * @param u Control input. * @param dt Time step. - * @param Q Process noise covariance (Eigen::Matrix). + * @param Q Process noise covariance. */ template > void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) { - // Note: The lambda below captures f by reference. Ensure f's lifetime. - // If f is an rvalue, std::forward or std::move might be needed if the lambda is stored. - // Here, the lambda is temporary, so [&] is fine. return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); } diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index 0f405c3ae..d45f1fdf9 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -18,9 +18,6 @@ * 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 leverage Eigen's static or dynamic matrices for efficiency. - * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ @@ -40,22 +37,30 @@ namespace gtsam { /** - * @class ManifoldEKF - * @brief Extended Kalman Filter on a generic manifold M - * - * @tparam M Manifold type providing: - * - `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. The covariance P is `Eigen::Matrix`, - * where Dim is the compile-time dimension of M (or Eigen::Dynamic). - * 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. - */ + * @class ManifoldEKF + * @brief Extended Kalman Filter on a generic manifold M + * + * @tparam M Manifold type (must satisfy Manifold concept). + * + * 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. + * + * **Handling Static and Dynamic Dimensions:** + * The filter supports manifolds M with either a compile-time fixed dimension or a + * runtime dynamic dimension. This is determined by `gtsam::traits::dimension`. + * - If `dimension` is an integer (e.g., 3, 6), it's a fixed-size manifold. + * - If `dimension` is `Eigen::Dynamic`, it's a dynamically-sized manifold. In this case, + * `gtsam::traits::GetDimension(const M&)` must be available to retrieve the + * actual dimension at runtime. + * The internal protected member `n_` stores this runtime dimension. + * Covariance matrices (e.g., `P_`, method argument `Q`) and Jacobians (e.g., method argument `F`) + * are typed using `Covariance` and `Jacobian` typedefs, which are specializations of + * `Eigen::Matrix`, where `Dim` is `traits::dimension`. + * For dynamically-sized manifolds (`Dim == Eigen::Dynamic`), these Eigen types + * represent dynamically-sized matrices. + */ template class ManifoldEKF { public: @@ -73,30 +78,28 @@ namespace gtsam { /** * Constructor: initialize with state and covariance. * @param X0 Initial state on manifold M. - * @param P0 Initial covariance in the tangent space at X0. - * Provided as a dynamic gtsam::Matrix for flexibility, - * but will be stored internally as Covariance. + * @param P0 Initial covariance in the tangent space at X0 */ - ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0) { + ManifoldEKF(const M& X0, const Covariance& P0) : X_(X0) { static_assert(IsManifold::value, "Template parameter M must be a GTSAM Manifold."); if constexpr (Dim == Eigen::Dynamic) { - n_ = traits::GetDimension(X0); // Runtime dimension for dynamic case + n_ = traits::GetDimension(X0); + // 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_) + ")."); + } } else { - n_ = Dim; // Runtime dimension is fixed compile-time dimension + n_ = Dim; } - // 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_) + ")."); - } - P_ = P0; // Assigns MatrixXd to Eigen::Matrix + P_ = P0; } virtual ~ManifoldEKF() = default; @@ -104,10 +107,10 @@ namespace gtsam { /// @return current state estimate on manifold M. const M& state() const { return X_; } - /// @return current covariance estimate (Eigen::Matrix). + /// @return current covariance estimate. const Covariance& covariance() const { return P_; } - /// @return runtime dimension of the tangent space. + /// @return runtime dimension of the manifold. int dimension() const { return n_; } /** @@ -119,8 +122,8 @@ namespace gtsam { * state transition in local coordinates around X_k. * * @param X_next The predicted state at time k+1 on manifold M. - * @param F The state transition Jacobian (Eigen::Matrix). - * @param Q Process noise covariance matrix (Eigen::Matrix). + * @param F The state transition Jacobian. + * @param Q Process noise covariance matrix. */ void predict(const M& X_next, const Jacobian& F, const Covariance& Q) { if constexpr (Dim == Eigen::Dynamic) { @@ -130,7 +133,6 @@ namespace gtsam { std::to_string(n_) + "."); } } - // For fixed Dim, types enforce dimensions. X_ = X_next; P_ = F * P_ * F.transpose() + Q; @@ -143,9 +145,8 @@ namespace gtsam { * @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). - * Type: Eigen::Matrix. * @param z Observed measurement. - * @param R Measurement noise covariance (Eigen::Matrix). + * @param R Measurement noise covariance. */ template void update(const Measurement& prediction, @@ -158,14 +159,13 @@ namespace gtsam { static constexpr int MeasDim = traits::dimension; - int m_runtime; // Measurement dimension at runtime + int m_runtime; if constexpr (MeasDim == Eigen::Dynamic) { m_runtime = traits::GetDimension(z); if (traits::GetDimension(prediction) != m_runtime) { throw std::invalid_argument( "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions."); } - // Runtime check for H and R if they involve dynamic dimensions if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || R.cols() != m_runtime) { throw std::invalid_argument( "ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch for dynamic measurement."); @@ -173,8 +173,6 @@ namespace gtsam { } else { m_runtime = MeasDim; - // Types enforce dimensions for H and R if MeasDim and Dim are fixed. - // If Dim is dynamic but MeasDim is fixed, H.cols() needs check. if constexpr (Dim == Eigen::Dynamic) { if (H.cols() != n_) { throw std::invalid_argument( @@ -219,25 +217,23 @@ namespace gtsam { * Measurement update: Corrects the state and covariance using a measurement * model function. * - * @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 `Eigen::Matrix&` or - * `OptionalJacobian&`. - * The Jacobian H should be d(h)/d(local(X)). + * @tparam Measurement Type of the measurement vector. + * @tparam MeasurementFunction Functor/lambda providing measurement and its Jacobian. + * Signature: `Measurement h(const M& x, Jac& H_jacobian)` + * where H = d(h)/d(local(X)). * @param h Measurement model function. * @param z Observed measurement. - * @param R Measurement noise covariance (Eigen::Matrix). + * @param R Measurement noise covariance. */ template - void update(MeasurementFunction&& h_func, const Measurement& z, + void update(MeasurementFunction&& h, const Measurement& z, const Eigen::Matrix::dimension, traits::dimension>& R) { static_assert(IsManifold::value, "Template parameter Measurement must be a GTSAM Manifold."); static constexpr int MeasDim = traits::dimension; - int m_runtime; // Measurement dimension at runtime + int m_runtime; if constexpr (MeasDim == Eigen::Dynamic) { m_runtime = traits::GetDimension(z); } @@ -245,16 +241,9 @@ namespace gtsam { m_runtime = MeasDim; } - // Prepare Jacobian H for the measurement function - Matrix H(m_runtime, n_); - if constexpr (MeasDim == Eigen::Dynamic || Dim == Eigen::Dynamic) { - // If H involves any dynamic dimension, it needs explicit resizing. - H.resize(m_runtime, n_); - } - // If H is fully fixed-size, its dimensions are set at compile time. - // Predict measurement and get Jacobian H = dh/dlocal(X) - Measurement prediction = h_func(X_, H); + Matrix H(m_runtime, n_); + Measurement prediction = h(X_, H); // Call the other update function update(prediction, H, z, R); From 2a22123d5d92572631b6e7fa1c9fff2f835f6c8a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:46:25 -0400 Subject: [PATCH 05/10] Add AdjointMap into traits --- gtsam/base/Lie.h | 29 +++++++++++++++++++---------- gtsam/base/VectorSpace.h | 33 ++++++++++++++++++--------------- gtsam/navigation/InvariantEKF.h | 6 +++--- 3 files changed, 40 insertions(+), 28 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 4248f16b2..17c35446e 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -169,32 +169,34 @@ namespace internal { /// To use this for your gtsam type, define: /// template<> struct traits : public internal::LieGroupTraits {}; /// Assumes existence of: identity, dimension, localCoordinates, retract, -/// and additionally Logmap, Expmap, compose, between, and inverse +/// and additionally Logmap, Expmap, AdjointMap, compose, between, and inverse template -struct LieGroupTraits: public GetDimensionImpl { +struct LieGroupTraits : public GetDimensionImpl { using structure_category = lie_group_tag; /// @name Group /// @{ using group_flavor = multiplicative_group_tag; - static Class Identity() { return Class::Identity();} + static Class Identity() { return Class::Identity(); } /// @} /// @name Manifold /// @{ using ManifoldType = Class; + // Note: Class::dimension can be an int or Eigen::Dynamic. + // GetDimensionImpl handles resolving this to a static value or providing GetDimension(obj). inline constexpr static auto dimension = Class::dimension; using TangentVector = Eigen::Matrix; using ChartJacobian = OptionalJacobian; static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { - return origin.localCoordinates(other, Horigin, Hother); + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + return origin.localCoordinates(other, H1, H2); } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian Horigin = {}, ChartJacobian Hv = {}) { - return origin.retract(v, Horigin, Hv); + ChartJacobian H = {}, ChartJacobian Hv = {}) { + return origin.retract(v, H, Hv); } /// @} @@ -209,19 +211,26 @@ struct LieGroupTraits: public GetDimensionImpl { } static Class Compose(const Class& m1, const Class& m2, // - ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.compose(m2, H1, H2); } static Class Between(const Class& m1, const Class& m2, // - ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.between(m2, H1, H2); } static Class Inverse(const Class& m, // - ChartJacobian H = {}) { + ChartJacobian H = {}) { return m.inverse(H); } + + static Eigen::Matrix AdjointMap(const Class& m) { + // This assumes that the Class itself provides a member function `AdjointMap()` + // For dynamically-sized types (dimension == Eigen::Dynamic), + // m.AdjointMap() must return a gtsam::Matrix of the correct runtime dimensions. + return m.AdjointMap(); + } /// @} }; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 1a486886d..b81b19ac3 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -35,7 +35,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); - Class v = other-origin; + Class v = other - origin; return v.vector(); } @@ -82,12 +82,12 @@ struct VectorSpaceImpl { return -v; } - static LieAlgebra Hat(const TangentVector& v) { - return v; - } + static LieAlgebra Hat(const TangentVector& v) { return v; } - static TangentVector Vee(const LieAlgebra& X) { - return X; + static TangentVector Vee(const LieAlgebra& X) { return X; } + + static Jacobian AdjointMap(const Class& /*m*/) { + return Jacobian::Identity(); } /// @} }; @@ -118,7 +118,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Eye(origin); if (H2) *H2 = Eye(other); - Class v = other-origin; + Class v = other - origin; return v.vector(); } @@ -141,8 +141,7 @@ struct VectorSpaceImpl { static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { Class result(v); - if (Hv) - *Hv = Eye(v); + if (Hv) *Hv = Eye(v); return result; } @@ -165,6 +164,7 @@ struct VectorSpaceImpl { return -v; } + static Eigen::MatrixXd AdjointMap(const Class& m) { return Eye(m); } /// @} }; @@ -273,8 +273,8 @@ struct ScalarTraits : VectorSpaceImpl { if (H) (*H)[0] = 1.0; return v[0]; } + // AdjointMap for ScalarTraits is inherited from VectorSpaceImpl /// @} - }; } // namespace internal @@ -352,6 +352,9 @@ struct traits > : if (H) *H = Jacobian::Identity(); return m + Eigen::Map(v.data()); } + + // AdjointMap for fixed-size Eigen matrices is inherited from + // internal::VectorSpaceImpl< Eigen::Matrix , M*N > /// @} }; @@ -404,7 +407,7 @@ struct DynamicTraits { static TangentVector Local(const Dynamic& m, const Dynamic& other, // ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = -Eye(m); - if (H2) *H2 = Eye(m); + if (H2) *H2 = Eye(m); TangentVector v(GetDimension(m)); Eigen::Map(v.data(), m.rows(), m.cols()) = other - m; return v; @@ -425,7 +428,7 @@ struct DynamicTraits { static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) { if (H) *H = Eye(m); TangentVector result(GetDimension(m)); - Eigen::Map(result.data(), m.cols(), m.rows()) = m; + Eigen::Map(result.data(), m.rows(), m.cols()) = m; return result; } @@ -460,7 +463,8 @@ struct DynamicTraits { static TangentVector Vee(const LieAlgebra& X) { return X; } - + + static Jacobian AdjointMap(const Dynamic& m) { return Eye(m); } /// @} }; @@ -505,5 +509,4 @@ private: T p, q, r; }; -} // namespace gtsam - +} // namespace gtsam diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 08ba24def..20d74285f 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -77,9 +77,9 @@ namespace gtsam { * @param Q Process noise covariance. */ void predict(const G& U, const Covariance& Q) { - this->X_ = this->X_.compose(U); - // TODO(dellaert): traits::AdjointMap should exist - const Jacobian A = traits::Inverse(U).AdjointMap(); + this->X_ = traits::Compose(this->X_, U); + const G U_inv = traits::Inverse(U); + const Jacobian A = traits::AdjointMap(U_inv); // P_ is Covariance. A is Jacobian. Q is Covariance. // All are Eigen::Matrix. this->P_ = A * this->P_ * A.transpose() + Q; From 192b6a26ffd178c8c778a95d8920741a89714b36 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:48:56 -0400 Subject: [PATCH 06/10] Deal with Matrix --- gtsam/navigation/InvariantEKF.h | 10 ++- gtsam/navigation/LieGroupEKF.h | 31 +++++-- gtsam/navigation/tests/testInvariantEKF.cpp | 93 +++++++++++++++++++++ 3 files changed, 125 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 20d74285f..a2eea3719 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -95,7 +95,15 @@ namespace gtsam { * @param Q Process noise covariance matrix. */ void predict(const TangentVector& u, double dt, const Covariance& Q) { - const G U = traits::Expmap(u * dt); + G U; + if constexpr (std::is_same_v) { + const Matrix& X = static_cast(this->X_); + U.resize(X.rows(), X.cols()); + Eigen::Map(static_cast(U).data(), U.size()) = u * dt; + } + else { + 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 07efa0b0b..4421ab660 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -92,15 +92,30 @@ namespace gtsam { G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { Jacobian Df, Dexp; // Eigen::Matrix - TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX) - G U = traits::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt - G X_next = this->X_.compose(U); - - if (A) { - // State transition Jacobian for left-invariant EKF: - *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + 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; + 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 + } + 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) { + // State transition Jacobian for left-invariant EKF: + *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + } + return this->X_.compose(U); } - return X_next; } diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 74a70fbbe..2916d6da0 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -119,6 +119,99 @@ TEST(IEKF_Pose2, PredictUpdateSequence) { } +// 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. + // This is mainly for verification; IEKF predict will use tangent vector directly. + Matrix predictNextStateManually(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) + double 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, p.size()); // p.size() is rows*cols + (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2, so 1x4 + } + return p(0, 0) + p(1, 1); + } +} // namespace exampleDynamicMatrix + +// Test fixture for InvariantEKF 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(InvariantEKF_DynamicMatrix, Predict) { + DynamicMatrixEKFTest data; + InvariantEKF 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()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + // --- Perform EKF prediction using InvariantEKF::predict(tangentVector, dt, Q) --- + ekf.predict(data.velocityTangent, data.dt, data.processNoiseCovariance); + // --- Verification --- + // 1. Calculate expected next state + Matrix pNextExpected = exampleDynamicMatrix::predictNextStateManually(data.p0Matrix, data.velocityTangent, data.dt); + EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); + // 2. Calculate expected covariance + // For VectorSpace, AdjointMap is Identity. So P_next = P_prev + Q. + Matrix pCovarianceExpected = data.p0Covariance + data.processNoiseCovariance; + EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); +} + +TEST(InvariantEKF_DynamicMatrix, Update) { + DynamicMatrixEKFTest data; + Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); + Matrix pStartCovariance = I_4x4 * 0.02; + InvariantEKF ekf(pStartMatrix, pStartCovariance); + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) + double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here + double zObserved = zTrue - 0.03; // Add some "error" + // --- Perform EKF update --- + // The update method is inherited from ManifoldEKF. + ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); + // --- Verification (Manual Kalman Update Steps) --- + // 1. Predict measurement and get Jacobian H + Matrix H_manual(1, 4); // This will be 1x4 for a 2x2 matrix measurement + double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H_manual); + // 2. Innovation and Innovation Covariance + // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) + // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + double innovationY_tangent = zObserved - zPredictionManual; + Matrix innovationCovarianceS = H_manual * pStartCovariance * H_manual.transpose() + data.measurementNoiseCovariance; + // 3. Kalman Gain K + Matrix kalmanGainK = pStartCovariance * H_manual.transpose() * innovationCovarianceS.inverse(); // K is 4x1 + // 4. State Correction (in tangent space of Matrix) + Vector deltaXiTangent = kalmanGainK * innovationY_tangent; // deltaXi is 4x1 Vector + // 5. Expected Updated State and Covariance (using Joseph form) + Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); + Matrix I_KH = I_4x4 - kalmanGainK * H_manual; + Matrix pUpdatedCovarianceExpected = I_KH * pStartCovariance * I_KH.transpose() + kalmanGainK * data.measurementNoiseCovariance * kalmanGainK.transpose(); + // --- 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; return TestRegistry::runAllTests(tr); From 5758c7ef0cc83ab3d738d699947062d65af6b712 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:58:15 -0400 Subject: [PATCH 07/10] Simplify tests --- gtsam/navigation/tests/testInvariantEKF.cpp | 121 ++++++--------- gtsam/navigation/tests/testManifoldEKF.cpp | 157 +++++++++----------- 2 files changed, 118 insertions(+), 160 deletions(-) diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 2916d6da0..4797f79a1 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -121,97 +121,66 @@ TEST(IEKF_Pose2, PredictUpdateSequence) { // 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. - // This is mainly for verification; IEKF predict will use tangent vector directly. - Matrix predictNextStateManually(const Matrix& p, const Vector& vTangent, double dt) { + Matrix f(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) - double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + double h(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, p.size()); // p.size() is rows*cols - (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2, so 1x4 + H->resize(1, p.size()); + (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2 } - return p(0, 0) + p(1, 1); + return p(0, 0) + p(1, 1); // trace ! } } // namespace exampleDynamicMatrix -// Test fixture for InvariantEKF 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(InvariantEKF_DynamicMatrix, PredictAndUpdate) { + // --- Setup --- + Matrix p0Matrix = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix p0Covariance = I_4x4 * 0.01; + Vector velocityTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); + double dt = 0.1; + Matrix Q = I_4x4 * 0.001; + Matrix R = Matrix::Identity(1, 1) * 0.005; -TEST(InvariantEKF_DynamicMatrix, Predict) { - DynamicMatrixEKFTest data; - InvariantEKF 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()); - EXPECT_LONGS_EQUAL(4, ekf.dimension()); - // --- Perform EKF prediction using InvariantEKF::predict(tangentVector, dt, Q) --- - ekf.predict(data.velocityTangent, data.dt, data.processNoiseCovariance); - // --- Verification --- - // 1. Calculate expected next state - Matrix pNextExpected = exampleDynamicMatrix::predictNextStateManually(data.p0Matrix, data.velocityTangent, data.dt); - EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); - // 2. Calculate expected covariance - // For VectorSpace, AdjointMap is Identity. So P_next = P_prev + Q. - Matrix pCovarianceExpected = data.p0Covariance + data.processNoiseCovariance; - EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); -} - -TEST(InvariantEKF_DynamicMatrix, Update) { - DynamicMatrixEKFTest data; - Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); - Matrix pStartCovariance = I_4x4 * 0.02; - InvariantEKF ekf(pStartMatrix, pStartCovariance); + InvariantEKF ekf(p0Matrix, p0Covariance); EXPECT_LONGS_EQUAL(4, ekf.state().size()); EXPECT_LONGS_EQUAL(4, ekf.dimension()); - // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) - double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here - double zObserved = zTrue - 0.03; // Add some "error" - // --- Perform EKF update --- - // The update method is inherited from ManifoldEKF. - ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); - // --- Verification (Manual Kalman Update Steps) --- - // 1. Predict measurement and get Jacobian H - Matrix H_manual(1, 4); // This will be 1x4 for a 2x2 matrix measurement - double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H_manual); - // 2. Innovation and Innovation Covariance - // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) - // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + + // --- Predict --- + ekf.predict(velocityTangent, dt, Q); + + // Verification for Predict + Matrix pPredictedExpected = exampleDynamicMatrix::f(p0Matrix, velocityTangent, dt); + Matrix pCovariancePredictedExpected = p0Covariance + Q; + EXPECT(assert_equal(pPredictedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pCovariancePredictedExpected, ekf.covariance(), 1e-9)); + + // --- Update --- + // Use the state after prediction for the update step + Matrix pStateBeforeUpdate = ekf.state(); + Matrix pCovarianceBeforeUpdate = ekf.covariance(); + + double zTrue = exampleDynamicMatrix::h(pStateBeforeUpdate); + double zObserved = zTrue - 0.03; // Simulated measurement with some error + + ekf.update(exampleDynamicMatrix::h, zObserved, R); + + // Verification for Update (Manual Kalman Steps) + Matrix H(1, 4); + double zPredictionManual = exampleDynamicMatrix::h(pStateBeforeUpdate, H); double innovationY_tangent = zObserved - zPredictionManual; - Matrix innovationCovarianceS = H_manual * pStartCovariance * H_manual.transpose() + data.measurementNoiseCovariance; - // 3. Kalman Gain K - Matrix kalmanGainK = pStartCovariance * H_manual.transpose() * innovationCovarianceS.inverse(); // K is 4x1 - // 4. State Correction (in tangent space of Matrix) - Vector deltaXiTangent = kalmanGainK * innovationY_tangent; // deltaXi is 4x1 Vector - // 5. Expected Updated State and Covariance (using Joseph form) - Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); - Matrix I_KH = I_4x4 - kalmanGainK * H_manual; - Matrix pUpdatedCovarianceExpected = I_KH * pStartCovariance * I_KH.transpose() + kalmanGainK * data.measurementNoiseCovariance * kalmanGainK.transpose(); - // --- Compare EKF result with manual calculation --- + Matrix S = H * pCovarianceBeforeUpdate * H.transpose() + R; + Matrix kalmanGainK = pCovarianceBeforeUpdate * H.transpose() * S.inverse(); + Vector deltaXiTangent = kalmanGainK * innovationY_tangent; + Matrix pUpdatedExpected = traits::Retract(pStateBeforeUpdate, deltaXiTangent); + Matrix I_KH = I_4x4 - kalmanGainK * H; + Matrix pUpdatedCovarianceExpected = I_KH * pCovarianceBeforeUpdate * I_KH.transpose() + kalmanGainK * R * kalmanGainK.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); diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 7516cd1cc..8fe96557d 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -151,110 +151,99 @@ TEST(ManifoldEKF_Unit3, Update) { namespace exampleDynamicMatrix { // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. + // For Matrix (a VectorSpace type), Retract(M, v) is typically M + v. 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) - double 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 << 1.0, 0.0, 0.0, 1.0; + // H is the Jacobian dh/d(flatten(p)) + double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H_opt = {}) { + if (H_opt) { + // The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size(). + // For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1]. + Matrix H = Matrix::Zero(1, p.size()); + if (p.rows() >= 2 && p.cols() >= 2) { // Ensure it's at least 2x2 for trace definition + H(0, 0) = 1.0; // d(trace)/dp00 + H(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) + // For 2x2: H(0, 2*1 + 1) = H(0,3) + } + *H_opt = H; } - return p(0, 0) + p(1, 1); + if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error + double trace = 0.0; + for (DenseIndex i = 0; i < std::min(p.rows(), p.cols()); ++i) { + trace += p(i, i); + } + return trace; } } // 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) +TEST(ManifoldEKF_DynamicMatrix, CombinedPredictAndUpdate) { + Matrix p_initial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix P_initial = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4) + Vector v_tangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec + double dt = 0.1; + Matrix Q = I_4x4 * 0.001; // Process noise covariance (4x4) + Matrix R = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (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); + ManifoldEKF ekf(p_initial, P_initial); // 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()); + EXPECT_LONGS_EQUAL(p_initial.rows() * p_initial.cols(), ekf.state().size()); - // --- Prepare inputs for ManifoldEKF::predict --- - Matrix pNextExpected = exampleDynamicMatrix::predictNextState(data.p0Matrix, data.velocityTangent, data.dt); + // Predict Step + Matrix p_predicted_mean = exampleDynamicMatrix::predictNextState(p_initial, v_tangent, 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 + // For this linear prediction model (p_next = p_current + V*dt in tangent space), + // Derivative w.r.t delta_xi is Identity. + Matrix F = I_4x4; - // --- Perform EKF prediction --- - ekf.predict(pNextExpected, jacobianF, data.processNoiseCovariance); + ekf.predict(p_predicted_mean, F, Q); - // --- 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)); + EXPECT(assert_equal(p_predicted_mean, ekf.state(), 1e-9)); + Matrix P_predicted_expected = F * P_initial * F.transpose() + Q; + EXPECT(assert_equal(P_predicted_expected, ekf.covariance(), 1e-9)); + + // Update Step + Matrix p_current_for_update = ekf.state(); + Matrix P_current_for_update = ekf.covariance(); + + // True trace of p_current_for_update (which is p_predicted_mean) + // p_predicted_mean = p_initial + v_tangent*dt + // = [1.0, 2.0; 3.0, 4.0] + [0.05, -0.01; 0.01, -0.05] (v_tangent reshaped to 2x2 col-major) + // Trace = 1.05 + 3.95 = 5.0 + double z_true = exampleDynamicMatrix::measureTrace(p_current_for_update); + EXPECT_DOUBLES_EQUAL(5.0, z_true, 1e-9); + double z_observed = z_true - 0.03; + + ekf.update(exampleDynamicMatrix::measureTrace, z_observed, R); + + // Manual Kalman Update Steps for Verification + Matrix H(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement) + double z_prediction_manual = exampleDynamicMatrix::measureTrace(p_current_for_update, H); + Matrix H_expected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished(); + EXPECT(assert_equal(H_expected, H, 1e-9)); + + + // Innovation: y = z_observed - z_prediction_manual (since measurement is double) + double y_innovation = z_observed - z_prediction_manual; + Matrix S = H * P_current_for_update * H.transpose() + R; // S is 1x1 + + Matrix K_gain = P_current_for_update * H.transpose() * S.inverse(); // K is 4x1 + + // State Correction (in tangent space of Matrix) + Vector delta_xi_tangent = K_gain * y_innovation; // delta_xi is 4x1 Vector + + Matrix p_updated_manual_expected = traits::Retract(p_current_for_update, delta_xi_tangent); + Matrix P_updated_manual_expected = (I_4x4 - K_gain * H) * P_current_for_update; + + EXPECT(assert_equal(p_updated_manual_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P_updated_manual_expected, 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) - double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here - double zObserved = zTrue - 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 - double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H); - - // 2. Innovation and Innovation Covariance - // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) - // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. - double 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; From 8efce78419a3c220fb7fd048f89e9a499b2d8798 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 17:46:52 -0400 Subject: [PATCH 08/10] Added dynamic test for LieGroupEKF as well --- gtsam/navigation/InvariantEKF.h | 1 + gtsam/navigation/LieGroupEKF.h | 13 +--- gtsam/navigation/tests/testLieGroupEKF.cpp | 88 ++++++++++++++++++++++ 3 files changed, 93 insertions(+), 9 deletions(-) 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); From da09c4a2c30226c10e05d88ae8d2d118a5c1c689 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 17:57:21 -0400 Subject: [PATCH 09/10] naming --- gtsam/navigation/tests/testManifoldEKF.cpp | 104 ++++++++++----------- 1 file changed, 47 insertions(+), 57 deletions(-) diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 8fe96557d..ff7a9a68d 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -30,7 +30,7 @@ using namespace gtsam; namespace exampleUnit3 { // Predicts the next state given current state, tangent velocity, and dt - Unit3 predictNextState(const Unit3& p, const Vector2& v, double dt) { + Unit3 f(const Unit3& p, const Vector2& v, double dt) { return p.retract(v * dt); } @@ -76,13 +76,13 @@ TEST(ManifoldEKF_Unit3, Predict) { // --- Prepare inputs for ManifoldEKF::predict --- // 1. Compute expected next state - Unit3 p_next_expected = exampleUnit3::predictNextState(data.p0, data.velocity, data.dt); + Unit3 p_next_expected = exampleUnit3::f(data.p0, data.velocity, data.dt); // 2. Compute state transition Jacobian F = d(local(p_next)) / d(local(p)) - // We can compute this numerically using the predictNextState function. + // We can compute this numerically using the f function. // GTSAM's numericalDerivative handles derivatives *between* manifolds. auto predict_wrapper = [&](const Unit3& p) -> Unit3 { - return exampleUnit3::predictNextState(p, data.velocity, data.dt); + return exampleUnit3::f(p, data.velocity, data.dt); }; Matrix2 F = numericalDerivative11(predict_wrapper, data.p0); @@ -100,7 +100,7 @@ TEST(ManifoldEKF_Unit3, Predict) { // Check F manually for a simple case (e.g., zero velocity should give Identity) Vector2 zero_velocity = Vector2::Zero(); auto predict_wrapper_zero = [&](const Unit3& p) -> Unit3 { - return exampleUnit3::predictNextState(p, zero_velocity, data.dt); + return exampleUnit3::f(p, zero_velocity, data.dt); }; Matrix2 F_zero = numericalDerivative11(predict_wrapper_zero, data.p0); EXPECT(assert_equal(I_2x2, F_zero, 1e-8)); @@ -152,23 +152,19 @@ namespace exampleDynamicMatrix { // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. // For Matrix (a VectorSpace type), Retract(M, v) is typically M + v. - Matrix predictNextState(const Matrix& p, const Vector& vTangent, double dt) { + Matrix f(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) // H is the Jacobian dh/d(flatten(p)) - double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H_opt = {}) { - if (H_opt) { + double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + if (H) { // The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size(). // For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1]. - Matrix H = Matrix::Zero(1, p.size()); - if (p.rows() >= 2 && p.cols() >= 2) { // Ensure it's at least 2x2 for trace definition - H(0, 0) = 1.0; // d(trace)/dp00 - H(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) - // For 2x2: H(0, 2*1 + 1) = H(0,3) - } - *H_opt = H; + H->resize(1, p.size()); + (*H)(0, 0) = 1.0; // d(trace)/dp00 + (*H)(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) } if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error double trace = 0.0; @@ -181,70 +177,64 @@ namespace exampleDynamicMatrix { } // namespace exampleDynamicMatrix TEST(ManifoldEKF_DynamicMatrix, CombinedPredictAndUpdate) { - Matrix p_initial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); - Matrix P_initial = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4) - Vector v_tangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec - double dt = 0.1; - Matrix Q = I_4x4 * 0.001; // Process noise covariance (4x4) - Matrix R = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (1x1) + Matrix pInitial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix pInitialCovariance = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4) + Vector vTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec + double deltaTime = 0.1; + Matrix processNoiseCovariance = I_4x4 * 0.001; // Process noise covariance (4x4) + Matrix measurementNoiseCovariance = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (1x1) - ManifoldEKF ekf(p_initial, P_initial); + ManifoldEKF ekf(pInitial, pInitialCovariance); // For a 2x2 Matrix, tangent space dimension is 2*2=4. EXPECT_LONGS_EQUAL(4, ekf.state().size()); - EXPECT_LONGS_EQUAL(p_initial.rows() * p_initial.cols(), ekf.state().size()); + EXPECT_LONGS_EQUAL(pInitial.rows() * pInitial.cols(), ekf.state().size()); // Predict Step - Matrix p_predicted_mean = exampleDynamicMatrix::predictNextState(p_initial, v_tangent, dt); + Matrix pPredictedMean = exampleDynamicMatrix::f(pInitial, vTangent, deltaTime); - // For this linear prediction model (p_next = p_current + V*dt in tangent space), - // Derivative w.r.t delta_xi is Identity. - Matrix F = I_4x4; + // For this linear prediction model (pNext = pCurrent + V*dt in tangent space), + // Derivative w.r.t deltaXi is Identity. + Matrix fJacobian = I_4x4; - ekf.predict(p_predicted_mean, F, Q); + ekf.predict(pPredictedMean, fJacobian, processNoiseCovariance); - EXPECT(assert_equal(p_predicted_mean, ekf.state(), 1e-9)); - Matrix P_predicted_expected = F * P_initial * F.transpose() + Q; - EXPECT(assert_equal(P_predicted_expected, ekf.covariance(), 1e-9)); + EXPECT(assert_equal(pPredictedMean, ekf.state(), 1e-9)); + Matrix pPredictedCovarianceExpected = fJacobian * pInitialCovariance * fJacobian.transpose() + processNoiseCovariance; + EXPECT(assert_equal(pPredictedCovarianceExpected, ekf.covariance(), 1e-9)); // Update Step - Matrix p_current_for_update = ekf.state(); - Matrix P_current_for_update = ekf.covariance(); + Matrix pCurrentForUpdate = ekf.state(); + Matrix pCurrentCovarianceForUpdate = ekf.covariance(); - // True trace of p_current_for_update (which is p_predicted_mean) - // p_predicted_mean = p_initial + v_tangent*dt - // = [1.0, 2.0; 3.0, 4.0] + [0.05, -0.01; 0.01, -0.05] (v_tangent reshaped to 2x2 col-major) - // Trace = 1.05 + 3.95 = 5.0 - double z_true = exampleDynamicMatrix::measureTrace(p_current_for_update); - EXPECT_DOUBLES_EQUAL(5.0, z_true, 1e-9); - double z_observed = z_true - 0.03; + // True trace of pCurrentForUpdate (which is pPredictedMean) + double zTrue = exampleDynamicMatrix::h(pCurrentForUpdate); + EXPECT_DOUBLES_EQUAL(5.0, zTrue, 1e-9); + double zObserved = zTrue - 0.03; - ekf.update(exampleDynamicMatrix::measureTrace, z_observed, R); + ekf.update(exampleDynamicMatrix::h, zObserved, measurementNoiseCovariance); // Manual Kalman Update Steps for Verification - Matrix H(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement) - double z_prediction_manual = exampleDynamicMatrix::measureTrace(p_current_for_update, H); - Matrix H_expected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished(); - EXPECT(assert_equal(H_expected, H, 1e-9)); + Matrix hJacobian(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement) + double zPredictionManual = exampleDynamicMatrix::h(pCurrentForUpdate, hJacobian); + Matrix hJacobianExpected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished(); + EXPECT(assert_equal(hJacobianExpected, hJacobian, 1e-9)); + // Innovation: y = zObserved - zPredictionManual (since measurement is double) + double yInnovation = zObserved - zPredictionManual; + Matrix innovationCovariance = hJacobian * pCurrentCovarianceForUpdate * hJacobian.transpose() + measurementNoiseCovariance; - // Innovation: y = z_observed - z_prediction_manual (since measurement is double) - double y_innovation = z_observed - z_prediction_manual; - Matrix S = H * P_current_for_update * H.transpose() + R; // S is 1x1 - - Matrix K_gain = P_current_for_update * H.transpose() * S.inverse(); // K is 4x1 + Matrix kalmanGain = pCurrentCovarianceForUpdate * hJacobian.transpose() * innovationCovariance.inverse(); // K is 4x1 // State Correction (in tangent space of Matrix) - Vector delta_xi_tangent = K_gain * y_innovation; // delta_xi is 4x1 Vector + Vector deltaXiTangent = kalmanGain * yInnovation; // deltaXi is 4x1 Vector - Matrix p_updated_manual_expected = traits::Retract(p_current_for_update, delta_xi_tangent); - Matrix P_updated_manual_expected = (I_4x4 - K_gain * H) * P_current_for_update; + Matrix pUpdatedManualExpected = traits::Retract(pCurrentForUpdate, deltaXiTangent); + Matrix pUpdatedCovarianceManualExpected = (I_4x4 - kalmanGain * hJacobian) * pCurrentCovarianceForUpdate; - EXPECT(assert_equal(p_updated_manual_expected, ekf.state(), 1e-9)); - EXPECT(assert_equal(P_updated_manual_expected, ekf.covariance(), 1e-9)); + EXPECT(assert_equal(pUpdatedManualExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceManualExpected, ekf.covariance(), 1e-9)); } - - int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 87797c687e0219b7e7ab66092b49821da9ee5109 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 10 May 2025 10:29:47 -0400 Subject: [PATCH 10/10] Fix bug in Debug --- gtsam/navigation/tests/testLieGroupEKF.cpp | 20 +++++++++----------- gtsam/navigation/tests/testManifoldEKF.cpp | 20 +++++++------------- 2 files changed, 16 insertions(+), 24 deletions(-) diff --git a/gtsam/navigation/tests/testLieGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp index f70511293..031ab743c 100644 --- a/gtsam/navigation/tests/testLieGroupEKF.cpp +++ b/gtsam/navigation/tests/testLieGroupEKF.cpp @@ -126,19 +126,17 @@ namespace exampleLieGroupDynamicMatrix { 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 + // Measurement function h(X, H) + double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + // Specialized for a 2x2 matrix! + if (p.rows() != 2 || p.cols() != 2) { + throw std::invalid_argument("Matrix must be 2x2."); } - // 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] + if (H) { + H->resize(1, p.size()); + *H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11 } - return p(0, 0) + p(1, 1); // trace + return p(0, 0) + p(1, 1); // Trace of the matrix } } // namespace exampleLieGroupDynamicMatrix diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index ff7a9a68d..50edbe049 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -151,27 +151,21 @@ TEST(ManifoldEKF_Unit3, Update) { namespace exampleDynamicMatrix { // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. - // For Matrix (a VectorSpace type), Retract(M, v) is typically M + v. Matrix f(const Matrix& p, const Vector& vTangent, double dt) { - return traits::Retract(p, vTangent * dt); + return traits::Retract(p, vTangent * dt); // + } // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) - // H is the Jacobian dh/d(flatten(p)) double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + // Specialized for a 2x2 matrix! + if (p.rows() != 2 || p.cols() != 2) { + throw std::invalid_argument("Matrix must be 2x2."); + } if (H) { - // The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size(). - // For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1]. H->resize(1, p.size()); - (*H)(0, 0) = 1.0; // d(trace)/dp00 - (*H)(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) + *H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11 } - if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error - double trace = 0.0; - for (DenseIndex i = 0; i < std::min(p.rows(), p.cols()); ++i) { - trace += p(i, i); - } - return trace; + return p(0, 0) + p(1, 1); // Trace of the matrix } } // namespace exampleDynamicMatrix