Kalman Filter and Variants — Visual Experiments¶

Goal: See how prediction, measurement correction, covariance, nonlinear approximations, and smoothing interact in state estimation.

What we will explore¶

#	Experiment	Question answered
1	Scalar fusion	How does Kalman gain divide trust between prior and sensor?
2	Position-velocity tracking	How can position measurements reveal an unobserved velocity?
3	Sensor dropout	What happens to uncertainty when measurements disappear?
4	Innovation consistency	How do NIS and coverage expose incorrect noise assumptions?
5	EKF versus UKF	Why can a local Jacobian miss nonlinear uncertainty?
6	RTS smoothing	How does future evidence revise past state estimates?

Linked theory: Kalman Filter and Variants

In [1]:

import warnings

import matplotlib.pyplot as plt
import numpy as np

warnings.filterwarnings('ignore')

plt.rcParams.update({
    'figure.dpi': 110,
    'font.size': 11,
    'axes.titlesize': 12,
    'axes.labelsize': 11,
    'figure.facecolor': 'white',
    'axes.facecolor': '#f8f9fa',
    'axes.grid': True,
    'grid.alpha': 0.35,
})

def run_constant_velocity_filter(measurements, process_scale=0.035, measurement_variance=0.64):
    step_count = len(measurements)
    transition = np.array([[1.0, 1.0], [0.0, 1.0]])
    observation = np.array([[1.0, 0.0]])
    process_covariance = process_scale * np.array([[0.25, 0.5], [0.5, 1.0]])
    measurement_covariance = np.array([[measurement_variance]])
    identity = np.eye(2)

    filtered_means = np.zeros((step_count, 2))
    filtered_covariances = np.zeros((step_count, 2, 2))
    predicted_means = np.zeros((step_count, 2))
    predicted_covariances = np.zeros((step_count, 2, 2))
    gains = np.zeros((step_count, 2))
    innovations = np.full(step_count, np.nan)
    innovation_variances = np.full(step_count, np.nan)

    mean = np.array([0.0, 0.0])
    covariance = np.diag([4.0, 1.0])

    for step, measurement in enumerate(measurements):
        prior_mean = transition @ mean
        prior_covariance = transition @ covariance @ transition.T + process_covariance
        predicted_means[step] = prior_mean
        predicted_covariances[step] = prior_covariance

        if np.isfinite(measurement):
            innovation = measurement - (observation @ prior_mean)[0]
            innovation_covariance = observation @ prior_covariance @ observation.T + measurement_covariance
            gain = np.linalg.solve(innovation_covariance.T, (prior_covariance @ observation.T).T).T
            mean = prior_mean + gain[:, 0] * innovation
            correction = identity - gain @ observation
            covariance = (
                correction @ prior_covariance @ correction.T
                + gain @ measurement_covariance @ gain.T
            )
            gains[step] = gain[:, 0]
            innovations[step] = innovation
            innovation_variances[step] = innovation_covariance[0, 0]
        else:
            mean = prior_mean
            covariance = prior_covariance

        filtered_means[step] = mean
        filtered_covariances[step] = covariance

    return {
        'transition': transition,
        'filtered_means': filtered_means,
        'filtered_covariances': filtered_covariances,
        'predicted_means': predicted_means,
        'predicted_covariances': predicted_covariances,
        'gains': gains,
        'innovations': innovations,
        'innovation_variances': innovation_variances,
    }

import warnings import matplotlib.pyplot as plt import numpy as np warnings.filterwarnings('ignore') plt.rcParams.update({ 'figure.dpi': 110, 'font.size': 11, 'axes.titlesize': 12, 'axes.labelsize': 11, 'figure.facecolor': 'white', 'axes.facecolor': '#f8f9fa', 'axes.grid': True, 'grid.alpha': 0.35, }) def run_constant_velocity_filter(measurements, process_scale=0.035, measurement_variance=0.64): step_count = len(measurements) transition = np.array([[1.0, 1.0], [0.0, 1.0]]) observation = np.array([[1.0, 0.0]]) process_covariance = process_scale * np.array([[0.25, 0.5], [0.5, 1.0]]) measurement_covariance = np.array([[measurement_variance]]) identity = np.eye(2) filtered_means = np.zeros((step_count, 2)) filtered_covariances = np.zeros((step_count, 2, 2)) predicted_means = np.zeros((step_count, 2)) predicted_covariances = np.zeros((step_count, 2, 2)) gains = np.zeros((step_count, 2)) innovations = np.full(step_count, np.nan) innovation_variances = np.full(step_count, np.nan) mean = np.array([0.0, 0.0]) covariance = np.diag([4.0, 1.0]) for step, measurement in enumerate(measurements): prior_mean = transition @ mean prior_covariance = transition @ covariance @ transition.T + process_covariance predicted_means[step] = prior_mean predicted_covariances[step] = prior_covariance if np.isfinite(measurement): innovation = measurement - (observation @ prior_mean)[0] innovation_covariance = observation @ prior_covariance @ observation.T + measurement_covariance gain = np.linalg.solve(innovation_covariance.T, (prior_covariance @ observation.T).T).T mean = prior_mean + gain[:, 0] * innovation correction = identity - gain @ observation covariance = ( correction @ prior_covariance @ correction.T + gain @ measurement_covariance @ gain.T ) gains[step] = gain[:, 0] innovations[step] = innovation innovation_variances[step] = innovation_covariance[0, 0] else: mean = prior_mean covariance = prior_covariance filtered_means[step] = mean filtered_covariances[step] = covariance return { 'transition': transition, 'filtered_means': filtered_means, 'filtered_covariances': filtered_covariances, 'predicted_means': predicted_means, 'predicted_covariances': predicted_covariances, 'gains': gains, 'innovations': innovations, 'innovation_variances': innovation_variances, }

