Common Use Cases & Recipes

This guide provides ready-to-use code for common tracking and estimation scenarios. Each recipe is self-contained and can be adapted to your needs.

See Library Architecture for module organization and API Navigation Guide for function discovery.

A note on return types: the filter functions return named tuples, not plain pairs. kf_predict/ekf_predict/ukf_predict return a KalmanPrediction with fields x and P; kf_update/ekf_update/ukf_update return a KalmanUpdate with fields x, P, y, S, K, and likelihood. Unpack fields by name.

Basic Single-Target Kalman Filtering

Problem: Track a single object with position and velocity in 2D, given range/bearing measurements.

Recipe:

import numpy as np
from pytcl.dynamic_estimation.kalman import kf_predict, kf_update
from pytcl.dynamic_models import f_constant_velocity, q_constant_velocity
from pytcl.coordinate_systems.conversions import sphere2cart

class SimpleTracker:
    def __init__(self, T=0.1, sigma_a=0.1, meas_std=0.5):
        """
        T: time step (seconds)
        sigma_a: acceleration standard deviation (m/s^2)
        meas_std: measurement standard deviation (meters)
        """
        self.T = T

        # State: [x, vx, y, vy]
        self.x = np.array([100.0, 0.0, 0.0, 0.0])
        self.P = np.eye(4) * 10.0  # Initial uncertainty

        # Motion model (constant velocity)
        self.F = f_constant_velocity(T, num_dims=2)
        self.Q = q_constant_velocity(T, sigma_a, num_dims=2)

        # Measurement model (observe position only)
        self.H = np.array([[1.0, 0.0, 0.0, 0.0],
                           [0.0, 0.0, 1.0, 0.0]])
        self.R = np.eye(2) * meas_std**2

    def process_measurement(self, range_az_el):
        """
        Process a [range, azimuth, elevation] measurement (radians):
        convert to Cartesian and update the filter.
        """
        # Predict
        pred = kf_predict(self.x, self.P, self.F, self.Q)

        # Convert measurement from spherical to Cartesian; keep x, y.
        # 'az-el' puts az=0, el=0 on the +x axis (radar convention).
        r, az, el = range_az_el
        cart_meas = sphere2cart(r, az, el, "az-el")[:2]

        # Update
        upd = kf_update(pred.x, pred.P, cart_meas, self.H, self.R)
        self.x, self.P = upd.x, upd.P

        return self.x[[0, 2]], np.sqrt(np.diag(self.P)[[0, 2]])

# Usage
tracker = SimpleTracker()

# Process measurements over time
measurements = [
    np.array([100.0, 0.0, 0.0]),      # range=100, az=0, el=0
    np.array([100.5, 0.01, 0.0]),
    np.array([101.2, 0.02, 0.0]),
]

for meas in measurements:
    pos, std = tracker.process_measurement(meas)
    print(f"Position: {np.round(pos, 2)}, Std: {np.round(std, 3)}")

Output:

Position: [100.   0.], Std: [0.494 0.494]
Position: [100.29   0.58], Std: [0.381 0.381]
Position: [100.82   1.46], Std: [0.373 0.373]

Multi-Target Tracking with Assignment

Problem: Track multiple targets with ambiguous measurements using Global Nearest Neighbor (GNN).

Recipe: MultiTargetTracker takes the motion and measurement models up front, manages track initiation/confirmation/deletion internally, and is driven with process(measurements, dt):

import numpy as np
from pytcl.dynamic_models import f_constant_velocity, q_constant_velocity
from pytcl.trackers import MultiTargetTracker

T = 1.0

# State [x, vx, y, vy]; measurements are [x, y] positions
F = f_constant_velocity(T, num_dims=2)
Q = q_constant_velocity(T, sigma_a=0.5, num_dims=2)
H = np.array([[1.0, 0.0, 0.0, 0.0],
              [0.0, 0.0, 1.0, 0.0]])
R = np.eye(2)

tracker = MultiTargetTracker(
    state_dim=4,
    meas_dim=2,
    F=F,
    H=H,
    Q=Q,
    R=R,
    gate_probability=0.99,
    confirm_hits=3,     # hits needed to confirm a track...
    confirm_window=5,   # ...within this many updates
    max_misses=5,       # delete after this many consecutive misses
)

