def test_subspace_det_algo1_mimo(self):
"""
Subspace deterministic algorithm (MIMO).
"""
ss2 = sysid.StateSpaceDiscreteLinear(A=np.array([[0, 0.1, 0.2],
[0.2, 0.3, 0.4],
[0.4, 0.3, 0.2]]),
B=np.array([[1, 0], [0, 1],
[0, -1]]),
C=np.array([[1, 0, 0], [0, 1,
0]]),
D=np.array([[0, 0], [0, 0]]),
Q=np.diag([0.01, 0.01, 0.01]),
R=np.diag([0.01, 0.01]),
dt=0.1)
np.random.seed(1234)
prbs1 = sysid.prbs(1000)
prbs2 = sysid.prbs(1000)
def f_prbs_2d(t, x, i):
"input function"
#pylint: disable=unused-argument
i = i % 1000
return 2 * np.array([[prbs1[i] - 0.5], [prbs2[i] - 0.5]])
tf = 8
data = ss2.simulate(f_u=f_prbs_2d, x0=np.array([[0, 0, 0]]).T, tf=tf)
ss2_id = sysid.subspace_det_algo1(y=data.y,
u=data.u,
f=5,
p=5,
s_tol=0.1,
dt=ss2.dt)
data_id = ss2_id.simulate(
f_u=f_prbs_2d,
# x0=np.array(np.matrix(np.zeros(ss2_id.A.shape[0])).T),
x0=np.array([np.zeros(ss2_id.A.shape[0])]).T,
tf=tf)
nrms = sysid.nrms(data_id.y, data.y)
self.assertGreater(nrms, 0.9)
if ENABLE_PLOTTING:
for i in range(2):
plt.figure()
plt.plot(data_id.t.T,
data_id.y[i, :].T,
label='$y_{:d}$ true'.format(i))
plt.plot(data.t.T,
data.y[i, :].T,
label='$y_{:d}$ id'.format(i))
plt.legend()
plt.grid()
def test_subspace_det_algo1_siso(self):
"""
Subspace deterministic algorithm (SISO).
"""
ss1 = sysid.StateSpaceDiscreteLinear(A=0.9,
B=0.5,
C=1,
D=0,
Q=0.01,
R=0.01,
dt=0.1)
np.random.seed(1234)
prbs1 = sysid.prbs(1000)
def f_prbs(t, x, i):
"input function"
# pylint: disable=unused-argument, unused-variable
return prbs1[i]
tf = 10
data = ss1.simulate(f_u=f_prbs, x0=np.matrix(0), tf=tf)
ss1_id = sysid.subspace_det_algo1(y=data.y,
u=data.u,
f=5,
p=5,
s_tol=1e-1,
dt=ss1.dt)
data_id = ss1_id.simulate(f_u=f_prbs, x0=0, tf=tf)
nrms = sysid.subspace.nrms(data_id.y, data.y)
self.assertGreater(nrms, 0.9)
if ENABLE_PLOTTING:
plt.plot(data_id.t.T, data_id.x.T, label='id')
plt.plot(data.t.T, data.x.T, label='true')
plt.legend()
plt.grid()
def test_subspace_simulate(self):
# ss1 = sysid.ss.StateSpaceDiscreteLinear(
# A=0.9, B=0.5, C=1, D=0, Q=0.01, R=0.01, dt=0.1)
ss1 = sysid.StateSpaceDiscreteLinear(A=np.array([[0.9]]),
B=np.array([[0.5]]),
C=np.array([[1]]),
D=np.array([[0]]),
Q=np.diag([0.01]),
R=np.diag([0.01]),
dt=0.1)
np.random.seed(1234)
# prbs1 = np.array(np.matrix(sysid.subspace.prbs(1000)))
prbs1 = sysid.subspace.prbs(1000)
def f_prbs(t, x, i):
return prbs1[i]
tf = 10
data = ss1.simulate(
f_u=f_prbs,
# x0=np.array(0),
x0=np.array([[0]]).T,
tf=tf)
import sysid.subspace
import time
import pandas as pd
import scipy
import numpy as np
# import cupy as np
simulation_example = False
if simulation_example:
# 4 internal states MIMO [2 IN, 3 OUT]
ss3 = sysid.StateSpaceDiscreteLinear(
A=np.array([[0, 0.01, 0.2, 0.4], [0.1, 0.2, 0.2, 0.3], [0.11, 0.12, 0.21, 0.13], [0.11, 0.12, 0.21, 0.13]]), # X x X
B=np.array([[1, 0], [0, 1], [1, 0], [0, 1]]), # X x u
C=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 1, 0, 0]]), # y x x
D=np.array([[0, 0], [0, 0], [0, 0]]), # y x u
Q=np.diag([0.2, 0.1, 0.1, 0.1]), # X x X
R=np.diag([0.04, 0.04, 0.04]), # R? y x y
dt=0.1)
np.random.seed(1234)
prbs1 = sysid.prbs(1000)
prbs2 = sysid.prbs(1000)
prbs3 = sysid.prbs(1000)
def f_prbs_3d(t, x, i):
i = i%1000
return 2 * np.array([[prbs1[i]-0.5], [prbs2[i]-0.5]])
tf = 10
data3 = ss3.simulate(
f_u=f_prbs_3d,
def subspace_det_algo1(y, u, f, p, s_tol, dt, order=-1):
assert f > 1
assert p > 1
# setup matrices
y = np.array(y)
n_y = y.shape[0]
u = np.array(u)
n_u = u.shape[0]
w = np.vstack([y, u])
n_w = w.shape[0]
# make sure the input is column vectors
assert y.shape[0] < y.shape[1]
assert u.shape[0] < u.shape[1]
W = block_hankel(w, f + p)
U = block_hankel(u, f + p)
Y = block_hankel(y, f + p)
W_p = W[:n_w * p, :]
W_pp = W[:n_w * (p + 1), :]
Y_f = Y[n_y * f:, :]
U_f = U[n_y * f:, :]
Y_fm = Y[n_y * (f + 1):, :]
U_fm = U[n_u * (f + 1):, :]
# step 1, calculate the oblique projections
# ------------------------------------------
# Y_p = G_i Xd_p + Hd_i U_p
# After the oblique projection, U_p component is eliminated,
# without changing the Xd_p component:
# Proj_perp_(U_p) Y_p = W1 O_i W2 = G_i Xd_p
O_i = Y_f @ project_oblique(U_f, W_p)
O_im = Y_fm @ project_oblique(U_fm, W_pp)
# step 2, calculate the SVD of the weighted oblique projection
# ------------------------------------------
# given: W1 O_i W2 = G_i Xd_p
# want to solve for G_i, but know product, and not Xd_p
# so can only find Xd_p up to a similarity transformation
W1 = np.eye(O_i.shape[0])
W2 = np.eye(O_i.shape[1])
U0, s0, VT0 = np.linalg.svd(W1 @ O_i @ W2) # pylint: disable=unused-variable
# step 3, determine the order by inspecting the singular
# ------------------------------------------
# values in S and partition the SVD accordingly to obtain U1, S1
# print s0
if order == -1:
n_x = int(np.where(s0 / s0.max() > s_tol)[0][-1] +
1) # Cupy doesn't see it as int, but as ndarray
else:
n_x = order
# print("n_x", n_x)
U1 = U0[:, :n_x]
# S1 = np.matrix(np.diag(s0[:n_x]))
# VT1 = VT0[:n_x, :n_x]
# step 4, determine Gi and Gim
# ------------------------------------------
G_i = np.linalg.pinv(W1) @ U1 @ np.diag(np.sqrt(s0[:n_x]))
G_im = G_i[:-n_y, :] # check
# step 5, determine Xd_ip and Xd_p
# ------------------------------------------
# only know Xd up to a similarity transformation
Xd_i = np.linalg.pinv(G_i) @ O_i
Xd_ip = np.linalg.pinv(G_im) @ O_im
# step 6, solve the set of linear eqs
# for A, B, C, D
# ------------------------------------------
Y_ii = Y[n_y * p:n_y * (p + 1), :]
U_ii = U[n_u * p:n_u * (p + 1), :]
a_mat = np.vstack([Xd_ip, Y_ii])
b_mat = np.vstack([Xd_i, U_ii])
# ss_mat = a_mat*b_mat.I
ss_mat = a_mat @ np.linalg.pinv(b_mat)
A_id = ss_mat[:n_x, :n_x]
B_id = ss_mat[:n_x, n_x:]
assert B_id.shape[0] == n_x
assert B_id.shape[1] == n_u
C_id = ss_mat[n_x:, :n_x]
assert C_id.shape[0] == n_y
assert C_id.shape[1] == n_x
D_id = ss_mat[n_x:, n_x:]
assert D_id.shape[0] == n_y
assert D_id.shape[1] == n_u
if np.linalg.matrix_rank(C_id) == n_x:
T = np.linalg.pinv(
C_id
) # try to make C identity, want it to look like state feedback
else:
T = np.eye(n_x)
Q_id = np.zeros((n_x, n_x))
R_id = np.zeros((n_y, n_y))
sys = sysid.StateSpaceDiscreteLinear(A=np.linalg.pinv(T) @ A_id @ T,
B=np.linalg.pinv(T) @ B_id,
C=C_id @ T,
D=D_id,
Q=Q_id,
R=R_id,
dt=dt)
return sys
