Kalman Filter

class pathsim.blocks.kalman.KalmanFilter(F, H, Q, R, B=None, x0=None, P0=None, dt=None)

Bases: Block

Discrete-time Kalman filter for state estimation.

Implements the standard Kalman filter algorithm to estimate the state of a linear dynamic system from noisy measurements. The filter recursively updates state estimates by combining predictions from a system model with incoming measurements, weighted by their respective uncertainties.

The filter processes measurements at each time step through a two-stage process: prediction (using the system model) and update (incorporating measurements).

The system model is:

\[ \begin{align}\begin{aligned}x_{k+1} = F x_k + B u_k + w_k\\z_k = H x_k + v_k\end{aligned}\end{align} \]

where \(w_k \sim \mathcal{N}(0, Q)\) is process noise and \(v_k \sim \mathcal{N}(0, R)\) is measurement noise.

At each time step, the filter performs:

Prediction:

\[ \begin{align}\begin{aligned}\hat{x}_{k|k-1} = F \hat{x}_{k-1} + B u_k\\P_{k|k-1} = F P_{k-1} F^T + Q\end{aligned}\end{align} \]

Update:

\[ \begin{align}\begin{aligned}y_k = z_k - H \hat{x}_{k|k-1}\\S_k = H P_{k|k-1} H^T + R\\K_k = P_{k|k-1} H^T S_k^{-1}\\\hat{x}_k = \hat{x}_{k|k-1} + K_k y_k\\P_k = (I - K_k H) P_{k|k-1}\end{aligned}\end{align} \]

Note

The block expects inputs in the following order:

• First m inputs: measurements \(z\)

• Next p inputs (if B is provided): control inputs \(u\)

The block outputs the n-dimensional state estimate \(\hat{x}\).

Parameters:

• F (ndarray) – State transition matrix (n x n). Describes how the state evolves from one time step to the next.

• H (ndarray) – Measurement matrix (m x n). Maps the state space to the measurement space.

• Q (ndarray) – Process noise covariance matrix (n x n). Represents uncertainty in the system model.

• R (ndarray) – Measurement noise covariance matrix (m x m). Represents uncertainty in the measurements.

• B (ndarray, optional) – Control input matrix (n x p). Maps control inputs to state changes. Default is None (no control input).

• x0 (ndarray, optional) – Initial state estimate (n,). Default is zero vector.

• P0 (ndarray, optional) – Initial error covariance matrix (n x n). Default is identity matrix.

x

Current state estimate \(\hat{x}_k\)

Type:

ndarray

P

Current error covariance matrix \(P_k\)

Type:

ndarray

n

State dimension

Type:

int

m

Measurement dimension

Type:

int

p

Control input dimension

Type:

int

to_checkpoint(prefix, recordings=False)

Serialize Kalman filter state estimate and covariance.

load_checkpoint(prefix, json_data, npz)

Restore Kalman filter state estimate and covariance.

sample(t, dt)

Sample after successful timestep.

Updates the internal state estimate using the current measurements and control inputs, then outputs the updated state estimate.

Parameters:

• t (float) – evaluation time for sampling

• dt (float) – integration timestep