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