archimedes.observers.ExtendedKalmanFilter¶

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

Extended Kalman Filter for nonlinear state estimation.

The Extended Kalman Filter (EKF) [1] handles nonlinear dynamics and observation models by linearizing them around the current state estimate using Jacobian matrices. This makes it computationally efficient while providing reasonable performance for mildly nonlinear systems.

The EKF uses first-order Taylor series approximations:

F[k] ≈ ∂f/∂x |_{x[k|k-1]}    (dynamics Jacobian)
H[k] ≈ ∂h/∂x |_{x[k|k-1]}    (observation Jacobian)

These Jacobians are computed using automatic differentiation for accuracy and efficiency.

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.

Notes

The EKF is most suitable for systems where:

Nonlinearities are mild (locally approximately linear)
Computational efficiency is important
The system dynamics and observations are smooth and differentiable

For highly nonlinear systems, consider using UnscentedKalmanFilter which can better capture nonlinear transformations of uncertainty.

The EKF algorithm consists of two steps:

Prediction: Propagate state and covariance using linearized dynamics
Update: Incorporate measurements using linearized observation model

The linearization can introduce bias in the state estimates and may cause filter divergence if the nonlinearities are too strong or if the initial estimate is far from the true state. See Gelb [2] for a general treatment of the underlying estimation theory.

Examples

>>> import numpy as np
>>> from archimedes.observers import ExtendedKalmanFilter
>>>
>>> # Define nonlinear dynamics (simple pendulum)
>>> def f(t, x):
...     dt = 0.1
...     return np.hstack([
...         x[0] + dt * x[1],                # position
...         x[1] - dt * 9.81 * np.sin(x[0])  # velocity
...     ])
>>>
>>> # Define observations
>>> def h(t, x):
...     return x[0]
>>>
>>> # Create EKF
>>> Q = np.eye(2) * 0.01
>>> R = np.array([[0.1]])
>>> ekf = ExtendedKalmanFilter(dyn=f, obs=h, Q=Q, R=R)
>>>
>>> # Filtering step
>>> x = np.array([0.1, 0.0])  # Initial state
>>> P = np.eye(2) * 0.1       # Initial covariance
>>> y = np.array([0.12])      # Measurement
>>> x_new, P_new, innov = ekf.step(0.0, x, y, P)

See also

KalmanFilterBase: Abstract base class for all Kalman filters
UnscentedKalmanFilter: Alternative for highly nonlinear systems

References

Methods

`correct`(t, x, y, P[, args])	Perform the measurement update (correction) step of the EKF.
`missing`()
`replace`(**updates)	Returns a new object replacing the specified fields with new values.
`step`(t, x, y, P[, args])	Perform one complete EKF step (prediction + update).

Attributes

`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>) → None¶

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

Perform the measurement update (correction) step of the EKF.

This method applies the measurement update equations to incorporate new observations into the state estimate. It computes the observation Jacobian automatically and updates both the state estimate and its covariance matrix.

Parameters:

t (float) – Current time step.
x (array_like) – Prior state estimate of shape (nx,), typically from the prediction step.
y (array_like) – Measurement vector of shape (ny,).
P (array_like) – Prior state covariance matrix of shape (nx, nx), typically from the prediction step.
args (tuple, optional) – Additional arguments to pass to the observation function. Default is None.

Returns:

x_post (ndarray) – Posterior (updated) state estimate of shape (nx,).
P_post (ndarray) – Posterior (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 prior state estimate.

Notes

This method can be used independently of the prediction step, allowing for custom prediction schemes or processing multiple measurements at the same time step.

The method computes the observation Jacobian H = ∂h/∂x using automatic differentiation, then applies the standard Kalman update equations:

innovation = y - h(x_prior)
S = H @ P_prior @ H.T + R
K = P_prior @ H.T @ S^(-1)
x_post = x_prior + K @ innovation
P_post = (I - K @ H) @ P_prior

Examples

>>> # Apply correction with custom prediction
>>> x_pred = custom_prediction_step(x_prev)
>>> P_pred = custom_covariance_prediction(P_prev)
>>> x_new, P_new, innov = ekf.correct(t, x_pred, y, P_pred)

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

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

Perform one complete EKF step (prediction + update).

This method implements the full Extended Kalman Filter algorithm, performing both the prediction step (using the dynamics model) and the update step (incorporating the measurement) in sequence.

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

Notes

The method performs the following sequence:

Prediction Step: - Compute dynamics Jacobian F = ∂f/∂x - Propagate state: x_pred = f(t, x) - Propagate covariance: P_pred = F @ P @ F.T + Q
Update Step: - Apply measurement correction using correct()

The automatic differentiation ensures that Jacobians are computed accurately and efficiently, even for complex nonlinear functions.

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

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