Dynamic Estimation

Dynamic state estimation algorithms.

This module provides filtering and smoothing algorithms for state estimation: - Kalman filter family (KF, EKF, UKF, CKF, etc.) - Square-root Kalman filters (numerically stable) - Interacting Multiple Model (IMM) estimator - Particle filters (bootstrap) - Smoothers (RTS, fixed-lag, fixed-interval, two-filter) - Information filters (standard and square-root)

Kalman Filters

Kalman filter family implementations.

This module provides: - Linear Kalman filter (predict, update, smoothing) - Extended Kalman filter (EKF) - Constrained Extended Kalman filter (CEKF) - Unscented Kalman filter (UKF) - Cubature Kalman filter (CKF) - Information filter - Square-root Kalman filters (numerically stable) - U-D factorization filter (Bierman’s method) - H-infinity filter (robust filtering)

Linear Kalman Filter

Linear Kalman filter implementation.

This module provides the standard linear Kalman filter for systems with linear dynamics and linear measurements with Gaussian noise.

class pytcl.dynamic_estimation.kalman.linear.KalmanState(x, P)

Bases: NamedTuple

State of a Kalman filter.

Variables:

• x (ndarray) – State estimate.

• P (ndarray) – State covariance.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

class pytcl.dynamic_estimation.kalman.linear.KalmanPrediction(x, P)

Bases: NamedTuple

Result of Kalman filter prediction step.

Variables:

• x (ndarray) – Predicted state estimate.

• P (ndarray) – Predicted state covariance.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

class pytcl.dynamic_estimation.kalman.linear.KalmanUpdate(x, P, y, S, K, likelihood)

Bases: NamedTuple

Result of Kalman filter update step.

Variables:

• x (ndarray) – Updated state estimate.

• P (ndarray) – Updated state covariance.

• y (ndarray) – Innovation (measurement residual).

• S (ndarray) – Innovation covariance.

• K (ndarray) – Kalman gain.

• likelihood (float) – Measurement likelihood (for association).

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

y: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

S: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 3

K: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 4

likelihood: float

Alias for field number 5

pytcl.dynamic_estimation.kalman.linear.kf_predict(x, P, F, Q, B=None, u=None)

Kalman filter prediction (time update) step.

Computes the predicted state and covariance:

x_pred = F @ x + B @ u P_pred = F @ P @ F’ + Q

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• P (array_like) – Current state covariance, shape (n, n).

• F (array_like) – State transition matrix, shape (n, n).

• Q (array_like) – Process noise covariance, shape (n, n).

• B (array_like, optional) – Control input matrix, shape (n, m).

• u (array_like, optional) – Control input, shape (m,).

Returns:

result – Named tuple with predicted state x and covariance P.

Return type:

KalmanPrediction

Examples

>>> import numpy as np
>>> x = np.array([0.0, 1.0])  # position=0, velocity=1
>>> P = np.eye(2) * 0.1
>>> F = np.array([[1, 1], [0, 1]])  # CV model, T=1
>>> Q = np.array([[0.25, 0.5], [0.5, 1.0]])
>>> pred = kf_predict(x, P, F, Q)
>>> pred.x
array([1., 1.])

See also

kf_update

Measurement update step.

pytcl.dynamic_estimation.kalman.linear.kf_update(x, P, z, H, R)

Kalman filter update (measurement update) step.

Computes the updated state and covariance given a measurement:

y = z - H @ x (innovation) S = H @ P @ H’ + R (innovation covariance) K = P @ H’ @ S^{-1} (Kalman gain) x_upd = x + K @ y (updated state) P_upd = (I - K @ H) @ P (updated covariance)

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• P (array_like) – Predicted state covariance, shape (n, n).

• z (array_like) – Measurement, shape (m,).

• H (array_like) – Measurement matrix, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m).

Returns:

result – Named tuple with updated state, covariance, innovation, etc.

Return type:

KalmanUpdate

Examples

>>> import numpy as np
>>> x = np.array([1.0, 1.0])
>>> P = np.array([[0.35, 0.5], [0.5, 1.1]])
>>> z = np.array([1.2])  # position measurement
>>> H = np.array([[1, 0]])
>>> R = np.array([[0.1]])
>>> upd = kf_update(x, P, z, H, R)
>>> upd.x  # updated state
array([1.15..., 1.22...])

See also

kf_predict

Time prediction step.

pytcl.dynamic_estimation.kalman.linear.kf_predict_update(x, P, z, F, Q, H, R, B=None, u=None)

Combined Kalman filter prediction and update step.

Parameters:

• x (array_like) – Current state estimate.

• P (array_like) – Current state covariance.

• z (array_like) – Measurement.

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

• H (array_like) – Measurement matrix.

• R (array_like) – Measurement noise covariance.

• B (array_like, optional) – Control input matrix.

• u (array_like, optional) – Control input.

Returns:

result – Updated state and covariance with innovation statistics.

Return type:

KalmanUpdate

Examples

Track a 1D constant-velocity target with position measurements:

>>> import numpy as np
>>> x = np.array([0.0, 1.0])  # [position, velocity]
>>> P = np.eye(2) * 0.1
>>> F = np.array([[1, 1], [0, 1]])  # CV model, T=1s
>>> Q = np.array([[0.25, 0.5], [0.5, 1.0]])  # process noise
>>> H = np.array([[1, 0]])  # measure position only
>>> R = np.array([[0.5]])  # measurement noise variance
>>> z = np.array([1.1])  # measured position
>>> result = kf_predict_update(x, P, z, F, Q, H, R)
>>> result.x  # updated state
array([1.0..., 1.0...])

See also

kf_predict

Prediction step only.

kf_update

Update step only.

pytcl.dynamic_estimation.kalman.linear.kf_smooth(x_filt, P_filt, x_pred, P_pred, x_smooth_next, P_smooth_next, F)

Rauch-Tung-Striebel (RTS) smoother backward step.

Given forward filter results and next smoothed state, compute current smoothed state.

Parameters:

• x_filt (array_like) – Filtered state at current time.

• P_filt (array_like) – Filtered covariance at current time.

• x_pred (array_like) – Predicted state at next time (from filter).

• P_pred (array_like) – Predicted covariance at next time.

• x_smooth_next (array_like) – Smoothed state at next time.

• P_smooth_next (array_like) – Smoothed covariance at next time.

• F (array_like) – State transition matrix.

Returns:

• x_smooth (ndarray) – Smoothed state at current time.

• P_smooth (ndarray) – Smoothed covariance at current time.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Notes

The RTS smoother runs backward through the filter results:

G = P_filt @ F’ @ P_pred^{-1} x_smooth = x_filt + G @ (x_smooth_next - x_pred) P_smooth = P_filt + G @ (P_smooth_next - P_pred) @ G’

Examples

Apply RTS smoothing to improve a filtered estimate:

>>> import numpy as np
>>> # Filtered estimate at time k
>>> x_filt = np.array([1.0, 0.9])
>>> P_filt = np.array([[0.1, 0.05], [0.05, 0.2]])
>>> # Predicted estimate at time k+1 (from forward pass)
>>> x_pred = np.array([1.9, 0.9])
>>> P_pred = np.array([[0.35, 0.25], [0.25, 0.4]])
>>> # Smoothed estimate at time k+1 (already computed)
>>> x_smooth_next = np.array([2.0, 1.0])
>>> P_smooth_next = np.array([[0.08, 0.03], [0.03, 0.15]])
>>> F = np.array([[1, 1], [0, 1]])  # CV model
>>> x_s, P_s = kf_smooth(x_filt, P_filt, x_pred, P_pred,
...                      x_smooth_next, P_smooth_next, F)
>>> x_s  # smoothed state at time k
array([1.0..., 0.9...])

See also

kf_predict_update

Forward filtering step.

pytcl.dynamic_estimation.kalman.linear.information_filter_predict(y, Y, F, Q)

Information filter prediction step.

The information filter uses the information matrix Y = P^{-1} and information vector y = P^{-1} @ x instead of P and x.

Parameters:

• y (array_like) – Information vector (Y @ x).

• Y (array_like) – Information matrix (P^{-1}).

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

Returns:

• y_pred (ndarray) – Predicted information vector.

• Y_pred (ndarray) – Predicted information matrix.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> import numpy as np
>>> # Convert from state form to information form
>>> x = np.array([1.0, 0.5])
>>> P = np.eye(2) * 0.1
>>> Y = np.linalg.inv(P)
>>> y = Y @ x
>>> # Predict with constant velocity model
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.01
>>> y_pred, Y_pred = information_filter_predict(y, Y, F, Q)
>>> y_pred.shape
(2,)

Notes

The prediction requires converting to state space, predicting, and converting back (information form prediction is complex).

pytcl.dynamic_estimation.kalman.linear.information_filter_update(y, Y, z, H, R)

Information filter update step.

Parameters:

• y (array_like) – Predicted information vector.

• Y (array_like) – Predicted information matrix.

• z (array_like) – Measurement.

• H (array_like) – Measurement matrix.

• R (array_like) – Measurement noise covariance.

Returns:

• y_upd (ndarray) – Updated information vector.

• Y_upd (ndarray) – Updated information matrix.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> import numpy as np
>>> # Initial information form (from prior)
>>> Y = np.eye(2) * 10.0  # High prior information
>>> y = np.array([10.0, 5.0])  # Corresponds to state [1.0, 0.5]
>>> # Measurement z = H @ x + noise
>>> z = np.array([1.1])
>>> H = np.array([[1, 0]])  # Measure position only
>>> R = np.array([[0.1]])
>>> y_upd, Y_upd = information_filter_update(y, Y, z, H, R)
>>> Y_upd[0, 0] > Y[0, 0]  # Information increased
True

Notes

The update in information form is additive:

Y_upd = Y + H’ @ R^{-1} @ H y_upd = y + H’ @ R^{-1} @ z

Extended Kalman Filter

Extended Kalman Filter (EKF) implementation.

The EKF handles nonlinear dynamics and/or measurements by linearizing around the current state estimate using Jacobians.

pytcl.dynamic_estimation.kalman.extended.ekf_predict(x, P, f, F, Q)

Extended Kalman filter prediction step.

Uses nonlinear dynamics f(x) with Jacobian F for covariance propagation.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• P (array_like) – Current state covariance, shape (n, n).

• f (callable) – Nonlinear state transition function f(x) -> x_next.

• F (array_like) – Jacobian of f evaluated at x, shape (n, n).

• Q (array_like) – Process noise covariance, shape (n, n).

Returns:

result – Predicted state and covariance.

Return type:

KalmanPrediction

Examples

>>> import numpy as np
>>> # Coordinated turn dynamics
>>> def f_turn(x, omega=0.1, T=1.0):
...     px, vx, py, vy = x
...     return np.array([
...         px + np.sin(omega*T)/omega * vx - (1-np.cos(omega*T))/omega * vy,
...         np.cos(omega*T) * vx - np.sin(omega*T) * vy,
...         py + (1-np.cos(omega*T))/omega * vx + np.sin(omega*T)/omega * vy,
...         np.sin(omega*T) * vx + np.cos(omega*T) * vy,
...     ])
>>> x = np.array([0, 10, 0, 0])
>>> P = np.eye(4)
>>> F = np.eye(4)  # Jacobian (simplified)
>>> Q = np.eye(4) * 0.1
>>> pred = ekf_predict(x, P, f_turn, F, Q)

See also

ekf_update

EKF measurement update.

pytcl.dynamic_estimation.kalman.extended.ekf_update(x, P, z, h, H, R)

Extended Kalman filter update step.

Uses nonlinear measurement function h(x) with Jacobian H.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• P (array_like) – Predicted state covariance, shape (n, n).

• z (array_like) – Measurement, shape (m,).

• h (callable) – Nonlinear measurement function h(x) -> z.

• H (array_like) – Jacobian of h evaluated at x, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m).

Returns:

result – Updated state, covariance, and innovation statistics.

Return type:

KalmanUpdate

Examples

>>> import numpy as np
>>> # Range-bearing measurement
>>> def h_rb(x):
...     px, vx, py, vy = x
...     r = np.sqrt(px**2 + py**2)
...     theta = np.arctan2(py, px)
...     return np.array([r, theta])
>>> x = np.array([100, 10, 50, 5])
>>> P = np.eye(4)
>>> z = np.array([112, 0.46])
>>> H = np.array([[0.89, 0, 0.45, 0], [-0.004, 0, 0.008, 0]])
>>> R = np.diag([1.0, 0.01])
>>> upd = ekf_update(x, P, z, h_rb, H, R)

See also

ekf_predict

EKF prediction step.

pytcl.dynamic_estimation.kalman.extended.numerical_jacobian(f, x, dx=1e-07)

Compute Jacobian numerically using central differences.

Parameters:

• f (callable) – Function f(x) -> y.

• x (array_like) – Point at which to evaluate Jacobian.

• dx (float, optional) – Step size for finite differences.

Returns:

J – Jacobian matrix of shape (m, n) where m = len(f(x)), n = len(x).

Return type:

ndarray

Examples

>>> import numpy as np
>>> def f(x):
...     return np.array([x[0]**2, x[0]*x[1]])
>>> x = np.array([2.0, 3.0])
>>> J = numerical_jacobian(f, x)
>>> J  # [[2*x[0], 0], [x[1], x[0]]] = [[4, 0], [3, 2]]
array([[4., 0.],
       [3., 2.]])

pytcl.dynamic_estimation.kalman.extended.ekf_predict_auto(x, P, f, Q, dx=1e-07)

EKF prediction with automatic Jacobian computation.

Parameters:

• x (array_like) – Current state estimate.

• P (array_like) – Current state covariance.

• f (callable) – Nonlinear state transition function.

• Q (array_like) – Process noise covariance.

• dx (float, optional) – Step size for numerical Jacobian.

Returns:

result – Predicted state and covariance.

Return type:

KalmanPrediction

Examples

>>> import numpy as np
>>> # Simple nonlinear dynamics
>>> def f(x):
...     return np.array([x[0] + x[1], 0.9 * x[1]])
>>> x = np.array([0.0, 1.0])
>>> P = np.eye(2) * 0.1
>>> Q = np.eye(2) * 0.01
>>> pred = ekf_predict_auto(x, P, f, Q)
>>> pred.x
array([1. , 0.9])

See also

ekf_predict

EKF prediction with explicit Jacobian.

pytcl.dynamic_estimation.kalman.extended.ekf_update_auto(x, P, z, h, R, dx=1e-07)

EKF update with automatic Jacobian computation.

Parameters:

• x (array_like) – Predicted state estimate.

• P (array_like) – Predicted state covariance.

• z (array_like) – Measurement.

• h (callable) – Nonlinear measurement function.

• R (array_like) – Measurement noise covariance.

• dx (float, optional) – Step size for numerical Jacobian.

Returns:

result – Updated state and covariance.

Return type:

KalmanUpdate

Examples

>>> import numpy as np
>>> # Range measurement h(x) = sqrt(x[0]^2 + x[1]^2)
>>> def h(x):
...     return np.array([np.sqrt(x[0]**2 + x[1]**2)])
>>> x = np.array([3.0, 4.0])  # predicted position
>>> P = np.eye(2) * 0.5
>>> z = np.array([5.1])  # measured range
>>> R = np.array([[0.1]])
>>> upd = ekf_update_auto(x, P, z, h, R)
>>> np.linalg.norm(upd.x)  # estimate moves toward measurement
5.0...

See also

ekf_update

EKF update with explicit Jacobian.

pytcl.dynamic_estimation.kalman.extended.iterated_ekf_update(x, P, z, h, H_func, R, max_iter=10, tol=1e-06)

Iterated Extended Kalman Filter (IEKF) update.

The IEKF iteratively relinearizes around the updated estimate to improve accuracy for highly nonlinear measurements.

Parameters:

• x (array_like) – Predicted state estimate.

• P (array_like) – Predicted state covariance.

• z (array_like) – Measurement.

• h (callable) – Nonlinear measurement function h(x) -> z.

• H_func (callable) – Function returning Jacobian H(x) at given state.

• R (array_like) – Measurement noise covariance.

• max_iter (int, optional) – Maximum iterations.

• tol (float, optional) – Convergence tolerance.

Returns:

result – Updated state and covariance after convergence.

Return type:

KalmanUpdate

Examples

>>> import numpy as np
>>> # Bearing-only measurement (highly nonlinear)
>>> def h(x):
...     return np.array([np.arctan2(x[1], x[0])])
>>> def H_func(x):
...     r2 = x[0]**2 + x[1]**2
...     return np.array([[-x[1]/r2, x[0]/r2]])
>>> x = np.array([10.0, 10.0])
>>> P = np.eye(2) * 5.0
>>> z = np.array([np.radians(50)])  # measured bearing
>>> R = np.array([[0.01]])
>>> upd = iterated_ekf_update(x, P, z, h, H_func, R)
>>> upd.x.shape
(2,)

