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.
Source
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)))Source
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()Loading...
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.
Source
# 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()Loading...
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.
Source
# 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()Loading...
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.
Source
# 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()Loading...