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