See also

ekf_update

Standard (non-iterated) EKF update.

Constrained Extended Kalman Filter

State-constrained filtering using Lagrange multiplier methods. Enforces equality and inequality constraints on the state estimate.

Constrained Extended Kalman Filter (CEKF).

Extends the Extended Kalman Filter to enforce constraints on the state estimate. Uses Lagrange multiplier method to project onto constraint manifold while maintaining positive definite covariance.

References

• Simon, D. (2006). Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches. Wiley-Interscience.

• Simon, D. & Simon, D. L. (2010). Constrained Kalman filtering via density function truncation. Journal of Guidance, Control, and Dynamics.

class pytcl.dynamic_estimation.kalman.constrained.ConstraintFunction(g, G=None, constraint_type='inequality')

Bases: object

Base class for state constraints.

__init__(g, G=None, constraint_type='inequality')

Define a constraint: g(x) ≤ 0 (inequality) or g(x) = 0 (equality).

Parameters:

• g (callable) – Constraint function: g(x) -> scalar or array Inequality: g(x) ≤ 0 Equality: g(x) = 0

• G (callable, optional) – Jacobian of g with respect to x: ∂g/∂x If None, computed numerically.

• constraint_type ({'inequality', 'equality'}) – Constraint type.

evaluate(x)

Evaluate constraint at state x.

jacobian(x)

Compute constraint Jacobian at x.

is_satisfied(x, tol=1e-06)

Check if constraint is satisfied.

class pytcl.dynamic_estimation.kalman.constrained.ConstrainedEKF

Bases: object

Extended Kalman Filter with state constraints.

Enforces linear and/or nonlinear constraints on state estimate using Lagrange multiplier method with covariance projection.

Variables:

constraints (list of ConstraintFunction) – List of active constraints.

__init__()

Initialize Constrained EKF.

add_constraint(constraint)

Add a constraint to the filter.

Parameters:

constraint (ConstraintFunction) – Constraint to enforce.

predict(x, P, f, F, Q)

Constrained EKF prediction step.

Performs standard EKF prediction. Constraints are neither enforced nor checked here – the body is a plain ekf_predict call and self.constraints is not consulted, so a predicted state may violate them silently. Enforcement happens in the update step.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• P (array_like) – Current state covariance, shape (n, n).

• f (callable) – Nonlinear state transition function.

• F (array_like) – Jacobian of f at current state.

• Q (array_like) – Process noise covariance, shape (n, n).

Returns:

result – Predicted state and covariance.

Return type:

KalmanPrediction

update(x, P, z, h, H, R)

Constrained EKF update step.

Performs standard EKF update, then projects result onto constraint manifold.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• P (array_like) – Predicted state covariance, shape (n, n).

• z (array_like) – Measurement, shape (m,).

• h (callable) – Nonlinear measurement function.

• H (array_like) – Jacobian of h at current state.

• R (array_like) – Measurement noise covariance, shape (m, m).

Returns:

result – Updated state and covariance (constrained).

Return type:

KalmanUpdate

pytcl.dynamic_estimation.kalman.constrained.constrained_ekf_predict(x, P, f, F, Q)

Convenience function for constrained EKF prediction.

Parameters:

• x (array_like) – Current state estimate.

• P (array_like) – Current covariance.

• f (callable) – Nonlinear dynamics function.

• F (array_like) – Jacobian of f.

• Q (array_like) – Process noise covariance.

Returns:

result – Predicted state and covariance.

Return type:

KalmanPrediction

pytcl.dynamic_estimation.kalman.constrained.constrained_ekf_update(x, P, z, h, H, R, constraints=None)

Convenience function for constrained EKF update.

Parameters:

• x (array_like) – Predicted state.

• P (array_like) – Predicted covariance.

• z (array_like) – Measurement.

• h (callable) – Nonlinear measurement function.

• H (array_like) – Jacobian of h.

• R (array_like) – Measurement noise covariance.

• constraints (list, optional) – List of ConstraintFunction objects.

Returns:

result – Updated state and covariance.

Return type:

KalmanUpdate

Unscented & Cubature Kalman Filter

Unscented Kalman Filter (UKF) implementation.

The UKF uses the unscented transform to propagate the mean and covariance through nonlinear functions without requiring Jacobian computation.

class pytcl.dynamic_estimation.kalman.unscented.SigmaPoints(points, Wm, Wc)

Bases: NamedTuple

Sigma points and weights for unscented transform.

Variables:

• points (ndarray) – Sigma points, shape (2n+1, n).

• Wm (ndarray) – Weights for mean computation.

• Wc (ndarray) – Weights for covariance computation.

points: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

Wm: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

Wc: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

pytcl.dynamic_estimation.kalman.unscented.sigma_points_merwe(x, P, alpha=0.001, beta=2.0, kappa=0.0)

Generate sigma points using Van der Merwe’s scaled method.

Parameters:

• x (array_like) – State mean, shape (n,).

• P (array_like) – State covariance, shape (n, n).

• alpha (float, optional) – Spread of sigma points (default: 1e-3). Small values keep points close to mean.

• beta (float, optional) – Prior knowledge of distribution (default: 2.0 for Gaussian).

• kappa (float, optional) – Secondary scaling parameter (default: 0, or 3-n).

Returns:

result – Sigma points and weights.

Return type:

SigmaPoints

Examples

>>> import numpy as np
>>> x = np.array([0.0, 0.0])
>>> P = np.eye(2)
>>> sp = sigma_points_merwe(x, P)
>>> sp.points.shape
(5, 2)

Notes

Generates 2n+1 sigma points:

X_0 = x X_i = x + sqrt((n+lambda)*P)_i, i = 1..n X_{i+n} = x - sqrt((n+lambda)*P)_i, i = 1..n

where lambda = alpha^2 * (n + kappa) - n.

References

• Van der Merwe, R., “Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models”, PhD Thesis, 2004.

pytcl.dynamic_estimation.kalman.unscented.sigma_points_julier(x, P, kappa=0.0)

Generate sigma points using Julier’s original method.

Parameters:

• x (array_like) – State mean.

• P (array_like) – State covariance.

• kappa (float, optional) – Scaling parameter (default: 0, typical: 3-n).

Returns:

result – Sigma points and weights.

Return type:

SigmaPoints

Examples

>>> import numpy as np
>>> x = np.array([1.0, 2.0])
>>> P = np.eye(2) * 0.1
>>> sp = sigma_points_julier(x, P, kappa=1.0)
>>> sp.points.shape  # 2*n+1 = 5 points for n=2
(5, 2)
>>> np.allclose(sp.Wm.sum(), 1.0)
True

Notes

Julier’s method is a special case of Merwe’s with alpha=1, beta=0.

pytcl.dynamic_estimation.kalman.unscented.unscented_transform(sigmas, Wm, Wc, noise_cov=None)

Compute mean and covariance from transformed sigma points.

Parameters:

• sigmas (ndarray) – Transformed sigma points, shape (2n+1, m).

• Wm (ndarray) – Weights for mean.

• Wc (ndarray) – Weights for covariance.

• noise_cov (array_like, optional) – Additive noise covariance.

Returns:

• mean (ndarray) – Weighted mean, shape (m,).

• cov (ndarray) – Weighted covariance, shape (m, m).

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> import numpy as np
>>> # Generate sigma points
>>> x = np.array([0.0, 0.0])
>>> P = np.eye(2)
>>> sp = sigma_points_merwe(x, P)
>>> # Pass through nonlinear function
>>> def f(x):
...     return np.array([x[0]**2, x[1]])
>>> sigmas_f = np.array([f(sp.points[i]) for i in range(len(sp.points))])
>>> mean, cov = unscented_transform(sigmas_f, sp.Wm, sp.Wc)
>>> mean.shape
(2,)

pytcl.dynamic_estimation.kalman.unscented.ukf_predict(x, P, f, Q, alpha=0.001, beta=2.0, kappa=0.0)

Unscented Kalman filter prediction step.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• P (array_like) – Current state covariance, shape (n, n).

• f (callable) – Nonlinear state transition function f(x) -> x_next.

• Q (array_like) – Process noise covariance, shape (n, n).

• alpha (float, optional) – UKF spread parameter.

• beta (float, optional) – UKF distribution parameter.

• kappa (float, optional) – UKF scaling parameter.

Returns:

result – Predicted state and covariance.

Return type:

KalmanPrediction

Examples

>>> import numpy as np
>>> def f(x):
...     return np.array([x[0] + x[1], x[1]])
>>> x = np.array([0.0, 1.0])
>>> P = np.eye(2) * 0.1
>>> Q = np.eye(2) * 0.01
>>> pred = ukf_predict(x, P, f, Q)

See also

ukf_update

UKF measurement update.

sigma_points_merwe

Sigma point generation.

pytcl.dynamic_estimation.kalman.unscented.ukf_update(x, P, z, h, R, alpha=0.001, beta=2.0, kappa=0.0)

Unscented Kalman filter update step.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• P (array_like) – Predicted state covariance, shape (n, n).

• z (array_like) – Measurement, shape (m,).

• h (callable) – Nonlinear measurement function h(x) -> z.

• R (array_like) – Measurement noise covariance, shape (m, m).

• alpha (float, optional) – UKF spread parameter.

• beta (float, optional) – UKF distribution parameter.

• kappa (float, optional) – UKF scaling parameter.

Returns:

result – Updated state, covariance, and innovation statistics.

Return type:

KalmanUpdate

Examples

Update with a range-bearing measurement:

>>> import numpy as np
>>> # State: [x, y], range-bearing measurement
>>> def h_rb(x):
...     r = np.sqrt(x[0]**2 + x[1]**2)
...     theta = np.arctan2(x[1], x[0])
...     return np.array([r, theta])
>>> x = np.array([100.0, 50.0])
>>> P = np.eye(2) * 10.0
>>> z = np.array([112.0, 0.46])  # measured range and bearing
>>> R = np.diag([1.0, 0.01])  # measurement noise
>>> upd = ukf_update(x, P, z, h_rb, R)
>>> upd.x.shape
(2,)

See also

ukf_predict

UKF prediction step.

pytcl.dynamic_estimation.kalman.unscented.ckf_spherical_cubature_points(n)

Generate cubature points for Cubature Kalman Filter.

Parameters:

n (int) – State dimension.

Returns:

• points (ndarray) – Unit cubature points, shape (2n, n).

• weights (ndarray) – Cubature weights.

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> import numpy as np
>>> points, weights = ckf_spherical_cubature_points(3)
>>> points.shape  # 2n = 6 points for n=3
(6, 3)
>>> np.allclose(weights.sum(), 1.0)
True

Notes

The CKF uses a third-degree spherical-radial cubature rule with 2n points at ±sqrt(n) along each axis.

See also

pytcl.mathematical_functions.numerical_integration.cubature_points.second_order_cubature_points

A structurally different (n+2)-point rule (the scaled unscented transformation’s minimal spherical simplex points), exact through degree 2 only – fewer points than this 2n-point rule, but without this rule’s degree-3 exactness. Not an alternate parameterization of this rule.

pytcl.dynamic_estimation.kalman.unscented.ckf_predict(x, P, f, Q, *, points=None, weights=None)

Cubature Kalman filter prediction step.

The CKF uses spherical-radial cubature for numerical integration. Its 2n-point rule avoids the negative center weight the standard UKF parameterization produces for n > 3, which is the basis of the CKF’s high-dimensional robustness argument (Arasaratnam & Haykin, 2009) – not a general accuracy guarantee over the UKF.

Parameters:

• x (array_like) – Current state estimate.

• P (array_like) – Current state covariance.

• f (callable) – Nonlinear state transition function.

• Q (array_like) – Process noise covariance.

• points (array_like, optional) – Custom unit cubature points, shape (m, n), in the N(0, I) convention (i.e. the points that would be used to integrate against a standard normal, before scaling by the Cholesky factor of P). Must be provided together with weights. If omitted, the default third-degree spherical-radial rule (ckf_spherical_cubature_points) is used.

• weights (array_like, optional) – Weights matching points, shape (m,), summing to 1. May contain negative values (e.g. higher-degree rules).

Returns:

result – Predicted state and covariance.

Return type:

KalmanPrediction

Examples

>>> import numpy as np
>>> # Linear dynamics: x_k+1 = F @ x_k
>>> def f(x):
...     F = np.array([[1, 1], [0, 1]])
...     return F @ x
>>> x = np.array([0.0, 1.0])
>>> P = np.eye(2) * 0.1
>>> Q = np.eye(2) * 0.01
>>> pred = ckf_predict(x, P, f, Q)
>>> pred.x  # Should be approximately [1, 1]
array([1., 1.])

See also

pytcl.mathematical_functions.numerical_integration.cubature_points

Higher-degree Gaussian cubature rules (e.g. fifth_order_cubature_points) that can be passed via points/weights.

References

• Arasaratnam, I. and Haykin, S., “Cubature Kalman Filters”, IEEE Trans. Automatic Control, 2009.

pytcl.dynamic_estimation.kalman.unscented.ckf_update(x, P, z, h, R, *, points=None, weights=None)

Cubature Kalman filter update step.

Parameters:

• x (array_like) – Predicted state estimate.

• P (array_like) – Predicted state covariance.

• z (array_like) – Measurement.

• h (callable) – Nonlinear measurement function.

• R (array_like) – Measurement noise covariance.

• points (array_like, optional) – Custom unit cubature points, shape (m, n), in the N(0, I) convention (i.e. the points that would be used to integrate against a standard normal, before scaling by the Cholesky factor of P). Must be provided together with weights. If omitted, the default third-degree spherical-radial rule (ckf_spherical_cubature_points) is used.

• weights (array_like, optional) – Weights matching points, shape (m,), summing to 1. May contain negative values (e.g. higher-degree rules).

Returns:

result – Updated state and covariance.

Return type:

KalmanUpdate

Examples

>>> import numpy as np
>>> # Position measurement h(x) = x[0]
>>> def h(x):
...     return np.array([x[0]])
>>> x = np.array([3.0, 1.0])  # predicted [position, velocity]
>>> P = np.eye(2) * 0.5
>>> z = np.array([3.1])  # measurement
>>> R = np.array([[0.1]])
>>> upd = ckf_update(x, P, z, h, R)
>>> upd.x.shape
(2,)

See also

pytcl.mathematical_functions.numerical_integration.cubature_points

Higher-degree Gaussian cubature rules (e.g. fifth_order_cubature_points) that can be passed via points/weights.

Square-Root Kalman Filters

Numerically stable Kalman filter implementations that propagate the Cholesky factor of the covariance matrix.

Square-root Kalman filter implementations.

This module provides numerically stable Kalman filter variants that propagate the square root (Cholesky factor) of the covariance matrix instead of the covariance itself. This improves numerical stability and guarantees positive semi-definiteness of the covariance.

Implementations include: - Square-root Kalman filter (Cholesky-based)

For U-D factorization filters, see pytcl.dynamic_estimation.kalman.ud_filter. For square-root UKF, see pytcl.dynamic_estimation.kalman.sr_ukf.

pytcl.dynamic_estimation.kalman.square_root.cholesky_update(S, v, sign=1.0)

Rank-1 Cholesky update/downdate.

Computes the Cholesky factor of P ± v @ v.T given S where P = S @ S.T.

Parameters:

• S (ndarray) – Lower triangular Cholesky factor, shape (n, n).

• v (ndarray) – Vector for rank-1 update, shape (n,).

• sign (float) – +1 for update (addition), -1 for downdate (subtraction).

Returns:

S_new – Updated lower triangular Cholesky factor.

Return type:

ndarray

Notes

Uses the efficient O(n²) algorithm from [1].

References

• P. E. Gill, G. H. Golub, W. Murray, and M. A. Saunders, “Methods for modifying matrix factorizations,” Mathematics of Computation, vol. 28, pp. 505-535, 1974.

Examples

>>> import numpy as np
>>> S = np.linalg.cholesky(np.eye(2))
>>> v = np.array([0.5, 0.5])
>>> S_updated = cholesky_update(S, v, sign=1.0)
>>> P_updated = S_updated @ S_updated.T
>>> np.allclose(P_updated, np.eye(2) + np.outer(v, v))
True

pytcl.dynamic_estimation.kalman.square_root.qr_update(S_x, S_noise, F=None)

QR-based covariance square root update.

Computes the Cholesky factor of F @ P @ F.T + Q given S_x (where P = S_x @ S_x.T) and S_noise (where Q = S_noise @ S_noise.T).

Parameters:

• S_x (ndarray) – Lower triangular Cholesky factor of state covariance, shape (n, n).

• S_noise (ndarray) – Lower triangular Cholesky factor of noise covariance, shape (n, n).

• F (ndarray, optional) – State transition matrix, shape (n, n). If None, uses identity.

Returns:

S_new – Lower triangular Cholesky factor of the updated covariance.

Return type:

ndarray

Notes

