Michael Brenndoerfer | Data & AI, Private Equity, Technology

Michael Brenndoerfer

Observations, Sensors, and Multimodal Data › Worked Example: Fusing a Delayed Camera with Fast Encoders

Michael BrenndoerferJune 13, 202611 min read

World Models HandbookAll chapters

Part 3 of 12Representing Agents and Worlds

Chapter 13Observations, Sensors, and Multimodal Data › Worked Example: Fusing a Delayed Camera with Fast Encoders

Reading section 7 of 11 · Worked Example: Fusing a Delayed Camera with Fast Encoders · 11 minSection 7 of 11 · 11 min

Section 7 of 11 · 11 min read

Observations, Sensors, and Multimodal Data › Worked Example: Fusing a Delayed Camera with Fast EncodersOne section · Sections in this chapter7 / 11 ↓

Sections in this chapter11

Reading tools Definitions & inline links

Choose your expertise level to adjust how many terms are explained. Beginners see more tooltips, experts see fewer to maintain reading flow. Hover over underlined terms for instant definitions.

Article links

Make inline references clickable

Let us build the canonical scenario from scratch. A cart moves in one dimension with position $p_t$ and velocity $v_t$ , driven by acceleration commands and disturbed by random acceleration noise. This is a discretized double integrator, written as a linear state-space model:

\begin{bmatrix} p_{t+1} \\ v_{t+1} \end{bmatrix} = \underbrace{\begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}}_{F} \begin{bmatrix} p_t \\ v_t \end{bmatrix} + \underbrace{\begin{bmatrix} \Delta t^2 / 2 \\ \Delta t \end{bmatrix}}_{B} u_t + w_t

where:

$p_t$ , $v_t$ : the cart's position and velocity (the latent state; no sensor observes them exactly)
$\Delta t$ : the time step between consecutive states (0.01 s at the 100 Hz base rate below)
$F$ : the state-transition matrix, integrating velocity into position and carrying velocity forward
$u_t$ : the acceleration command applied over the interval
$B$ : the control-input matrix, mapping acceleration to changes in position and velocity
$w_t$ : the process noise, random acceleration disturbances accumulated over the step

The $\Delta t^2/2$ entry is the kinematic contribution of constant acceleration to displacement over one step. It is the discrete-time shadow of the $\tfrac{1}{2} a t^2$ term in constant-acceleration kinematics, and it exists because the acceleration acts over the whole interval, not just at its endpoint. Even though the state is two-dimensional, only the position is ever measured; the velocity must be inferred. The coupled model makes velocity observable from repeated position measurements when $\Delta t\ne 0$ , whereas an uncoupled position-only model cannot infer velocity. Each position update then constrains velocity through the model, and each velocity estimate sharpens the next position prediction.

Two sensors watch it. A joint encoder measures position at 100 Hz with moderate noise, effectively instantly. A camera measures position more accurately at 10 Hz, but with 60 milliseconds of processing latency, occasional dropouts, and one long occlusion. We will build three filters, one per modality alone and one fusing both, and use a rewind-and-refilter scheme for the camera's out-of-sequence measurements. The camera contributes accurate absolute fixes; the encoder contributes frequent updates while the camera is absent.

Now we simulate the ground-truth trajectory and both sensor streams, carefully timestamping everything:

In[8]:

Code

import numpy as np

rng = np.random.default_rng(0)

dt = 0.01  # 100 Hz base rate
T = 1200  # 12 seconds
q_accel = 0.3**2  # white-acceleration PSD (m^2 / s^3)

F = np.array([[1.0, dt], [0.0, 1.0]])
B = np.array([0.5 * dt**2, dt])
Q = q_accel * np.array(
    [
        [dt**3 / 3, dt**2 / 2],
        [dt**2 / 2, dt],
    ]
)

# Smooth acceleration command whose 100-second common period exceeds the run
t_axis = np.arange(T) * dt
u = 0.8 * np.sin(2 * np.pi * 0.3 * t_axis) + 0.5 * np.sin(
    2 * np.pi * 0.77 * t_axis + 1.0
)

# Ground truth uses the same zero-order-held dynamics and process covariance
x_true = np.zeros((T, 2))
for t in range(T - 1):
    w_t = rng.multivariate_normal(np.zeros(2), Q)
    x_true[t + 1] = F @ x_true[t] + B * u[t] + w_t

p_true = x_true[:, 0]
v_true = x_true[:, 1]

