NavState IMU EKF Tutorial

This tutorial shows how to use GTSAM’s NavState-specific EKF (NavStateImuEKF) with synthetic IMU data and occasional position measurements. You’ll learn:

How the predict step integrates IMU into a NavState increment and propagates covariance.
How to write a position measurement in the EKF’s local coordinates (the correct H = [0, R, 0]).
How to interpret the results with ±2σ uncertainty bands for yaw/pitch/roll, velocity, and position.

We simulate a simple motion and run the EKF end-to-end, then visualize estimates vs. ground truth along with uncertainty.

import numpy as np
import plotly.graph_objects as go
import pandas as pd

import gtsam
from gtsam import NavState, Rot3
from gtsam import ConstantTwistScenario, ScenarioRunner

# Helper to display yaw/pitch/roll in degrees
def rot3_ypr_rad(R: Rot3):
    """Return yaw, pitch, roll (degrees) for a Rot3."""
    return np.degrees(R.ypr())

Background and setup¶

State: NavState X = (R, p, v) with rotation R ∈ SO(3), position p ∈ R^3, velocity v ∈ R^3.

EKF local coordinates are [δθ, δp_body, δv_body], i.e., position/velocity increments are expressed in the body frame.
For a world-position measurement z ≈ p_world, the linearized measurement Jacobian is H = [0, R, 0].

We’ll now define a simple scenario that provides ground truth and noise-free IMU to drive the predict step.

# --- Scenario: camera orbiting a fixed point ---
radius = 30.0
angular_velocity = np.pi  # rad/sec (half-turn per second)
w_b = np.array([0, 0, angular_velocity])  # body yaw rate
v_n = np.array([radius * angular_velocity, 0, 0])  # world-frame velocity

scenario = ConstantTwistScenario(w_b, v_n)

# Simulation parameters
dt = 1.0 / 180.0  # 1 degree per step
T = 4.0           # total duration (s)
N = int(T / dt)
params = gtsam.PreintegrationParams.MakeSharedD(9.81)  # gravity in m/s^2

# IMU covariances (tune as needed)
params.setAccelerometerCovariance(np.diag([1e-3] * 3))
params.setIntegrationCovariance(np.diag([1e-3] * 3))  # integration noise
params.setGyroscopeCovariance(np.diag([1e-4] * 3))

runner = ScenarioRunner(scenario, params, dt,
                         gtsam.imuBias.ConstantBias(np.zeros(3), np.zeros(3)))

times=np.linspace(0.0,T,N+1); states=[scenario.navState(t) for t in times]; positions=np.array([s.position() for s in states]); rotations=[s.attitude() for s in states]
fig=go.Figure(); fig.add_trace(go.Scatter(x=positions[:,0],y=positions[:,1],mode='markers+lines',name='Scenario Trajectory',line=dict(width=2,color='black'),marker=dict(size=4,color=times[:positions.shape[0]],colorscale='Gray',colorbar=dict(title='Time (s)'))))
scale=6.0; arrow_skip=45
for i in range(0,len(positions),arrow_skip):
    R=rotations[i].matrix(); pos=positions[i,:2]; f=R[:2,0]; r=R[:2,1]
    fig.add_annotation(x=pos[0]+scale*f[0],y=pos[1]+scale*f[1],ax=pos[0],ay=pos[1],xref='x',yref='y',axref='x',ayref='y',showarrow=True,arrowhead=2,arrowsize=1,arrowwidth=1.5,arrowcolor='red',standoff=0,startstandoff=0)
    fig.add_annotation(x=pos[0]+scale*r[0],y=pos[1]+scale*r[1],ax=pos[0],ay=pos[1],xref='x',yref='y',axref='x',ayref='y',showarrow=True,arrowhead=2,arrowsize=1,arrowwidth=1.5,arrowcolor='green',standoff=0,startstandoff=0)
fig.update_layout(title='Scenario Trajectory (Top View: X vs Y, colored by time)',xaxis_title='X (m)',yaxis_title='Y (m)',margin=dict(l=0,r=0,b=0,t=40),yaxis=dict(scaleanchor='x',scaleratio=1)); fig.show()