np.random.seed(0)
for frame in range(10):
    # Two real targets plus one clutter point per frame
    measurements = [
        np.array([100.0 + 5.0 * frame, 50.0]) + np.random.randn(2),
        np.array([200.0 - 3.0 * frame, 80.0 + 2.0 * frame]) + np.random.randn(2),
        np.random.uniform(0.0, 300.0, size=2),   # clutter
    ]
    tracker.process(measurements, dt=T)

# Confirmed tracks survive; clutter-born tracks never confirm
for track in tracker.confirmed_tracks:
    pos = track.state[[0, 2]]
    vel = track.state[[1, 3]]
    print(f"Track {track.id}: pos={np.round(pos, 1)}, "
          f"vel={np.round(vel, 1)}, hits={track.hits}")

Output:

Track 0: pos=[144.2  50. ], vel=[5.  0.3], hits=10
Track 1: pos=[172.   97.3], vel=[-3.2  1.7], hits=10

process returns the full track list (Track named tuples with id, state, covariance, status, hits, misses, and time); the confirmed_tracks property filters it to confirmed ones.

Extended Kalman Filter with Nonlinear Dynamics

Problem: Track with nonlinear motion (e.g., coordinated turn model).

Recipe:

import numpy as np
from pytcl.dynamic_estimation.kalman import ekf_predict, ekf_update

def coordinated_turn(x, T):
    """Coordinated turn transition; state [x, vx, y, vy, omega]."""
    px, vx, py, vy, omega = x
    if abs(omega) > 1e-6:
        s, c = np.sin(omega * T), np.cos(omega * T)
        return np.array([
            px + vx / omega * s - vy / omega * (1 - c),
            vx * c - vy * s,
            py + vx / omega * (1 - c) + vy / omega * s,
            vx * s + vy * c,
            omega,
        ])
    # Straight line when omega is approximately 0
    x_new = x.copy()
    x_new[0] += vx * T
    x_new[2] += vy * T
    return x_new

def coordinated_turn_jacobian(x, T):
    """Jacobian of the coordinated turn model."""
    px, vx, py, vy, omega = x
    F = np.eye(5)
    if abs(omega) > 1e-6:
        s, c = np.sin(omega * T), np.cos(omega * T)
        F[0, 1] = s / omega
        F[0, 3] = -(1 - c) / omega
        F[1, 1] = c
        F[1, 3] = -s
        F[2, 1] = (1 - c) / omega
        F[2, 3] = s / omega
        F[3, 1] = s
        F[3, 3] = c
        # Sensitivity to the turn rate
        F[0, 4] = vx / omega * (T * c - s / omega) - vy / omega * (T * s - (1 - c) / omega)
        F[1, 4] = -T * (vx * s + vy * c)
        F[2, 4] = vx / omega * (T * s - (1 - c) / omega) + vy / omega * (T * c - s / omega)
        F[3, 4] = T * (vx * c - vy * s)
    else:
        F[0, 1] = T
        F[2, 3] = T
    return F

class EKFTracker:
    def __init__(self, T=0.1):
        self.T = T

        # State: [x, vx, y, vy, omega]
        self.x = np.array([0.0, 10.0, 0.0, 5.0, 0.05])
        self.P = np.diag([1.0, 1.0, 1.0, 1.0, 0.01])

        self.Q = np.diag([0.01, 0.1, 0.01, 0.1, 1e-4])
        self.H = np.array([[1.0, 0.0, 0.0, 0.0, 0.0],
                           [0.0, 0.0, 1.0, 0.0, 0.0]])  # measure [x, y]
        self.R = np.eye(2) * 0.5**2

    def step(self, z):
        F = coordinated_turn_jacobian(self.x, self.T)
        pred = ekf_predict(
            self.x, self.P,
            lambda x: coordinated_turn(x, self.T),
            F, self.Q,
        )
        upd = ekf_update(
            pred.x, pred.P, z,
            lambda x: x[[0, 2]],
            self.H, self.R,
        )
        self.x, self.P = upd.x, upd.P
        return self.x

# Usage: simulate a turning target and track it
np.random.seed(1)
ekf = EKFTracker(T=0.1)
truth = np.array([0.0, 10.0, 0.0, 5.0, 0.1])

for k in range(100):
    truth = coordinated_turn(truth, 0.1)
    z = truth[[0, 2]] + np.random.randn(2) * 0.5
    est = ekf.step(z)