Uses QR decomposition for numerical stability. The compound matrix [F @ S_x, S_noise].T is QR decomposed, and R.T gives the new Cholesky factor.

Examples

>>> import numpy as np
>>> S_x = np.linalg.cholesky(np.eye(2) * 0.1)
>>> S_noise = np.linalg.cholesky(np.eye(2) * 0.01)
>>> F = np.array([[1, 1], [0, 1]])
>>> S_new = qr_update(S_x, S_noise, F)

pytcl.dynamic_estimation.kalman.square_root.srkf_predict(x, S, F, S_Q, B=None, u=None)

Square-root Kalman filter prediction step.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• S (array_like) – Lower triangular Cholesky factor of current covariance, shape (n, n). Satisfies P = S @ S.T.

• F (array_like) – State transition matrix, shape (n, n).

• S_Q (array_like) – Lower triangular Cholesky factor of process noise, shape (n, n). Satisfies Q = S_Q @ S_Q.T.

• B (array_like, optional) – Control input matrix, shape (n, m).

• u (array_like, optional) – Control input, shape (m,).

Returns:

result – Named tuple with predicted state x and Cholesky factor S.

Return type:

SRKalmanPrediction

Examples

>>> import numpy as np
>>> x = np.array([0.0, 1.0])
>>> S = np.linalg.cholesky(np.eye(2) * 0.1)
>>> F = np.array([[1, 1], [0, 1]])
>>> S_Q = np.linalg.cholesky(np.array([[0.25, 0.1], [0.1, 1.0]]))
>>> pred = srkf_predict(x, S, F, S_Q)
>>> pred.x
array([1., 1.])
>>> P_pred = F @ (S @ S.T) @ F.T + S_Q @ S_Q.T
>>> bool(np.allclose(pred.S @ pred.S.T, P_pred))
True

See also

srkf_update

Measurement update step.

kf_predict

Standard Kalman filter prediction.

pytcl.dynamic_estimation.kalman.square_root.srkf_update(x, S, z, H, S_R)

Square-root Kalman filter update step.

Uses the Potter square-root filter formulation for the measurement update.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• S (array_like) – Lower triangular Cholesky factor of predicted covariance, shape (n, n).

• z (array_like) – Measurement, shape (m,).

• H (array_like) – Measurement matrix, shape (m, n).

• S_R (array_like) – Lower triangular Cholesky factor of measurement noise, shape (m, m). Satisfies R = S_R @ S_R.T.

Returns:

result – Named tuple with updated state, Cholesky factor, innovation, etc.

Return type:

SRKalmanUpdate

Examples

>>> import numpy as np
>>> x = np.array([1.0, 1.0])
>>> S = np.linalg.cholesky(np.array([[0.35, 0.5], [0.5, 1.1]]))
>>> z = np.array([1.2])
>>> H = np.array([[1, 0]])
>>> S_R = np.linalg.cholesky(np.array([[0.1]]))
>>> upd = srkf_update(x, S, z, H, S_R)

See also

srkf_predict

Prediction step.

kf_update

Standard Kalman filter update.

pytcl.dynamic_estimation.kalman.square_root.srkf_predict_update(x, S, z, F, S_Q, H, S_R, B=None, u=None)

Combined square-root Kalman filter prediction and update.

Parameters:

• x (array_like) – Current state estimate.

• S (array_like) – Cholesky factor of current covariance.

• z (array_like) – Measurement.

• F (array_like) – State transition matrix.

• S_Q (array_like) – Cholesky factor of process noise.

• H (array_like) – Measurement matrix.

• S_R (array_like) – Cholesky factor of measurement noise.

• B (array_like, optional) – Control input matrix.

• u (array_like, optional) – Control input.

Returns:

result – Updated state and Cholesky factor.

Return type:

SRKalmanUpdate

Examples

>>> import numpy as np
>>> x = np.array([0.0, 1.0])
>>> S = np.linalg.cholesky(np.eye(2) * 0.1)
>>> F = np.array([[1, 1], [0, 1]])
>>> S_Q = np.linalg.cholesky(np.eye(2) * 0.01)
>>> H = np.array([[1, 0]])
>>> S_R = np.linalg.cholesky(np.array([[0.1]]))
>>> z = np.array([1.05])
>>> result = srkf_predict_update(x, S, z, F, S_Q, H, S_R)

class pytcl.dynamic_estimation.kalman.square_root.UDState(x, U, D)

Bases: NamedTuple

State of a U-D factorization filter.

The covariance is represented as P = U @ D @ U.T where U is unit upper triangular and D is diagonal.

Variables:

• x (ndarray) – State estimate.

• U (ndarray) – Unit upper triangular factor.

• D (ndarray) – Diagonal elements (1D array).

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

U: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

D: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

pytcl.dynamic_estimation.kalman.square_root.ud_factorize(P)

Compute U-D factorization of a symmetric positive definite matrix.

Decomposes P = U @ D @ U.T where U is unit upper triangular and D is diagonal.

Parameters:

P (array_like) – Symmetric positive definite matrix, shape (n, n).

Returns:

• U (ndarray) – Unit upper triangular matrix.

• D (ndarray) – Diagonal elements (1D array).

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Notes

The U-D factorization is equivalent to a modified Cholesky decomposition and requires only n(n+1)/2 storage elements.

Examples

>>> import numpy as np
>>> P = np.array([[4.0, 2.0], [2.0, 3.0]])
>>> U, D = ud_factorize(P)
>>> np.allclose(U @ np.diag(D) @ U.T, P)
True

pytcl.dynamic_estimation.kalman.square_root.ud_reconstruct(U, D)

Reconstruct covariance matrix from U-D factors.

Parameters:

• U (array_like) – Unit upper triangular matrix.

• D (array_like) – Diagonal elements.

Returns:

P – Covariance matrix P = U @ diag(D) @ U.T.

Return type:

ndarray

Examples

>>> import numpy as np
>>> U = np.array([[1.0, 0.5], [0.0, 1.0]])
>>> D = np.array([2.0, 1.0])
>>> P = ud_reconstruct(U, D)
>>> P
array([[2.25, 0.5 ],
       [0.5 , 1.  ]])

pytcl.dynamic_estimation.kalman.square_root.ud_predict(x, U, D, F, Q)

U-D filter prediction step.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• U (array_like) – Unit upper triangular factor, shape (n, n).

• D (array_like) – Diagonal elements, shape (n,).

• F (array_like) – State transition matrix, shape (n, n).

• Q (array_like) – Process noise covariance, shape (n, n).

Returns:

• x_pred (ndarray) – Predicted state.

• U_pred (ndarray) – Predicted unit upper triangular factor.

• D_pred (ndarray) – Predicted diagonal elements.

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Notes

This reconstructs the dense covariance P = U @ D @ U.T, propagates it as F @ P @ F.T + Q, and re-factorizes the result with ud_factorize – it does not propagate U and D directly. Numerically this step is equivalent to kf_predict (the two agree to ~6.7e-16) and gains none of the stability benefit that Bierman’s method delivers on the update side (ud_update_scalar).

Examples

>>> import numpy as np
>>> x = np.array([1.0, 0.0])
>>> U = np.eye(2)
>>> D = np.array([0.1, 0.1])
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.01
>>> x_pred, U_pred, D_pred = ud_predict(x, U, D, F, Q)

pytcl.dynamic_estimation.kalman.square_root.ud_update_scalar(x, U, D, z, h, r)

U-D filter scalar measurement update (Bierman’s algorithm).

This is the most efficient form - for vector measurements, process each component sequentially.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• U (array_like) – Unit upper triangular factor, shape (n, n).

• D (array_like) – Diagonal elements, shape (n,).

• z (float) – Scalar measurement.

• h (array_like) – Measurement row vector, shape (n,).

• r (float) – Measurement noise variance.

Returns:

• x_upd (ndarray) – Updated state.

• U_upd (ndarray) – Updated unit upper triangular factor.

• D_upd (ndarray) – Updated diagonal elements.

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Notes

This implements Bierman’s sequential scalar update algorithm which is numerically stable and efficient for U-D filters.

Examples

>>> import numpy as np
>>> x = np.array([1.0, 0.5])
>>> U = np.eye(2)
>>> D = np.array([0.2, 0.1])
>>> z = 1.1
>>> h = np.array([1.0, 0.0])
>>> r = 0.1
>>> x_upd, U_upd, D_upd = ud_update_scalar(x, U, D, z, h, r)

pytcl.dynamic_estimation.kalman.square_root.ud_update(x, U, D, z, H, R)

U-D filter vector measurement update.

Processes measurements sequentially using scalar updates.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• U (array_like) – Unit upper triangular factor, shape (n, n).

• D (array_like) – Diagonal elements, shape (n,).

• z (array_like) – Measurement vector, shape (m,).

• H (array_like) – Measurement matrix, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m). Should be diagonal for sequential processing.

Returns:

• x_upd (ndarray) – Updated state.

• U_upd (ndarray) – Updated unit upper triangular factor.

• D_upd (ndarray) – Updated diagonal elements.

• y (ndarray) – Innovation vector.

• likelihood (float) – Measurement likelihood.

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], float]

Notes

For correlated measurement noise (non-diagonal R), the measurements are decorrelated first using a Cholesky decomposition.

Examples

>>> import numpy as np
>>> x = np.array([1.0, 0.5])
>>> U = np.eye(2)
>>> D = np.array([0.2, 0.1])
>>> z = np.array([1.1])
>>> H = np.array([[1.0, 0.0]])
>>> R = np.array([[0.1]])
>>> x_upd, U_upd, D_upd, y, likelihood = ud_update(x, U, D, z, H, R)

pytcl.dynamic_estimation.kalman.square_root.sr_ukf_predict(x, S, f, S_Q, alpha=0.001, beta=2.0, kappa=0.0)

Square-root Unscented Kalman Filter prediction step.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• S (array_like) – Lower triangular Cholesky factor of covariance, shape (n, n).

• f (callable) – State transition function f(x) -> x_next.

• S_Q (array_like) – Cholesky factor of process noise covariance.

• alpha (float, optional) – Spread of sigma points around mean. Default 1e-3.

• beta (float, optional) – Prior knowledge about distribution. Default 2.0 (Gaussian).

• kappa (float, optional) – Secondary scaling parameter. Default 0.0.

Returns:

result – Predicted state and Cholesky factor.

Return type:

SRKalmanPrediction

Examples

>>> import numpy as np
>>> def f(x):
...     return np.array([x[0] + x[1], x[1]])
>>> x = np.array([1.0, 0.5])
>>> S = np.linalg.cholesky(np.eye(2) * 0.1)
>>> S_Q = np.linalg.cholesky(np.eye(2) * 0.01)
>>> pred = sr_ukf_predict(x, S, f, S_Q)

See also

sr_ukf_update

Measurement update step.

pytcl.dynamic_estimation.kalman.square_root.sr_ukf_update(x, S, z, h, S_R, alpha=0.001, beta=2.0, kappa=0.0)

Square-root Unscented Kalman Filter update step.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• S (array_like) – Lower triangular Cholesky factor of covariance, shape (n, n).

• z (array_like) – Measurement, shape (m,).

• h (callable) – Measurement function h(x) -> z.

• S_R (array_like) – Cholesky factor of measurement noise covariance.

• alpha (float, optional) – Spread of sigma points around mean. Default 1e-3.

• beta (float, optional) – Prior knowledge about distribution. Default 2.0 (Gaussian).

• kappa (float, optional) – Secondary scaling parameter. Default 0.0.

Returns:

result – Updated state and Cholesky factor.

Return type:

SRKalmanUpdate

Examples

>>> import numpy as np
>>> def h(x):
...     return np.array([x[0]])  # Measure first state
>>> x = np.array([1.0, 0.5])
>>> S = np.linalg.cholesky(np.eye(2) * 0.1)
>>> z = np.array([1.1])
>>> S_R = np.linalg.cholesky(np.array([[0.05]]))
>>> upd = sr_ukf_update(x, S, z, h, S_R)

See also

sr_ukf_predict

Prediction step.

Interacting Multiple Model (IMM) Estimator

The IMM estimator handles systems with multiple possible dynamic modes.

Interacting Multiple Model (IMM) estimator.

The IMM estimator handles targets with multiple possible motion modes (e.g., constant velocity, coordinated turn, acceleration) by maintaining a bank of filters and mixing their outputs based on mode probabilities.

The IMM algorithm consists of four steps: 1. Mode probability mixing (interaction) 2. Mode-matched filtering (prediction/update per mode) 3. Mode probability update 4. Output combination

class pytcl.dynamic_estimation.imm.IMMState(x, P, mode_states, mode_covs, mode_probs)

Bases: NamedTuple

State of an IMM estimator.

Variables:

• x (ndarray) – Combined state estimate, shape (n,).

• P (ndarray) – Combined state covariance, shape (n, n).

• mode_states (list of ndarray) – State estimates for each mode, each shape (n,).

• mode_covs (list of ndarray) – Covariances for each mode, each shape (n, n).

• mode_probs (ndarray) – Mode probabilities, shape (r,) where r is number of modes.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

mode_states: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 2

mode_covs: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 3

mode_probs: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 4

class pytcl.dynamic_estimation.imm.IMMPrediction(x, P, mode_states, mode_covs, mode_probs, mixing_probs)

Bases: NamedTuple

Result of IMM prediction step.

Variables:

• x (ndarray) – Combined predicted state estimate.

• P (ndarray) – Combined predicted state covariance.

• mode_states (list of ndarray) – Predicted state estimates for each mode.

• mode_covs (list of ndarray) – Predicted covariances for each mode.

• mode_probs (ndarray) – PREDICTED mode probabilities, Pi.T @ mode_probs – the transition matrix has already been applied. (This read “unchanged during prediction”, which was wrong in the dangerous direction: a caller re-applying Pi would double-predict, and imm_update relies on these being the predicted values.)

• mixing_probs (ndarray) – Mixing probabilities used, shape (r, r).

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

mode_states: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 2

mode_covs: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 3

mode_probs: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 4

mixing_probs: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 5

class pytcl.dynamic_estimation.imm.IMMUpdate(x, P, mode_states, mode_covs, mode_probs, mode_likelihoods)

Bases: NamedTuple

Result of IMM update step.

Variables:

• x (ndarray) – Combined updated state estimate.

• P (ndarray) – Combined updated state covariance.

• mode_states (list of ndarray) – Updated state estimates for each mode.

• mode_covs (list of ndarray) – Updated covariances for each mode.

• mode_probs (ndarray) – Updated mode probabilities.

• mode_likelihoods (ndarray) – Measurement likelihoods for each mode.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

mode_states: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 2

mode_covs: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 3

mode_probs: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 4

mode_likelihoods: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 5

pytcl.dynamic_estimation.imm.compute_mixing_probabilities(mode_probs, transition_matrix)

Compute mixing probabilities and predicted mode probabilities.

Parameters:

• mode_probs (array_like) – Current mode probabilities, shape (r,).

• transition_matrix (array_like) – Mode transition probability matrix, shape (r, r). Element [i, j] is P(mode_j at k | mode_i at k-1).

Returns:

• mixing_probs (ndarray) – Mixing probabilities, shape (r, r). Element [i, j] is P(mode_i at k-1 | mode_j at k).

• c_bar (ndarray) – Predicted mode probabilities, shape (r,).

Return type:

tuple[ndarray[tuple[Any, …], dtype[Any]], ndarray[tuple[Any, …], dtype[Any]]]

pytcl.dynamic_estimation.imm.mix_states(mode_states, mode_covs, mixing_probs)

Mix states and covariances for interaction step.

Parameters:

• mode_states (list of ndarray) – State estimates for each mode, each shape (n,).

• mode_covs (list of ndarray) – Covariances for each mode, each shape (n, n).

• mixing_probs (ndarray) – Mixing probabilities, shape (r, r).

Returns:

• mixed_states (list of ndarray) – Mixed state estimates for each mode.

• mixed_covs (list of ndarray) – Mixed covariances for each mode.

Return type:

tuple[List[ndarray[tuple[Any, …], dtype[Any]]], List[ndarray[tuple[Any, …], dtype[Any]]]]

pytcl.dynamic_estimation.imm.combine_estimates(mode_states, mode_covs, mode_probs)

Combine mode-conditioned estimates into overall estimate.

Parameters:

• mode_states (list of ndarray) – State estimates for each mode.

• mode_covs (list of ndarray) – Covariances for each mode.

• mode_probs (ndarray) – Mode probabilities.

Returns:

• x (ndarray) – Combined state estimate.

• P (ndarray) – Combined covariance.

Return type:

tuple[ndarray[tuple[Any, …], dtype[Any]], ndarray[tuple[Any, …], dtype[Any]]]

pytcl.dynamic_estimation.imm.imm_predict(mode_states, mode_covs, mode_probs, transition_matrix, F_list, Q_list)

IMM prediction step.

