Merge pull request #2129 from borglab/feature/dynamicSizes

Allow for dynamic M/G in new EKF variants

gtsam/base/Lie.h

@@ -169,32 +169,34 @@ namespace internal {
 /// To use this for your gtsam type, define:
 /// template<> struct traits<Class> : public internal::LieGroupTraits<Class> {};
 /// 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<class Class>
-struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
+struct LieGroupTraits : public GetDimensionImpl<Class, Class::dimension> {
   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<double, dimension, 1>;
   using ChartJacobian = OptionalJacobian<dimension, dimension>;
 
   static TangentVector Local(const Class& origin, const Class& other,
-      ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+    ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
-    return origin.localCoordinates(other, Horigin, Hother);
+    return origin.localCoordinates(other, H1, H2);
   }
 
   static Class Retract(const Class& origin, const TangentVector& v,
-      ChartJacobian Horigin = {}, ChartJacobian Hv = {}) {
+    ChartJacobian H = {}, ChartJacobian Hv = {}) {
-    return origin.retract(v, Horigin, Hv);
+    return origin.retract(v, H, Hv);
   }
   /// @}
 
@@ -209,19 +211,26 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
   }
 
   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<double, dimension, dimension> 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();
+  }
   /// @}
 };
 

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) {
+  static LieAlgebra Hat(const TangentVector& v) { return v; }
-    return v;
-  }
 
-  static TangentVector Vee(const LieAlgebra& X) {
+  static TangentVector Vee(const LieAlgebra& X) { return X; }
-    return X;
+
+  static Jacobian AdjointMap(const Class& /*m*/) {
+    return Jacobian::Identity();
   }
   /// @}
 };
@@ -118,7 +118,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
       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<Class,Eigen::Dynamic> {
 
   static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
     Class result(v);
-    if (Hv)
+    if (Hv) *Hv = Eye(v);
-      *Hv = Eye(v);
     return result;
   }
 
@@ -165,6 +164,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
     return -v;
   }
 
+  static Eigen::MatrixXd AdjointMap(const Class& m) { return Eye(m); }
   /// @}
 };
 
@@ -273,8 +273,8 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
     if (H) (*H)[0] = 1.0;
     return v[0];
   }
+  // AdjointMap for ScalarTraits is inherited from VectorSpaceImpl<Scalar, 1>
   /// @}
-
 };
 
 } // namespace internal
@@ -352,6 +352,9 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
     if (H) *H = Jacobian::Identity();
     return m + Eigen::Map<const Fixed>(v.data());
   }
