kalmankit.standard module¶

Vanilla implementation of the standard Kalman filter algorithm

class kalmankit.standard.KalmanFilter(A: ndarray, xk: ndarray, B: ndarray, Pk: ndarray, H: ndarray, Q: ndarray, R: ndarray, state_size: int = None)[source]¶

Bases: object

Standard Kalman filter algorithm.

The standard Kalman Filter uses a form of feedback-control loop of two stages to model dynamic linear systems of the form:

\[\begin{split}x_{k} = A x_{k} + B u_{k} + q_{k} \\ z_{k} = H x_{k} + r_{k}\end{split}\]

with \(q_{k} \sim \mathcal{N}(0, Q)\) and \(r_{k} \sim \mathcal{N}(0, R)\)

For each time step \(k\)

Predict step

\[\begin{split}\hat{x}_{k}^{-} = A_{k} \hat{x}_{k-1}^{-} + B_{k} u_{k-1} \\ P_{k}^{-} = A_{k}P_{k-1}A_{k}^{T} + Q_{k}\end{split}\]

Update step

\[\begin{split}K_k = P_{k}^{-} H_{k}^{T} (H_{k} P_{k}^{-} H_{k}^{T} + R_{k})^{-1} \\ \hat{x}_{k} = \hat{x}_{k}^{-} + K_k (z_k - H_{k} \hat{x}_{k}^{-}) \\ P_k = (I - K_k H) P_{k}^{-}\end{split}\]

See 2nd and 3rd references to understand notation.

Parameters:

A (numpy.ndarray) – Transition matrix. A matrix that relates the state at the previous time step \(k-1\) to the state at the current step \(k\).
xk (numpy.ndarray) – Initial (\(k=0\)) mean estimate.
B (numpy.ndarray) – Control-input matrix.
Pk (numpy.ndarray) – Initial (\(k=0\)) covariance estimate.
H (numpy.ndarray) – Observation matrix. A matrix that relates the prior state \(xk\) to the measurement \(z_{k}\).
Q (numpy.ndarray) – Process noise covariance (transition covariance).
R (numpy.ndarray or float.) – Measurement noise covariance (observation covariance).

state_size¶

Dimensionality of the state \((n)\).

Type:: int

I¶

Identity matrix \((n x n)\). This attribute is not accessible by the user.

Type:: numpy.ndarray

kalman_gains¶

The list of computed Kalman gains.

Type:: list of numpy.ndarray

Returns:

states (numpy.ndarray) – A posteriori state estimates for each time step.
errors (numpy.ndarray) – A posteriori estimate error covariances for each time step.

References

Warning

Please, take your time to ensure the matrices shapes are correct or the filter will not work properly.

filter(Z: ndarray, U: ndarray | None = None) → Tuple[ndarray, ndarray][source]¶

Run filter over Z and U.

Applies the filtering process over \(Z\) and \(U\) and returns all errors and covariances. That is: given \(Z\) and \(U\), this function applies the predict and update feedback loop for each \(zk\), where \(k\) is a timestamp.

Parameters:

Z (numpy.ndarray) – Observed variable
U (numpy.ndarray, optional, default: None) – Control-input vector.

Returns:

states (list of numpy.ndarray) – A posteriori state estimates for each time step \(k\).
errors (list of numpy.ndarray) – A posteriori estimate error covariances for each time step \(k\).

predict(Ak: ndarray, xk: ndarray, Bk: ndarray, uk: ndarray, Pk: ndarray, Qk: ndarray) → Tuple[ndarray, ndarray][source]¶

Predicts states and covariances.

Predict step of the Kalman filter. Computes the prior values of state and covariance using the previous timestep (if any).

\[\begin{split}\hat{x}_{k}^{-} = A_{k} \hat{x}_{k-1}^{-} + B_{k} u_{k-1} \\ P_{k}^{-} = A_{k}P_{k-1}A_{k}^{T} + Q_{k}\end{split}\]

Here, the output are the a priori or predicted estimates of the mean \(\hat{x}_{k}^{-}\) and covariance \(P_{k}^{-}\).

Parameters:

Ak (numpy.ndarray) – Transition matrix at time \(k\).
xk (numpy.ndarray) – Mean estimate at time \(k\).
Bk (numpy.ndarray) – Control-input matrix at time \(k\). Can be set to None to ignore the control part of the filter.
uk (numpy.ndarray) – Control-input vector at time \(k\). Can be set to None to ignore the control part of the filter.
Pk (numpy.ndarray) – Covariance estimate at time \(k\).
Qk (numpy.ndarray) – Process noise covariance at time \(k\).

Returns:

xk_prior (numpy.ndarray) – Prior value of state mean.
Pk_prior (numpy.ndarray) – Prior value of state covariance.

smooth(Z: ndarray, U: ndarray | None = None) → Tuple[ndarray, ndarray][source]¶

Rauch-Tung-Striebel (RTS) smoother.

The smoothing process refines the estimates in the light of new data. Formally, for each time step \(k\), the smoothing process can be described as

\[\begin{split}x_{k+1}^{-} = A_{k} x_{k} \\ P_{k+1}^{-} = A_{k} P_{k} A_{k}^{T} + Q_{k} \\ G_{k} = P_{k} A_{k}^{T} \left( P_{k+1}^{-} \right)^{-1} \\ x_{k}^{s} = x_{k}+G_{k} \left( m_{k+1}^{s} - m_{k+1}^{-} \right) \\ P_{k}^{s} = P_{k} + G_{k} \left( P_{k+1}^{s} - P_{k+1}^{-} \right) G_{k}^{T}\end{split}\]

Parameters:

Z (numpy.ndarray) – Observed variable
U (numpy.ndarray, optional, default: None) – Control-input vector.

Returns:

xk_smooth (list of numpy.ndarray) – Smoothed means for each time step \(k\).
Pk_smooth (list of numpy.ndarray) – Smoothed error covariances for each time step \(k\).

update(Hk: ndarray, xk: ndarray, Pk: ndarray, zk: ndarray, Rk: ndarray) → Tuple[ndarray, ndarray][source]¶

Updates states and covariances.

Update step of the Kalman filter. That is, the filter combines the predictions with the observed variable \(Z\) at time \(k\).

\[\begin{split}K_k = P_{k}^{-} H_{k}^{T} (H_{k} P_{k}^{-} H_{k}^{T}+R_{k})^{-1} \\ \hat{x}_{k} = \hat{x}_{k}^{-}+K_k (z_k - H_{k} \hat{x}_{k}^{-}) \\ P_k = (I - K_k H) P_{k}^{-}\end{split}\]

Here, the output are the a posteriori or corrected estimates of the mean \(\hat{x}_{k}\) and covariance \(P_k\).

Parameters:

Hk (numpy.ndarray) – Observation matrix at time \(k\).
xk (numpy.ndarray) – Prior mean state estimate at time \(k\).
Pk (numpy.ndarray) – Prior covariance state estimate at time \(k\).
zk (numpy.ndarray) – Observation at time \(k\).
Rk (numpy.ndarray) – Measurement noise covariance at time \(k\).

Returns:

xk_posterior (numpy.ndarray) – A posteriori estimate error mean at time \(k\).
Pk_posterior (numpy.ndarray) – A posteriori estimate error covariance at time \(k\).

Notes

The original formula to compute the Kalman gain would be implemented as Kk = Pk @ (Hk.T @ np.linalg.pinv(Sk)). Instead, a linear matrix is solved due to being a more efficient and precise way.