The command signal is a sum of sinusoids at 0.3 and 0.77 Hz. Their ratio is $30/77$ , so they are commensurate, but their common period is 100 seconds, much longer than the 12-second run. The composite command therefore does not repeat during the experiment. The process increment is drawn from the same covariance $Q$ used by the filter, so the simulated system and estimator share one zero-order-held double-integrator model.

The trajectory gives us the ground truth that no sensor sees. Next, the two sensor streams. Every camera measurement records both its generation time and its arrival time, and the simulation includes random dropouts plus a two-second occlusion:

In[9]:

Code

R_ENC = 0.05**2  # encoder noise variance
R_CAM = 0.02**2  # camera noise variance
DELAY = 6  # camera latency: 60 ms at 100 Hz
OCCL = (500, 700)  # 2-second window with no generated camera frames

enc_z = np.zeros(T)  # encoder position each step
cam_gen = np.full(T, np.nan)  # camera reading by generation time
cam_arrivals = {}  # arrival step -> (gen step, reading)

for t in range(T):
    enc_z[t] = p_true[t] + rng.normal(0.0, np.sqrt(R_ENC))
    in_occlusion = OCCL[0] <= t < OCCL[1]
    if (
        t % 10 == 0
        and not in_occlusion
        and rng.random() > 0.15
        and t + DELAY < T
    ):
        z = p_true[t] + rng.normal(0.0, np.sqrt(R_CAM))
        cam_gen[t] = z
        cam_arrivals[t + DELAY] = (t, z)

The camera samples at one tenth the encoder rate. Each camera frame is also dropped independently with probability 0.15 outside the structured occlusion. The cam_arrivals dictionary is indexed by arrival step while storing the generation step alongside the reading. That pair lets the filter apply a delayed measurement to the state it describes.

We have paired the streams on a common clock and recorded latency explicitly, which is exactly the discipline the synchronization section demanded. Now the filter itself. The rewind_update method implements buffer-and-refilter: restore the belief as it was at the measurement's timestamp, apply the update there, and replay the stored predictions and encoder updates forward:

In[10]:

Code

H = np.array([[1.0, 0.0]])


class RewindableKF:
    def __init__(self, x0, P0):
        self.x, self.P = x0.copy(), P0.copy()
        # history[t] is the posterior for physical state time t
        self.history = []

    def predict(self, u_t):
        self.x = F @ self.x + B * u_t
        self.P = F @ self.P @ F.T + Q

    def update(self, z, R):
        y = z - H @ self.x
        S = (H @ self.P @ H.T)[0, 0] + R
        K = (self.P @ H.T) / S
        self.x = self.x + K.ravel() * y[0]
        self.P = (np.eye(2) - K @ H) @ self.P

    def snapshot(self):
        self.history.append((self.x.copy(), self.P.copy()))

    def rewind_update(self, g, z, R, us, enc_updates):
        """Fuse a measurement of the state at step g, then replay to now."""
        now = len(self.history) - 1
        self.x, self.P = (a.copy() for a in self.history[g])
        self.update(z, R)
        self.history[g] = (self.x.copy(), self.P.copy())
        for s in range(g + 1, now + 1):
            self.predict(us[s - 1])
            if enc_updates[s] is not None:
                self.update(enc_updates[s], R_ENC)
            self.history[s] = (self.x.copy(), self.P.copy())

The entries $dt^3/3$ , $dt^2/2$ , and $dt$ are the exact discretization of continuous-time white acceleration with spectral density $q_a$ ; consequently $q_a$ has units of m $^2$ /s $^3$ . The relative scale of $Q$ and $R$ determines how much the filter trusts prediction versus measurement. The history convention is equally important: history[t] stores the posterior for state time $t$ . A delayed camera update starts from that exact posterior, then replays controls and encoder updates from $g+1$ through the current state time, overwriting the corrected history so later delayed frames build on it. Here the six-step latency is shorter than the ten-step camera period, so no second camera generation falls inside a replay window. A general multisensor implementation must log and replay every intervening update in timestamp order.

With the filter in hand, we run three passes over the same data: encoder-only, camera-only (all camera updates at arrival time via rewind), and fused. Each pass sees only the measurements its modality provides:

In[11]:

Code

