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 82a3f8fec..d33e0829b 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 { @@ -36,37 +38,50 @@ 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. + * For details on how static and dynamic dimensions are handled, please refer to + * the `ManifoldEKF` class documentation. */ 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. Eigen::Matrix. + using Jacobian = typename Base::Jacobian; + /// Covariance matrix type. Eigen::Matrix. + using Covariance = typename Base::Covariance; - /// 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. + */ + 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. /** * 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. */ - 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(); + void predict(const G& U, const Covariance& Q) { + 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; } @@ -77,10 +92,19 @@ 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. */ - void predict(const TangentVector& u, double dt, const Matrix& Q) { - G U = traits::Expmap(u * dt); + 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; + } + 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 fe16eaefe..007ed3ad0 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -37,91 +37,111 @@ 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 (`gtsam::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 { public: using Base = ManifoldEKF; ///< Base class type - static constexpr int n = Base::n; ///< Group dimension (tangent space 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 + 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 - 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. + */ + LieGroupEKF(const G& X0, const Covariance& P0) : Base(X0, P0) { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); } /** * 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)) */ 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 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 > - G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { - Jacobian Df, Dexp; - 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); + G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { + Jacobian Df, Dexp; // Eigen::Matrix - if (A) { - // Full state transition Jacobian for left-invariant EKF: - *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + if constexpr (std::is_same_v) { + // 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_); + *A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix + } + return nextX; + } + else { + 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; } /** * Predict step with state-dependent dynamics: - * Uses predictMean to compute X_{k+1} and F, then calls base predict. - * 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 (size nxn). + * @param Q Process noise covariance. */ template > - void predict(Dynamics&& f, double dt, const Matrix& Q) { + void predict(Dynamics&& f, double dt, const Covariance& Q) { Jacobian A; + 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&> >; /** @@ -130,7 +150,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. @@ -138,8 +158,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); } @@ -149,15 +169,15 @@ 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 (size nxn). + * @param Q Process noise covariance. */ 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) { + 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 7951b88cb..d45f1fdf9 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -27,53 +27,91 @@ #include #include #include -#include // Include for traits +#include // Include for traits and IsManifold #include +#include +#include #include namespace gtsam { /** - * @class ManifoldEKF - * @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. - * - * 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. - */ + * @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: - /// Manifold dimension (tangent space dimension) - static constexpr int n = traits::dimension; + /// Compile-time dimension of the manifold M. + static constexpr int Dim = traits::dimension; - /// Tangent vector type for the manifold M + /// 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; - /// Square matrix of size n for covariance and Jacobians - using MatrixN = Eigen::Matrix; - /// 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"); + /** + * Constructor: initialize with state and covariance. + * @param X0 Initial state on manifold M. + * @param P0 Initial covariance in the tangent space at 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); + // 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; + } + + P_ = P0; } - 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. + const Covariance& covariance() const { return P_; } + + /// @return runtime dimension of the manifold. + int dimension() const { return n_; } /** * Basic predict step: Updates state and covariance given the predicted @@ -83,73 +121,138 @@ 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 F The state transition Jacobian (size nxn). - * @param Q Process noise covariance matrix in the tangent space (size nxn). + * @param X_next The predicted state at time k+1 on manifold M. + * @param F The state transition Jacobian. + * @param Q Process noise covariance matrix. */ - void predict(const M& X_next, const MatrixN& F, const Matrix& Q) { + 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_) + "."); + } + } + X_ = X_next; P_ = F * P_ * F.transpose() + Q; } /** - * 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 + * 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 Prediction Functor signature: Measurement h(const M&, - * OptionalJacobian&) - * where m is the measurement dimension. - * - * @param h Measurement model functor returning predicted measurement z_pred - * and its Jacobian H = d(h)/d(local(X)). + * @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). * @param z Observed measurement. - * @param R Measurement noise covariance (size m x m). + * @param R Measurement noise covariance. */ - template - void update(Prediction&& h, const Measurement& z, const Matrix& R) { - constexpr int m = traits::dimension; - Eigen::Matrix H; + template + void update(const Measurement& prediction, + const Eigen::Matrix::dimension, Dim>& H, + const Measurement& z, + const Eigen::Matrix::dimension, traits::dimension>& R) { - // Predict measurement and get Jacobian H = dh/dlocal(X) - Measurement z_pred = h(X_, H); + static_assert(IsManifold::value, + "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates."); - // 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) + static constexpr int MeasDim = traits::dimension; - // 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) + 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."); + } + 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_runtime = MeasDim; + 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_) + "."); + } + } + } - // Correction vector in tangent space - TangentVector delta_xi = -K * innovation; // delta_xi = - K * y + // Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z) + typename traits::TangentVector innovation = + traits::Local(prediction, z); - // Update state using retract - X_ = traits::Retract(X_, delta_xi); // X <- X.retract(delta_xi) + // Innovation covariance: S = H P H^T + R + // S will be Eigen::Matrix + Eigen::Matrix S = H * P_ * H.transpose() + R; - // Update covariance using Joseph form: - MatrixN I_KH = I_n - K * H; + // Kalman Gain: K = P H^T S^-1 + // 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 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 + 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(); } - protected: - M X_; ///< manifold state estimate - MatrixN P_; ///< covariance in tangent space at X_ + /** + * Measurement update: Corrects the state and covariance using a measurement + * model function. + * + * @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. + */ + template + 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."); - private: - /// Identity matrix of size n - static const MatrixN I_n; + static constexpr int MeasDim = traits::dimension; + + int m_runtime; + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); + } + else { + m_runtime = MeasDim; + } + + // Predict measurement and get Jacobian H = dh/dlocal(X) + Matrix H(m_runtime, n_); + Measurement prediction = h(X_, H); + + // Call the other update function + update(prediction, H, z, R); + } + + protected: + M X_; ///< Manifold state estimate. + Covariance P_; ///< Covariance (Eigen::Matrix). + int n_; ///< Runtime tangent space dimension of M. }; - // 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/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 74a70fbbe..4797f79a1 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -119,6 +119,68 @@ TEST(IEKF_Pose2, PredictUpdateSequence) { } +// Define simple dynamics and measurement for a 2x2 Matrix state +namespace exampleDynamicMatrix { + Matrix f(const Matrix& p, const Vector& vTangent, double dt) { + return traits::Retract(p, vTangent * dt); + } + double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + if (H) { + H->resize(1, p.size()); + (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2 + } + return p(0, 0) + p(1, 1); // trace ! + } +} // namespace exampleDynamicMatrix + +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; + + InvariantEKF ekf(p0Matrix, p0Covariance); + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + + // --- 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 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/testLieGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp index 62a35473d..031ab743c 100644 --- a/gtsam/navigation/tests/testLieGroupEKF.cpp +++ b/gtsam/navigation/tests/testLieGroupEKF.cpp @@ -108,6 +108,92 @@ 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) + 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) { + 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 of the matrix + } +} // 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); diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 1d74bb704..50edbe049 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -30,20 +30,20 @@ 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); } // 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 @@ -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)); @@ -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 @@ -147,6 +147,87 @@ 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 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 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) { + 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 of the matrix + } + +} // namespace exampleDynamicMatrix + +TEST(ManifoldEKF_DynamicMatrix, CombinedPredictAndUpdate) { + 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(pInitial, pInitialCovariance); + // For a 2x2 Matrix, tangent space dimension is 2*2=4. + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(pInitial.rows() * pInitial.cols(), ekf.state().size()); + + // Predict Step + Matrix pPredictedMean = exampleDynamicMatrix::f(pInitial, vTangent, deltaTime); + + // 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(pPredictedMean, fJacobian, processNoiseCovariance); + + 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 pCurrentForUpdate = ekf.state(); + Matrix pCurrentCovarianceForUpdate = ekf.covariance(); + + // 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::h, zObserved, measurementNoiseCovariance); + + // Manual Kalman Update Steps for Verification + 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; + + Matrix kalmanGain = pCurrentCovarianceForUpdate * hJacobian.transpose() * innovationCovariance.inverse(); // K is 4x1 + + // State Correction (in tangent space of Matrix) + Vector deltaXiTangent = kalmanGain * yInnovation; // deltaXi is 4x1 Vector + + Matrix pUpdatedManualExpected = traits::Retract(pCurrentForUpdate, deltaXiTangent); + Matrix pUpdatedCovarianceManualExpected = (I_4x4 - kalmanGain * hJacobian) * pCurrentCovarianceForUpdate; + + EXPECT(assert_equal(pUpdatedManualExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceManualExpected, ekf.covariance(), 1e-9)); +} int main() { TestResult tr;