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, auxiliary, regularized) - Smoothers (RTS, fixed-lag, fixed-interval, two-filter) - Information filters (standard and square-root) - Batch estimation methods

class pytcl.dynamic_estimation.SmoothedState(x, P)

Bases: NamedTuple

Smoothed state estimate.

x

Smoothed state estimate.

Type:

ndarray

P

Smoothed state covariance.

Type:

ndarray

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.RTSResult(x_smooth, P_smooth, x_filt, P_filt)

Bases: NamedTuple

Result of RTS (Rauch-Tung-Striebel) smoother.

x_smooth

Smoothed state estimates at each time step.

Type:

list of ndarray

P_smooth

Smoothed covariances at each time step.

Type:

list of ndarray

x_filt

Filtered state estimates (forward pass).

Type:

list of ndarray

P_filt

Filtered covariances (forward pass).

Type:

list of ndarray

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.FixedLagResult(x_smooth, P_smooth, lag)

Bases: NamedTuple

Result of fixed-lag smoother.

x_smooth

Smoothed state estimates with lag L.

Type:

list of ndarray

P_smooth

Smoothed covariances with lag L.

Type:

list of ndarray

lag

Smoothing lag used.

Type:

int

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.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

[1]

Rauch, H.E., Tung, F., and Striebel, C.T., “Maximum likelihood estimates of linear dynamic systems”, AIAA Journal, 1965.

pytcl.dynamic_estimation.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.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.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

[1]

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.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)

class pytcl.dynamic_estimation.InformationState(y, Y)

Bases: NamedTuple

State in information form.

y

Information vector (Y @ x).

Type:

ndarray

Y

Information matrix (P^{-1}).

Type:

ndarray

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.InformationFilterResult(y_filt, Y_filt, x_filt, P_filt)

Bases: NamedTuple

Result of running an information filter.

y_filt

Filtered information vectors at each time step.

Type:

list of ndarray

Y_filt

Filtered information matrices at each time step.

Type:

list of ndarray

x_filt

Filtered state estimates (converted from information form).

Type:

list of ndarray

P_filt

Filtered covariances (converted from information form).

Type:

list of ndarray

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.SRIFState(r, R)

Bases: NamedTuple

State for Square-Root Information Filter.

r

Information vector (R^{-T} @ x where Y = R @ R.T).

Type:

ndarray

R

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

Type:

ndarray

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.SRIFResult(r_filt, R_filt, x_filt, P_filt)

Bases: NamedTuple

Result of Square-Root Information Filter.

r_filt

Filtered information vectors.

Type:

list of ndarray

R_filt

Filtered square-root information matrices.

Type:

list of ndarray

x_filt

Filtered state estimates.

Type:

list of ndarray

P_filt

Filtered covariances.

Type:

list of ndarray

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_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.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(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

[1]

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

pytcl.dynamic_estimation.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.inv(np.linalg.cholesky(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 SRIF prediction uses: - Convert to state space - Apply prediction - Use Cholesky/QR to get square root form back

pytcl.dynamic_estimation.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.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}).

• 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.inv(np.linalg.cholesky(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 but uses orthogonal transformations (QR decomposition) instead of matrix inversions. This provides:

• Better numerical stability

• Guaranteed positive semi-definiteness

• More accurate results for ill-conditioned problems

References

[1]

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

[2]

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.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.

class pytcl.dynamic_estimation.KalmanState(x, P)

Bases: NamedTuple

State of a Kalman filter.

x

State estimate.

Type:

ndarray

P

State covariance.

Type:

ndarray

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.KalmanPrediction(x, P)

Bases: NamedTuple

Result of Kalman filter prediction step.

x

Predicted state estimate.

Type:

ndarray

P

Predicted state covariance.

Type:

ndarray

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.KalmanUpdate(x, P, y, S, K, likelihood)

Bases: NamedTuple

Result of Kalman filter update step.

x

Updated state estimate.

Type:

ndarray

P

Updated state covariance.

Type:

ndarray

y

Innovation (measurement residual).

Type:

ndarray

S

Innovation covariance.

Type:

ndarray

K

Kalman gain.

Type:

ndarray

likelihood

Measurement likelihood (for association).

Type:

float

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.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.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.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.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.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.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

pytcl.dynamic_estimation.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.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.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.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.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.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.

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

Bases: NamedTuple

Sigma points and weights for unscented transform.

points

Sigma points, shape (2n+1, n).

Type:

ndarray

Wm

Weights for mean computation.

Type:

ndarray

Wc

Weights for covariance computation.

Type:

ndarray

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.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

[1]

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

pytcl.dynamic_estimation.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.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.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.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.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.

pytcl.dynamic_estimation.ckf_predict(x, P, f, Q)

Cubature Kalman filter prediction step.

The CKF uses spherical-radial cubature for numerical integration, which is more accurate than the UKF for high-dimensional states.

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.

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.])

References

[1]

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

pytcl.dynamic_estimation.ckf_update(x, P, z, h, R)

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.

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,)

