InvariantEKF User Guide

This short guide summarizes the InvariantEKF API (and the related LieGroupEKF) and points to concrete examples in the repository.

What is the InvariantEKF?¶

A left-invariant EKF on a Lie group G.
State transition can be given by right multiplication (predict(U, Q)) or by a left–right pair (predict(W, U, Q)) that executes X_{k+1} = W · X_k · U.

Python usage sketch¶

import numpy as np
import gtsam

# State type
G = gtsam.Pose2

# Initialize filter
X0 = G.Identity()
P0 = np.eye(3) * 0.1
ekf = gtsam.InvariantEKFPose2(X0, P0)

# Right-increment predict with discrete Q
U = G(1.0, 0.0, 0.1)
Qd = np.eye(3) * 0.01
ekf.predict(U, Qd)

# Left–right predict: X <- W * X * U
W = G(0.1, 0.0, 0.0)
ekf.predict(W, U, Qd)

# In python, update using ManifoldEKF updateWithVector
X = ekf.state()
prediction = np.array([X.x(), X.y()])
H = np.array([[1, 0, 0], [0, 1, 0]])
z = np.array([1.0, 0.0])
R = np.eye(2) * 0.01
ekf.updateWithVector(prediction, H, z, R)

Usage sketch (C++)¶

using namespace gtsam;
using G = Pose2;
Matrix3 P0 = I_3x3 * 0.1;
InvariantEKF<G> ekf(G(), P0);

G U(1.0, 0.0, 0.1);
Matrix3 Qd = I_3x3 * 0.01;
ekf.predict(U, Qd);            // right increment
G W(0.1, 0.0, 0.0);
ekf.predict(W, U, Qd);         // left–right increment

auto h = [](const G& X, OptionalJacobian<2,3> H) {
    if (H) *H << 1, 0, 0, 0, 1, 0;
    return Vector2(X.x(), X.y());
};
Vector2 z; z << 1.0, 0.0;
Matrix2 R = Matrix2::Identity() * 0.01;
ekf.update(h, z, R);

Notes¶

predict(W, U, Q) treats Q as discrete (no dt).
predict(u, dt, Q) uses continuous-time Q and scales by dt internally.
IMU-specific EKFs (NavStateImuEKF, Gal3ImuEKF) reuse these interfaces; see the notebooks above for end-to-end flows.

Where it is used today¶

C++ tests: gtsam/navigation/tests/testInvariantEKF.cpp exercises predict(U, Q) and the dt overload.
C++ examples: examples/IEKF_NavstateExample.cpp (NavState), examples/GEKF_Rot3Example.cpp (LieGroupEKF on SO(3)).
Docs: gtsam/navigation/doc/InvariantEKF.md (math/background).
Python notebooks with IMU-specialized EKFs built on the same interface:

If you add new examples that use predict(W, U, Q), please link them here for discoverability.