def step(
self,
u: np.ndarray = None,
e: np.ndarray = None
) -> np.ndarray:
"""
Calculates the output of the state-space model and returns it.
Updates the internal state of the model as well.
The input ``u`` is optional, as is the noise ``e``.
"""
if u is None:
u = np.zeros((self.u_dim, 1))
if e is None:
e = np.zeros((self.y_dim, 1))
Utils.validate_matrix_shape(u, (self.u_dim, 1), 'u')
Utils.validate_matrix_shape(e, (self.y_dim, 1), 'e')
x = self.xs[-1] if self.xs else self._x_init
x, y = (
self.a @ x + self.b @ u + self.k @ e,
self.output(x, u, e)
)
self.us.append(u)
self.xs.append(x)
self.ys.append(y)
return y
def step(self, y: Optional[np.ndarray], u: np.ndarray):
"""
Given an observed input ``u`` and output ``y``, update the filtered and predicted states of the Kalman filter.
Follows the implementation of the conventional Kalman filter in [1] on page 140.
The output ``y`` can be missing by setting ``y=None``.
In that case, the Kalman filter will obtain the next internal state by stepping the state space model.
[1] Verhaegen, Michel, and Vincent Verdult. *Filtering and system identification: a least squares approach.*
Cambridge university press, 2007.
"""
if y is not None:
Utils.validate_matrix_shape(y, (self.state_space.y_dim, 1), 'y')
Utils.validate_matrix_shape(u, (self.state_space.u_dim, 1), 'u')
x_pred = self.x_predicteds[-1] if self.x_predicteds else np.zeros(
(self.state_space.x_dim, 1))
p_pred = self.p_predicteds[-1] if self.p_predicteds else np.eye(
self.state_space.x_dim)
k_filtered = p_pred @ self.state_space.c.T @ np.linalg.pinv(
self.r + self.state_space.c @ p_pred @ self.state_space.c.T)
self.p_filtereds.append(p_pred -
k_filtered @ self.state_space.c @ p_pred)
self.x_filtereds.append(x_pred +
k_filtered @ (y - self.state_space.d @ u -
self.state_space.c @ x_pred)
if y is not None else x_pred)
k_pred = (
self.s + self.state_space.a @ p_pred @ self.state_space.c.T
) @ np.linalg.pinv(self.r +
self.state_space.c @ p_pred @ self.state_space.c.T)
self.p_predicteds.append(
self.state_space.a @ p_pred @ self.state_space.a.T + self.q -
k_pred @ (self.s +
self.state_space.a @ p_pred @ self.state_space.c.T).T)
x_predicted = self.state_space.a @ x_pred + self.state_space.b @ u
if y is not None:
x_predicted += k_pred @ (y - self.state_space.d @ u -
self.state_space.c @ x_pred)
self.x_predicteds.append(x_predicted)
self.us.append(u)
self.ys.append(y if y is not None else np.full((self.state_space.y_dim,
1), np.nan))
self.y_filtereds.append(
self.state_space.output(self.x_filtereds[-1], self.us[-1]))
self.y_predicteds.append(self.state_space.output(
self.x_predicteds[-1]))
self.kalman_gains.append(k_pred)
return self.y_filtereds[-1], self.y_predicteds[-1]
def __init__(self, state_space: StateSpace, noise_covariance: np.ndarray):
self.state_space = state_space
Utils.validate_matrix_shape(
noise_covariance,
(self.state_space.y_dim + self.state_space.x_dim,
self.state_space.y_dim + self.state_space.x_dim),
'noise_covariance')
self.r = noise_covariance[:self.state_space.y_dim, :self.state_space.
y_dim]
self.s = noise_covariance[
self.state_space.y_dim:, :self.state_space.y_dim]
self.q = noise_covariance[self.state_space.y_dim:,
self.state_space.y_dim:]
self.x_filtereds = []
self.x_predicteds = []
self.p_filtereds = []
self.p_predicteds = []
self.us = []
self.ys = []
self.y_filtereds = []
self.y_predicteds = []
self.kalman_gains = []
def _set_matrices(
self,
a: np.ndarray,
b: np.ndarray,
c: np.ndarray,
d: np.ndarray,
k: np.ndarray
):
""" Validate if the shapes make sense and set the system matrices. """
if k is None:
k = np.zeros((self.x_dim, self.y_dim))
Utils.validate_matrix_shape(a, (self.x_dim, self.x_dim), 'a')
Utils.validate_matrix_shape(b, (self.x_dim, self.u_dim), 'b')
Utils.validate_matrix_shape(c, (self.y_dim, self.x_dim), 'c')
Utils.validate_matrix_shape(d, (self.y_dim, self.u_dim), 'd')
Utils.validate_matrix_shape(k, (self.x_dim, self.y_dim), 'k')
self.a = a
self.b = b
self.c = c
self.d = d
self.k = k
self.xs = []
self.ys = []
self.us = []
def output(
self,
x: np.ndarray,
u: np.ndarray = None,
e: np.ndarray = None):
"""
Calculate the output of the state-space model.
This function calculates the updated :math:`y_k` of the state-space model in the class description.
The current state ``x`` is required.
Providing an input ``u`` is optional.
Providing a noise term ``e`` to be added is optional as well.
"""
if u is None:
u = np.zeros((self.u_dim, 1))
if e is None:
e = np.zeros((self.y_dim, 1))
Utils.validate_matrix_shape(x, (self.x_dim, 1), 'x')
Utils.validate_matrix_shape(u, (self.u_dim, 1), 'u')
Utils.validate_matrix_shape(e, (self.y_dim, 1), 'e')
return self.c @ x + self.d @ u + e
def test_validate_matrix_shape(self):
with self.assertRaises(ValueError):
Utils.validate_matrix_shape(np.array([[0]]), (42), 'error')
def _set_x_init(self, x_init: np.ndarray):
""" Set the initial state, if it is given. """
if x_init is None:
x_init = np.zeros((self.x_dim, 1))
Utils.validate_matrix_shape(x_init, (self.x_dim, 1), 'x_dim')
self._x_init = x_init