print(f"True position:      ({truth[0]:.1f}, {truth[2]:.1f})")
print(f"Estimated position: ({est[0]:.1f}, {est[2]:.1f})")
print(f"Estimated turn rate: {est[4]:.3f} (true 0.100)")

Output:

True position:      (61.2, 88.0)
Estimated position: (61.6, 88.2)
Estimated turn rate: 0.096 (true 0.100)

Unscented Kalman Filter

Problem: Track a nonlinear system without computing Jacobians.

Recipe:

import numpy as np
from pytcl.dynamic_estimation.kalman import ukf_predict, ukf_update

class UKFTracker:
    def __init__(self, T=0.1, alpha=1e-3, beta=2.0, kappa=0.0):
        """
        Unscented Kalman Filter (no Jacobians needed!)

        alpha, beta, kappa: sigma-point parameters
        """
        self.T = T
        self.alpha = alpha
        self.beta = beta
        self.kappa = kappa

        # State: [x, vx, y, vy]
        self.x = np.array([0.0, 1.0, 0.0, 0.0])
        self.P = np.eye(4) * 0.1

        self.Q = np.eye(4) * 0.01
        self.R = np.eye(2) * 0.5**2

    def motion_model(self, x, dt):
        """Nonlinear motion: could be any complex model"""
        x_new = x.copy()
        x_new[0] += x[1] * dt
        x_new[2] += x[3] * dt
        return x_new

    def measurement_model(self, x):
        """Measure position and speed"""
        return np.array([x[0], np.sqrt(x[1]**2 + x[3]**2)])

    def update(self, measurement):
        def f(x):
            return self.motion_model(x, self.T)

        pred = ukf_predict(
            self.x, self.P, f, self.Q,
            alpha=self.alpha, beta=self.beta, kappa=self.kappa,
        )
        upd = ukf_update(
            pred.x, pred.P, measurement, self.measurement_model, self.R,
            alpha=self.alpha, beta=self.beta, kappa=self.kappa,
        )

        self.x = upd.x
        self.P = upd.P

        return self.x[[0, 2]]

# Usage: measurements are [position, speed]
np.random.seed(2)
ukf = UKFTracker()
for k in range(5):
    z = np.array([0.1 * (k + 1), 1.0]) + np.random.randn(2) * 0.1
    pos = ukf.update(z)
    print(f"Position estimate: {np.round(pos, 3)}")

Output:

Position estimate: [0.086 0.   ]
Position estimate: [0.136 0.   ]
Position estimate: [ 0.205 -0.   ]
Position estimate: [ 0.329 -0.   ]
Position estimate: [ 0.413 -0.   ]

INS/GNSS Navigation

Problem: Fuse inertial measurements (IMU) with occasional GNSS updates.

Recipe: use the shipped strapdown mechanization and loose-coupled error-state filter from pytcl.navigation. All angles are in radians.

import numpy as np
from pytcl.navigation.ins import (
    IMUData,
    earth_rate_ned,
    gravity_ned,
    initialize_ins_state,
)
from pytcl.navigation.ins_gnss import (
    GNSSMeasurement,
    initialize_ins_gnss,
    loose_coupled_predict,
    loose_coupled_update_position,
    position_std_to_error_state_units,
)

# Initial state: stationary, level, facing north
lat0, lon0, alt0 = np.radians(40.0), np.radians(-74.0), 100.0
ins0 = initialize_ins_state(lat=lat0, lon=lon0, alt=alt0)
state = initialize_ins_gnss(ins0, position_std=10.0, velocity_std=1.0)

# High-rate IMU (100 Hz for 1 s). A stationary, level IMU senses the
# gravity reaction on the accelerometers and Earth rotation on the
# gyros; here we corrupt the accelerometer with a small bias.
dt = 0.01
accel_true = -gravity_ned(lat0, alt0)     # specific force, NED
accel_bias = np.array([0.05, 0.0, 0.0])   # 0.05 m/s^2 north bias
gyro_true = earth_rate_ned(lat0)

for _ in range(100):
    imu = IMUData(accel=accel_true + accel_bias, gyro=gyro_true, dt=dt)
    state = loose_coupled_predict(state, imu)

def north_error_m(state):
    return (state.ins_state.position[0] - lat0) * 6.378e6

