Filtering and State Estimation

This guide covers the filtering algorithms available in the library.

Kalman Filter Family

Linear Kalman Filter

The standard Kalman filter is optimal for linear-Gaussian systems:

\[ \begin{align}\begin{aligned}x_{k+1} = F x_k + w_k, \quad w_k \sim \mathcal{N}(0, Q)\\z_k = H x_k + v_k, \quad v_k \sim \mathcal{N}(0, R)\end{aligned}\end{align} \]

Prediction step:

import numpy as np

from pytcl.dynamic_estimation import kf_predict

# Constant-velocity state [position, velocity]
x = np.array([0.0, 1.0])
P = np.eye(2) * 0.5
F = np.array([[1.0, 0.1], [0.0, 1.0]])
Q = np.eye(2) * 0.01

prediction = kf_predict(x, P, F, Q)
x_pred = prediction.x
P_pred = prediction.P

Update step:

from pytcl.dynamic_estimation import kf_update

# z: measurement, H: measurement matrix, R: measurement noise
z = np.array([0.12])
H = np.array([[1.0, 0.0]])
R = np.array([[0.1]])

update = kf_update(x_pred, P_pred, z, H, R)
x_upd = update.x
P_upd = update.P

kf_update returns a NamedTuple with six fields: x (updated state), P (updated covariance), y (innovation), S (innovation covariance), K (Kalman gain), and likelihood (measurement likelihood).

Combined predict-update:

from pytcl.dynamic_estimation import kf_predict_update

update = kf_predict_update(x, P, z, F, Q, H, R)

Extended Kalman Filter (EKF)

For nonlinear systems, the EKF linearizes around the current estimate. ekf_predict and ekf_update take the Jacobian evaluated at the linearization point (a matrix, not a callable):

from pytcl.dynamic_estimation import ekf_predict, ekf_update

# Define nonlinear dynamics and Jacobian
def f(x):
    # Nonlinear state transition
    return np.array([x[0] + x[1], x[1] * 0.99])

def F_jacobian(x):
    return np.array([[1, 1], [0, 0.99]])

# Predict (Jacobian evaluated at the current estimate)
pred = ekf_predict(x, P, f, F_jacobian(x), Q)

# Define nonlinear measurement and Jacobian
def h(x):
    # Range measurement
    return np.array([np.sqrt(x[0]**2 + x[1]**2)])

def H_jacobian(x):
    r = np.sqrt(x[0]**2 + x[1]**2)
    return np.array([[x[0]/r, x[1]/r]])

# Update (Jacobian evaluated at the predicted state)
upd = ekf_update(pred.x, pred.P, z, h, H_jacobian(pred.x), R)

Automatic Jacobian computation:

from pytcl.dynamic_estimation import ekf_predict_auto, ekf_update_auto

# Uses numerical differentiation
pred = ekf_predict_auto(x, P, f, Q)
upd = ekf_update_auto(pred.x, pred.P, z, h, R)

Unscented Kalman Filter (UKF)

The UKF uses sigma points to capture the mean and covariance through nonlinear transformations:

from pytcl.dynamic_estimation import ukf_predict, ukf_update

# No Jacobians needed!
pred = ukf_predict(x, P, f, Q)
upd = ukf_update(pred.x, pred.P, z, h, R)

Cubature Kalman Filter (CKF)

The CKF uses spherical-radial cubature points, providing a good balance between accuracy and computational cost:

from pytcl.dynamic_estimation import ckf_predict, ckf_update

pred = ckf_predict(x, P, f, Q)
upd = ckf_update(pred.x, pred.P, z, h, R)

Particle Filters

For non-Gaussian or highly nonlinear systems, particle filters provide a flexible Monte Carlo approach:

from pytcl.dynamic_estimation import (
    initialize_particles,
    bootstrap_pf_step,
    particle_mean,
    particle_covariance,
)

