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 subspace_identification(self): """ Perform subspace identification based on the PO-MOESP method. The instrumental variable contains past outputs and past inputs. The implementation uses a QR-decomposition for numerical efficiency and is based on page 329 of [1]. A key result of this function is the eigenvalue decomposition of the :math:`R_{32}` matrix ``self.R32_decomposition``, based on which the order of the system should be determined. [1] Verhaegen, Michel, and Vincent Verdult. *Filtering and system identification: a least squares approach.* Cambridge university press, 2007. """ u_hankel = Utils.block_hankel_matrix(self.u_array, self.num_block_rows) y_hankel = Utils.block_hankel_matrix(self.y_array, self.num_block_rows) u_past, u_future = u_hankel[:, :-self. num_block_rows], u_hankel[:, self. num_block_rows:] y_past, y_future = y_hankel[:, :-self. num_block_rows], y_hankel[:, self. num_block_rows:] u_instrumental_y = np.concatenate([u_future, u_past, y_past, y_future]) q, r = map(lambda matrix: matrix.T, np.linalg.qr(u_instrumental_y.T, mode='reduced')) y_rows, u_rows = self.y_dim * self.num_block_rows, self.u_dim * self.num_block_rows self.R32 = r[-y_rows:, u_rows:-y_rows] self.R22 = r[u_rows:-y_rows, u_rows:-y_rows] self.R32_decomposition = Utils.eigenvalue_decomposition(self.R32)
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 _get_observability_matrix_decomposition(self) -> Decomposition: """ Calculate the eigenvalue decomposition of the estimate of the observability matrix as per N4SID. """ u_hankel = Utils.block_hankel_matrix(self.u_array, self.num_block_rows) y_hankel = Utils.block_hankel_matrix(self.y_array, self.num_block_rows) u_and_y = np.concatenate([u_hankel, y_hankel]) observability = self.R32 @ np.linalg.pinv(self.R22) @ u_and_y observability_decomposition = Utils.reduce_decomposition( Utils.eigenvalue_decomposition(observability), self.x_dim) return observability_decomposition
def test_unvectorize(self): matrix = np.array([ [0], [1], [2], [3], ]) result = Utils.unvectorize(matrix, num_rows=2) self.assertTrue(np.all(np.isclose(np.array([[0, 2], [1, 3]]), result))) with self.assertRaises(ValueError): Utils.unvectorize(matrix, num_rows=3) incompatible_matrix = matrix.T with self.assertRaises(ValueError): Utils.unvectorize(incompatible_matrix, 1)
def test_vectorize(self): matrix = np.array([[0, 2], [1, 3]]) result = Utils.vectorize(matrix) self.assertTrue( np.all(np.isclose(np.array([ [0], [1], [2], [3], ]), result)))
def test_block_hankel_matrix(self): matrix = np.array(range(15)).reshape((5, 3)) hankel = Utils.block_hankel_matrix(matrix, 2) desired_result = np.array([ [0., 3., 6., 9.], [1., 4., 7., 10.], [2., 5., 8., 11.], [3., 6., 9., 12.], [4., 7., 10., 13.], [5., 8., 11., 14.], ]) self.assertTrue(np.all(np.isclose(desired_result, hankel)))
def test_eigenvalue_decomposition(self): matrix = np.fliplr(np.diag(range(1, 3))) decomposition = Utils.eigenvalue_decomposition(matrix) self.assertTrue( np.all( np.isclose([[0, -1], [-1, 0]], decomposition.left_orthogonal))) self.assertTrue( np.all(np.isclose([2, 1], np.diagonal(decomposition.eigenvalues)))) self.assertTrue( np.all( np.isclose([[-1, 0], [0, -1]], decomposition.right_orthogonal))) reduced_decomposition = Utils.reduce_decomposition(decomposition, 1) self.assertTrue( np.all( np.isclose([[0], [-1]], reduced_decomposition.left_orthogonal))) self.assertTrue( np.all(np.isclose([[2]], reduced_decomposition.eigenvalues))) self.assertTrue( np.all( np.isclose([[-1, 0]], reduced_decomposition.right_orthogonal)))
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