Now handle dynamic types

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 {
 
@@ -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 <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.
-    using Jacobian = typename Base::Jacobian; ///< Jacobian matrix type specific to the group G
+    // 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
+     * 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 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<G>::AdjointMap should exist
-      Jacobian A = traits<G>::Inverse(U).AdjointMap();
+      const Jacobian A = traits<G>::Inverse(U).AdjointMap();
+      // P_ is Matrix. A is Eigen::Matrix<double,n,n>. 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<G>::Expmap(u * dt);
+      const G U = traits<G>::Expmap(u * dt);
       predict(U, Q); // Call the group composition predict
     }
 

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<G>::value`).
+   *       Must satisfy LieGroup concept (`IsLieGroup<G>::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<G> {
   public:
     using Base = ManifoldEKF<G>; ///< 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<G>::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
+    // Jacobian for group-specific operations like Adjoint, Expmap derivatives.
-    using Jacobian = Eigen::Matrix<double, n, n>; ///< Jacobian matrix type specific to the group G
+    // Becomes Matrix if n is Eigen::Dynamic.
+    using Jacobian = Eigen::Matrix<double, n, n>;
 
-    /// 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<G>::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<n, n> Df)
      * Df = d(xi)/d(local(X))
+     * If n is Eigen::Dynamic, OptionalJacobian<n,n> is OptionalJacobian<Dynamic,Dynamic>.
      */
     template <typename Dynamics>
     using enable_if_dynamics = std::enable_if_t<
@@ -79,7 +86,7 @@ namespace gtsam {
      * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<n,n>&)
      * @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>>
@@ -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<n,n>&)
      * @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 <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
     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 <typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
     void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {

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 <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 {
@@ -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
+     *           - `traits<M>` specialization must exist, defining
-     *           - using TangentVector = Eigen::Vector...
+     *             `dimension` (compile-time or Eigen::Dynamic),
-     *           - A `retract(const TangentVector&)` method (member or static)
+     *             `TangentVector`, `Retract`, and `LocalCoordinates`.
-     *           - A `localCoordinates(const M&)` method (member or static)
+     *             If `dimension` is Eigen::Dynamic, `GetDimension(const M&)`
-     *           - `gtsam::traits<M>` specialization must exist.
+     *             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
+     * tangent space at X. The covariance P is always stored as a gtsam::Matrix.
-     * and the state transition Jacobian F. Updates apply a measurement function h
+     * Prediction requires providing the predicted next state and the state transition Jacobian F.
-     * and correct the state using the tangent space error.
+     * Updates apply a measurement function h and correct the state using the tangent space error.
      */
   template <typename M>
   class ManifoldEKF {
   public:
-    /// Manifold dimension (tangent space dimension)
+    /// Tangent vector type for the manifold M, as defined by its traits.
-    static constexpr int n = traits<M>::dimension;
-
-    /// Tangent vector type for the manifold M
     using TangentVector = typename traits<M>::TangentVector;
 
-    /// Square matrix of size n for covariance and Jacobians
+    /**
-    using MatrixN = Eigen::Matrix<double, n, n>;
+     * 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<M>::value,
+        "Template parameter M must be a GTSAM Manifold.");
 
-    /// Constructor: initialize with state and covariance
+      // Determine tangent space dimension n_ at runtime.
-    ManifoldEKF(const M& X0, const MatrixN& P0) : X_(X0), P_(P0) {
+      if constexpr (traits<M>::dimension == Eigen::Dynamic) {
-      static_assert(IsManifold<M>::value, "Template parameter M must be a GTSAM Manifold");
+        // If M::dimension is dynamic, traits<M>::GetDimension(M) must exist.
+        n_ = traits<M>::GetDimension(X0);
+      }
+      else {
+        n_ = traits<M>::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
+    /// @return current covariance estimate in the tangent space (always a Matrix).
-    const MatrixN& covariance() const { return P_; }
+    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<m>).
+     * @tparam Measurement Type of the measurement vector (e.g., VectorN<m>, 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 <typename Measurement>
     void update(const Measurement& prediction,
-      const Eigen::Matrix<double, traits<Measurement>::dimension, n>& H,
+      const Matrix& H,
-      const Measurement& z, const Matrix& R) {
+      const Measurement& z,
-      // Innovation z \ominus prediction
+      const Matrix& R) {
-      Vector innovation = traits<Measurement>::Local(z, prediction);
 
-      // Innovation covariance and Kalman Gain
+      static_assert(IsManifold<Measurement>::value,
-      auto S = H * P_ * H.transpose() + R; // S is m x m
+        "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates.");
-      Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m)
 
-      // Correction vector in tangent space
+      int m; // Measurement dimension
-      TangentVector delta_xi = -K * innovation; // delta_xi is n x 1
+      if constexpr (traits<Measurement>::dimension == Eigen::Dynamic) {
+        m = traits<Measurement>::GetDimension(z);
+        if (traits<Measurement>::GetDimension(prediction) != m) {
+          throw std::invalid_argument(
+            "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions.");
+        }
+      }
+      else {
+        m = traits<Measurement>::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<Measurement>::TangentVector innovation =
+        traits<Measurement>::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<M>::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<m>)
+     * @tparam Measurement Type of the measurement vector (e.g., VectorN<m>, Vector).
-     * @tparam Prediction Functor signature: Measurement h(const M&, OptionalJacobian<m,n>&)
+     * @tparam MeasurementFunction Functor/lambda with signature compatible with:
-     * @param h Measurement model functor returning predicted measurement prediction
+     *        `Measurement h(const M& x, Jac& H_jacobian)`
-     *          and its Jacobian H = d(h)/d(local(X)).
+     *        where `Jac` can be `Matrix&` or `OptionalJacobian<m, n_>&`.
+     *        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 <typename Measurement, typename Prediction>
+    template <typename Measurement, typename MeasurementFunction>
-    void update(Prediction&& h, const Measurement& z, const Matrix& R) {
+    void update(MeasurementFunction&& h, const Measurement& z, const Matrix& R) {
-      constexpr int m = traits<Measurement>::dimension;
+      static_assert(IsManifold<Measurement>::value,
+        "Template parameter Measurement must be a GTSAM Manifold.");
+
+      int m; // Measurement dimension
+      if constexpr (traits<Measurement>::dimension == Eigen::Dynamic) {
+        m = traits<Measurement>::GetDimension(z);
+      }
+      else {
+        m = traits<Measurement>::dimension;
+      }
+
       // Predict measurement and get Jacobian H = dh/dlocal(X)
-      Eigen::Matrix<double, m, n> 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
+    M X_;      ///< Manifold state estimate.
-    MatrixN P_; ///< covariance in tangent space at X_
+    Matrix P_; ///< Covariance in tangent space at X_ (always a dynamic Matrix).
-
+    int n_;    ///< Tangent space dimension of M, determined at construction.
-  private:
-    /// Identity matrix of size n
-    static const MatrixN I_n;
   };
 
-  // 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/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<Matrix>::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<Matrix> 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<Matrix> 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<Measurement>::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<Matrix>::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;