def sum(SS1, SS2, negative=False): ''' Given 2 systems or gain matrices (or a combination of the two) having the same amount of input/output, the function returns a gain or state space model summing the two. Namely, given: u -> SS1 -> y1 u -> SS2 -> y2 we obtain: u -> SStot -> y1+y2 if negative=False ''' type_dlti = scsig.ltisys.StateSpaceDiscrete if isinstance(SS1, np.ndarray) and isinstance(SS2, np.ndarray): SStot = SS1 + SS2 elif isinstance(SS1, np.ndarray) and isinstance(SS2, type_dlti): Kmat = SS1 A = SS2.A B = SS2.B C = SS2.C D = SS2.D + Kmat SStot = scsig.StateSpace(A, B, C, D, dt=SS2.dt) elif isinstance(SS1, type_dlti) and isinstance(SS2, np.ndarray): Kmat = SS2 A = SS1.A B = SS1.B C = SS1.C D = SS1.D + Kmat SStot = scsig.StateSpace(A, B, C, D, dt=SS2.dt) elif isinstance(SS1, type_dlti) and isinstance(SS2, type_dlti): assert np.abs(1.-SS1.dt/SS2.dt)<1e-13,\ 'State-space models must have the same time-step' Nin01, Nout01 = SS1.inputs, SS1.outputs Nin02, Nout02 = SS2.inputs, SS2.outputs Nx01, Nx02 = SS1.A.shape[0], SS2.A.shape[0] A = np.block([[SS1.A, np.zeros((Nx01, Nx02))], [np.zeros((Nx02, Nx01)), SS2.A]]) B = np.block([[ SS1.B, ], [SS2.B]]) C = np.block([SS1.C, SS2.C]) D = SS1.D + SS2.D SStot = scsig.StateSpace(A, B, C, D, dt=SS1.dt) else: raise NameError( 'Input types not recognised in any implemented option!') return SStot
def ss_concatenate(sys1, sys2, F1=None, H1=None): """ Concatenate two systems in series. w ---- u1 ------ y1 ---- u2 ------ y2 ---> | F1 | ---> | sys1 | ---> | H1 | ---> | sys2 | ---> ---- ------ ---- ------ Args: sys1 (StateSpace): The first state space system sys2 (StateSpace): The second state space system F1 (matrix, optional): The first system's input selection matrix. Defaults to the identity matrix H1 (matrix, optional): The first system's output selection matrix. Defaults to the identity matrix Returns: StateSpace: The series system where [ x1 ] x = [----], u = w, y = y2 [ x2 ] """ A1 = sys1.A B1 = sys1.B C1 = sys1.C D1 = sys1.D A2 = sys2.A B2 = sys2.B C2 = sys2.C D2 = sys2.D (n1, p1) = B1.shape (q1, n1) = C1.shape (n2, p2) = B2.shape (q2, n2) = C2.shape if F1 is None: F1 = np.matrix(np.eye(p1)) if F2 is None: F2 = np.matrix(np.eye(p2)) if H1 is None: H1 = np.matrix(np.eye(q1)) if H2 is None: H2 = np.matrix(np.eye(q2)) A_top_row = np.concatenate((A1, np.zeros((n1, n2))), axis=1) A_bot_row = np.concatenate(B2*H1*C1, A2), axis=1) A = np.concatenate((A_top_row, A_bot_row), axis=0) B = np.concatenate((B1*F1, B2*H1*D1*F1), axis=0) C = np.concatenate((D2*H1*C1, C2), axis=1) D = np.matrix(D2*H1*D1*F1) return signal.StateSpace(A, B, C, D)
def discrete_ss(self, dt): ''' Convert the continuous system to a discrete system ''' # system = signal.cont2discrete((self.A, self.B, self.C, self.D), dt) self.sys_d = signal.StateSpace(self.A, self.B, self.C, self.D).to_discrete(dt) return self.sys_d
def _generate_system(self): """ Prepared by Arpan Mukherjee :return: state-space representation of the 1-D Shallow Water Model """ return signal.StateSpace(self.A, self.B, self.C, self.D, dt=self.dt)
def _generate_system_clusters(self, clust_matrix): """ Prepared by Arpan Mukherjee :param clust_matrix: :return: list of shallow water equation objects for specific length of cluster """ A = self.A B = self.B C = self.C D = self.D number_cluster = clust_matrix.shape[0] _state_space_clust = [] for k in range(number_cluster): cluster = clust_matrix[k, :] cl = cluster.astype(bool) A_cl = A[cl][:, cl] B_cl = B[cl] C_cl = C[cl][:, cl] D_cl = D[cl] _state_space_clust.append(signal.StateSpace(A_cl, B_cl, C_cl, D_cl, dt=self.dt)) return _state_space_clust
def linearize(self): """Return the linearized system. Returns ------- a : ndarray The state matrix. b : ndarray The action matrix. """ gravity = self.gravity length = self.length friction = self.friction inertia = self.inertia A = np.array([[0, 1], [gravity / length, -friction / inertia]], dtype=config.np_dtype) B = np.array([[0], [1 / inertia]], dtype=config.np_dtype) if self.normalization is not None: Tx, Tu = map(np.diag, self.normalization) Tx_inv, Tu_inv = map(np.diag, self.inv_norm) A = np.linalg.multi_dot((Tx_inv, A, Tx)) B = np.linalg.multi_dot((Tx_inv, B, Tu)) sys = signal.StateSpace(A, B, np.eye(2), np.zeros((2, 1))) sysd = sys.to_discrete(self.dt) return sysd.A, sysd.B
def system_cluster(self, cluster_matrix): A = self._ss.A B = self._ss.B C = self._ss.C D = self._ss.D dt = self._ss.dt number_cluster = cluster_matrix.shape[0] _state_space_clust = [] for k in range(number_cluster): cluster = cluster_matrix[k, :] cl = cluster.astype(bool) A_cl = A[cl][:, cl] B_cl = B[cl] C_cl = C[cl][:, cl] D_cl = D[cl] _state_space_clust.append(signal.StateSpace(A_cl, B_cl, C_cl, D_cl, dt=dt)) return _state_space_clust
def linearize(self): """Return the linearized system. Returns ------- a : ndarray The state matrix. b : ndarray The action matrix. """ k = self.k mass = self.mass c = self.c m = self.m A = np.array([[0, 1], [k / mass, -c / m]], dtype=config.np_dtype) B = np.array([[0], [1 / (m * mass**2)]], dtype=config.np_dtype) if self.normalization is not None: Tx, Tu = map(np.diag, self.normalization) Tx_inv, Tu_inv = map(np.diag, self.inv_norm) A = np.linalg.multi_dot((Tx_inv, A, Tx)) B = np.linalg.multi_dot((Tx_inv, B, Tu)) sys = signal.StateSpace(A, B, np.eye(2), np.zeros((2, 1))) sysd = sys.to_discrete(self.dt) return sysd.A, sysd.B
def createStateSpace(order, stateLength, t, omg, zeta, gamma_d, gamma_omg): Ac = np.zeros([stateLength, stateLength], dtype=float) Bc = np.zeros([stateLength, 1], dtype=float) Cc = np.zeros([1, stateLength], dtype=float) Dc = 0 #This is to populate State matrices # -Python takes the LHS which forms a 2x2 matrix, matches it to the RHS which, to python looks like a # 2x2 matrix and assigns each element accordingly. Process follows suite for Bc matrix for k in range(order): i = (1 + k) * 2 k = k + 1 #handles the one off error Ac[i - 2:i, i - 2:i] = [[0, 1], [float(-(k * omg)**2), 0]] Bc[i - 2:i] = [[0], [(k * omg)**2]] Cc[0][i - 1] = (2 * zeta) / (k * omg) #Assigns edge values Bc[len(Bc) - 1][0] = gamma_d Cc[0][len(Cc[0]) - 1] = 1 #uses the "scipy" library and converts the continuous matrixes into a SS # -Then takes the continuous system and decritizes it (NOTE: method for c2d may not match model) dt = float(t[1] - t[0]) contSystem = signal.StateSpace(Ac, Bc, Cc, Dc) discSystem = contSystem.to_discrete(dt) #Posible solution if .to_discrete system does not work. # -function for computing matrix exponenetal (Future implimentation) # -might need to maintain stability #discSystem = signal.cont2discrete(contSystem,dt,'impulse') return discSystem
def get_state_space(self): """Get a state space representation of this filter Returns: ss: a scipy.signal state space representation of the filter """ zs, ps, k = self.get_zpk(Hz=False) return sig.StateSpace(*sig.zpk2ss(assertArr(zs), assertArr(ps), k))
def ss_common_in(sys1, sys2, F1=None): """ Connection of two systems with a common input ------ y1 |---> | sys1 | ---> | ------ w ---- u | ---> | F1 | ---| ---- | | ------ y2 |---> | sys2 | ---> ------ Args: sys1 (StateSpace): The first state space system sys2 (StateSpace): The second state space system F1 (matrix, optional): Each system's input selection matrix. Defaults to the identity matrix Returns: StateSpace: The common input system where [ x1 ] [ y1 ] x = [----], u = w, y = [----] [ x2 ] [ y2 ] """ A1 = sys1.A B1 = sys1.B C1 = sys1.C D1 = sys1.D A2 = sys2.A B2 = sys2.B C2 = sys2.C D2 = sys2.D (n1, p1) = B1.shape (q1, n1) = C1.shape (n2, p2) = B2.shape (q2, n2) = C2.shape if F1 is None: F1 = np.matrix(np.eye(p1)) A_top_row = np.concatenate((A1, np.zeros((n1, n2))), axis=1) A_bot_row = np.concatenate((np.zeros((n2, n1)), A2), axis=1) A = np.concatenate((A_top_row, A_bot_row), axis=0) B = np.concatenate((B1*F1, B2*F1), axis=0) C_top_row = np.concatenate((C1, np.zeros((q1, n2))), axis=1) C_bot_row = np.concatenate((np.zeros((q2, n1)), sys), axis=1) C = np.concatenate((C_top_row, C_bot_row), axis=0) D = np.concatenate((D1*F1, D2*F1), axis=0) return signal.StateSpace(A, B, C, D)
def continous_time_step_response(A, B, C, D, plot=True): ss = sig.StateSpace(A, B, C, D) t, ys = sig.step(system=ss) if plot: for i in range(ys.shape[1]): plt.plot(t, ys[:, i]) plt.title("Response Y" + str(i)) plt.show() return (t, ys)
def plot_w1w2(Ra,La,Kg,K1,Km,N1,N2,J2,c,br,grid): # Pembuatan model transfer function A= [[(-Ra/La),(-Kg/La)],[Km/((N2/N1)**2 * J2),(-c+br)]] B= [[(K1/La)],[0]] C= [[0,1],[0,(N2/N1)]] D= [[0],[0]] sys1=signal.StateSpace(A,B,C,D) t1,y1=signal.step(sys1) plt.title("Plot $\\omega_1$ dan $\\omega_2$") plt.plot(t1,y1)
def MotorControl(): J = .01 # kgm**2 b = .1 # N.m.s Ke = .01 # V/rad/sec Kt = .01 # N.m/Amp R = 1 # Electrical resistance L = .5 # Henries K = Kt # or Ke, Ke == Kt here # State Space Model A = np.matrix([[-b / J, K / J], [-K / L, -R / L]]) B = np.matrix([[0], [1 / L]]) C = np.matrix([1, 0]) D = np.matrix([0]) dt = .025 # Discrete Sample Time ss = sig.StateSpace(A, B, C, D) ssd = sig.cont2discrete((A, B, C, D), dt) Ad = ssd[0] Bd = ssd[1] Cd = ssd[2] evc, evecc = np.linalg.eig(A) evd, evecd = np.linalg.eig(Ad) # Create Bode #w, mag, phase = sig.bode(ss) #plot_SISO(w,mag, "Freq Response Mag vs w") #plot_SISO(w, phase, "Freq Response Phase vs w") #discrete_time_frequency_repsonse() # Transfer Function tf = ss.to_tf() print(tf) t, y = sig.step(ss) td, yd = sig.dstep(ssd) yd = np.reshape(yd, -1) #plot_SISO(t,y) #plot_SISO(td, yd) #pole_zeros(A, B, C, plot=True) #pole_zeros(Ad, Bd, Cd, plot=True) T = 10 N = T / dt times = np.arange(0, T, dt) U = np.ones(int(N)) x0 = np.matrix([0, 0]).T xr = np.matrix([2.5, 0]) kp = 15 kd = 4 ki = 100 Q = Cd.T * Cd R = np.matrix([.0005]) discrete_time_response(Ad, Bd, Cd, U, x0, dt, plot=True) discrete_time_frequency_repsonse(Ad, Bd, Cd, 20, dt) continous_time_frequency_repsonse(A, B, C) discrete_pid_response((Ad, Bd, Cd), x0, xr, times, kp, ki, kd) sim_dlqr(Ad, Bd, Cd, Q, R, x0, xr, dt, T) x = 9
def fullModel(IsPfc, angles, Rc, time): Is2Bpol2 = buildIs2BpolB(isttok_mag_2) RIc = isttok_mag['RM'] + isttok_mag['Rcopper'] * np.cos(anglesIc) ZIc = isttok_mag['Rcopper'] * np.sin(anglesIc) A2, Bs2, C2, Ds2 = buildLtiModelB(RIc, ZIc, isttok_mag_2, Rc) Ic2Bpol = buildIc2Bpol(RIc, ZIc) magSys2 = signal.StateSpace(A2, Bs2, C2, Ds2) bPolIs2 = np.matmul(Is2Bpol2, IsPfc) tout2, Ic2, x2 = signal.lsim(magSys2, IsPfc.T, time) bPolIc2 = np.matmul(Ic2Bpol, Ic2.T) bPolTot2 = (bPolIs2.T + bPolIc2.T) * 50 * 49e-6 return bPolTot2, (RIc, ZIc)
def mass_spring_damper(): m = .25 k = 10 # n/m b = .2 # Nm/s A = np.matrix([[0, 1], [-k / m, -b / m]]) B = np.matrix([0, 1 / m]).T C = np.matrix([1, 0]) D = np.matrix([0]) dt = .025 ss = sig.StateSpace(A, B, C, D) ssd = sig.cont2discrete((A, B, C, D), dt) ctrb_rank = np.linalg.matrix_rank(ctrb(A, B))
def data_gen(): parser = argparse.ArgumentParser( description='Generate Training and Testing Samples') parser.add_argument('-n', action="store", dest="ts1", type=int) parser.add_argument('-t', action="store", dest="ts2", type=int) trainingsample = parser.parse_args().ts1 testingsample = parser.parse_args().ts2 # Simulate two mass spring system A = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [-2, 1, 0, 0], [1, -2, 0, 0]]) B = np.array([[0, 0], [0, 0], [1, 0], [0, 1]]) C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) D = np.array([[0, 0], [0, 0]]) sys = sg.StateSpace(A, B, C, D).to_discrete(0.1) #sample period 0.1s print "Two Mass Spring System:\n", sys, '\n' x = np.array([[10], [20], [0], [0]], dtype=np.float64) #initial state ls = [] for i in range(trainingsample): mu, sigma = 0, 0.25 # mean and standard deviation #s = np.random.normal(mu, sigma, 2)# generate noise for the training #x[0:2,0]+=s f = 2 * (np.random.rand(2, 1) - 0.5) #random force to the mass x = sys.A.dot(x) + sys.B.dot(f) y = sys.C.dot(x) + sys.D.dot(f) ls.append([f, y]) #store the training data in a file for future analysis with open(r'./traindata.log', 'wb') as afile: pickle.dump(ls, afile) #reload object from the file """ with open(r'./td', 'rb') as _load_file: new_d = pickle.load(_load_file) """ ls = [] for i in range(testingsample): f = 2 * (np.random.rand(2, 1) - 0.5) #random force to the mass x = sys.A.dot(x) + sys.B.dot(f) y = sys.C.dot(x) + sys.D.dot(f) ls.append([f, y]) #store the training data in a file for future analysis with open(r'./testdata.log', 'wb') as afile: pickle.dump(ls, afile)
def InvertedPendulum(): x0 = 0 dx0 = 0 theta0 = np.pi dtheta0 = 0 M = .5 m = 0.2 b = 0.1 I = 0.006 g = 9.8 l = 0.3 F = symbols('F') x, theta, dx, dtheta = symbols('x, theta, dx, dtheta') X = Matrix([x, dx, theta, dtheta]) U = Matrix([F]) RHS = Matrix([[M + m, -m * l * cos(theta)], [m * l * cos(theta), I + m * l**2]]) LHS1 = RHS.inv() * Matrix([ -b * x + m * l * dtheta**2 * sin(theta), -m * l * g * sin(theta) ]) #Dynamics LHS2 = RHS.inv() * Matrix([F, 0]) #Input Force A = LHS1.jacobian(X).subs([(x, x0), (theta, theta0), (dx, dx0), (dtheta, dtheta0)]) B = LHS2.jacobian(U).subs([(x, x0), (theta, theta0), (dx, dx0), (dtheta, dtheta0)]) A = np.reshape( np.matrix(np.vstack( (np.matrix([0, 1, 0, 0]), A[0, :], np.matrix([0, 0, 0, 1]), A[1, :])), dtype=np.float), (4, 4)) B = np.matrix([0, B[0, 0], 0, B[1, 0]], dtype=np.float).T C = np.matrix([[1, 0, 0, 0], [0, 0, 1, 0]]) D = np.matrix([[0], [0]]) ss = sig.StateSpace(A, B, C, D) dt = .025 ssd = sig.cont2discrete((A, B, C, D), dt) tc, yc = sig.impulse(ss) td, yd = sig.dimpulse(ssd) plt.plot(td, [y[0, 0] for y in yd]) plt.show() plt.plot(td, [y[1, 0] for y in yd]) plt.show() f00 = 9
def cruise_control(): m = 1000 b = 50 u = 500 A = np.matrix([[0, .99], [0, -b / m]]) B = np.matrix([[0], [u / m]]) C = np.matrix(np.eye(2)) D = np.matrix([[0], [0]]) evals, evecs = np.linalg.eig(A) pole_zeros(A, B, C[1, :]) ss = sig.StateSpace(A, B, C, D) continous_time_impulse_response(A, B, C, D, plot=True) continous_time_step_response(A, B, C, D, plot=True) #continous_time_frequency_repsonse(A,B,C, plot = True) Gc = gram_ctrb(A, B) U = continous_time_least_norm_input(A, B, np.matrix([0, 10]), 5, .025) #Go = gram_obsv(A,C) foo = 0
def discrete_model(env, measurements): # linearized continuous model J = 1 / 12 * (env.masspole * 2)**2 beta = env.masspole * env.masscart * env.length**2 + J * (env.masspole + env.masscart) ac = -(env.masspole**2 * env.length**2 * env.gravity) / beta bc = (J + env.masspole * env.length**2) / beta cc = env.masspole * env.length * env.gravity * (env.masscart + env.masspole) / beta dc = -env.masspole * env.length / beta A_c = np.array([[0, 1, 0, 0], [0, 0, ac, 0], [0, 0, 0, 1], [0, 0, cc, 0]]) B_c = np.array([[0], [bc], [0], [dc]]) # C_c = np.array([[1,env.tau,0,0], # [0,1,0,0], # [0,0,0,1]]) # D_c = np.array([[0], # [0], # [0]]) # C_c = np.array([[1,0,0,0]]) # D_c = np.array([[0]]) if measurements is 2: C_c = np.array([[1, 0, 0, 0], [0, 0, 0, 1]]) D_c = np.array([[0], [0]]) elif measurements is 1: C_c = np.array([[1, 0, 0, 0]]) D_c = np.array([[0]]) # discrete linearized model sys = signal.StateSpace(A_c, B_c, C_c, D_c) discrete_sys = sys.to_discrete(env.tau) A = discrete_sys.A B = discrete_sys.B C = discrete_sys.C D = discrete_sys.D # print(A) # print(B) # print(C) # while True: # pass return A, B, C, D
def cavity_ss(RoverQ, Qg, Q0, Qprobe, bw, Kg_r, Kg_i, Ib, foffset): Rg = RoverQ * Qg Kdrive = 2 * np.sqrt(Rg) Ql = 1.0 / (1.0/Qg + 1.0/Q0 + 1.0/Qprobe) K_beam = RoverQ * Ql w_d = 2 * np.pi * foffset a = Kdrive * bw b = K_beam * bw c = bw A = -c B = a C = 1.0 D = 0 sys = signal.StateSpace(A,B,C,D) t,y = signal.step(sys) return t, y
def SerialPlotIdentify(signals): ''' Remove voltage from output vector. ''' labels = signals['labels'] data = signals['data'] speedidx = labels.index('Speed') curridx = labels.index('Current') voltidx = labels.index('Voltage') yvec = np.array([data[:, speedidx], data[:, curridx]]).T yvec = yvec[1:, :] xvec = np.array([data[:, speedidx], data[:, curridx], data[:, voltidx]]).T xvec = xvec[:-1, :] logging.debug('XVec dims: {}'.format(xvec.shape)) logging.debug('YVec dims: {}'.format(yvec.shape)) params, res, rank, s = np.linalg.lstsq(xvec, yvec, rcond=None) logging.debug('Params: {}'.format(params)) logging.debug('Params Dims: {}'.format(params.shape)) yhat = xvec.dot(params) amtx = params[:yhat.shape[1], :].T bmtx = params[-1, :].reshape(1, 2) cmtx = np.eye(amtx.shape[0]) dmtx = np.zeros([2, 1]) logging.debug('A-Mat Shape: {}'.format(amtx.shape)) logging.debug('B-Mat Shape: {}'.format(bmtx.T.shape)) logging.debug('C-Mat Shape: {}'.format(cmtx.shape)) logging.debug('D-Mat Shape: {}'.format(dmtx.shape)) sys = signal.StateSpace(amtx, bmtx.T, cmtx, dmtx, dt=T_SAMPLE) u = data[:, voltidx] t = np.arange(len(u)) * T_SAMPLE x0 = xvec[0, :-1] yhat = np.squeeze(signal.dlsim(sys, u, x0=x0)[-1]) ylabels = [labels[speedidx], labels[curridx]] for i in range(yhat.shape[1]): plt.subplot(yhat.shape[1], 1, i + 1) plt.plot(yvec[:, i]) plt.plot(yhat[:, i], 'r--', LineWidth=0.9) plt.title(ylabels[i]) plt.legend(['Actual', 'Estimate']) plt.grid() plt.show()
def linearize(self): """Return the linearized system. Returns ------- a : ndarray The state matrix. b : ndarray The action matrix. """ gravity = self.gravity length = self.length friction = self.friction inertia = self.inertia a = np.array([[0, 1], [gravity / length, -friction / inertia]]) b = np.array([[0], [1 / inertia]]) sys = signal.StateSpace(a, b, np.eye(2), np.zeros((2, 1))) sysd = sys.to_discrete(self.step_size) return LinearSystem(sysd.A, sysd.B)
def test_unstable_system(self): T = 15 Ts = 1e-1 A = [[1.178, 0.001, 0.511, -0.403], [-0.051, 0.661, -0.011, 0.061], [0.076, 0.335, 0.560, 0.382], [0., 0.335, 0.089, 0.849]] B = [[0.004, -0.087], [0.467, 0.001], [0.213, -0.235], [0.213, -0.016]] A = np.array(A) B = np.array(B) n, m = B.shape sys = scipysig.StateSpace(A, B, np.identity(n), np.zeros((n, m)), dt=Ts) u = np.random.randn(T + 1, m) _, _, x = scipysig.dlsim(sys, u) Qs = [np.identity(n), 0 * np.identity(n)] Rs = [np.identity(m), 0 * np.identity(m)] for Q in Qs: for R in Rs: if np.all(Q == Qs[1]): with self.assertRaises(ValueError): K, V = optimal_control(x, u, Q, R) else: # Solve optimal control problem with Riccati equation P = dare(A, B, Q, R) trueK = -np.linalg.inv(B.T @ P @ B + R) @ B.T @ P @ A # Solve using Willems' lemma K, V = optimal_control(x, u, Q, R) # Compare results self.assertTrue(np.linalg.norm(K - trueK) < 1e-4)
def discretize_state_space(self, dt): """ Calculate discrete state space matrices Input: - dt: timestep length Output: - discrete state space matrices A, B, C, D """ # model constants lf = self.lf lr = self.lr Caf = self.Caf Car = self.Car Iz = self.Iz m = self.m vx = self.vx self.A = np.array([ [0, 1, 0, 0], [ 0, -2 * (Caf + Car) / (m * vx), 2 * (Caf + Car) / m, 2 * (Car * lr - Caf * lf) / (m * vx) ], [0, 0, 0, 1], [ 0, -2 * (lf * Caf - lr * Car) / (Iz * vx), 2 * (Caf * lf - Car * lr) / Iz, -2 * (lf**2 * Caf + lr**2 * Car) / (Iz * vx) ], ]) Gcont = signal.StateSpace(self.A, self.B, self.C, self.D) Gdisc = Gcont.to_discrete(dt) return Gdisc.A, Gdisc.B, Gdisc.C, Gdisc.D
Bm[1, 1] = alpha2.value[0] / c5 Cm[0, 2] = 1 Cm[1, 3] = 1 # state space simulation ss = GEKKO() x, y, u = ss.state_space(Am, Bm, Cm, D=None) u[0].value = data['Q1'].values u[1].value = data['Q2'].values ss.time = data['Time'].values ss.options.IMODE = 7 ss.solve(disp=False) # state space simulation with scipy sys = signal.StateSpace(Am, Bm, Cm, Dm) tsys = data['Time'].values Qsys = np.vstack((data['Q1'].values, data['Q2'].values)) Qsys = Qsys.T tsys, ysys, xsys = signal.lsim(sys, Qsys, tsys) sae = 0.0 for i in range(len(data)): sae += np.abs(data['T1'][i] - TC1.value[i]) sae += np.abs(data['T2'][i] - TC2.value[i]) print('SAE Energy Balance: ' + str(sae)) # Create plot plt.figure(figsize=(10, 7)) ax = plt.subplot(2, 1, 1) ax.grid()
def control_update(self): traj = self.traj vehicle = self.vehicle lr = vehicle.lr lf = vehicle.lf Ca = vehicle.Ca Iz = vehicle.Iz f = vehicle.f m = vehicle.m g = vehicle.g delT = 0.05 #reading current vehicle states X = vehicle.state.X Y = vehicle.state.Y xdot = vehicle.state.xd ydot = vehicle.state.yd phi = vehicle.state.phi phidot = vehicle.state.phid delta = vehicle.state.delta vx = xdot # ---------------|Lateral Controller|------------------------- dist, index1 = closest_node(X, Y, traj) n = 180 index = index1 + n if (index >= 8203): index = 8202 X_new = traj[index][0] Y_new = traj[index][1] K_p = 2500 K_i = 0.0001 K_d = 1.5 dist1 = math.sqrt((Y_new - Y)**2 + (X_new - X)**2) v_d = dist1 / ((n - 80) * delT) v_d = 8 v = math.sqrt(xdot**2 + ydot**2) # e22 = wrap2pi(phi - np.arctan2(Y_new-Y, X_new-X)) # curv=self.curv # ------------|Lateral Controller|--------------------------------- A = np.array([[0, 1, 0, 0], [ 0, -(4 * Ca) / (m * xdot), (4 * Ca) / m, (-(2 * Ca * lf) + (2 * Ca * lr)) / (m * xdot) ], [0, 0, 0, 1], [ 0, (-(2 * Ca * lf) + (2 * Ca * lr)) / (Iz * xdot), ((2 * Ca * lf) - (2 * Ca * lr)) / (Iz), (-(2 * Ca * lf * lf) - (2 * Ca * lr * lr)) / (Iz * xdot) ]]) # print(np.shape(A)) B = np.array([[0], [(2 * Ca) / m], [0], [(2 * Ca * lf) / Iz]]) C = np.identity(4) D = [[0], [0], [0], [0]] syscont = signal.StateSpace(A, B, C, D) sysdisc = syscont.to_discrete(delT) Ad = sysdisc.A Bd = sysdisc.B R = np.zeros((1, 1)) R[0][0] = 0.06 Q = 0.8 * np.identity(4) S = np.matrix(scipy.linalg.solve_discrete_are(Ad, Bd, Q, R)) r = wrap2pi(phi - np.arctan2(Y_new - Y, X_new - X)) e21 = 0.1 e22 = ydot + (vx * r) e23 = r e24 = phidot e = np.matrix([e21, e22, e23, e24]) #print(len(e), len(e[0])) K = np.matrix(scipy.linalg.inv((Bd.T @ S @ Bd) + R) @ (Bd.T @ S @ Ad)) #W= np.identity(4) V = (np.identity(4)) * 500 #V=np.eye(4) W = [[1, 0, 0, 0], [0, 0.5, 0, 0], [0, 0, 0.5, 0], [0, 0, 0, 0.05]] #V=np.matrix([1,0.5,0.5,0.05]) xk_km = (Ad) @ (self.xkm) + (Bd) * (self.Z) Pk_km = ((Ad) @ (self.Pkm)) @ (Ad.T) + W yk = e.T Lk = ((Pk_km) @ (C.T)) @ (np.linalg.inv(((C) @ (Pk_km) @ (C.T) + V))) xk_k = xk_km + Lk @ (yk - C @ xk_km) Pk_k = (np.identity(4) - (Lk @ C)) @ Pk_km deltades = float(-K @ (xk_k)) deltad = ((deltades - delta)) / 0.05 # deltad = float(-K*(e)) #delta = np.matmul(K, xk_k) #deltad = -delta[0][0] #deltad = float(deltad) #--------|Longitudinal Controller|------------------------------ e11 = (v_d - v) F = K_p * (e11) + K_i * (self.e + e11) + K_d * (e11 - self.e111) self.e += e11 self.e111 = e11 # ----------------------------------------------------------------- # Communicating the control commands with the BuggySimulator controlinp = vehicle.command(F, deltad) self.Z = deltades self.xkm = xk_k self.Pkm = Pk_k #print(e) # F: Force # deltad: desired rate of steering command return controlinp
def join(SS1, SS2): ''' Join two state-spaces or gain matrices such that, given: u1 -> SS1 -> y1 u2 -> SS2 -> y2 we obtain: u -> SStot -> y with u=(u1,u2)^T and y=(y1,y2)^T. The output SStot is either a gain matrix or a state-space system according to the input SS1 and SS2 ''' type_dlti = scsig.ltisys.StateSpaceDiscrete if isinstance(SS1, np.ndarray) and isinstance(SS2, np.ndarray): Nin01, Nin02 = SS1.shape[1], SS2.shape[1] Nout01, Nout02 = SS1.shape[0], SS2.shape[0] SStot = np.block([[SS1, np.zeros((Nout01, Nin02))], [np.zeros((Nout02, Nin01)), SS2]]) elif isinstance(SS1, np.ndarray) and isinstance(SS2, type_dlti): Nin01, Nout01 = SS1.shape[1], SS1.shape[0] Nin02, Nout02 = SS2.inputs, SS2.outputs Nx02 = SS2.A.shape[0] A = SS2.A B = np.block([np.zeros((Nx02, Nin01)), SS2.B]) C = np.block([[np.zeros((Nout01, Nx02))], [SS2.C]]) D = np.block([[SS1, np.zeros((Nout01, Nin02))], [np.zeros((Nout02, Nin01)), SS2.D]]) SStot = scsig.StateSpace(A, B, C, D, dt=SS2.dt) elif isinstance(SS1, type_dlti) and isinstance(SS2, np.ndarray): Nin01, Nout01 = SS1.inputs, SS1.outputs Nin02, Nout02 = SS2.shape[1], SS2.shape[0] Nx01 = SS1.A.shape[0] A = SS1.A B = np.block([SS1.B, np.zeros((Nx01, Nin02))]) C = np.block([[SS1.C], [np.zeros((Nout02, Nx01))]]) D = np.block([[SS1.D, np.zeros((Nout01, Nin02))], [np.zeros((Nout02, Nin01)), SS2]]) SStot = scsig.StateSpace(A, B, C, D, dt=SS1.dt) elif isinstance(SS1, type_dlti) and isinstance(SS2, type_dlti): assert SS1.dt == SS2.dt, 'State-space models must have the same time-step' Nin01, Nout01 = SS1.inputs, SS1.outputs Nin02, Nout02 = SS2.inputs, SS2.outputs Nx01, Nx02 = SS1.A.shape[0], SS2.A.shape[0] A = np.block([[SS1.A, np.zeros((Nx01, Nx02))], [np.zeros((Nx02, Nx01)), SS2.A]]) B = np.block([[SS1.B, np.zeros((Nx01, Nin02))], [np.zeros((Nx02, Nin01)), SS2.B]]) C = np.block([[SS1.C, np.zeros((Nout01, Nx02))], [np.zeros((Nout02, Nx01)), SS2.C]]) D = np.block([[SS1.D, np.zeros((Nout01, Nin02))], [np.zeros((Nout02, Nin01)), SS2.D]]) SStot = scsig.StateSpace(A, B, C, D, dt=SS1.dt) else: raise NameError( 'Input types not recognised in any implemented option!') return SStot
# check parallel connector Nout = 2 Nin01, Nin02 = 2, 3 Nst01, Nst02 = 4, 2 # build random systems fac = 0.1 A01, A02 = fac * np.random.rand(Nst01, Nst01), fac * np.random.rand( Nst02, Nst02) B01, B02 = np.random.rand(Nst01, Nin01), np.random.rand(Nst02, Nin02) C01, C02 = np.random.rand(Nout, Nst01), np.random.rand(Nout, Nst02) D01, D02 = np.random.rand(Nout, Nin01), np.random.rand(Nout, Nin02) dt = 0.1 SS01 = scsig.StateSpace(A01, B01, C01, D01, dt=dt) SS02 = scsig.StateSpace(A02, B02, C02, D02, dt=dt) # simulate NT = 11 U01, U02 = np.random.rand(NT, Nin01), np.random.rand(NT, Nin02) # reference Y01, X01 = simulate(SS01, U01) Y02, X02 = simulate(SS02, U02) Yref = Y01 + Y02 # parallel SStot = parallel(SS01, SS02) Utot = np.block([U01, U02]) Ytot, Xtot = simulate(SStot, Utot)
if x0[i] > xg[i]: u[i] = 0.0 return u c = 0.0001 #room transfer coef cu = 0.02 #heating on coef A = [[-c, c], [c, -c]] B = [[cu, 0.0], [0.0, cu]] C = A D = B x0 = [50.0, 10.0] xg = [50.0, 200.0] sys1 = signal.StateSpace(A, B, C, D) simTime = 60 * 60 * 1 controlStep = 15 * 60 c = greedyController(x0, xg) tall = [] uall = np.array([]).reshape((0, len(x0))) xall = np.array([]).reshape((0, len(x0))) for ti in range(0, simTime, 1): t = np.linspace(ti, ti + 1, 10) if ti % controlStep == 0: c = greedyController(x0, xg) u = np.transpose([np.ones(len(t)) * c[0], np.ones(len(t)) * c[1]]) t1, y1, x1 = signal.lsim(sys1, u, t, x0) x0 = x1[len(x1) - 3] tall = np.concatenate((tall, t1), axis=0)