Retain efficiency in static case

gtsam/navigation/InvariantEKF.h

@@ -38,30 +38,32 @@ namespace gtsam {
    *
    * This filter inherits from LieGroupEKF but restricts the prediction interface
    * to only the left-invariant prediction methods:
-   * 1. Prediction via group composition: `predict(const G& U, const Matrix& Q)`
+   * 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.
-   * Covariances (P, Q) are `Matrix`.
+   * Covariances (P, Q) are `Eigen::Matrix<double, Dim, Dim>`.
    */
   template <typename G>
   class InvariantEKF : public LieGroupEKF<G> {
   public:
     using Base = LieGroupEKF<G>; ///< Base class type
     using TangentVector = typename Base::TangentVector; ///< Tangent vector type
-    // Jacobian for group-specific operations like AdjointMap.
+    /// Jacobian for group-specific operations like AdjointMap. Eigen::Matrix<double, Dim, Dim>.
-    // Becomes Matrix if G has dynamic dimension.
     using Jacobian = typename Base::Jacobian;
+    /// Covariance matrix type. Eigen::Matrix<double, Dim, Dim>.
+    using Covariance = typename Base::Covariance;
+
 
     /**
      * Constructor: forwards to LieGroupEKF constructor.
      * @param X0 Initial state on Lie group G.
-     * @param P0 Initial covariance in the tangent space at X0 (must be Matrix).
+     * @param P0_input Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix).
      */
-    InvariantEKF(const G& X0, const Matrix& P0) : Base(X0, P0) {}
+    InvariantEKF(const G& X0, const Matrix& P0_input) : Base(X0, P0_input) {}
 
-    // We hide state-dependent predict methods from LieGroupEKF by only providing the 
+    // We hide state-dependent predict methods from LieGroupEKF by only providing the
     // invariant predict methods below.
 
     /**
@@ -71,13 +73,14 @@ namespace gtsam {
      * where Ad_{U^{-1}} is the Adjoint map of U^{-1}.
      *
      * @param U Lie group element representing the motion increment.
-     * @param Q Process noise covariance in the tangent space (must be Matrix, size n_ x n_).
+     * @param Q Process noise covariance (Eigen::Matrix<double, Dim, Dim>).
      */
-    void predict(const G& U, const Matrix& Q) {
+    void predict(const G& U, const Covariance& Q) {
       this->X_ = this->X_.compose(U);
       // TODO(dellaert): traits<G>::AdjointMap should exist
       const Jacobian A = traits<G>::Inverse(U).AdjointMap();
-      // P_ is Matrix. A is Eigen::Matrix<double,n,n>. Q is Matrix.
+      // P_ is Covariance. A is Jacobian. Q is Covariance.
+      // All are Eigen::Matrix<double,Dim,Dim>.
       this->P_ = A * this->P_ * A.transpose() + Q;
     }
 
@@ -88,9 +91,9 @@ namespace gtsam {
      *
      * @param u Tangent space control vector.
      * @param dt Time interval.
-     * @param Q Process noise covariance matrix (Matrix, size n_ x n_).
+     * @param Q Process noise covariance matrix (Eigen::Matrix<double, Dim, Dim>).
      */
-    void predict(const TangentVector& u, double dt, const Matrix& Q) {
+    void predict(const TangentVector& u, double dt, const Covariance& Q) {
       const G U = traits<G>::Expmap(u * dt);
       predict(U, Q); // Call the group composition predict
     }

gtsam/navigation/LieGroupEKF.h

@@ -48,17 +48,16 @@ namespace gtsam {
   class LieGroupEKF : public ManifoldEKF<G> {
   public:
     using Base = ManifoldEKF<G>; ///< Base class type
-    // n is the tangent space dimension of G. If G::dimension is Eigen::Dynamic, n is Eigen::Dynamic.
+    static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G.
-    static constexpr int n = traits<G>::dimension;
+    using TangentVector = typename Base::TangentVector; ///< Tangent vector type for G.
-    using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G
+    /// Jacobian for group operations (Adjoint, Expmap derivatives, F).
-    // Jacobian for group-specific operations like Adjoint, Expmap derivatives.
+    using Jacobian = typename Base::Jacobian; // Eigen::Matrix<double, Dim, Dim>
-    // Becomes Matrix if n is Eigen::Dynamic.
+    using Covariance = typename Base::Covariance; // Eigen::Matrix<double, Dim, Dim>
-    using Jacobian = Eigen::Matrix<double, n, n>;
 
     /**
      * Constructor: initialize with state and covariance.
      * @param X0 Initial state on Lie group G.
-     * @param P0 Initial covariance in the tangent space at X0 (must be Matrix).
+     * @param P0 Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix).
      */
     LieGroupEKF(const G& X0, const Matrix& P0) : Base(X0, P0) {
       static_assert(IsLieGroup<G>::value, "Template parameter G must be a GTSAM Lie Group");
@@ -66,38 +65,39 @@ namespace gtsam {
 
     /**
      * SFINAE check for correctly typed state-dependent dynamics function.
-     * Signature: TangentVector f(const G& X, OptionalJacobian<n, n> Df)
+     * Signature: TangentVector f(const G& X, OptionalJacobian<Dim, Dim> 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<
       !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 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) {
         // Full state transition Jacobian for left-invariant EKF:
+        // AdjointMap returns Jacobian (Eigen::Matrix<double,Dim,Dim>)
         *A = traits<G>::Inverse(U).AdjointMap() + Dexp * Df * dt;
       }
       return X_next;
@@ -106,29 +106,32 @@ namespace gtsam {
 
     /**
      * Predict step with state-dependent dynamics:
-     * Uses predictMean to compute X_{k+1} and F, then updates covariance.
+     * 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 (Matrix, size n_ x n_).
+     * @param Q Process noise covariance (Eigen::Matrix<double, Dim, Dim>).
      */
     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;
+      Jacobian A; // Eigen::Matrix<double, Dim, Dim>
+      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>&>
     >;
 
     /**
@@ -137,7 +140,7 @@ namespace gtsam {
      *   xi = f(X_k, u, Df)
      *
      * @tparam Control Control input type.
-     * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<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.
@@ -145,8 +148,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);
     }
 
 
@@ -156,15 +159,18 @@ namespace gtsam {
      *   xi = f(X_k, u, Df)
      *
      * @tparam Control Control input type.
-     * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<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 (must be Matrix, size n_ x n_).
+     * @param Q Process noise covariance (Eigen::Matrix<double, Dim, Dim>).
      */
     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);
+      // Note: The lambda below captures f by reference. Ensure f's lifetime.
+      // If f is an rvalue, std::forward or std::move might be needed if the lambda is stored.
+      // Here, the lambda is temporary, so [&] is fine.
+      return predict([&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); }, dt, Q);
     }
 
   }; // LieGroupEKF

gtsam/navigation/ManifoldEKF.h

@@ -19,7 +19,7 @@
   * localCoordinates operations.
   *
   * Works with manifolds M that may have fixed or dynamic tangent space dimensions.
-  * Covariances and Jacobians are handled as `Matrix` (dynamic-size Eigen matrices).
+  * Covariances and Jacobians leverage Eigen's static or dynamic matrices for efficiency.
   *
   * @date    April 24, 2025
   * @authors Scott Baker, Matt Kielo, Frank Dellaert
@@ -51,33 +51,41 @@ namespace gtsam {
      *             must be provided by traits.
      *
      * This filter maintains a state X in the manifold M and covariance P in the
-     * tangent space at X. The covariance P is always stored as a gtsam::Matrix.
+     * tangent space at X. The covariance P is `Eigen::Matrix<double, Dim, Dim>`,
+     * where Dim is the compile-time dimension of M (or Eigen::Dynamic).
      * Prediction requires providing the predicted next state and the state transition Jacobian F.
      * Updates apply a measurement function h and correct the state using the tangent space error.
      */
   template <typename M>
   class ManifoldEKF {
   public:
-    /// Tangent vector type for the manifold M, as defined by its traits.
+    /// Compile-time dimension of the manifold M.
+    static constexpr int Dim = traits<M>::dimension;
+
+    /// 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>;
+
 
     /**
      * 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
+     * @param P0 Initial covariance in the tangent space at X0.
-     *           Matrix whose dimensions match the tangent space dimension of X0.
+     *                 Provided as a dynamic gtsam::Matrix for flexibility,
+     *                 but will be stored internally as Covariance.
      */
-    ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0), P_(P0) {
+    ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0) {
       static_assert(IsManifold<M>::value,
         "Template parameter M must be a GTSAM Manifold.");
 
-      // Determine tangent space dimension n_ at runtime.
+      if constexpr (Dim == Eigen::Dynamic) {
-      if constexpr (traits<M>::dimension == Eigen::Dynamic) {
+        n_ = traits<M>::GetDimension(X0); // Runtime dimension for dynamic case
-        // If M::dimension is dynamic, traits<M>::GetDimension(M) must exist.
-        n_ = traits<M>::GetDimension(X0);
       }
       else {
-        n_ = traits<M>::dimension;
+        n_ = Dim; // Runtime dimension is fixed compile-time dimension
       }
 
       // Validate dimensions of initial covariance P0.
@@ -88,6 +96,7 @@ namespace gtsam {
           ") do not match state's tangent space dimension (" +
           std::to_string(n_) + ").");
       }
+      P_ = P0; // Assigns MatrixXd to Eigen::Matrix<double,Dim,Dim>
     }
 
     virtual ~ManifoldEKF() = default;
@@ -95,8 +104,11 @@ namespace gtsam {
     /// @return current state estimate on manifold M.
     const M& state() const { return X_; }
 
-    /// @return current covariance estimate in the tangent space (always a Matrix).
+    /// @return current covariance estimate (Eigen::Matrix<double, Dim, Dim>).
-    const Matrix& covariance() const { return P_; }
+    const Covariance& covariance() const { return P_; }
+
+    /// @return runtime dimension of the tangent space.
+    int dimension() const { return n_; }
 
     /**
      * Basic predict step: Updates state and covariance given the predicted
@@ -107,22 +119,19 @@ namespace gtsam {
      * state transition in local coordinates around X_k.
      *
      * @param X_next The predicted state at time k+1 on manifold M.
-     * @param F The state transition Jacobian (size nxn).
+     * @param F The state transition Jacobian (Eigen::Matrix<double, Dim, Dim>).
-     * @param Q Process noise covariance matrix in the tangent space (size nxn).
+     * @param Q Process noise covariance matrix (Eigen::Matrix<double, Dim, Dim>).
      */
-    void predict(const M& X_next, const Matrix& F, const Matrix& Q) {
+    void predict(const M& X_next, const Jacobian& F, const Covariance& Q) {
-      if (F.rows() != n_ || F.cols() != n_) {
+      if constexpr (Dim == Eigen::Dynamic) {
-        throw std::invalid_argument(
+        if (F.rows() != n_ || F.cols() != n_ || Q.rows() != n_ || Q.cols() != n_) {
-          "ManifoldEKF::predict: Jacobian F dimensions (" +
+          throw std::invalid_argument(
-          std::to_string(F.rows()) + "x" + std::to_string(F.cols()) +
+            "ManifoldEKF::predict: Dynamic F/Q dimensions must match state dimension " +
-          ") must be " + std::to_string(n_) + "x" + std::to_string(n_) + ".");
+            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_) + ".");
       }
+      // For fixed Dim, types enforce dimensions.
+
       X_ = X_next;
       P_ = F * P_ * F.transpose() + Q;
     }
@@ -134,64 +143,75 @@ namespace gtsam {
      * @tparam Measurement Type of the measurement vector (e.g., VectorN<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.
+     *          Type: Eigen::Matrix<double, MeasDim, Dim>.
      * @param z Observed measurement.
-     * @param R Measurement noise covariance (size m x m).
+     * @param R Measurement noise covariance (Eigen::Matrix<double, MeasDim, MeasDim>).
      */
     template <typename Measurement>
     void update(const Measurement& prediction,
-      const Matrix& H,
+      const Eigen::Matrix<double, traits<Measurement>::dimension, Dim>& H,
       const Measurement& z,
-      const Matrix& R) {
+      const Eigen::Matrix<double, traits<Measurement>::dimension, traits<Measurement>::dimension>& R) {
 
       static_assert(IsManifold<Measurement>::value,
         "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates.");
 
-      int m; // Measurement dimension
+      static constexpr int MeasDim = traits<Measurement>::dimension;
-      if constexpr (traits<Measurement>::dimension == Eigen::Dynamic) {
+
-        m = traits<Measurement>::GetDimension(z);
+      int m_runtime; // Measurement dimension at runtime
-        if (traits<Measurement>::GetDimension(prediction) != m) {
+      if constexpr (MeasDim == Eigen::Dynamic) {
+        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.");
         }
+        // Runtime check for H and R if they involve dynamic dimensions
+        if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || R.cols() != m_runtime) {
+          throw std::invalid_argument(
+            "ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch for dynamic measurement.");
+        }
       }
       else {
-        m = traits<Measurement>::dimension;
+        m_runtime = MeasDim;
-      }
+        // Types enforce dimensions for H and R if MeasDim and Dim are fixed.
-
+        // If Dim is dynamic but MeasDim is fixed, H.cols() needs check.
-      if (H.rows() != m || H.cols() != n_) {
+        if constexpr (Dim == Eigen::Dynamic) {
-        throw std::invalid_argument(
+          if (H.cols() != n_) {
-          "ManifoldEKF::update: Jacobian H dimensions (" +
+            throw std::invalid_argument(
-          std::to_string(H.rows()) + "x" + std::to_string(H.cols()) +
+              "ManifoldEKF::update: Jacobian H columns must match state dimension " + std::to_string(n_) + ".");
-          ") 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
+      // S will be Eigen::Matrix<double, MeasDim, MeasDim>
+      Eigen::Matrix<double, MeasDim, MeasDim> S = H * P_ * H.transpose() + R;
 
       // Kalman Gain: K = P H^T S^-1
-      const Matrix K = P_ * H.transpose() * S.inverse(); // K is n_ x m
+      // K will be Eigen::Matrix<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 n_ x 1
+      const TangentVector delta_xi = K * innovation; // delta_xi is Dim x 1 (or n_ x 1 if dynamic)
 
       // Update state using retract: X_new = retract(X_old, delta_xi)
       X_ = traits<M>::Retract(X_, delta_xi);
 
       // Update covariance using Joseph form for numerical stability
-      const auto I_n = Matrix::Identity(n_, n_);
+      Jacobian I_n; // Eigen::Matrix<double, Dim, Dim>
-      const Matrix I_KH = I_n - K * H; // I_KH is n x n
+      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();
     }
 
@@ -202,37 +222,48 @@ namespace gtsam {
      * @tparam Measurement Type of the measurement vector (e.g., VectorN<m>, Vector).
      * @tparam MeasurementFunction Functor/lambda with signature compatible with:
      *        `Measurement h(const M& x, Jac& H_jacobian)`
-     *        where `Jac` can be `Matrix&` or `OptionalJacobian<m, n_>&`.
+     *        where `Jac` can be `Eigen::Matrix<double, MeasDim, Dim>&` or
+     *        `OptionalJacobian<MeasDim, Dim>&`.
      *        The Jacobian H should be d(h)/d(local(X)).
      * @param h Measurement model function.
      * @param z Observed measurement.
-     * @param R Measurement noise covariance (must be an m x m Matrix).
+     * @param R Measurement noise covariance (Eigen::Matrix<double, MeasDim, MeasDim>).
      */
     template <typename Measurement, typename MeasurementFunction>
-    void update(MeasurementFunction&& h, const Measurement& z, const Matrix& R) {
+    void update(MeasurementFunction&& h_func, 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.");
 
-      int m; // Measurement dimension
+      static constexpr int MeasDim = traits<Measurement>::dimension;
-      if constexpr (traits<Measurement>::dimension == Eigen::Dynamic) {
+
-        m = traits<Measurement>::GetDimension(z);
+      int m_runtime; // Measurement dimension at runtime
+      if constexpr (MeasDim == Eigen::Dynamic) {
+        m_runtime = traits<Measurement>::GetDimension(z);
       }
       else {
-        m = traits<Measurement>::dimension;
+        m_runtime = MeasDim;
       }
 
+      // Prepare Jacobian H for the measurement function
+      Matrix H(m_runtime, n_);
+      if constexpr (MeasDim == Eigen::Dynamic || Dim == Eigen::Dynamic) {
+        // If H involves any dynamic dimension, it needs explicit resizing.
+        H.resize(m_runtime, n_);
+      }
+      // If H is fully fixed-size, its dimensions are set at compile time.
+
       // Predict measurement and get Jacobian H = dh/dlocal(X)
-      Matrix H(m, n_);
+      Measurement prediction = h_func(X_, H);
-      Measurement prediction = h(X_, H);
 
       // Call the other update function
       update(prediction, H, z, R);
     }
 
   protected:
-    M X_;      ///< Manifold state estimate.
+    M X_;              ///< Manifold state estimate.
-    Matrix P_; ///< Covariance in tangent space at X_ (always a dynamic Matrix).
+    Covariance P_;     ///< Covariance (Eigen::Matrix<double, Dim, Dim>).
-    int n_;    ///< Tangent space dimension of M, determined at construction.
+    int n_;            ///< Runtime tangent space dimension of M.
   };
 
 }  // namespace gtsam

gtsam/navigation/tests/testManifoldEKF.cpp

@@ -36,14 +36,14 @@ namespace exampleUnit3 {
 
   // Define a measurement model: measure the z-component of the Unit3 direction
   // H is the Jacobian dh/d(local(p))
-  Vector1 measureZ(const Unit3& p, OptionalJacobian<1, 2> H) {
+  double measureZ(const Unit3& p, OptionalJacobian<1, 2> H) {
     if (H) {
       // H = d(p.point3().z()) / d(local(p))
       // Calculate numerically for simplicity in test
-      auto h = [](const Unit3& p_) { return Vector1(p_.point3().z()); };
+      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
@@ -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
@@ -156,16 +156,15 @@ namespace exampleDynamicMatrix {
   }
 
   // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here)
-  Vector1 measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
+  double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
     if (H) {
       // p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11]
       // trace = p(0,0) + p(1,1)
       // H = d(trace)/d(p_flat) = [1, 0, 0, 1]
       // The Jacobian H will be 1x4 for a 2x2 matrix.
-      H->resize(1, 4);
       *H << 1.0, 0.0, 0.0, 1.0;
     }
-    return Vector1(p(0, 0) + p(1, 1));
+    return p(0, 0) + p(1, 1);
   }
 
 } // namespace exampleDynamicMatrix
@@ -223,8 +222,8 @@ TEST(ManifoldEKF_DynamicMatrix, Update) {
   EXPECT_LONGS_EQUAL(4, ekf.state().size());
 
   // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0)
-  Vector1 zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here
+  double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here
-  Vector1 zObserved = zTrue - Vector1(0.03); // Add some "error"
+  double zObserved = zTrue - 0.03; // Add some "error"
 
   // --- Perform EKF update ---
   ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance);
@@ -232,12 +231,12 @@ TEST(ManifoldEKF_DynamicMatrix, Update) {
   // --- Verification (Manual Kalman Update Steps) ---
   // 1. Predict measurement and get Jacobian H
   Matrix H(1, 4); // This will be 1x4 for a 2x2 matrix measurement
-  Vector1 zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H);
+  double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H);
 
   // 2. Innovation and Innovation Covariance
   // EKF calculates innovation_tangent = traits<Measurement>::Local(prediction, zObserved)
-  // For Vector1 (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual.
+  // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual.
-  Vector1 innovationY = zObserved - zPredictionManual;
+  double innovationY = zObserved - zPredictionManual;
   Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance;
 
   // 3. Kalman Gain K