From c403e4af5435f2b4bcc7a43b4a18aa85fadd8769 Mon Sep 17 00:00:00 2001 From: scottiyio Date: Tue, 1 Apr 2025 17:43:52 -0400 Subject: [PATCH 01/30] Add files via upload A Left Invariant IEKF on SE(2). --- examples/SE2LIEKF_NoFactors.cpp | 124 ++++++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) create mode 100644 examples/SE2LIEKF_NoFactors.cpp diff --git a/examples/SE2LIEKF_NoFactors.cpp b/examples/SE2LIEKF_NoFactors.cpp new file mode 100644 index 000000000..51693b7ee --- /dev/null +++ b/examples/SE2LIEKF_NoFactors.cpp @@ -0,0 +1,124 @@ + +/* ---------------------------------------------------------------------------- + + * 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 SE2LIEKF_NoFactors.cpp + * + * A simple left invariant EKF operating in SE(2) using an odometry and GPS measurements. + * No factors are used here. + * 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 April 1, 2025 + * @author Scott Baker + */ + + + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +#define PI 3.14159265358 + +// Create an adjoint class +// Function for the prediction stage. +// Note that U is the exponential map of some control vector (u*dt) + void predict(Pose2& X, Matrix& P, Vector3& u, Matrix& Q) + { + //Pose2 U = Pose2::Expmap(u*dt); + Pose2 U(u(0), u(1), u(2)); + Matrix Adj = U.inverse().AdjointMap(); + X = X.compose(U); + P = Adj * P * Adj.transpose() + Q; + } + + // Update stage that uses GPS measurements in a left invariant update way. + void update(Pose2& X, Matrix& P, Point2& z, Matrix& H, Matrix& R) + { + // find residual R^T * (z - zhat) + Point2 zhat = X.translation(); + Point2 e_world = z-zhat; + Point2 residual = X.rotation().unrotate(e_world); + Matrix S = H*P*H.transpose() + R; + Matrix K = P*H.transpose()*S.inverse(); + Vector3 delta = K*residual; + X = X * Pose2::Expmap(delta); + P = (Matrix3::Identity() - K*H) * P; + } + + + +int main() { + + // Define movements and measurements + const double dt = 1.0; + + Vector3 u1(1.0, 1.0, 0.5), u2(1.0,1.0,0.0); // odometry + Point2 z1(1.0, 0.0), z2(1.0, 1.0); // gps + + + + // Set up noise models (uses simple GPS) + + // Odometry Process + Vector3 pModel(0.05, 0.05, 0.001); // covariance of the odometry process (rad) + Matrix Q = pModel.asDiagonal(); // Q covariance matrix + + // GPS process + Vector2 rModel(0.01, 0.01); + Matrix R = rModel.asDiagonal(); + Matrix H(2,3); + H << 1,0,0, + 0,1,0; + + // A matrix that transforms the P matrix into a form of [theta, x, y] for comparison to InEKF's structure. + Matrix3 TransformP; + TransformP << 0, 0, 1, + 1,0,0, + 0,1,0; + +// Initialize + Pose2 X(0, 0, 0); + Matrix P = Matrix3::Identity()*0.1; + + cout << "Initial X\n" << X << endl; + cout << "Initial P\n" << TransformP * P * TransformP.transpose() << endl; + + // First Prediction + predict(X, P, u1, dt, Q); + cout << "Predicted X1\n" << X << endl; + cout << "Predicted P1\n" << TransformP * P * TransformP.transpose() << endl; + + // First Update + update(X, P, z1, H, R); + cout << "Updated X1\n" << X << endl; + cout << "Updated P1\n" << TransformP * P * TransformP.transpose() << endl; + + // Second Prediction + predict(X, P, u2, dt, Q); + cout << "Predicted X2\n" << X << endl; + cout << "Predicted P2\n" << TransformP * P * TransformP.transpose() << endl; + + // Second Update + update(X, P, z2, H, R); + cout << "Updated X2\n" << X << endl; + cout << "Updated P2\n" << TransformP * P * TransformP.transpose() << endl; + + return 0; + } + From aafeabe1e3c63734f94e7d863dadc8e53cc05445 Mon Sep 17 00:00:00 2001 From: scottiyio Date: Thu, 24 Apr 2025 21:10:39 -0400 Subject: [PATCH 02/30] Add files via upload --- examples/LIEKF_SE2_SimpleGPSExample.cpp | 74 +++++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 examples/LIEKF_SE2_SimpleGPSExample.cpp diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp new file mode 100644 index 000000000..ae6396be2 --- /dev/null +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -0,0 +1,74 @@ +// +// Created by Scott on 4/18/2025. +// +#include +#include +#include + +using namespace std; +using namespace gtsam; + + // Measurement Processor + Vector2 h_gps(const Pose2& X, + OptionalJacobian<2,3> H = {}) { + return X.translation(H); + } + int main() { + static const int dim = traits::dimension; + + // Initialization + Pose2 X0(0.0, 0.0, 0.0); + Matrix3 P0 = Matrix3::Identity() * 0.1; + double dt = 1.0; + + // Define GPS measurements + Matrix23 H; + h_gps(X0, H); + Vector2 z1, z2; + z1 << 1.0, 0.0; + z2 << 1.0, 1.0; + + std::function)> measurement_function = h_gps; + LIEKF ekf(X0, P0, measurement_function); + + // Define Covariances + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal(); + + // Define odometry movements + Pose2 U1(1.0,1.0,0.5), U2(1.0,1.0,0.0); + + // Define a transformation matrix to convert the covariance into (theta, x, y) form. + Matrix3 TransformP; + TransformP << 0, 0, 1, + 1,0,0, + 0,1,0; + + // Predict / update stages + cout << "\nInitialization:\n"; + cout << "X0: " << ekf.getState() << endl; + cout << "P0: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + + ekf.predict(U1, Q); + cout << "\nFirst Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.update(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.predict(U2, Q); + cout << "\nSecond Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + return 0; + } \ No newline at end of file From 693a08990e1c3c686476726858d4e3c01f75bb58 Mon Sep 17 00:00:00 2001 From: scottiyio Date: Thu, 24 Apr 2025 21:11:26 -0400 Subject: [PATCH 03/30] Add files via upload --- examples/LIEKF_NavstateExample.cpp | 88 ++++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 examples/LIEKF_NavstateExample.cpp diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp new file mode 100644 index 000000000..042140cd3 --- /dev/null +++ b/examples/LIEKF_NavstateExample.cpp @@ -0,0 +1,88 @@ +// +// Created by Scott on 4/18/2025. +// +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// define dynamics +Vector9 dynamics(const NavState& X, const Vector6& imu, + OptionalJacobian<9, 9> H = {}) { + const auto a = imu.head<3>(); + const auto w = imu.tail<3>(); + Vector9 result; + result << w, Z_3x1, a; + if (H) { + *H = Matrix::Zero(9, 9); + } + return result; +} + +// define measurement processor + Vector3 h_gps(const NavState& X, + OptionalJacobian<3,9> H = {}) { + if (H) *H << Z_3x3, Z_3x3, X.R(); + return X.t(); + } + + int main() { + // Initialization + NavState X0; + Matrix9 P0 = Matrix9::Identity() * 0.1; + double dt = 1.0; + + // Create measurement function h_func + GeneralLIEKF::MeasurementFunction h_func = + [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; + + // Create dynamics + GeneralLIEKF::Dynamics dynamics_func = dynamics; + + // Initialize filter + GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); + + // Covariances + Matrix9 Q = Matrix9::Identity() * 0.1; + Matrix3 R = Matrix3::Identity()*0.01; + + // IMU measurements + Vector6 imu1, imu2; + imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + + // GPS measurements + Vector3 z1, z2; + z1 << 0.0, 0.0, 0.0; + z2 << 0.0, 0.0, 0.0; + + // Predict / update stages + cout << "\nInitialization:\n"; + cout << "X0: " << ekf.getState() << endl; + cout << "P0: " << ekf.getCovariance() << endl; + + + ekf.predict(imu1, dt, Q); + cout << "\nFirst Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << ekf.getCovariance() << endl; + + ekf.update(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << ekf.getCovariance() << endl; + + ekf.predict(imu2, dt, Q); + cout << "\nSecond Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << ekf.getCovariance() << endl; + + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << ekf.getCovariance() << endl; + + return 0; + } \ No newline at end of file From c521dbc45946a0c94267d2b337b4056a9e1e7009 Mon Sep 17 00:00:00 2001 From: scottiyio Date: Thu, 24 Apr 2025 21:12:19 -0400 Subject: [PATCH 04/30] Add files via upload --- gtsam/nonlinear/LIEKF.h | 176 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 176 insertions(+) create mode 100644 gtsam/nonlinear/LIEKF.h diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h new file mode 100644 index 000000000..4e510f33b --- /dev/null +++ b/gtsam/nonlinear/LIEKF.h @@ -0,0 +1,176 @@ +/* ---------------------------------------------------------------------------- + +* 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 LIEKF.h + * @brief Base and classes for Left Invariant Extended Kalman Filters + * + * Templates are implemented for a Left Invariant Extended Kalman Filter operating on Lie Groups. + * + * + * @date April 24, 2025 + * @author Scott Baker + * @author Matt Kielo + */ + +#pragma once +#include +#include +#include +#include +#include +namespace gtsam { + +/** +* @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) +* +* This class provides the prediction and update structure based on control inputs and a measurement function. +* +* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) +* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) +*/ + +template +class LIEKF { + +public: + static constexpr int n = traits::dimension; ///< Dimension of the state. + static constexpr int m = traits::dimension; ///< Dimension of the measurement. + + using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. + using MatrixN = Eigen::Matrix; ///< Typedef for the identity matrix. + + /** + * @brief Construct with a measurement function + * @param X0 Initial State + * @param P0 Initial Covariance + * @param h Measurement function + */ + LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ + : X(X0), P(P0), h_(h) {} + + + /** + * @brief Get current state estimate. + * @return Const reference to the state estiamte. + */ + const LieGroup& getState() const { return X; } + + /** + * @brief Get current covariance estimate. + * @return Const reference to the covariance estimate. + */ + const Matrix& getCovariance() const { return P; } + + /** + * @brief Prediction stage with a Lie group element U. + * @param U Lie group control input + * @param Q Process noise covariance matrix. + */ + void predict(const LieGroup& U, const Matrix& Q) { + LieGroup::Jacobian A; + X = X.compose(U, A); + P = A * P * A.transpose() + Q; + } + + /** + * @brief Prediction stage with a control vector u and a time interval dt. + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ + void predict(const Vector& u, double dt, const Matrix& Q) { + predict(LieGroup::Expmap(u*dt), Q);d + } + + /** + * @brief Update stage using a measurement and measurement covariance. + * @param z Measurement + * @param R Measurement noise covariance matrix. + */ + void update(const Measurement& z, const Matrix& R) { + Matrix H(m, n); + Vector y = h_(X, H)-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; // move Identity to be a constant. + } + + protected: + LieGroup X; ///< Current state estimate. + Matrix P; ///< Current covariance estimate. + + private: + MeasurementFunction h_; ///< Measurement function + static const MatrixN I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. + +}; + +/** +* @brief Create the static identity matrix I_n of size nxn for use in the update stage. +* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) +* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) +*/ +template +const typename LIEKF::MatrixN LIEKF::I_n + = typename LIEKF::MatrixN::Identity(); + +/** + * @brief General Left Invariant Extended Kalman Filter with dynamics function. + * + * This class extends the LIEKF class to include a dynamics function f. The dynamics maps + * a state and control vector to a tangent vector xi. + * + * @tparam LieGroup The Lie group type for the state. + * @tparam Measurement The type of the measurement. + * @tparam _p The dimension of the control vector. + */ +template +class GeneralLIEKF:public LIEKF { + public: + using Control = Eigen::Matrix; ///< Typedef for the control vector. + using TangentVector = typename traits::TangentVector; ///< Typedef for the tangent vector. + using Dynamics = std::function)>; + + + /** + * @brief Construct with general dynamics + * @param X0 Initial State + * @param P0 Initial Covariance + * @param f Dynamics function that depends on state and control vector + * @param h Measurement function + */ + GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, MeasurementFunction& +h) : LIEKF(X0, P0, h), f_(f) {} + + /** + * @brief Prediction stage with a dynamics function that calculates the tangent vector xi in the tangent space. + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ + void predict(const Control& u, double dt, const Matrix& Q) { + LieGroup::Jacobian H; + const TangentVector xi = f_(X, u, H); + LieGroup U = LieGroup::Expmap(xi * dt); + auto A = U.inverse().AdjointMap() * H; + X = X.compose(U); + P = A * P * A.transpose() + Q; + } + private: + Dynamics f_; ///< Dynamics function. +}; + +}// ends namespace + + From 136a7cdfd02a6dc07692703744f764d8fd7830ff Mon Sep 17 00:00:00 2001 From: scottiyio Date: Thu, 24 Apr 2025 22:06:46 -0400 Subject: [PATCH 05/30] Delete examples/SE2LIEKF_NoFactors.cpp --- examples/SE2LIEKF_NoFactors.cpp | 124 -------------------------------- 1 file changed, 124 deletions(-) delete mode 100644 examples/SE2LIEKF_NoFactors.cpp diff --git a/examples/SE2LIEKF_NoFactors.cpp b/examples/SE2LIEKF_NoFactors.cpp deleted file mode 100644 index 51693b7ee..000000000 --- a/examples/SE2LIEKF_NoFactors.cpp +++ /dev/null @@ -1,124 +0,0 @@ - -/* ---------------------------------------------------------------------------- - - * 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 SE2LIEKF_NoFactors.cpp - * - * A simple left invariant EKF operating in SE(2) using an odometry and GPS measurements. - * No factors are used here. - * 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 April 1, 2025 - * @author Scott Baker - */ - - - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -#define PI 3.14159265358 - -// Create an adjoint class -// Function for the prediction stage. -// Note that U is the exponential map of some control vector (u*dt) - void predict(Pose2& X, Matrix& P, Vector3& u, Matrix& Q) - { - //Pose2 U = Pose2::Expmap(u*dt); - Pose2 U(u(0), u(1), u(2)); - Matrix Adj = U.inverse().AdjointMap(); - X = X.compose(U); - P = Adj * P * Adj.transpose() + Q; - } - - // Update stage that uses GPS measurements in a left invariant update way. - void update(Pose2& X, Matrix& P, Point2& z, Matrix& H, Matrix& R) - { - // find residual R^T * (z - zhat) - Point2 zhat = X.translation(); - Point2 e_world = z-zhat; - Point2 residual = X.rotation().unrotate(e_world); - Matrix S = H*P*H.transpose() + R; - Matrix K = P*H.transpose()*S.inverse(); - Vector3 delta = K*residual; - X = X * Pose2::Expmap(delta); - P = (Matrix3::Identity() - K*H) * P; - } - - - -int main() { - - // Define movements and measurements - const double dt = 1.0; - - Vector3 u1(1.0, 1.0, 0.5), u2(1.0,1.0,0.0); // odometry - Point2 z1(1.0, 0.0), z2(1.0, 1.0); // gps - - - - // Set up noise models (uses simple GPS) - - // Odometry Process - Vector3 pModel(0.05, 0.05, 0.001); // covariance of the odometry process (rad) - Matrix Q = pModel.asDiagonal(); // Q covariance matrix - - // GPS process - Vector2 rModel(0.01, 0.01); - Matrix R = rModel.asDiagonal(); - Matrix H(2,3); - H << 1,0,0, - 0,1,0; - - // A matrix that transforms the P matrix into a form of [theta, x, y] for comparison to InEKF's structure. - Matrix3 TransformP; - TransformP << 0, 0, 1, - 1,0,0, - 0,1,0; - -// Initialize - Pose2 X(0, 0, 0); - Matrix P = Matrix3::Identity()*0.1; - - cout << "Initial X\n" << X << endl; - cout << "Initial P\n" << TransformP * P * TransformP.transpose() << endl; - - // First Prediction - predict(X, P, u1, dt, Q); - cout << "Predicted X1\n" << X << endl; - cout << "Predicted P1\n" << TransformP * P * TransformP.transpose() << endl; - - // First Update - update(X, P, z1, H, R); - cout << "Updated X1\n" << X << endl; - cout << "Updated P1\n" << TransformP * P * TransformP.transpose() << endl; - - // Second Prediction - predict(X, P, u2, dt, Q); - cout << "Predicted X2\n" << X << endl; - cout << "Predicted P2\n" << TransformP * P * TransformP.transpose() << endl; - - // Second Update - update(X, P, z2, H, R); - cout << "Updated X2\n" << X << endl; - cout << "Updated P2\n" << TransformP * P * TransformP.transpose() << endl; - - return 0; - } - From 12d422341d66aac2c386443c8b8cfe41b3f2ba9d Mon Sep 17 00:00:00 2001 From: scottiyio Date: Fri, 25 Apr 2025 14:43:19 -0400 Subject: [PATCH 06/30] clang-formatted, changed getFoobar() to foobar() --- gtsam/nonlinear/LIEKF.h | 192 ++++++++++++++++++++++------------------ 1 file changed, 104 insertions(+), 88 deletions(-) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 4e510f33b..b3520b06b 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -13,7 +13,8 @@ * @file LIEKF.h * @brief Base and classes for Left Invariant Extended Kalman Filters * - * Templates are implemented for a Left Invariant Extended Kalman Filter operating on Lie Groups. + * Templates are implemented for a Left Invariant Extended Kalman Filter + * operating on Lie Groups. * * * @date April 24, 2025 @@ -22,59 +23,66 @@ */ #pragma once -#include #include -#include #include +#include + #include +#include namespace gtsam { /** -* @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) -* -* This class provides the prediction and update structure based on control inputs and a measurement function. -* -* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) -* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) -*/ + * @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) + * + * This class provides the prediction and update structure based on control + * inputs and a measurement function. + * + * @tparam LieGroup Lie group used for state representation (e.g., Pose2, + * Pose3, NavState) + * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement + * for 3D position) + */ template class LIEKF { + public: + static constexpr int n = + traits::dimension; ///< Dimension of the state. + static constexpr int m = + traits::dimension; ///< Dimension of the measurement. -public: - static constexpr int n = traits::dimension; ///< Dimension of the state. - static constexpr int m = traits::dimension; ///< Dimension of the measurement. - - using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. - using MatrixN = Eigen::Matrix; ///< Typedef for the identity matrix. + using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. + using MatrixN = + Eigen::Matrix; ///< Typedef for the identity matrix. /** - * @brief Construct with a measurement function - * @param X0 Initial State - * @param P0 Initial Covariance - * @param h Measurement function - */ - LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ + * @brief Construct with a measurement function + * @param X0 Initial State + * @param P0 Initial Covariance + * @param h Measurement function + */ + LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ : X(X0), P(P0), h_(h) {} + /** + * @brief Get current state estimate. + * @return Const reference to the state estiamte. + */ + const LieGroup& state() const { return X; } /** - * @brief Get current state estimate. - * @return Const reference to the state estiamte. - */ - const LieGroup& getState() const { return X; } + * @brief Get current covariance estimate. + * @return Const reference to the covariance estimate. + */ + const Matrix& covariance() const { return P; } /** - * @brief Get current covariance estimate. - * @return Const reference to the covariance estimate. - */ - const Matrix& getCovariance() const { return P; } - - /** - * @brief Prediction stage with a Lie group element U. - * @param U Lie group control input - * @param Q Process noise covariance matrix. - */ + * @brief Prediction stage with a Lie group element U. + * @param U Lie group control input + * @param Q Process noise covariance matrix. + */ void predict(const LieGroup& U, const Matrix& Q) { LieGroup::Jacobian A; X = X.compose(U, A); @@ -82,83 +90,92 @@ public: } /** - * @brief Prediction stage with a control vector u and a time interval dt. - * @param u Control vector element - * @param dt Time interval - * @param Q Process noise covariance matrix. - */ - void predict(const Vector& u, double dt, const Matrix& Q) { - predict(LieGroup::Expmap(u*dt), Q);d + * @brief Prediction stage with a control vector u and a time interval dt. + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ + void predict(const Vector& u, double dt, const Matrix& Q) { + predict(LieGroup::Expmap(u * dt), Q); } /** - * @brief Update stage using a measurement and measurement covariance. - * @param z Measurement - * @param R Measurement noise covariance matrix. - */ + * @brief Update stage using a measurement and measurement covariance. + * @param z Measurement + * @param R Measurement noise covariance matrix. + */ void update(const Measurement& z, const Matrix& R) { Matrix H(m, n); - Vector y = h_(X, H)-z; + Vector y = h_(X, H) - 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; // move Identity to be a constant. + P = (I_n - K * H) * P; // move Identity to be a constant. } - protected: - LieGroup X; ///< Current state estimate. - Matrix P; ///< Current covariance estimate. - - private: - MeasurementFunction h_; ///< Measurement function - static const MatrixN I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. + protected: + LieGroup X; ///< Current state estimate. + Matrix P; ///< Current covariance estimate. + private: + MeasurementFunction h_; ///< Measurement function + static const MatrixN + I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. }; /** -* @brief Create the static identity matrix I_n of size nxn for use in the update stage. -* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) -* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) -*/ + * @brief Create the static identity matrix I_n of size nxn for use in the + * update stage. + * @tparam LieGroup Lie group used for state representation (e.g., Pose2, + * Pose3, NavState) + * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement + * for 3D position) + */ template -const typename LIEKF::MatrixN LIEKF::I_n - = typename LIEKF::MatrixN::Identity(); +const typename LIEKF::MatrixN + LIEKF::I_n = + typename LIEKF::MatrixN::Identity(); /** * @brief General Left Invariant Extended Kalman Filter with dynamics function. * - * This class extends the LIEKF class to include a dynamics function f. The dynamics maps - * a state and control vector to a tangent vector xi. + * This class extends the LIEKF class to include a dynamics function f. The + * dynamics maps a state and control vector to a tangent vector xi. * * @tparam LieGroup The Lie group type for the state. * @tparam Measurement The type of the measurement. * @tparam _p The dimension of the control vector. */ template -class GeneralLIEKF:public LIEKF { +class GeneralLIEKF : public LIEKF { public: - using Control = Eigen::Matrix; ///< Typedef for the control vector. - using TangentVector = typename traits::TangentVector; ///< Typedef for the tangent vector. - using Dynamics = std::function)>; - + using Control = + Eigen::Matrix; ///< Typedef for the control vector. + using TangentVector = + typename traits::TangentVector; ///< Typedef for the tangent + ///< vector. + using Dynamics = std::function)>; /** - * @brief Construct with general dynamics - * @param X0 Initial State - * @param P0 Initial Covariance - * @param f Dynamics function that depends on state and control vector - * @param h Measurement function - */ - GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, MeasurementFunction& -h) : LIEKF(X0, P0, h), f_(f) {} + * @brief Construct with general dynamics + * @param X0 Initial State + * @param P0 Initial Covariance + * @param f Dynamics function that depends on state and control vector + * @param h Measurement function + */ + GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, + MeasurementFunction& h) + : LIEKF(X0, P0, h), f_(f) {} /** - * @brief Prediction stage with a dynamics function that calculates the tangent vector xi in the tangent space. - * @param u Control vector element - * @param dt Time interval - * @param Q Process noise covariance matrix. - */ + * @brief Prediction stage with a dynamics function that calculates the + * tangent vector xi in the tangent space. + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ void predict(const Control& u, double dt, const Matrix& Q) { LieGroup::Jacobian H; const TangentVector xi = f_(X, u, H); @@ -167,10 +184,9 @@ h) : LIEKF(X0, P0, h), f_(f) {} X = X.compose(U); P = A * P * A.transpose() + Q; } - private: - Dynamics f_; ///< Dynamics function. + + private: + Dynamics f_; ///< Dynamics function. }; -}// ends namespace - - +} // namespace gtsam From 8aed7316a52bca940a70d691a288131d034ae37c Mon Sep 17 00:00:00 2001 From: scottiyio Date: Fri, 25 Apr 2025 15:02:30 -0400 Subject: [PATCH 07/30] Added documentation and format changes. - Added comments - Clang-formatted --- examples/LIEKF_NavstateExample.cpp | 149 ++++++++++++++++++----------- 1 file changed, 92 insertions(+), 57 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 042140cd3..6c6d594a7 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -1,14 +1,43 @@ -// -// Created by Scott on 4/18/2025. -// -#include +/* ---------------------------------------------------------------------------- + + * 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 LIEKF_NavstateExample.cpp + * @brief A left invariant Extended Kalman Filter example using the GeneralLIEKF + * on NavState using IMU/GPS measurements. + * + * This example uses the templated GeneralLIEKF class to estimate the state of + * an object using IMU/GPS measurements. The prediction stage of the LIEKF uses + * a generic dynamics function to predict the state. This simulates a navigation + * state of (pose, velocity, position) + * + * @date Apr 25, 2025 + * @author Scott Baker + * @author Matt Kielo + * @author Frank Dellaert + */ #include +#include + #include using namespace std; using namespace gtsam; -// define dynamics +// Define a dynamics function. +// The dynamics function for NavState returns a result vector of +// size 9 of [angular_velocity, 0, 0, 0, linear_acceleration] as well as +// a Jacobian of the dynamics function with respect to the state X. +// Since this is a left invariant EKF, the error dynamics do not rely on the +// state Vector9 dynamics(const NavState& X, const Vector6& imu, OptionalJacobian<9, 9> H = {}) { const auto a = imu.head<3>(); @@ -21,68 +50,74 @@ Vector9 dynamics(const NavState& X, const Vector6& imu, return result; } -// define measurement processor - Vector3 h_gps(const NavState& X, - OptionalJacobian<3,9> H = {}) { - if (H) *H << Z_3x3, Z_3x3, X.R(); - return X.t(); - } +// define a GPS measurement processor. The GPS measurement processor returns +// the expected measurement h(x) = translation of X with a Jacobian H used in +// the update stage of the LIEKF. +Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { + if (H) *H << Z_3x3, Z_3x3, X.R(); + return X.t(); +} - int main() { - // Initialization - NavState X0; - Matrix9 P0 = Matrix9::Identity() * 0.1; - double dt = 1.0; +int main() { + // Initialize the filter's state, covariance, and time interval values. + NavState X0; + Matrix9 P0 = Matrix9::Identity() * 0.1; + double dt = 1.0; - // Create measurement function h_func - GeneralLIEKF::MeasurementFunction h_func = - [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; + // Create the measurement function h_func that wraps h_gps + GeneralLIEKF::MeasurementFunction h_func = + [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; - // Create dynamics - GeneralLIEKF::Dynamics dynamics_func = dynamics; + // Create the dynamics function dynamics_func + GeneralLIEKF::Dynamics dynamics_func = dynamics; - // Initialize filter - GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); + // Create the filter with the initial state, covariance, and dynamics and + // measurement functions. + GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); - // Covariances - Matrix9 Q = Matrix9::Identity() * 0.1; - Matrix3 R = Matrix3::Identity()*0.01; + // Create the process covariance and measurement covariance matrices Q and R. + Matrix9 Q = Matrix9::Identity() * 0.01; + Matrix3 R = Matrix3::Identity() * 0.5; - // IMU measurements - Vector6 imu1, imu2; - imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + // Create the IMU measurements of the form (linear_acceleration, + // angular_velocity) + Vector6 imu1, imu2; + imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - // GPS measurements - Vector3 z1, z2; - z1 << 0.0, 0.0, 0.0; - z2 << 0.0, 0.0, 0.0; + // Create the GPS measurements of the form (px, py, pz) + Vector3 z1, z2; + z1 << 0.0, 0.0, 0.0; + z2 << 0.0, 0.0, 0.0; - // Predict / update stages - cout << "\nInitialization:\n"; - cout << "X0: " << ekf.getState() << endl; - cout << "P0: " << ekf.getCovariance() << endl; + // Run the predict and update stages, and print their results. + cout << "\nInitialization:\n"; + cout << "X0: " << ekf.state() << endl; + cout << "P0: " << ekf.covariance() << endl; + // First prediction stage + ekf.predict(imu1, dt, Q); + cout << "\nFirst Prediction:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << ekf.covariance() << endl; - ekf.predict(imu1, dt, Q); - cout << "\nFirst Prediction:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << ekf.getCovariance() << endl; + // First update stage + ekf.update(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << ekf.covariance() << endl; - ekf.update(z1, R); - cout << "\nFirst Update:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << ekf.getCovariance() << endl; + // Second prediction stage + ekf.predict(imu2, dt, Q); + cout << "\nSecond Prediction:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << ekf.covariance() << endl; - ekf.predict(imu2, dt, Q); - cout << "\nSecond Prediction:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << ekf.getCovariance() << endl; + // Second update stage + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << ekf.covariance() << endl; - ekf.update(z2, R); - cout << "\nSecond Update:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << ekf.getCovariance() << endl; - - return 0; - } \ No newline at end of file + return 0; +} \ No newline at end of file From 6ff04399f5e59796d5a0a378adf606900423d4eb Mon Sep 17 00:00:00 2001 From: scottiyio Date: Fri, 25 Apr 2025 15:31:52 -0400 Subject: [PATCH 08/30] Added authors --- gtsam/nonlinear/LIEKF.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index b3520b06b..598a6300a 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -20,6 +20,7 @@ * @date April 24, 2025 * @author Scott Baker * @author Matt Kielo + * @author Frank Dellaert */ #pragma once From f857001931cead6d0e174df6bb2067adc4eec68d Mon Sep 17 00:00:00 2001 From: scottiyio Date: Fri, 25 Apr 2025 15:32:31 -0400 Subject: [PATCH 09/30] Added documentation & clang formatting --- examples/LIEKF_SE2_SimpleGPSExample.cpp | 164 ++++++++++++++++-------- 1 file changed, 107 insertions(+), 57 deletions(-) diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index ae6396be2..c2d0acdb6 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -1,74 +1,124 @@ -// -// Created by Scott on 4/18/2025. -// -#include +/* ---------------------------------------------------------------------------- + + * 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 LIEKF_SE2_SimpleGPSExample.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 LIEKF class to estimate the state of + * an object using odometry / GPS measurements The prediction stage of the LIEKF + * uses a Lie Group element to propagate the stage in a discrete LIEKF. 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 + * @author Scott Baker + * @author Matt Kielo + * @author Frank Dellaert + */ #include +#include + #include using namespace std; using namespace gtsam; - // Measurement Processor - Vector2 h_gps(const Pose2& X, - OptionalJacobian<2,3> H = {}) { - return X.translation(H); - } - int main() { - static const int dim = traits::dimension; +// 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 LIEKF 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; + double dt = 1.0; - // Initialization - Pose2 X0(0.0, 0.0, 0.0); - Matrix3 P0 = Matrix3::Identity() * 0.1; - double dt = 1.0; + // Create the function measurement_function that wraps h_gps. + std::function)> + measurement_function = h_gps; + // Create the filter with the initial state, covariance, and measurement + // function. + LIEKF ekf(X0, P0, measurement_function); - // Define GPS measurements - Matrix23 H; - h_gps(X0, H); - Vector2 z1, z2; - z1 << 1.0, 0.0; - z2 << 1.0, 1.0; + // Define the process covariance and measurement covariance matrices Q and R. + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal(); - std::function)> measurement_function = h_gps; - LIEKF ekf(X0, P0, measurement_function); + // 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); - // Define Covariances - Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); - Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal(); + // 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 odometry movements - Pose2 U1(1.0,1.0,0.5), U2(1.0,1.0,0.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; - // Define a transformation matrix to convert the covariance into (theta, x, y) form. - 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; - // Predict / update stages - cout << "\nInitialization:\n"; - cout << "X0: " << ekf.getState() << endl; - cout << "P0: " << TransformP * ekf.getCovariance() * 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(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; - ekf.predict(U1, Q); - cout << "\nFirst Prediction:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << TransformP * ekf.getCovariance() * 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; - ekf.update(z1, R); - cout << "\nFirst Update:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + // Second update stage + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; - ekf.predict(U2, Q); - cout << "\nSecond Prediction:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; - - ekf.update(z2, R); - cout << "\nSecond Update:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; - - return 0; - } \ No newline at end of file + return 0; +} \ No newline at end of file From 0d832a65995eff723fc65ae76edd138b93c71966 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 11:14:57 -0400 Subject: [PATCH 11/30] no_boost --- examples/ISAM2_City10000.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/ISAM2_City10000.cpp b/examples/ISAM2_City10000.cpp index 2c210ab44..b756fb1e1 100644 --- a/examples/ISAM2_City10000.cpp +++ b/examples/ISAM2_City10000.cpp @@ -27,8 +27,6 @@ #include #include -#include -#include #include #include #include From b1c32cb6f157af11533f62e26d27cc3e9bb1edc0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 11:47:46 -0400 Subject: [PATCH 12/30] Templated predict/update --- examples/LIEKF_NavstateExample.cpp | 21 ++-- examples/LIEKF_SE2_SimpleGPSExample.cpp | 10 +- gtsam/nonlinear/LIEKF.h | 128 ++++++++---------------- 3 files changed, 51 insertions(+), 108 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 6c6d594a7..22639acb1 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -11,10 +11,10 @@ /** * @file LIEKF_NavstateExample.cpp - * @brief A left invariant Extended Kalman Filter example using the GeneralLIEKF + * @brief A left invariant Extended Kalman Filter example using the LIEKF * on NavState using IMU/GPS measurements. * - * This example uses the templated GeneralLIEKF class to estimate the state of + * This example uses the templated LIEKF class to estimate the state of * an object using IMU/GPS measurements. The prediction stage of the LIEKF uses * a generic dynamics function to predict the state. This simulates a navigation * state of (pose, velocity, position) @@ -64,16 +64,9 @@ int main() { Matrix9 P0 = Matrix9::Identity() * 0.1; double dt = 1.0; - // Create the measurement function h_func that wraps h_gps - GeneralLIEKF::MeasurementFunction h_func = - [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; - - // Create the dynamics function dynamics_func - GeneralLIEKF::Dynamics dynamics_func = dynamics; - // Create the filter with the initial state, covariance, and dynamics and // measurement functions. - GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); + LIEKF ekf(X0, P0); // Create the process covariance and measurement covariance matrices Q and R. Matrix9 Q = Matrix9::Identity() * 0.01; @@ -96,25 +89,25 @@ int main() { cout << "P0: " << ekf.covariance() << endl; // First prediction stage - ekf.predict(imu1, dt, Q); + ekf.predict<6>(dynamics, imu1, dt, Q); cout << "\nFirst Prediction:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; // First update stage - ekf.update(z1, R); + ekf.update(h_gps, z1, R); cout << "\nFirst Update:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; // Second prediction stage - ekf.predict(imu2, dt, Q); + ekf.predict<6>(dynamics, imu2, dt, Q); cout << "\nSecond Prediction:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; // Second update stage - ekf.update(z2, R); + ekf.update(h_gps, z2, R); cout << "\nSecond Update:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index c2d0acdb6..1f0a01212 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -53,14 +53,10 @@ 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; - double dt = 1.0; - // Create the function measurement_function that wraps h_gps. - std::function)> - measurement_function = h_gps; // Create the filter with the initial state, covariance, and measurement // function. - LIEKF ekf(X0, P0, measurement_function); + LIEKF ekf(X0, P0); // Define the process covariance and measurement covariance matrices Q and R. Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); @@ -100,7 +96,7 @@ int main() { << endl; // First update stage - ekf.update(z1, R); + ekf.update(h_gps, z1, R); cout << "\nFirst Update:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() @@ -114,7 +110,7 @@ int main() { << endl; // Second update stage - ekf.update(z2, R); + ekf.update(h_gps, z2, R); cout << "\nSecond Update:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 598a6300a..93fe5c8b5 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -38,23 +38,17 @@ namespace gtsam { * This class provides the prediction and update structure based on control * inputs and a measurement function. * - * @tparam LieGroup Lie group used for state representation (e.g., Pose2, + * @tparam G Lie group used for state representation (e.g., Pose2, * Pose3, NavState) * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement * for 3D position) */ -template +template class LIEKF { public: - static constexpr int n = - traits::dimension; ///< Dimension of the state. - static constexpr int m = - traits::dimension; ///< Dimension of the measurement. + static constexpr int n = traits::dimension; ///< Dimension of the state. - using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. using MatrixN = Eigen::Matrix; ///< Typedef for the identity matrix. @@ -64,14 +58,13 @@ class LIEKF { * @param P0 Initial Covariance * @param h Measurement function */ - LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ - : X(X0), P(P0), h_(h) {} + LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {} /** * @brief Get current state estimate. - * @return Const reference to the state estiamte. + * @return Const reference to the state estimate. */ - const LieGroup& state() const { return X; } + const G& state() const { return X; } /** * @brief Get current covariance estimate. @@ -84,8 +77,8 @@ class LIEKF { * @param U Lie group control input * @param Q Process noise covariance matrix. */ - void predict(const LieGroup& U, const Matrix& Q) { - LieGroup::Jacobian A; + void predict(const G& U, const Matrix& Q) { + typename G::Jacobian A; X = X.compose(U, A); P = A * P * A.transpose() + Q; } @@ -97,17 +90,41 @@ class LIEKF { * @param Q Process noise covariance matrix. */ void predict(const Vector& u, double dt, const Matrix& Q) { - predict(LieGroup::Expmap(u * dt), Q); + predict(G::Expmap(u * dt), Q); + } + + /** + * @brief Prediction stage with a dynamics function that calculates the + * tangent vector xi in the tangent space. + * @tparam _p The dimension of the control vector. + * @tparam Dynamics : (G, VectorP, OptionalJacobian) -> TangentVector + * @param f Dynamics function that depends on state and control vector + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ + template + void predict(Dynamics&& f, const Eigen::Matrix& u, double dt, + const Matrix& Q) { + typename G::Jacobian F; + const typename G::TangentVector xi = f(X, u, F); + G U = G::Expmap(xi * dt); + auto A = U.inverse().AdjointMap() * F; + X = X.compose(U); + P = A * P * A.transpose() + Q; } /** * @brief Update stage using a measurement and measurement covariance. + * @tparam Measurement + * @tparam Prediction : (G, OptionalJacobian) -> Measurement * @param z Measurement * @param R Measurement noise covariance matrix. */ - void update(const Measurement& z, const Matrix& R) { - Matrix H(m, n); - Vector y = h_(X, H) - z; + template + void update(Prediction&& h, const Measurement& z, const Matrix& R) { + Eigen::Matrix::dimension, n> H; + Vector y = h(X, H) - z; Matrix S = H * P * H.transpose() + R; Matrix K = P * H.transpose() * S.inverse(); X = X.expmap(-K * y); @@ -115,79 +132,16 @@ class LIEKF { } protected: - LieGroup X; ///< Current state estimate. - Matrix P; ///< Current covariance estimate. + G X; ///< Current state estimate. + Matrix P; ///< Current covariance estimate. private: - MeasurementFunction h_; ///< Measurement function static const MatrixN I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. }; -/** - * @brief Create the static identity matrix I_n of size nxn for use in the - * update stage. - * @tparam LieGroup Lie group used for state representation (e.g., Pose2, - * Pose3, NavState) - * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement - * for 3D position) - */ -template -const typename LIEKF::MatrixN - LIEKF::I_n = - typename LIEKF::MatrixN::Identity(); - -/** - * @brief General Left Invariant Extended Kalman Filter with dynamics function. - * - * This class extends the LIEKF class to include a dynamics function f. The - * dynamics maps a state and control vector to a tangent vector xi. - * - * @tparam LieGroup The Lie group type for the state. - * @tparam Measurement The type of the measurement. - * @tparam _p The dimension of the control vector. - */ -template -class GeneralLIEKF : public LIEKF { - public: - using Control = - Eigen::Matrix; ///< Typedef for the control vector. - using TangentVector = - typename traits::TangentVector; ///< Typedef for the tangent - ///< vector. - using Dynamics = std::function)>; - - /** - * @brief Construct with general dynamics - * @param X0 Initial State - * @param P0 Initial Covariance - * @param f Dynamics function that depends on state and control vector - * @param h Measurement function - */ - GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, - MeasurementFunction& h) - : LIEKF(X0, P0, h), f_(f) {} - - /** - * @brief Prediction stage with a dynamics function that calculates the - * tangent vector xi in the tangent space. - * @param u Control vector element - * @param dt Time interval - * @param Q Process noise covariance matrix. - */ - void predict(const Control& u, double dt, const Matrix& Q) { - LieGroup::Jacobian H; - const TangentVector xi = f_(X, u, H); - LieGroup U = LieGroup::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * H; - X = X.compose(U); - P = A * P * A.transpose() + Q; - } - - private: - Dynamics f_; ///< Dynamics function. -}; +/// Create the static identity matrix I_n of size nxn for use in update +template +const typename LIEKF::MatrixN LIEKF::I_n = LIEKF::MatrixN::Identity(); } // namespace gtsam From 935246549459b34f805fe9f97d1151a69660d375 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:14:12 -0400 Subject: [PATCH 13/30] More symmetric templating --- examples/LIEKF_NavstateExample.cpp | 4 ++-- gtsam/nonlinear/LIEKF.h | 24 +++++++++++++----------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 22639acb1..a99e91398 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -89,7 +89,7 @@ int main() { cout << "P0: " << ekf.covariance() << endl; // First prediction stage - ekf.predict<6>(dynamics, imu1, dt, Q); + ekf.predict(dynamics, imu1, dt, Q); cout << "\nFirst Prediction:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; @@ -101,7 +101,7 @@ int main() { cout << "P: " << ekf.covariance() << endl; // Second prediction stage - ekf.predict<6>(dynamics, imu2, dt, Q); + ekf.predict(dynamics, imu2, dt, Q); cout << "\nSecond Prediction:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 93fe5c8b5..3d71337fa 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -84,10 +84,13 @@ class LIEKF { } /** - * @brief Prediction stage with a control vector u and a time interval dt. + * @brief Prediction stage with a tangent vector xi and a time interval dt. * @param u Control vector element * @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 Vector& u, double dt, const Matrix& Q) { predict(G::Expmap(u * dt), Q); @@ -95,28 +98,27 @@ class LIEKF { /** * @brief Prediction stage with a dynamics function that calculates the - * tangent vector xi in the tangent space. - * @tparam _p The dimension of the control vector. - * @tparam Dynamics : (G, VectorP, OptionalJacobian) -> TangentVector - * @param f Dynamics function that depends on state and control vector - * @param u Control vector element + * tangent vector xi that *depends on the state*. + * @tparam Control The control input type + * @tparam Dynamics : (G, Control, OptionalJacobian) -> TangentVector + * @param f Dynamics function that depends on state and control input + * @param u Control input * @param dt Time interval * @param Q Process noise covariance matrix. */ - template - void predict(Dynamics&& f, const Eigen::Matrix& u, double dt, - const Matrix& Q) { + template + void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { typename G::Jacobian F; const typename G::TangentVector xi = f(X, u, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; + auto A = U.inverse().AdjointMap() * F; // chain rule for compose and f X = X.compose(U); P = A * P * A.transpose() + Q; } /** * @brief Update stage using a measurement and measurement covariance. - * @tparam Measurement + * @tparam Measurement The measurement output type * @tparam Prediction : (G, OptionalJacobian) -> Measurement * @param z Measurement * @param R Measurement noise covariance matrix. From f0e35aecea7edf61f74a4f110b9c85ce418d7160 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:24:00 -0400 Subject: [PATCH 14/30] Comments with o4-mini --- gtsam/nonlinear/LIEKF.h | 166 ++++++++++++++++++++++------------------ 1 file changed, 90 insertions(+), 76 deletions(-) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 3d71337fa..840d004a7 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- -* GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -10,82 +10,82 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF.h - * @brief Base and classes for Left Invariant Extended Kalman Filters + * @file LIEKF.h + * @brief Left-Invariant Extended Kalman Filter (LIEKF) implementation * - * Templates are implemented for a Left Invariant Extended Kalman Filter - * operating on Lie Groups. + * This file defines the LIEKF 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 - * @author Scott Baker - * @author Matt Kielo - * @author Frank Dellaert + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #pragma once + #include #include #include #include -#include + + namespace gtsam { /** - * @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) + * @class LIEKF + * @brief Left-Invariant Extended Kalman Filter (LIEKF) on a Lie group G * - * This class provides the prediction and update structure based on control - * inputs and a measurement function. + * @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() * - * @tparam G Lie group used for state representation (e.g., Pose2, - * Pose3, NavState) - * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement - * for 3D position) + * 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 LIEKF { public: - static constexpr int n = traits::dimension; ///< Dimension of the state. + /// Tangent-space dimension + static constexpr int n = traits::dimension; - using MatrixN = - Eigen::Matrix; ///< Typedef for the identity matrix. + /// Square matrix of size n for covariance and Jacobians + using MatrixN = Eigen::Matrix; + + /// Constructor: initialize with state and covariance + LIEKF(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_; } /** - * @brief Construct with a measurement function - * @param X0 Initial State - * @param P0 Initial Covariance - * @param h Measurement function - */ - LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {} - - /** - * @brief Get current state estimate. - * @return Const reference to the state estimate. - */ - const G& state() const { return X; } - - /** - * @brief Get current covariance estimate. - * @return Const reference to the covariance estimate. - */ - const Matrix& covariance() const { return P; } - - /** - * @brief Prediction stage with a Lie group element U. - * @param U Lie group control input - * @param Q Process noise covariance matrix. + * 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. + * + * @param U Lie group increment (e.g., Expmap of control * dt) + * @param Q process noise covariance in tangent space */ void predict(const G& U, const Matrix& Q) { typename G::Jacobian A; - X = X.compose(U, A); - P = A * P * A.transpose() + Q; + X_ = X_.compose(U, A); + P_ = A * P_ * A.transpose() + Q; } /** - * @brief Prediction stage with a tangent vector xi and a time interval dt. - * @param u Control vector element + * 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. * @@ -97,52 +97,66 @@ class LIEKF { } /** - * @brief Prediction stage with a dynamics function that calculates the - * tangent vector xi that *depends on the state*. - * @tparam Control The control input type - * @tparam Dynamics : (G, Control, OptionalJacobian) -> TangentVector - * @param f Dynamics function that depends on state and control input - * @param u Control input - * @param dt Time interval - * @param Q Process noise covariance matrix. + * Predict step with state-dependent dynamics: + * xi = f(X, u, F) + * U = Expmap(xi * dt) + * A = Ad_{U^{-1}} * F + * + * @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) { typename G::Jacobian F; - const typename G::TangentVector xi = f(X, u, F); + auto xi = f(X_, u, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; // chain rule for compose and f - X = X.compose(U); - P = A * P * A.transpose() + Q; + auto A = U.inverse().AdjointMap() * F; + X_ = X_.compose(U); + P_ = A * P_ * A.transpose() + Q; } /** - * @brief Update stage using a measurement and measurement covariance. - * @tparam Measurement The measurement output type - * @tparam Prediction : (G, OptionalJacobian) -> Measurement - * @param z Measurement - * @param R Measurement noise covariance matrix. + * 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; - Vector y = h(X, H) - 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; // move Identity to be a constant. + 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; ///< Current state estimate. - Matrix P; ///< Current covariance estimate. + G X_; ///< group state estimate + Matrix P_; ///< covariance in tangent space private: - static const MatrixN - I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. + /// Identity matrix of size n + static const MatrixN I_n; }; -/// Create the static identity matrix I_n of size nxn for use in update +// Define static identity I_n template const typename LIEKF::MatrixN LIEKF::I_n = LIEKF::MatrixN::Identity(); From f63255be5b81a9ef44423c80935f3adff3a56977 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:30:41 -0400 Subject: [PATCH 15/30] Move to nav --- gtsam/{nonlinear => navigation}/LIEKF.h | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/{nonlinear => navigation}/LIEKF.h (100%) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/navigation/LIEKF.h similarity index 100% rename from gtsam/nonlinear/LIEKF.h rename to gtsam/navigation/LIEKF.h From 885ab38a7e7a1ee3d324a4710cfc022950fc2fdf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:30:55 -0400 Subject: [PATCH 16/30] Docs with o4-mini --- examples/LIEKF_NavstateExample.cpp | 113 ++++++++++++++--------------- 1 file changed, 54 insertions(+), 59 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index a99e91398..0747d9190 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -11,19 +11,14 @@ /** * @file LIEKF_NavstateExample.cpp - * @brief A left invariant Extended Kalman Filter example using the LIEKF - * on NavState using IMU/GPS measurements. - * - * This example uses the templated LIEKF class to estimate the state of - * an object using IMU/GPS measurements. The prediction stage of the LIEKF uses - * a generic dynamics function to predict the state. This simulates a navigation - * state of (pose, velocity, position) - * - * @date Apr 25, 2025 - * @author Scott Baker - * @author Matt Kielo - * @author Frank Dellaert + * @brief Example of a Left-Invariant Extended Kalman Filter on NavState + * using IMU (predict) and GPS (update) measurements. + * @date April 25, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert */ + +#include +#include #include #include @@ -32,85 +27,85 @@ using namespace std; using namespace gtsam; -// Define a dynamics function. -// The dynamics function for NavState returns a result vector of -// size 9 of [angular_velocity, 0, 0, 0, linear_acceleration] as well as -// a Jacobian of the dynamics function with respect to the state X. -// Since this is a left invariant EKF, the error dynamics do not rely on the -// state +/** + * @brief Left-invariant dynamics for NavState. + * @param X Current state (unused for left-invariant error dynamics). + * @param imu 6×1 vector [a; ω]: linear accel (first 3) and angular vel (last + * 3). + * @param H Optional 9×9 Jacobian w.r.t. X (always zero here). + * @return 9×1 tangent: [ω; 0₃; a]. + */ Vector9 dynamics(const NavState& X, const Vector6& imu, OptionalJacobian<9, 9> H = {}) { - const auto a = imu.head<3>(); - const auto w = imu.tail<3>(); - Vector9 result; - result << w, Z_3x1, a; - if (H) { - *H = Matrix::Zero(9, 9); - } - return result; + auto a = imu.head<3>(); + auto w = imu.tail<3>(); + Vector9 xi; + xi << w, Vector3::Zero(), a; + if (H) *H = Matrix9::Zero(); + return xi; } -// define a GPS measurement processor. The GPS measurement processor returns -// the expected measurement h(x) = translation of X with a Jacobian H used in -// the update stage of the LIEKF. +/** + * @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 = {}) { - if (H) *H << Z_3x3, Z_3x3, X.R(); + if (H) { + // H = [ 0₃×3, 0₃×3, R ] + *H << Z_3x3, Z_3x3, X.R(); + } return X.t(); } int main() { - // Initialize the filter's state, covariance, and time interval values. + // Initial state, covariance, and time step NavState X0; Matrix9 P0 = Matrix9::Identity() * 0.1; double dt = 1.0; - // Create the filter with the initial state, covariance, and dynamics and - // measurement functions. + // Create the filter with the initial state and covariance. LIEKF ekf(X0, P0); - // Create the process covariance and measurement covariance matrices Q and R. + // Process & measurement noise Matrix9 Q = Matrix9::Identity() * 0.01; Matrix3 R = Matrix3::Identity() * 0.5; // Create the IMU measurements of the form (linear_acceleration, // angular_velocity) Vector6 imu1, imu2; - imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + imu1 << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0; + imu2 << 0.0, 0.3, 0.0, 0.4, 0.0, 0.0; // Create the GPS measurements of the form (px, py, pz) Vector3 z1, z2; - z1 << 0.0, 0.0, 0.0; - z2 << 0.0, 0.0, 0.0; + z1 << 0.3, 0.0, 0.0; + z2 << 0.6, 0.0, 0.0; - // Run the predict and update stages, and print their results. - cout << "\nInitialization:\n"; - cout << "X0: " << ekf.state() << endl; - cout << "P0: " << ekf.covariance() << endl; + cout << "=== Initialization ===\n" + << "X0: " << ekf.state() << "\n" + << "P0: " << ekf.covariance() << "\n\n"; - // First prediction stage ekf.predict(dynamics, imu1, dt, Q); - cout << "\nFirst Prediction:\n"; - cout << "X: " << ekf.state() << endl; - cout << "P: " << ekf.covariance() << endl; + cout << "--- After first predict ---\n" + << "X: " << ekf.state() << "\n" + << "P: " << ekf.covariance() << "\n\n"; - // First update stage ekf.update(h_gps, z1, R); - cout << "\nFirst Update:\n"; - cout << "X: " << ekf.state() << endl; - cout << "P: " << ekf.covariance() << endl; + cout << "--- After first update ---\n" + << "X: " << ekf.state() << "\n" + << "P: " << ekf.covariance() << "\n\n"; - // Second prediction stage ekf.predict(dynamics, imu2, dt, Q); - cout << "\nSecond Prediction:\n"; - cout << "X: " << ekf.state() << endl; - cout << "P: " << ekf.covariance() << endl; + cout << "--- After second predict ---\n" + << "X: " << ekf.state() << "\n" + << "P: " << ekf.covariance() << "\n\n"; - // Second update stage ekf.update(h_gps, z2, R); - cout << "\nSecond Update:\n"; - cout << "X: " << ekf.state() << endl; - cout << "P: " << ekf.covariance() << endl; + cout << "--- After second update ---\n" + << "X: " << ekf.state() << "\n" + << "P: " << ekf.covariance() << "\n"; return 0; -} \ No newline at end of file +} From 0ee0a8beecd33f0c5c909ed54a29a9f074514d8e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:31:02 -0400 Subject: [PATCH 17/30] Test WIP --- gtsam/navigation/tests/testLIEKF.cpp | 61 ++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 gtsam/navigation/tests/testLIEKF.cpp diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp new file mode 100644 index 000000000..81e80a69d --- /dev/null +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + * 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 testLIEKFNavState.cpp + * @brief Unit test for the NavState dynamics Jacobian in LIEKF example. + * @date April 26, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#include +#include +#include +#include + +using namespace gtsam; + +// Duplicate the dynamics function under test: +namespace example { +Vector9 dynamics(const NavState& X, const Vector6& imu, + OptionalJacobian<9, 9> H = {}) { + auto a = imu.head<3>(); + auto w = imu.tail<3>(); + Vector9 xi; + xi << w, Vector3::Zero(), a; + if (H) *H = Matrix9::Zero(); + return xi; +} +} // namespace example + +TEST(LIEKFNavState, dynamicsJacobian) { + // Construct a nontrivial state and IMU input + NavState X(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1.0, 2.0, 3.0), + Vector3(0.5, -0.5, 0.5)); + Vector6 imu; + imu << 0.1, -0.1, 0.2, // acceleration + 0.01, -0.02, 0.03; // angular velocity + + // Analytic Jacobian (always zero for left-invariant dynamics) + OptionalJacobian<9, 9> H_analytic; + example::dynamics(X, imu, H_analytic); + Matrix actualH = *H_analytic; + + // Numeric Jacobian w.r.t. the state X + auto f = [&](const NavState& X_) { return example::dynamics(X_, imu); }; + Matrix expectedH = numericalDerivative11(f, X, 1e-6); + + // Compare + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} From 5af25dc30f6c4fe846af49a71c4df89aa814eb1c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:33:05 -0400 Subject: [PATCH 18/30] Use nav version --- examples/LIEKF_NavstateExample.cpp | 2 +- examples/LIEKF_SE2_SimpleGPSExample.cpp | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 0747d9190..67ae23fd1 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index 1f0a01212..ecef67cf7 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -29,12 +29,10 @@ * Beard, and Joshua G. Mangelson * * @date Apr 25, 2025 - * @author Scott Baker - * @author Matt Kielo - * @author Frank Dellaert + * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #include -#include +#include #include From 062bdf64ea680e3f13d53427a21df6e174ee4ead Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 10:40:28 -0400 Subject: [PATCH 19/30] Added Rot3 example to show state-dependent control --- examples/LIEKF_NavstateExample.cpp | 81 ++++++++++++---------------- examples/LIEKF_Rot3Example.cpp | 78 +++++++++++++++++++++++++++ gtsam/navigation/LIEKF.h | 22 ++++++++ gtsam/navigation/tests/testLIEKF.cpp | 36 ++++++------- 4 files changed, 150 insertions(+), 67 deletions(-) create mode 100644 examples/LIEKF_Rot3Example.cpp diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 67ae23fd1..250d5cda8 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -11,16 +11,15 @@ /** * @file LIEKF_NavstateExample.cpp - * @brief Example of a Left-Invariant Extended Kalman Filter on NavState - * using IMU (predict) and GPS (update) measurements. + * @brief LIEKF 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 #include @@ -29,19 +28,14 @@ using namespace gtsam; /** * @brief Left-invariant dynamics for NavState. - * @param X Current state (unused for left-invariant error dynamics). - * @param imu 6×1 vector [a; ω]: linear accel (first 3) and angular vel (last - * 3). - * @param H Optional 9×9 Jacobian w.r.t. X (always zero here). + * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity. * @return 9×1 tangent: [ω; 0₃; a]. */ -Vector9 dynamics(const NavState& X, const Vector6& imu, - OptionalJacobian<9, 9> H = {}) { +Vector9 dynamics(const Vector6& imu, OptionalJacobian<9, 9> H = {}) { auto a = imu.head<3>(); auto w = imu.tail<3>(); Vector9 xi; xi << w, Vector3::Zero(), a; - if (H) *H = Matrix9::Zero(); return xi; } @@ -52,60 +46,51 @@ Vector9 dynamics(const NavState& X, const Vector6& imu, * @return 3×1 position vector. */ Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { - if (H) { - // H = [ 0₃×3, 0₃×3, R ] - *H << Z_3x3, Z_3x3, X.R(); - } + if (H) *H << Z_3x3, Z_3x3, X.R().matrix(); return X.t(); } int main() { - // Initial state, covariance, and time step - NavState X0; + // Initial state & covariances + NavState X0; // R=I, v=0, t=0 Matrix9 P0 = Matrix9::Identity() * 0.1; - double dt = 1.0; - - // Create the filter with the initial state and covariance. LIEKF ekf(X0, P0); - // Process & measurement noise + // Noise & timestep + double dt = 1.0; Matrix9 Q = Matrix9::Identity() * 0.01; Matrix3 R = Matrix3::Identity() * 0.5; - // Create the IMU measurements of the form (linear_acceleration, - // angular_velocity) - Vector6 imu1, imu2; - imu1 << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0; - imu2 << 0.0, 0.3, 0.0, 0.4, 0.0, 0.0; + // 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; - // Create the GPS measurements of the form (px, py, pz) - Vector3 z1, z2; - z1 << 0.3, 0.0, 0.0; - z2 << 0.6, 0.0, 0.0; + // Two GPS fixes + Vector3 z1; + z1 << 0.3, 0, 0; + Vector3 z2; + z2 << 0.6, 0, 0; - cout << "=== Initialization ===\n" - << "X0: " << ekf.state() << "\n" - << "P0: " << ekf.covariance() << "\n\n"; - - ekf.predict(dynamics, imu1, dt, Q); - cout << "--- After first predict ---\n" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n\n"; + 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 first update ---\n" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n\n"; - - ekf.predict(dynamics, imu2, dt, Q); - cout << "--- After second predict ---\n" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n\n"; + 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 second update ---\n" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n"; + cout << "--- After update 2 ---\nX: " << ekf.state() + << "\nP: " << ekf.covariance() << "\n"; return 0; } diff --git a/examples/LIEKF_Rot3Example.cpp b/examples/LIEKF_Rot3Example.cpp new file mode 100644 index 000000000..331c01bb1 --- /dev/null +++ b/examples/LIEKF_Rot3Example.cpp @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------------- + + * 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 LIEKF_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φ; + Vector3 φ = Rot3::Logmap(X, Dφ); + // zero out yaw + φ[2] = 0.0; + Dφ.row(2).setZero(); + + if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR + return -k * φ; // 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; + LIEKF 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/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index 840d004a7..843365c6d 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -98,6 +98,28 @@ class LIEKF { /** * Predict step with state-dependent dynamics: + * xi = f(X, F) + * U = Expmap(xi * dt) + * A = Ad_{U^{-1}} * F + * + * @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 F; + auto xi = f(X_, F); + G U = G::Expmap(xi * dt); + auto A = U.inverse().AdjointMap() * F; + X_ = X_.compose(U); + P_ = A * P_ * A.transpose() + Q; + } + + /** + * Predict step with state and control input dynamics: * xi = f(X, u, F) * U = Expmap(xi * dt) * A = Ad_{U^{-1}} * F diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 81e80a69d..23f812274 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -21,35 +21,33 @@ using namespace gtsam; -// Duplicate the dynamics function under test: +// Duplicate the dynamics function in LIEKF_Rot3Example namespace example { -Vector9 dynamics(const NavState& X, const Vector6& imu, - OptionalJacobian<9, 9> H = {}) { - auto a = imu.head<3>(); - auto w = imu.tail<3>(); - Vector9 xi; - xi << w, Vector3::Zero(), a; - if (H) *H = Matrix9::Zero(); - return xi; +static constexpr double k = 0.5; +Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { + // φ = Logmap(R), Dφ = ∂φ/∂δR + Matrix3 Dφ; + Vector3 φ = Rot3::Logmap(X, Dφ); + // zero out yaw + φ[2] = 0.0; + Dφ.row(2).setZero(); + + if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR + return -k * φ; // xi ∈ 𝔰𝔬(3) } } // namespace example TEST(LIEKFNavState, dynamicsJacobian) { // Construct a nontrivial state and IMU input - NavState X(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1.0, 2.0, 3.0), - Vector3(0.5, -0.5, 0.5)); - Vector6 imu; - imu << 0.1, -0.1, 0.2, // acceleration - 0.01, -0.02, 0.03; // angular velocity + Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); // Analytic Jacobian (always zero for left-invariant dynamics) - OptionalJacobian<9, 9> H_analytic; - example::dynamics(X, imu, H_analytic); - Matrix actualH = *H_analytic; + Matrix3 actualH; + example::dynamics(R, actualH); // Numeric Jacobian w.r.t. the state X - auto f = [&](const NavState& X_) { return example::dynamics(X_, imu); }; - Matrix expectedH = numericalDerivative11(f, X, 1e-6); + auto f = [&](const Rot3& X_) { return example::dynamics(X_); }; + Matrix3 expectedH = numericalDerivative11(f, R, 1e-6); // Compare EXPECT(assert_equal(expectedH, actualH, 1e-8)); From 1ced4d0470862c5478afc11a0debc065482b793d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 13:35:24 -0400 Subject: [PATCH 20/30] Review comments --- examples/LIEKF_NavstateExample.cpp | 2 +- examples/LIEKF_Rot3Example.cpp | 12 ++++++------ examples/LIEKF_SE2_SimpleGPSExample.cpp | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 250d5cda8..9798dca02 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -31,7 +31,7 @@ using namespace gtsam; * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity. * @return 9×1 tangent: [ω; 0₃; a]. */ -Vector9 dynamics(const Vector6& imu, OptionalJacobian<9, 9> H = {}) { +Vector9 dynamics(const Vector6& imu) { auto a = imu.head<3>(); auto w = imu.tail<3>(); Vector9 xi; diff --git a/examples/LIEKF_Rot3Example.cpp b/examples/LIEKF_Rot3Example.cpp index 331c01bb1..172af894d 100644 --- a/examples/LIEKF_Rot3Example.cpp +++ b/examples/LIEKF_Rot3Example.cpp @@ -32,14 +32,14 @@ using namespace gtsam; static constexpr double k = 0.5; Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) { // φ = Logmap(R), Dφ = ∂φ/∂δR - Matrix3 Dφ; - Vector3 φ = Rot3::Logmap(X, Dφ); + Matrix3 D_phi; + Vector3 phi = Rot3::Logmap(X, D_phi); // zero out yaw - φ[2] = 0.0; - Dφ.row(2).setZero(); + phi[2] = 0.0; + D_phi.row(2).setZero(); - if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR - return -k * φ; // xi ∈ 𝔰𝔬(3) + if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR + return -k * phi; // xi ∈ 𝔰𝔬(3) } // --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× --- diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index ecef67cf7..0985d2372 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -58,7 +58,7 @@ int main() { // Define the process covariance and measurement covariance matrices Q and R. Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); - Matrix2 R = (Vector2(0.01, 0.01)).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. From a0c6902c8efbf5303095b6e7477e73a57a97b21d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 13:35:56 -0400 Subject: [PATCH 21/30] SFINAE to restrict template matching --- gtsam/navigation/LIEKF.h | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index 843365c6d..a8a136094 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -29,7 +29,7 @@ #include #include - +#include namespace gtsam { @@ -92,7 +92,7 @@ class LIEKF { * @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 Vector& u, double dt, const Matrix& Q) { + void predict(const typename G::TangentVector& u, double dt, const Matrix& Q) { predict(G::Expmap(u * dt), Q); } @@ -108,12 +108,18 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template + template + // …and is callable as a true functor + && std::is_invocable_r_v&> > > void predict(Dynamics&& f, double dt, const Matrix& Q) { typename G::Jacobian F; - auto xi = f(X_, F); + typename G::TangentVector xi = f(X_, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; + typename G::Jacobian A = U.inverse().AdjointMap() * F; X_ = X_.compose(U); P_ = A * P_ * A.transpose() + Q; } @@ -133,12 +139,19 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template + template & // H + > > > void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { typename G::Jacobian F; - auto xi = f(X_, u, F); + typename G::TangentVector xi = f(X_, u, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; + typename G::Jacobian A = U.inverse().AdjointMap() * F; X_ = X_.compose(U); P_ = A * P_ * A.transpose() + Q; } From 4f2a62aa3a45d113660b451921a8c09a5d8e4808 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 17:59:34 -0400 Subject: [PATCH 22/30] Update testLIEKF.cpp --- gtsam/navigation/tests/testLIEKF.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 23f812274..65325c63e 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -26,14 +26,14 @@ namespace example { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { // φ = Logmap(R), Dφ = ∂φ/∂δR - Matrix3 Dφ; - Vector3 φ = Rot3::Logmap(X, Dφ); + Matrix3 D_phi; + Vector3 phi = Rot3::Logmap(X, D_phi); // zero out yaw - φ[2] = 0.0; - Dφ.row(2).setZero(); + phi[2] = 0.0; + D_phi.row(2).setZero(); - if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR - return -k * φ; // xi ∈ 𝔰𝔬(3) + if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR + return -k * phi; // xi ∈ 𝔰𝔬(3) } } // namespace example From 104341f4cf84d3a845daf07658a1a01d214519f7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 20:42:17 -0400 Subject: [PATCH 23/30] well-tested predictMean --- gtsam/navigation/LIEKF.h | 51 ++++++++++++++++++++-------- gtsam/navigation/tests/testLIEKF.cpp | 42 ++++++++++++++++++----- 2 files changed, 71 insertions(+), 22 deletions(-) diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index a8a136094..fda75ccd5 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -96,6 +96,34 @@ class LIEKF { predict(G::Expmap(u * dt), Q); } + /** + * Predict mean only with state-dependent dynamics: + * xi = f(X, F) + * U = Expmap(xi * dt) + * A = Ad_{U^{-1}} * F + * + * @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 + // …and is callable as a true functor + && std::is_invocable_r_v>>> + G predictMean(Dynamics&& f, double dt, const Matrix& Q, + OptionalJacobian A = {}) const { + typename G::Jacobian F1, F2; + typename G::TangentVector xi = f(X_, F1); + G U = G::Expmap(xi * dt, F2); + if (A) *A = U.inverse().AdjointMap() + F2 * F1 * dt; + return X_.compose(U); + } + /** * Predict step with state-dependent dynamics: * xi = f(X, F) @@ -114,13 +142,10 @@ class LIEKF { !std::is_convertible_v // …and is callable as a true functor && std::is_invocable_r_v&> > > + const G&, OptionalJacobian>>> void predict(Dynamics&& f, double dt, const Matrix& Q) { - typename G::Jacobian F; - typename G::TangentVector xi = f(X_, F); - G U = G::Expmap(xi * dt); - typename G::Jacobian A = U.inverse().AdjointMap() * F; - X_ = X_.compose(U); + typename G::Jacobian A; + X_ = predictMean(f, dt, Q, &A); P_ = A * P_ * A.transpose() + Q; } @@ -145,15 +170,13 @@ class LIEKF { Dynamics, // functor const G&, // X const Control&, // u - OptionalJacobian& // H - > > > + OptionalJacobian // H + >>> void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { - typename G::Jacobian F; - typename G::TangentVector xi = f(X_, u, F); - G U = G::Expmap(xi * dt); - typename G::Jacobian A = U.inverse().AdjointMap() * F; - X_ = X_.compose(U); - P_ = A * P_ * A.transpose() + Q; + auto g = [f, u](const G& X, OptionalJacobian F) { + return f(X, u, F); + }; + predict(g, dt, Q); } /** diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 65325c63e..9790833f8 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -17,12 +17,14 @@ #include #include #include +#include +#include #include using namespace gtsam; // Duplicate the dynamics function in LIEKF_Rot3Example -namespace example { +namespace exampleSO3 { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { // φ = Logmap(R), Dφ = ∂φ/∂δR @@ -35,22 +37,46 @@ Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR return -k * phi; // xi ∈ 𝔰𝔬(3) } -} // namespace example +} // namespace exampleSO3 -TEST(LIEKFNavState, dynamicsJacobian) { +TEST(IEKF, dynamicsJacobian) { // Construct a nontrivial state and IMU input Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); - // Analytic Jacobian (always zero for left-invariant dynamics) + // Analytic Jacobian Matrix3 actualH; - example::dynamics(R, actualH); + exampleSO3::dynamics(R, actualH); // Numeric Jacobian w.r.t. the state X - auto f = [&](const Rot3& X_) { return example::dynamics(X_); }; - Matrix3 expectedH = numericalDerivative11(f, R, 1e-6); + auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); }; + Matrix3 expectedH = numericalDerivative11(f, R); // Compare - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH)); +} + +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; + LIEKF iekf0(R0, P0); + iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + LIEKF iekf(R, P0); + return iekf.predictMean(exampleSO3::dynamics, dt, Q); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); } int main() { From 87c4445f53ee53f39b2aba7a226fdae32fd97bfd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:01:12 -0400 Subject: [PATCH 24/30] predictMean for state=control f --- gtsam/navigation/LIEKF.h | 83 ++++++++++++++++------------ gtsam/navigation/tests/testLIEKF.cpp | 30 ++++++++++ 2 files changed, 78 insertions(+), 35 deletions(-) diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index fda75ccd5..9b4b6d2b5 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -96,53 +96,51 @@ class LIEKF { predict(G::Expmap(u * dt), Q); } + /** + * 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) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * 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 - // …and is callable as a true functor - && std::is_invocable_r_v>>> + template > G predictMean(Dynamics&& f, double dt, const Matrix& Q, OptionalJacobian A = {}) const { - typename G::Jacobian F1, F2; - typename G::TangentVector xi = f(X_, F1); - G U = G::Expmap(xi * dt, F2); - if (A) *A = U.inverse().AdjointMap() + F2 * F1 * dt; + 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); } /** * Predict step with state-dependent dynamics: * xi = f(X, F) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * 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 - // …and is callable as a true functor - && std::is_invocable_r_v>>> + template > void predict(Dynamics&& f, double dt, const Matrix& Q) { typename G::Jacobian A; X_ = predictMean(f, dt, Q, &A); @@ -150,10 +148,9 @@ class LIEKF { } /** - * Predict step with state and control input dynamics: + * Predict mean only with state and control input dynamics: * xi = f(X, u, F) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * F + * X_.expmap(xi * dt) * * @tparam Control control input type * @tparam Dynamics signature: G f(const G&, const Control&, @@ -164,14 +161,30 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template // H - >>> + 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 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); diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 9790833f8..5577a4357 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -79,6 +79,36 @@ TEST(IEKF, PredictNumericState) { EXPECT(assert_equal(expectedH, actualH)); } +TEST(IEKF, 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; + LIEKF iekf0(R0, P0); + iekf0.predictMean(f, dummy_u, dt, Q, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + LIEKF iekf(R, P0); + return iekf.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); From 5bc01081ee1bf4e4b28683c85a7ec7fb7c8bc38a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:03:34 -0400 Subject: [PATCH 25/30] Correct jacobian for h_gps --- examples/LIEKF_NavstateExample.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 9798dca02..a32d367b2 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -46,8 +46,7 @@ Vector9 dynamics(const Vector6& imu) { * @return 3×1 position vector. */ Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { - if (H) *H << Z_3x3, Z_3x3, X.R().matrix(); - return X.t(); + return X.position(H); } int main() { From ab605770fb0c774e6494504ae16133fb0cdb9e57 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:11:11 -0400 Subject: [PATCH 26/30] Rename --- examples/{LIEKF_NavstateExample.cpp => IEKF_NavstateExample.cpp} | 0 examples/{LIEKF_Rot3Example.cpp => IEKF_Rot3Example.cpp} | 0 examples/{LIEKF_SE2_SimpleGPSExample.cpp => IEKF_SE2Example.cpp} | 0 gtsam/navigation/{LIEKF.h => InvariantEKF.h} | 0 gtsam/navigation/tests/{testLIEKF.cpp => testInvariantEKF.cpp} | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename examples/{LIEKF_NavstateExample.cpp => IEKF_NavstateExample.cpp} (100%) rename examples/{LIEKF_Rot3Example.cpp => IEKF_Rot3Example.cpp} (100%) rename examples/{LIEKF_SE2_SimpleGPSExample.cpp => IEKF_SE2Example.cpp} (100%) rename gtsam/navigation/{LIEKF.h => InvariantEKF.h} (100%) rename gtsam/navigation/tests/{testLIEKF.cpp => testInvariantEKF.cpp} (100%) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/IEKF_NavstateExample.cpp similarity index 100% rename from examples/LIEKF_NavstateExample.cpp rename to examples/IEKF_NavstateExample.cpp diff --git a/examples/LIEKF_Rot3Example.cpp b/examples/IEKF_Rot3Example.cpp similarity index 100% rename from examples/LIEKF_Rot3Example.cpp rename to examples/IEKF_Rot3Example.cpp diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/IEKF_SE2Example.cpp similarity index 100% rename from examples/LIEKF_SE2_SimpleGPSExample.cpp rename to examples/IEKF_SE2Example.cpp diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/InvariantEKF.h similarity index 100% rename from gtsam/navigation/LIEKF.h rename to gtsam/navigation/InvariantEKF.h diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp similarity index 100% rename from gtsam/navigation/tests/testLIEKF.cpp rename to gtsam/navigation/tests/testInvariantEKF.cpp From 51e89d298e3e94cac02fa7804be20b05fc40c02a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:11:30 -0400 Subject: [PATCH 27/30] Fix compilation --- examples/IEKF_NavstateExample.cpp | 8 +++---- examples/IEKF_Rot3Example.cpp | 9 ++++---- examples/IEKF_SE2Example.cpp | 25 +++++++++++---------- gtsam/navigation/InvariantEKF.h | 16 ++++++------- gtsam/navigation/tests/testInvariantEKF.cpp | 16 ++++++------- 5 files changed, 37 insertions(+), 37 deletions(-) diff --git a/examples/IEKF_NavstateExample.cpp b/examples/IEKF_NavstateExample.cpp index a32d367b2..ef3ef204b 100644 --- a/examples/IEKF_NavstateExample.cpp +++ b/examples/IEKF_NavstateExample.cpp @@ -10,15 +10,15 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF_NavstateExample.cpp - * @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update) + * @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 #include @@ -53,7 +53,7 @@ int main() { // Initial state & covariances NavState X0; // R=I, v=0, t=0 Matrix9 P0 = Matrix9::Identity() * 0.1; - LIEKF ekf(X0, P0); + InvariantEKF ekf(X0, P0); // Noise & timestep double dt = 1.0; diff --git a/examples/IEKF_Rot3Example.cpp b/examples/IEKF_Rot3Example.cpp index 172af894d..88989c67e 100644 --- a/examples/IEKF_Rot3Example.cpp +++ b/examples/IEKF_Rot3Example.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF_Rot3Example.cpp + * @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 @@ -20,11 +20,10 @@ #include #include #include -#include +#include #include - using namespace std; using namespace gtsam; @@ -39,7 +38,7 @@ Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) { D_phi.row(2).setZero(); if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR - return -k * phi; // xi ∈ 𝔰𝔬(3) + return -k * phi; // xi ∈ 𝔰𝔬(3) } // --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× --- @@ -54,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; - LIEKF ekf(R0, P0); + InvariantEKF ekf(R0, P0); // Timestep, process noise, measurement noise double dt = 0.1; diff --git a/examples/IEKF_SE2Example.cpp b/examples/IEKF_SE2Example.cpp index 0985d2372..1a7772c7c 100644 --- a/examples/IEKF_SE2Example.cpp +++ b/examples/IEKF_SE2Example.cpp @@ -10,17 +10,17 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF_SE2_SimpleGPSExample.cpp + * @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 LIEKF class to estimate the state of - * an object using odometry / GPS measurements The prediction stage of the LIEKF - * uses a Lie Group element to propagate the stage in a discrete LIEKF. 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 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 @@ -32,7 +32,7 @@ * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #include -#include +#include #include @@ -45,8 +45,9 @@ using namespace gtsam; Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) { return X.translation(H); } -// Define a LIEKF class that uses the Pose2 Lie group as the state and the -// Vector2 measurement type. + +// 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); @@ -54,7 +55,7 @@ int main() { // Create the filter with the initial state, covariance, and measurement // function. - LIEKF ekf(X0, P0); + 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(); diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 9b4b6d2b5..443f730c4 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF.h - * @brief Left-Invariant Extended Kalman Filter (LIEKF) implementation + * @file InvariantEKF.h + * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) implementation * - * This file defines the LIEKF class template for performing prediction and + * 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. @@ -34,8 +34,8 @@ namespace gtsam { /** - * @class LIEKF - * @brief Left-Invariant Extended Kalman Filter (LIEKF) on a Lie group G + * @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 @@ -50,7 +50,7 @@ namespace gtsam { * using the left-invariant error in the tangent space. */ template -class LIEKF { +class InvariantEKF { public: /// Tangent-space dimension static constexpr int n = traits::dimension; @@ -59,7 +59,7 @@ class LIEKF { using MatrixN = Eigen::Matrix; /// Constructor: initialize with state and covariance - LIEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} + InvariantEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} /// @return current state estimate const G& state() const { return X_; } @@ -229,6 +229,6 @@ class LIEKF { // Define static identity I_n template -const typename LIEKF::MatrixN LIEKF::I_n = LIEKF::MatrixN::Identity(); +const typename InvariantEKF::MatrixN InvariantEKF::I_n = InvariantEKF::MatrixN::Identity(); } // namespace gtsam diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 5577a4357..c3f93d429 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -8,8 +8,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testLIEKFNavState.cpp - * @brief Unit test for the NavState dynamics Jacobian in LIEKF example. + * @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 */ @@ -18,12 +18,12 @@ #include #include #include -#include +#include #include using namespace gtsam; -// Duplicate the dynamics function in LIEKF_Rot3Example +// Duplicate the dynamics function in InvariantEKF_Rot3Example namespace exampleSO3 { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { @@ -64,12 +64,12 @@ TEST(IEKF, PredictNumericState) { // Analytic Jacobian Matrix3 actualH; - LIEKF iekf0(R0, P0); + InvariantEKF iekf0(R0, P0); iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH); // wrap predict into a state->state functor (mapping on SO(3)) auto g = [&](const Rot3& R) -> Rot3 { - LIEKF iekf(R, P0); + InvariantEKF iekf(R, P0); return iekf.predictMean(exampleSO3::dynamics, dt, Q); }; @@ -94,12 +94,12 @@ TEST(IEKF, StateAndControl) { // Analytic Jacobian Matrix3 actualH; - LIEKF iekf0(R0, P0); + InvariantEKF iekf0(R0, P0); iekf0.predictMean(f, dummy_u, dt, Q, actualH); // wrap predict into a state->state functor (mapping on SO(3)) auto g = [&](const Rot3& R) -> Rot3 { - LIEKF iekf(R, P0); + InvariantEKF iekf(R, P0); return iekf.predictMean(f, dummy_u, dt, Q); }; From 4cc2e22b59515767f79d41934c99749116521fa0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Apr 2025 00:20:04 -0400 Subject: [PATCH 28/30] Add value to concepts --- gtsam/base/Lie.h | 6 +++++- gtsam/base/Manifold.h | 5 ++++- 2 files changed, 9 insertions(+), 2 deletions(-) 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); From 3bcc5563eb6b1f639ba3031096023ce07ab90d94 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Apr 2025 00:20:41 -0400 Subject: [PATCH 29/30] 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 From ec8c762bb982d3f4ddece2151d0736b4b3dfe78d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 7 May 2025 09:55:45 -0400 Subject: [PATCH 30/30] Renamed GroupEKF -> LieGroupEKF, moved invariant predicts --- examples/GEKF_Rot3Example.cpp | 4 +- gtsam/navigation/InvariantEKF.h | 34 ++++++----- .../navigation/{GroupEKF.h => LieGroupEKF.h} | 57 +++++-------------- .../{testGroupEKF.cpp => testLieGroupEKF.cpp} | 14 ++--- 4 files changed, 41 insertions(+), 68 deletions(-) rename gtsam/navigation/{GroupEKF.h => LieGroupEKF.h} (77%) rename gtsam/navigation/tests/{testGroupEKF.cpp => testLieGroupEKF.cpp} (90%) diff --git a/examples/GEKF_Rot3Example.cpp b/examples/GEKF_Rot3Example.cpp index d931b7909..d81e01d54 100644 --- a/examples/GEKF_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; - GroupEKF ekf(R0, P0); + LieGroupEKF ekf(R0, P0); // Timestep, process noise, measurement noise double dt = 0.1; diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 1d39981d7..82a3f8fec 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -13,10 +13,10 @@ * @file InvariantEKF.h * @brief Left-Invariant Extended Kalman Filter implementation. * - * This file defines the InvariantEKF class template, inheriting from GroupEKF, + * 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 GroupEKF. + * motion models), hiding the state-dependent prediction variants from LieGroupEKF. * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert @@ -24,7 +24,7 @@ #pragma once -#include // Include the base class +#include // Include the base class namespace gtsam { @@ -34,50 +34,54 @@ namespace gtsam { * * @tparam G Lie group type (must satisfy LieGroup concept). * - * This filter inherits from GroupEKF but restricts the prediction interface + * 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 GroupEKF are hidden. - * The update step remains the same as in ManifoldEKF/GroupEKF. + * The state-dependent prediction methods from LieGroupEKF are hidden. + * The update step remains the same as in ManifoldEKF/LieGroupEKF. */ template - class InvariantEKF : public GroupEKF { + class InvariantEKF : public LieGroupEKF { public: - using Base = GroupEKF; ///< Base class type + 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 GroupEKF constructor + /// Constructor: forwards to LieGroupEKF constructor InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {} - // --- Expose only the Invariant Prediction Methods --- - /** * 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. + * 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) { - Base::predict(U, 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). Calls the base class implementation. + * 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) { - Base::predict(u, dt, Q); + G U = traits::Expmap(u * dt); + predict(U, Q); // Call the group composition predict } }; // InvariantEKF diff --git a/gtsam/navigation/GroupEKF.h b/gtsam/navigation/LieGroupEKF.h similarity index 77% rename from gtsam/navigation/GroupEKF.h rename to gtsam/navigation/LieGroupEKF.h index f2f2990e6..fe16eaefe 100644 --- a/gtsam/navigation/GroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -10,13 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file GroupEKF.h + * @file LieGroupEKF.h * @brief Extended Kalman Filter derived class for Lie groups G. * - * This file defines the GroupEKF class template, inheriting from ManifoldEKF, + * 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 utilizing group composition, tangent space - * controls (via exponential map), and state-dependent dynamics functions. + * 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 @@ -34,18 +34,18 @@ namespace gtsam { /** - * @class GroupEKF + * @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 convenient - * prediction methods based on group composition or dynamics functions defining - * motion in the tangent space. + * 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 GroupEKF : public ManifoldEKF { + class LieGroupEKF : public ManifoldEKF { public: using Base = ManifoldEKF; ///< Base class type static constexpr int n = Base::n; ///< Group dimension (tangent space dimension) @@ -54,41 +54,10 @@ namespace gtsam { 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) { + LieGroupEKF(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) @@ -142,8 +111,8 @@ namespace gtsam { 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 + this->X_ = predictMean(std::forward(f), dt, A); + this->P_ = A * this->P_ * A.transpose() + Q; } /** @@ -191,6 +160,6 @@ namespace gtsam { return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); } - }; // GroupEKF + }; // LieGroupEKF } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp similarity index 90% rename from gtsam/navigation/tests/testGroupEKF.cpp rename to gtsam/navigation/tests/testLieGroupEKF.cpp index 5ce7c477b..62a35473d 100644 --- a/gtsam/navigation/tests/testGroupEKF.cpp +++ b/gtsam/navigation/tests/testLieGroupEKF.cpp @@ -9,7 +9,7 @@ /** * @file testGroupEKF.cpp - * @brief Unit test for GroupEKF, as well as dynamics used in Rot3 example. + * @brief Unit test for LieGroupEKF, as well as dynamics used in Rot3 example. * @date April 26, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ @@ -18,12 +18,12 @@ #include #include #include -#include +#include #include using namespace gtsam; -// Duplicate the dynamics function in GroupEKF_Rot3Example +// Duplicate the dynamics function in GEKF_Rot3Example namespace exampleSO3 { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { @@ -63,12 +63,12 @@ TEST(GroupeEKF, PredictNumericState) { // Analytic Jacobian Matrix3 actualH; - GroupEKF ekf0(R0, P0); + 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 { - GroupEKF ekf(R, P0); + LieGroupEKF ekf(R, P0); return ekf.predictMean(exampleSO3::dynamics, dt); }; @@ -93,12 +93,12 @@ TEST(GroupeEKF, StateAndControl) { // Analytic Jacobian Matrix3 actualH; - GroupEKF ekf0(R0, P0); + 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 { - GroupEKF ekf(R, P0); + LieGroupEKF ekf(R, P0); return ekf.predictMean(f, dummy_u, dt, Q); };