+
+  // AdjointMap for fixed-size Eigen matrices is inherited from
+  // internal::VectorSpaceImpl< Eigen::Matrix<double, M, N, ...> , 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<Dynamic>(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<Dynamic>(result.data(), m.cols(), m.rows()) = m;
+    Eigen::Map<Dynamic>(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
-

gtsam/navigation/InvariantEKF.h

@@ -25,6 +25,8 @@
 #pragma once
 
 #include <gtsam/navigation/LieGroupEKF.h> // Include the base class
+#include <gtsam/base/Lie.h> // 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)`
+   * 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 Matrix& 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 <typename G>
   class InvariantEKF : public LieGroupEKF<G> {
   public:
     using Base = LieGroupEKF<G>; ///< Base class type
     using TangentVector = typename Base::TangentVector; ///< Tangent vector type
-    using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc.
+    /// Jacobian for group-specific operations like AdjointMap. Eigen::Matrix<double, Dim, Dim>.
-    using Jacobian = typename Base::Jacobian; ///< Jacobian matrix type specific to the group G
+    using Jacobian = typename Base::Jacobian;
+    /// Covariance matrix type. Eigen::Matrix<double, Dim, Dim>.
+    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
+     * where Ad_{U^{-1}} is the Adjoint map of U^{-1}.
-     * F = Ad_{U^{-1}} in the base class predict method.
      *
      * @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) {
+    void predict(const G& U, const Covariance& Q) {
-      this->X_ = this->X_.compose(U);
+      this->X_ = traits<G>::Compose(this->X_, U);
-      // TODO(dellaert): traits<G>::AdjointMap should exist
+      const G U_inv = traits<G>::Inverse(U);
-      Jacobian A = traits<G>::Inverse(U).AdjointMap();
+      const Jacobian A = traits<G>::AdjointMap(U_inv);
+      // P_ is Covariance. A is Jacobian. Q is Covariance.
+      // All are Eigen::Matrix<double,Dim,Dim>.
       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) {
+    void predict(const TangentVector& u, double dt, const Covariance& Q) {
-      G U = traits<G>::Expmap(u * dt);
+      G U;
+      if constexpr (std::is_same_v<G, Matrix>) {
+        // Specialize to Matrix case as its Expmap is not defined.
+        const Matrix& X = static_cast<const Matrix&>(this->X_);
+        U.resize(X.rows(), X.cols());
+        Eigen::Map<Vector>(static_cast<Matrix&>(U).data(), U.size()) = u * dt;
+      }
+      else {
+        U = traits<G>::Expmap(u * dt);
+      }
       predict(U, Q); // Call the group composition predict
     }
 

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.
+   * @tparam G Lie group type (must satisfy LieGroup concept).
-   *       Must satisfy LieGroup concept (`gtsam::IsLieGroup<G>::value`).
    *
    * 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 <typename G>
   class LieGroupEKF : public ManifoldEKF<G> {
   public:
     using Base = ManifoldEKF<G>; ///< Base class type
-    static constexpr int n = Base::n; ///< Group dimension (tangent space dimension)
+    static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G.
-    using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G
+    using TangentVector = typename Base::TangentVector; ///< Tangent vector type for G.
-    using MatrixN = typename Base::MatrixN; ///< Square matrix of size n for covariance and Jacobians
+    /// Jacobian for group operations (Adjoint, Expmap derivatives, F).
-    using Jacobian = Eigen::Matrix<double, n, n>; ///< Jacobian matrix type specific to the group G
+    using Jacobian = typename Base::Jacobian; // Eigen::Matrix<double, Dim, Dim>
+    using Covariance = typename Base::Covariance; // Eigen::Matrix<double, Dim, Dim>
 
-    /// 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<G>::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<n, n> Df)
+     * Signature: TangentVector f(const G& X, OptionalJacobian<Dim, Dim> Df)
      * Df = d(xi)/d(local(X))
      */
     template <typename Dynamics>
     using enable_if_dynamics = std::enable_if_t<
       !std::is_convertible_v<Dynamics, TangentVector>&&
       std::is_invocable_r_v<TangentVector, Dynamics, const G&,
-      OptionalJacobian<n, n>&>>;
+      OptionalJacobian<Dim, Dim>&>>;
 
     /**
      * 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)
+     *   X_{k+1} = X_k * U         (Predict next state)
-     *   F = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian)
+     *   A = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian)
      *
-     * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<n,n>&)
+     * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<Dim,Dim>&)
      * @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 <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
-    G predictMean(Dynamics&& f, double dt, OptionalJacobian<n, n> A = {}) const {
+    G predictMean(Dynamics&& f, double dt, OptionalJacobian<Dim, Dim> A = {}) const {
-      Jacobian Df, Dexp;
+      Jacobian Df, Dexp; // Eigen::Matrix<double, Dim, Dim>
-      TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX)
-      G U = traits<G>::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt
-      G X_next = this->X_.compose(U);
 
-      if (A) {
+      if constexpr (std::is_same_v<G, Matrix>) {
-        // Full state transition Jacobian for left-invariant EKF:
+        // Specialize to Matrix case as its Expmap is not defined.
-        *A = traits<G>::Inverse(U).AdjointMap() + Dexp * Df * dt;
+        TangentVector xi = f(this->X_, A ? &Df : nullptr);
+        const Matrix nextX = traits<Matrix>::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<G>::Expmap(xi * dt, A ? &Dexp : nullptr);
+        if (A) {
+          // State transition Jacobian for left-invariant EKF:
+          *A = traits<G>::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.
+     * Uses predictMean to compute X_{k+1} and A, then updates covariance.
-     *   X_{k+1}, F = predictMean(f, dt)
+     *   X_{k+1}, A = predictMean(f, dt)
-     *   P_{k+1} = F P_k F^T + Q
+     *   P_{k+1} = A P_k A^T + Q
      *
-     * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<n,n>&)
+     * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<Dim,Dim>&)
      * @param f Dynamics functor.
      * @param dt Time step.
-     * @param Q Process noise covariance (size nxn).
+     * @param Q Process noise covariance.
      */
     template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
-    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<Dynamics>(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<n, n> Df)
+     * Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian<Dim, Dim> Df)
      */
     template<typename Control, typename Dynamics>
     using enable_if_full_dynamics = std::enable_if_t<
-      std::is_invocable_r_v<TangentVector, Dynamics, const G&, const Control&, OptionalJacobian<n, n>&>
+      std::is_invocable_r_v<TangentVector, Dynamics, const G&, const Control&, OptionalJacobian<Dim, Dim>&>
     >;
 
     /**
@@ -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<n,n>&)
+     * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<Dim,Dim>&)
      * @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 <typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
-    G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian<n, n> A = {}) const {
+    G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian<Dim, Dim> A = {}) const {
-      return predictMean([&](const G& X, OptionalJacobian<n, n> Df) { return f(X, u, Df); }, dt, A);
+      return predictMean([&](const G& X, OptionalJacobian<Dim, Dim> 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<n,n>&)
+     * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<Dim,Dim>&)
      * @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 <typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
-    void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
+    void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) {
-      return predict([&](const G& X, OptionalJacobian<n, n> Df) { return f(X, u, Df); }, dt, Q);
+      return predict([&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); }, dt, Q);
     }
 
   }; // LieGroupEKF

gtsam/navigation/ManifoldEKF.h

@@ -27,53 +27,91 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/Vector.h>
-#include <gtsam/base/Manifold.h> // Include for traits
+#include <gtsam/base/Manifold.h> // Include for traits and IsManifold
 
 #include <Eigen/Dense>
+#include <string>
+#include <stdexcept>
 #include <type_traits>
 
 namespace gtsam {
 
   /**
-     * @class ManifoldEKF
+   * @class ManifoldEKF
-     * @brief Extended Kalman Filter on a generic manifold M
+   * @brief Extended Kalman Filter on a generic manifold M
-     *
+   *
-     * @tparam M  Manifold type providing:
+   * @tparam M  Manifold type (must satisfy Manifold concept).
-     *           - static int dimension = tangent dimension
+   *
-     *           - using TangentVector = Eigen::Vector...
+   * This filter maintains a state X in the manifold M and covariance P in the
-     *           - A `retract(const TangentVector&)` method (member or static)
+   * tangent space at X.
-     *           - A `localCoordinates(const M&)` method (member or static)
+   * Prediction requires providing the predicted next state and the state transition Jacobian F.
-     *           - `gtsam::traits<M>` specialization must exist.
+   * Updates apply a measurement function h and correct the state using the tangent space error.
-     *
+   *
-     * This filter maintains a state X in the manifold M and covariance P in the
+   * **Handling Static and Dynamic Dimensions:**
-     * tangent space at X. Prediction requires providing the predicted next state
+   * The filter supports manifolds M with either a compile-time fixed dimension or a
-     * and the state transition Jacobian F. Updates apply a measurement function h
+   * runtime dynamic dimension. This is determined by `gtsam::traits<M>::dimension`.
-     * and correct the state using the tangent space error.
+   * - 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<M>::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<double, Dim, Dim>`, where `Dim` is `traits<M>::dimension`.
+   * For dynamically-sized manifolds (`Dim == Eigen::Dynamic`), these Eigen types
+   * represent dynamically-sized matrices.
+   */
   template <typename M>
   class ManifoldEKF {
   public:
-    /// Manifold dimension (tangent space dimension)
+    /// Compile-time dimension of the manifold M.
-    static constexpr int n = traits<M>::dimension;
+    static constexpr int Dim = traits<M>::dimension;
 
-    /// Tangent vector type for the manifold M
+    /// Tangent vector type for the manifold M.
     using TangentVector = typename traits<M>::TangentVector;
+    /// Covariance matrix type (P, Q).
+    using Covariance = Eigen::Matrix<double, Dim, Dim>;
+    /// State transition Jacobian type (F).
+    using Jacobian = Eigen::Matrix<double, Dim, Dim>;
 
-    /// Square matrix of size n for covariance and Jacobians
-    using MatrixN = Eigen::Matrix<double, n, n>;
 
-    /// Constructor: initialize with state and covariance
+    /**
-    ManifoldEKF(const M& X0, const MatrixN& P0) : X_(X0), P_(P0) {
+     * Constructor: initialize with state and covariance.
-      static_assert(IsManifold<M>::value, "Template parameter M must be a GTSAM Manifold");
+     * @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<M>::value,
+        "Template parameter M must be a GTSAM Manifold.");
+
+      if constexpr (Dim == Eigen::Dynamic) {
+        n_ = traits<M>::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
+    /// @return current covariance estimate.
-    const MatrixN& covariance() const { return P_; }
+    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 X_next The predicted state at time k+1 on manifold M.
-     * @param F The state transition Jacobian (size nxn).
+     * @param F The state transition Jacobian.
-     * @param Q Process noise covariance matrix in the tangent space (size nxn).
+     * @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.
+     * Measurement update: Corrects the state and covariance using a pre-calculated
-     *   z_pred, H = h(X)
+     * predicted measurement and its Jacobian.
-     *   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<m>)
+     * @tparam Measurement Type of the measurement vector (e.g., VectorN<m>, Vector).
-     * @tparam Prediction Functor signature: Measurement h(const M&,
+     * @param prediction Predicted measurement.
-     * OptionalJacobian<m,n>&)
+     * @param H Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X).
-     *                    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)).
      * @param z Observed measurement.
-     * @param R Measurement noise covariance (size m x m).
+     * @param R Measurement noise covariance.
      */
-    template <typename Measurement, typename Prediction>
+    template <typename Measurement>
-    void update(Prediction&& h, const Measurement& z, const Matrix& R) {
+    void update(const Measurement& prediction,
-      constexpr int m = traits<Measurement>::dimension;
+      const Eigen::Matrix<double, traits<Measurement>::dimension, Dim>& H,
-      Eigen::Matrix<double, m, n> H;
+      const Measurement& z,
+      const Eigen::Matrix<double, traits<Measurement>::dimension, traits<Measurement>::dimension>& R) {
 
-      // Predict measurement and get Jacobian H = dh/dlocal(X)
+      static_assert(IsManifold<Measurement>::value,
-      Measurement z_pred = h(X_, H);
+        "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates.");
 
-      // Innovation
+      static constexpr int MeasDim = traits<Measurement>::dimension;
-      // Ensure consistent subtraction for manifold types if Measurement is one
-      Vector innovation = traits<Measurement>::Local(z, z_pred); // y = z_pred (-) z (in tangent space)
 
-      // Innovation covariance and Kalman Gain
+      int m_runtime;
-      auto S = H * P_ * H.transpose() + R;
+      if constexpr (MeasDim == Eigen::Dynamic) {
-      Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m)
+        m_runtime = traits<Measurement>::GetDimension(z);
+        if (traits<Measurement>::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
+      // Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z)
-      TangentVector delta_xi = -K * innovation; // delta_xi = - K * y
+      typename traits<Measurement>::TangentVector innovation =
+        traits<Measurement>::Local(prediction, z);
 
-      // Update state using retract
+      // Innovation covariance: S = H P H^T + R
-      X_ = traits<M>::Retract(X_, delta_xi); // X <- X.retract(delta_xi)
+      // S will be Eigen::Matrix<double, MeasDim, MeasDim>
+      Eigen::Matrix<double, MeasDim, MeasDim> S = H * P_ * H.transpose() + R;
 
-      // Update covariance using Joseph form:
+      // Kalman Gain: K = P H^T S^-1
-      MatrixN I_KH = I_n - K * H;
+      // K will be Eigen::Matrix<double, Dim, MeasDim>
+      Eigen::Matrix<double, Dim, MeasDim> 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<M>::Retract(X_, delta_xi);
+
+      // Update covariance using Joseph form for numerical stability
+      Jacobian I_n; // Eigen::Matrix<double, Dim, Dim>
+      if constexpr (Dim == Eigen::Dynamic) {
+        I_n = Jacobian::Identity(n_, n_);
+      }
+      else {
+        I_n = Jacobian::Identity();
+      }
+
+      // I_KH will be Eigen::Matrix<double, Dim, Dim>
+      Jacobian I_KH = I_n - K * H;
       P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose();
     }
 
-  protected:
+    /**
-    M X_;      ///< manifold state estimate
+     * Measurement update: Corrects the state and covariance using a measurement
-    MatrixN P_; ///< covariance in tangent space at X_
+     * 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 <typename Measurement, typename MeasurementFunction>
+    void update(MeasurementFunction&& h, const Measurement& z,
+      const Eigen::Matrix<double, traits<Measurement>::dimension, traits<Measurement>::dimension>& R) {
+      static_assert(IsManifold<Measurement>::value,
+        "Template parameter Measurement must be a GTSAM Manifold.");
 
-  private:
+      static constexpr int MeasDim = traits<Measurement>::dimension;
-    /// Identity matrix of size n
+
-    static const MatrixN I_n;
+      int m_runtime;
+      if constexpr (MeasDim == Eigen::Dynamic) {
+        m_runtime = traits<Measurement>::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<double, Dim, Dim>).
+    int n_;            ///< Runtime tangent space dimension of M.
   };
 
-  // Define static identity I_n
-  template <typename M>
-  const typename ManifoldEKF<M>::MatrixN ManifoldEKF<M>::I_n = ManifoldEKF<M>::MatrixN::Identity();
-
 }  // namespace gtsam

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<Matrix>::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<Matrix> 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<Matrix>::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);

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<Eigen::Dynamic, Eigen::Dynamic> 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<Matrix> 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<Matrix>::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<Measurement>::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<Matrix>::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);

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()); };
+      auto h = [](const Unit3& p_) { return p_.point3().z(); };
-      *H = numericalDerivative11<Vector1, Unit3, 2>(h, p);
+      *H = numericalDerivative11<double, Unit3, 2>(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<Unit3, Unit3>(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<Unit3, Unit3>(predict_wrapper_zero, data.p0);
   EXPECT(assert_equal<Matrix2>(I_2x2, F_zero, 1e-8));
@@ -116,8 +116,8 @@ TEST(ManifoldEKF_Unit3, Update) {
   ManifoldEKF<Unit3> ekf(p_start, P_start);
 
   // Simulate a measurement (e.g., true value + noise)
-  Vector1 z_true = exampleUnit3::measureZ(p_start, {});
+  double z_true = exampleUnit3::measureZ(p_start, {});
-  Vector1 z_observed = z_true + Vector1(0.02); // Add some noise
+  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<Matrix>::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<Matrix> 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<Matrix>::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;