---

Experiment 1: Kalman gain is a trust allocation rule¶

Question: How does relative uncertainty determine whether the posterior follows the model or the measurement?

For a scalar state, the gain is $K=P^-/(P^-+R)$. We hold the prior and measurement means apart, then vary the ratio between measurement variance and prior variance.

In [2]:

np.random.seed(1)
prior_mean = 0.0
measurement = 4.0
prior_variance = 1.0
variance_ratios = np.logspace(-2.0, 2.0, 300)
measurement_variances = prior_variance * variance_ratios
gains = prior_variance / (prior_variance + measurement_variances)
posterior_means = prior_mean + gains * (measurement - prior_mean)
posterior_variances = (1.0 - gains) * prior_variance

selected_ratios = np.array([0.05, 1.0, 20.0])
selected_gains = 1.0 / (1.0 + selected_ratios)
selected_means = selected_gains * measurement
selected_standard_deviations = np.sqrt(1.0 - selected_gains)

fig, axes = plt.subplots(1, 2, figsize=(13.5, 4.8))
fig.suptitle('The Kalman gain follows relative precision, not a fixed smoothing rate', fontsize=14, fontweight='bold')

axes[0].semilogx(variance_ratios, gains, color='#457b9d', linewidth=2.5, label='Kalman gain')
axes[0].semilogx(variance_ratios, posterior_means / measurement, color='#e76f51', linewidth=2.2, label='posterior fraction toward measurement')
axes[0].axvline(1.0, color='black', linestyle='--', linewidth=1.1, label='equal variances')
axes[0].set_title('Trust as measurement noise changes')
axes[0].set_xlabel('Measurement variance / prior variance')
axes[0].set_ylabel('Gain or normalized correction')
axes[0].legend(fontsize=9)

axes[1].errorbar(
    selected_means,
    np.arange(len(selected_ratios)),
    xerr=2.0 * selected_standard_deviations,
    fmt='o',
    color='#2a9d8f',
    capsize=5,
    label='posterior mean ± 2σ',
)
axes[1].axvline(prior_mean, color='#457b9d', linestyle='--', linewidth=1.5, label='prior mean')
axes[1].axvline(measurement, color='#e76f51', linestyle='--', linewidth=1.5, label='measurement')
axes[1].set_yticks(np.arange(len(selected_ratios)), [f'R/P = {ratio:g}' for ratio in selected_ratios])
axes[1].set_title('Three posterior beliefs')
axes[1].set_xlabel('Scalar state value')
axes[1].set_ylabel('Relative sensor noise')
axes[1].legend(fontsize=9)

plt.tight_layout()
plt.show()

np.random.seed(1) prior_mean = 0.0 measurement = 4.0 prior_variance = 1.0 variance_ratios = np.logspace(-2.0, 2.0, 300) measurement_variances = prior_variance * variance_ratios gains = prior_variance / (prior_variance + measurement_variances) posterior_means = prior_mean + gains * (measurement - prior_mean) posterior_variances = (1.0 - gains) * prior_variance selected_ratios = np.array([0.05, 1.0, 20.0]) selected_gains = 1.0 / (1.0 + selected_ratios) selected_means = selected_gains * measurement selected_standard_deviations = np.sqrt(1.0 - selected_gains) fig, axes = plt.subplots(1, 2, figsize=(13.5, 4.8)) fig.suptitle('The Kalman gain follows relative precision, not a fixed smoothing rate', fontsize=14, fontweight='bold') axes[0].semilogx(variance_ratios, gains, color='#457b9d', linewidth=2.5, label='Kalman gain') axes[0].semilogx(variance_ratios, posterior_means / measurement, color='#e76f51', linewidth=2.2, label='posterior fraction toward measurement') axes[0].axvline(1.0, color='black', linestyle='--', linewidth=1.1, label='equal variances') axes[0].set_title('Trust as measurement noise changes') axes[0].set_xlabel('Measurement variance / prior variance') axes[0].set_ylabel('Gain or normalized correction') axes[0].legend(fontsize=9) axes[1].errorbar( selected_means, np.arange(len(selected_ratios)), xerr=2.0 * selected_standard_deviations, fmt='o', color='#2a9d8f', capsize=5, label='posterior mean ± 2σ', ) axes[1].axvline(prior_mean, color='#457b9d', linestyle='--', linewidth=1.5, label='prior mean') axes[1].axvline(measurement, color='#e76f51', linestyle='--', linewidth=1.5, label='measurement') axes[1].set_yticks(np.arange(len(selected_ratios)), [f'R/P = {ratio:g}' for ratio in selected_ratios]) axes[1].set_title('Three posterior beliefs') axes[1].set_xlabel('Scalar state value') axes[1].set_ylabel('Relative sensor noise') axes[1].legend(fontsize=9) plt.tight_layout() plt.show()

What you see¶

1. What the plots show: A precise sensor drives the gain toward one and moves the posterior near the measurement. A noisy sensor drives the gain toward zero and leaves the posterior near the prior.
2. Why it looks this way: Independent Gaussian information combines through precision. The source with smaller variance contributes more to the posterior mean, while both sources reduce posterior uncertainty.
3. Key takeaway: Kalman gain is adaptive confidence weighting. Treating it as a hand-tuned interpolation coefficient loses its probabilistic meaning.

---

Experiment 2: Position measurements reveal velocity¶

