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); };