# Initialize particles from prior
x0 = np.array([0.0, 1.0])
P0 = np.eye(2) * 0.5
state = initialize_particles(x0, P0, N=1000)

# Define process noise sampler
def Q_sample(N, rng):
    return rng.multivariate_normal(np.zeros(2), Q, size=N)

# Run filter step
state = bootstrap_pf_step(
    state.particles, state.weights,
    z, f, h, Q_sample, R,
    resample_method="systematic"
)

# Extract estimates
x_est = particle_mean(state.particles, state.weights)
P_est = particle_covariance(state.particles, state.weights)

Smoothing

The library provides RTS (Rauch-Tung-Striebel) smoothing for obtaining optimal estimates using future measurements. kf_smooth performs one backward step at a time, so run it in a loop over the stored forward filter results:

from pytcl.dynamic_estimation import kf_smooth

# Forward pass: store filtered and predicted results
zs = [np.array([0.1]), np.array([0.25]), np.array([0.4])]
x_filt, P_filt = [], []
x_pred_list, P_pred_list = [], []
xk, Pk = x0, P0
for z_k in zs:
    pred = kf_predict(xk, Pk, F, Q)
    x_pred_list.append(pred.x)
    P_pred_list.append(pred.P)
    upd = kf_update(pred.x, pred.P, z_k, H, R)
    xk, Pk = upd.x, upd.P
    x_filt.append(xk)
    P_filt.append(Pk)

# Backward pass: one RTS step per time index
n = len(zs)
x_smooth = [None] * n
P_smooth = [None] * n
x_smooth[-1], P_smooth[-1] = x_filt[-1], P_filt[-1]
for k in range(n - 2, -1, -1):
    x_smooth[k], P_smooth[k] = kf_smooth(
        x_filt[k], P_filt[k],
        x_pred_list[k + 1], P_pred_list[k + 1],
        x_smooth[k + 1], P_smooth[k + 1],
        F,
    )

Information Filter

The information filter is the dual of the Kalman filter, working with the information matrix (inverse covariance):

from pytcl.dynamic_estimation import (
    information_filter_predict,
    information_filter_update,
)

# Work with information form: y = P^{-1} x, Y = P^{-1}
Y = np.linalg.inv(P)
y = Y @ x

y_pred, Y_pred = information_filter_predict(y, Y, F, Q)
y_upd, Y_upd = information_filter_update(y_pred, Y_pred, z, H, R)

Square-Root Kalman Filters

Square-root filters propagate the Cholesky factor of the covariance matrix instead of the covariance itself. This provides improved numerical stability and guarantees positive semi-definiteness.

Square-Root Kalman Filter (SRKF)

from pytcl.dynamic_estimation import srkf_predict, srkf_update
import numpy as np

# Initialize with Cholesky factors instead of covariances
x = np.array([0.0, 1.0, 0.0, 0.5])
P = np.eye(4) * 0.1
S = np.linalg.cholesky(P)  # S @ S.T = P

# System matrices
F = np.array([[1, 0.1, 0, 0], [0, 1, 0, 0],
              [0, 0, 1, 0.1], [0, 0, 0, 1]])
Q = np.eye(4) * 0.01
S_Q = np.linalg.cholesky(Q)

# Prediction
pred = srkf_predict(x, S, F, S_Q)
x_pred, S_pred = pred.x, pred.S

# Update
z = np.array([0.1, 0.05])
H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
R = np.eye(2) * 0.1
S_R = np.linalg.cholesky(R)

upd = srkf_update(x_pred, S_pred, z, H, S_R)
x_upd, S_upd = upd.x, upd.S

# Reconstruct covariance if needed
P_upd = S_upd @ S_upd.T

U-D Factorization Filter

Bierman’s U-D filter uses a different factorization: P = U @ D @ U.T where U is unit upper triangular and D is diagonal.

from pytcl.dynamic_estimation import (
    ud_factorize, ud_reconstruct,
    ud_predict, ud_update
)

