← Work

2.1.1Sensor fusion as a sheaf

A working tutorial for codynamic-sensor-data: what goes wrong when measurements arrive out of order, and what you write instead.

Five steps. Each one has the code and a demonstration of what that code does, so you can see the behaviour before deciding whether it is worth your afternoon.

The code is the library's real API. The demonstrations are JavaScript written for this page — the library is Python, and this is a browser. Runnable versions are in the repository under examples/.

The failure you already have

You have a fast sensor that drifts — wheel encoders, an IMU — and a slow one that is accurate but late: vision, a motion-capture frame, anything with processing behind it. The accurate measurement describes where the robot was, and it lands after you have already acted on the fast one.

There are two usual answers and both are bad. Fuse it on arrival and the estimate jumps, because you have attached an old measurement to the present — and a controller downstream will fight that step as though the robot really moved. Drop it and you keep the drift you were trying to correct.

Demo · the three strategies realtime

Peak jump—

RMS error—

Strategy—

Strategy Vision latency Drift

true pose estimate vision fix, at the time it describes

Fuse-on-arrival is the one worth pushing the latency slider on. The jump is not noise — it is the estimate being told, all at once, about a discrepancy that accumulated over the whole latency window.

Peak jump is the largest single-step change in the estimate. It is the number a controller feels, and the one that produces a visible twitch in a real arm.

RMS error is against the true pose over the whole run — the number that says whether you are actually where you think you are.

Ten lines that run

Before any of the theory, there is a helper that assembles a working pipeline: a linear-Gaussian estimator, a belief you can query at any time, and a place to record what you actuated.

quickstart.py

import numpy as np
from codynamic_sensor_data import quickstart_pipeline

pipeline = quickstart_pipeline(
    state_dim=1,
    obs_dim=1,
    process_noise=0.01,
    measurement_noise=0.05,
)

for t, y in sensor_stream:
    pipeline.ingest(t, np.array([y]))
    estimate = pipeline.state_at(t).mean()
    pipeline.actuate(t, estimate)      # control law: u = x_hat

Demo · trusting the sensor more, or less —

measurement_noise process_noise

The two noise terms are the only tuning most pipelines need: how much you believe the sensor, against how much you believe the model. Push measurement_noise up and the estimate stops chasing every sample.

state_at(t) is the part that matters later — the belief is queryable at any time, not just the present, which is what makes rewinding possible at all.

Late data becomes a RewindEvent

Ingest a sample whose timestamp is older than one you have already folded in. The simulator does not mistake it for a jump and does not staple it onto the present: it rewinds, recomputes, and records what that cost.

late.py

sim.record_control(FloatTime(0.0), np.array([1.0]))   # x_dot = 1
sim.ingest(FloatTime(0.0),  np.array([0.0]),  sensor_id="encoder")
sim.ingest(FloatTime(10.0), np.array([10.0]), sensor_id="encoder")

# Arrives now, but describes t = 5.0.
sim.ingest(FloatTime(5.0), np.array([5.0]), sensor_id="encoder")

event = sim.rewinds[-1]
event.budget.effort
event.evidence["present_pull"]

—

Demo · where the sample belongs

Timestamp Disagreement

belief before belief after rewinding

Slide the timestamp toward the present and watch present_pull grow. That is the rule the budget encodes: a sample describing the distant past barely moves your current belief, however wrong it was; one describing a moment ago moves it a lot.

Recorded controls are never rewritten. x_dot = 1 is a fact about what you did, not a prediction to be revised, so the smoother reconciles known inputs with new evidence rather than arguing with history.

Arrival-order jump detectors are skipped for late samples by default, so a late arrival is not double-reported as a discontinuity.

Two sensors that have to agree

Each sensor gives you a local section over the interval it can see. Where two intervals overlap, the sections have to agree — and if they do, they glue into one global section. That is the sheaf condition, and it is the whole of fusion.

The useful part is what happens when they do not agree: you get a GluingFailure, not a confident average of two incompatible stories.

glue.py

sheaf = ModelAwareIntervalSheaf(
    {
        "base_encoder":   SensorModel("base_encoder",   lambda t, y: y),
        "offset_encoder": SensorModel("offset_encoder", lambda t, y: y - 10.0),
    },
    tol=0.05,
)

base = ModeledSection("base_encoder",
    SampledSection(ts=np.array([0.0, 1.0]), ys=np.array([0.0, 1.0])))
offset = ModeledSection("offset_encoder",
    SampledSection(ts=np.array([0.0, 1.0]), ys=np.array([10.0, 11.0])))

glued = sheaf.glue({Interval(0.0, 1.0): base,
                    Interval(0.0, 1.1): offset})

—

Demo · agreement on the overlap

Second sensor tol

Note that the two sensors disagree as raw arrays — one reads about 10 higher than the other. They agree only after each SensorModel maps it into a common frame. A known calibration offset is not a disagreement, and the model is where you say so.

Sheaf.restrict, Sheaf.agree, Sheaf.glue. Use IntervalSheaf when your sensors are already in the same frame and ModelAwareIntervalSheaf when they are not.

When a sensor stops deserving trust

Sensors are treated as ground truth by default. That assumption is forfeit in exactly three regimes, and each trips a Surprisal carrying a structured cause. Detection is the library's job; deciding what to do is yours.

Demo · induce a fault nominal

SurprisalLog is empty — all sensors within expectation.

Unresponsive is silence past the sensor's expected cadence. Misaligned is the gluing failure from step 4, arriving here as an event rather than an exception. Discontinuous is a jump that the sensor's own past says is not physically possible.

The three causes live in surprisal.py as Surprisal, SurprisalCause and SurprisalLog. Check the repository for the current accessor before wiring a policy to them — this page shows the causes, not a handler.

Where to go next

codynamic-sensor-datathe library
examples/head_imu_to_arm.pyfuller example
head IMU → UR5ea real arm

The estimator is a slot: linear-Gaussian, EKF and UKF ship with it, and particle filters or factor graphs plug into the same Estimator protocol. Time is a slot too — UncertainTime carries a Gaussian over the instant, for when the clock is the noisy thing.

CodynamicSimulator[Tm, S, X, A] is generic in time, sensor space, state space and actuation space. Numpy arrays, dataclasses, JAX pytrees and ROS messages all work if they satisfy the protocol.