pathsim.blocks.kalman module¶
- class pathsim.blocks.kalman.KalmanFilter(F, H, Q, R, B=None, x0=None, P0=None, dt=None)[source]¶
Bases:
BlockDiscrete-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