def test_State_algebra_mul_rmul_miso_simo(): # gh-73 # MISO Example G1 = Transfer( [[[1], [1]]], # end of num [1, 2, 1] # common den ) G2 = transfer_to_state(G1) G3 = Transfer([1], [1, 0]) # Pure integrator G = G2 * G3 assert G.NumberOfStates == 3 assert G.NumberOfInputs == 2 assert G.NumberOfOutputs == 1 # SIMO Example G1 = Transfer( [[1], [1]], # end of num [1, 2, 1] # common den ) G2 = transfer_to_state(G1) G3 = Transfer([1], [1, 0]) # Pure integrator G = G2 * G3 assert G.NumberOfStates == 3 assert G.NumberOfInputs == 1 assert G.NumberOfOutputs == 2
def test_transfer_to_state(): # Models with static column/row num, den = [[1, -1], [[1, -1], 0]], [[[1, 2], 1], [[1, 2], 1]] den2, num2 = [list(i) for i in zip(*den)], [list(i) for i in zip(*num)] G = Transfer(num, den) H = Transfer(num2, den2) Gs = transfer_to_state(G) Hs = transfer_to_state(H) Gm = concatenate_state_matrices(Gs) Hm = concatenate_state_matrices(Hs) assert_array_almost_equal(Gm, np.array([[-2, 1, 0], [1, 0, -1], [-3, 1, 0]])) assert_array_almost_equal( Hm, np.array([[-2., 0., 1., 0.], [0., -2., 0., 1.], [1., -3., 0., 1.], [0., 0., -1., 0.]])) # Example from Kalman 1963 num = [[3 * np.poly([-3, -5]), [6, 6], [2, 7], [2, 5]], [2, 1, [2, 10], [8, 16]], [[2, 14, 36], [-2, 0], 1, 2 * np.convolve([5, 17], [1, 2])]] den = [[np.poly([-1, -2, -4]), [1, 6, 8], [1, 7, 12], [1, 5, 6]], [[1, 8, 15], [1, 3], np.poly([-1, -2, -3]), np.poly([-1, -3, -5])], [np.poly([-1, -3, -5]), [1, 4, 3], [1, 3], np.poly([-1, -3, -5])]] G = Transfer(num, den) H = transfer_to_state(G) p = H.poles p.sort() assert_array_almost_equal( p, np.array([ -5. + 0.j, -5. + 0.j, -4. + 0.j, -3. + 0.j, -3. + 0.j, -3. + 0.j, -2. + 0.j, -2. + 0.j, -1. + 0.j, -1. + 0.j, -1. + 0.j ])) # Reported in gh-#42 G = Transfer([[[87.8, 8.78], [-103.68, -8.64]], [[129.84, 10.82], [-109.6, -10.96]]], [562.5, 82.5, 1]) Gss = transfer_to_state(G) assert_array_almost_equal( Gss.a, np.kron(np.eye(2), [[0., 1.], [-2 / 1125, -11 / 75]])) assert_array_almost_equal(Gss.b, [[0, 0], [1, 0], [0, 0], [0, 1]]) des_c = np.array([[ 0.01560888888888889, 0.1560888888888889, -0.015360000000000002, -0.18432 ], [ 0.019235555555555558, 0.23082666666666668, -0.019484444444444447, -0.19484444444444443 ]]) assert_array_almost_equal(Gss.c, des_c) assert_array_almost_equal(Gss.d, np.zeros([2, 2]))
def test_static_model_conversion_sampling_period(): G = State(np.eye(5), dt=0.001) H = state_to_transfer(G) assert_(H._isgain) # 0 assert_(not H._isSISO) # 1 assert_equal(H.SamplingPeriod, 0.001) # 2 K = transfer_to_state(H) assert_equal(K.SamplingPeriod, 0.001) # 3
def test_system_norm_simple(): G = Transfer([100], [1, 10, 100]) assert_almost_equal(system_norm(G), 1.1547, decimal=5) assert_almost_equal(system_norm(G, p=2), 2.2360679) F = transfer_to_state(G) assert_almost_equal(system_norm(F), 1.1547, decimal=5) assert_almost_equal(system_norm(F, p=2), 2.2360679)
def test_system_norm_simple(): G = Transfer([100], [1, 10, 100]) assert_almost_equal(system_norm(G), 1.1547, decimal=5) assert_almost_equal(system_norm(G, p=2), 2.2360679) F = transfer_to_state(G) assert_almost_equal(system_norm(F), 1.1547, decimal=5) assert_almost_equal(system_norm(F, p=2), 2.2360679)
def test_static_model_conversion_sampling_period(): G = State(np.eye(5), dt=0.001) H = state_to_transfer(G) assert_(H._isgain) # 0 assert_(not H._isSISO) # 1 assert_equal(H.SamplingPeriod, 0.001) # 2 K = transfer_to_state(H) assert_equal(K.SamplingPeriod, 0.001) # 3
def test_transfer_to_state(): # Models with static column/row num, den = [[1, -1], [[1, -1], 0]], [[[1, 2], 1], [[1, 2], 1]] den2, num2 = [list(i) for i in zip(*den)], [list(i) for i in zip(*num)] G = Transfer(num, den) H = Transfer(num2, den2) Gs = transfer_to_state(G) Hs = transfer_to_state(H) Gm = concatenate_state_matrices(Gs) Hm = concatenate_state_matrices(Hs) assert_array_almost_equal(Gm, np.array([[-2, 1, 0], [1, 0, -1], [-3, 1, 0]])) assert_array_almost_equal( Hm, np.array([[-2., 0., 1., 0.], [0., -2., 0., 1.], [1., -3., 0., 1.], [0., 0., -1., 0.]])) # Example from Kalman 1963 num = [[3 * np.poly([-3, -5]), [6, 6], [2, 7], [2, 5]], [2, 1, [2, 10], [8, 16]], [[2, 14, 36], [-2, 0], 1, 2 * np.convolve([5, 17], [1, 2])]] den = [[np.poly([-1, -2, -4]), [1, 6, 8], [1, 7, 12], [1, 5, 6]], [[1, 8, 15], [1, 3], np.poly([-1, -2, -3]), np.poly([-1, -3, -5])], [np.poly([-1, -3, -5]), [1, 4, 3], [1, 3], np.poly([-1, -3, -5])]] G = Transfer(num, den) H = transfer_to_state(G) p = H.poles p.sort() assert_array_almost_equal( p, np.array([ -5. + 0.j, -5. + 0.j, -4. + 0.j, -3. + 0.j, -3. + 0.j, -3. + 0.j, -2. + 0.j, -2. + 0.j, -1. + 0.j, -1. + 0.j, -1. + 0.j ]))
def do_sys_id(self): num_poles = 2 num_zeros = 1 if not self._use_subspace: method = 'ARMAX' #sys_id = system_identification(self._y.T, self._u[0], method, IC='BIC', na_ord=[0, 5], nb_ord=[1, 5], nc_ord=[0, 5], delays=[0, 5], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal') sys_id = system_identification(self._y.T, self._u[0], method, ARMAX_orders=[num_poles, 1, 1, 0], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal') print(sys_id.G) if self._verbose: print(sys_id.G) print("System poles of discrete G: ", cnt.pole(sys_id.G)) # Convert to continuous tf G = harold.Transfer(sys_id.NUMERATOR, sys_id.DENOMINATOR, dt=self._Ts) G_cont = harold.undiscretize(G, method='zoh') self._sys_tf = G_cont self._A, self._B, self._C, self._D = harold.transfer_to_state( G_cont, output='matrices') if self._verbose: print("Continuous tf:", G_cont) # Convert to state space, because ARMAX gives transfer function ss_roll = cnt.tf2ss(sys_id.G) A = np.asarray(ss_roll.A) B = np.asarray(ss_roll.B) C = np.asarray(ss_roll.C) D = np.asarray(ss_roll.D) if self._verbose: print(ss_roll) # simulate identified system using input from data xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u) y_error = self._y - yid self._fitness = 1 - (y_error.var() / self._y.var())**2 if self._verbose: print("Fittness %", self._fitness * 100) if self._plot: plt.figure(1) plt.plot(self._t[0], self._y[0]) plt.plot(self._t[0], yid[0]) plt.xlabel("Time") plt.title("Time response Y(t)=U*G(t)") plt.legend([ self._y_name, self._y_name + '_identified: ' + '{:.3f} fitness'.format(self._fitness) ]) plt.grid() plt.show() else: sys_id = system_identification(self._y, self._u, self._subspace_method, SS_fixed_order=num_poles, SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal') #sys_id = system_identification(self._y, self._u, self._subspace_method, SS_orders=[1,10], SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal') if self._verbose: print("x0", sys_id.x0) print("A", sys_id.A) print("B", sys_id.B) print("C", sys_id.C) print("D", sys_id.D) A = sys_id.A B = sys_id.B C = sys_id.C D = sys_id.D # Get discrete transfer function from state space sys_tf = cnt.ss2tf(A, B, C, D) if self._verbose: print("TF ***in z domain***", sys_tf) # Get numerator and denominator (num, den) = cnt.tfdata(sys_tf) # Convert to continuous tf G = harold.Transfer(num, den, dt=self._Ts) if self._verbose: print(G) G_cont = harold.undiscretize(G, method='zoh') self._sys_tf = G_cont self._A, self._B, self._C, self._D = harold.transfer_to_state( G_cont, output='matrices') if self._verbose: print("Continuous tf:", G_cont) # get zeros tmp_tf = cnt.ss2tf(self._A, self._B, self._C, self._D) self._zeros = cnt.zero(tmp_tf) # simulate identified system using discrete system xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u, sys_id.x0) y_error = self._y - yid self._fitness = 1 - (y_error.var() / self._y.var())**2 if self._verbose: print("Fittness %", self._fitness * 100) if self._plot: plt.figure(1) plt.plot(self._t[0], self._y[0]) plt.plot(self._t[0], yid[0]) plt.xlabel("Time") plt.title("Time response Y(t)=U*G(t)") plt.legend([ self._y_name, self._y_name + '_identified: ' + '{:.3f} fitness'.format(self._fitness) ]) plt.grid() plt.show()
def from_tf_coefficients(num, den, delays): """ array-like: (numerator, denominator, delays) numerator, denominator, delays can either be in SISO form or MIMO form MIMO form e.g.: num = [[num12, num12], [num21, num22]] den = [[den12, den12], [den21, den22]] delay = [[delay12, delay12], [delay21, delay22]] """ num, den, delays = [numpy.array(i) for i in [num, den, delays]] are_siso = [isinstance(l[0], numbers.Number) for l in [num, den, delays]] if numpy.any(are_siso) and not numpy.all(are_siso): raise ValueError("Some of num, den and delays are SISO and some are MIMO") elif numpy.all(are_siso): num, den, delays = [numpy.array([[i]]) for i in [num, den, delays]] num_dim = (len(num), len(num[0])) den_dim = (len(den), len(den[0])) delay_dim = (len(delays), len(delays[0])) # Check dimensions if num_dim != den_dim or den_dim != delay_dim: raise ValueError("num, den and delays are not the same dimension") As, B1s, B2s, Cs, Ds = [], [], [], [], [] D11 = None delay_list = list(set(delays.flatten())) for delay in delay_list: if delay < 0: raise ValueError("delay cannot be negative") num_i = numpy.zeros(num_dim).tolist() den_i = numpy.zeros(den_dim).tolist() for r in range(num_dim[0]): for c in range(num_dim[1]): num_i[r][c] = num[r][c] if delays[r][c] == delay else [0] den_i[r][c] = den[r][c] if delays[r][c] == delay else [1] Gss_i = harold.transfer_to_state(harold.Transfer(num_i, den_i)) Ai, Bi, Ci, Di = Gss_i.a, Gss_i.b, Gss_i.c, Gss_i.d Ai = numpy.array([0]) if Ai.size == 0 else Ai Bi = numpy.zeros((Ai.shape[0], num_dim[1])) if Bi.size == 0 else Bi Ci = numpy.zeros((num_dim[0], Ai.shape[0])) if Ci.size == 0 else Ci Di = numpy.zeros(num_dim) if Di.size == 0 else Di if delay == 0: D11 = Di B2i = numpy.zeros_like(Bi) for ls, m in zip([As, B1s, B2s, Cs], [Ai, Bi, B2i, Ci]): ls.append(m) else: B1i = numpy.zeros_like(Bi) for ls, m in zip([As, B1s, B2s, Cs, Ds], [Ai, B1i, Bi, Ci, Di]): ls.append(m) if 0 in delay_list and len(B2s) != 1: B2s = [numpy.vstack(B2s[:2])] + B2s[2:] if 0 in delay_list and len(delay_list) != 1: delay_list.remove(0) A = scipy.linalg.block_diag(*As) if As != [] else numpy.array([[0]]) No, Ni = num_dim Nx = A.shape[0] Nd = len(delay_list) Nw = Nd * Ni B1 = numpy.vstack(B1s) if B1s != [] else numpy.zeros((Nx, Ni)) B2 = scipy.linalg.block_diag(*B2s) if B2s != [] else numpy.zeros((Nx, Nw)) C1 = numpy.hstack(Cs) if Cs != [] else numpy.zeros((Nx, No)) C2 = numpy.zeros((Nw, Nx)) D11 = D11 if D11 is not None else numpy.zeros((No, Ni)) D12 = numpy.hstack(Ds) if Ds != [] else numpy.zeros((No, Nw)) D21 = numpy.tile(numpy.eye(Ni), (Nd, 1)) D22 = numpy.zeros((Nw, Nw)) matrices = A, B1, B2, C1, C2, D11, D12, D21, D22 reshaped = [mi.reshape((mi.shape[0], 1)) if len(mi.shape) == 1 else mi for mi in matrices] A, B1, B2, C1, C2, D11, D12, D21, D22 = reshaped return InternalDelay(A, B1, B2, C1, C2, D11, D12, D21, D22, numpy.repeat(delay_list, Ni))
def test_transfer_to_state(): # Models with static column/row num, den = [[1, -1], [[1, -1], 0]], [[[1, 2], 1], [[1, 2], 1]] den2, num2 = [list(i) for i in zip(*den)], [list(i) for i in zip(*num)] G = Transfer(num, den) H = Transfer(num2, den2) Gs = transfer_to_state(G) Hs = transfer_to_state(H) Gm = concatenate_state_matrices(Gs) Hm = concatenate_state_matrices(Hs) assert_array_almost_equal(Gm, np.array([[-2, 1, 0], [1, 0, -1], [-3, 1, 0]])) assert_array_almost_equal( Hm, np.array([[-2., 0., 1., 0.], [0., -2., 0., 1.], [1., -3., 0., 1.], [0., 0., -1., 0.]])) # Example from Kalman 1963 num = [[3 * np.poly([-3, -5]), [6, 6], [2, 7], [2, 5]], [2, 1, [2, 10], [8, 16]], [[2, 14, 36], [-2, 0], 1, 2 * np.convolve([5, 17], [1, 2])]] den = [[np.poly([-1, -2, -4]), [1, 6, 8], [1, 7, 12], [1, 5, 6]], [[1, 8, 15], [1, 3], np.poly([-1, -2, -3]), np.poly([-1, -3, -5])], [np.poly([-1, -3, -5]), [1, 4, 3], [1, 3], np.poly([-1, -3, -5])]] G = Transfer(num, den) H = transfer_to_state(G) p = H.poles p.sort() assert_array_almost_equal( p, np.array([ -5. + 0.j, -5. + 0.j, -4. + 0.j, -3. + 0.j, -3. + 0.j, -3. + 0.j, -2. + 0.j, -2. + 0.j, -1. + 0.j, -1. + 0.j, -1. + 0.j ])) # Reported in gh-#42 G = Transfer([[[87.8, 8.78], [-103.68, -8.64]], [[129.84, 10.82], [-109.6, -10.96]]], [562.5, 82.5, 1]) Gss = transfer_to_state(G) assert_array_almost_equal( Gss.a, np.kron(np.eye(2), [[0., 1.], [-2 / 1125, -11 / 75]])) assert_array_almost_equal(Gss.b, [[0, 0], [1, 0], [0, 0], [0, 1]]) des_c = np.array([[ 0.01560888888888889, 0.1560888888888889, -0.015360000000000002, -0.18432 ], [ 0.019235555555555558, 0.23082666666666668, -0.019484444444444447, -0.19484444444444443 ]]) assert_array_almost_equal(Gss.c, des_c) assert_array_almost_equal(Gss.d, np.zeros([2, 2])) # reported in gh-#50 num = [[[61.79732492202783, 36.24988430260625, 0.7301196233698941], [0.0377840674057878, 0.9974993795127982, 21.763622825733773]]] den = [[[84.64, 18.4, 1.0], [1.0, 7.2, 144.0]]] TF = transfer_to_state((num, den)) assert_array_almost_equal([ -3.6 - 1.14472704e+01j, -3.6 + 1.14472704e+01j, -0.10869565 - 1.74405237e-07j, -0.10869565 + 1.74405237e-07j, ], np.sort(TF.poles)) assert TF.zeros.size == 0 # rectengular static gain gain = np.ones([2, 3]) Gss = transfer_to_state(Transfer(gain)) assert_array_almost_equal(gain, Gss.d)
def from_tf_coefficients(num, den, delays): """ array-like: (numerator, denominator, delays) numerator, denominator, delays can either be in SISO form or MIMO form MIMO form e.g.: num = [[num12, num12], [num21, num22]] den = [[den12, den12], [den21, den22]] delay = [[delay12, delay12], [delay21, delay22]] """ num, den, delays = [numpy.array(i) for i in [num, den, delays]] are_siso = [isinstance(l[0], numbers.Number) for l in [num, den, delays]] if numpy.any(are_siso) and not numpy.all(are_siso): raise ValueError("Some of num, den and delays are SISO and some are MIMO") elif numpy.all(are_siso): num, den, delays = [numpy.array([[i]]) for i in [num, den, delays]] num_dim = (len(num), len(num[0])) den_dim = (len(den), len(den[0])) delay_dim = (len(delays), len(delays[0])) # Check dimensions if num_dim != den_dim or den_dim != delay_dim: raise ValueError("num, den and delays are not the same dimension") As, B1s, B2s, Cs, Ds = [], [], [], [], [] D11 = None delay_list = list(set(delays.flatten())) for delay in delay_list: if delay < 0: raise ValueError("delay cannot be negative") num_i = numpy.zeros(num_dim).tolist() den_i = numpy.zeros(den_dim).tolist() for r in range(num_dim[0]): for c in range(num_dim[1]): num_i[r][c] = num[r][c] if delays[r][c] == delay else [0] den_i[r][c] = den[r][c] if delays[r][c] == delay else [1] Gss_i = harold.transfer_to_state(harold.Transfer(num_i, den_i)) Ai, Bi, Ci, Di = Gss_i.a, Gss_i.b, Gss_i.c, Gss_i.d Ai, Bi, Ci, Di = [numpy.array([0]) if i.size == 0 else i for i in [Ai, Bi, Ci, Di]] if delay == 0: D11 = Di B2i = numpy.zeros_like(Bi) for ls, m in zip([As, B1s, B2s, Cs], [Ai, Bi, B2i, Ci]): ls.append(m) else: B1i = numpy.zeros_like(Bi) for ls, m in zip([As, B1s, B2s, Cs, Ds], [Ai, B1i, Bi, Ci, Di]): ls.append(m) if 0 in delay_list and len(delay_list) != 1: delay_list.remove(0) A = scipy.linalg.block_diag(*As) if As != [] else numpy.array([[0]]) No, Ni = num_dim Nx = A.shape[0] Nd = len(delay_list) Nw = Nd * Ni B1 = numpy.vstack(B1s) if B1s != [] else numpy.zeros((Nx, Ni)) B2 = numpy.vstack(B2s) if B2s != [] else numpy.zeros((Nx, Nw)) C1 = numpy.hstack(Cs) if Cs != [] else numpy.zeros((Nx, No)) C2 = numpy.zeros((Nw, Nx)) D11 = D11 if D11 is not None else numpy.zeros((No, Ni)) D12 = numpy.hstack(Ds) if Ds != [] else numpy.zeros((No, Nw)) D21 = numpy.tile(numpy.eye(Ni), (Nd, 1)) D22 = numpy.zeros((Nw, Nw)) matrices = A, B1, B2, C1, C2, D11, D12, D21, D22 reshaped = [mi.reshape((mi.shape[0], 1)) if len(mi.shape) == 1 else mi for mi in matrices] A, B1, B2, C1, C2, D11, D12, D21, D22 = reshaped return InternalDelay(A, B1, B2, C1, C2, D11, D12, D21, D22, numpy.repeat(delay_list, Ni))
def test_transfer_to_state(): # Models with static column/row num, den = [[1, -1], [[1, -1], 0]], [[[1, 2], 1], [[1, 2], 1]] den2, num2 = [list(i) for i in zip(*den)], [list(i) for i in zip(*num)] G = Transfer(num, den) H = Transfer(num2, den2) Gs = transfer_to_state(G) Hs = transfer_to_state(H) Gm = concatenate_state_matrices(Gs) Hm = concatenate_state_matrices(Hs) assert_array_almost_equal(Gm, np.array([[-2, 1, 0], [1, 0, -1], [-3, 1, 0]])) assert_array_almost_equal(Hm, np.array([[-2., 0., 1., 0.], [0., -2., 0., 1.], [1., -3., 0., 1.], [0., 0., -1., 0.]])) # Example from Kalman 1963 num = [[3*np.poly([-3, -5]), [6, 6], [2, 7], [2, 5]], [2, 1, [2, 10], [8, 16]], [[2, 14, 36], [-2, 0], 1, 2*np.convolve([5, 17], [1, 2])]] den = [[np.poly([-1, -2, -4]), [1, 6, 8], [1, 7, 12], [1, 5, 6]], [[1, 8, 15], [1, 3], np.poly([-1, -2, -3]), np.poly([-1, -3, -5])], [np.poly([-1, -3, -5]), [1, 4, 3], [1, 3], np.poly([-1, -3, -5])]] G = Transfer(num, den) H = transfer_to_state(G) p = H.poles p.sort() assert_array_almost_equal(p, np.array([-5.+0.j, -5.+0.j, -4.+0.j, -3.+0.j, -3.+0.j, -3.+0.j, -2.+0.j, -2.+0.j, -1.+0.j, -1.+0.j, -1.+0.j])) # Reported in gh-#42 G = Transfer([[[87.8, 8.78], [-103.68, -8.64]], [[129.84, 10.82], [-109.6, -10.96]]], [562.5, 82.5, 1]) Gss = transfer_to_state(G) assert_array_almost_equal(Gss.a, np.kron(np.eye(2), [[0., 1.], [-2/1125, -11/75]])) assert_array_almost_equal(Gss.b, [[0, 0], [1, 0], [0, 0], [0, 1]]) des_c = np.array([[0.01560888888888889, 0.1560888888888889, -0.015360000000000002, -0.18432], [0.019235555555555558, 0.23082666666666668, -0.019484444444444447, -0.19484444444444443]]) assert_array_almost_equal(Gss.c, des_c) assert_array_almost_equal(Gss.d, np.zeros([2, 2])) # reported in gh-#50 num = [[[61.79732492202783, 36.24988430260625, 0.7301196233698941], [0.0377840674057878, 0.9974993795127982, 21.763622825733773]]] den = [[[84.64, 18.4, 1.0], [1.0, 7.2, 144.0]]] TF = transfer_to_state((num, den)) assert_array_almost_equal([-3.6-1.14472704e+01j, -3.6+1.14472704e+01j, -0.10869565-1.74405237e-07j, -0.10869565+1.74405237e-07j, ], np.sort(TF.poles)) assert TF.zeros.size == 0