diff --git a/examples/GEKF_Rot3Example.cpp b/examples/GEKF_Rot3Example.cpp new file mode 100644 index 000000000..d81e01d54 --- /dev/null +++ b/examples/GEKF_Rot3Example.cpp @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * 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 IEKF_Rot3Example.cpp + * @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control + * and a single magnetometer update. + * @date April 25, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// --- 1) Closed‐loop dynamics f(X): xi = –k·[φx,φy,0], H = ∂xi/∂φ·Dφ --- +static constexpr double k = 0.5; +Vector3 dynamicsSO3(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) +} + +// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× --- +static const Vector3 m_world(0, 0, -1); +Vector3 h_mag(const Rot3& X, OptionalJacobian<3, 3> H = {}) { + Vector3 z = X.inverse().rotate(m_world); + if (H) *H = -skewSymmetric(z); + return z; +} + +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; + LieGroupEKF ekf(R0, P0); + + // Timestep, process noise, measurement noise + double dt = 0.1; + Matrix3 Q = Matrix3::Identity() * 0.01; + Matrix3 Rm = Matrix3::Identity() * 0.05; + + cout << "=== Init ===\nR:\n" + << ekf.state().matrix() << "\nP:\n" + << ekf.covariance() << "\n\n"; + + // Predict using state‐dependent f + ekf.predict(dynamicsSO3, dt, Q); + cout << "--- After predict ---\nR:\n" << ekf.state().matrix() << "\n\n"; + + // Magnetometer measurement = body‐frame reading of m_world + Vector3 z = h_mag(R0); + ekf.update(h_mag, z, Rm); + cout << "--- After update ---\nR:\n" << ekf.state().matrix() << "\n"; + + return 0; +} diff --git a/examples/IEKF_NavstateExample.cpp b/examples/IEKF_NavstateExample.cpp new file mode 100644 index 000000000..ef3ef204b --- /dev/null +++ b/examples/IEKF_NavstateExample.cpp @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * 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 IEKF_NavstateExample.cpp + * @brief InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update) + * @date April 25, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/** + * @brief Left-invariant dynamics for NavState. + * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity. + * @return 9×1 tangent: [ω; 0₃; a]. + */ +Vector9 dynamics(const Vector6& imu) { + auto a = imu.head<3>(); + auto w = imu.tail<3>(); + Vector9 xi; + xi << w, Vector3::Zero(), a; + return xi; +} + +/** + * @brief GPS measurement model: returns position and its Jacobian. + * @param X Current state. + * @param H Optional 3×9 Jacobian w.r.t. X. + * @return 3×1 position vector. + */ +Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { + return X.position(H); +} + +int main() { + // Initial state & covariances + NavState X0; // R=I, v=0, t=0 + Matrix9 P0 = Matrix9::Identity() * 0.1; + InvariantEKF ekf(X0, P0); + + // Noise & timestep + double dt = 1.0; + Matrix9 Q = Matrix9::Identity() * 0.01; + Matrix3 R = Matrix3::Identity() * 0.5; + + // Two IMU samples [a; ω] + Vector6 imu1; + imu1 << 0.1, 0, 0, 0, 0.2, 0; + Vector6 imu2; + imu2 << 0, 0.3, 0, 0.4, 0, 0; + + // Two GPS fixes + Vector3 z1; + z1 << 0.3, 0, 0; + Vector3 z2; + z2 << 0.6, 0, 0; + + cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance() + << "\n\n"; + + // --- first predict/update --- + ekf.predict(dynamics(imu1), dt, Q); + cout << "--- After predict 1 ---\nX: " << ekf.state() + << "\nP: " << ekf.covariance() << "\n\n"; + ekf.update(h_gps, z1, R); + cout << "--- After update 1 ---\nX: " << ekf.state() + << "\nP: " << ekf.covariance() << "\n\n"; + + // --- second predict/update --- + ekf.predict(dynamics(imu2), dt, Q); + cout << "--- After predict 2 ---\nX: " << ekf.state() + << "\nP: " << ekf.covariance() << "\n\n"; + ekf.update(h_gps, z2, R); + cout << "--- After update 2 ---\nX: " << ekf.state() + << "\nP: " << ekf.covariance() << "\n"; + + return 0; +} diff --git a/examples/IEKF_SE2Example.cpp b/examples/IEKF_SE2Example.cpp new file mode 100644 index 000000000..1a7772c7c --- /dev/null +++ b/examples/IEKF_SE2Example.cpp @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * 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 IEKF_SE2Example.cpp + * @brief A left invariant Extended Kalman Filter example using a Lie group + * odometry as the prediction stage on SE(2) and + * + * This example uses the templated InvariantEKF class to estimate the state of + * an object using odometry / GPS measurements The prediction stage of the + * InvariantEKF uses a Lie Group element to propagate the stage in a discrete + * InvariantEKF. For most cases, U = exp(u^ * dt) if u is a control vector that + * is constant over the interval dt. However, if u is not constant over dt, + * other approaches are needed to find the value of U. This approach simply + * takes a Lie group element U, which can be found in various different ways. + * + * This data was compared to a left invariant EKF on SE(2) using identical + * measurements and noise from the source of the InEKF plugin + * https://inekf.readthedocs.io/en/latest/ Based on the paper "An Introduction + * to the Invariant Extended Kalman Filter" by Easton R. Potokar, Randal W. + * Beard, and Joshua G. Mangelson + * + * @date Apr 25, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// 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. +Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) { + return X.translation(H); +} + +// Define a InvariantEKF class that uses the Pose2 Lie group as the state and +// the Vector2 measurement type. +int main() { + // // Initialize the filter's state, covariance, and time interval values. + Pose2 X0(0.0, 0.0, 0.0); + Matrix3 P0 = Matrix3::Identity() * 0.1; + + // Create the filter with the initial state, covariance, and measurement + // function. + InvariantEKF ekf(X0, P0); + + // Define the process covariance and measurement covariance matrices Q and R. + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = I_2x2 * 0.01; + + // Define odometry movements. + // U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta. + // U2: Move 1 unit in X, 1 unit in Y, 0 radians in theta. + Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0); + + // Create GPS measurements. + // z1: Measure robot at (1, 0) + // z2: Measure robot at (1, 1) + Vector2 z1, z2; + z1 << 1.0, 0.0; + z2 << 1.0, 1.0; + + // Define a transformation matrix to convert the covariance into (theta, x, y) + // form. The paper and data mentioned above uses a (theta, x, y) notation, + // whereas GTSAM uses (x, y, theta). The transformation matrix is used to + // directly compare results of the covariance matrix. + Matrix3 TransformP; + TransformP << 0, 0, 1, 1, 0, 0, 0, 1, 0; + + // Propagating/updating the filter + // Initial state and covariance + cout << "\nInitialization:\n"; + cout << "X0: " << ekf.state() << endl; + cout << "P0: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; + + // First prediction stage + ekf.predict(U1, Q); + cout << "\nFirst Prediction:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; + + // First update stage + ekf.update(h_gps, z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; + + // Second prediction stage + ekf.predict(U2, Q); + cout << "\nSecond Prediction:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; + + // Second update stage + ekf.update(h_gps, z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; + + return 0; +} \ No newline at end of file diff --git a/examples/ISAM2_City10000.cpp b/examples/ISAM2_City10000.cpp index 526883b82..831d20f37 100644 --- a/examples/ISAM2_City10000.cpp +++ b/examples/ISAM2_City10000.cpp @@ -27,8 +27,6 @@ #include #include -#include -#include #include #include #include diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 5b3173b37..4248f16b2 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -277,6 +277,10 @@ inline Class expmap_default(const Class& t, const Vector& d) { template class IsLieGroup: public IsGroup, public IsManifold { public: + // Concept marker: allows checking IsLieGroup::value in templates + static constexpr bool value = + std::is_base_of::structure_category>::value; + typedef typename traits::structure_category structure_category_tag; typedef typename traits::ManifoldType ManifoldType; typedef typename traits::TangentVector TangentVector; @@ -284,7 +288,7 @@ public: GTSAM_CONCEPT_USAGE(IsLieGroup) { static_assert( - (std::is_base_of::value), + value, "This type's trait does not assert it is a Lie group (or derived)"); // group operations with Jacobians diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 80a9cd166..18ae9b616 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -138,10 +138,13 @@ public: static const int dim = traits::dimension; typedef typename traits::ManifoldType ManifoldType; typedef typename traits::TangentVector TangentVector; + // Concept marker: allows checking IsManifold::value in templates + static constexpr bool value = + std::is_base_of::value; GTSAM_CONCEPT_USAGE(IsManifold) { static_assert( - (std::is_base_of::value), + value, "This type's structure_category trait does not assert it as a manifold (or derived)"); static_assert(TangentVector::SizeAtCompileTime == dim); diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h new file mode 100644 index 000000000..82a3f8fec --- /dev/null +++ b/gtsam/navigation/InvariantEKF.h @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * 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 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 + +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 Matrix& Q)` + * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Matrix& Q)` + * + * The state-dependent prediction methods from LieGroupEKF are hidden. + * The update step remains the same as in ManifoldEKF/LieGroupEKF. + */ + template + class InvariantEKF : public LieGroupEKF { + public: + using Base = LieGroupEKF; ///< Base class type + using TangentVector = typename Base::TangentVector; ///< Tangent vector type + using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc. + using Jacobian = typename Base::Jacobian; ///< Jacobian matrix type specific to the group G + + /// Constructor: forwards to LieGroupEKF constructor + InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {} + + /** + * 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) { + this->X_ = this->X_.compose(U); + // TODO(dellaert): traits::AdjointMap should exist + Jacobian A = traits::Inverse(U).AdjointMap(); + 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 (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 + } + + }; // InvariantEKF + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h new file mode 100644 index 000000000..fe16eaefe --- /dev/null +++ b/gtsam/navigation/LieGroupEKF.h @@ -0,0 +1,165 @@ +/* ---------------------------------------------------------------------------- + + * 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 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 +#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 providing group operations and Expmap/AdjointMap. + * Must satisfy LieGroup concept (`gtsam::IsLieGroup::value`). + * + * This filter specializes ManifoldEKF for Lie groups, offering predict methods + * with state-dependent dynamics functions. + * Use the InvariantEKF class for prediction via group composition. + */ + template + class LieGroupEKF : 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 + LieGroupEKF(const G& X0, const MatrixN& 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) + * 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; + 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 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); + } + + }; // LieGroupEKF + +} // 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/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp new file mode 100644 index 000000000..74a70fbbe --- /dev/null +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + * 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 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 + +using namespace std; +using namespace gtsam; + +// 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_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; + + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = I_2x2 * 0.01; + + Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0); + + Vector2 z1, z2; + z1 << 1.0, 0.0; + z2 << 1.0, 1.0; + + // Create the filter + InvariantEKF ekf(X0, P0); + EXPECT(assert_equal(X0, ekf.state())); + EXPECT(assert_equal(P0, ekf.covariance())); + + // --- First Prediction --- + ekf.predict(U1, Q); + + // 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; + + // Verify + EXPECT(assert_equal(X1_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P1_expected, ekf.covariance(), 1e-9)); + + + // --- 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/testLieGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp new file mode 100644 index 000000000..62a35473d --- /dev/null +++ b/gtsam/navigation/tests/testLieGroupEKF.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 LieGroupEKF, 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 GEKF_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; + LieGroupEKF 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 { + LieGroupEKF 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; + LieGroupEKF 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 { + LieGroupEKF 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/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