Performs: 1. Compute mixing probabilities 2. Mix states and covariances 3. Mode-matched prediction for each filter

Parameters:

• mode_states (list of array_like) – Current state estimates for each mode, each shape (n,).

• mode_covs (list of array_like) – Current covariances for each mode, each shape (n, n).

• mode_probs (array_like) – Current mode probabilities, shape (r,).

• transition_matrix (array_like) – Mode transition probability matrix, shape (r, r).

• F_list (list of array_like) – State transition matrices for each mode.

• Q_list (list of array_like) – Process noise covariances for each mode.

Returns:

result – Predicted states, covariances, and mode probabilities.

Return type:

IMMPrediction

Examples

>>> import numpy as np
>>> # Two modes: constant velocity and coordinated turn
>>> x1 = np.array([0., 1., 0., 0.])  # Mode 1 state
>>> x2 = np.array([0., 1., 0., 0.])  # Mode 2 state
>>> P1 = np.eye(4) * 0.1
>>> P2 = np.eye(4) * 0.1
>>> mu = np.array([0.9, 0.1])  # Mostly CV
>>> Pi = np.array([[0.95, 0.05], [0.05, 0.95]])  # Transition matrix
>>> F1 = np.eye(4)  # CV transition
>>> F2 = np.eye(4)  # CT transition
>>> Q1 = np.eye(4) * 0.01
>>> Q2 = np.eye(4) * 0.01
>>> pred = imm_predict([x1, x2], [P1, P2], mu, Pi, [F1, F2], [Q1, Q2])

pytcl.dynamic_estimation.imm.imm_update(mode_states, mode_covs, mode_probs, z, H_list, R_list)

IMM update step.

Performs: 1. Mode-matched measurement update for each filter 2. Mode probability update using measurement likelihoods 3. Output combination

Parameters:

• mode_states (list of array_like) – Predicted state estimates for each mode.

• mode_covs (list of array_like) – Predicted covariances for each mode.

• mode_probs (array_like) – Predicted mode probabilities.

• z (array_like) – Measurement.

• H_list (list of array_like) – Measurement matrices for each mode.

• R_list (list of array_like) – Measurement noise covariances for each mode.

Returns:

result – Updated states, covariances, and mode probabilities.

Return type:

IMMUpdate

Examples

>>> import numpy as np
>>> # After prediction
>>> x1 = np.array([1., 1., 0., 0.])
>>> x2 = np.array([1., 1., 0., 0.])
>>> P1 = np.eye(4) * 0.2
>>> P2 = np.eye(4) * 0.2
>>> mu = np.array([0.9, 0.1])
>>> z = np.array([1.1, 0.1])  # Position measurement
>>> H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
>>> R = np.eye(2) * 0.1
>>> upd = imm_update([x1, x2], [P1, P2], mu, z, [H, H], [R, R])

pytcl.dynamic_estimation.imm.imm_predict_update(mode_states, mode_covs, mode_probs, transition_matrix, z, F_list, Q_list, H_list, R_list)

Combined IMM prediction and update step.

Parameters:

• mode_states (list of array_like) – Current state estimates for each mode.

• mode_covs (list of array_like) – Current covariances for each mode.

• mode_probs (array_like) – Current mode probabilities.

• transition_matrix (array_like) – Mode transition probability matrix.

• z (array_like) – Measurement.

• F_list (list of array_like) – State transition matrices for each mode.

• Q_list (list of array_like) – Process noise covariances for each mode.

• H_list (list of array_like) – Measurement matrices for each mode.

• R_list (list of array_like) – Measurement noise covariances for each mode.

Returns:

result – Updated states, covariances, and mode probabilities.

Return type:

IMMUpdate

Examples

Track a target with 2 motion modes (CV and CA):

>>> import numpy as np
>>> # Two modes: constant velocity and constant acceleration
>>> states = [np.array([0, 1, 0, 1]), np.array([0, 1, 0, 1])]
>>> covs = [np.eye(4) * 0.1, np.eye(4) * 0.1]
>>> probs = np.array([0.9, 0.1])  # likely CV mode
>>> # Mode transition matrix (90% stay, 10% switch)
>>> trans = np.array([[0.9, 0.1], [0.1, 0.9]])
>>> # Dynamics and measurement matrices for each mode
>>> F_cv = np.array([[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]])
>>> F_ca = F_cv.copy()  # simplified
>>> H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
>>> Q = np.eye(4) * 0.01
>>> R = np.eye(2) * 0.1
>>> z = np.array([1.0, 1.0])
>>> result = imm_predict_update(states, covs, probs, trans, z,
...                             [F_cv, F_ca], [Q, Q], [H, H], [R, R])
>>> len(result.mode_probs)
2

See also

imm_predict

IMM prediction step only.

imm_update

IMM update step only.

IMMEstimator

Object-oriented interface.

class pytcl.dynamic_estimation.imm.IMMEstimator(n_modes=None, state_dim=None, transition_matrix=None, initial_mode_probs=None, *, config=None)

Bases: object

Interacting Multiple Model (IMM) estimator class.

Provides an object-oriented interface for IMM filtering with automatic state management.

Parameters:

• n_modes (int) – Number of motion modes.

• state_dim (int) – Dimension of state vector.

• transition_matrix (array_like) – Mode transition probability matrix, shape (n_modes, n_modes).

• initial_mode_probs (array_like, optional) – Initial mode probabilities. Default is uniform.

Variables:

• mode_states (list of ndarray) – Current state estimates for each mode.

• mode_covs (list of ndarray) – Current covariances for each mode.

• mode_probs (ndarray) – Current mode probabilities.

• x (ndarray) – Combined state estimate.

• P (ndarray) – Combined covariance.

Examples

>>> import numpy as np
>>> # 2-mode IMM (CV and CT) for 4D state [x, vx, y, vy]
>>> Pi = np.array([[0.95, 0.05], [0.05, 0.95]])
>>> imm = IMMEstimator(n_modes=2, state_dim=4, transition_matrix=Pi)
>>> # Initialize
>>> x0 = np.array([0., 1., 0., 0.])
>>> P0 = np.eye(4) * 0.1
>>> imm.initialize(x0, P0)
>>> # Set models
>>> F1 = np.array([[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]])
>>> Q1 = np.eye(4) * 0.01
>>> imm.set_mode_model(0, F1, Q1)
>>> imm.set_mode_model(1, F1, Q1)  # Same F for simplicity

__init__(n_modes=None, state_dim=None, transition_matrix=None, initial_mode_probs=None, *, config=None)

F_list: List[ndarray[tuple[Any, ...], dtype[Any]]]

Q_list: List[ndarray[tuple[Any, ...], dtype[Any]]]

H_list: List[ndarray[tuple[Any, ...], dtype[Any]]]

R_list: List[ndarray[tuple[Any, ...], dtype[Any]]]

initialize(x, P, mode_probs=None)

Initialize all modes with the same state.

Parameters:

• x (array_like) – Initial state estimate.

• P (array_like) – Initial covariance.

• mode_probs (array_like, optional) – Initial mode probabilities.

set_mode_model(mode_idx, F, Q)

Set the dynamic model for a specific mode.

Parameters:

• mode_idx (int) – Mode index.

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

set_measurement_model(H, R, mode_specific=False)

Set the measurement model.

Parameters:

• H (array_like or list of array_like) – Measurement matrix. If mode_specific=True, should be a list.

• R (array_like or list of array_like) – Measurement noise covariance. If mode_specific=True, should be a list.

• mode_specific (bool) – If True, H and R are lists with different models per mode.

predict()

Perform IMM prediction step.

Returns:

result – Prediction result.

Return type:

IMMPrediction

update(z)

Perform IMM update step.

Parameters:

z (array_like) – Measurement.

Returns:

result – Update result.

Return type:

IMMUpdate

predict_update(z)

Combined prediction and update.

Parameters:

z (array_like) – Measurement.

Returns:

result – Update result.

Return type:

IMMUpdate

get_state()

Get current IMM state.

Returns:

state – Current state.

Return type:

IMMState

Particle Filters

Particle filter (Sequential Monte Carlo) implementations.

This module provides: - Bootstrap particle filter - Resampling methods (multinomial, systematic, residual) - Particle statistics (mean, covariance, ESS)

Bootstrap Particle Filter

Particle filter (Sequential Monte Carlo) implementations.

This module provides particle filtering algorithms for nonlinear/non-Gaussian state estimation.

class pytcl.dynamic_estimation.particle_filters.bootstrap.ParticleState(particles, weights)

Bases: NamedTuple

State of a particle filter.

Variables:

• particles (ndarray) – Particle states, shape (N, n).

• weights (ndarray) – Normalized particle weights, shape (N,).

particles: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

weights: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

pytcl.dynamic_estimation.particle_filters.bootstrap.resample_multinomial(particles, weights, rng=None)

Multinomial resampling.

Parameters:

• particles (ndarray) – Particle states, shape (N, n).

• weights (ndarray) – Normalized weights, shape (N,).

• rng (Generator, optional) – Random number generator.

Returns:

resampled – Resampled particles with uniform weights.

Return type:

ndarray

Examples

>>> rng = np.random.default_rng(42)
>>> particles = np.array([[1.0], [2.0], [3.0], [4.0]])
>>> weights = np.array([0.1, 0.1, 0.1, 0.7])  # particle 4 dominant
>>> resampled = resample_multinomial(particles, weights, rng)
>>> resampled.shape
(4, 1)

pytcl.dynamic_estimation.particle_filters.bootstrap.resample_systematic(particles, weights, rng=None)

Systematic (stratified) resampling.

More efficient than multinomial resampling with lower variance.

Parameters:

• particles (ndarray) – Particle states, shape (N, n).

• weights (ndarray) – Normalized weights, shape (N,).

• rng (Generator, optional) – Random number generator.

Returns:

resampled – Resampled particles.

Return type:

ndarray

Examples

>>> rng = np.random.default_rng(42)
>>> particles = np.array([[0.0], [1.0], [2.0], [3.0]])
>>> weights = np.array([0.25, 0.25, 0.25, 0.25])
>>> resampled = resample_systematic(particles, weights, rng)
>>> resampled.shape
(4, 1)

pytcl.dynamic_estimation.particle_filters.bootstrap.resample_residual(particles, weights, rng=None)

Residual resampling.

Deterministically copies particles with weight > 1/N, then uses multinomial resampling for the remainder.

Parameters:

• particles (ndarray) – Particle states, shape (N, n).

• weights (ndarray) – Normalized weights, shape (N,).

• rng (Generator, optional) – Random number generator.

Returns:

resampled – Resampled particles.

Return type:

ndarray

Examples

>>> rng = np.random.default_rng(42)
>>> particles = np.array([[1.0], [2.0], [3.0], [4.0]])
>>> weights = np.array([0.1, 0.2, 0.3, 0.4])
>>> resampled = resample_residual(particles, weights, rng)
>>> resampled.shape
(4, 1)
>>> # High-weight particles are copied deterministically
>>> np.sum(resampled == 4.0) >= 1
True

pytcl.dynamic_estimation.particle_filters.bootstrap.effective_sample_size(weights)

Compute effective sample size (ESS) of particle weights.

Parameters:

weights (ndarray) – Normalized particle weights.

Returns:

ess – Effective sample size, in range [1, N].

Return type:

float

Examples

>>> # Uniform weights -> ESS = N
>>> weights = np.array([0.25, 0.25, 0.25, 0.25])
>>> effective_sample_size(weights)
4.0
>>> # Degenerate weights -> ESS approaches 1
>>> weights = np.array([0.97, 0.01, 0.01, 0.01])
>>> effective_sample_size(weights)
1.06...

Notes

ESS = 1 / sum(w_i^2)

ESS = N means all weights are equal (no degeneracy). ESS = 1 means one particle has all the weight.

pytcl.dynamic_estimation.particle_filters.bootstrap.bootstrap_pf_predict(particles, f, Q_sample, rng=None)

Bootstrap particle filter prediction step.

Propagates particles through dynamics with process noise.

Parameters:

• particles (ndarray) – Current particles, shape (N, n).

• f (callable) – Dynamics function f(x) -> x_next (operates on single particle).

• Q_sample (callable) – Function to sample process noise: Q_sample(N, rng) -> noise (N, n).

• rng (Generator, optional) – Random number generator.

Returns:

particles_pred – Predicted particles.

Return type:

ndarray

Examples

Predict particles through constant velocity dynamics:

>>> import numpy as np
>>> rng = np.random.default_rng(42)
>>> # 50 particles for 2D state [position, velocity]
>>> particles = np.column_stack([
...     np.zeros(50),  # position
...     np.ones(50)    # velocity = 1
... ])
>>> dt = 0.1
>>> # x_next = [pos + vel*dt, vel]
>>> f = lambda x: np.array([x[0] + x[1] * dt, x[1]])
>>> # Process noise sampler
>>> Q_sample = lambda n, r: r.normal(0, 0.01, (n, 2))
>>> pred = bootstrap_pf_predict(particles, f, Q_sample, rng)
>>> pred.shape
(50, 2)
>>> # Positions moved forward by ~0.1
>>> float(np.round(np.mean(pred[:, 0]), 2))
0.1

pytcl.dynamic_estimation.particle_filters.bootstrap.bootstrap_pf_update(particles, weights, z, likelihood_func)

Bootstrap particle filter update step.

Updates weights based on measurement likelihood.

Parameters:

• particles (ndarray) – Predicted particles, shape (N, n).

• weights (ndarray) – Current weights, shape (N,).

• z (array_like) – Measurement.

• likelihood_func (callable) – Function computing p(z|x) for a particle: likelihood_func(z, x) -> float.

Returns:

• weights_new (ndarray) – Updated normalized weights.

• log_likelihood (float) – Log marginal likelihood of measurement.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], float]

Examples

Update particle weights with a position measurement:

>>> import numpy as np
>>> # 4 particles at different positions
>>> particles = np.array([[0.0], [1.0], [2.0], [3.0]])
>>> weights = np.ones(4) / 4  # uniform initial weights
>>> z = np.array([2.0])  # measured position
>>> # Gaussian likelihood centered on measurement
>>> def likelihood(z, x):
...     return np.exp(-0.5 * (z[0] - x[0])**2)
>>> new_weights, log_lik = bootstrap_pf_update(particles, weights, z, likelihood)
>>> # Particle at x=2 should have highest weight
>>> np.argmax(new_weights)
2
>>> np.sum(new_weights)  # weights sum to 1
1.0

pytcl.dynamic_estimation.particle_filters.bootstrap.gaussian_likelihood(z, z_pred, R)

Gaussian measurement likelihood.

Parameters:

• z (ndarray) – Measurement.

• z_pred (ndarray) – Predicted measurement for a particle.

• R (ndarray) – Measurement noise covariance.

Returns:

likelihood – p(z|x) = N(z; z_pred, R).

Return type:

float

Examples

>>> z = np.array([1.0, 2.0])  # actual measurement
>>> z_pred = np.array([1.0, 2.0])  # perfect prediction
>>> R = np.eye(2) * 0.1  # measurement noise covariance
>>> lik = gaussian_likelihood(z, z_pred, R)
>>> lik > 0
True
>>> # Zero innovation -> maximum likelihood
>>> z_off = np.array([10.0, 10.0])
>>> gaussian_likelihood(z_off, z_pred, R) < lik
True

pytcl.dynamic_estimation.particle_filters.bootstrap.bootstrap_pf_step(particles, weights, z, f, h, Q_sample, R, resample_threshold=0.5, resample_method='systematic', rng=None)

Complete bootstrap particle filter step (predict + update + resample).

Parameters:

• particles (ndarray) – Current particles, shape (N, n).

• weights (ndarray) – Current weights, shape (N,).

• z (array_like) – Measurement.

• f (callable) – Dynamics function.

• h (callable) – Measurement function h(x) -> z_pred.

• Q_sample (callable) – Process noise sampler.

• R (array_like) – Measurement noise covariance.

• resample_threshold (float, optional) – Resample when ESS/N < threshold (default: 0.5).

• resample_method (str, optional) – ‘multinomial’, ‘systematic’, or ‘residual’.

• rng (Generator, optional) – Random number generator.

Returns:

state – Updated particles and weights.

Return type:

ParticleState

Examples

Track a 1D random walk with position measurements:

>>> import numpy as np
>>> rng = np.random.default_rng(42)
>>> # 100 particles for 1D state
>>> particles = rng.normal(0, 1, (100, 1))
>>> weights = np.ones(100) / 100
>>> # Simple dynamics: x_k+1 = x_k + w
>>> f = lambda x: x
>>> h = lambda x: x  # measure position directly
>>> Q_sample = lambda n, r: r.normal(0, 0.1, (n, 1))
>>> R = np.array([[0.5]])  # measurement noise
>>> z = np.array([0.5])  # measurement
>>> state = bootstrap_pf_step(particles, weights, z, f, h, Q_sample, R, rng=rng)
>>> state.particles.shape
(100, 1)

