Invariant EKF

[dellaert]

A Left Invariant IEKF on SE(2).
Changes by Frank: template methods rather than a base class
Replaces #2113

This introduced the following class structure:

graph TD; 
ManifoldEKF --> LieGroupEKF --> InvariantEKF

Loading

[Copilot]

Pull Request Overview

This pull request introduces a new templated LIEKF implementation along with multiple examples and tests demonstrating its use on different Lie groups (SE(2), SO(3), and NavState) while also deprecating unused Boost string utilities in an ISAM2 example.

• Introduces the LIEKF header implementing various prediction and update methods based on templated dynamics functions.
• Adds new examples demonstrating LIEKF usage for SE(2), Rot3, and NavState along with comprehensive test cases.
• Removes unused Boost includes from the ISAM2 example for cleaner dependencies.

Reviewed Changes

Copilot reviewed 6 out of 6 changed files in this pull request and generated no comments.

Show a summary per file

File	Description
gtsam/navigation/tests/testLIEKF.cpp	Adds tests for dynamics Jacobian computation.
gtsam/navigation/LIEKF.h	Implements a templated LIEKF class with multiple prediction/update methods.
examples/LIEKF_SE2_SimpleGPSExample.cpp	Demonstrates LIEKF usage on SE(2) with GPS measurements.
examples/LIEKF_Rot3Example.cpp	Provides an example of using LIEKF with SO(3) dynamics and magnetometer updates.
examples/LIEKF_NavstateExample.cpp	Presents LIEKF on NavState with IMU-based prediction and GPS updates.
examples/ISAM2_City10000.cpp	Removes unused Boost dependencies.

Comments suppressed due to low confidence (1)

examples/LIEKF_NavstateExample.cpp:49

• Ensure that the identifier Z_3x3 is defined or replace it with an explicit zero matrix (e.g., Matrix3::Zero()) to avoid potential compilation errors.

if (H) *H << Z_3x3, Z_3x3, X.R().matrix();

[dellaert]

Ci on windows fails because of Unicode characters. I’ll fix together with any other comments.

[scottiyio]

Looks great! I saw no errors, just some minor includes that may not be needed.

[dellaert]

@scottiyio I made the following changes:

• I added new unit-tests for state-dependent dynamics, with new method predictMean
• renamed to InvariantEKF
• reverted this to draft PR because the connection with invariant filtering is not clear enough yet

I would like to look at Potokar’s code how the dynamics and measurements there are specified in general.

[dellaert]

@scottiyio Gemini and I created the ManifoldEKF -> GroupEKF -> InvariantEKF hierarchy (plus tests)
Take a look.
GroupEKF or LieEKF or LieGroupEKF?

[Copilot]

Pull Request Overview

This PR implements a Left Invariant IEKF on SE(2) by replacing the previous base class design with template‐based methods. Key changes include:

• New and updated unit tests for ManifoldEKF, InvariantEKF, and GroupEKF.
• Refactoring and extended implementations in ManifoldEKF.h, InvariantEKF.h, and GroupEKF.h.
• Updates to examples demonstrating IEKF usage and removal of obsolete boost dependencies.

Reviewed Changes

Copilot reviewed 12 out of 12 changed files in this pull request and generated no comments.

Show a summary per file

File	Description
gtsam/navigation/tests/testManifoldEKF.cpp	Adds unit tests for the ManifoldEKF using Unit3 dynamics.
gtsam/navigation/tests/testInvariantEKF.cpp	Introduces IEKF tests on Pose2, following the left-invariant formulation.
gtsam/navigation/tests/testGroupEKF.cpp	Adds unit tests for the GroupEKF with SO(3) dynamics and Jacobian checks.
gtsam/navigation/ManifoldEKF.h	Implements the EKF on a generic manifold with retract and localCoordinates.
gtsam/navigation/InvariantEKF.h	Implements the Left-Invariant EKF by restricting prediction methods.
gtsam/navigation/GroupEKF.h	Provides EKF prediction methods specific to Lie groups via composition.
gtsam/base/Manifold.h & gtsam/base/Lie.h	Update concept markers for manifold and Lie group types.
examples/*	Updates example files to demonstrate usage of the new IEKF implementations and remove boost dependencies.

Comments suppressed due to low confidence (2)

examples/GEKF_Rot3Example.cpp:13

• The file header comment still refers to 'IEKF_Rot3Example.cpp' while the actual file name is 'GEKF_Rot3Example.cpp'. Please update the comment to match the file name for clarity.

 * @file IEKF_Rot3Example.cpp

gtsam/navigation/GroupEKF.h:74

• [nitpick] There is a TODO comment regarding 'traits::AdjointMap' on this line. If the implementation is complete and correct, consider removing or updating the comment to avoid confusion.

      Jacobian A = traits<G>::Inverse(U).AdjointMap(); // A = Adjoint(U.inverse())

[dellaert]

I am thinking of moving these to InvariantEKF and comment that, if the dynamics does not depend on the state, they should use an InvariantEKF

[scottiyio]

I know that InvariantEKF inherits from this.

I wish we could call it something like "StateDependentEKF" because you should use GroupEKF on state dependency, but considering InvariantEKF inherits from here as a special case I don't know if that makes sense.

Or maybe, it inherits that the "state dependency" is zero. That structure, to me, makes it more clear thaty you should use Invariant if you have no state dependency but Group if there is.

[dellaert]

I opted for

graph TD; 
ManifoldEKF --> LieGroupEKF --> InvariantEKF

Loading