# Factorize initial covariance
P = np.diag([0.5, 1.0, 0.5, 1.0])
U, D = ud_factorize(P)

# Prediction
x_pred, U_pred, D_pred = ud_predict(x, U, D, F, Q)

# Update
x_upd, U_upd, D_upd, innovation, likelihood = ud_update(
    x_pred, U_pred, D_pred, z, H, R
)

# Reconstruct covariance if needed
P_upd = ud_reconstruct(U_upd, D_upd)

Square-Root UKF

The square-root UKF combines the benefits of the UKF (no Jacobians needed) with numerical stability of square-root formulations.

from pytcl.dynamic_estimation import sr_ukf_predict, sr_ukf_update

# Nonlinear state transition
def f(x):
    return np.array([x[0] + x[1] * 0.1, x[1] * 0.99])

# Nonlinear measurement
def h(x):
    return np.array([np.sqrt(x[0]**2 + x[1]**2)])

# Two-state setup with Cholesky factors
x = np.array([0.0, 1.0])
S = np.linalg.cholesky(np.eye(2) * 0.5)
S_Q = np.linalg.cholesky(np.eye(2) * 0.01)
S_R = np.linalg.cholesky(np.array([[0.1]]))
z = np.array([0.9])

# Predict and update
pred = sr_ukf_predict(x, S, f, S_Q)
upd = sr_ukf_update(pred.x, pred.S, z, h, S_R)

Interacting Multiple Model (IMM) Estimator

The IMM estimator handles systems that can switch between multiple dynamic models. Each model represents a different motion mode (e.g., constant velocity vs. maneuvering).

Basic IMM Usage

from pytcl.dynamic_estimation import imm_predict, imm_update
from pytcl.dynamic_models import f_coord_turn_2d

# Two modes: constant velocity (CV) and coordinated turn (CT)
x = np.array([0.0, 10.0, 0.0, 5.0])  # [x, vx, y, vy]
P = np.eye(4) * 1.0

# Mode probabilities and transition matrix
mu = np.array([0.9, 0.1])  # Start in CV mode
Pi = np.array([[0.95, 0.05],   # CV -> CV, CV -> CT
               [0.10, 0.90]])  # CT -> CV, CT -> CT

# Model-specific dynamics
dt = 0.1
F_cv = np.array([[1, dt, 0, 0], [0, 1, 0, 0],
                 [0, 0, 1, dt], [0, 0, 0, 1]])
F_ct = f_coord_turn_2d(T=dt, omega=0.2)  # Coordinated turn model

Q_cv = np.eye(4) * 0.01
Q_ct = np.eye(4) * 0.1  # Higher uncertainty for maneuvering

# Predict
pred = imm_predict(
    [x, x], [P, P], mu, Pi,
    [F_cv, F_ct], [Q_cv, Q_ct]
)

# Update
z = np.array([0.5, 0.3])
H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
R = np.eye(2) * 0.1

upd = imm_update(
    pred.mode_states, pred.mode_covs, pred.mode_probs,
    z, [H, H], [R, R]
)

# Combined estimate
x_est = upd.x
P_est = upd.P
mode_probs = upd.mode_probs  # Current mode probabilities

IMMEstimator Class

For stateful IMM filtering:

from pytcl.dynamic_estimation import IMMEstimator

# Initialize
Pi = np.array([[0.95, 0.05], [0.10, 0.90]])
imm = IMMEstimator(n_modes=2, state_dim=4, transition_matrix=Pi)

imm.initialize(x, P)
imm.set_mode_model(0, F_cv, Q_cv)
imm.set_mode_model(1, F_ct, Q_ct)
imm.set_measurement_model(H, R)

# Filter loop
measurements = [np.array([1.0, 0.5]), np.array([2.1, 1.1])]
for z in measurements:
    result = imm.predict_update(z)
    print(f"State: {result.x}, Mode probs: {imm.mode_probs}")