Question: How can a filter estimate a state dimension that the sensor never measures directly?

The state contains position and velocity, while the sensor reports only noisy position. The transition couples velocity into future position, making velocity observable across a sequence.

In [3]:

np.random.seed(2)
step_count = 90
time = np.arange(step_count)
true_states = np.zeros((step_count, 2))
true_states[0] = [0.0, 0.45]

for step in range(1, step_count):
    maneuver = 0.055 if 30 <= step < 42 else (-0.045 if 62 <= step < 72 else 0.0)
    acceleration = maneuver + 0.035 * np.random.randn()
    true_states[step, 0] = true_states[step - 1, 0] + true_states[step - 1, 1] + 0.5 * acceleration
    true_states[step, 1] = true_states[step - 1, 1] + acceleration

measurements = true_states[:, 0] + 0.8 * np.random.randn(step_count)
tracking = run_constant_velocity_filter(measurements)
position_sigma = np.sqrt(tracking['filtered_covariances'][:, 0, 0])
velocity_sigma = np.sqrt(tracking['filtered_covariances'][:, 1, 1])

fig, axes = plt.subplots(1, 3, figsize=(16, 4.8))
fig.suptitle('Dynamics make hidden velocity observable through a sequence of positions', fontsize=14, fontweight='bold')

axes[0].plot(time, true_states[:, 0], color='black', linewidth=2.3, label='true position')
axes[0].scatter(time, measurements, color='#e76f51', alpha=0.38, s=16, label='position measurement')
axes[0].plot(time, tracking['filtered_means'][:, 0], color='#2a9d8f', linewidth=2.1, label='filtered position')
axes[0].fill_between(
    time,
    tracking['filtered_means'][:, 0] - 2.0 * position_sigma,
    tracking['filtered_means'][:, 0] + 2.0 * position_sigma,
    color='#2a9d8f',
    alpha=0.16,
    label='± 2σ position',
)
axes[0].set_title('Observed position')
axes[0].set_xlabel('Time step')
axes[0].set_ylabel('Position')
axes[0].legend(fontsize=8)

axes[1].plot(time, true_states[:, 1], color='black', linewidth=2.3, label='true velocity')
axes[1].plot(time, tracking['filtered_means'][:, 1], color='#457b9d', linewidth=2.1, label='filtered velocity')
axes[1].fill_between(
    time,
    tracking['filtered_means'][:, 1] - 2.0 * velocity_sigma,
    tracking['filtered_means'][:, 1] + 2.0 * velocity_sigma,
    color='#457b9d',
    alpha=0.16,
    label='± 2σ velocity',
)
axes[1].set_title('Unobserved velocity')
axes[1].set_xlabel('Time step')
axes[1].set_ylabel('Velocity')
axes[1].legend(fontsize=8)

axes[2].plot(time, tracking['gains'][:, 0], color='#e76f51', linewidth=2.1, label='position gain')
axes[2].plot(time, tracking['gains'][:, 1], color='#6a4c93', linewidth=2.1, label='velocity gain')
axes[2].set_title('One position residual corrects both states')
axes[2].set_xlabel('Time step')
axes[2].set_ylabel('Kalman gain component')
axes[2].legend(fontsize=9)

plt.tight_layout()
plt.show()

np.random.seed(2) step_count = 90 time = np.arange(step_count) true_states = np.zeros((step_count, 2)) true_states[0] = [0.0, 0.45] for step in range(1, step_count): maneuver = 0.055 if 30 <= step < 42 else (-0.045 if 62 <= step < 72 else 0.0) acceleration = maneuver + 0.035 * np.random.randn() true_states[step, 0] = true_states[step - 1, 0] + true_states[step - 1, 1] + 0.5 * acceleration true_states[step, 1] = true_states[step - 1, 1] + acceleration measurements = true_states[:, 0] + 0.8 * np.random.randn(step_count) tracking = run_constant_velocity_filter(measurements) position_sigma = np.sqrt(tracking['filtered_covariances'][:, 0, 0]) velocity_sigma = np.sqrt(tracking['filtered_covariances'][:, 1, 1]) fig, axes = plt.subplots(1, 3, figsize=(16, 4.8)) fig.suptitle('Dynamics make hidden velocity observable through a sequence of positions', fontsize=14, fontweight='bold') axes[0].plot(time, true_states[:, 0], color='black', linewidth=2.3, label='true position') axes[0].scatter(time, measurements, color='#e76f51', alpha=0.38, s=16, label='position measurement') axes[0].plot(time, tracking['filtered_means'][:, 0], color='#2a9d8f', linewidth=2.1, label='filtered position') axes[0].fill_between( time, tracking['filtered_means'][:, 0] - 2.0 * position_sigma, tracking['filtered_means'][:, 0] + 2.0 * position_sigma, color='#2a9d8f', alpha=0.16, label='± 2σ position', ) axes[0].set_title('Observed position') axes[0].set_xlabel('Time step') axes[0].set_ylabel('Position') axes[0].legend(fontsize=8) axes[1].plot(time, true_states[:, 1], color='black', linewidth=2.3, label='true velocity') axes[1].plot(time, tracking['filtered_means'][:, 1], color='#457b9d', linewidth=2.1, label='filtered velocity') axes[1].fill_between( time, tracking['filtered_means'][:, 1] - 2.0 * velocity_sigma, tracking['filtered_means'][:, 1] + 2.0 * velocity_sigma, color='#457b9d', alpha=0.16, label='± 2σ velocity', ) axes[1].set_title('Unobserved velocity') axes[1].set_xlabel('Time step') axes[1].set_ylabel('Velocity') axes[1].legend(fontsize=8) axes[2].plot(time, tracking['gains'][:, 0], color='#e76f51', linewidth=2.1, label='position gain') axes[2].plot(time, tracking['gains'][:, 1], color='#6a4c93', linewidth=2.1, label='velocity gain') axes[2].set_title('One position residual corrects both states') axes[2].set_xlabel('Time step') axes[2].set_ylabel('Kalman gain component') axes[2].legend(fontsize=9) plt.tight_layout() plt.show()

