archimedes.observers.UnscentedKalmanFilter¶

class archimedes.observers.UnscentedKalmanFilter(dyn: ~typing.Callable, obs: ~typing.Callable, Q: ~numpy.ndarray, R: ~numpy.ndarray, missing: ~typing.Callable[[~numpy.ndarray], bool] = <function _default_missing>, kappa: float = 0.0)¶

Unscented Kalman Filter for highly nonlinear state estimation.

The Unscented Kalman Filter (UKF) [2] uses the unscented transform to handle nonlinear dynamics and observation models. Instead of linearizing like the EKF, it propagates a carefully chosen set of sample points (sigma points) through the nonlinear transformations to capture the mean and covariance to higher-order accuracy.

The UKF provides several advantages over the Extended Kalman Filter:

Higher accuracy: Captures mean and covariance to 2nd order for any nonlinearity (3rd order for Gaussian distributions)
No Jacobian computation: Uses sigma points instead of derivatives
Better stability: Less prone to divergence with strong nonlinearities
Symmetric treatment: Handles positive and negative perturbations equally

The filter uses the Julier sigma point generation scheme [1] with symmetric scaling, providing 2n+1 sigma points for an n-dimensional state.

Parameters:

dyn (callable) – Dynamics function with signature f(t, x, *args) that returns the predicted state at the next time step. Must be compatible with automatic differentiation for gradient-based filters.
obs (callable) – Observation function with signature h(t, x, *args) that maps state to expected measurements. Must be compatible with automatic differentiation for gradient-based filters.
Q (array_like) – Process noise covariance matrix of shape (nx, nx) where nx is the state dimension. Must be positive semi-definite.
R (array_like) – Measurement noise covariance matrix of shape (ny, ny) where ny is the measurement dimension. Must be positive definite.
kappa (float, default=0.0) –
Sigma point scaling parameter. Controls the spread of sigma points around the mean. Typical values:
- kappa = 0 (default): Minimal spread, good for most applications
- kappa = 3 - n: Traditional choice for Gaussian distributions
- kappa > 0: Larger spread, may help with highly nonlinear systems
where n is the state dimension.

Notes

The UKF is most suitable for systems where:

Nonlinearities are moderate to strong
Higher accuracy is needed than EKF can provide
The computational cost (≈ 3x EKF) is acceptable
Robustness to modeling errors is important

For mildly nonlinear systems where computational efficiency is critical, ExtendedKalmanFilter may be more appropriate.

The UKF algorithm consists of:

Sigma point generation: Create 2n+1 points capturing mean/covariance
Prediction: Propagate points through dynamics, compute statistics
Update: Propagate points through observation model, apply correction

The implementation uses the Julier symmetric sigma point scheme with equal weights for covariance calculations, providing excellent numerical properties. See Van der Merwe [3] for a comprehensive treatment of sigma-point filtering.

Examples

>>> import numpy as np
>>> from archimedes.observers import UnscentedKalmanFilter
>>>
>>> # Define highly nonlinear dynamics (attitude dynamics)
>>> def f(t, x):
...     dt = 0.1
...     q0, q1, q2, q3, wx, wy, wz = x
...     # Quaternion kinematics (highly nonlinear)
...     dq = 0.5 * dt * np.hstack([
...         -wx*q1 - wy*q2 - wz*q3,
...         wx*q0 + wz*q2 - wy*q3,
...         wy*q0 - wz*q1 + wx*q3,
...         wz*q0 + wy*q1 - wx*q2
...     ])
...     return np.hstack([x[:4] + dq, x[4:]])  # Constant angular velocity
>>>
>>> # Nonlinear observation (magnetic field measurement)
>>> def h(t, x):
...     q0, q1, q2, q3 = x[:4]
...     # Rotate reference vector by quaternion (highly nonlinear)
...     R11 = q0**2 + q1**2 - q2**2 - q3**2
...     R21 = 2*(q1*q2 + q0*q3)
...     R31 = 2*(q1*q3 - q0*q2)
...     return np.hstack([R11, R21, R31])  # Simplified magnetic field
>>>
>>> # Create UKF with appropriate scaling
>>> Q = np.eye(7) * 0.01  # Process noise
>>> R = np.eye(3) * 0.1   # Measurement noise
>>> ukf = UnscentedKalmanFilter(dyn=f, obs=h, Q=Q, R=R, kappa=0.0)
>>>
>>> # Filtering step
>>> x = np.array([1, 0, 0, 0, 0.1, 0.1, 0.1])  # Initial quaternion + rates
>>> P = np.eye(7) * 0.1                         # Initial covariance
>>> y = np.array([0.95, 0.2, 0.15])            # Magnetic field measurement
>>> x_new, P_new, innov = ukf.step(0.0, x, y, P)