class pytcl.dynamic_estimation.SRKalmanState(x, S)

Bases: NamedTuple

State of a square-root Kalman filter.

x

State estimate.

Type:

ndarray

S

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

Type:

ndarray

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.SRKalmanPrediction(x, S)

Bases: NamedTuple

Result of square-root Kalman filter prediction step.

x

Predicted state estimate.

Type:

ndarray

S

Lower triangular Cholesky factor of predicted covariance.

Type:

ndarray

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.SRKalmanUpdate(x, S, y, S_y, K, likelihood)

Bases: NamedTuple

Result of square-root Kalman filter update step.

x

Updated state estimate.

Type:

ndarray

S

Lower triangular Cholesky factor of updated covariance.

Type:

ndarray

y

Innovation (measurement residual).

Type:

ndarray

S_y

Lower triangular Cholesky factor of innovation covariance.

Type:

ndarray

K

Kalman gain.

Type:

ndarray

likelihood

Measurement likelihood (for association).

Type:

float

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

pytcl.dynamic_estimation.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.5], [0.5, 1.0]]))
>>> pred = srkf_predict(x, S, F, S_Q)
>>> pred.x
array([1., 1.])

See also

srkf_update

Measurement update step.

kf_predict

Standard Kalman filter prediction.

pytcl.dynamic_estimation.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.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.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.

x

State estimate.

Type:

ndarray

U

Unit upper triangular factor.

Type:

ndarray

D

Diagonal elements (1D array).

Type:

ndarray

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.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.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.5, 0.5],
       [0.5, 1. ]])

pytcl.dynamic_estimation.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]]]

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.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.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.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.

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

Bases: NamedTuple

State of an IMM estimator.

x

Combined state estimate, shape (n,).

Type:

ndarray

P

Combined state covariance, shape (n, n).

Type:

ndarray

mode_states

State estimates for each mode, each shape (n,).

Type:

list of ndarray

mode_covs

Covariances for each mode, each shape (n, n).

Type:

list of ndarray

mode_probs

Mode probabilities, shape (r,) where r is number of modes.

Type:

ndarray

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.IMMPrediction(x, P, mode_states, mode_covs, mode_probs, mixing_probs)

Bases: NamedTuple

Result of IMM prediction step.

x

Combined predicted state estimate.

Type:

ndarray

P

Combined predicted state covariance.

Type:

ndarray

mode_states

Predicted state estimates for each mode.

Type:

list of ndarray

mode_covs

Predicted covariances for each mode.

Type:

list of ndarray

mode_probs

Mode probabilities (unchanged during prediction).

Type:

ndarray

mixing_probs

Mixing probabilities used, shape (r, r).

Type:

ndarray

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.IMMUpdate(x, P, mode_states, mode_covs, mode_probs, mode_likelihoods)

Bases: NamedTuple

Result of IMM update step.

x

Combined updated state estimate.

Type:

ndarray

P

Combined updated state covariance.

Type:

ndarray

mode_states

Updated state estimates for each mode.

Type:

list of ndarray

mode_covs

Updated covariances for each mode.

Type:

list of ndarray

mode_probs

Updated mode probabilities.

Type:

ndarray

mode_likelihoods

Measurement likelihoods for each mode.

Type:

ndarray

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_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_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_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.IMMEstimator(n_modes, state_dim, transition_matrix, initial_mode_probs=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.

mode_states

Current state estimates for each mode.

Type:

list of ndarray

mode_covs

Current covariances for each mode.

Type:

list of ndarray

mode_probs

Current mode probabilities.

Type:

ndarray

x

Combined state estimate.

Type:

ndarray

P

Combined covariance.

Type:

ndarray

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, state_dim, transition_matrix, initial_mode_probs=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

class pytcl.dynamic_estimation.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.GaussianSumFilter(max_components=5, merge_threshold=0.01, prune_threshold=0.001)

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.

components

Current mixture components (state, covariance, weight).

Type:

list[GaussianComponent]

max_components

Maximum components to maintain (via pruning/merging).

Type:

int

merge_threshold

KL divergence threshold for merging components.

Type:

float

prune_threshold

Weight threshold below which to prune components.

Type:

float

__init__(max_components=5, merge_threshold=0.01, prune_threshold=0.001)

Initialize Gaussian Sum Filter.

Parameters:

• max_components (int) – Maximum number of Gaussian components to maintain.

• 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.

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_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_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

class pytcl.dynamic_estimation.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.RBPFFilter(max_particles=100, resample_threshold=0.5, merge_threshold=0.5)

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)

particles

Current particles with nonlinear/linear states and weights

Type:

list[RBPFParticle]

max_particles

Maximum number of particles (default 100)

Type:

int

resample_threshold

Resample when N_eff < resample_threshold * N (default 0.5)

Type:

float

merge_threshold

Merge nearby particles when KL divergence < threshold (default 0.5)

Type:

float

__init__(max_particles=100, resample_threshold=0.5, merge_threshold=0.5)

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

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, G, Qy, f, F, Qx)

Predict step: propagate particles and linear states.

Parameters:

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

• G (NDArray) – Jacobian of g with respect to y (for covariance propagation)

• 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

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_predict(particles, g, G, Qy, f, F, Qx)

Predict step for RBPF particles.

Parameters:

• particles (list[RBPFParticle]) – Current particles

• g (callable) – Nonlinear state transition

• G (NDArray) – Jacobian of nonlinear 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
>>> G = np.eye(1)
>>> 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, G, Qy, f, F, Qx)
>>> len(predicted)
3

pytcl.dynamic_estimation.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

class pytcl.dynamic_estimation.ParticleState(particles, weights)

Bases: NamedTuple

State of a particle filter.

particles

Particle states, shape (N, n).

Type:

ndarray

weights

Normalized particle weights, shape (N,).

Type:

ndarray

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.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.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.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.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.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
>>> np.mean(pred[:, 0])
0.1...

pytcl.dynamic_estimation.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.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.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_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_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.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

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)

class pytcl.dynamic_estimation.kalman.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.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.

constraints

List of active constraints.

Type:

list of ConstraintFunction

__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 not enforced here, only checked). Constraint enforcement happens in 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_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_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

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

Bases: NamedTuple

State of a Kalman filter.

x

State estimate.

Type:

ndarray

P

State covariance.

Type:

ndarray

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.KalmanPrediction(x, P)

Bases: NamedTuple

Result of Kalman filter prediction step.

x

Predicted state estimate.

Type:

ndarray

P

Predicted state covariance.

Type:

ndarray

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.KalmanUpdate(x, P, y, S, K, likelihood)

Bases: NamedTuple

Result of Kalman filter update step.

x

Updated state estimate.

Type:

ndarray

P

Updated state covariance.

Type:

ndarray

y

Innovation (measurement residual).

Type:

ndarray

S

Innovation covariance.

Type:

ndarray

K

Kalman gain.

Type:

ndarray

likelihood

Measurement likelihood (for association).

Type:

float

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.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.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.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.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.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.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

pytcl.dynamic_estimation.kalman.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.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.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.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.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.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.

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

Bases: NamedTuple

Sigma points and weights for unscented transform.

points

Sigma points, shape (2n+1, n).

Type:

ndarray

Wm

Weights for mean computation.

Type:

ndarray

Wc

Weights for covariance computation.

Type:

ndarray

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.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

[1]

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

pytcl.dynamic_estimation.kalman.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_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.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.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.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.

pytcl.dynamic_estimation.kalman.ckf_predict(x, P, f, Q)

Cubature Kalman filter prediction step.

The CKF uses spherical-radial cubature for numerical integration, which is more accurate than the UKF for high-dimensional states.

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.

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.])

References

[1]

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

pytcl.dynamic_estimation.kalman.ckf_update(x, P, z, h, R)

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.

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,)

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