def run_filter(use_enc, use_cam):
    kf = RewindableKF(np.zeros(2), np.eye(2))
    enc_updates = [None] * T
    est = np.zeros(T)
    for t in range(T):
        if t > 0:
            kf.predict(u[t - 1])
        if use_enc:
            enc_updates[t] = np.array([enc_z[t]])
            kf.update(enc_updates[t], R_ENC)
        kf.snapshot()
        if use_cam and t in cam_arrivals:
            g, z = cam_arrivals[t]
            replay_enc = enc_updates if use_enc else [None] * T
            kf.rewind_update(g, np.array([z]), R_CAM, u, replay_enc)
        est[t] = kf.x[0]
    return est


est_enc = run_filter(True, False)
est_cam = run_filter(False, True)
est_fused = run_filter(True, True)

The enc_updates list doubles as the live stream and the replay log. The camera-only pass replays predictions without encoder updates; the fused pass replays each intervening encoder measurement once. Snapshotting before processing an arrival makes the current posterior part of the history that rewind corrects, so the replay lands back at state time $t$ rather than one step early.

Everything is computed. Before plotting, let us quantify the outcome, both over the full run and inside the occlusion window where the camera goes dark:

Out[12]:

Console

Encoder only   overall RMSE 0.0159 m, occlusion-window RMSE 0.0195 m
Camera only    overall RMSE 0.1612 m, occlusion-window RMSE 0.3757 m
Fused          overall RMSE 0.0152 m, occlusion-window RMSE 0.0194 m

Read the occlusion column carefully. The camera-only filter is reduced to open-loop prediction for two seconds, so its realized error and covariance grow as process disturbances accumulate. The encoder continues to update throughout the gap. The fused estimate stays close to the encoder during occlusion and uses the more accurate camera fixes when frames are available. The printed values make the comparison explicit rather than relying on the shape of one line segment.

Out[13]:

Visualization

Line chart of true and estimated positions with a shaded occlusion band, showing the camera-only estimate diverging inside the band. — Position estimates around the two-second occlusion. The camera-only filter dead-reckons while frames are missing; the fused filter continues to use encoder updates.

Bar chart of RMSE for encoder-only, camera-only, and fused filters, with fused lowest. — Overall position RMSE for the seeded run. The fused estimate is lower than either single-modality estimate under this parameterization.

The fused estimate uses camera precision when frames arrive and encoder continuity while they do not. When a post-occlusion frame arrives, rewind applies it at generation time and replays the intervening encoder updates exactly once. This avoids injecting a stale frame as if it described the current cart position.

Alignment Errors Masquerade as Model ErrorsLink Copied

Now let us isolate temporal alignment from missingness. We create a dense synthetic camera stream for this diagnostic only, then fit a linear estimator of current position from the encoder at time $t$ and camera at time $t+k$ . Every lag uses the same target times and the same 70/30 chronological split, so the only changed variable is the camera offset:

In[14]:

Code

align_rng = np.random.default_rng(123)
cam_alignment = p_true + align_rng.normal(0.0, np.sqrt(R_CAM), T)
lags = np.arange(-8, 9)  # +/-80 ms
common_t = np.arange(8, T - 8)


def fit_misaligned(lag):
    """Estimate p_t from encoder t and a camera feature shifted by lag."""
    X = np.column_stack([enc_z[common_t], cam_alignment[common_t + lag]])
    y = p_true[common_t]
    n_train = int(0.7 * len(y))
    Xtr = np.column_stack([np.ones(n_train), X[:n_train]])
    Xte = np.column_stack([np.ones(len(y) - n_train), X[n_train:]])
    w, *_ = np.linalg.lstsq(Xtr, y[:n_train], rcond=None)
    resid = Xte @ w - y[n_train:]
    return np.mean(resid**2)


align_mse = np.array([fit_misaligned(k) for k in lags])
best_lag = lags[int(np.argmin(align_mse))]

This is deliberately a two-feature regression. Because sample membership and targets are fixed across lags, a change in validation error can be attributed to the imposed offset within this synthetic experiment:

Out[15]:

Console

Best-fitting lag: 0 steps (0 ms)
MSE at best lag:  0.000318
MSE at ±50 ms:    0.000853

Out[16]:

Visualization

Curve of position-estimation MSE versus camera time shift with its minimum at zero milliseconds. — Validation error of a linear fused position estimator as the dense camera feature is shifted relative to the encoder on a fixed evaluation set. The minimum occurs at the true zero offset.

The minimum at zero offset confirms that the paired features should describe the same state time in this controlled setup. A 50 millisecond shift raises validation error even though the regression still fits normally. A lag sweep can therefore reveal a synchronization problem worth investigating, but a nonzero optimum is not proof by itself: the comparison must hold sample membership fixed and rule out correlated dynamics, interpolation artifacts, and clock drift.