What you see¶

1. What the plots show: The filtered position rejects much of the sensor noise, and the filter also reconstructs velocity despite never observing it directly. The velocity component of the gain is nonzero.
2. Why it looks this way: Position and velocity become correlated during prediction. A position innovation therefore contains information about velocity, and the cross-covariance maps that residual into both state dimensions.
3. Key takeaway: Observability is a property of measurements plus dynamics over time, not of one observation matrix viewed in isolation.

---

Experiment 3: Missing measurements expand the belief¶

Question: What should a calibrated filter report while a sensor is unavailable?

We remove position measurements for twenty steps. The filter can continue predicting, but process noise must accumulate until observation correction resumes.

In [4]:

np.random.seed(3)
dropout_measurements = measurements.copy()
dropout_start, dropout_end = 38, 58
dropout_measurements[dropout_start:dropout_end] = np.nan
dropout_tracking = run_constant_velocity_filter(dropout_measurements)
dropout_position_sigma = np.sqrt(dropout_tracking['filtered_covariances'][:, 0, 0])

fig, axes = plt.subplots(1, 2, figsize=(13.5, 4.8))
fig.suptitle('Prediction continues through dropout, but confidence must decrease', fontsize=14, fontweight='bold')

axes[0].plot(time, true_states[:, 0], color='black', linewidth=2.2, label='true position')
axes[0].scatter(time, dropout_measurements, color='#e76f51', alpha=0.45, s=17, label='available measurements')
axes[0].plot(time, dropout_tracking['filtered_means'][:, 0], color='#2a9d8f', linewidth=2.2, label='filtered position')
axes[0].fill_between(
    time,
    dropout_tracking['filtered_means'][:, 0] - 2.0 * dropout_position_sigma,
    dropout_tracking['filtered_means'][:, 0] + 2.0 * dropout_position_sigma,
    color='#2a9d8f',
    alpha=0.18,
    label='± 2σ position',
)
axes[0].axvspan(dropout_start, dropout_end - 1, color='#f4a261', alpha=0.16, label='sensor dropout')
axes[0].set_title('State estimate during missing data')
axes[0].set_xlabel('Time step')
axes[0].set_ylabel('Position')
axes[0].legend(fontsize=8)

axes[1].plot(time, dropout_position_sigma, color='#6a4c93', linewidth=2.3, label='position standard deviation')
axes[1].plot(time, dropout_tracking['gains'][:, 0], color='#457b9d', linewidth=2.1, label='position gain')
axes[1].axvspan(dropout_start, dropout_end - 1, color='#f4a261', alpha=0.16, label='sensor dropout')
axes[1].set_title('Uncertainty and correction strength')
axes[1].set_xlabel('Time step')
axes[1].set_ylabel('Standard deviation or gain')
axes[1].legend(fontsize=9)

plt.tight_layout()
plt.show()

np.random.seed(3) dropout_measurements = measurements.copy() dropout_start, dropout_end = 38, 58 dropout_measurements[dropout_start:dropout_end] = np.nan dropout_tracking = run_constant_velocity_filter(dropout_measurements) dropout_position_sigma = np.sqrt(dropout_tracking['filtered_covariances'][:, 0, 0]) fig, axes = plt.subplots(1, 2, figsize=(13.5, 4.8)) fig.suptitle('Prediction continues through dropout, but confidence must decrease', fontsize=14, fontweight='bold') axes[0].plot(time, true_states[:, 0], color='black', linewidth=2.2, label='true position') axes[0].scatter(time, dropout_measurements, color='#e76f51', alpha=0.45, s=17, label='available measurements') axes[0].plot(time, dropout_tracking['filtered_means'][:, 0], color='#2a9d8f', linewidth=2.2, label='filtered position') axes[0].fill_between( time, dropout_tracking['filtered_means'][:, 0] - 2.0 * dropout_position_sigma, dropout_tracking['filtered_means'][:, 0] + 2.0 * dropout_position_sigma, color='#2a9d8f', alpha=0.18, label='± 2σ position', ) axes[0].axvspan(dropout_start, dropout_end - 1, color='#f4a261', alpha=0.16, label='sensor dropout') axes[0].set_title('State estimate during missing data') axes[0].set_xlabel('Time step') axes[0].set_ylabel('Position') axes[0].legend(fontsize=8) axes[1].plot(time, dropout_position_sigma, color='#6a4c93', linewidth=2.3, label='position standard deviation') axes[1].plot(time, dropout_tracking['gains'][:, 0], color='#457b9d', linewidth=2.1, label='position gain') axes[1].axvspan(dropout_start, dropout_end - 1, color='#f4a261', alpha=0.16, label='sensor dropout') axes[1].set_title('Uncertainty and correction strength') axes[1].set_xlabel('Time step') axes[1].set_ylabel('Standard deviation or gain') axes[1].legend(fontsize=9) plt.tight_layout() plt.show()

What you see¶