print(f"INS-only north drift after 1 s: {north_error_m(state):.3f} m")

# Low-rate GNSS position fix (lat, lon, alt) pulls the error back.
# The innovation is [dlat, dlon, dheight] in (rad, rad, m), so a
# position accuracy quoted in meters must be converted for the
# horizontal components:
horiz = position_std_to_error_state_units(5.0, lat0, alt0)  # 5 m accuracy
gnss = GNSSMeasurement(
    position=np.array([lat0, lon0, alt0]),
    velocity=None,
    position_cov=np.diag([horiz[0]**2, horiz[1]**2, 10.0**2]),
    velocity_cov=None,
    time=1.0,
)
result = loose_coupled_update_position(state, gnss)
state = result.state

print(f"North error after GNSS update:  {north_error_m(state):.3f} m")

Output:

INS-only north drift after 1 s: 0.025 m
North error after GNSS update:  0.005 m

For velocity fixes use loose_coupled_update_velocity, and for combined position/velocity fixes use loose_coupled_update. Tightly-coupled (pseudorange) updates are available via tight_coupled_update.

Particle Filter for Nonlinear Non-Gaussian Systems

Problem: Track a system with non-Gaussian noise or highly nonlinear dynamics.

Recipe: bootstrap_pf_step performs predict + weight update + adaptive resampling in one call. The process noise is specified as a sampler, so any distribution works – here heavy-tailed Cauchy noise, which no Kalman filter can represent:

import numpy as np
from pytcl.dynamic_estimation.particle_filters import (
    bootstrap_pf_step,
    initialize_particles,
    particle_mean,
)

rng = np.random.default_rng(42)

# State [x, vx, y, vy]; measurement is the range from the origin
x0 = np.array([10.0, 1.0, 5.0, 0.0])
P0 = np.eye(4)
state = initialize_particles(x0, P0, N=2000, rng=rng)

dt = 0.1

def f(x):
    """Constant-velocity motion (per particle)."""
    return np.array([x[0] + x[1] * dt, x[1], x[2] + x[3] * dt, x[3]])

def h(x):
    """Nonlinear measurement: distance from origin."""
    return np.array([np.hypot(x[0], x[2])])

def q_sample(n, rng):
    """Heavy-tailed (Cauchy) process noise."""
    return rng.standard_cauchy((n, 4)) * 0.05

R = np.array([[0.5**2]])

true_x = x0.copy()
for k in range(10):
    true_x = f(true_x)
    z = h(true_x) + rng.normal(0.0, 0.5, 1)
    state = bootstrap_pf_step(
        state.particles, state.weights, z, f, h, q_sample, R, rng=rng,
    )

estimate = particle_mean(state.particles, state.weights)
print(f"True range: {h(true_x)[0]:.2f}, estimated range: {h(estimate)[0]:.2f}")

Output:

True range: 12.08, estimated range: 11.65

Resampling happens automatically when the effective sample size drops below resample_threshold * N; particle_covariance and effective_sample_size are available from the same module.

Adaptive Kalman Filtering

Problem: Track when the measurement noise varies over time.

Recipe:

import numpy as np
from pytcl.dynamic_estimation.kalman import kf_predict, kf_update

class AdaptiveKalmanFilter:
    def __init__(self, T=0.1, sigma_a=0.1):
        # State: [x, vx, y, vy]
        self.x = np.array([0.0, 1.0, 0.0, 1.0])
        self.P = np.eye(4) * 0.1

        self.F = np.array([
            [1, T, 0, 0],
            [0, 1, 0, 0],
            [0, 0, 1, T],
            [0, 0, 0, 1],
        ], dtype=float)
        self.Q = np.eye(4) * sigma_a**2
        self.H = np.array([[1.0, 0.0, 0.0, 0.0],
                           [0.0, 0.0, 1.0, 0.0]])  # measure [x, y]
        self.R = np.eye(2) * 0.5**2

        # Measurement noise adaptation
        self.innovation_history = []
        self.max_history = 30

    def update(self, measurement):
        # Predict
        pred = kf_predict(self.x, self.P, self.F, self.Q)

        # Compute innovation
        innovation = measurement - self.H @ pred.x
        self.innovation_history.append(np.linalg.norm(innovation))

        # Keep history bounded
        if len(self.innovation_history) > self.max_history:
            self.innovation_history = self.innovation_history[1:]

        # Adapt measurement noise based on innovation statistics
        if len(self.innovation_history) >= 10:
            mean_innovation = np.mean(self.innovation_history)
            std_innovation = np.std(self.innovation_history)

            # Inflate R if innovations get erratic (measurement unreliable)
            if std_innovation > mean_innovation * 2:
                self.R = np.eye(2) * mean_innovation**2
            else:
                self.R = np.eye(2) * 0.5**2  # Default

        # Update with adapted R
        upd = kf_update(pred.x, pred.P, measurement, self.H, self.R)
        self.x, self.P = upd.x, upd.P

        return self.x[[0, 2]]