See also

bootstrap_pf_predict

Prediction step only.

bootstrap_pf_update

Update step only.

pytcl.dynamic_estimation.particle_filters.bootstrap.particle_mean(particles, weights)

Compute weighted mean of particles.

Parameters:

• particles (ndarray) – Particles, shape (N, n).

• weights (ndarray) – Weights, shape (N,).

Returns:

mean – Weighted mean estimate.

Return type:

ndarray

Examples

>>> particles = np.array([[0.0], [1.0], [2.0], [3.0]])
>>> weights = np.array([0.1, 0.2, 0.3, 0.4])
>>> particle_mean(particles, weights)
array([2.])

pytcl.dynamic_estimation.particle_filters.bootstrap.particle_covariance(particles, weights, mean=None)

Compute weighted covariance of particles.

Parameters:

• particles (ndarray) – Particles, shape (N, n).

• weights (ndarray) – Weights, shape (N,).

• mean (ndarray, optional) – Precomputed mean.

Returns:

cov – Weighted covariance.

Return type:

ndarray

Examples

>>> particles = np.array([[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]])
>>> weights = np.array([0.25, 0.25, 0.25, 0.25])
>>> cov = particle_covariance(particles, weights)
>>> cov.shape
(2, 2)

pytcl.dynamic_estimation.particle_filters.bootstrap.initialize_particles(x0, P0, N, rng=None)

Initialize particles from Gaussian prior.

Parameters:

• x0 (array_like) – Prior mean.

• P0 (array_like) – Prior covariance.

• N (int) – Number of particles.

• rng (Generator, optional) – Random number generator.

Returns:

state – Initial particles with uniform weights.

Return type:

ParticleState

Examples

>>> rng = np.random.default_rng(42)
>>> x0 = np.array([0.0, 0.0])
>>> P0 = np.eye(2) * 0.1
>>> state = initialize_particles(x0, P0, N=100, rng=rng)
>>> state.particles.shape
(100, 2)
>>> np.allclose(state.weights, 0.01)  # uniform 1/N
True

Rao-Blackwellized Particle Filter

Hybrid particle filter for systems with nonlinear and linear subsystems. Each particle maintains an independent Kalman filter for the linear components, reducing estimator variance.

Rao-Blackwellized Particle Filter (RBPF).

The RBPF partitions the state into a nonlinear part (handled by particles) and a linear part (handled by Kalman filters for each particle). This provides better estimation quality than plain particle filters for systems with both nonlinear and linear dynamics.

The algorithm:

1. Maintain N particles, each with:
   - Position in nonlinear state space (y)
   - Kalman filter state (x, P) for linear subspace
   - Weight w based on measurement likelihood
2. For each time step:
   - Predict: Propagate nonlinear particles, update KF for each
   - Update: Compute measurement likelihood, adapt weights
   - Resample: When effective sample size is low, draw new particles
   - Merge: Combine nearby particles to reduce variance

References:

- Doucet, A., de Freitas, N., Murphy, K., and Russell, S.,
  "Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks,"
  Proceedings of the 16th Conference on Uncertainty in Artificial
  Intelligence (UAI), 2000.
- Schon, T., Gustafsson, F., and Nordlund, P.-J., "Marginalized
  Particle Filters for Mixed Linear/Nonlinear State-Space Models,"
  IEEE Transactions on Signal Processing, 2005.

class pytcl.dynamic_estimation.rbpf.RBPFParticle(y, x, P, w)

Bases: NamedTuple

Rao-Blackwellized particle with nonlinear and linear components.

Parameters:

• y (NDArray) – Nonlinear state component (propagated by particle transition)

• x (NDArray) – Linear state component (estimated by Kalman filter for this particle)

• P (NDArray) – Covariance of linear state component

• w (float) – Particle weight (typically normalized to sum to 1)

y: ndarray[tuple[Any, ...], dtype[Any]]

Alias for field number 0

x: ndarray[tuple[Any, ...], dtype[Any]]

Alias for field number 1

P: ndarray[tuple[Any, ...], dtype[Any]]

Alias for field number 2

w: float

Alias for field number 3

class pytcl.dynamic_estimation.rbpf.RBPFFilter(max_particles=None, resample_threshold=None, merge_threshold=None, *, config=None, rng=None)

Bases: object

Rao-Blackwellized Particle Filter.

Combines particle filtering for nonlinear states with Kalman filtering for conditionally-linear states. For a system partitioned as:

- y: nonlinear state (particles)
- x: linear state given y (Kalman filter)

Variables:

• particles (list[RBPFParticle]) – Current particles with nonlinear/linear states and weights

• max_particles (int) – Maximum number of particles (default 100)

• resample_threshold (float) – Resample when N_eff < resample_threshold * N (default 0.5)

• merge_threshold (float) – Merge nearby particles when KL divergence < threshold (default 0.5)

__init__(max_particles=None, resample_threshold=None, merge_threshold=None, *, config=None, rng=None)

Initialize RBPF.

Parameters:

• max_particles (int) – Maximum number of particles to maintain

• resample_threshold (float) – Resample threshold as fraction of max particles

• merge_threshold (float) – KL divergence threshold for merging particles

• config (RBPFConfig, optional) – Typed configuration. Mutually exclusive with the individual keyword arguments above.

• rng (numpy.random.Generator, optional) – Instance random number generator for particle sampling and resampling. When None (default), falls back to the legacy global numpy.random state.

particles: list[RBPFParticle]

initialize(y0, x0, P0, num_particles=100)

Initialize particles.

Parameters:

• y0 (NDArray) – Initial nonlinear state (broadcasted to all particles)

• x0 (NDArray) – Initial linear state (broadcasted to all particles)

• P0 (NDArray) – Initial linear state covariance (same for all particles)

• num_particles (int) – Number of particles to initialize

predict(g, Qy, f, F, Qx)

Predict step: propagate particles and linear states.

Parameters:

• g (callable) – Nonlinear state transition: y[k+1] = g(y[k])

• Qy (NDArray) – Process noise covariance for nonlinear state

• f (callable) – Linear transition: x[k+1] = f(x[k], y[k])

• F (NDArray) – Jacobian matrix dF/dx (linearized around y)

• Qx (NDArray) – Process noise covariance for linear state

Notes

There is deliberately no Jacobian argument for g. In a Rao-Blackwellised particle filter the nonlinear state is carried by the particle cloud itself – each particle holds a single point y and the uncertainty lives in the spread across particles – so there is no nonlinear covariance for a Jacobian to propagate. Only the linear component, marginalised per particle, has a covariance, and that is what F propagates. A G parameter documented as “Jacobian of g with respect to y (for covariance propagation)” was accepted here until it was removed as unimplementable.

update(z, h, H, R)

Update step: adapt particle weights based on measurement.

Parameters:

• z (NDArray) – Measurement vector

• h (callable) – Measurement function: z = h(x, y)

• H (NDArray) – Jacobian matrix dH/dx (measurement sensitivity)

• R (NDArray) – Measurement noise covariance

estimate()

Estimate state as weighted mean and covariance.

Returns:

• y_est (NDArray) – Weighted mean of nonlinear components

• x_est (NDArray) – Weighted mean of linear components

• P_est (NDArray) – Weighted covariance (includes mixture and linear uncertainties)

Return type:

tuple[ndarray[tuple[Any, …], dtype[Any]], ndarray[tuple[Any, …], dtype[Any]], ndarray[tuple[Any, …], dtype[Any]]]

get_particles()

Get current particles.

Returns:

Current particle list

Return type:

list[RBPFParticle]

pytcl.dynamic_estimation.rbpf.rbpf_predict(particles, g, Qy, f, F, Qx)

Predict step for RBPF particles.

Parameters:

• particles (list[RBPFParticle]) – Current particles

• g (callable) – Nonlinear state transition

• Qy (NDArray) – Process noise covariance for nonlinear state

• f (callable) – Linear state transition

• F (NDArray) – Jacobian of linear transition

• Qx (NDArray) – Process noise covariance for linear state

Returns:

Predicted particles

Return type:

list[RBPFParticle]

Examples

>>> import numpy as np
>>> from pytcl.dynamic_estimation.rbpf import RBPFParticle
>>> np.random.seed(42)
>>> # 3 particles with nonlinear bearing and linear position
>>> particles = [
...     RBPFParticle(y=np.array([0.1]), x=np.array([0.0, 1.0]),
...                  P=np.eye(2) * 0.5, w=1/3),
...     RBPFParticle(y=np.array([0.0]), x=np.array([0.0, 1.0]),
...                  P=np.eye(2) * 0.5, w=1/3),
...     RBPFParticle(y=np.array([-0.1]), x=np.array([0.0, 1.0]),
...                  P=np.eye(2) * 0.5, w=1/3),
... ]
>>> # Nonlinear dynamics for bearing
>>> g = lambda y: y  # bearing stays constant
>>> Qy = np.eye(1) * 0.01
>>> # Linear dynamics for position
>>> f = lambda x, y: np.array([x[0] + x[1] * 0.1, x[1]])
>>> F = np.array([[1, 0.1], [0, 1]])
>>> Qx = np.eye(2) * 0.01
>>> predicted = rbpf_predict(particles, g, Qy, f, F, Qx)
>>> len(predicted)
3

pytcl.dynamic_estimation.rbpf.rbpf_update(particles, z, h, H, R)

Update step for RBPF particles.

Parameters:

• particles (list[RBPFParticle]) – Predicted particles

• z (NDArray) – Measurement

• h (callable) – Measurement function

• H (NDArray) – Jacobian of measurement function

• R (NDArray) – Measurement noise covariance

Returns:

Updated particles with adapted weights

Return type:

list[RBPFParticle]

Examples

>>> import numpy as np
>>> from pytcl.dynamic_estimation.rbpf import RBPFParticle
>>> # 3 particles at different bearings
>>> particles = [
...     RBPFParticle(y=np.array([0.5]), x=np.array([1.0, 0.0]),
...                  P=np.eye(2) * 0.5, w=1/3),
...     RBPFParticle(y=np.array([0.0]), x=np.array([1.0, 0.0]),
...                  P=np.eye(2) * 0.5, w=1/3),
...     RBPFParticle(y=np.array([-0.5]), x=np.array([1.0, 0.0]),
...                  P=np.eye(2) * 0.5, w=1/3),
... ]
>>> # Position measurement
>>> z = np.array([1.1])
>>> h = lambda x, y: np.array([x[0]])  # measure position
>>> H = np.array([[1, 0]])
>>> R = np.array([[0.1]])
>>> updated = rbpf_update(particles, z, h, H, R)
>>> len(updated)
3
>>> # Weights should sum to 1
>>> abs(sum(p.w for p in updated) - 1.0) < 1e-10
True

Gaussian Sum Filter

Gaussian Sum Filter (GSF) for nonlinear state estimation.

The Gaussian Sum Filter represents the posterior distribution as a weighted mixture of Gaussians. This allows approximation of multi-modal distributions and nonlinear systems better than a single EKF.

Each component has: - Weight (w_i): Probability of that Gaussian - Mean (μ_i): Component state estimate - Covariance (P_i): Component uncertainty

The filter performs: 1. Predict: Propagate each component independently 2. Update: Update each component with measurement, adapt weights 3. Manage: Prune low-weight components, merge similar ones

References

Bar-Shalom, Y., Li, X. R., & Kirubarajan, T. (2001). Estimation with Applications to Tracking and Navigation. Wiley-Interscience.

class pytcl.dynamic_estimation.gaussian_sum_filter.GaussianComponent(x, P, w)

Bases: NamedTuple

Single Gaussian component in mixture.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

w: float

Alias for field number 2

class pytcl.dynamic_estimation.gaussian_sum_filter.GaussianSumFilter(max_components=None, merge_threshold=None, prune_threshold=None, *, config=None, rng=None)

Bases: object

Gaussian Sum Filter for nonlinear state estimation.

A mixture model approach that represents the posterior distribution as a weighted sum of Gaussians. Useful for multi-modal distributions and nonlinear systems.

Variables:

• components (list[GaussianComponent]) – Current mixture components (state, covariance, weight).

• max_components (int) – Merge target, not a hard cap: merging runs while the count exceeds this AND the two closest components are within merge_threshold KL divergence of each other. Six well-separated components against max_components=3 remain six – exceeding the target rather than force-merging dissimilar modes is deliberate.

• merge_threshold (float) – KL divergence threshold for merging components.

• prune_threshold (float) – Weight threshold below which to prune components.

__init__(max_components=None, merge_threshold=None, prune_threshold=None, *, config=None, rng=None)

Initialize Gaussian Sum Filter.

Parameters:

• max_components (int) – Merge target for the component count; exceeded when the remaining components are too dissimilar to merge (see the class docstring).

• merge_threshold (float) – KL divergence threshold for merging. Components with KL divergence below this are merged.

• prune_threshold (float) – Weight threshold for pruning. Components with weight below this are removed.

• config (GaussianSumConfig, optional) – Typed configuration. Mutually exclusive with the individual keyword arguments above.

• rng (numpy.random.Generator, optional) – Instance random number generator for component initialization. When None (default), falls back to the legacy global numpy.random state.

components: List[GaussianComponent]

initialize(x0, P0, num_components=1)

Initialize filter with initial state.

Parameters:

• x0 (array_like) – Initial state estimate, shape (n,).

• P0 (array_like) – Initial covariance, shape (n, n).

• num_components (int) – Number of components to initialize with. If > 1, will create multiple components with slightly perturbed means.

predict(f, F, Q)

Predict step: propagate each component.

Parameters:

• f (callable) – Nonlinear state transition function f(x).

• F (array_like) – Jacobian of f, shape (n, n).

• Q (array_like) – Process noise covariance, shape (n, n).

update(z, h, H, R)

Update step: update each component, adapt weights.

Parameters:

• z (array_like) – Measurement, shape (m,).

• h (callable) – Nonlinear measurement function h(x).

• H (array_like) – Jacobian of h, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m).

estimate()

Get overall state estimate (weighted mean and covariance).

Returns:

• x (ndarray) – Weighted mean of components.

• P (ndarray) – Weighted covariance of components.

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

get_components()

Get current mixture components.

Returns:

components – List of components with (x, P, w).

Return type:

list[GaussianComponent]

get_num_components()

Get number of current components.

pytcl.dynamic_estimation.gaussian_sum_filter.gaussian_sum_filter_predict(components, f, F, Q)

Convenience function for GSF prediction.

Parameters:

• components (list[GaussianComponent]) – Current mixture components.

• f (callable) – Nonlinear state transition function.

• F (array_like) – Jacobian of f.

• Q (array_like) – Process noise covariance.

Returns:

components_new – Predicted components.

Return type:

list[GaussianComponent]

Examples

>>> import numpy as np
>>> from pytcl.dynamic_estimation.gaussian_sum_filter import GaussianComponent
>>> # Two-component mixture for position-velocity state
>>> comp1 = GaussianComponent(
...     x=np.array([0.0, 1.0]),  # moving right
...     P=np.eye(2) * 0.5,
...     w=0.5
... )
>>> comp2 = GaussianComponent(
...     x=np.array([0.0, -1.0]),  # moving left
...     P=np.eye(2) * 0.5,
...     w=0.5
... )
>>> components = [comp1, comp2]
>>> # Constant velocity dynamics
>>> dt = 0.1
>>> f = lambda x: np.array([x[0] + x[1] * dt, x[1]])
>>> F = np.array([[1, dt], [0, 1]])
>>> Q = np.eye(2) * 0.01
>>> predicted = gaussian_sum_filter_predict(components, f, F, Q)
>>> len(predicted)
2
>>> predicted[0].w  # weights unchanged in prediction
0.5

pytcl.dynamic_estimation.gaussian_sum_filter.gaussian_sum_filter_update(components, z, h, H, R)

Convenience function for GSF update.

Parameters:

• components (list[GaussianComponent]) – Predicted mixture components.

• z (array_like) – Measurement.

• h (callable) – Nonlinear measurement function.

• H (array_like) – Jacobian of h.

• R (array_like) – Measurement noise covariance.

Returns:

components_new – Updated components with adapted weights.

Return type:

list[GaussianComponent]

Examples

>>> import numpy as np
>>> from pytcl.dynamic_estimation.gaussian_sum_filter import GaussianComponent
>>> # Two-component mixture
>>> comp1 = GaussianComponent(
...     x=np.array([1.0, 0.5]),
...     P=np.eye(2) * 0.5,
...     w=0.5
... )
>>> comp2 = GaussianComponent(
...     x=np.array([3.0, 0.5]),
...     P=np.eye(2) * 0.5,
...     w=0.5
... )
>>> components = [comp1, comp2]
>>> # Position measurement near component 1
>>> z = np.array([1.1])
>>> h = lambda x: np.array([x[0]])
>>> H = np.array([[1, 0]])
>>> R = np.array([[0.1]])
>>> updated = gaussian_sum_filter_update(components, z, h, H, R)
>>> len(updated)
2
>>> # Component 1 should have higher weight (closer to measurement)
>>> updated[0].w > updated[1].w
True