1. What the plots show: The estimate remains continuous during dropout, but its confidence band widens. The gain is zero without a measurement and jumps when the sensor returns because the prior has become uncertain.
2. Why it looks this way: Every prediction propagates old covariance and adds process noise. No measurement is available to remove uncertainty, so a calibrated filter must admit that multiple trajectories have become plausible.
3. Key takeaway: A smooth prediction is not the same as a confident prediction. Missing-data handling must preserve covariance growth rather than silently forward-fill observations.

---

Experiment 4: Innovation statistics audit noise assumptions¶

Question: Can normalized innovation and empirical coverage reveal when the filter is overconfident or underconfident?

The true sensor standard deviation is 0.8. We run the same scalar random-walk filter while assuming sensor noise that is too small, correct, or too large.

In [5]:

np.random.seed(4)
simulation_count = 180
consistency_steps = 120
true_process_variance = 0.05
true_measurement_variance = 0.64
assumed_variances = {'too small R': 0.12, 'correct R': 0.64, 'too large R': 2.10}
colors = {'too small R': '#e76f51', 'correct R': '#2a9d8f', 'too large R': '#457b9d'}
results = {}

for label, assumed_variance in assumed_variances.items():
    all_nis = []
    covered = []
    first_run_nis = None
    for simulation in range(simulation_count):
        state = 0.0
        estimate = 0.0
        variance = 1.0
        run_nis = []
        for step in range(consistency_steps):
            state = state + np.sqrt(true_process_variance) * np.random.randn()
            measurement_value = state + np.sqrt(true_measurement_variance) * np.random.randn()
            prior_variance = variance + true_process_variance
            innovation = measurement_value - estimate
            innovation_variance = prior_variance + assumed_variance
            gain = prior_variance / innovation_variance
            estimate = estimate + gain * innovation
            variance = (1.0 - gain) * prior_variance
            run_nis.append(innovation**2 / innovation_variance)
            covered.append(abs(state - estimate) <= 1.96 * np.sqrt(variance))
        all_nis.extend(run_nis)
        if simulation == 0:
            first_run_nis = np.asarray(run_nis)
    results[label] = {
        'mean_nis': np.mean(all_nis),
        'coverage': np.mean(covered),
        'first_run_nis': first_run_nis,
    }

fig, axes = plt.subplots(1, 3, figsize=(16, 4.7))
fig.suptitle('Wrong noise scales leave fingerprints in innovation and coverage', fontsize=14, fontweight='bold')

window = 15
kernel = np.ones(window) / window
for label, result in results.items():
    rolling_nis = np.convolve(result['first_run_nis'], kernel, mode='valid')
    axes[0].plot(np.arange(window - 1, consistency_steps), rolling_nis, color=colors[label], linewidth=2.0, label=label)
axes[0].axhline(1.0, color='black', linestyle='--', linewidth=1.2, label='expected mean NIS')
axes[0].set_title('Rolling normalized innovation squared')
axes[0].set_xlabel('Time step')
axes[0].set_ylabel('15-step mean NIS')
axes[0].legend(fontsize=8)

labels = list(results)
axes[1].bar(labels, [results[label]['mean_nis'] for label in labels], color=[colors[label] for label in labels])
axes[1].axhline(1.0, color='black', linestyle='--', linewidth=1.2, label='expected mean')
axes[1].set_title('Mean NIS across simulations')
axes[1].set_xlabel('Assumed measurement noise')
axes[1].set_ylabel('Mean NIS')
axes[1].tick_params(axis='x', rotation=18)
axes[1].legend(fontsize=9)

axes[2].bar(labels, [results[label]['coverage'] for label in labels], color=[colors[label] for label in labels])
axes[2].axhline(0.95, color='black', linestyle='--', linewidth=1.2, label='nominal 95% coverage')
axes[2].set_title('Empirical ±1.96σ state coverage')
axes[2].set_xlabel('Assumed measurement noise')
axes[2].set_ylabel('Coverage fraction')
axes[2].set_ylim(0.0, 1.02)
axes[2].tick_params(axis='x', rotation=18)
axes[2].legend(fontsize=9)

plt.tight_layout()
plt.show()

np.random.seed(4) simulation_count = 180 consistency_steps = 120 true_process_variance = 0.05 true_measurement_variance = 0.64 assumed_variances = {'too small R': 0.12, 'correct R': 0.64, 'too large R': 2.10} colors = {'too small R': '#e76f51', 'correct R': '#2a9d8f', 'too large R': '#457b9d'} results = {} for label, assumed_variance in assumed_variances.items(): all_nis = [] covered = [] first_run_nis = None for simulation in range(simulation_count): state = 0.0 estimate = 0.0 variance = 1.0 run_nis = [] for step in range(consistency_steps): state = state + np.sqrt(true_process_variance) * np.random.randn() measurement_value = state + np.sqrt(true_measurement_variance) * np.random.randn() prior_variance = variance + true_process_variance innovation = measurement_value - estimate innovation_variance = prior_variance + assumed_variance gain = prior_variance / innovation_variance estimate = estimate + gain * innovation variance = (1.0 - gain) * prior_variance run_nis.append(innovation**2 / innovation_variance) covered.append(abs(state - estimate) <= 1.96 * np.sqrt(variance)) all_nis.extend(run_nis) if simulation == 0: first_run_nis = np.asarray(run_nis) results[label] = { 'mean_nis': np.mean(all_nis), 'coverage': np.mean(covered), 'first_run_nis': first_run_nis, } fig, axes = plt.subplots(1, 3, figsize=(16, 4.7)) fig.suptitle('Wrong noise scales leave fingerprints in innovation and coverage', fontsize=14, fontweight='bold') window = 15 kernel = np.ones(window) / window for label, result in results.items(): rolling_nis = np.convolve(result['first_run_nis'], kernel, mode='valid') axes[0].plot(np.arange(window - 1, consistency_steps), rolling_nis, color=colors[label], linewidth=2.0, label=label) axes[0].axhline(1.0, color='black', linestyle='--', linewidth=1.2, label='expected mean NIS') axes[0].set_title('Rolling normalized innovation squared') axes[0].set_xlabel('Time step') axes[0].set_ylabel('15-step mean NIS') axes[0].legend(fontsize=8) labels = list(results) axes[1].bar(labels, [results[label]['mean_nis'] for label in labels], color=[colors[label] for label in labels]) axes[1].axhline(1.0, color='black', linestyle='--', linewidth=1.2, label='expected mean') axes[1].set_title('Mean NIS across simulations') axes[1].set_xlabel('Assumed measurement noise') axes[1].set_ylabel('Mean NIS') axes[1].tick_params(axis='x', rotation=18) axes[1].legend(fontsize=9) axes[2].bar(labels, [results[label]['coverage'] for label in labels], color=[colors[label] for label in labels]) axes[2].axhline(0.95, color='black', linestyle='--', linewidth=1.2, label='nominal 95% coverage') axes[2].set_title('Empirical ±1.96σ state coverage') axes[2].set_xlabel('Assumed measurement noise') axes[2].set_ylabel('Coverage fraction') axes[2].set_ylim(0.0, 1.02) axes[2].tick_params(axis='x', rotation=18) axes[2].legend(fontsize=9) plt.tight_layout() plt.show()