Simulate IMU and run the EKF¶

We integrate IMU in the predict step and occasionally update with a position measurement. The position Jacobian must be H = [0, R, 0] because EKF local coordinates use body-frame increments for p and v.

# Initialize EKF with ground-truth state and modest covariance
X0: NavState = scenario.navState(0.0)
P0 = np.eye(9) * 0.1  # std ~ 0.316 on each component
ekf = gtsam.NavStateImuEKF(X0, P0, params)

times = np.linspace(0.0, T, N + 1)

# Storage for plotting
ypr_true, ypr_est = [], []
vel_true, vel_est = [], []
pos_true, pos_est = [], []

# Std-dev storage from EKF covariance (for ±2σ bands)
rot_std_deg_list = []  # yaw/pitch/roll std in degrees
pos_std_list = []      # x/y/z std in meters
vel_std_list = []      # vx/vy/vz std in m/s

# Simulate and filter
Xk = X0
for k, t in enumerate(times):
    # Ground truth state from scenario
    X_true: NavState = scenario.navState(t)
    ypr_true.append(rot3_ypr_rad(X_true.attitude()))
    vel_true.append(np.asarray(X_true.velocity()))
    pos_true.append(np.asarray(X_true.position()))

    if k == 0:
        # Record initial estimate and covariance
        X_est = ekf.state()
        ypr_est.append(rot3_ypr_rad(X_est.attitude()))
        vel_est.append(np.asarray(X_est.velocity()))
        pos_est.append(np.asarray(X_est.position()))
        P = ekf.covariance()
        std = np.sqrt(np.diag(P))
        rot_std_deg_list.append(np.rad2deg(std[0:3]))
        pos_std_list.append(std[3:6])
        vel_std_list.append(std[6:9])
        continue

    # Predict using noise-free IMU from scenario (at interval start)
    omega_meas = runner.actualAngularVelocity(t - dt)
    acc_meas = runner.actualSpecificForce(t - dt)
    ekf.predict(omega_meas, acc_meas, dt)

    # Log estimate after predict
    X_est = ekf.state()
    ypr_est.append(rot3_ypr_rad(X_est.attitude()))
    vel_est.append(np.asarray(X_est.velocity()))
    pos_est.append(np.asarray(X_est.position()))

    # Log current covariance std-devs
    P = ekf.covariance()  # post-predict, pre-update
    std = np.sqrt(np.diag(P))
    rot_std_deg_list.append(np.rad2deg(std[0:3]))
    pos_std_list.append(std[3:6])
    vel_std_list.append(std[6:9])

    # Position measurement every M steps
    M = 30  # measurement cadence
    if k % M == 0:
        predicted_position = X_est.position()
        measured_position = X_true.position()
        R = np.eye(3) * 1.0  # measurement covariance (tune as needed)

        # Measurement Jacobian in EKF local coords: H = [0, R, 0]
        H = np.zeros((3, 9))
        H[:, 3:6] = X_est.attitude().matrix()
        ekf.updateWithVector(predicted_position, H, measured_position, R)

# Convert to arrays for plotting
ypr_true = np.unwrap(np.vstack(ypr_true), axis=0)
ypr_est = np.unwrap(np.vstack(ypr_est), axis=0)
vel_true = np.vstack(vel_true)
vel_est = np.vstack(vel_est)
pos_true = np.vstack(pos_true)
pos_est = np.vstack(pos_est)

rot_std_deg = np.vstack(rot_std_deg_list)
pos_std = np.vstack(pos_std_list)
vel_std = np.vstack(vel_std_list)

Plot Yaw/Pitch/Roll (degrees)¶

We show estimated angles with ±2σ bands and dashed ground truth. Ideally, the truth lies mostly within the shaded bands if covariances are well tuned.