Information Filter

Information filter implementations.

The information filter is an alternative formulation of the Kalman filter that uses the information matrix Y = P^{-1} and information vector y = Y @ x instead of the state covariance P and state x.

Advantages

• Update step is additive (easy to incorporate multiple measurements)

• Natural initialization for unknown initial state (Y = 0)

• Multi-sensor fusion is simpler

• Better for some numerical conditioning

This module provides

• Information filter (standard form)

• Square-Root Information Filter (SRIF) – the name notwithstanding, only srif_update propagates the square root directly via QR/Householder transformations; srif_predict (and therefore srif_filter) routes through explicit matrix inversions and does not deliver the square-root conditioning the SRIF name implies (see srif_predict Notes, gh-25)

class pytcl.dynamic_estimation.information_filter.InformationState(y, Y)

Bases: NamedTuple

State in information form.

Variables:

• y (ndarray) – Information vector (Y @ x).

• Y (ndarray) – Information matrix (P^{-1}).

y: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

Y: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

class pytcl.dynamic_estimation.information_filter.InformationFilterResult(y_filt, Y_filt, x_filt, P_filt)

Bases: NamedTuple

Result of running an information filter.

Variables:

• y_filt (list of ndarray) – Filtered information vectors at each time step.

• Y_filt (list of ndarray) – Filtered information matrices at each time step.

• x_filt (list of ndarray) – Filtered state estimates (converted from information form).

• P_filt (list of ndarray) – Filtered covariances (converted from information form).

y_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 0

Y_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 1

x_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 2

P_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 3

class pytcl.dynamic_estimation.information_filter.SRIFState(r, R)

Bases: NamedTuple

State for Square-Root Information Filter.

Variables:

• r (ndarray) – Information vector (R @ x where Y = R.T @ R).

• R (ndarray) – Upper triangular square root of information matrix (Y = R.T @ R).

r: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

R: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

class pytcl.dynamic_estimation.information_filter.SRIFResult(r_filt, R_filt, x_filt, P_filt)

Bases: NamedTuple

Result of Square-Root Information Filter.

Variables:

• r_filt (list of ndarray) – Filtered information vectors.

• R_filt (list of ndarray) – Filtered square-root information matrices.

• x_filt (list of ndarray) – Filtered state estimates.

• P_filt (list of ndarray) – Filtered covariances.

r_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 0

R_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 1

x_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 2

P_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 3

pytcl.dynamic_estimation.information_filter.information_to_state(y, Y)

Convert information form to state form.

Parameters:

• y (array_like) – Information vector.

• Y (array_like) – Information matrix.

Returns:

• x (ndarray) – State estimate.

• P (ndarray) – State covariance.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> import numpy as np
>>> # Information form: Y = inv(P), y = Y @ x
>>> x_true = np.array([1.0, 2.0])
>>> P_true = np.eye(2) * 0.5
>>> Y = np.linalg.inv(P_true)  # Information matrix
>>> y = Y @ x_true              # Information vector
>>> x_recovered, P_recovered = information_to_state(y, Y)
>>> np.allclose(x_recovered, x_true)
True
>>> np.allclose(P_recovered, P_true)
True

pytcl.dynamic_estimation.information_filter.state_to_information(x, P)

Convert state form to information form.

Parameters:

• x (array_like) – State estimate.

• P (array_like) – State covariance.

Returns:

• y (ndarray) – Information vector.

• Y (ndarray) – Information matrix.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> import numpy as np
>>> x = np.array([1.0, 2.0])
>>> P = np.eye(2) * 0.5  # Covariance
>>> y, Y = state_to_information(x, P)
>>> Y  # Should be inv(P) = 2*I
array([[2., 0.],
       [0., 2.]])
>>> y  # Should be Y @ x = [2, 4]
array([2., 4.])

pytcl.dynamic_estimation.information_filter.information_filter(y0, Y0, measurements, F, Q, H, R, F_list=None, Q_list=None, H_list=None, R_list=None)

Run information filter on a sequence of measurements.

Parameters:

• y0 (array_like) – Initial information vector.

• Y0 (array_like) – Initial information matrix.

• measurements (list of array_like) – List of measurements. Use None for missing measurements.

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

• H (array_like) – Measurement matrix.

• R (array_like) – Measurement noise covariance.

• F_list (list of array_like, optional) – Time-varying transition matrices.

• Q_list (list of array_like, optional) – Time-varying process noise.

• H_list (list of array_like, optional) – Time-varying measurement matrices.

• R_list (list of array_like, optional) – Time-varying measurement noise.

Returns:

result – Information filter results including both information form and state form estimates.

Return type:

InformationFilterResult

Examples

>>> import numpy as np
>>> # Start with unknown state (zero information)
>>> n = 2
>>> y0 = np.zeros(n)
>>> Y0 = np.zeros((n, n))  # Unknown initial state
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.1
>>> H = np.array([[1, 0]])
>>> R = np.array([[1.0]])
>>> measurements = [np.array([1.0]), np.array([2.1]), np.array([3.3])]
>>> result = information_filter(y0, Y0, measurements, F, Q, H, R)
>>> len(result.x_filt)
3

Notes

The information filter is particularly useful when: - The initial state is completely unknown (set Y0 = 0) - Fusing measurements from multiple sensors - Measurements arrive from distributed sources

References

• Mutambara, A.G.O., “Decentralized Estimation and Control for Multisensor Systems”, CRC Press, 1998.

pytcl.dynamic_estimation.information_filter.srif_predict(r, R, F, Q)

Square-Root Information Filter prediction step.

Uses QR decomposition to maintain numerical stability.

Parameters:

• r (array_like) – Information vector (related to R @ x).

• R (array_like) – Upper triangular square root of information matrix.

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

Returns:

• r_pred (ndarray) – Predicted information vector.

• R_pred (ndarray) – Predicted square root information matrix.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> import numpy as np
>>> n = 2
>>> # Initialize with known state
>>> P0 = np.eye(n)
>>> R0 = np.linalg.cholesky(np.linalg.inv(P0)).T
>>> x0 = np.array([1.0, 0.5])
>>> r0 = R0 @ x0
>>> # Prediction step
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.1
>>> r_pred, R_pred = srif_predict(r0, R0, F, Q)
>>> r_pred.shape
(2,)
>>> R_pred.shape
(2, 2)

Notes

The prediction step converts to covariance space, propagates there, and factorizes back:

P      = inv(Y)
P_pred = F P F' + Q
R_pred = chol(inv(P_pred))

This is not the square-root algorithm the name implies. A true SRIF prediction propagates the information square root directly, by QR or Householder triangularization of a stacked array, and never forms P or inverts anything – which is the entire numerical point of the square-root formulation. Routing through two explicit inversions gives an algebraically correct answer with the conditioning the square-root form exists to avoid (gh-25).

So the result is right; the numerical robustness advertised by “square root information filter” is not delivered by this step. For an ill-conditioned Y the ordinary information filter would do no worse.

pytcl.dynamic_estimation.information_filter.srif_update(r, R, z, H, R_meas)

Square-Root Information Filter update step.

Uses Householder or Givens rotations for numerical stability.

Parameters:

• r (array_like) – Predicted information vector.

• R (array_like) – Predicted square root information matrix.

• z (array_like) – Measurement.

• H (array_like) – Measurement matrix.

• R_meas (array_like) – Measurement noise covariance.

Returns:

• r_upd (ndarray) – Updated information vector.

• R_upd (ndarray) – Updated square root information matrix.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> import numpy as np
>>> n = 2
>>> # Prior in SRIF form
>>> P_prior = np.eye(n) * 2.0
>>> R_prior = np.linalg.inv(np.linalg.cholesky(P_prior)).T
>>> x_prior = np.array([1.0, 0.5])
>>> r_prior = R_prior @ x_prior
>>> # Measurement update
>>> z = np.array([1.2])  # Position measurement
>>> H = np.array([[1, 0]])
>>> R_meas = np.array([[0.5]])  # Measurement noise covariance
>>> r_upd, R_upd = srif_update(r_prior, R_prior, z, H, R_meas)
>>> r_upd.shape
(2,)
>>> # Updated information matrix has more information (higher values)
>>> np.linalg.det(R_upd.T @ R_upd) > np.linalg.det(R_prior.T @ R_prior)
True

Notes

The update uses QR decomposition to combine the prior information with the measurement information while maintaining numerical stability.

pytcl.dynamic_estimation.information_filter.srif_filter(r0, R0, measurements, F, Q, H, R_meas)

Run Square-Root Information Filter on a sequence of measurements.

The SRIF maintains the square root of the information matrix, providing better numerical stability than the standard information filter, especially for large or ill-conditioned problems.

Parameters:

• r0 (array_like) – Initial information vector (R0 @ x0).

• R0 (array_like) –

  Initial square root information matrix (upper triangular, such that R0.T @ R0 = Y0 = P0^{-1}).

  Build it as cholesky(inv(P0)).T. The natural-looking inv(cholesky(P0)).T is not equivalent: it satisfies the identity only when P0 is diagonal, and silently gives a different factor for any correlated prior (gh-25). Both forms passed the doctests here because every example used a diagonal P0.

• measurements (list of array_like) – List of measurements. Use None for missing measurements.

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

• H (array_like) – Measurement matrix.

• R_meas (array_like) – Measurement noise covariance.

Returns:

result – SRIF results including square root information form and state form estimates.

Return type:

SRIFResult

Examples

>>> import numpy as np
>>> n = 2
>>> # Initialize with some prior knowledge
>>> P0 = np.eye(n) * 10.0
>>> R0 = np.linalg.cholesky(np.linalg.inv(P0)).T
>>> x0 = np.array([0.0, 0.0])
>>> r0 = R0 @ x0
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.1
>>> H = np.array([[1, 0]])
>>> R_meas = np.array([[1.0]])
>>> measurements = [np.array([1.0]), np.array([2.0])]
>>> result = srif_filter(r0, R0, measurements, F, Q, H, R_meas)
>>> len(result.x_filt)
2

Notes

The SRIF is algebraically equivalent to the standard Kalman filter. srif_update combines prior and measurement information via QR decomposition and delivers the square-root conditioning the SRIF name implies. srif_predict, however, converts to covariance space, propagates there, and re-factorizes – two explicit matrix inversions per step – so the filter as a whole (predict + update) does not deliver square-root numerical stability; for an ill-conditioned information matrix, the ordinary information filter would do no worse. See srif_predict Notes for the details (gh-25).

References

• Bierman, G.J., “Factorization Methods for Discrete Sequential Estimation”, Academic Press, 1977.

• Kaminski, P.G., Bryson, A.E., and Schmidt, S.F., “Discrete Square Root Filtering: A Survey of Current Techniques”, IEEE Trans. Automatic Control, 1971.

pytcl.dynamic_estimation.information_filter.fuse_information(info_states)

Fuse multiple information states.

This is useful for multi-sensor fusion where each sensor produces its own information contribution.

Parameters:

info_states (list of InformationState) – List of information states to fuse.

Returns:

fused – Fused information state.

Return type:

InformationState

Examples

>>> import numpy as np
>>> # Two sensors with different measurements
>>> state1 = InformationState(
...     y=np.array([1.0, 0.5]),
...     Y=np.array([[0.5, 0], [0, 0.1]])
... )
>>> state2 = InformationState(
...     y=np.array([0.8, 0.6]),
...     Y=np.array([[0.3, 0], [0, 0.2]])
... )
>>> fused = fuse_information([state1, state2])
>>> fused.Y[0, 0]  # Should be 0.5 + 0.3 = 0.8
0.8

Notes

Information fusion is additive:

Y_fused = Y_1 + Y_2 + … + Y_n y_fused = y_1 + y_2 + … + y_n

This is a major advantage of the information form for multi-sensor systems - each sensor can independently compute its contribution, and fusion is simply addition.

H Infinity

H-infinity filter implementation.

This module provides the H-infinity filter, a robust estimation approach that provides bounded-error estimation in the presence of model uncertainty. Unlike the Kalman filter which minimizes mean-squared error assuming Gaussian noise, the H-infinity filter minimizes the worst-case estimation error.

The H-infinity filter is particularly useful when: - Process and measurement noise statistics are uncertain - The system model contains unmodeled dynamics - Robustness to worst-case disturbances is required

References

• Simon, D., “Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches,” Wiley, 2006.

• Shen, X. and Deng, L., “Game Theory Approach to Discrete H∞ Filter Design,” IEEE Trans. Signal Processing, 1997.

• Shaked, U. and Theodor, Y., “H∞-Optimal Estimation: A Tutorial,” Proc. IEEE CDC, 1992.

class pytcl.dynamic_estimation.kalman.h_infinity.HInfinityUpdate(x, P, y, S, K, gamma, feasible)

Bases: NamedTuple

Result of H-infinity filter update step.

Variables:

• x (ndarray) – Updated state estimate.

• P (ndarray) – Updated state covariance (error bound matrix).

• y (ndarray) – Innovation (measurement residual).

• S (ndarray) – Innovation covariance.

• K (ndarray) – Filter gain.

• gamma (float) – Performance bound parameter used.

• feasible (bool) – Whether the solution satisfies the H-infinity constraint.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

y: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

S: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 3

K: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 4

gamma: float

Alias for field number 5

feasible: bool

Alias for field number 6

class pytcl.dynamic_estimation.kalman.h_infinity.HInfinityPrediction(x, P)

Bases: NamedTuple

Result of H-infinity filter prediction step.

Variables:

• x (ndarray) – Predicted state estimate.

• P (ndarray) – Predicted error bound matrix.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

pytcl.dynamic_estimation.kalman.h_infinity.hinf_predict(x, P, F, Q, B=None, u=None)

H-infinity filter prediction (time update) step.

The prediction step is identical to the standard Kalman filter:

x_pred = F @ x + B @ u P_pred = F @ P @ F’ + Q

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• P (array_like) – Current error bound matrix, shape (n, n).

• F (array_like) – State transition matrix, shape (n, n).

• Q (array_like) – Process noise covariance, shape (n, n).

• B (array_like, optional) – Control input matrix, shape (n, m).

• u (array_like, optional) – Control input, shape (m,).

Returns:

result – Named tuple with predicted state x and error bound matrix P.

Return type:

HInfinityPrediction

Examples

>>> import numpy as np
>>> x = np.array([0.0, 1.0])
>>> P = np.eye(2) * 0.1
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.01
>>> pred = hinf_predict(x, P, F, Q)
>>> pred.x
array([1., 1.])

See also

hinf_update

H-infinity measurement update step.

pytcl.dynamic_estimation.kalman.h_infinity.hinf_update(x, P, z, H, R, gamma, L=None)

H-infinity filter measurement update step.

Computes the updated state estimate that minimizes the worst-case estimation error bound. The performance level gamma determines the trade-off between robustness and estimation accuracy.

The H-infinity filter modifies the Kalman update to account for worst-case disturbances on the estimation error:

P_inv_mod = P^{-1} - gamma^{-2} * L’ @ L + H’ @ R^{-1} @ H K = P_new @ H’ @ R^{-1} x_new = x + K @ (z - H @ x) P_new = P_inv_mod^{-1}

where L is the matrix that weights the estimation error (typically the identity matrix or a subset selecting states of interest).

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• P (array_like) – Predicted error bound matrix, shape (n, n).

• z (array_like) – Measurement vector, shape (m,).

• H (array_like) – Measurement matrix, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m).

• gamma (float) – Performance bound parameter (gamma > 0). Smaller values provide more robustness but require the constraint to be feasible. As gamma -> infinity, the filter approaches the Kalman filter.

• L (array_like, optional) – Error weighting matrix, shape (p, n). Defines which linear combinations of states to bound. Default is identity (all states).

Returns:

result – Named tuple containing: - x: Updated state estimate - P: Updated error bound matrix - y: Innovation - S: Innovation covariance - K: Filter gain - gamma: Performance bound used - feasible: Whether the H-infinity constraint is satisfied

Return type:

HInfinityUpdate

Notes

The H-infinity constraint is feasible if and only if:

P^{-1} - gamma^{-2} * L’ @ L + H’ @ R^{-1} @ H > 0

If the constraint is not feasible (returns feasible=False), the result uses a regularized solution and may not satisfy the performance bound.

The parameter gamma should be chosen based on the desired robustness level. Typical values range from 1 to 100. Lower values provide more robustness but are more restrictive.

Examples

>>> import numpy as np
>>> x = np.array([1.0, 1.0])
>>> P = np.eye(2) * 0.1
>>> z = np.array([1.1])
>>> H = np.array([[1.0, 0.0]])
>>> R = np.array([[0.01]])
>>> gamma = 10.0
>>> result = hinf_update(x, P, z, H, R, gamma)
>>> result.feasible
True