What you see¶

1. What the plots show: Underestimating sensor noise produces excessive NIS and confidence intervals that miss the true state too often. Overestimating noise suppresses NIS and produces overly broad coverage.
2. Why it looks this way: NIS divides the squared innovation by the variance the filter predicted for that innovation. Persistent disagreement means the residual scale and the reported uncertainty are inconsistent.
3. Key takeaway: Low estimation error on one trajectory is insufficient. Calibration requires distributional checks such as NIS, NEES, coverage, and innovation autocorrelation.

---

Experiment 5: A Jacobian can erase nonlinear uncertainty¶

Question: Why can the unscented transform capture moments that first-order EKF linearization misses?

Let $x\sim\mathcal{N}(0,1)$ and transform it with $y=x^2$. The derivative at the mean is zero, even though the output distribution has positive mean and substantial variance.

In [6]:

np.random.seed(5)
sample_count = 200000
samples = np.random.randn(sample_count)
transformed_samples = samples**2
monte_carlo_mean = np.mean(transformed_samples)
monte_carlo_variance = np.var(transformed_samples)

ekf_mean = 0.0**2
ekf_jacobian = 2.0 * 0.0
ekf_variance = ekf_jacobian**2 * 1.0

lambda_value = 2.0
sigma_points = np.array([0.0, np.sqrt(1.0 + lambda_value), -np.sqrt(1.0 + lambda_value)])
sigma_weights = np.array([
    lambda_value / (1.0 + lambda_value),
    1.0 / (2.0 * (1.0 + lambda_value)),
    1.0 / (2.0 * (1.0 + lambda_value)),
])
transformed_sigma_points = sigma_points**2
ukf_mean = np.sum(sigma_weights * transformed_sigma_points)
ukf_variance = np.sum(sigma_weights * (transformed_sigma_points - ukf_mean)**2)

fig, axes = plt.subplots(1, 3, figsize=(16, 4.7))
fig.suptitle('Local linearization can miss curvature that sigma points expose', fontsize=14, fontweight='bold')

input_grid = np.linspace(-3.2, 3.2, 500)
axes[0].plot(input_grid, input_grid**2, color='#264653', linewidth=2.4, label='$y=x^2$')
axes[0].plot(input_grid, np.zeros_like(input_grid), color='#e76f51', linestyle='--', linewidth=2.0, label='EKF tangent at mean')
axes[0].scatter(sigma_points, transformed_sigma_points, color='#2a9d8f', s=70, zorder=3, label='UKF sigma points')
axes[0].set_title('Function, local tangent, and sigma points')
axes[0].set_xlabel('Input state x')
axes[0].set_ylabel('Transformed value y')
axes[0].legend(fontsize=9)

axes[1].hist(transformed_samples, bins=100, density=True, color='#457b9d', alpha=0.55, label='Monte Carlo output')
axes[1].axvline(ekf_mean, color='#e76f51', linestyle='--', linewidth=2.0, label='EKF mean')
axes[1].axvline(ukf_mean, color='#2a9d8f', linestyle='--', linewidth=2.0, label='UKF mean')
axes[1].set_xlim(-0.1, 7.0)
axes[1].set_title('Non-Gaussian transformed distribution')
axes[1].set_xlabel('Transformed value y')
axes[1].set_ylabel('Probability density')
axes[1].legend(fontsize=9)

moment_labels = ['mean', 'variance']
x_positions = np.arange(len(moment_labels))
width = 0.25
axes[2].bar(x_positions - width, [monte_carlo_mean, monte_carlo_variance], width, color='#457b9d', label='Monte Carlo')
axes[2].bar(x_positions, [ekf_mean, ekf_variance], width, color='#e76f51', label='EKF')
axes[2].bar(x_positions + width, [ukf_mean, ukf_variance], width, color='#2a9d8f', label='UKF')
axes[2].set_xticks(x_positions, moment_labels)
axes[2].set_title('First two transformed moments')
axes[2].set_xlabel('Output moment')
axes[2].set_ylabel('Estimated value')
axes[2].legend(fontsize=9)