See also

KalmanFilterBase: Abstract base class for all Kalman filters
ExtendedKalmanFilter: More efficient alternative for mildly nonlinear systems

References

Methods

`missing`()
`replace`(**updates)	Returns a new object replacing the specified fields with new values.
`step`(t, x, y, P[, args])	Perform one complete UKF step using the unscented transform.

Attributes

`kappa`
`nx`	State dimension derived from process noise covariance matrix.
`ny`	Measurement dimension derived from measurement noise covariance matrix.
`dyn`
`obs`
`Q`
`R`

__init__(dyn: ~typing.Callable, obs: ~typing.Callable, Q: ~numpy.ndarray, R: ~numpy.ndarray, missing: ~typing.Callable[[~numpy.ndarray], bool] = <function _default_missing>, kappa: float = 0.0) → None¶

replace(**updates) → T¶: Returns a new object replacing the specified fields with new values.

step(t, x, y, P, args=None)¶

Perform one complete UKF step using the unscented transform.

This method implements the full Unscented Kalman Filter algorithm, performing both the prediction step (using sigma points propagated through the dynamics model) and the update step (incorporating the measurement through the unscented transform) in sequence.

The UKF uses sigma points to capture the mean and covariance of the state distribution to higher-order accuracy without requiring Jacobian computations. This makes it particularly suitable for highly nonlinear systems where the EKF’s linearization may be inadequate.

Parameters:

t (float) – Current time step.
x (array_like) – Previous state estimate of shape (nx,).
y (array_like) – Current measurement vector of shape (ny,).
P (array_like) – Previous state covariance matrix of shape (nx, nx).
args (tuple, optional) – Additional arguments to pass to both dynamics and observation functions. Default is None.

Returns:

x_new (ndarray) – Updated state estimate of shape (nx,).
P_new (ndarray) – Updated state covariance matrix of shape (nx, nx).
innovation (ndarray) – Measurement residual of shape (ny,), computed as the difference between the actual measurement and the predicted measurement from the unscented transform.

Notes

The method performs the following sequence:

Sigma Point Generation: Create 2n+1 sigma points using the Julier symmetric scheme to capture the current state distribution.
Prediction Step: - Propagate sigma points through dynamics function - Compute predicted state mean using weighted sigma points - Compute predicted covariance using weighted outer products - Regenerate sigma points around predicted state (critical for accuracy)
Update Step: - Propagate predicted sigma points through observation function - Compute predicted measurement mean using weighted sigma points - Compute innovation covariance and cross-covariance matrices - Apply Kalman update equations using computed statistics

The regeneration of sigma points around the predicted state in step 2 is crucial for maintaining the accuracy of the unscented transform, especially for highly nonlinear dynamics.

The weights are computed using the Julier scheme where mean weights Wm and covariance weights Wc are identical, providing symmetric treatment of positive and negative perturbations.

Examples

>>> # Complete UKF filtering step for nonlinear system
>>> x_k, P_k, innov = ukf.step(t_k, x_prev, y_k, P_prev)
>>>
>>> # With additional function arguments
>>> extra_params = (param1, param2)
>>> x_k, P_k, innov = ukf.step(t_k, x_prev, y_k, P_prev, args=extra_params)

property nx¶: State dimension derived from process noise covariance matrix.

property ny¶: Measurement dimension derived from measurement noise covariance matrix.