See also

hinf_predict

H-infinity prediction step.

hinf_predict_update

Combined predict and update step.

References

• Simon, D., “Optimal State Estimation,” Chapter 6, Wiley, 2006.

pytcl.dynamic_estimation.kalman.h_infinity.hinf_predict_update(x, P, z, F, Q, H, R, gamma, B=None, u=None, L=None)

Combined H-infinity filter prediction and update step.

Performs prediction followed by measurement update in a single call.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• P (array_like) – Current error bound matrix, shape (n, n).

• z (array_like) – Measurement vector, shape (m,).

• F (array_like) – State transition matrix, shape (n, n).

• Q (array_like) – Process noise covariance, shape (n, n).

• H (array_like) – Measurement matrix, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m).

• gamma (float) – Performance bound parameter (gamma > 0).

• B (array_like, optional) – Control input matrix, shape (n, m).

• u (array_like, optional) – Control input, shape (m,).

• L (array_like, optional) – Error weighting matrix, shape (p, n).

Returns:

result – Named tuple with updated state, covariance, and filter quantities.

Return type:

HInfinityUpdate

See also

hinf_predict

Prediction step only.

hinf_update

Update step only.

pytcl.dynamic_estimation.kalman.h_infinity.extended_hinf_update(x, P, z, h, H, R, gamma, L=None)

Extended H-infinity filter measurement update for nonlinear systems.

Uses a linearized measurement model around the current estimate, similar to the extended Kalman filter approach.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• P (array_like) – Predicted error bound matrix, shape (n, n).

• z (array_like) – Measurement vector, shape (m,).

• h (callable) – Nonlinear measurement function h(x) -> z_pred.

• H (array_like) – Measurement Jacobian dh/dx evaluated at x, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m).

• gamma (float) – Performance bound parameter (gamma > 0).

• L (array_like, optional) – Error weighting matrix, shape (p, n).

Returns:

result – Named tuple with updated state and covariance.

Return type:

HInfinityUpdate

Notes

The innovation is computed using the nonlinear function:

y = z - h(x)

while the gain computation uses the linearized Jacobian H.

See also

hinf_update

Linear H-infinity update.

pytcl.dynamic_estimation.kalman.h_infinity.find_min_gamma(P, H, R, L=None, tol=1e-06)

Find the minimum feasible gamma for H-infinity filtering.

Computes the minimum value of gamma for which the H-infinity constraint is satisfied (P_inv_mod is positive definite).

Parameters:

• P (array_like) – Predicted error bound matrix, shape (n, n).

• H (array_like) – Measurement matrix, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m).

• L (array_like, optional) – Error weighting matrix, shape (p, n). Default is identity.

• tol (float, optional) – Tolerance for feasibility check. Default 1e-6.

Returns:

gamma_min – Minimum feasible gamma value.

Return type:

float

Notes

The minimum gamma is found by solving for when the minimum eigenvalue of P_inv_mod equals zero:

min_eig(P^{-1} - gamma^{-2} * L’ @ L + H’ @ R^{-1} @ H) = 0

This is solved via bisection search.

Examples

>>> import numpy as np
>>> P = np.eye(2) * 0.1
>>> H = np.array([[1.0, 0.0]])
>>> R = np.array([[0.01]])
>>> gamma_min = find_min_gamma(P, H, R)
>>> gamma_min < 1.0  # Typical result
True

Matrix Utils

Matrix utility functions for Kalman filter implementations.

This module provides numerically stable matrix operations used across multiple Kalman filter implementations. Separating these utilities prevents circular imports between filter implementations.

Functions include: - Cholesky factor update/downdate (Numba JIT optimized) - QR-based covariance propagation - Matrix symmetry enforcement - Matrix square root computation - Innovation likelihood computation

Performance Notes

Critical functions use Numba JIT compilation: - _cholesky_update_core: Rank-1 Cholesky update inner loop - _cholesky_downdate_core: Rank-1 Cholesky downdate inner loop

Measured 2026-08-17 (n=6, 20000 calls, warmed-up JIT, CPython 3.x on the CI-representative dev machine): the jitted _cholesky_update_core ran ~15.6x faster than the equivalent pure-Python loop. The exact ratio is size- and machine-dependent; re-measure before relying on a specific number.

pytcl.dynamic_estimation.kalman.matrix_utils.cholesky_update(S, v, sign=1.0)

Rank-1 Cholesky update/downdate.

Computes the Cholesky factor of P ± v @ v.T given S where P = S @ S.T.

Parameters:

• S (ndarray) – Lower triangular Cholesky factor, shape (n, n).

• v (ndarray) – Vector for rank-1 update, shape (n,).

• sign (float) – +1 for update (addition), -1 for downdate (subtraction).

Returns:

S_new – Updated lower triangular Cholesky factor.

Return type:

ndarray

Notes

Uses the efficient O(n²) algorithm from [1].

References

• P. E. Gill, G. H. Golub, W. Murray, and M. A. Saunders, “Methods for modifying matrix factorizations,” Mathematics of Computation, vol. 28, pp. 505-535, 1974.

Examples

>>> import numpy as np
>>> S = np.linalg.cholesky(np.eye(2))
>>> v = np.array([0.5, 0.5])
>>> S_updated = cholesky_update(S, v, sign=1.0)
>>> P_updated = S_updated @ S_updated.T
>>> np.allclose(P_updated, np.eye(2) + np.outer(v, v))
True

pytcl.dynamic_estimation.kalman.matrix_utils.qr_update(S_x, S_noise, F=None)

QR-based covariance square root update.

Computes the Cholesky factor of F @ P @ F.T + Q given S_x (where P = S_x @ S_x.T) and S_noise (where Q = S_noise @ S_noise.T).

Parameters:

• S_x (ndarray) – Lower triangular Cholesky factor of state covariance, shape (n, n).

• S_noise (ndarray) – Lower triangular Cholesky factor of noise covariance, shape (n, n).

• F (ndarray, optional) – State transition matrix, shape (n, n). If None, uses identity.

Returns:

S_new – Lower triangular Cholesky factor of the updated covariance.

Return type:

ndarray

Notes

Uses QR decomposition for numerical stability. The compound matrix [F @ S_x, S_noise].T is QR decomposed, and R.T gives the new Cholesky factor.

Examples

>>> import numpy as np
>>> S_x = np.linalg.cholesky(np.eye(2) * 0.1)
>>> S_noise = np.linalg.cholesky(np.eye(2) * 0.01)
>>> F = np.array([[1, 1], [0, 1]])
>>> S_new = qr_update(S_x, S_noise, F)

pytcl.dynamic_estimation.kalman.matrix_utils.ensure_symmetric(P)

Enforce symmetry of a covariance matrix.

Computes (P + P.T) / 2 to ensure the matrix is exactly symmetric, which can be lost due to numerical precision issues in matrix operations.

Parameters:

P (ndarray) – Square matrix to symmetrize, shape (n, n).

Returns:

P_sym – Symmetric matrix.

Return type:

ndarray

Examples

>>> import numpy as np
>>> P = np.array([[1.0, 0.5 + 1e-15], [0.5, 1.0]])
>>> P_sym = ensure_symmetric(P)
>>> np.allclose(P_sym, P_sym.T)
True

pytcl.dynamic_estimation.kalman.matrix_utils.compute_matrix_sqrt(P, scale=1.0, use_eigh_fallback=True)

Compute the matrix square root using Cholesky or eigendecomposition.

Attempts Cholesky decomposition first (faster, but requires positive definiteness). If that fails and use_eigh_fallback is True, falls back to eigendecomposition which is more robust for nearly singular matrices.

Parameters:

• P (ndarray) – Symmetric positive semi-definite matrix, shape (n, n).

• scale (float, optional) – Scale factor to multiply P by before taking square root. Default is 1.0.

• use_eigh_fallback (bool, optional) – If True, fall back to eigendecomposition if Cholesky fails. Default is True.

Returns:

sqrt_P – Lower triangular matrix such that sqrt_P @ sqrt_P.T ≈ scale * P.

Return type:

ndarray

Raises:

np.linalg.LinAlgError – If Cholesky fails and use_eigh_fallback is False.

Examples

>>> import numpy as np
>>> P = np.array([[4.0, 2.0], [2.0, 3.0]])
>>> sqrt_P = compute_matrix_sqrt(P)
>>> np.allclose(sqrt_P @ sqrt_P.T, P)
True

pytcl.dynamic_estimation.kalman.matrix_utils.compute_innovation_likelihood(innovation, S, S_is_cholesky=False)

Compute the likelihood of an innovation (measurement residual).

Computes the multivariate Gaussian probability density for the innovation, which is used for track scoring and association in multi-target tracking.

Parameters:

• innovation (ndarray) – Innovation (measurement residual) vector, shape (m,).

• S (ndarray) – Innovation covariance matrix, shape (m, m), or its lower triangular Cholesky factor if S_is_cholesky is True.

• S_is_cholesky (bool, optional) – If True, S is treated as the lower triangular Cholesky factor. Default is False.

Returns:

likelihood – Probability density value. Returns 0.0 if covariance is singular.

Return type:

float

Examples

>>> import numpy as np
>>> y = np.array([0.1, -0.2])
>>> S = np.array([[0.5, 0.1], [0.1, 0.4]])
>>> likelihood = compute_innovation_likelihood(y, S)
>>> likelihood > 0
True

pytcl.dynamic_estimation.kalman.matrix_utils.compute_mahalanobis_distance(innovation, S, S_is_cholesky=False)

Compute the Mahalanobis distance of an innovation.

The Mahalanobis distance is sqrt(y.T @ S^{-1} @ y), which measures how many standard deviations the innovation is from zero.

Parameters:

• innovation (ndarray) – Innovation (measurement residual) vector, shape (m,).

• S (ndarray) – Innovation covariance matrix, shape (m, m), or its lower triangular Cholesky factor if S_is_cholesky is True.

• S_is_cholesky (bool, optional) – If True, S is treated as the lower triangular Cholesky factor. Default is False.

Returns:

distance – Mahalanobis distance.

Return type:

float

Examples

>>> import numpy as np
>>> y = np.array([1.0, 0.0])
>>> S = np.eye(2)
>>> dist = compute_mahalanobis_distance(y, S)
>>> np.isclose(dist, 1.0)
True

pytcl.dynamic_estimation.kalman.matrix_utils.compute_merwe_weights(n, alpha=0.001, beta=2.0, kappa=0.0)

Compute sigma point weights for the Van der Merwe scaled UKF.

Parameters:

• n (int) – State dimension.

• alpha (float, optional) – Spread of sigma points around mean. Default is 1e-3.

• beta (float, optional) – Prior knowledge about distribution. Default is 2.0 (Gaussian).

• kappa (float, optional) – Secondary scaling parameter. Default is 0.0.

Returns:

• W_m (ndarray) – Mean weights, shape (2n+1,).

• W_c (ndarray) – Covariance weights, shape (2n+1,).

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Examples

>>> W_m, W_c = compute_merwe_weights(4, alpha=1e-3, beta=2.0, kappa=0.0)
>>> np.isclose(W_m.sum(), 1.0)
True

Sr Ukf

Square-root Unscented Kalman Filter (SR-UKF).

The SR-UKF propagates the square root of the covariance matrix directly, providing improved numerical stability for the Unscented Kalman Filter. This is particularly important for nonlinear systems with high-dimensional state spaces.

References

• R. van der Merwe and E. A. Wan, “The Square-Root Unscented Kalman Filter for State and Parameter-Estimation,” ICASSP 2001.

• S. J. Julier and J. K. Uhlmann, “Unscented Filtering and Nonlinear Estimation,” Proceedings of the IEEE, 2004.

pytcl.dynamic_estimation.kalman.sr_ukf.sr_ukf_predict(x, S, f, S_Q, alpha=0.001, beta=2.0, kappa=0.0)

Square-root Unscented Kalman Filter prediction step.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• S (array_like) – Lower triangular Cholesky factor of covariance, shape (n, n).

• f (callable) – State transition function f(x) -> x_next.

• S_Q (array_like) – Cholesky factor of process noise covariance.

• alpha (float, optional) – Spread of sigma points around mean. Default 1e-3.

• beta (float, optional) – Prior knowledge about distribution. Default 2.0 (Gaussian).

• kappa (float, optional) – Secondary scaling parameter. Default 0.0.

Returns:

result – Predicted state and Cholesky factor.

Return type:

SRKalmanPrediction

Examples

>>> import numpy as np
>>> def f(x):
...     return np.array([x[0] + x[1], x[1]])
>>> x = np.array([1.0, 0.5])
>>> S = np.linalg.cholesky(np.eye(2) * 0.1)
>>> S_Q = np.linalg.cholesky(np.eye(2) * 0.01)
>>> pred = sr_ukf_predict(x, S, f, S_Q)

See also

sr_ukf_update

Measurement update step.

pytcl.dynamic_estimation.kalman.sr_ukf.sr_ukf_update(x, S, z, h, S_R, alpha=0.001, beta=2.0, kappa=0.0)

Square-root Unscented Kalman Filter update step.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• S (array_like) – Lower triangular Cholesky factor of covariance, shape (n, n).

• z (array_like) – Measurement, shape (m,).

• h (callable) – Measurement function h(x) -> z.

• S_R (array_like) – Cholesky factor of measurement noise covariance.

• alpha (float, optional) – Spread of sigma points around mean. Default 1e-3.

• beta (float, optional) – Prior knowledge about distribution. Default 2.0 (Gaussian).

• kappa (float, optional) – Secondary scaling parameter. Default 0.0.

Returns:

result – Updated state and Cholesky factor.

Return type:

SRKalmanUpdate

Examples

>>> import numpy as np
>>> def h(x):
...     return np.array([x[0]])  # Measure first state
>>> x = np.array([1.0, 0.5])
>>> S = np.linalg.cholesky(np.eye(2) * 0.1)
>>> z = np.array([1.1])
>>> S_R = np.linalg.cholesky(np.array([[0.05]]))
>>> upd = sr_ukf_update(x, S, z, h, S_R)

See also

sr_ukf_predict

Prediction step.

Types

Type definitions for Kalman filter implementations.

This module provides shared NamedTuple types used across multiple Kalman filter implementations. Separating types into their own module prevents circular imports between filter implementations.

class pytcl.dynamic_estimation.kalman.types.SRKalmanState(x, S)

Bases: NamedTuple

State of a square-root Kalman filter.

Variables:

• x (ndarray) – State estimate.

• S (ndarray) – Lower triangular Cholesky factor of covariance (P = S @ S.T).

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

S: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

class pytcl.dynamic_estimation.kalman.types.SRKalmanPrediction(x, S)

Bases: NamedTuple

Result of square-root Kalman filter prediction step.

Variables:

• x (ndarray) – Predicted state estimate.

• S (ndarray) – Lower triangular Cholesky factor of predicted covariance.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

S: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

class pytcl.dynamic_estimation.kalman.types.SRKalmanUpdate(x, S, y, S_y, K, likelihood)

Bases: NamedTuple

Result of square-root Kalman filter update step.

Variables:

• x (ndarray) – Updated state estimate.

• S (ndarray) – Lower triangular Cholesky factor of updated covariance.

• y (ndarray) – Innovation (measurement residual).

• S_y (ndarray) – Lower triangular Cholesky factor of innovation covariance.

• K (ndarray) – Kalman gain.

• likelihood (float) – Measurement likelihood (for association).

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

S: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

y: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

S_y: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 3

K: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 4

likelihood: float

Alias for field number 5

class pytcl.dynamic_estimation.kalman.types.UDState(x, U, D)

Bases: NamedTuple

State of a U-D factorization filter.

The covariance is represented as P = U @ D @ U.T where U is unit upper triangular and D is diagonal.

Variables:

• x (ndarray) – State estimate.

• U (ndarray) – Unit upper triangular factor.

• D (ndarray) – Diagonal elements (1D array).

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

U: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

D: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

Ud Filter

U-D factorization Kalman filter (Bierman’s method).

The U-D filter represents the covariance matrix as P = U @ D @ U.T where U is unit upper triangular and D is diagonal. Bierman’s scalar update (ud_update_scalar, and ud_update which calls it sequentially) delivers the numerically stable, factorization-preserving update this form is known for. ud_predict does not: it reconstructs the dense covariance, propagates it exactly as an ordinary Kalman filter would, and re-factorizes from scratch, so it carries no numerical advantage over kf_predict (the two agree to ~6.7e-16) – see its Notes.

References

• G. J. Bierman, “Factorization Methods for Discrete Sequential Estimation,” Academic Press, 1977.

• C. L. Thornton and G. J. Bierman, “Gram-Schmidt Algorithms for Covariance Propagation,” Int. J. Control, 1978.

class pytcl.dynamic_estimation.kalman.ud_filter.UDState(x, U, D)

Bases: NamedTuple

State of a U-D factorization filter.