plt.tight_layout()
plt.show()

np.random.seed(5) sample_count = 200000 samples = np.random.randn(sample_count) transformed_samples = samples**2 monte_carlo_mean = np.mean(transformed_samples) monte_carlo_variance = np.var(transformed_samples) ekf_mean = 0.0**2 ekf_jacobian = 2.0 * 0.0 ekf_variance = ekf_jacobian**2 * 1.0 lambda_value = 2.0 sigma_points = np.array([0.0, np.sqrt(1.0 + lambda_value), -np.sqrt(1.0 + lambda_value)]) sigma_weights = np.array([ lambda_value / (1.0 + lambda_value), 1.0 / (2.0 * (1.0 + lambda_value)), 1.0 / (2.0 * (1.0 + lambda_value)), ]) transformed_sigma_points = sigma_points**2 ukf_mean = np.sum(sigma_weights * transformed_sigma_points) ukf_variance = np.sum(sigma_weights * (transformed_sigma_points - ukf_mean)**2) fig, axes = plt.subplots(1, 3, figsize=(16, 4.7)) fig.suptitle('Local linearization can miss curvature that sigma points expose', fontsize=14, fontweight='bold') input_grid = np.linspace(-3.2, 3.2, 500) axes[0].plot(input_grid, input_grid**2, color='#264653', linewidth=2.4, label='$y=x^2$') axes[0].plot(input_grid, np.zeros_like(input_grid), color='#e76f51', linestyle='--', linewidth=2.0, label='EKF tangent at mean') axes[0].scatter(sigma_points, transformed_sigma_points, color='#2a9d8f', s=70, zorder=3, label='UKF sigma points') axes[0].set_title('Function, local tangent, and sigma points') axes[0].set_xlabel('Input state x') axes[0].set_ylabel('Transformed value y') axes[0].legend(fontsize=9) axes[1].hist(transformed_samples, bins=100, density=True, color='#457b9d', alpha=0.55, label='Monte Carlo output') axes[1].axvline(ekf_mean, color='#e76f51', linestyle='--', linewidth=2.0, label='EKF mean') axes[1].axvline(ukf_mean, color='#2a9d8f', linestyle='--', linewidth=2.0, label='UKF mean') axes[1].set_xlim(-0.1, 7.0) axes[1].set_title('Non-Gaussian transformed distribution') axes[1].set_xlabel('Transformed value y') axes[1].set_ylabel('Probability density') axes[1].legend(fontsize=9) moment_labels = ['mean', 'variance'] x_positions = np.arange(len(moment_labels)) width = 0.25 axes[2].bar(x_positions - width, [monte_carlo_mean, monte_carlo_variance], width, color='#457b9d', label='Monte Carlo') axes[2].bar(x_positions, [ekf_mean, ekf_variance], width, color='#e76f51', label='EKF') axes[2].bar(x_positions + width, [ukf_mean, ukf_variance], width, color='#2a9d8f', label='UKF') axes[2].set_xticks(x_positions, moment_labels) axes[2].set_title('First two transformed moments') axes[2].set_xlabel('Output moment') axes[2].set_ylabel('Estimated value') axes[2].legend(fontsize=9) plt.tight_layout() plt.show()

What you see¶

1. What the plots show: The EKF tangent is flat at the input mean, so it predicts zero output variance and the wrong mean. The symmetric sigma points recover the Monte Carlo mean and variance for this quadratic example.
2. Why it looks this way: First-order Taylor expansion sees only the derivative at one point. The unscented transform evaluates the curved function away from the mean and reconstructs output moments from those deterministic samples.
3. Key takeaway: UKF improves moment propagation for many smooth nonlinearities, but it still compresses the non-Gaussian output into a single mean and covariance.

---

Experiment 6: Smoothing uses information from the future¶

Question: How does a Rauch-Tung-Striebel backward pass improve earlier estimates, and why is that not an online result?

We apply an RTS smoother to the filtered position-velocity trajectory from Experiment 2. Each backward correction uses the next smoothed state to revise the current filtered state.

In [7]:

np.random.seed(6)
transition = tracking['transition']
smoothed_means = tracking['filtered_means'].copy()
smoothed_covariances = tracking['filtered_covariances'].copy()

for step in range(step_count - 2, -1, -1):
    cross_covariance = tracking['filtered_covariances'][step] @ transition.T
    smoother_gain = np.linalg.solve(
        tracking['predicted_covariances'][step + 1].T,
        cross_covariance.T,
    ).T
    smoothed_means[step] = (
        tracking['filtered_means'][step]
        + smoother_gain @ (smoothed_means[step + 1] - tracking['predicted_means'][step + 1])
    )
    smoothed_covariances[step] = (
        tracking['filtered_covariances'][step]
        + smoother_gain
        @ (smoothed_covariances[step + 1] - tracking['predicted_covariances'][step + 1])
        @ smoother_gain.T
    )

filtered_position_error = tracking['filtered_means'][:, 0] - true_states[:, 0]
smoothed_position_error = smoothed_means[:, 0] - true_states[:, 0]
filtered_position_sigma = np.sqrt(tracking['filtered_covariances'][:, 0, 0])
smoothed_position_sigma = np.sqrt(np.maximum(smoothed_covariances[:, 0, 0], 0.0))

fig, axes = plt.subplots(1, 3, figsize=(16, 4.8))
fig.suptitle('Future measurements sharpen past estimates in an offline smoother', fontsize=14, fontweight='bold')