# Usage
np.random.seed(3)
adaptive_kf = AdaptiveKalmanFilter()

for k in range(20):
    z = np.array([0.1 * (k + 1), 0.1 * (k + 1)]) + np.random.randn(2) * 0.5
    pos = adaptive_kf.update(z)

print(f"Final position estimate: {np.round(pos, 2)}")

Output:

Final position estimate: [1.91 1.72]

Batch Processing / Smoothing

Problem: Process all data offline to get the best parameter estimates.

Recipe: fit a constant-velocity trajectory to all measurements at once with ordinary_least_squares:

import numpy as np
from pytcl.static_estimation import ordinary_least_squares

# Truth: [x0, vx, y0, vy]
truth = np.array([100.0, 5.0, 50.0, -2.0])

np.random.seed(4)
T = 0.1
times = np.arange(20) * T
measurements = np.column_stack([
    truth[0] + truth[1] * times,
    truth[2] + truth[3] * times,
]) + np.random.randn(20, 2) * 0.5

# Linear model: z = A @ [x0, vx, y0, vy]
A = np.zeros((2 * len(times), 4))
A[0::2, 0] = 1.0
A[0::2, 1] = times
A[1::2, 2] = 1.0
A[1::2, 3] = times
b = measurements.ravel()

result = ordinary_least_squares(A, b)
print(f"Estimated [x0, vx, y0, vy]: {np.round(result.x, 2)}")

Output:

Estimated [x0, vx, y0, vy]: [99.83  5.26 49.98 -2.01]

ordinary_least_squares returns an LSResult with fields x, residuals, rank, and singular_values. For recursive smoothing of dynamic systems (RTS and two-filter smoothers), see Smoothing Algorithms & Offline Estimation.

Data Association with Gating

Problem: Only associate measurements within a statistical gate.

Recipe: pytcl ships the gating utilities – chi2_gate_threshold for the gate size, mahalanobis_distance for the squared statistical distance, and gnn_association to solve the gated assignment:

import numpy as np
from pytcl.assignment_algorithms import (
    chi2_gate_threshold,
    gnn_association,
    mahalanobis_distance,
)

# Two predicted track positions with innovation covariances
track_positions = [np.array([10.0, 0.0]), np.array([0.0, 20.0])]
S_list = [np.eye(2) * 2.0, np.eye(2) * 2.0]

measurements = [
    np.array([10.5, 0.3]),    # near track 0
    np.array([-0.2, 19.5]),   # near track 1
    np.array([50.0, 50.0]),   # clutter
]

# 99% gate for a 2-dimensional measurement
gate = chi2_gate_threshold(0.99, num_dimensions=2)
print(f"Gate threshold: {gate:.2f}")

# Cost matrix of squared Mahalanobis distances
cost = np.zeros((len(track_positions), len(measurements)))
for i, (pred, S) in enumerate(zip(track_positions, S_list)):
    for j, z in enumerate(measurements):
        cost[i, j] = mahalanobis_distance(z - pred, S)

# Gated global-nearest-neighbor assignment
assoc = gnn_association(cost, gate_threshold=gate)
print(f"Track-to-measurement assignment: {assoc.track_to_measurement}")

Output:

Gate threshold: 9.21
Track-to-measurement assignment: [0 1]

The clutter point exceeds the gate for both tracks, so it is left unassigned. For k-best assignments use kbest_assign2d (Murty’s algorithm), and for higher-dimensional (multi-scan) assignment problems see assign3d and assignment_nd.

See Also

• Library Architecture - Module organization

• API Navigation Guide - Function discovery

• Kalman Filter Tuning Guide - Parameter tuning

• Performance Optimization Guide - Performance tips

• examples/ - More complete examples