This notebook is a compact introduction to legged state estimation in GTSAM using one staircase sequence and three estimators:
LeggedInvariantEKF: an invariant filter.LeggedInvariantIEKF: the same invariant state representation, but with a small factor-graph fragment for each contact update.LeggedFixedLagSmoother: a fixed-lag smoother with a single IMU bias state.
All three estimators see the same IMU stream and the same contact measurements. That makes the comparison easy to read: when the curves differ, the difference comes from the estimator design rather than from the data.
import numpy as np
import plotly.graph_objects as go
from plotly.subplots import make_subplots
from gtsam.examples.LeggedEstimatorExample import (
display_trajectory,
first_full_contact_time,
load_legged_csv_dataset,
make_contact_animation,
make_contact_side_animation,
replay_legged_variants,
)
DATASET = load_legged_csv_dataset()
RESULTS = replay_legged_variants(dataset=DATASET)
DISPLAY_START_TIME_S = first_full_contact_time(DATASET)
DISPLAY_RESULTS = {
variant: {
**result,
"trajectory": display_trajectory(result["trajectory"], DISPLAY_START_TIME_S),
}
for variant, result in RESULTS.items()
}
LABELS = {
"invariant_ekf": "Invariant filter",
"invariant_graph": "Invariant filter + graph fragment",
"fixed_lag_single_bias": "Fixed-lag smoother (single bias)",
}
COLORS = {
"invariant_ekf": "#1f77b4",
"invariant_graph": "#ff7f0e",
"fixed_lag_single_bias": "#2ca02c",
}
LINE_STYLES = {
"invariant_ekf": "solid",
"invariant_graph": "solid",
"fixed_lag_single_bias": "dash",
}What Problem Are We Solving?¶
A legged robot needs an estimate of its orientation, position, velocity, and contact geometry while it walks. IMU data is fast but drifts. Foot contacts are informative, but only when the robot is actually in stance and the contact geometry is trustworthy. A good legged estimator combines both sources.
In this example, each contact measurement says: this foot is touching the world here, relative to the robot body. That turns a foot in stance into a temporary geometric constraint. As the robot climbs stairs, those constraints anchor the base state and reduce drift.
Why GTSAM?¶
GTSAM is a library for inference on geometric systems. In this notebook that matters in two different ways:
The filter variants use Lie-group state representations so that rotations, velocities, and footholds are updated consistently.
The smoother variant uses GTSAM exactly as a small sliding-window factor graph, where recent states and contact episodes are optimized together.
So the notebook is not just comparing three implementations. It is comparing three inference styles built on the same geometric model.
A Few Terms First¶
If you are new to legged robotics, these are the main ideas to keep in mind:
Stance means a foot is in contact with the ground and can help constrain the robot state.
Swing means a foot is moving through the air and should not be treated as a ground anchor.
A contact measurement tells the estimator where a foot is relative to the robot body at that instant.
A factor graph is GTSAM’s way of expressing many constraints together and solving for the states that best satisfy them.
You do not need to know all the implementation details to read the plots. The main idea is that the estimator is constantly balancing fast but drifting IMU information with slower but geometrically informative foot contacts.
Dataset¶
This example uses one staircase sequence. The CSV files store only what this tutorial needs: IMU samples, active contacts, touchdown flags, and foot names. Contact packets are sampled at a modest rate, while touchdown events are kept explicitly so that contact transitions remain clear.
print(f"Feet: {DATASET['foot_names']}")
print(f"IMU samples: {len(DATASET['imu_samples'])}")
print(f"Contact events: {len(DATASET['contact_events'])}")
print(f"Plots start at first full contact: {DISPLAY_START_TIME_S:.3f} s")Feet: ['FL', 'FR', 'RL', 'RR']
IMU samples: 2399
Contact events: 269
Plots start at first full contact: 0.625 s
The Three Estimators¶
The invariant filter performs one-step recursive estimation. It is light-weight and updates immediately as new measurements arrive.
The invariant filter with graph fragment still behaves like a filter overall, but each contact update is solved as a nonlinear subproblem across all feet, instead of a pure EKF correction.
The fixed-lag smoother keeps a short recent history and re-optimizes that history each time new information arrives. It is more expensive, but it can use delayed information more effectively.
A useful way to read the plots is: the filter variants show what can be done with minimal memory and immediate updates, while the smoother shows what changes when a short optimization window is allowed.
For readers who are new to GTSAM, the key distinction is simple: filters summarize the past into the current estimate, while smoothers keep a small window of recent states alive and refine them together.
Animated Geometry¶
These animations show the estimated body and IMU frames at each contact update. Active feet are connected back to the body origin with dotted leg segments, and past footholds remain behind as a breadcrumb trail once that contact episode ends. The second view projects the same motion into the x-z plane so the staircase profile is easier to read.
ANIMATION_VARIANT = "fixed_lag_single_bias"
results = RESULTS[ANIMATION_VARIANT]
labels = LABELS[ANIMATION_VARIANT]
animation = make_contact_animation(results, labels, start_time_s=DISPLAY_START_TIME_S)
animation.show()
side_animation = make_contact_side_animation(
results, labels, start_time_s=DISPLAY_START_TIME_S
)
side_animation.show()# Uncomment to save the gif animations - takes a while...
# from gtsam.examples.LeggedEstimatorExample import write_contact_animation_gif, write_contact_side_animation_gif,
# write_contact_animation_gif(results, labels, "stairs_3d.gif", start_time_s=DISPLAY_START_TIME_S)
# write_contact_side_animation_gif(results, labels, "stairs_side.gif", start_time_s=DISPLAY_START_TIME_S)Position and Height¶
The staircase profile is easiest to see in forward motion and vertical position. The middle of the sequence includes a landing, so the height curve is not just one uniform climb.
For the early flat segment before the staircase, the replay helper applies a known terrain height of 0 m. That keeps the pre-stair height estimate well anchored without adding extra detector machinery to the tutorial.
The plots below start at the first full-contact event. That removes the initial transient where the estimators are still waiting for enough stance contacts to perform their one-time contact-based initialization.
fig = make_subplots(rows=1, cols=2, subplot_titles=("Forward position", "Estimated height"))
for variant, result in DISPLAY_RESULTS.items():
trajectory = result["trajectory"]
times = np.array([row["timestamp_s"] for row in trajectory])
xs = np.array([row["x"] for row in trajectory])
zs = np.array([row["z"] for row in trajectory])
line = dict(color=COLORS[variant], width=3, dash=LINE_STYLES[variant])
fig.add_trace(go.Scatter(x=times, y=xs, mode="lines", name=LABELS[variant], line=line, legendgroup=variant), row=1, col=1)
fig.add_trace(go.Scatter(x=times, y=zs, mode="lines", name=LABELS[variant], line=line, legendgroup=variant, showlegend=False), row=1, col=2)
fig.update_xaxes(title_text="time [s]", row=1, col=1)
fig.update_xaxes(title_text="time [s]", row=1, col=2)
fig.update_yaxes(title_text="x [m]", row=1, col=1)
fig.update_yaxes(title_text="z [m]", row=1, col=2)
fig.update_layout(height=420, width=1100, template="plotly_white", legend=dict(x=0.01, y=0.99))
fig.show()
The main question here is not which line is always highest or lowest. It is how each estimator reacts to the same contacts when support geometry changes. On stairs, even small differences in how contact constraints are handled show up as visible differences in the height trace.
Attitude Behavior¶
Roll and pitch are good diagnostic signals for legged estimation. If stance contacts are modeled well, the estimate should remain physically plausible as the robot transitions from one support pattern to the next.
fig = make_subplots(rows=1, cols=2, subplot_titles=("Roll", "Pitch"))
for variant, result in DISPLAY_RESULTS.items():
trajectory = result["trajectory"]
times = np.array([row["timestamp_s"] for row in trajectory])
rolls = np.rad2deg([row["roll"] for row in trajectory])
pitches = np.rad2deg([row["pitch"] for row in trajectory])
line = dict(color=COLORS[variant], width=3, dash=LINE_STYLES[variant])
fig.add_trace(go.Scatter(x=times, y=rolls, mode="lines", name=LABELS[variant], line=line, legendgroup=variant), row=1, col=1)
fig.add_trace(go.Scatter(x=times, y=pitches, mode="lines", name=LABELS[variant], line=line, legendgroup=variant, showlegend=False), row=1, col=2)
fig.update_xaxes(title_text="time [s]", row=1, col=1)
fig.update_xaxes(title_text="time [s]", row=1, col=2)
fig.update_yaxes(title_text="degrees", row=1, col=1)
fig.update_yaxes(title_text="degrees", row=1, col=2)
fig.update_layout(height=420, width=1100, template="plotly_white", legend=dict(x=0.01, y=0.99))
fig.show()
Velocity Components¶
Base velocity is often easier to interpret than bias for a first pass through the example. The three panels below show the estimated forward, lateral, and vertical velocity components. The same color convention is used in every panel: red for v_x, green for v_y, and blue for v_z.
fig = make_subplots(rows=1, cols=3, subplot_titles=("v_x", "v_y", "v_z"))
velocity_components = (
("vx", "v_x [m/s]"),
("vy", "v_y [m/s]"),
("vz", "v_z [m/s]"),
)
for col, (field, axis_label) in enumerate(velocity_components, start=1):
for variant, result in DISPLAY_RESULTS.items():
trajectory = result["trajectory"]
times = np.array([row["timestamp_s"] for row in trajectory])
values = np.array([row[field] for row in trajectory])
fig.add_trace(
go.Scatter(
x=times,
y=values,
mode="lines",
name=LABELS[variant],
line=dict(color=COLORS[variant], width=3, dash=LINE_STYLES[variant]),
legendgroup=variant,
showlegend=(col == 1),
),
row=1,
col=col,
)
fig.update_xaxes(title_text="time [s]", row=1, col=col)
fig.update_yaxes(title_text=axis_label, row=1, col=col)
fig.update_layout(height=420, width=1200, template="plotly_white", legend=dict(x=0.01, y=0.99))
fig.show()