# YPR (deg) with ±2σ bands, RGB colors, dashed GT
fig = go.Figure(); color_rgb = {0: ('rgba(255,0,0,0.15)', '#ff0000'), 1: ('rgba(0,128,0,0.15)', '#008000'), 2: ('rgba(0,0,255,0.15)', '#0000ff')}; names = {0: 'Yaw', 1: 'Pitch', 2: 'Roll'}
for i in range(3):
    fill_rgba, line_color = color_rgb[i]; mean = ypr_est[:, i]; std2p = 2.0 * rot_std_deg[:, i]; upper = mean + std2p; lower = mean - std2p
    fig.add_scatter(x=times, y=upper, line=dict(color=line_color, width=0), showlegend=False)
    fig.add_scatter(x=times, y=lower, fill='tonexty', fillcolor=fill_rgba, line=dict(color=line_color, width=0), showlegend=False)
    fig.add_scatter(x=times, y=mean, name=f"{names[i]} est", line=dict(color=line_color, width=2))
    fig.add_scatter(x=times, y=ypr_true[:, i], name=f"{names[i]} true", line=dict(color=line_color, dash='dash', width=2))
fig.update_layout(title='YPR (deg) with ±2σ bands', xaxis_title='Time (s)', yaxis_title='Degrees'); fig.show()

Plot Velocity (m/s)¶

Velocity often benefits from frequent updates (e.g., GPS Doppler). Here we only have occasional position updates; expect velocity uncertainty to grow between updates and shrink slightly after them.

# Velocity (m/s) with ±2σ bands
fig = go.Figure(); color_rgb = {0: ('rgba(255,0,0,0.15)', '#ff0000'), 1: ('rgba(0,128,0,0.15)', '#008000'), 2: ('rgba(0,0,255,0.15)', '#0000ff')}
for i, comp in enumerate(['x','y','z']):
    fill_rgba, line_color = color_rgb[i]; mean = vel_est[:, i]; std2p = 2.0 * vel_std[:, i]; upper = mean + std2p; lower = mean - std2p
    fig.add_scatter(x=times, y=upper, line=dict(color=line_color, width=0), showlegend=False)
    fig.add_scatter(x=times, y=lower, fill='tonexty', fillcolor=fill_rgba, line=dict(color=line_color, width=0), showlegend=False)
    fig.add_scatter(x=times, y=mean, name=f"v{comp} est", line=dict(color=line_color, width=2))
    fig.add_scatter(x=times, y=vel_true[:, i], name=f"v{comp} true", line=dict(color=line_color, dash='dash', width=2))
fig.update_layout(title='Velocity (m/s) with ±2σ bands', xaxis_title='Time (s)', yaxis_title='m/s'); fig.show()

Plot Position (m)¶

Position updates should pull the estimate toward truth and reduce the ±2σ band. If divergence occurs, check:

H matches EKF local coords ([0, R, 0] for world-position).
Measurement noise R reflects your sensor.
Process noise (IMU) is not overconfident.

# Position (m) with ±2σ bands
fig = go.Figure(); color_rgb = {0: ('rgba(255,0,0,0.15)', '#ff0000'), 1: ('rgba(0,128,0,0.15)', '#008000'), 2: ('rgba(0,0,255,0.15)', '#0000ff')}
for i, comp in enumerate(['x','y','z']):
    fill_rgba, line_color = color_rgb[i]; mean = pos_est[:, i]; std2p = 2.0 * pos_std[:, i]; upper = mean + std2p; lower = mean - std2p
    fig.add_scatter(x=times, y=upper, line=dict(color=line_color, width=0), showlegend=False)
    fig.add_scatter(x=times, y=lower, fill='tonexty', fillcolor=fill_rgba, line=dict(color=line_color, width=0), showlegend=False)
    fig.add_scatter(x=times, y=mean, name=f"p{comp} est", line=dict(color=line_color, width=2))
    fig.add_scatter(x=times, y=pos_true[:, i], name=f"p{comp} true", line=dict(color=line_color, dash='dash', width=2))
fig.update_layout(title='Position (m) with ±2σ bands', xaxis_title='Time (s)', yaxis_title='m'); fig.show()