The covariance is represented as P = U @ D @ U.T where U is unit upper triangular and D is diagonal.

Variables:

• x (ndarray) – State estimate.

• U (ndarray) – Unit upper triangular factor.

• D (ndarray) – Diagonal elements (1D array).

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

U: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

D: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

pytcl.dynamic_estimation.kalman.ud_filter.ud_factorize(P)

Compute U-D factorization of a symmetric positive definite matrix.

Decomposes P = U @ D @ U.T where U is unit upper triangular and D is diagonal.

Parameters:

P (array_like) – Symmetric positive definite matrix, shape (n, n).

Returns:

• U (ndarray) – Unit upper triangular matrix.

• D (ndarray) – Diagonal elements (1D array).

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Notes

The U-D factorization is equivalent to a modified Cholesky decomposition and requires only n(n+1)/2 storage elements.

Examples

>>> import numpy as np
>>> P = np.array([[4.0, 2.0], [2.0, 3.0]])
>>> U, D = ud_factorize(P)
>>> np.allclose(U @ np.diag(D) @ U.T, P)
True

pytcl.dynamic_estimation.kalman.ud_filter.ud_reconstruct(U, D)

Reconstruct covariance matrix from U-D factors.

Parameters:

• U (array_like) – Unit upper triangular matrix.

• D (array_like) – Diagonal elements.

Returns:

P – Covariance matrix P = U @ diag(D) @ U.T.

Return type:

ndarray

Examples

>>> import numpy as np
>>> U = np.array([[1.0, 0.5], [0.0, 1.0]])
>>> D = np.array([2.0, 1.0])
>>> P = ud_reconstruct(U, D)
>>> P
array([[2.25, 0.5 ],
       [0.5 , 1.  ]])

pytcl.dynamic_estimation.kalman.ud_filter.ud_predict(x, U, D, F, Q)

U-D filter prediction step.

Parameters:

• x (array_like) – Current state estimate, shape (n,).

• U (array_like) – Unit upper triangular factor, shape (n, n).

• D (array_like) – Diagonal elements, shape (n,).

• F (array_like) – State transition matrix, shape (n, n).

• Q (array_like) – Process noise covariance, shape (n, n).

Returns:

• x_pred (ndarray) – Predicted state.

• U_pred (ndarray) – Predicted unit upper triangular factor.

• D_pred (ndarray) – Predicted diagonal elements.

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Notes

This reconstructs the dense covariance P = U @ D @ U.T, propagates it as F @ P @ F.T + Q, and re-factorizes the result with ud_factorize – it does not propagate U and D directly. Numerically this step is equivalent to kf_predict (the two agree to ~6.7e-16) and gains none of the stability benefit that Bierman’s method delivers on the update side (ud_update_scalar).

Examples

>>> import numpy as np
>>> x = np.array([1.0, 0.0])
>>> U = np.eye(2)
>>> D = np.array([0.1, 0.1])
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.01
>>> x_pred, U_pred, D_pred = ud_predict(x, U, D, F, Q)

pytcl.dynamic_estimation.kalman.ud_filter.ud_update_scalar(x, U, D, z, h, r)

U-D filter scalar measurement update (Bierman’s algorithm).

This is the most efficient form - for vector measurements, process each component sequentially.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• U (array_like) – Unit upper triangular factor, shape (n, n).

• D (array_like) – Diagonal elements, shape (n,).

• z (float) – Scalar measurement.

• h (array_like) – Measurement row vector, shape (n,).

• r (float) – Measurement noise variance.

Returns:

• x_upd (ndarray) – Updated state.

• U_upd (ndarray) – Updated unit upper triangular factor.

• D_upd (ndarray) – Updated diagonal elements.

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

Notes

This implements Bierman’s sequential scalar update algorithm which is numerically stable and efficient for U-D filters.

Examples

>>> import numpy as np
>>> x = np.array([1.0, 0.5])
>>> U = np.eye(2)
>>> D = np.array([0.2, 0.1])
>>> z = 1.1
>>> h = np.array([1.0, 0.0])
>>> r = 0.1
>>> x_upd, U_upd, D_upd = ud_update_scalar(x, U, D, z, h, r)

pytcl.dynamic_estimation.kalman.ud_filter.ud_update(x, U, D, z, H, R)

U-D filter vector measurement update.

Processes measurements sequentially using scalar updates.

Parameters:

• x (array_like) – Predicted state estimate, shape (n,).

• U (array_like) – Unit upper triangular factor, shape (n, n).

• D (array_like) – Diagonal elements, shape (n,).

• z (array_like) – Measurement vector, shape (m,).

• H (array_like) – Measurement matrix, shape (m, n).

• R (array_like) – Measurement noise covariance, shape (m, m). Should be diagonal for sequential processing.

Returns:

• x_upd (ndarray) – Updated state.

• U_upd (ndarray) – Updated unit upper triangular factor.

• D_upd (ndarray) – Updated diagonal elements.

• y (ndarray) – Innovation vector.

• likelihood (float) – Measurement likelihood.

Return type:

tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]], float]

Notes

For correlated measurement noise (non-diagonal R), the measurements are decorrelated first using a Cholesky decomposition.

Examples

>>> import numpy as np
>>> x = np.array([1.0, 0.5])
>>> U = np.eye(2)
>>> D = np.array([0.2, 0.1])
>>> z = np.array([1.1])
>>> H = np.array([[1.0, 0.0]])
>>> R = np.array([[0.1]])
>>> x_upd, U_upd, D_upd, y, likelihood = ud_update(x, U, D, z, H, R)

Smoothers

Smoothing algorithms for state estimation.

This module provides optimal fixed-interval, fixed-lag, and two-filter smoothers for linear systems. Smoothers use both past and future measurements to produce optimal state estimates.

The main algorithms are: - RTS (Rauch-Tung-Striebel) smoother for linear systems - Fixed-lag smoother for real-time applications - Fixed-interval smoother for batch processing - Two-filter smoother for parallel processing

class pytcl.dynamic_estimation.smoothers.SmoothedState(x, P)

Bases: NamedTuple

Smoothed state estimate.

Variables:

• x (ndarray) – Smoothed state estimate.

• P (ndarray) – Smoothed state covariance.

x: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

P: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

class pytcl.dynamic_estimation.smoothers.RTSResult(x_smooth, P_smooth, x_filt, P_filt)

Bases: NamedTuple

Result of RTS (Rauch-Tung-Striebel) smoother.

Variables:

• x_smooth (list of ndarray) – Smoothed state estimates at each time step.

• P_smooth (list of ndarray) – Smoothed covariances at each time step.

• x_filt (list of ndarray) – Filtered state estimates (forward pass).

• P_filt (list of ndarray) – Filtered covariances (forward pass).

x_smooth: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 0

P_smooth: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 1

x_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 2

P_filt: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 3

class pytcl.dynamic_estimation.smoothers.FixedLagResult(x_smooth, P_smooth, lag)

Bases: NamedTuple

Result of fixed-lag smoother.

Variables:

• x_smooth (list of ndarray) – Smoothed state estimates with lag L.

• P_smooth (list of ndarray) – Smoothed covariances with lag L.

• lag (int) – Smoothing lag used.

x_smooth: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 0

P_smooth: List[ndarray[tuple[Any, ...], dtype[floating]]]

Alias for field number 1

lag: int

Alias for field number 2

pytcl.dynamic_estimation.smoothers.rts_smoother(x0, P0, measurements, F, Q, H, R, F_list=None, Q_list=None, H_list=None, R_list=None)

Rauch-Tung-Striebel (RTS) fixed-interval smoother.

Runs a forward Kalman filter pass followed by a backward smoothing pass to produce optimal smoothed estimates using all measurements.

Parameters:

• x0 (array_like) – Initial state estimate, shape (n,).

• P0 (array_like) – Initial state covariance, shape (n, n).

• measurements (list of array_like) – List of measurements at each time step. Use None for missing.

• F (array_like) – State transition matrix, shape (n, n). Used if F_list not provided.

• Q (array_like) – Process noise covariance. Used if Q_list not provided.

• H (array_like) – Measurement matrix. Used if H_list not provided.

• R (array_like) – Measurement noise covariance. Used if R_list not provided.

• F_list (list of array_like, optional) – Time-varying state transition matrices.

• Q_list (list of array_like, optional) – Time-varying process noise covariances.

• H_list (list of array_like, optional) – Time-varying measurement matrices.

• R_list (list of array_like, optional) – Time-varying measurement noise covariances.

Returns:

result – Named tuple containing smoothed states/covariances and filtered states/covariances.

Return type:

RTSResult

Examples

>>> import numpy as np
>>> # Simple 1D position tracking with velocity
>>> x0 = np.array([0.0, 0.0])  # [position, velocity]
>>> P0 = np.eye(2) * 10.0
>>> F = np.array([[1, 1], [0, 1]])  # CV model
>>> Q = np.array([[0.25, 0.5], [0.5, 1.0]]) * 0.1
>>> H = np.array([[1, 0]])  # Position measurement
>>> R = np.array([[1.0]])
>>> measurements = [np.array([1.1]), np.array([2.3]), np.array([3.2])]
>>> result = rts_smoother(x0, P0, measurements, F, Q, H, R)
>>> len(result.x_smooth)
3

Notes

The RTS smoother provides the optimal linear estimate using all available data. It consists of two passes:

Forward pass (Kalman filter):

x_k|k, P_k|k from measurements z_1, …, z_k

Backward pass:

G_k = P_k|k @ F_k’ @ P_{k+1|k}^{-1} x_k|N = x_k|k + G_k @ (x_{k+1|N} - x_{k+1|k}) P_k|N = P_k|k + G_k @ (P_{k+1|N} - P_{k+1|k}) @ G_k’

where N is the total number of measurements.

References

• Rauch, H.E., Tung, F., and Striebel, C.T., “Maximum likelihood estimates of linear dynamic systems”, AIAA Journal, 1965.

pytcl.dynamic_estimation.smoothers.fixed_lag_smoother(x0, P0, measurements, F, Q, H, R, lag=5)

Fixed-lag smoother.

Produces smoothed estimates with a fixed delay of L time steps. At time k, outputs the smoothed estimate for time k-L using measurements up to time k.

Parameters:

• x0 (array_like) – Initial state estimate.

• P0 (array_like) – Initial state covariance.

• measurements (list of array_like) – List of measurements.

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

• H (array_like) – Measurement matrix.

• R (array_like) – Measurement noise covariance.

• lag (int, optional) – Smoothing lag in time steps (default: 5).

Returns:

result – Smoothed estimates with specified lag.

Return type:

FixedLagResult

Examples

>>> import numpy as np
>>> x0 = np.array([0.0, 0.0])
>>> P0 = np.eye(2) * 10.0
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.1
>>> H = np.array([[1, 0]])
>>> R = np.array([[1.0]])
>>> measurements = [np.array([i + 0.1*np.random.randn()]) for i in range(10)]
>>> result = fixed_lag_smoother(x0, P0, measurements, F, Q, H, R, lag=3)
>>> len(result.x_smooth)
10

Notes

The fixed-lag smoother is suitable for real-time applications where a delay of L steps is acceptable. It requires storing only the last L filter results instead of the entire sequence.

At each time k: - Run forward filter to get x_{k|k}, P_{k|k} - Apply backward smoothing for L steps - Output x_{k-L|k}, P_{k-L|k}

pytcl.dynamic_estimation.smoothers.fixed_interval_smoother(x0, P0, measurements, F, Q, H, R)

Fixed-interval smoother (alias for RTS smoother).

Produces optimal smoothed estimates over a fixed time interval using all measurements in that interval.

Parameters:

• x0 (array_like) – Initial state estimate.

• P0 (array_like) – Initial state covariance.

• measurements (list of array_like) – List of measurements in the interval.

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

• H (array_like) – Measurement matrix.

• R (array_like) – Measurement noise covariance.

Returns:

result – Smoothed estimates over the interval.

Return type:

RTSResult

Examples

>>> import numpy as np
>>> # 1D constant velocity model
>>> x0 = np.array([0.0, 1.0])  # [position, velocity]
>>> P0 = np.eye(2) * 5.0
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.array([[0.25, 0.5], [0.5, 1.0]]) * 0.01
>>> H = np.array([[1, 0]])
>>> R = np.array([[0.5]])
>>> measurements = [np.array([0.9]), np.array([2.1]), np.array([3.0]),
...                 np.array([4.2]), np.array([4.9])]
>>> result = fixed_interval_smoother(x0, P0, measurements, F, Q, H, R)
>>> len(result.x_smooth)
5
>>> # Smoothed estimates have lower uncertainty
>>> np.trace(result.P_smooth[2]) < np.trace(result.P_filt[2])
True

See also

rts_smoother

Full RTS smoother with time-varying parameters.

Notes

This is equivalent to the RTS smoother but with a simpler interface for the common case of time-invariant system matrices.

pytcl.dynamic_estimation.smoothers.two_filter_smoother(x0_fwd, P0_fwd, x0_bwd, P0_bwd, measurements, F, Q, H, R)

Two-filter smoother (Fraser-Potter form).

Combines forward and backward filter passes to produce smoothed estimates. Useful for parallel implementation.

Parameters:

• x0_fwd (array_like) – Initial state for forward filter.

• P0_fwd (array_like) – Initial covariance for forward filter.

• x0_bwd (array_like) – Initial state for backward filter (typically diffuse).

• P0_bwd (array_like) – Initial covariance for backward filter (typically large).

• measurements (list of array_like) – List of measurements.

• F (array_like) – State transition matrix.

• Q (array_like) – Process noise covariance.

• H (array_like) – Measurement matrix.

• R (array_like) – Measurement noise covariance.

Returns:

result – Smoothed estimates.

Return type:

RTSResult

Examples

>>> import numpy as np
>>> # 1D position-velocity system
>>> x0_fwd = np.array([0.0, 1.0])
>>> P0_fwd = np.eye(2) * 5.0
>>> # Backward filter starts with diffuse (high uncertainty) prior
>>> x0_bwd = np.array([5.0, 1.0])  # approximate final state
>>> P0_bwd = np.eye(2) * 100.0     # diffuse prior
>>> F = np.array([[1, 1], [0, 1]])
>>> Q = np.eye(2) * 0.1
>>> H = np.array([[1, 0]])
>>> R = np.array([[1.0]])
>>> measurements = [np.array([0.8]), np.array([1.9]), np.array([3.1]),
...                 np.array([4.0]), np.array([5.2])]
>>> result = two_filter_smoother(x0_fwd, P0_fwd, x0_bwd, P0_bwd,
...                               measurements, F, Q, H, R)
>>> len(result.x_smooth)
5

Notes

The two-filter smoother runs two independent Kalman filters: - Forward filter: uses measurements z_1, …, z_k - Backward filter: uses measurements z_N, …, z_{k+1}

The smoothed estimate combines both filters using information fusion:

Y_k|N = Y_k|k^{fwd} + Y_k|k^{bwd}
y_k|N = y_k|k^{fwd} + y_k|k^{bwd}

This form is useful for parallel computation since both filters can run simultaneously.

References

• Fraser, D.C. and Potter, J.E., “The optimum linear smoother as a combination of two optimum linear filters”, IEEE Trans. Automatic Control, 1969.

pytcl.dynamic_estimation.smoothers.rts_smoother_single_step(x_filt, P_filt, x_pred_next, P_pred_next, x_smooth_next, P_smooth_next, F)

Single backward step of RTS smoother.

This is a convenience wrapper around kf_smooth that returns a SmoothedState named tuple.

Parameters:

• x_filt (array_like) – Filtered state at current time.

• P_filt (array_like) – Filtered covariance at current time.

• x_pred_next (array_like) – Predicted state at next time.

• P_pred_next (array_like) – Predicted covariance at next time.

• x_smooth_next (array_like) – Smoothed state at next time.

• P_smooth_next (array_like) – Smoothed covariance at next time.

• F (array_like) – State transition matrix.

Returns:

result – Smoothed state and covariance at current time.

Return type:

SmoothedState

Examples

>>> import numpy as np
>>> # After running forward filter and getting smoothed estimate at k+1
>>> x_filt = np.array([1.0, 0.5])      # filtered state at k
>>> P_filt = np.eye(2) * 0.5           # filtered covariance at k
>>> x_pred_next = np.array([1.5, 0.5]) # predicted state at k+1
>>> P_pred_next = np.eye(2) * 0.6      # predicted covariance at k+1
>>> x_smooth_next = np.array([1.4, 0.6])  # smoothed state at k+1
>>> P_smooth_next = np.eye(2) * 0.3       # smoothed covariance at k+1
>>> F = np.array([[1, 1], [0, 1]])
>>> result = rts_smoother_single_step(x_filt, P_filt, x_pred_next,
...                                    P_pred_next, x_smooth_next,
...                                    P_smooth_next, F)
>>> result.x.shape
(2,)
>>> result.P.shape
(2, 2)