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

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)

`__init__`(dyn, obs, Q, R[, missing, kappa])
`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.

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