NavStateImuEKF

Overview¶

The NavStateImuEKF is a left-invariant Lie group EKF for GTSAM’s NavState X = (R, p, v). It integrates IMU to predict motion and supports generic measurements (e.g., position). Increments for p and v are in the body frame, consistent with GTSAM’s conventions.

This user guide covers:

The EKF state and local coordinates [δθ, δp_body, δv_body].
Predict from IMU and covariance propagation at each step.
Adding a world-position measurement with the correct Jacobian H = [0, R, 0].
Visualizing results with ±2σ uncertainty bands and basic tuning.

See also: NavState IMU EKF Tutorial for a longer, step-by-step walkthrough.

import numpy as np

import gtsam
from gtsam import NavState, Rot3, Pose3

State and local coordinates¶

State: X = (R, p, v) with rotation R ∈ SO(3), position p ∈ R³, velocity v ∈ R³.
Local coordinates: [δθ, δp_body, δv_body]. Increments for position/velocity are in the body frame.
Position measurement (world): z ≈ p_world has Jacobian H = [0, R, 0] in these local coordinates.

API overview¶

Class: gtsam.NavStateImuEKF(X0, P0, params)

Constructor: initial state X0: NavState, covariance P0: 9x9, and PreintegrationParams (gravity and IMU covariances).
Accessors: state() -> NavState, covariance() -> 9x9 numpy array.
Predict: predict(omega, accel, dt) integrates IMU over dt and propagates covariance.
Update (Python): updateWithVector(prediction, H, measurement, R) applies a linear update with measurement vector and Jacobian H in EKF local coordinates.
- Use this to mimic measurement-function-based updates by building H and z for your sensor.

# Example of modeling an IMU using NED coordinates as the navigation frame 
# with sensor measurements in the body/sensor frame
params = gtsam.PreintegrationParams.MakeSharedD(9.81)  # gravity (m/s^2)
params.setAccelerometerCovariance(np.diag([1e-3, 1e-3, 1e-3]))
params.setIntegrationCovariance(np.diag([1e-3, 1e-3, 1e-3]))
params.setGyroscopeCovariance(np.diag([1e-4, 1e-4, 1e-4]))

# Initial state and covariance, FRD frame aligned with NED (i.e., looking north)
X0 = NavState(Pose3(), np.zeros(3))
P0 = np.eye(9) * 0.1

ekf = gtsam.NavStateImuEKF(X0, P0, params)
print("Initialized EKF:", ekf.state())

Initialized EKF: R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
p: 0 0 0
v: 0 0 0

Predict: integrate IMU¶

Signature: predict(omega, accel, dt)

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

Effect: builds a NavState increment from (omega, accel, dt), composes it onto the state, and updates covariance using a linearized transition. Process noise is scaled by dt inside the filter.

# 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 (static)
dt = 0.01  # s
ekf.predict(omega_b, f_b, dt)
print("After predict, state:", ekf.state())
print("Covariance diag:", np.sqrt(np.diag(ekf.covariance())))

After predict, state: R: [
	1, 0.001, 0;
	-0.001, 1, 0;
	0, 0, 1
]
p: 0 0 0
v: 0 0 0

Covariance diag: [0.31622935 0.31622935 0.31622935 0.31625943 0.31625943 0.31625939
 0.31776148 0.31776148 0.31624358]

Update: using `updateWithVector` (Python)¶

Signature: updateWithVector(prediction, H, measurement, R)

prediction: the measurement predicted by the current state (e.g., current world position p).
H: measurement Jacobian w.r.t. EKF local coordinates [δθ, δp_body, δv_body].
measurement: the actual measurement vector (same shape as prediction).
R: measurement covariance matrix.

Position example (world position z ≈ p_world):

Prediction: p_pred = ekf.state().position()
Jacobian: H = [0, R, 0] where R = ekf.state().attitude().matrix() and H has shape (3,9).

# One-shot world-position update example
X_pred = ekf.state()
p_pred = X_pred.position()  # shape (3,)
R_state = X_pred.attitude().matrix()  # 3x3
H = np.zeros((3, 9))
H[:, 3:6] = R_state

# Suppose we measured a world position (e.g., GPS)
z = p_pred + np.array([0.1, -0.05, 0.0])  # fake measurement a bit away
R_meas = np.eye(3) * 0.5  # covariance (m^2)

ekf.updateWithVector(p_pred, H, z, R_meas)
print("After position update, state:", ekf.state())

After position update, state: R: [
	0.999999, 0.001, -8.17473e-06;
	-0.001, 0.999999, 4.08736e-06;
	8.17881e-06, -4.07919e-06, 1
]
p:   0.0166694 -0.00833472           0
v:  0.000167463 -8.37315e-05            0

Full examples and plotting¶

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

NavState IMU EKF Tutorial: ../../../python/gtsam/examples/navigation/NavStateImuExample.ipynb

Tips for other sensors¶

Any measurement can be used by providing its prediction and Jacobian H in the EKF’s local coordinates.
Examples:
- Body-frame velocity: H typically selects the δv_body block.
- Orientation (e.g., magnetometer yaw): derive H w.r.t. δθ.
Always ensure units/frames are consistent (world vs. body) when building prediction and H.