kalmankit.extended module

Implementation of the extended Kalman filter algorithm

class kalmankit.extended.ExtendedKalmanFilter(xk: ndarray, Pk: ndarray, Q: ndarray, R: ndarray, f: Callable, h: Callable, jacobian_A: Callable, jacobian_H: Callable)[source]

Bases: object

Extended Kalman filter algorithm.

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

\[\begin{split}x_{k} = f(x_{k-1}, u_{k-1}) + q_{k} \\ z_{k} = h(x_{k}) + r_{k}\end{split}\]

with \(q_{k} \sim \mathcal{N}(0, Q)\), \(r_{k} \sim \mathcal{N}(0, R)\) and \(f\) and \(h\) being nonlinear functions.

For each time step \(k\)

  1. Predict step

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

with \(A_{k} = \frac{\partial f}{x}\) evaluated at \(\hat{x}_{k-1}^{-}\).

  1. 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(\hat{x}_{k}^{-})) \\ P_k = (I - K_k H) P_{k}^{-}\end{split}\]

with \(H_{k} = \frac{\partial h}{x}\) evaluated at \(\hat{x}_{k}^{-}\).

The Extended Kalman Filter can deal with non-linear systems but it does not guarantee an optimal estimation unless the system is linear and the noises are drawn from a normal distribution, which will cause the Extended Kalman Filter to behave as a standard Kalman Filter.

Parameters:

  • xk (numpy.ndarray) – Initial (\(k=0\)) mean estimate.

  • Pk (numpy.ndarray) – Initial (\(k=0\)) covariance estimate.

  • Q (numpy.ndarray) – Process noise covariance (transition covariance).

  • R (numpy.ndarray or float.) – Measurement noise covariance (observation covariance).

  • f (function) – Non-linear state transition function. It is a nonlinear function that relates the state at the previous time step \(k-1\) to state at the current step \(k\). It must receive two arguments: \(xk\) and \(uk\), which are the state and the control-input.

  • h (function) – Non-linear observation function. It is a nonlinear function that relates the prior state \(xk\) to the measurement \(zk\). It must receive one argument: \(xk\).

  • jacobian_A (function) – The function that computes the jacobian of \(f\) with respect to \(x\). It must receive two arguments: \(x\) and \(u\).

  • jacobian_H (function) – The function that computes the jacobian of \(h\) with respect to \(x\). It must receive one argument: \(x\).

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(xk: 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}^{-} = f(\hat{x}_{k-1}^{-}, 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:

  • xk (numpy.ndarray) – Mean estimate at time \(k\).

  • uk (numpy.ndarray) – Control-input vector at time \(k\).

  • 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]

Extended Rauch-Tung-Striebel (RTS) smoother.

update(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(\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:

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