Bases: NamedTuple

State of a square-root Kalman filter.

x

State estimate.

Type:

ndarray

S

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

Type:

ndarray

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.SRKalmanPrediction(x, S)

Bases: NamedTuple

Result of square-root Kalman filter prediction step.

x

Predicted state estimate.

Type:

ndarray

S

Lower triangular Cholesky factor of predicted covariance.

Type:

ndarray

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.SRKalmanUpdate(x, S, y, S_y, K, likelihood)

Bases: NamedTuple

Result of square-root Kalman filter update step.

x

Updated state estimate.

Type:

ndarray

S

Lower triangular Cholesky factor of updated covariance.

Type:

ndarray

y

Innovation (measurement residual).

Type:

ndarray

S_y

Lower triangular Cholesky factor of innovation covariance.

Type:

ndarray

K

Kalman gain.

Type:

ndarray

likelihood

Measurement likelihood (for association).

Type:

float

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

pytcl.dynamic_estimation.kalman.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

[1]

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.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.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.5], [0.5, 1.0]]))
>>> pred = srkf_predict(x, S, F, S_Q)
>>> pred.x
array([1., 1.])

See also

srkf_update

Measurement update step.

kf_predict

Standard Kalman filter prediction.

pytcl.dynamic_estimation.kalman.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.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.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.

x

State estimate.

Type:

ndarray

U

Unit upper triangular factor.

Type:

ndarray

D

Diagonal elements (1D array).

Type:

ndarray

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_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_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.5, 0.5],
       [0.5, 1. ]])

pytcl.dynamic_estimation.kalman.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]]]

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_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_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.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_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.

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

Bases: NamedTuple

Result of H-infinity filter prediction step.

x

Predicted state estimate.

Type:

ndarray

P

Predicted error bound matrix.

Type:

ndarray

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.HInfinityUpdate(x, P, y, S, K, gamma, feasible)

Bases: NamedTuple

Result of H-infinity filter update step.

x

Updated state estimate.

Type:

ndarray

P

Updated state covariance (error bound matrix).

Type:

ndarray

y

Innovation (measurement residual).

Type:

ndarray

S

Innovation covariance.

Type:

ndarray

K

Filter gain.

Type:

ndarray

gamma

Performance bound parameter used.

Type:

float

feasible

Whether the solution satisfies the H-infinity constraint.

Type:

bool

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

pytcl.dynamic_estimation.kalman.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.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

[1]

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

pytcl.dynamic_estimation.kalman.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.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.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

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.

x

State estimate.

Type:

ndarray

P

State covariance.

Type:

ndarray

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.

x

Predicted state estimate.

Type:

ndarray

P

Predicted state covariance.

Type:

ndarray

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.

x

Updated state estimate.

Type:

ndarray

P

Updated state covariance.

Type:

ndarray

y

Innovation (measurement residual).

Type:

ndarray

S

Innovation covariance.

Type:

ndarray

K

Kalman gain.

Type:

ndarray

likelihood

Measurement likelihood (for association).

Type:

float

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

[1]

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

[2]

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.

constraints

List of active constraints.

Type:

list of ConstraintFunction

__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 not enforced here, only checked). Constraint enforcement happens in 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.

points

Sigma points, shape (2n+1, n).

Type:

ndarray

Wm

Weights for mean computation.

Type:

ndarray

Wc

Weights for covariance computation.

Type:

ndarray

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

[1]

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.

pytcl.dynamic_estimation.kalman.unscented.ckf_predict(x, P, f, Q)

Cubature Kalman filter prediction step.

The CKF uses spherical-radial cubature for numerical integration, which is more accurate than the UKF for high-dimensional states.

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.

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.])

References

[1]

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)

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.

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,)

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.

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

Bases: NamedTuple

State of a square-root Kalman filter.

x

State estimate.

Type:

ndarray

S

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

Type:

ndarray

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.square_root.SRKalmanPrediction(x, S)

Bases: NamedTuple

Result of square-root Kalman filter prediction step.

x

Predicted state estimate.

Type:

ndarray

S

Lower triangular Cholesky factor of predicted covariance.

Type:

ndarray

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.square_root.SRKalmanUpdate(x, S, y, S_y, K, likelihood)

