diff --git a/examples/GEKF_Rot3Example.cpp b/examples/IEKF_Rot3Example.cpp similarity index 100% rename from examples/GEKF_Rot3Example.cpp rename to examples/IEKF_Rot3Example.cpp diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index d33e0829b0..3286fc31bf 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -9,105 +9,106 @@ * -------------------------------------------------------------------------- */ - /** - * @file InvariantEKF.h - * @brief Left-Invariant Extended Kalman Filter implementation. - * - * This file defines the InvariantEKF class template, inheriting from LieGroupEKF, - * which specifically implements the Left-Invariant EKF formulation. It restricts - * prediction methods to only those based on group composition (state-independent - * motion models), hiding the state-dependent prediction variants from LieGroupEKF. - * - * @date April 24, 2025 - * @authors Scott Baker, Matt Kielo, Frank Dellaert - */ +/** + * @file InvariantEKF.h + * @brief Left-Invariant Extended Kalman Filter implementation. + * + * This file defines the InvariantEKF class template, inheriting from + * LieGroupEKF, which specifically implements the Left-Invariant EKF + * formulation. It restricts prediction methods to only those based on group + * composition (state-independent motion models), hiding the state-dependent + * prediction variants from LieGroupEKF. + * + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ #pragma once -#include // Include the base class -#include // For traits (needed for AdjointMap, Expmap) - +#include // For traits (needed for AdjointMap, Expmap) +#include // Include the base class namespace gtsam { +/** + * @class InvariantEKF + * @brief Left-Invariant Extended Kalman Filter on a Lie group G. + * + * @tparam G Lie group type (must satisfy LieGroup concept). + * + * 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 Covariance& + * Q)` + * 2. Prediction via tangent control vector: `predict(const TangentVector& u, + * double dt, const Covariance& Q)` + * + * The state-dependent prediction methods from LieGroupEKF are hidden. + * The update step remains the same as in ManifoldEKF/LieGroupEKF. + * For details on how static and dynamic dimensions are handled, please refer to + * the `ManifoldEKF` class documentation. + */ +template +class InvariantEKF : public LieGroupEKF { + public: + using Base = LieGroupEKF; ///< Base class type + using TangentVector = typename Base::TangentVector; ///< Tangent vector type + /// Jacobian for group-specific operations like AdjointMap. + /// Eigen::Matrix. + using Jacobian = typename Base::Jacobian; + /// Covariance matrix type. Eigen::Matrix. + using Covariance = typename Base::Covariance; + /** - * @class InvariantEKF - * @brief Left-Invariant Extended Kalman Filter on a Lie group G. - * - * @tparam G Lie group type (must satisfy LieGroup concept). - * - * 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 Covariance& Q)` - * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Covariance& Q)` - * - * The state-dependent prediction methods from LieGroupEKF are hidden. - * The update step remains the same as in ManifoldEKF/LieGroupEKF. - * For details on how static and dynamic dimensions are handled, please refer to - * the `ManifoldEKF` class documentation. + * Constructor: forwards to LieGroupEKF constructor. + * @param X0 Initial state on Lie group G. + * @param P0 Initial covariance in the tangent space at X0. */ - template - class InvariantEKF : public LieGroupEKF { - public: - using Base = LieGroupEKF; ///< Base class type - using TangentVector = typename Base::TangentVector; ///< Tangent vector type - /// Jacobian for group-specific operations like AdjointMap. Eigen::Matrix. - using Jacobian = typename Base::Jacobian; - /// Covariance matrix type. Eigen::Matrix. - using Covariance = typename Base::Covariance; - - - /** - * Constructor: forwards to LieGroupEKF constructor. - * @param X0 Initial state on Lie group G. - * @param P0 Initial covariance in the tangent space at X0. - */ - InvariantEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {} + 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. + // 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}. - * - * @param U Lie group element representing the motion increment. - * @param Q Process noise covariance. - */ - void predict(const G& U, const Covariance& Q) { - this->X_ = traits::Compose(this->X_, U); - const G U_inv = traits::Inverse(U); - const Jacobian A = traits::AdjointMap(U_inv); - // P_ is Covariance. A is Jacobian. Q is Covariance. - // All are Eigen::Matrix. - this->P_ = A * this->P_ * A.transpose() + Q; - } + /** + * 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}. + * + * @param U Lie group element representing the motion increment. + * @param Q Process noise covariance. + */ + void predict(const G& U, const Covariance& Q) { + this->X_ = traits::Compose(this->X_, U); + const G U_inv = traits::Inverse(U); + const Jacobian A = traits::AdjointMap(U_inv); + // P_ is Covariance. A is Jacobian. Q is Covariance. + // All are Eigen::Matrix. + this->P_ = A * this->P_ * A.transpose() + Q; + } - /** - * Predict step via tangent control vector: - * U = Expmap(u * dt) - * Then calls predict(U, Q). - * - * @param u Tangent space control vector. - * @param dt Time interval. - * @param Q Process noise covariance matrix. - */ - void predict(const TangentVector& u, double dt, const Covariance& Q) { - G U; - if constexpr (std::is_same_v) { - // Specialize to Matrix case as its Expmap is not defined. - const Matrix& X = static_cast(this->X_); - U.resize(X.rows(), X.cols()); - Eigen::Map(static_cast(U).data(), U.size()) = u * dt; - } - else { - U = traits::Expmap(u * dt); - } - predict(U, Q); // Call the group composition predict + /** + * Predict step via tangent control vector: + * U = Expmap(u * dt) + * Then calls predict(U, Q). + * + * @param u Tangent space control vector. + * @param dt Time interval. + * @param Q Process noise covariance matrix. + */ + void predict(const TangentVector& u, double dt, const Covariance& Q) { + G U; + if constexpr (std::is_same_v) { + // Specialize to Matrix case as its Expmap is not defined. + const Matrix& X = static_cast(this->X_); + U.resize(X.rows(), X.cols()); + Eigen::Map(static_cast(U).data(), U.size()) = u * dt; + } else { + U = traits::Expmap(u * dt); } + predict(U, Q); // Call the group composition predict + } - }; // InvariantEKF +}; // InvariantEKF -} // namespace gtsam \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index 007ed3ad09..6e28bc6291 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -9,177 +9,197 @@ * -------------------------------------------------------------------------- */ - /** - * @file LieGroupEKF.h - * @brief Extended Kalman Filter derived class for Lie groups G. - * - * This file defines the LieGroupEKF class template, inheriting from ManifoldEKF, - * for performing EKF steps specifically on states residing in a Lie group. - * It provides predict methods with state-dependent dynamics functions. - * Please use the InvariantEKF class for prediction via group composition. - * - * @date April 24, 2025 - * @authors Scott Baker, Matt Kielo, Frank Dellaert - */ +/** + * @file LieGroupEKF.h + * @brief Extended Kalman Filter derived class for Lie groups G. + * + * This file defines the LieGroupEKF class template, inheriting from + * ManifoldEKF, for performing EKF steps specifically on states residing in a + * Lie group. It provides predict methods with state-dependent dynamics + * functions. Please use the InvariantEKF class for prediction via group + * composition. + * + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ #pragma once -#include // Include the base class -#include // Include for Lie group traits and operations +#include // Include for Lie group traits and operations +#include +#include // Include the base class #include +#include // For std::function #include -#include // For std::function namespace gtsam { +/** + * @class LieGroupEKF + * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF + * + * @tparam G Lie group type (must satisfy LieGroup concept). + * + * This filter specializes ManifoldEKF for Lie groups, offering predict methods + * with state-dependent dynamics functions. + * Use the InvariantEKF class for prediction via group composition. + * For details on how static and dynamic dimensions are handled, please refer to + * the `ManifoldEKF` class documentation. + */ +template +class LieGroupEKF : public ManifoldEKF { + public: + using Base = ManifoldEKF; ///< Base class type + static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G. + using TangentVector = + typename Base::TangentVector; ///< Tangent vector type for G. + /// Jacobian for group operations (Adjoint, Expmap derivatives, F). + using Jacobian = typename Base::Jacobian; // Eigen::Matrix + using Covariance = + typename Base::Covariance; // Eigen::Matrix + /** - * @class LieGroupEKF - * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF - * - * @tparam G Lie group type (must satisfy LieGroup concept). - * - * This filter specializes ManifoldEKF for Lie groups, offering predict methods - * with state-dependent dynamics functions. - * Use the InvariantEKF class for prediction via group composition. - * For details on how static and dynamic dimensions are handled, please refer to - * the `ManifoldEKF` class documentation. + * Constructor: initialize with state and covariance. + * @param X0 Initial state on Lie group G. + * @param P0 Initial covariance in the tangent space at X0. */ - template - class LieGroupEKF : public ManifoldEKF { - public: - using Base = ManifoldEKF; ///< Base class type - static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G. - using TangentVector = typename Base::TangentVector; ///< Tangent vector type for G. - /// Jacobian for group operations (Adjoint, Expmap derivatives, F). - using Jacobian = typename Base::Jacobian; // Eigen::Matrix - using Covariance = typename Base::Covariance; // Eigen::Matrix - - /** - * Constructor: initialize with state and covariance. - * @param X0 Initial state on Lie group G. - * @param P0 Initial covariance in the tangent space at X0. - */ - LieGroupEKF(const G& X0, const Covariance& P0) : Base(X0, P0) { - static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); - } - - /** - * SFINAE check for correctly typed state-dependent dynamics function. - * Signature: TangentVector f(const G& X, OptionalJacobian Df) - * Df = d(xi)/d(local(X)) - */ - template - using enable_if_dynamics = std::enable_if_t< - !std::is_convertible_v&& - std::is_invocable_r_v&>>; - - /** - * Predict mean and Jacobian A with state-dependent dynamics: - * 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) - * A = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian) - * - * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) - * @param f Dynamics functor returning tangent vector xi and its Jacobian Df w.r.t. local(X). - * @param dt Time step. - * @param A OptionalJacobian to store the computed state transition Jacobian A. - * @return Predicted state X_{k+1}. - */ - template > - G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { - Jacobian Df, Dexp; // Eigen::Matrix - - if constexpr (std::is_same_v) { - // Specialize to Matrix case as its Expmap is not defined. - TangentVector xi = f(this->X_, A ? &Df : nullptr); - const Matrix nextX = traits::Retract(this->X_, xi * dt, A ? &Dexp : nullptr); // just addition - if (A) { - const Matrix I_n = Matrix::Identity(this->n_, this->n_); - *A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix - } - return nextX; - } - else { - TangentVector xi = f(this->X_, A ? &Df : nullptr); // xi and Df = d(xi)/d(localX) - G U = traits::Expmap(xi * dt, A ? &Dexp : nullptr); - if (A) { - // State transition Jacobian for left-invariant EKF: - *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; - } - return this->X_.compose(U); - } - } + LieGroupEKF(const G& X0, const Covariance& P0) : Base(X0, P0) { + static_assert(IsLieGroup::value, + "Template parameter G must be a GTSAM Lie Group"); + } + /** + * SFINAE check for correctly typed state-dependent dynamics function. + * Signature: TangentVector f(const G& X, OptionalJacobian Df) + * Df = d(xi)/d(local(X)) + */ + template + using enable_if_dynamics = + std::enable_if_t && + std::is_invocable_r_v&>>; - /** - * Predict step with state-dependent dynamics: - * Uses predictMean to compute X_{k+1} and A, then updates covariance. - * X_{k+1}, A = predictMean(f, dt) - * P_{k+1} = A P_k A^T + Q - * - * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) - * @param f Dynamics functor. - * @param dt Time step. - * @param Q Process noise covariance. - */ - template > - void predict(Dynamics&& f, double dt, const Covariance& Q) { - Jacobian A; - if constexpr (Dim == Eigen::Dynamic) { - A.resize(this->n_, this->n_); + /** + * Predict mean and Jacobian A with state-dependent dynamics: + * 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) A = + * Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian) + * + * @tparam Dynamics Functor signature: TangentVector f(const G&, + * OptionalJacobian&) + * @param f Dynamics functor returning tangent vector xi and its Jacobian Df + * w.r.t. local(X). + * @param dt Time step. + * @param A OptionalJacobian to store the computed state transition Jacobian + * A. + * @return Predicted state X_{k+1}. + */ + template > + G predictMean(Dynamics&& f, double dt, + OptionalJacobian A = {}) const { + Jacobian Df, Dexp; // Eigen::Matrix + + if constexpr (std::is_same_v) { + // Specialize to Matrix case as its Expmap is not defined. + TangentVector xi = f(this->X_, A ? &Df : nullptr); + const Matrix nextX = traits::Retract( + this->X_, xi * dt, A ? &Dexp : nullptr); // just addition + if (A) { + const Matrix I_n = Matrix::Identity(this->n_, this->n_); + *A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix } - this->X_ = predictMean(std::forward(f), dt, A); - this->P_ = A * this->P_ * A.transpose() + Q; + return nextX; + } else { + TangentVector xi = + f(this->X_, A ? &Df : nullptr); // xi and Df = d(xi)/d(localX) + G U = traits::Expmap(xi * dt, A ? &Dexp : nullptr); + if (A) { + // State transition Jacobian for left-invariant EKF: + *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + } + return this->X_.compose(U); } + } - /** - * SFINAE check for state- and control-dependent dynamics function. - * Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian Df) - */ - template - using enable_if_full_dynamics = std::enable_if_t< - std::is_invocable_r_v&> - >; - - /** - * Predict mean and Jacobian A with state and control input dynamics: - * Wraps the dynamics function and calls the state-only predictMean. - * xi = f(X_k, u, Df) - * - * @tparam Control Control input type. - * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) - * @param f Dynamics functor. - * @param u Control input. - * @param dt Time step. - * @param A Optional pointer to store the computed state transition Jacobian A. - * @return Predicted state X_{k+1}. - */ - template > - G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian A = {}) const { - return predictMean([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, A); + /** + * Predict step with state-dependent dynamics: + * Uses predictMean to compute X_{k+1} and A, then updates covariance. + * X_{k+1}, A = predictMean(f, dt) + * P_{k+1} = A P_k A^T + Q + * + * @tparam Dynamics Functor signature: TangentVector f(const G&, + * OptionalJacobian&) + * @param f Dynamics functor. + * @param dt Time step. + * @param Q Process noise covariance. + */ + template > + void predict(Dynamics&& f, double dt, const Covariance& Q) { + Jacobian A; + if constexpr (Dim == Eigen::Dynamic) { + A.resize(this->n_, this->n_); } + this->X_ = predictMean(std::forward(f), dt, A); + this->P_ = A * this->P_ * A.transpose() + Q; + } + /** + * SFINAE check for state- and control-dependent dynamics function. + * Signature: TangentVector f(const G& X, const Control& u, + * OptionalJacobian Df) + */ + template + using enable_if_full_dynamics = std::enable_if_t< + std::is_invocable_r_v&>>; - /** - * Predict step with state and control input dynamics: - * Wraps the dynamics function and calls the state-only predict. - * xi = f(X_k, u, Df) - * - * @tparam Control Control input type. - * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) - * @param f Dynamics functor. - * @param u Control input. - * @param dt Time step. - * @param Q Process noise covariance. - */ - template > - void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) { - return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); - } + /** + * Predict mean and Jacobian A with state and control input dynamics: + * Wraps the dynamics function and calls the state-only predictMean. + * xi = f(X_k, u, Df) + * + * @tparam Control Control input type. + * @tparam Dynamics Functor signature: TangentVector f(const G&, const + * Control&, OptionalJacobian&) + * @param f Dynamics functor. + * @param u Control input. + * @param dt Time step. + * @param A Optional pointer to store the computed state transition Jacobian + * A. + * @return Predicted state X_{k+1}. + */ + template > + G predictMean(Dynamics&& f, const Control& u, double dt, + OptionalJacobian A = {}) const { + return predictMean( + [&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, + dt, A); + } - }; // LieGroupEKF + /** + * Predict step with state and control input dynamics: + * Wraps the dynamics function and calls the state-only predict. + * xi = f(X_k, u, Df) + * + * @tparam Control Control input type. + * @tparam Dynamics Functor signature: TangentVector f(const G&, const + * Control&, OptionalJacobian&) + * @param f Dynamics functor. + * @param u Control input. + * @param dt Time step. + * @param Q Process noise covariance. + */ + template > + void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) { + return predict( + [&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, + dt, Q); + } + +}; // LieGroupEKF } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index d45f1fdf91..9b60ea0293 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -9,250 +9,260 @@ * -------------------------------------------------------------------------- */ - /** - * @file ManifoldEKF.h - * @brief Extended Kalman Filter base class on a generic manifold M - * - * This file defines the ManifoldEKF class template for performing prediction - * and update steps of an Extended Kalman Filter on states residing in a - * differentiable manifold. It relies on the manifold's retract and - * localCoordinates operations. - * - * @date April 24, 2025 - * @authors Scott Baker, Matt Kielo, Frank Dellaert - */ +/** + * @file ManifoldEKF.h + * @brief Extended Kalman Filter base class on a generic manifold M + * + * This file defines the ManifoldEKF class template for performing prediction + * and update steps of an Extended Kalman Filter on states residing in a + * differentiable manifold. It relies on the manifold's retract and + * localCoordinates operations. + * + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ #pragma once +#include // Include for traits and IsManifold #include #include #include -#include // Include for traits and IsManifold #include -#include #include +#include #include namespace gtsam { +/** + * @class ManifoldEKF + * @brief Extended Kalman Filter on a generic manifold M + * + * @tparam M Manifold type (must satisfy Manifold concept). + * + * This filter maintains a state X in the manifold M and covariance P in the + * tangent space at X. + * Prediction requires providing the predicted next state and the state + * transition Jacobian F. Updates apply a measurement function h and correct the + * state using the tangent space error. + * + * **Handling Static and Dynamic Dimensions:** + * The filter supports manifolds M with either a compile-time fixed dimension or + * a runtime dynamic dimension. This is determined by + * `gtsam::traits::dimension`. + * - If `dimension` is an integer (e.g., 3, 6), it's a fixed-size manifold. + * - If `dimension` is `Eigen::Dynamic`, it's a dynamically-sized manifold. In + * this case, `gtsam::traits::GetDimension(const M&)` must be available to + * retrieve the actual dimension at runtime. The internal protected member `n_` + * stores this runtime dimension. Covariance matrices (e.g., `P_`, method + * argument `Q`) and Jacobians (e.g., method argument `F`) are typed using + * `Covariance` and `Jacobian` typedefs, which are specializations of + * `Eigen::Matrix`, where `Dim` is `traits::dimension`. + * For dynamically-sized manifolds (`Dim == Eigen::Dynamic`), these Eigen types + * represent dynamically-sized matrices. + */ +template +class ManifoldEKF { + public: + /// Compile-time dimension of the manifold M. + static constexpr int Dim = traits::dimension; + + /// Tangent vector type for the manifold M. + using TangentVector = typename traits::TangentVector; + /// Covariance matrix type (P, Q). + using Covariance = Eigen::Matrix; + /// State transition Jacobian type (F). + using Jacobian = Eigen::Matrix; + /** - * @class ManifoldEKF - * @brief Extended Kalman Filter on a generic manifold M - * - * @tparam M Manifold type (must satisfy Manifold concept). - * - * This filter maintains a state X in the manifold M and covariance P in the - * tangent space at X. - * Prediction requires providing the predicted next state and the state transition Jacobian F. - * Updates apply a measurement function h and correct the state using the tangent space error. - * - * **Handling Static and Dynamic Dimensions:** - * The filter supports manifolds M with either a compile-time fixed dimension or a - * runtime dynamic dimension. This is determined by `gtsam::traits::dimension`. - * - If `dimension` is an integer (e.g., 3, 6), it's a fixed-size manifold. - * - If `dimension` is `Eigen::Dynamic`, it's a dynamically-sized manifold. In this case, - * `gtsam::traits::GetDimension(const M&)` must be available to retrieve the - * actual dimension at runtime. - * The internal protected member `n_` stores this runtime dimension. - * Covariance matrices (e.g., `P_`, method argument `Q`) and Jacobians (e.g., method argument `F`) - * are typed using `Covariance` and `Jacobian` typedefs, which are specializations of - * `Eigen::Matrix`, where `Dim` is `traits::dimension`. - * For dynamically-sized manifolds (`Dim == Eigen::Dynamic`), these Eigen types - * represent dynamically-sized matrices. + * Constructor: initialize with state and covariance. + * @param X0 Initial state on manifold M. + * @param P0 Initial covariance in the tangent space at X0 */ - template - class ManifoldEKF { - public: - /// Compile-time dimension of the manifold M. - static constexpr int Dim = traits::dimension; - - /// Tangent vector type for the manifold M. - using TangentVector = typename traits::TangentVector; - /// Covariance matrix type (P, Q). - using Covariance = Eigen::Matrix; - /// State transition Jacobian type (F). - using Jacobian = Eigen::Matrix; - - - /** - * Constructor: initialize with state and covariance. - * @param X0 Initial state on manifold M. - * @param P0 Initial covariance in the tangent space at X0 - */ - ManifoldEKF(const M& X0, const Covariance& P0) : X_(X0) { - static_assert(IsManifold::value, - "Template parameter M must be a GTSAM Manifold."); - - if constexpr (Dim == Eigen::Dynamic) { - n_ = traits::GetDimension(X0); - // Validate dimensions of initial covariance P0. - if (P0.rows() != n_ || P0.cols() != n_) { - throw std::invalid_argument( + ManifoldEKF(const M& X0, const Covariance& P0) : X_(X0) { + static_assert(IsManifold::value, + "Template parameter M must be a GTSAM Manifold."); + + if constexpr (Dim == Eigen::Dynamic) { + n_ = traits::GetDimension(X0); + // Validate dimensions of initial covariance P0. + if (P0.rows() != n_ || P0.cols() != n_) { + throw std::invalid_argument( "ManifoldEKF: Initial covariance P0 dimensions (" + std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) + ") do not match state's tangent space dimension (" + std::to_string(n_) + ")."); - } } - else { - n_ = Dim; - } - - P_ = P0; + } else { + n_ = Dim; } - virtual ~ManifoldEKF() = default; - - /// @return current state estimate on manifold M. - const M& state() const { return X_; } - - /// @return current covariance estimate. - const Covariance& covariance() const { return P_; } - - /// @return runtime dimension of the manifold. - int dimension() const { return n_; } - - /** - * Basic predict step: Updates state and covariance given the predicted - * next state and the state transition Jacobian F. - * X_{k+1} = X_next - * P_{k+1} = F P_k F^T + Q - * 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 on manifold M. - * @param F The state transition Jacobian. - * @param Q Process noise covariance matrix. - */ - void predict(const M& X_next, const Jacobian& F, const Covariance& Q) { - if constexpr (Dim == Eigen::Dynamic) { - 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 " + + P_ = P0; + } + + virtual ~ManifoldEKF() = default; + + /// @return current state estimate on manifold M. + const M& state() const { return X_; } + + /// @return current covariance estimate. + const Covariance& covariance() const { return P_; } + + /// @return runtime dimension of the manifold. + int dimension() const { return n_; } + + /** + * Basic predict step: Updates state and covariance given the predicted + * next state and the state transition Jacobian F. + * X_{k+1} = X_next + * P_{k+1} = F P_k F^T + Q + * 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 on manifold M. + * @param F The state transition Jacobian. + * @param Q Process noise covariance matrix. + */ + void predict(const M& X_next, const Jacobian& F, const Covariance& Q) { + if constexpr (Dim == Eigen::Dynamic) { + 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 pre-calculated - * predicted measurement and its Jacobian. - * - * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). - * @param prediction Predicted measurement. - * @param H Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X). - * @param z Observed measurement. - * @param R Measurement noise covariance. - */ - template - void update(const Measurement& prediction, + X_ = X_next; + P_ = F * P_ * F.transpose() + Q; + } + + /** + * Measurement update: Corrects the state and covariance using a + * pre-calculated predicted measurement and its Jacobian. + * + * @tparam Measurement Type of the measurement vector (e.g., VectorN, + * Vector). + * @param prediction Predicted measurement. + * @param H Jacobian of the measurement function h w.r.t. local(X), H = + * dh/dlocal(X). + * @param z Observed measurement. + * @param R Measurement noise covariance. + */ + template + void update( + const Measurement& prediction, const Eigen::Matrix::dimension, Dim>& H, const Measurement& z, - const Eigen::Matrix::dimension, traits::dimension>& R) { - - static_assert(IsManifold::value, - "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates."); - - static constexpr int MeasDim = traits::dimension; - - int m_runtime; - if constexpr (MeasDim == Eigen::Dynamic) { - m_runtime = traits::GetDimension(z); - if (traits::GetDimension(prediction) != m_runtime) { - throw std::invalid_argument( - "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions."); - } - if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || R.cols() != m_runtime) { - throw std::invalid_argument( - "ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch for dynamic measurement."); - } + const Eigen::Matrix::dimension, + traits::dimension>& R) { + static_assert(IsManifold::value, + "Template parameter Measurement must be a GTSAM Manifold for " + "LocalCoordinates."); + + static constexpr int MeasDim = traits::dimension; + + int m_runtime; + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); + if (traits::GetDimension(prediction) != m_runtime) { + throw std::invalid_argument( + "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' " + "have different dimensions."); + } + if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || + R.cols() != m_runtime) { + throw std::invalid_argument( + "ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch " + "for dynamic measurement."); } - else { - m_runtime = MeasDim; - if constexpr (Dim == Eigen::Dynamic) { - if (H.cols() != n_) { - throw std::invalid_argument( - "ManifoldEKF::update: Jacobian H columns must match state dimension " + std::to_string(n_) + "."); - } + } 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_) + "."); } } + } - // Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z) - typename traits::TangentVector innovation = + // Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z) + typename traits::TangentVector innovation = traits::Local(prediction, z); - // Innovation covariance: S = H P H^T + R - // S will be Eigen::Matrix - Eigen::Matrix S = H * P_ * H.transpose() + R; + // Innovation covariance: S = H P H^T + R + // S will be Eigen::Matrix + Eigen::Matrix S = H * P_ * H.transpose() + R; - // Kalman Gain: K = P H^T S^-1 - // K will be Eigen::Matrix - Eigen::Matrix K = P_ * H.transpose() * S.inverse(); + // Kalman Gain: K = P H^T S^-1 + // K will be Eigen::Matrix + Eigen::Matrix K = P_ * H.transpose() * S.inverse(); - // Correction vector in tangent space of M: delta_xi = K * innovation - const TangentVector delta_xi = K * innovation; // delta_xi is Dim x 1 (or n_ x 1 if dynamic) + // Correction vector in tangent space of M: delta_xi = K * innovation + const TangentVector delta_xi = + K * innovation; // delta_xi is Dim x 1 (or n_ x 1 if dynamic) - // Update state using retract: X_new = retract(X_old, delta_xi) - X_ = traits::Retract(X_, delta_xi); + // Update state using retract: X_new = retract(X_old, delta_xi) + X_ = traits::Retract(X_, delta_xi); - // Update covariance using Joseph form for numerical stability - Jacobian I_n; // Eigen::Matrix - if constexpr (Dim == Eigen::Dynamic) { - I_n = Jacobian::Identity(n_, n_); - } - else { - I_n = Jacobian::Identity(); - } - - // I_KH will be Eigen::Matrix - Jacobian I_KH = I_n - K * H; - P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); + // Update covariance using Joseph form for numerical stability + Jacobian I_n; // Eigen::Matrix + if constexpr (Dim == Eigen::Dynamic) { + I_n = Jacobian::Identity(n_, n_); + } else { + I_n = Jacobian::Identity(); } - /** - * Measurement update: Corrects the state and covariance using a measurement - * model function. - * - * @tparam Measurement Type of the measurement vector. - * @tparam MeasurementFunction Functor/lambda providing measurement and its Jacobian. - * Signature: `Measurement h(const M& x, Jac& H_jacobian)` - * where H = d(h)/d(local(X)). - * @param h Measurement model function. - * @param z Observed measurement. - * @param R Measurement noise covariance. - */ - template - void update(MeasurementFunction&& h, const Measurement& z, - const Eigen::Matrix::dimension, traits::dimension>& R) { - static_assert(IsManifold::value, - "Template parameter Measurement must be a GTSAM Manifold."); - - static constexpr int MeasDim = traits::dimension; - - int m_runtime; - if constexpr (MeasDim == Eigen::Dynamic) { - m_runtime = traits::GetDimension(z); - } - else { - m_runtime = MeasDim; - } - - // Predict measurement and get Jacobian H = dh/dlocal(X) - Matrix H(m_runtime, n_); - Measurement prediction = h(X_, H); + // I_KH will be Eigen::Matrix + Jacobian I_KH = I_n - K * H; + P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); + } - // Call the other update function - update(prediction, H, z, R); + /** + * Measurement update: Corrects the state and covariance using a measurement + * model function. + * + * @tparam Measurement Type of the measurement vector. + * @tparam MeasurementFunction Functor/lambda providing measurement and its + * Jacobian. Signature: `Measurement h(const M& x, Jac& H_jacobian)` where H = + * d(h)/d(local(X)). + * @param h Measurement model function. + * @param z Observed measurement. + * @param R Measurement noise covariance. + */ + template + void update(MeasurementFunction&& h, const Measurement& z, + const Eigen::Matrix::dimension, + traits::dimension>& R) { + static_assert(IsManifold::value, + "Template parameter Measurement must be a GTSAM Manifold."); + + static constexpr int MeasDim = traits::dimension; + + int m_runtime; + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); + } else { + m_runtime = MeasDim; } - protected: - M X_; ///< Manifold state estimate. - Covariance P_; ///< Covariance (Eigen::Matrix). - int n_; ///< Runtime tangent space dimension of M. - }; + // Predict measurement and get Jacobian H = dh/dlocal(X) + Matrix H(m_runtime, n_); + Measurement prediction = h(X_, H); + + // Call the other update function + update(prediction, H, z, R); + } + + protected: + M X_; ///< Manifold state estimate. + Covariance P_; ///< Covariance (Eigen::Matrix). + int n_; ///< Runtime tangent space dimension of M. +}; } // namespace gtsam \ No newline at end of file