From 3bcc5563eb6b1f639ba3031096023ce07ab90d94 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Apr 2025 00:20:41 -0400 Subject: [PATCH] EKF hierarchy --- ...F_Rot3Example.cpp => GEKF_Rot3Example.cpp} | 4 +- gtsam/navigation/GroupEKF.h | 196 +++++++++++++ gtsam/navigation/InvariantEKF.h | 267 ++++-------------- gtsam/navigation/ManifoldEKF.h | 155 ++++++++++ gtsam/navigation/tests/testGroupEKF.cpp | 114 ++++++++ gtsam/navigation/tests/testInvariantEKF.cpp | 172 +++++------ gtsam/navigation/tests/testManifoldEKF.cpp | 154 ++++++++++ 7 files changed, 771 insertions(+), 291 deletions(-) rename examples/{IEKF_Rot3Example.cpp => GEKF_Rot3Example.cpp} (93%) create mode 100644 gtsam/navigation/GroupEKF.h create mode 100644 gtsam/navigation/ManifoldEKF.h create mode 100644 gtsam/navigation/tests/testGroupEKF.cpp create mode 100644 gtsam/navigation/tests/testManifoldEKF.cpp diff --git a/examples/IEKF_Rot3Example.cpp b/examples/GEKF_Rot3Example.cpp similarity index 93% rename from examples/IEKF_Rot3Example.cpp rename to examples/GEKF_Rot3Example.cpp index 88989c67e..d931b7909 100644 --- a/examples/IEKF_Rot3Example.cpp +++ b/examples/GEKF_Rot3Example.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include @@ -53,7 +53,7 @@ int main() { // Initial estimate (identity) and covariance const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3); const Matrix3 P0 = Matrix3::Identity() * 0.1; - InvariantEKF ekf(R0, P0); + GroupEKF ekf(R0, P0); // Timestep, process noise, measurement noise double dt = 0.1; diff --git a/gtsam/navigation/GroupEKF.h b/gtsam/navigation/GroupEKF.h new file mode 100644 index 000000000..f2f2990e6 --- /dev/null +++ b/gtsam/navigation/GroupEKF.h @@ -0,0 +1,196 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + + /** + * @file GroupEKF.h + * @brief Extended Kalman Filter derived class for Lie groups G. + * + * This file defines the GroupEKF class template, inheriting from ManifoldEKF, + * for performing EKF steps specifically on states residing in a Lie group. + * It provides predict methods utilizing group composition, tangent space + * controls (via exponential map), and state-dependent dynamics functions. + * + * @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 +#include // For std::function + +namespace gtsam { + + /** + * @class GroupEKF + * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF + * + * @tparam G Lie group type providing group operations and Expmap/AdjointMap. + * Must satisfy LieGroup concept (`gtsam::IsLieGroup::value`). + * + * This filter specializes ManifoldEKF for Lie groups, offering convenient + * prediction methods based on group composition or dynamics functions defining + * motion in the tangent space. + */ + template + class GroupEKF : public ManifoldEKF { + public: + using Base = ManifoldEKF; ///< Base class type + static constexpr int n = Base::n; ///< Group dimension (tangent space dimension) + using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G + using MatrixN = typename Base::MatrixN; ///< Square matrix of size n for covariance and Jacobians + using Jacobian = Eigen::Matrix; ///< Jacobian matrix type specific to the group G + + /// Constructor: initialize with state and covariance + GroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) { + static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); + } + + /** + * Predict step via group composition (Left-Invariant): + * X_{k+1} = X_k * U + * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q + * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to + * F = Ad_{U^{-1}} in the base class predict method. + * + * @param U Lie group element representing the motion increment. + * @param Q Process noise covariance in the tangent space (size nxn). + */ + void predict(const G& U, const Matrix& Q) { + G X_next = this->X_.compose(U); + // TODO(dellaert): traits::AdjointMap should exist + Jacobian A = traits::Inverse(U).AdjointMap(); // A = Adjoint(U.inverse()) + Base::predict(X_next, A, Q); // Call base class 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 (size nxn). + */ + void predict(const TangentVector& u, double dt, const Matrix& Q) { + G U = traits::Expmap(u * dt); + predict(U, Q); // Call the group composition predict + } + + /** + * 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) + * F = 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 Optional pointer to store the computed state transition Jacobian A. + * @return Predicted state X_{k+1}. + */ + template > + G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { + Jacobian Df, Dexp; + TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX) + G U = traits::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt + G X_next = this->X_.compose(U); + + if (A) { + // Full state transition Jacobian for left-invariant EKF: + *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + } + return X_next; + } + + + /** + * Predict step with state-dependent dynamics: + * Uses predictMean to compute X_{k+1} and F, then calls base predict. + * X_{k+1}, F = predictMean(f, dt) + * P_{k+1} = F P_k F^T + Q + * + * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) + * @param f Dynamics functor. + * @param dt Time step. + * @param Q Process noise covariance (size nxn). + */ + template > + void predict(Dynamics&& f, double dt, const Matrix& Q) { + Jacobian A; + G X_next = predictMean(std::forward(f), dt, A); + Base::predict(X_next, A, Q); // Call base class predict + } + + /** + * 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 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 (size nxn). + */ + template > + void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { + return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); + } + + }; // GroupEKF + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 443f730c4..1d39981d7 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -9,226 +9,77 @@ * -------------------------------------------------------------------------- */ -/** - * @file InvariantEKF.h - * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) implementation - * - * This file defines the InvariantEKF class template for performing prediction and - * update steps of an Extended Kalman Filter on states residing in a Lie group. - * The class supports state evolution via group composition and dynamics - * functions, along with measurement updates using tangent-space corrections. - * - * @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 GroupEKF, + * 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 GroupEKF. + * + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ #pragma once -#include -#include -#include - -#include -#include +#include // Include the base class namespace gtsam { -/** - * @class InvariantEKF - * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) on a Lie group G - * - * @tparam G Lie group type providing: - * - static int dimension = tangent dimension - * - using TangentVector = Eigen::Vector... - * - using Jacobian = Eigen::Matrix... - * - methods: Expmap(), expmap(), compose(), inverse().AdjointMap() - * - * This filter maintains a state X in the group G and covariance P in the - * tangent space. Prediction steps are performed via group composition or a - * user-supplied dynamics function. Updates apply a measurement function h - * returning both predicted measurement and its Jacobian H, and correct state - * using the left-invariant error in the tangent space. - */ -template -class InvariantEKF { - public: - /// Tangent-space dimension - static constexpr int n = traits::dimension; - - /// Square matrix of size n for covariance and Jacobians - using MatrixN = Eigen::Matrix; - - /// Constructor: initialize with state and covariance - InvariantEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} - - /// @return current state estimate - const G& state() const { return X_; } - - /// @return current covariance estimate - const Matrix& covariance() const { return P_; } - /** - * Predict step via group composition: - * X_{k+1} = X_k * U - * P_{k+1} = A P_k A^T + Q - * where A = Ad_{U^{-1}}. i.e., d(X.compose(U))/dX evaluated at X_k. + * @class InvariantEKF + * @brief Left-Invariant Extended Kalman Filter on a Lie group G. * - * @param U Lie group increment (e.g., Expmap of control * dt) - * @param Q process noise covariance in tangent space + * @tparam G Lie group type (must satisfy LieGroup concept). + * + * This filter inherits from GroupEKF but restricts the prediction interface + * to only the left-invariant prediction methods: + * 1. Prediction via group composition: `predict(const G& U, const Matrix& Q)` + * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Matrix& Q)` + * + * The state-dependent prediction methods from GroupEKF are hidden. + * The update step remains the same as in ManifoldEKF/GroupEKF. */ - void predict(const G& U, const Matrix& Q) { - typename G::Jacobian A; - X_ = X_.compose(U, A); - P_ = A * P_ * A.transpose() + Q; - } + template + class InvariantEKF : public GroupEKF { + public: + using Base = GroupEKF; ///< Base class type + using TangentVector = typename Base::TangentVector; ///< Tangent vector type + using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc. - /** - * Predict step via tangent control vector: - * U = Expmap(u * dt) - * @param u tangent control vector - * @param dt Time interval - * @param Q Process noise covariance matrix. - * - * @note Use this if your dynamics does not depend on the state, e.g. - * predict(f(u), dt, q) where u is a control input - */ - void predict(const typename G::TangentVector& u, double dt, const Matrix& Q) { - predict(G::Expmap(u * dt), Q); - } + /// Constructor: forwards to GroupEKF constructor + InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {} - /** - * SFINAE check for correctly typed state-dependent dynamics function. - * xi = f(X, F) - * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - */ - template - using enable_if_dynamics = std::enable_if_t< - !std::is_convertible_v && - std::is_invocable_r_v&>>; - /** - * Predict mean only with state-dependent dynamics: - * xi = f(X, F) - * X_.expmap(xi * dt) - * - * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - * @param f dynamics functor depending on state and control - * @param dt time step - * @param Q process noise covariance - */ - template > - G predictMean(Dynamics&& f, double dt, const Matrix& Q, - OptionalJacobian A = {}) const { - typename G::Jacobian Df, Dexp; - typename G::TangentVector xi = f(X_, Df); - G U = G::Expmap(xi * dt, Dexp); - // derivative of mean = d[X.compose(U)]/dLog(X) - // = Ad_{U⁻¹} (the “left‐invariant” part) - // plus the effect of X→U via Expmap: - // Dexp * Df * dt (how U moves when X moves through f) - if (A) *A = U.inverse().AdjointMap() + Dexp * Df * dt; - return X_.compose(U); - } + // --- Expose only the Invariant Prediction Methods --- - /** - * Predict step with state-dependent dynamics: - * xi = f(X, F) - * X_.expmap(xi * dt) - * - * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - * @param f dynamics functor depending on state and control - * @param dt time step - * @param Q process noise covariance - */ - template > - void predict(Dynamics&& f, double dt, const Matrix& Q) { - typename G::Jacobian A; - X_ = predictMean(f, dt, Q, &A); - P_ = A * 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 + * Calls the base class implementation. + * + * @param U Lie group element representing the motion increment. + * @param Q Process noise covariance in the tangent space (size nxn). + */ + void predict(const G& U, const Matrix& Q) { + Base::predict(U, Q); + } - /** - * Predict mean only with state and control input dynamics: - * xi = f(X, u, F) - * X_.expmap(xi * dt) - * - * @tparam Control control input type - * @tparam Dynamics signature: G f(const G&, const Control&, - * OptionalJacobian&) - * - * @param f dynamics functor depending on state and control - * @param u control input - * @param dt time step - * @param Q process noise covariance - */ - template - G predictMean(Dynamics&& f, const Control& u, double dt, const Matrix& Q, - OptionalJacobian A = {}) { - auto g = [f, u](const G& X, OptionalJacobian F) { - return f(X, u, F); - }; - return predictMean(g, dt, Q, A); - } + /** + * Predict step via tangent control vector: + * U = Expmap(u * dt) + * Then calls predict(U, Q). Calls the base class implementation. + * + * @param u Tangent space control vector. + * @param dt Time interval. + * @param Q Process noise covariance matrix (size nxn). + */ + void predict(const TangentVector& u, double dt, const Matrix& Q) { + Base::predict(u, dt, Q); + } - /** - * Predict step with state and control input dynamics: - * xi = f(X, u, F) - * X_.expmap(xi * dt) - * - * @tparam Control control input type - * @tparam Dynamics signature: G f(const G&, const Control&, - * OptionalJacobian&) - * - * @param f dynamics functor depending on state and control - * @param u control input - * @param dt time step - * @param Q process noise covariance - */ - template - void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { - auto g = [f, u](const G& X, OptionalJacobian F) { - return f(X, u, F); - }; - predict(g, dt, Q); - } + }; // InvariantEKF - /** - * Measurement update: - * z_pred, H = h(X) - * K = P H^T (H P H^T + R)^{-1} - * X <- Expmap(-K (z_pred - z)) * X - * P <- (I - K H) P - * - * @tparam Measurement measurement type (e.g., Vector) - * @tparam Prediction functor signature: Measurement h(const G&, - * OptionalJacobian&) - * - * @param h measurement model returning predicted z and Jacobian H - * @param z observed measurement - * @param R measurement noise covariance - */ - template - void update(Prediction&& h, const Measurement& z, const Matrix& R) { - Eigen::Matrix::dimension, n> H; - auto z_pred = h(X_, H); - auto y = z_pred - z; - Matrix S = H * P_ * H.transpose() + R; - Matrix K = P_ * H.transpose() * S.inverse(); - X_ = X_.expmap(-K * y); - P_ = (I_n - K * H) * P_; - } - - protected: - G X_; ///< group state estimate - Matrix P_; ///< covariance in tangent space - - private: - /// Identity matrix of size n - static const MatrixN I_n; -}; - -// Define static identity I_n -template -const typename InvariantEKF::MatrixN InvariantEKF::I_n = InvariantEKF::MatrixN::Identity(); - -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h new file mode 100644 index 000000000..7951b88cb --- /dev/null +++ b/gtsam/navigation/ManifoldEKF.h @@ -0,0 +1,155 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + + /** + * @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 +#include +#include // Include for traits + +#include +#include + +namespace gtsam { + + /** + * @class ManifoldEKF + * @brief Extended Kalman Filter on a generic manifold M + * + * @tparam M Manifold type providing: + * - static int dimension = tangent dimension + * - using TangentVector = Eigen::Vector... + * - A `retract(const TangentVector&)` method (member or static) + * - A `localCoordinates(const M&)` method (member or static) + * - `gtsam::traits` specialization must exist. + * + * This filter maintains a state X in the manifold M and covariance P in the + * tangent space at X. Prediction requires providing the predicted next state + * and the state transition Jacobian F. Updates apply a measurement function h + * and correct the state using the tangent space error. + */ + template + class ManifoldEKF { + public: + /// Manifold dimension (tangent space dimension) + static constexpr int n = traits::dimension; + + /// Tangent vector type for the manifold M + using TangentVector = typename traits::TangentVector; + + /// Square matrix of size n for covariance and Jacobians + using MatrixN = Eigen::Matrix; + + /// Constructor: initialize with state and covariance + ManifoldEKF(const M& X0, const MatrixN& P0) : X_(X0), P_(P0) { + static_assert(IsManifold::value, "Template parameter M must be a GTSAM Manifold"); + } + + virtual ~ManifoldEKF() = default; // Add virtual destructor for base class + + /// @return current state estimate + const M& state() const { return X_; } + + /// @return current covariance estimate + const MatrixN& covariance() const { return P_; } + + /** + * 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. + * @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) { + X_ = X_next; + P_ = F * P_ * F.transpose() + Q; + } + + /** + * Measurement update: Corrects the state and covariance using a measurement. + * z_pred, H = h(X) + * y = z - z_pred (innovation, or z_pred - z depending on convention) + * S = H P H^T + R (innovation covariance) + * K = P H^T S^{-1} (Kalman gain) + * delta_xi = -K * y (correction in tangent space) + * X <- X.retract(delta_xi) + * P <- (I - K H) P + * + * @tparam Measurement Type of the measurement vector (e.g., VectorN) + * @tparam Prediction Functor signature: Measurement h(const M&, + * OptionalJacobian&) + * where m is the measurement dimension. + * + * @param h Measurement model functor returning predicted measurement z_pred + * and its Jacobian H = d(h)/d(local(X)). + * @param z Observed measurement. + * @param R Measurement noise covariance (size m x m). + */ + template + void update(Prediction&& h, const Measurement& z, const Matrix& R) { + constexpr int m = traits::dimension; + Eigen::Matrix H; + + // Predict measurement and get Jacobian H = dh/dlocal(X) + Measurement z_pred = h(X_, H); + + // Innovation + // Ensure consistent subtraction for manifold types if Measurement is one + Vector innovation = traits::Local(z, z_pred); // y = z_pred (-) z (in tangent space) + + // Innovation covariance and Kalman Gain + auto S = H * P_ * H.transpose() + R; + Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m) + + // Correction vector in tangent space + TangentVector delta_xi = -K * innovation; // delta_xi = - K * y + + // Update state using retract + X_ = traits::Retract(X_, delta_xi); // X <- X.retract(delta_xi) + + // Update covariance using Joseph form: + MatrixN I_KH = I_n - K * H; + P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); + } + + protected: + M X_; ///< manifold state estimate + MatrixN P_; ///< covariance in tangent space at X_ + + private: + /// Identity matrix of size n + static const MatrixN I_n; + }; + + // Define static identity I_n + template + const typename ManifoldEKF::MatrixN ManifoldEKF::I_n = ManifoldEKF::MatrixN::Identity(); + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testGroupEKF.cpp b/gtsam/navigation/tests/testGroupEKF.cpp new file mode 100644 index 000000000..5ce7c477b --- /dev/null +++ b/gtsam/navigation/tests/testGroupEKF.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + + /** + * @file testGroupEKF.cpp + * @brief Unit test for GroupEKF, as well as dynamics used in Rot3 example. + * @date April 26, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +// Duplicate the dynamics function in GroupEKF_Rot3Example +namespace exampleSO3 { + static constexpr double k = 0.5; + Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { + // φ = Logmap(R), Dφ = ∂φ/∂δR + Matrix3 D_phi; + Vector3 phi = Rot3::Logmap(X, D_phi); + // zero out yaw + phi[2] = 0.0; + D_phi.row(2).setZero(); + + if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR + return -k * phi; // xi ∈ 𝔰𝔬(3) + } +} // namespace exampleSO3 + +TEST(GroupeEKF, DynamicsJacobian) { + // Construct a nontrivial state and IMU input + Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); + + // Analytic Jacobian + Matrix3 actualH; + exampleSO3::dynamics(R, actualH); + + // Numeric Jacobian w.r.t. the state X + auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); }; + Matrix3 expectedH = numericalDerivative11(f, R); + + // Compare + EXPECT(assert_equal(expectedH, actualH)); +} + +TEST(GroupeEKF, PredictNumericState) { + // GIVEN + Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); + Matrix3 P0 = Matrix3::Identity() * 0.2; + double dt = 0.1; + + // Analytic Jacobian + Matrix3 actualH; + GroupEKF ekf0(R0, P0); + ekf0.predictMean(exampleSO3::dynamics, dt, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + GroupEKF ekf(R, P0); + return ekf.predictMean(exampleSO3::dynamics, dt); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); +} + +TEST(GroupeEKF, StateAndControl) { + auto f = [](const Rot3& X, const Vector2& dummy_u, + OptionalJacobian<3, 3> H = {}) { + return exampleSO3::dynamics(X, H); + }; + + // GIVEN + Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); + Matrix3 P0 = Matrix3::Identity() * 0.2; + Vector2 dummy_u(1, 2); + double dt = 0.1; + Matrix3 Q = Matrix3::Zero(); + + // Analytic Jacobian + Matrix3 actualH; + GroupEKF ekf0(R0, P0); + ekf0.predictMean(f, dummy_u, dt, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + GroupEKF ekf(R, P0); + return ekf.predictMean(f, dummy_u, dt, Q); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index c3f93d429..74a70fbbe 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -7,109 +7,119 @@ * See LICENSE for the license information * -------------------------------------------------------------------------- */ -/** - * @file testInvariantEKFNavState.cpp - * @brief Unit test for the NavState dynamics Jacobian in InvariantEKF example. - * @date April 26, 2025 - * @authors Scott Baker, Matt Kielo, Frank Dellaert - */ + /** + * @file testInvariantEKF_Pose2.cpp + * @brief Unit test for the InvariantEKF using Pose2 state. + * Based on the logic from IEKF_SE2Example.cpp + * @date April 26, 2025 + * @authors Frank Dellaert (adapted from example by Scott Baker, Matt Kielo) + */ #include #include #include -#include +#include #include -#include +#include + +using namespace std; using namespace gtsam; -// Duplicate the dynamics function in InvariantEKF_Rot3Example -namespace exampleSO3 { -static constexpr double k = 0.5; -Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { - // φ = Logmap(R), Dφ = ∂φ/∂δR - Matrix3 D_phi; - Vector3 phi = Rot3::Logmap(X, D_phi); - // zero out yaw - phi[2] = 0.0; - D_phi.row(2).setZero(); - - if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR - return -k * phi; // xi ∈ 𝔰𝔬(3) -} -} // namespace exampleSO3 - -TEST(IEKF, dynamicsJacobian) { - // Construct a nontrivial state and IMU input - Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); - - // Analytic Jacobian - Matrix3 actualH; - exampleSO3::dynamics(R, actualH); - - // Numeric Jacobian w.r.t. the state X - auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); }; - Matrix3 expectedH = numericalDerivative11(f, R); - - // Compare - EXPECT(assert_equal(expectedH, actualH)); +// Create a 2D GPS measurement function that returns the predicted measurement h +// and Jacobian H. The predicted measurement h is the translation of the state X. +// (Copied from IEKF_SE2Example.cpp) +Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) { + return X.translation(H); } -TEST(IEKF, PredictNumericState) { - // GIVEN - Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); - Matrix3 P0 = Matrix3::Identity() * 0.2; - double dt = 0.1; - Matrix3 Q = Matrix3::Zero(); - // Analytic Jacobian - Matrix3 actualH; - InvariantEKF iekf0(R0, P0); - iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH); +TEST(IEKF_Pose2, PredictUpdateSequence) { + // GIVEN: Initial state, covariance, noises, motions, measurements + // (from IEKF_SE2Example.cpp) + Pose2 X0(0.0, 0.0, 0.0); + Matrix3 P0 = Matrix3::Identity() * 0.1; - // wrap predict into a state->state functor (mapping on SO(3)) - auto g = [&](const Rot3& R) -> Rot3 { - InvariantEKF iekf(R, P0); - return iekf.predictMean(exampleSO3::dynamics, dt, Q); - }; + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = I_2x2 * 0.01; - // numeric Jacobian of g at R0 - Matrix3 expectedH = numericalDerivative11(g, R0); + Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0); - EXPECT(assert_equal(expectedH, actualH)); -} + Vector2 z1, z2; + z1 << 1.0, 0.0; + z2 << 1.0, 1.0; -TEST(IEKF, StateAndControl) { - auto f = [](const Rot3& X, const Vector2& dummy_u, - OptionalJacobian<3, 3> H = {}) { - return exampleSO3::dynamics(X, H); - }; + // Create the filter + InvariantEKF ekf(X0, P0); + EXPECT(assert_equal(X0, ekf.state())); + EXPECT(assert_equal(P0, ekf.covariance())); - // GIVEN - Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); - Matrix3 P0 = Matrix3::Identity() * 0.2; - Vector2 dummy_u(1, 2); - double dt = 0.1; - Matrix3 Q = Matrix3::Zero(); + // --- First Prediction --- + ekf.predict(U1, Q); - // Analytic Jacobian - Matrix3 actualH; - InvariantEKF iekf0(R0, P0); - iekf0.predictMean(f, dummy_u, dt, Q, actualH); + // Calculate expected state and covariance + Pose2 X1_expected = X0.compose(U1); + Matrix3 Ad_U1_inv = U1.inverse().AdjointMap(); + Matrix3 P1_expected = Ad_U1_inv * P0 * Ad_U1_inv.transpose() + Q; - // wrap predict into a state->state functor (mapping on SO(3)) - auto g = [&](const Rot3& R) -> Rot3 { - InvariantEKF iekf(R, P0); - return iekf.predictMean(f, dummy_u, dt, Q); - }; + // Verify + EXPECT(assert_equal(X1_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P1_expected, ekf.covariance(), 1e-9)); - // numeric Jacobian of g at R0 - Matrix3 expectedH = numericalDerivative11(g, R0); - EXPECT(assert_equal(expectedH, actualH)); + // --- First Update --- + ekf.update(h_gps, z1, R); + + // Calculate expected state and covariance (manual Kalman steps) + Matrix H1; // H = dh/dlocal(X) -> 2x3 + Vector2 z_pred1 = h_gps(X1_expected, H1); + Vector2 y1 = z_pred1 - z1; // Innovation + Matrix S1 = H1 * P1_expected * H1.transpose() + R; + Matrix K1 = P1_expected * H1.transpose() * S1.inverse(); // Gain (3x2) + Vector3 delta_xi1 = -K1 * y1; // Correction (tangent space) + Pose2 X1_updated_expected = X1_expected.retract(delta_xi1); + Matrix3 I_KH1 = Matrix3::Identity() - K1 * H1; + Matrix3 P1_updated_expected = I_KH1 * P1_expected; // Standard form P = (I-KH)P + + // Verify + EXPECT(assert_equal(X1_updated_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P1_updated_expected, ekf.covariance(), 1e-9)); + + + // --- Second Prediction --- + ekf.predict(U2, Q); + + // Calculate expected state and covariance + Pose2 X2_expected = X1_updated_expected.compose(U2); + Matrix3 Ad_U2_inv = U2.inverse().AdjointMap(); + Matrix3 P2_expected = Ad_U2_inv * P1_updated_expected * Ad_U2_inv.transpose() + Q; + + // Verify + EXPECT(assert_equal(X2_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P2_expected, ekf.covariance(), 1e-9)); + + + // --- Second Update --- + ekf.update(h_gps, z2, R); + + // Calculate expected state and covariance (manual Kalman steps) + Matrix H2; // 2x3 + Vector2 z_pred2 = h_gps(X2_expected, H2); + Vector2 y2 = z_pred2 - z2; // Innovation + Matrix S2 = H2 * P2_expected * H2.transpose() + R; + Matrix K2 = P2_expected * H2.transpose() * S2.inverse(); // Gain (3x2) + Vector3 delta_xi2 = -K2 * y2; // Correction (tangent space) + Pose2 X2_updated_expected = X2_expected.retract(delta_xi2); + Matrix3 I_KH2 = Matrix3::Identity() - K2 * H2; + Matrix3 P2_updated_expected = I_KH2 * P2_expected; // Standard form + + // Verify + EXPECT(assert_equal(X2_updated_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P2_updated_expected, ekf.covariance(), 1e-9)); + } int main() { TestResult tr; return TestRegistry::runAllTests(tr); -} +} \ No newline at end of file diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp new file mode 100644 index 000000000..1d74bb704 --- /dev/null +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -0,0 +1,154 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + + /** + * @file testManifoldEKF.cpp + * @brief Unit test for the ManifoldEKF base class using Unit3. + * @date April 26, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#include +#include +#include +#include +#include + +#include + +#include + +using namespace gtsam; + +// Define simple dynamics for Unit3: constant velocity in the tangent space +namespace exampleUnit3 { + + // Predicts the next state given current state, tangent velocity, and dt + Unit3 predictNextState(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) { + if (H) { + // H = d(p.point3().z()) / d(local(p)) + // Calculate numerically for simplicity in test + auto h = [](const Unit3& p_) { return Vector1(p_.point3().z()); }; + *H = numericalDerivative11(h, p); + } + return Vector1(p.point3().z()); + } + +} // namespace exampleUnit3 + +// Test fixture for ManifoldEKF with Unit3 +struct Unit3EKFTest { + Unit3 p0; + Matrix2 P0; + Vector2 velocity; + double dt; + Matrix2 Q; // Process noise + Matrix1 R; // Measurement noise + + Unit3EKFTest() : + p0(Unit3(Point3(1, 0, 0))), // Start pointing along X-axis + P0(I_2x2 * 0.01), + velocity((Vector2() << 0.0, M_PI / 4.0).finished()), // Rotate towards +Z axis + dt(0.1), + Q(I_2x2 * 0.001), + R(Matrix1::Identity() * 0.01) { + } +}; + + +TEST(ManifoldEKF_Unit3, Predict) { + Unit3EKFTest data; + + // Initialize the EKF + ManifoldEKF ekf(data.p0, data.P0); + + // --- Prepare inputs for ManifoldEKF::predict --- + // 1. Compute expected next state + Unit3 p_next_expected = exampleUnit3::predictNextState(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. + // GTSAM's numericalDerivative handles derivatives *between* manifolds. + auto predict_wrapper = [&](const Unit3& p) -> Unit3 { + return exampleUnit3::predictNextState(p, data.velocity, data.dt); + }; + Matrix2 F = numericalDerivative11(predict_wrapper, data.p0); + + // --- Perform EKF prediction --- + ekf.predict(p_next_expected, F, data.Q); + + // --- Verification --- + // Check state + EXPECT(assert_equal(p_next_expected, ekf.state(), 1e-8)); + + // Check covariance + Matrix2 P_expected = F * data.P0 * F.transpose() + data.Q; + EXPECT(assert_equal(P_expected, ekf.covariance(), 1e-8)); + + // 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); + }; + Matrix2 F_zero = numericalDerivative11(predict_wrapper_zero, data.p0); + EXPECT(assert_equal(I_2x2, F_zero, 1e-8)); + +} + +TEST(ManifoldEKF_Unit3, Update) { + Unit3EKFTest data; + + // Use a slightly different starting point and covariance for variety + Unit3 p_start = Unit3(Point3(0, 1, 0)).retract((Vector2() << 0.1, 0).finished()); // Perturb pointing along Y + Matrix2 P_start = I_2x2 * 0.05; + ManifoldEKF ekf(p_start, P_start); + + // Simulate a measurement (e.g., true value + noise) + Vector1 z_true = exampleUnit3::measureZ(p_start, {}); + Vector1 z_observed = z_true + Vector1(0.02); // Add some noise + + // --- Perform EKF update --- + ekf.update(exampleUnit3::measureZ, z_observed, data.R); + + // --- 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); + + // 2. Innovation and Covariance + Vector1 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 + Matrix K = P_start * H.transpose() * S.inverse(); // 2x1 matrix + + // 4. State Correction (in tangent space) + Vector2 delta_xi = -K * y; // 2x1 vector + + // 5. Expected Updated State and Covariance + Unit3 p_updated_expected = p_start.retract(delta_xi); + Matrix2 I_KH = I_2x2 - K * H; + Matrix2 P_updated_expected = I_KH * P_start; + + // --- Compare EKF result with manual calculation --- + EXPECT(assert_equal(p_updated_expected, ekf.state(), 1e-8)); + EXPECT(assert_equal(P_updated_expected, ekf.covariance(), 1e-8)); +} + + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} \ No newline at end of file