Bases: NamedTuple

Result of square-root Kalman filter update step.

x

Updated state estimate.

Type:

ndarray

S

Lower triangular Cholesky factor of updated covariance.

Type:

ndarray

y

Innovation (measurement residual).

Type:

ndarray

S_y

Lower triangular Cholesky factor of innovation covariance.

Type:

ndarray

K

Kalman gain.

Type:

ndarray

likelihood

Measurement likelihood (for association).

Type:

float

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

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

[1]

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.5], [0.5, 1.0]]))
>>> pred = srkf_predict(x, S, F, S_Q)
>>> pred.x
array([1., 1.])

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.

x

State estimate.

Type:

ndarray

U

Unit upper triangular factor.

Type:

ndarray

D

Diagonal elements (1D array).

Type:

ndarray

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.5, 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]]]

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.

x

Combined state estimate, shape (n,).

Type:

ndarray

P

Combined state covariance, shape (n, n).

Type:

ndarray

mode_states

State estimates for each mode, each shape (n,).

Type:

list of ndarray

mode_covs

Covariances for each mode, each shape (n, n).

Type:

list of ndarray

mode_probs

Mode probabilities, shape (r,) where r is number of modes.

Type:

ndarray

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.

x

Combined predicted state estimate.

Type:

ndarray

P

Combined predicted state covariance.

Type:

ndarray

mode_states

Predicted state estimates for each mode.

Type:

list of ndarray

mode_covs

Predicted covariances for each mode.

Type:

list of ndarray

mode_probs

Mode probabilities (unchanged during prediction).

Type:

ndarray

mixing_probs

Mixing probabilities used, shape (r, r).

Type:

ndarray

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.

x

Combined updated state estimate.

Type:

ndarray

P

Combined updated state covariance.

Type:

ndarray

mode_states

Updated state estimates for each mode.

Type:

list of ndarray

mode_covs

Updated covariances for each mode.

Type:

list of ndarray

mode_probs

Updated mode probabilities.

Type:

ndarray

mode_likelihoods

Measurement likelihoods for each mode.

Type:

ndarray

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, state_dim, transition_matrix, initial_mode_probs=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.

mode_states

Current state estimates for each mode.

Type:

list of ndarray

mode_covs

Current covariances for each mode.

Type:

list of ndarray

mode_probs

Current mode probabilities.

Type:

ndarray

x

Combined state estimate.

Type:

ndarray

P

Combined covariance.

Type:

ndarray

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, state_dim, transition_matrix, initial_mode_probs=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)

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

Bases: NamedTuple

State of a particle filter.

particles

Particle states, shape (N, n).

Type:

ndarray

weights

Normalized particle weights, shape (N,).

Type:

ndarray

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.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.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.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.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_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
>>> np.mean(pred[:, 0])
0.1...

pytcl.dynamic_estimation.particle_filters.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.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_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.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.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.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

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.

particles

Particle states, shape (N, n).

Type:

ndarray

weights

Normalized particle weights, shape (N,).

Type:

ndarray

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
>>> np.mean(pred[:, 0])
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 et al., “On Sequential Monte Carlo Sampling with Adaptive Weights”

(Doucet & Tadic, 2003)

• Andrieu et al., “Particle Methods for Change Detection, System Identification” (IEEE SPM, 2004)

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=100, resample_threshold=0.5, merge_threshold=0.5)

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)

particles

Current particles with nonlinear/linear states and weights

Type:

list[RBPFParticle]

max_particles

Maximum number of particles (default 100)

Type:

int

resample_threshold

Resample when N_eff < resample_threshold * N (default 0.5)

Type:

float

merge_threshold

Merge nearby particles when KL divergence < threshold (default 0.5)

Type:

float

__init__(max_particles=100, resample_threshold=0.5, merge_threshold=0.5)

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

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, G, Qy, f, F, Qx)

Predict step: propagate particles and linear states.

Parameters:

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

• G (NDArray) – Jacobian of g with respect to y (for covariance propagation)

• 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

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, G, Qy, f, F, Qx)

Predict step for RBPF particles.

Parameters:

• particles (list[RBPFParticle]) – Current particles

• g (callable) – Nonlinear state transition

• G (NDArray) – Jacobian of nonlinear 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
>>> G = np.eye(1)
>>> 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, 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