Gal3ImuEKF

Overview¶

The Gal3ImuEKF is a left-invariant Lie group EKF for the Gal3 group, which represents a kinematic state (R, p, v, t). It integrates IMU measurements to predict motion and supports generic measurement updates. This notebook provides a guide to its usage, covering:

The Gal3 state and the EKF’s modes of operation.
How to initialize and use the EKF.
The IMU prediction step and covariance propagation.
How to perform measurement updates.

For background on the Gal3 group itself, see the Gal3 documentation and the associated Jupyter notebook.

import numpy as np

import gtsam
from gtsam import Gal3, Rot3, Pose3

The Gal3 State and EKF Modes¶

A Gal3 state X = (R, p, v, t) contains:

R: attitude (rotation) in SO(3)
p: position in R³
v: velocity in R³
t: time in R

The Gal3ImuEKF has three modes of operation, defined by the Gal3ImuEKF.Mode enum:

NO_TIME: The EKF state remains in the NavState subgroup, and time is not tracked (t is always 0). This is analogous to a standard NavState-based filter.
TRACK_TIME_NO_COVARIANCE: Time is tracked (t increases with dt), but its uncertainty is not included in the covariance matrix. This is the default mode.
TRACK_TIME_WITH_COVARIANCE: Time is tracked, and its variance is included in the 10x10 covariance matrix. This makes the filter non-invariant, as the state transition depends on the current time t_k.

The tangent space (local coordinates) for Gal3 is a 10D vector [δθ, δv, δp, δt].

API Overview¶

Class: gtsam.Gal3ImuEKF(X0, P0, params, mode)

Constructor: Takes an initial state X0: Gal3, covariance P0: 9x9 or 10x10, PreintegrationParams, and an optional mode.
Accessors: state() -> Gal3, covariance() -> numpy array.
Predict: predict(omega, accel, dt) integrates IMU measurements over dt and propagates the covariance.
Update: Inherits from InvariantEKF, providing methods like update(measurement, H, R) for measurement updates.

# EKF parameters: gravity and IMU noise
params = gtsam.PreintegrationParams.MakeSharedD(9.81)  # gravity (m/s^2)
params.setAccelerometerCovariance(np.diag([1e-3, 1e-3, 1e-3]))
params.setGyroscopeCovariance(np.diag([1e-4, 1e-4, 1e-4]))
# Note: Gal3ImuEKF also uses integrationCovariance for the IMU velocity integral
params.setIntegrationCovariance(np.diag([1e-5, 1e-5, 1e-5]))


# Initial state and covariance
X0 = Gal3() # Identity Gal3 state (R=I, p=0, v=0, t=0)
P0 : np.ndarray = np.eye(9) * 0.1 # Initial covariance for (R, v, p)

# Create EKF in default mode (TRACK_TIME_NO_COVARIANCE)
# The covariance matrix will be 9x9
ekf = gtsam.Gal3ImuEKF(X0, P0, params)
print("Initialized EKF:", ekf.state())
print("Initial covariance shape:", ekf.covariance().shape)

# Create EKF that tracks time covariance
P0 = np.eye(10) * 0.1
ekf = gtsam.Gal3ImuEKF(X0, P0, params, gtsam.Gal3ImuEKF.Mode.TRACK_TIME_WITH_COVARIANCE)
print("\nInitialized 10D EKF:", ekf.state())
print("Initial 10D covariance shape:", ekf.covariance().shape)

Initialized EKF: R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
r: 0 0 0
v: 0 0 0
t: 0

Initial covariance shape: (10, 10)

Initialized 10D EKF: R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
r: 0 0 0
v: 0 0 0
t: 0

Initial 10D covariance shape: (10, 10)

Predict: Integrate IMU¶

The predict(omega, accel, dt) method advances the state and propagates covariance. The dynamics are modeled as X_{k+1} = W * X_k * U, where W is a left-multipled gravity term and U is a right-multiplied IMU increment.

omega: body angular velocity (rad/s).
accel: specific force (m/s²), in the body frame.
dt: timestep (s).

The IMU increment U is calculated by Gal3.Expmap of the IMU measurements, providing an exact integration for piecewise-constant measurements. The gravity term W depends on the chosen mode to handle time correctly.

# Example predict with constant yaw rate and gravity-only accel
omega_b = np.array([0.0, 0.0, -0.1])  # rad/s
f_b = np.array([0.0, 0.0, -9.81])  # m/s^2 (specific force)
dt = 0.01  # s

print("Default EKF (9x9 covariance):")
ekf.predict(omega_b, f_b, dt)
print("After predict, state:", ekf.state())
print("Covariance diag:", np.sqrt(np.diag(ekf.covariance())))

print("\n10D EKF (tracks time covariance):")
ekf.predict(omega_b, f_b, dt)
print("After predict, state:", ekf.state())
print("Covariance diag:", np.sqrt(np.diag(ekf.covariance())))

Default EKF (9x9 covariance):
After predict, state: R: [
	1, 0.001, 0;
	-0.001, 1, 0;
	0, 0, 1
]
r: 0 0 0
v: 0 0 0
t: 0.01

Covariance diag: [0.31622935 0.31622935 0.31622935 0.31776148 0.31776148 0.31624358
 0.31624377 0.31624377 0.31624374 0.31622777]

10D EKF (tracks time covariance):
After predict, state: R: [
	0.999998, 0.002, 0;
	-0.002, 0.999998, 0;
	0, 0, 1
]
r: 0 0 0
v: 0 0 0
t: 0.02

Covariance diag: [0.31623093 0.31623093 0.31623093 0.32228784 0.32228784 0.31625939
 0.31629193 0.31629193 0.31629132 0.31622777]

Update: Incorporating Measurements¶

The Gal3ImuEKF inherits its update method from InvariantEKF. The generic signature is update(measurement, H, R), where H is the measurement Jacobian with respect to the EKF’s local coordinates.

For a world position measurement z ≈ p_world:

The error is z - state.position().
The Jacobian H maps the 10D tangent vector [δθ, δv, δp, δt] to the measurement space. For a position measurement, this is H = [0, 0, I, 0], a 3x10 matrix.

# World-position update example for the 10D EKF
X_pred = ekf.state()
p_pred = X_pred.position()

# Jacobian for a world position measurement (w.r.t. 10D tangent space)
H = np.zeros((3, 10))
H[:, 6:9] = np.eye(3)

# Fake measurement with some noise
z = p_pred + np.array([0.1, -0.05, 0.0])
R_meas = np.eye(3) * 0.5  # measurement covariance (m^2)

# Create a measurement object
# The error is computed inside the update as (z - h(X)) = z - X.position()

ekf.updateWithVector(p_pred, H, z, R_meas)
print("After position update, state:", ekf.state())
print("Covariance diag:", np.sqrt(np.diag(ekf.covariance())))

After position update, state: R: [
	0.999998, 0.002, -3.26651e-05;
	-0.002, 0.999998, 1.64143e-05;
	3.26979e-05, -1.63489e-05, 1
]
r:   0.0166556 -0.00836948 3.40718e-07
v:  0.000339402 -0.000170551  6.94304e-09
t: 0.02

Covariance diag: [0.31623083 0.31623083 0.31623093 0.3222771  0.3222771  0.31624885
 0.28872394 0.28872394 0.28872348 0.31622777]

Full examples and plotting¶

For a step-by-step tutorial with scenarios, plots, and uncertainty bands, see:

Gal3 IMU EKF Tutorial: ../../../python/gtsam/examples/navigation/Gal3ImuExample.ipynb