axes[0].plot(time, true_states[:, 0], color='black', linewidth=2.3, label='true position')
axes[0].plot(time, tracking['filtered_means'][:, 0], color='#e76f51', linewidth=1.9, label='online filtered')
axes[0].plot(time, smoothed_means[:, 0], color='#2a9d8f', linewidth=2.2, label='offline smoothed')
axes[0].set_title('Filtered and smoothed position')
axes[0].set_xlabel('Time step')
axes[0].set_ylabel('Position')
axes[0].legend(fontsize=9)

axes[1].plot(time, filtered_position_error, color='#e76f51', linewidth=1.9, label='filtered error')
axes[1].plot(time, smoothed_position_error, color='#2a9d8f', linewidth=2.1, label='smoothed error')
axes[1].axhline(0.0, color='black', linestyle='--', linewidth=1.0, label='zero error')
axes[1].set_title('Position estimation error')
axes[1].set_xlabel('Time step')
axes[1].set_ylabel('Estimate - truth')
axes[1].legend(fontsize=9)

axes[2].plot(time, filtered_position_sigma, color='#e76f51', linewidth=2.0, label='filtered σ')
axes[2].plot(time, smoothed_position_sigma, color='#2a9d8f', linewidth=2.1, label='smoothed σ')
axes[2].set_title('Reported position uncertainty')
axes[2].set_xlabel('Time step')
axes[2].set_ylabel('Position standard deviation')
axes[2].legend(fontsize=9)

plt.tight_layout()
plt.show()

np.random.seed(6) transition = tracking['transition'] smoothed_means = tracking['filtered_means'].copy() smoothed_covariances = tracking['filtered_covariances'].copy() for step in range(step_count - 2, -1, -1): cross_covariance = tracking['filtered_covariances'][step] @ transition.T smoother_gain = np.linalg.solve( tracking['predicted_covariances'][step + 1].T, cross_covariance.T, ).T smoothed_means[step] = ( tracking['filtered_means'][step] + smoother_gain @ (smoothed_means[step + 1] - tracking['predicted_means'][step + 1]) ) smoothed_covariances[step] = ( tracking['filtered_covariances'][step] + smoother_gain @ (smoothed_covariances[step + 1] - tracking['predicted_covariances'][step + 1]) @ smoother_gain.T ) filtered_position_error = tracking['filtered_means'][:, 0] - true_states[:, 0] smoothed_position_error = smoothed_means[:, 0] - true_states[:, 0] filtered_position_sigma = np.sqrt(tracking['filtered_covariances'][:, 0, 0]) smoothed_position_sigma = np.sqrt(np.maximum(smoothed_covariances[:, 0, 0], 0.0)) fig, axes = plt.subplots(1, 3, figsize=(16, 4.8)) fig.suptitle('Future measurements sharpen past estimates in an offline smoother', fontsize=14, fontweight='bold') axes[0].plot(time, true_states[:, 0], color='black', linewidth=2.3, label='true position') axes[0].plot(time, tracking['filtered_means'][:, 0], color='#e76f51', linewidth=1.9, label='online filtered') axes[0].plot(time, smoothed_means[:, 0], color='#2a9d8f', linewidth=2.2, label='offline smoothed') axes[0].set_title('Filtered and smoothed position') axes[0].set_xlabel('Time step') axes[0].set_ylabel('Position') axes[0].legend(fontsize=9) axes[1].plot(time, filtered_position_error, color='#e76f51', linewidth=1.9, label='filtered error') axes[1].plot(time, smoothed_position_error, color='#2a9d8f', linewidth=2.1, label='smoothed error') axes[1].axhline(0.0, color='black', linestyle='--', linewidth=1.0, label='zero error') axes[1].set_title('Position estimation error') axes[1].set_xlabel('Time step') axes[1].set_ylabel('Estimate - truth') axes[1].legend(fontsize=9) axes[2].plot(time, filtered_position_sigma, color='#e76f51', linewidth=2.0, label='filtered σ') axes[2].plot(time, smoothed_position_sigma, color='#2a9d8f', linewidth=2.1, label='smoothed σ') axes[2].set_title('Reported position uncertainty') axes[2].set_xlabel('Time step') axes[2].set_ylabel('Position standard deviation') axes[2].legend(fontsize=9) plt.tight_layout() plt.show()

What you see¶

1. What the plots show: The smoothed trajectory is generally closer to the true path and reports lower uncertainty than the online filtered trajectory, especially away from the final time step.
2. Why it looks this way: A later measurement constrains earlier velocity and position through dynamics. The backward pass carries that information into the past, while a causal filter at time $k$ has access only to measurements through $k$.
3. Key takeaway: Filtering and smoothing solve different information problems. Smoothed offline accuracy must not be presented as online filtering performance.

---

Summary¶

Experiment	Key insight
1. Scalar fusion	Relative precision determines how far the posterior moves toward a measurement.
2. Position-velocity tracking	Dynamics and cross-covariance make hidden state dimensions observable over time.
3. Sensor dropout	Prediction can continue without data, but calibrated uncertainty must grow.
4. Innovation consistency	NIS and coverage distinguish useful confidence from overconfidence or excessive conservatism.
5. EKF versus UKF	Sigma points can capture curvature that a Jacobian at the mean completely misses.
6. RTS smoothing	Future evidence improves past estimates but changes the information available to the estimator.

Connection to Latent-Anything¶

A state-estimation adapter should expose prior, posterior, covariance representation, innovation, NIS, timestamp, frame, and whether a state is filtered or smoothed. predict, update, and smooth should remain separate operations so runtime code cannot accidentally use future evidence or hide sensor corrections inside an apparently autonomous rollout.