def main(): # 制御対象を定義(今回はモータを想定) J = 0.6 # 慣性 D = 0.2 # 粘性 Kt = 0.15 # トルク定数 P = crl.tf([Kt], [J, D, 0]) # モータ角度の電流制御 # 参照モデルの定義 M = crl.tf([50], [1, 50]) # シミュレーション条件を定義 Ts = 0.001 # サンプリング時間 # 制御器構造の定義 beta = crl.ss("0", "1", "0; 1", "1; 0") # PI制御器 print(crl.tf(beta)) beta = crl.c2d(beta, Ts) # 制御対象と参照モデルのゲイン線図 plt.figure(1) crl.bode([P, M]) plt.legend(["Plant", "Ref. model"]) # prbsを生成する n = 12 # m系列信号の次数 Tp = 5 # 何周期印加するか? Ap = 0.5 # 振幅 [A] u0 = 2 * prbs(n) u0 = Ap * (u0 - np.average(u0)) u0 = np.tile(u0, Tp) t = np.linspace(0, Ts * u0.shape[0], u0.shape[0]) plt.figure(2) plt.plot(t, u0) plt.xlabel("Time [s]") plt.ylabel("Current [A]") # 実験データの取得 print("=== Start Experiment ===") Pd = crl.c2d(P, Ts, "tustin") # 双一次変換で離散時間モデルを作成 t = np.arange(start=0, stop=Tp * Ts * (2**n - 1), step=Ts) (y0, t0, xout) = crl.lsim(Pd, u0[:], t[:]) # 応答を取得 plt.figure(3) plt.plot(y0) # VRFTを実行する print("=== Start VRFT ===") W = crl.tf([50], [1, 50]) # 周波数重み L = crl.minreal((1 - M) * M * W) # プレフィルタ Ld = crl.c2d(L, Ts, "tustin") (ul, tl, xlout) = crl.lsim(Ld, u0, t) (phi_l, tl, xlout) = crl.lsim(beta, y0[:]) print(phi_l) rho = np.linalg.solve(phi_l, ul) print(rho) plt.show()
def __init__(self): # define parameters self.mc = 1.0 self.mr = 0.25 self.g = 9.81 self.Fe = (self.mc + 2 * self.mr) * self.g self.mu = 0.1 self.Jc = 0.0042 self.d = 0.3 self.H = 8 # number of points in the horizon # define the state space equations of the form xdot = Ax + Bu self.A_long = np.array([[0.0, 1.0], [0.0, 0.0]]) self.B_long = np.array([[0], [1 / (self.mc + 2 * self.mr)]]) self.C_long = np.array([[1, 0], [0, 1]]) self.D_long = np.array([[0], [0]]) self.A_lat = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [ 0, -(self.Fe) / (self.mc + 2 * self.mr), (-self.mu) / (self.mc + 2 * self.mr), 0 ], [0, 0, 0, 0]]) self.B_lat = np.array([[0], [0], [0], [1 / (self.Jc + 2 * self.mr * self.d**2)]]) self.C_lat = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) self.D_lat = np.array([[0], [0]]) # convert the continuous state space system to a discrete time system self.statespace_long = StateSpace(self.A_long, self.B_long, self.C_long, self.D_long) self.statespace_lat = StateSpace(self.A_lat, self.B_lat, self.C_lat, self.D_lat) # The interval of time between each discrete interval self.Ts = 0.5 # creates an object which contains the discretized version of A, B, C, and D as well as a dt self.discrete_long = c2d(self.statespace_long, self.Ts, method='zoh', prewarp_frequency=None) self.discrete_lat = c2d(self.statespace_lat, self.Ts, method='zoh', prewarp_frequency=None) self.objhist_h = np.ones(self.H) self.objhist_z = np.ones(self.H) self.ulast_long = [] self.flast_long = [] self.glast_long = [] self.ulast_lat = [] self.flast_lat = [] self.glast_lat = []
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7): self.sysd = c2d(sysc, Ts) self.Ts = Ts self.sysc = sysc self.epsilon = epsilon self.est = Estimator(self.sysd, 500, self.epsilon) self.y_index = None self.worst_case_control = 'current' self.k = None self.y_up = None self.y_lo = None # diy self.p = 0.5 self.i = 7 self.d = 0 self.total = 10 self.slot = int(self.total / Ts) self.t_arr = linspace(0, self.total, self.slot + 1) self.ref = [0] * 251 + [1] * 250 self.thres = 5 self.drift = 0.12 self.x_0 = [0] self.y_real_arr = [] self.s = 0 self.att = 0 self.cin = 0 self.ymeasure = 0 self.yreal = 0 self.score = [] self.place = 350 self.maxc = 5 # -------------------------------------- self.safeset = {'lo': [-2.7], 'up': [2.7]}
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7): self.sysd = c2d(sysc, Ts) self.Ts = Ts self.sysc = sysc self.epsilon = epsilon self.est = Estimator(self.sysd, 500, self.epsilon) self.y_index = None self.worst_case_control = 'current' self.k = None self.y_up = None self.y_lo = None # diy self.p = 0.1 self.i = 0 self.d = 0.6 self.total = 30 self.slot = int(self.total / Ts) self.t_arr = linspace(0, self.total, self.slot + 1) self.ref = [2] * 601 + [4] * 600 + [2] * 300 self.thres = 3 self.drift = 1 self.x_0 = [0] self.y_real_arr = [] self.s = 0 self.att = 0 self.cin = 0 self.ymeasure = 0 self.yreal = 0 self.score = [] self.place = 700 self.maxc = 50 self.xmeasure = [[0], [0], [0], [0], [0], [0], [0], [0], [0]] self.xreal = [[0], [0], [0], [0], [0], [0], [0], [0], [0]] self.safeset = {'lo': [-10000] * 8 + [-1], 'up': [10000] * 8 + [8]}
def __init__(self, dt): num = np.array([0.01, 0.01, 0.02]) den = np.array([0.001, 1, 0]) sys_tf = ctrl.tf(num, den) self.A, self.B, self.C, self.D = ctrl.ssdata( ctrl.c2d(sys_tf, dt, 'foh')) self.X = np.zeros((self.A.shape[0]))
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7): self.sysd = c2d(sysc, Ts) self.Ts = Ts self.sysc = sysc self.epsilon = epsilon self.est = Estimator(self.sysd, 500, self.epsilon) self.y_index = None self.worst_case_control = 'current' self.k = None self.y_up = None self.y_lo = None # diy self.p = 14 self.i = 0.8 self.d = 5.7 self.total = 10 self.slot = int(self.total / Ts) self.t_arr = linspace(0, self.total, self.slot + 1) self.ref = [0.5] * 201 + [0.7] * 200 + [0.5] * 100 self.thres = 10 self.drift = 1 self.x_0 = [0] self.y_real_arr = [] self.s = 0 self.att = 0 self.cin = 0 self.ymeasure = 0 self.yreal = 0 self.score = [] self.place = 300 self.maxc = 20 self.xmeasure = [[0], [0], [0]] self.xreal = [[0], [0], [0]]
def __init__(self, sysc, Ts, epsilon=1e-7): self.sysd = c2d(sysc, Ts) self.Ts = Ts self.sysc = sysc self.epsilon = epsilon self.est = Estimator(self.sysd, 150, self.epsilon) self.y_index = None self.worst_case_control = 'current' self.k = None self.y_up = None self.y_lo = None
def testSS2cont(self): """Test c2d()""" sys = ss(np.array([[-3, 4, 2], [-1, -3, 0], [2, 5, 3]]), np.array([[1, 4], [-3, -3], [-2, 1]]), np.array([[4, 2, -3], [1, 4, 3]]), np.array([[-2, 4], [0, 1]])) sysd = c2d(sys, 0.1) np.testing.assert_array_almost_equal( np.array( [[0.742840837331905, 0.342242024293711, 0.203124211149560], [-0.074130792143890, 0.724553295044645, -0.009143771143630], [0.180264783290485, 0.544385612448419, 1.370501013067845]]), sysd.A) np.testing.assert_array_almost_equal( np.array([[0.012362066084719, 0.301932197918268], [-0.260952977031384, -0.274201791021713], [-0.304617775734327, 0.075182622718853]]), sysd.B)
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7): self.sysd = c2d(sysc, Ts) self.Ts = Ts self.sysc = sysc self.epsilon = epsilon self.est = Estimator(self.sysd, 500, self.epsilon) self.y_index = None self.worst_case_control = 'current' self.k = None self.y_up = None self.y_lo = None # diy self.p = 11 self.i = 0 self.d = 5 self.total = 24 self.slot = int(self.total / Ts) self.t_arr = linspace(0, self.total, self.slot + 1) self.ref = [math.pi / 2] * 71 + [math.pi / 2] * 50 self.thres = 0.2 self.drift = 0.01 self.x_0 = [0] self.y_real_arr = [] self.s = 0 self.att = 0 self.cin = 0 self.ymeasure = 0 self.yreal = 0 self.score = [] self.place = 20 self.maxc = 20 self.xmeasure = [[0], [0], [0]] self.xreal = [[0], [0], [0]] self.safeset = { 'lo': [-4, -1000000000, -100000000], 'up': [4, 1000000000, 100000000] }
# Ganancia del mixer. MixerGain = 8 lpfoutput = (lpfoutputI + 1j * lpfoutputQ) # there are L Nyquist periods in one period of p(t) (for Tropp's analysis # Tx=Tp or L=N). L = N # 1-st order RC low-pass filter # En este fragmento de codigo se hallo la respuesta al impulso del filtro RC # Implementado analogamente. tau = 1 / (2 * pi * fc) B = 1 A = [tau, 1] lpf = tf(B, A) lpf_d = c2d(lpf, 1 / (float(W)), 'tustin') [[Bd]], [[Ad]] = tfdata(lpf_d) original = x ### Simulando la funcion impz de matlab. x = zeros(25) x[0] = 1 h = lfilter(Bd, Ad, x) # Finds closest time value to perform sampling i_tstart = argmin(abs(tspice - Td)) lpfoutput = lpfoutput[i_tstart:-1] tspice = tspice[i_tstart:-1] lpfoutput = lpfoutput - mean(lpfoutput) lpfoutput = lpfoutput / MixerGain
def runOptimization(): m = 5 k = 3 b = 0.5 H = 10 # number of points in the horizon # define the state space equations of the form xdot = Ax + Bu A = np.array([[0, 1], [-k / m, -b / m]]) B = np.array([[0], [1 / m]]) C = np.array([[1, 0], [0, 1]]) D = np.array([[0], [0]]) # convert the continuous state space system to a discrete time system statespace = StateSpace(A, B, C, D) # The interval of time between each discrete interval Ts = 0.2 # creates an object which contains the discretized version of A, B, C, and D as well as a dt discrete = c2d(statespace, Ts, method='zoh', prewarp_frequency=None) objhist = np.ones(H) def objcon(u): f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon z = 0.0 # initial position of the mass zdot = 0.0 # initial velocity of the mass zref = 1.0 # the commanded position for the mass x = np.array([[z], [zdot]]) # vectorize the state variables g = np.array([0]) for i in range(len(u)): objhist[i] = z if i != 0: g = np.append(g, 100 - (u[i] - u[i - 1]) / Ts) g = np.append(g, 100 + (u[i] - u[i - 1]) / Ts) f = f + abs(zref - z) x_next = np.matmul(discrete.A, x) + np.matmul(discrete.B, np.array([[u[i]]])) z = x_next[0] x = x_next return f[0, 0], g def penalty(x, mu): f, g = objcon(x) sum = 0 for i in range(len(g)): sum = sum + min(0, g[i])**2 return f + (mu/2 * sum) xlast = np.array([]) flast = np.array([]) glast = np.array([]) dflast = np.array([]) dglast = np.array([]) def obj(x): nonlocal xlast, flast, glast, dflast, dglast if not np.array_equal(x, xlast): flast, glast = objcon(x) xlast = x return flast def con(x): nonlocal xlast, flast, glast, dflast, dglast if not np.array_equal(x, xlast): flast, glast = objcon(x) xlast = x return glast def constrained_optimizer(fun, x): mu = 1.0 rho = 1.1 k = 0 convergence = 1e-2 f_next = 100 f = 0 options = {'disp': True} while f_next-f > convergence: f = f_next res = minimize(fun, x, args=mu, bounds=Bounds(-10.0, 10.0), options=options, method='slsqp') mu = rho * mu x = res.x f_next = res.fun print('The forces are: ', x) print('mu = ', mu) k += 1 print('k = ', k) print('f = ', f) print('f_next = ', f_next) return x x0 = np.ones(H) t0 = time.time() x = constrained_optimizer(penalty, x0) t1 = time.time() - t0 print('Elapsed time: ', t1) f = obj(x) print('x = ', x) print('f = ', f) t = Ts * np.array(range(H)) plt.plot(t, objhist) plt.xlabel('Time (s)') plt.ylabel('Position z (m)') plt.show()
import numpy as np from control.matlab import c2d, StateSpace m = 5 k = 3 b = 0.5 A = np.array([[0, 1], [-k / m, -b / m]]) B = np.array([[0], [1 / m]]) C = np.array([[1, 0], [0, 1]]) D = np.array([[0], [0]]) statespace = StateSpace(A, B, C, D) Ts = 0.2 discrete = c2d(statespace, Ts, method='zoh', prewarp_frequency=None) print(discrete)
# z^2 - 1.98 z + 0.9802 # # Sample time: 0.01 seconds # Discrete-time transfer function. #Test if we generated a system with the same behaviour: # MATLAB: # [output, t] = step(syst_fake) # plot(t, output) # Python # output, t = step([syst_fake.num[0][0][0], syst_fake.den[0][0]]) # plot(output, t) # MATLAB wants the inverse order to plot # show() syst_fake_dis = c2d(syst_fake, 0.01, method='zoh') print syst_fake_dis # OLD scipy.signal.cont2discrete way! #syst_fake_dis = c2d([syst_fake.num[0][0][0], syst_fake.den[0][0]], 0.01) #print_c2d_matlablike(syst_fake_dis) # Python: # zero order hold by default # 4.96670866519e-05 z 4.93370716897e-05 # ----------------------- # z^2 1.0 z^2 -1.97990166083 z 0.980198673307 # # Sample time: 0.01 seconds # Discrete-time transfer function. # MATLAB:
""" Codigo para obter o equivalente digital para C(s) """ import control.matlab as control # Parametros calculados do compensador a = 10.5 b = 1.42 # Definindo FT do compensador C = control.tf([1, a], [1, b]) # Intervalo de amostragem Ta = 2e-3 # s. # Obtendo o equivalente de discreto pelo metodo de Tustin Cd = control.c2d(C, Ta, 'tustin') # Imprimindo valores print(Cd)
""" import control as ct import numpy as np import matplotlib.pyplot as plt import Plotter as mp import control.matlab as cm #Define the transfer function num= np.array([2000*np.pi]) den= np.array([1,2000*np.pi]) sys1 = ct.tf(num, den) sys2=cm.c2d(sys1,1/8000,method='tustin') #print tf print(sys1) print(sys2) #generate Bode mag, phase, omega = cm.bode(sys1,dB=True,Plot=False) mag2, phase2, omega2 = cm.bode(sys2,dB=True,Plot=False) fig,axes=plt.subplots(2) fig2,axes2=plt.subplots(2) mp.myPlotterBode(axes[0],axes[1],omega,mag,phase,param_dict={'color':'blue'})
cls() Kt = 1.0 Jt = 1.0 # [?] u1 = tf([Kt], [1]) u2 = tf([1 / Jt], [1]) # fixme: как добавить сигнал шума? u12 = series(u1, u2) u3 = tf([1], [1, 0]) u4 = copy.deepcopy(u3) u34 = series(u3, u4) # похоже нельзя соединить аналоговую и дискретную u34_d0 = c2d(u3, Ts=0.5) u34_d = c2d(u3, Ts=0.5) print series(u34_d0, u34_d) u14 = series(u12, u34) print u14 ts, xs = step_response(u14) # print (c2d(u14, Ts=0.5)) plot(ts, xs) grid() show()
from control import matlab import matplotlib.pyplot as plt import numpy as np Gs = matlab.tf(0.5, [1, 0.5]) print(Gs) T = 0.1 Gz = matlab.c2d(Gs, T, method='zoh') #'zoh'0次ホールド print(Gz)
K = c.place(A, B, P) # Place new poles Acl = np.array(A - B * K) # Create a closed loop A matrix Ecl = LA.eigvals(Acl) # Calculate new eigenvalues (this evaluates to the poles that I placed!) syscl = c.ss(Acl, B, C, D) # Closed loop state space model step = c.step_response(syscl) Kdc = c.dcgain(syscl) # Calculating a dc gain in order to reach intended output goal Kr = 1 / Kdc sysclNew = c.ss(Acl, B * Kr, C, D) sysclNewDiscrete = matlab.c2d(sysclNew, 0.01) stepNew = c.step_response(sysclNew) stepDiscrete = c.step_response(sysclNewDiscrete) plt.figure(1) yout, T = stepDiscrete plt.plot(yout.T, T.T) plt.xlabel("Time (Seconds)") plt.ylabel("Amplitude") plt.show() # Graph step response with time # plt.figure(2) # yout, T = stepNew # plt.plot(yout.T, T.T) # plt.xlabel("Time (Seconds)")
def main(): dt = 0.05 simulation_time = 10 # in seconds iteration_num = int(simulation_time / dt) # you must be care about this matrix # these A and B are for continuos system if you want to use discret system matrix please skip this step # lineared car system V = 5.0 A = np.array([[0., V], [0., 0.]]) B = np.array([[0.], [1.]]) C = np.eye(2) D = np.zeros((2, 1)) # make simulator with coninuous matrix init_xs_lead = np.array([5., 0., 0.]) init_xs_follow = np.array([0., 0., 0.]) lead_car = TwoWheeledSystem(init_states=init_xs_lead) follow_car = TwoWheeledSystem(init_states=init_xs_follow) # create system sysc = matlab.ss(A, B, C, D) # discrete system sysd = matlab.c2d(sysc, dt) Ad = sysd.A Bd = sysd.B # evaluation function weight Q = np.diag([1., 1.]) R = np.diag([5.]) pre_step = 15 # make controller with discreted matrix # please check the solver, if you want to use the scipy, set the MpcController_scipy lead_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step, dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]), input_upper=np.array([30.]), input_lower=np.array([-30.])) follow_controller = MpcController_cvxopt( Ad, Bd, Q, R, pre_step, dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]), input_upper=np.array([30.]), input_lower=np.array([-30.])) lead_controller.initialize_controller() follow_controller.initialize_controller() # reference lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten() for i in range(iteration_num): print("simulation time = {0}".format(i)) # make lead car's move if i > int(iteration_num / 3): lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten() lead_states = lead_car.xs lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference) lead_opt_u = np.hstack((np.array([V]), lead_opt_u)) # make follow car follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten() follow_states = follow_car.xs follow_opt_u = follow_controller.calc_input(follow_states[1:], follow_reference) follow_opt_u = np.hstack((np.array([V]), follow_opt_u)) lead_car.update_state(lead_opt_u, dt=dt) follow_car.update_state(follow_opt_u, dt=dt) # figures and animation lead_history_states = np.array(lead_car.history_xs) follow_history_states = np.array(follow_car.history_xs) time_history_fig = plt.figure() x_fig = time_history_fig.add_subplot(311) y_fig = time_history_fig.add_subplot(312) theta_fig = time_history_fig.add_subplot(313) car_traj_fig = plt.figure() traj_fig = car_traj_fig.add_subplot(111) traj_fig.set_aspect('equal') x_fig.plot(np.arange(0, simulation_time + 0.01, dt), lead_history_states[:, 0], label="lead") x_fig.plot(np.arange(0, simulation_time + 0.01, dt), follow_history_states[:, 0], label="follow") x_fig.set_xlabel("time [s]") x_fig.set_ylabel("x") x_fig.legend() y_fig.plot(np.arange(0, simulation_time + 0.01, dt), lead_history_states[:, 1], label="lead") y_fig.plot(np.arange(0, simulation_time + 0.01, dt), follow_history_states[:, 1], label="follow") y_fig.plot(np.arange(0, simulation_time + 0.01, dt), [4. for _ in range(iteration_num + 1)], linestyle="dashed") y_fig.set_xlabel("time [s]") y_fig.set_ylabel("y") y_fig.legend() theta_fig.plot(np.arange(0, simulation_time + 0.01, dt), lead_history_states[:, 2], label="lead") theta_fig.plot(np.arange(0, simulation_time + 0.01, dt), follow_history_states[:, 2], label="follow") theta_fig.plot(np.arange(0, simulation_time + 0.01, dt), [0. for _ in range(iteration_num + 1)], linestyle="dashed") theta_fig.set_xlabel("time [s]") theta_fig.set_ylabel("theta") theta_fig.legend() time_history_fig.tight_layout() traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead") traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow") traj_fig.set_xlabel("x") traj_fig.set_ylabel("y") traj_fig.legend() plt.show() lead_history_us = np.array(lead_controller.history_us) follow_history_us = np.array(follow_controller.history_us) input_history_fig = plt.figure() u_1_fig = input_history_fig.add_subplot(111) u_1_fig.plot(np.arange(0, simulation_time + 0.01, dt), lead_history_us[:, 0], label="lead") u_1_fig.plot(np.arange(0, simulation_time + 0.01, dt), follow_history_us[:, 0], label="follow") u_1_fig.set_xlabel("time [s]") u_1_fig.set_ylabel("u_omega") input_history_fig.tight_layout() plt.show() animdrawer = AnimDrawer([lead_history_states, follow_history_states]) animdrawer.draw_anim()
Uc = matlab.ctrb(A, b) print(Uc) rank = np.linalg.matrix_rank(Uc) print(rank) #可観測性の検証 Uo = matlab.obsv(A, C) print(Uo) #出力に三点リーダーが出てくるのはいっぱいあるのを省略している rank = np.linalg.matrix_rank(Uo) print(rank) #離散時間系でもやってみましょう! print("ここからは離散時間系での実行") S = matlab.ss(A, b, C, d) P = matlab.c2d(S, T, method='zoh') #continuous to discrete #科制御性の検証 Uc = matlab.ctrb(P.A, P.B) rank = np.linalg.matrix_rank(Uc) print(rank) #可観測性の検証 Uo = matlab.obsv(P.A, P.C) rank = np.linalg.matrix_rank(Uo) print(rank) ''' 中島「磯野~!安定判別しようぜ~!」 カツオ「ええ~!?そんな面倒くさいことしたくないよ~」 中島「そこでコンピュータの力を借りるのさ、固有値問題なんてすぐ解けるよ」 '''
def runoptimization(): # define parameters mc = 1.0 mr = 0.25 g = 9.81 Fe = (mc + 2 * mr) * g mu = 0.1 Jc = 0.0042 d = 0.3 H = 7 # number of points in the horizon # define the state space equations of the form xdot = Ax + Bu A_long = np.array([[0.0, 1.0], [0.0, 0.0]]) B_long = np.array([[0], [1 / (mc + 2 * mr)]]) C_long = np.array([[1, 0], [0, 1]]) D_long = np.array([[0], [0]]) A_lat = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [0, -(Fe) / (mc + 2 * mr), (-mu) / (mc + 2 * mr), 0], [0, 0, 0, 0]]) B_lat = np.array([[0], [0], [0], [1 / (Jc + 2 * mr * d ** 2)]]) C_lat = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) D_lat = np.array([[0], [0]]) # convert the continuous state space system to a discrete time system statespace_long = StateSpace(A_long, B_long, C_long, D_long) statespace_lat = StateSpace(A_lat, B_lat, C_lat, D_lat) # The interval of time between each discrete interval Ts = 0.2 # creates an object which contains the discretized version of A, B, C, and D as well as a dt discrete_long = c2d(statespace_long, Ts, method='zoh', prewarp_frequency=None) discrete_lat = c2d(statespace_lat, Ts, method='zoh', prewarp_frequency=None) objhist_h = np.ones(H) objhist_z = np.ones(H) def objcon_long(u, discrete_long): f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon h = 0.0 # initial position of the mass hdot = 0.0 # initial velocity of the mass href = 1.0 # the commanded longitudinal position for the VTOL x_long = np.array([[h], [hdot]]) # vectorize the state variables # g = np.array([0]) for i in range(len(u)): objhist_h[i] = h # if i != 0: # g = np.append(g, 100 - (u[i] - u[i-1])/Ts) # g = np.append(g, 100 + (u[i] - u[i-1])/Ts) f = f + abs(href - h) x_long_next = np.matmul(discrete_long.A, x_long) + np.matmul(discrete_long.B, np.array([[u[i]]])) h = x_long_next[0] x_long = x_long_next return f[0, 0] def objcon_lat(u, discrete_lat): f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon z = 0.0 theta = 0.0 zdot = 0.0 thetadot = 0.0 zref = 2.0 # the commanded lateral position for the VTOL x_lat = np.array([[z], [theta], [zdot], [thetadot]]) # g = np.array([0]) for i in range(len(u)): objhist_z[i] = z # if i != 0: # g = np.append(g, 100 - (u[i] - u[i-1])/Ts) # g = np.append(g, 100 + (u[i] - u[i-1])/Ts) f = f + abs(zref - z) x_lat_next = np.matmul(discrete_lat.A, x_lat) + np.matmul(discrete_lat.B, np.array([[u[i]]])) z = x_lat_next[0] x_lat = x_lat_next return f[0, 0] ulast_long = [] flast_long = [] glast_long = [] ulast_lat = [] flast_lat = [] glast_lat = [] def obj_long(u, discrete_long): nonlocal ulast_long, flast_long, glast_long if not np.array_equal(u, ulast_long): flast_long = objcon_long(u, discrete_long) ulast_long = u return flast_long def con_long(u): nonlocal ulast_long, flast_long, glast_long if not np.array_equal(u, ulast_long): flast_long = objcon_long(u, discrete_long) ulast_long = u return glast_long def obj_lat(u, discrete_lat): nonlocal ulast_lat, flast_lat, glast_lat if not np.array_equal(u, ulast_lat): flast_lat = objcon_lat(u, discrete_lat) ulast_lat = u return flast_lat def con_lat(u, discrete_lat): nonlocal ulast_lat, flast_lat, glast_lat if not np.array_equal(u, ulast_lat): flast_lat = objcon_lat(u, discrete_lat) ulast_lat = u return glast_lat def nelder_mead(fun, discrete, x, tau_x, tau_f): # initialize variables n = len(x) simplex = np.zeros((n, n+1)) simplex[:, 0] = x s = np.zeros(n) l = 0.0001 def gradx(simplex0, n): sum = 0 for i in range(n): sum += np.linalg.norm(simplex0[:, i] - simplex0[:, n]) return sum def gradf(fun, simplex0, n): sum = 0 for i in range(n): sum += (fun(simplex0[:, i], discrete) - fun(simplex0[:, n], discrete))**2 return np.sqrt(sum/(n+1)) for j in range(n): for i in range(n): if j == i: s[i] = (l/(n*np.sqrt(2)))*(np.sqrt(n+1)-1) + (1/np.sqrt(2)) else: s[i] = (l/(n*np.sqrt(2)))*(np.sqrt(n+1)-1) simplex[:, j+1] = x + s f = np.zeros(n+1) while gradx(simplex, n) > tau_x or gradf(fun, simplex, n) > tau_f: for i in range(n+1): f[i] = fun(simplex[:, i], discrete) simplex = simplex[:, np.argsort(f)] xc = (1/n) * (simplex.sum(axis=1)-simplex[:, -1]) xr = xc + (xc - simplex[:, -1]) if fun(xr, discrete) < fun(simplex[:, 0], discrete): xe = xc + 2*(xc - simplex[:, -1]) if fun(xe, discrete) < fun(simplex[:, 0], discrete): simplex[:, -1] = xe else: simplex[:, -1] = xr elif fun(xr, discrete) <= fun(simplex[:, -2], discrete): simplex[:, -1] = xr else: if fun(xr, discrete) > fun(simplex[:, -1], discrete): xic = xc - 0.5 * (xc - simplex[:, -1]) if fun(xic, discrete) < fun(simplex[:, -1], discrete): simplex[:, -1] = xic else: for i in range(n): simplex[:, i+1] = simplex[:, 0] + 0.5 * (simplex[:, i+1]-simplex[:, 0]) else: xoc = xc + 0.5 * (xc - simplex[:, -1]) if fun(xoc, discrete) < fun(xr, discrete): simplex[:, -1] = xoc else: for i in range(n): simplex[:, i+1] = simplex[:, 0] + 0.5 * (simplex[:, i+1] - simplex[:, 0]) return simplex[:,0] u0_long = 0.5 * np.ones(H) u0_lat = 0.5 * np.ones(H) constraints = {'type': 'ineq', 'fun': con_long} options = {'disp': False} x = nelder_mead(obj_long, discrete_long, u0_long, 1e-6, 1e-8) print('Optimized x: ', x) plt.plot(objhist_h) plt.xlabel('Time (s)') plt.ylabel('Position z (m)') lb = -100.0 ub = 100.0 bounds = Bounds([lb, lb, lb, lb, lb, lb, lb], [ub, ub, ub, ub, ub, ub, ub]) res_long = differential_evolution(obj_long, bounds=bounds, args=(discrete_long,), strategy='best1bin', maxiter=1000, popsize=15, tol=0.01, mutation=0.5, recombination=0.7, disp=True, polish=True, init='latinhypercube', atol=0) res_lat = differential_evolution(obj_lat, bounds=bounds, args=(discrete_lat,), strategy='best1bin', maxiter=1000, popsize=15, tol=0.01, mutation=0.5, recombination=0.7, disp=True, polish=True, init='latinhypercube', atol=0) print('Optimized x_gen: ', res_long.x) print('Optimized tau_gen: ', res_lat.x) plt.plot(objhist_h) # plt.plot(objhist_z) plt.xlabel('Time (s)') plt.ylabel('Position (m)') plt.show()
# ----------------------- # z^2 - 1.98 z + 0.9802 # # Sample time: 0.01 seconds # Discrete-time transfer function. #Test if we generated a system with the same behaviour: # MATLAB: # [output, t] = step(syst_fake) # plot(t, output) # Python # output, t = step([syst_fake.num[0][0][0], syst_fake.den[0][0]]) # plot(output, t) # MATLAB wants the inverse order to plot # show() syst_fake_dis = c2d(syst_fake, 0.01, method='zoh') print syst_fake_dis # OLD scipy.signal.cont2discrete way! #syst_fake_dis = c2d([syst_fake.num[0][0][0], syst_fake.den[0][0]], 0.01) #print_c2d_matlablike(syst_fake_dis) # Python: # zero order hold by default # 4.96670866519e-05 z 4.93370716897e-05 # ----------------------- # z^2 1.0 z^2 -1.97990166083 z 0.980198673307 # # Sample time: 0.01 seconds # Discrete-time transfer function. # MATLAB:
[0, 0, 0, 1], [-h12 * mp * g * Lp / DLT, h12 * myup / DLT, 0, -h11 * myum / DLT] ]) b = np.array([[0], [-h12 * Km / DLT], [0], [h11 * Km / DLT]]) C = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) d = np.array([[0], [0]]) Uc = matlab.ctrb(A, b) #連続系可制御性行列作成 print(Uc) #その表示 rank = np.linalg.matrix_rank(Uc) #そのランク調査 print(rank) #ランクが4なら可制御 Ev = np.linalg.eigvals(A) #固有値=特性根計算 print(Ev) #左半面なら安定 S = matlab.ss(A, b, C, d) #システム行列の設定 P = matlab.c2d(S, T, method='zoh') #離散系へ変換 Uc = matlab.ctrb(P.A, P.B) #離散系での可制御性行列 print(Uc) rank = np.linalg.matrix_rank(Uc) #ランク調査 print(rank) #ランクが4なら可制御 Ev = np.linalg.eigvals(P.A) #固有値=特性根計算 print(Ev) #単位円内なら安定 Uo = matlab.obsv(P.A, P.C) #可観測性行列作成 print(Uo) rank = np.linalg.matrix_rank(Uo) #ランク調査 print(rank) #ランクが4なら可観測 x0 = np.array([[0.1], [0.1], [0.1], [.1]]) N = 200 t = np.arange(0, N * T + T, T)
def runoptimization(): # define parameters mc = 1.0 mr = 0.25 g = 9.81 Fe = (mc + 2*mr)*g mu = 0.1 Jc = 0.0042 d = 0.3 H = 50 # number of points in the horizon # define the state space equations of the form xdot = Ax + Bu A_long = np.array([[0.0, 1.0], [0.0, 0.0]]) B_long = np.array([[0], [1 / (mc + 2*mr)]]) C_long = np.array([[1, 0], [0, 1]]) D_long = np.array([[0], [0]]) A_lat = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [0, -(Fe)/(mc+2*mr), (-mu)/(mc+2*mr), 0], [0, 0, 0, 0]]) B_lat = np.array([[0], [0], [0], [1/(Jc+2*mr*d**2)]]) C_lat = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) D_lat = np.array([[0], [0]]) # convert the continuous state space system to a discrete time system statespace_long = StateSpace(A_long, B_long, C_long, D_long) statespace_lat = StateSpace(A_lat, B_lat, C_lat, D_lat) # The interval of time between each discrete interval Ts = 0.05 # creates an object which contains the discretized version of A, B, C, and D as well as a dt discrete_long = c2d(statespace_long, Ts, method='zoh', prewarp_frequency=None) discrete_lat = c2d(statespace_lat, Ts, method='zoh', prewarp_frequency=None) objhist_h = np.ones(H) objhist_z = np.ones(H) def objcon_long(u, discrete_long): f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon h = 0.0 # initial position of the mass hdot = 0.0 # initial velocity of the mass href = 1.0 # the commanded longitudinal position for the VTOL x_long = np.array([[h], [hdot]]) # vectorize the state variables # g = np.array([0]) for i in range(len(u)): objhist_h[i] = h # if i != 0: # g = np.append(g, 100 - (u[i] - u[i-1])/Ts) # g = np.append(g, 100 + (u[i] - u[i-1])/Ts) f = f + abs(href - h) x_long_next = np.matmul(discrete_long.A, x_long) + np.matmul(discrete_long.B, np.array([[u[i]]])) h = x_long_next[0] x_long = x_long_next return f[0, 0] def objcon_lat(u, discrete_lat): f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon z = 0.0 theta = 0.0 zdot = 0.0 thetadot = 0.0 zref = 1.0 # the commanded lateral position for the VTOL x_lat = np.array([[z], [theta], [zdot], [thetadot]]) # g = np.array([0]) for i in range(len(u)): objhist_z[i] = z # if i != 0: # g = np.append(g, 100 - (u[i] - u[i-1])/Ts) # g = np.append(g, 100 + (u[i] - u[i-1])/Ts) f = f + abs(zref - z) x_lat_next = np.matmul(discrete_lat.A, x_lat) + np.matmul(discrete_lat.B, np.array([[u[i]]])) z = x_lat_next[0] x_lat = x_lat_next return f[0, 0] ulast_long = [] flast_long = [] glast_long = [] ulast_lat = [] flast_lat = [] glast_lat = [] def obj_long(u, discrete_long): nonlocal ulast_long, flast_long, glast_long if not np.array_equal(u, ulast_long): flast_long = objcon_long(u, discrete_long) ulast_long = u return flast_long def con_long(u): nonlocal ulast_long, flast_long, glast_long if not np.array_equal(u, ulast_long): flast_long = objcon_long(u, discrete_long) ulast_long = u return glast_long def obj_lat(u, discrete_lat): nonlocal ulast_lat, flast_lat, glast_lat if not np.array_equal(u, ulast_lat): flast_lat = objcon_lat(u, discrete_lat) ulast_lat = u return flast_lat def con_lat(u, discrete_lat): nonlocal ulast_lat, flast_lat, glast_lat if not np.array_equal(u, ulast_lat): flast_lat = objcon_lat(u, discrete_lat) ulast_lat = u return glast_lat u0_long = 0.5 * np.ones(H) u0_lat = 0.5 * np.ones(H) constraints = {'type': 'ineq', 'fun': con_long} options = {'disp': False} t0 = time.time() res_f = minimize(obj_long, u0_long, args=discrete_long, bounds=Bounds(-10.0, 10.0), options=options, method='slsqp') res_tau = minimize(obj_lat, u0_lat, args=discrete_lat, bounds=Bounds(-10.0, 10.0), options=options, method='slsqp') t1 = time.time() - t0 print('Time elapsed:', t1) # print("x = ", res.x) # print('f = ', res.fun) # print(res.success) # print(res.message) x_long_star = res_f.x x_lat_star = res_tau.x return x_long_star, x_lat_star, objhist_h, objhist_z
def main(): dt = 0.05 simulation_time = 30 # in seconds iteration_num = int(simulation_time / dt) # you must be care about this matrix # these A and B are for continuos system if you want to use discret system matrix please skip this step tau = 0.63 A = np.array([[-1. / tau, 0., 0., 0.], [0., -1. / tau, 0., 0.], [1., 0., 0., 0.], [0., 1., 0., 0.]]) B = np.array([[1. / tau, 0.], [0., 1. / tau], [0., 0.], [0., 0.]]) C = np.eye(4) D = np.zeros((4, 2)) # make simulator with coninuous matrix init_xs = np.array([0., 0., 0., 0.]) plant = FirstOrderSystem(A, B, C, init_states=init_xs) # create system sysc = matlab.ss(A, B, C, D) # discrete system sysd = matlab.c2d(sysc, dt) Ad = sysd.A Bd = sysd.B # evaluation function weight Q = np.diag([1., 1., 1., 1.]) R = np.diag([1., 1.]) pre_step = 10 # make controller with discreted matrix # please check the solver, if you want to use the scipy, set the MpcController_scipy controller = MpcController_cvxopt( Ad, Bd, Q, R, pre_step, dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]), input_upper=np.array([1., 3.]), input_lower=np.array([-1., -3.])) controller.initialize_controller() for i in range(iteration_num): print("simulation time = {0}".format(i)) reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten() states = plant.xs opt_u = controller.calc_input(states, reference) plant.update_state(opt_u, dt=dt) history_states = np.array(plant.history_xs) time_history_fig = plt.figure() x_fig = time_history_fig.add_subplot(411) y_fig = time_history_fig.add_subplot(412) v_x_fig = time_history_fig.add_subplot(413) v_y_fig = time_history_fig.add_subplot(414) v_x_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_states[:, 0]) v_x_fig.plot(np.arange(0, simulation_time + 0.01, dt), [0. for _ in range(iteration_num + 1)], linestyle="dashed") v_x_fig.set_xlabel("time [s]") v_x_fig.set_ylabel("v_x") v_y_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_states[:, 1]) v_y_fig.plot(np.arange(0, simulation_time + 0.01, dt), [0. for _ in range(iteration_num + 1)], linestyle="dashed") v_y_fig.set_xlabel("time [s]") v_y_fig.set_ylabel("v_y") x_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_states[:, 2]) x_fig.plot(np.arange(0, simulation_time + 0.01, dt), [-5. for _ in range(iteration_num + 1)], linestyle="dashed") x_fig.set_xlabel("time [s]") x_fig.set_ylabel("x") y_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_states[:, 3]) y_fig.plot(np.arange(0, simulation_time + 0.01, dt), [7.5 for _ in range(iteration_num + 1)], linestyle="dashed") y_fig.set_xlabel("time [s]") y_fig.set_ylabel("y") time_history_fig.tight_layout() plt.show() history_us = np.array(controller.history_us) input_history_fig = plt.figure() u_1_fig = input_history_fig.add_subplot(211) u_2_fig = input_history_fig.add_subplot(212) u_1_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_us[:, 0]) u_1_fig.set_xlabel("time [s]") u_1_fig.set_ylabel("u_x") u_2_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_us[:, 1]) u_2_fig.set_xlabel("time [s]") u_2_fig.set_ylabel("u_y") input_history_fig.tight_layout() plt.show()
[0, 0, (m * g) / (M * l) + (g / l), -0.25] ]) B = np.array([[ 0, ], [ 1 / M, ], [ 0, ], [ 1 / (M * l), ]]) C = np.array([[1., 0., 0., 0.], [0, 0, 1, 0]]) D = 0 sys_Cont = ct.StateSpace(A, B, C, D) sys_Dis = cm.c2d(sys_Cont, dt) Ad = sys_Dis.A Bd = sys_Dis.B Cd = sys_Dis.C # Controlability test Controlable = ct.ctrb(Ad, Bd) # print(np.linalg.matrix_rank(Controlable)) # full rank - therefore controllable # Observability Test: Observable = ct.obsv(Ad, Cd) # print(np.linalg.matrix_rank(Observable)) # full rank - therefore observable def pendControl(sys, sysC, x0, x0_est):
def runoptimization(): # define parameters m = 5 k = 3 b = 0.5 H = 10 # number of points in the horizon # define the state space equations of the form xdot = Ax + Bu A = np.array([[0, 1], [-k / m, -b / m]]) B = np.array([[0], [1 / m]]) C = np.array([[1, 0], [0, 1]]) D = np.array([[0], [0]]) # convert the continuous state space system to a discrete time system statespace = StateSpace(A, B, C, D) # The interval of time between each discrete interval Ts = 0.2 # creates an object which contains the discretized version of A, B, C, and D as well as a dt discrete = c2d(statespace, Ts, method='zoh', prewarp_frequency=None) objhist = np.ones(H) def objcon(u, discrete): f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon z = 0.0 # initial position of the mass zdot = 0.0 # initial velocity of the mass zref = 1.0 # the commanded position for the mass x = np.array([[z], [zdot]]) # vectorize the state variables g = np.array([0]) for i in range(len(u)): objhist[i] = z if i != 0: g = np.append(g, 100 - (u[i] - u[i - 1]) / Ts) g = np.append(g, 100 + (u[i] - u[i - 1]) / Ts) f = f + abs(zref - z) x_next = np.matmul(discrete.A, x) + np.matmul( discrete.B, np.array([[u[i]]])) z = x_next[0] x = x_next return f[0, 0], g ulast = [] flast = [] glast = [] def obj(u, discrete): nonlocal ulast, flast, glast if not np.array_equal(u, ulast): flast, glast = objcon(u, discrete) ulast = u return flast def con(u): nonlocal ulast, flast, glast if not np.array_equal(u, ulast): flast, glast = objcon(u, discrete) ulast = u return glast u0 = 0.5 * np.ones(H) constraints = {'type': 'ineq', 'fun': con} options = {'disp': False} t0 = time.time() res = minimize(obj, u0, args=discrete, bounds=Bounds(-10.0, 10.0), constraints=constraints, options=options, method='slsqp') t1 = time.time() - t0 print('Time elapsed:', t1) # print("x = ", res.x) # print('f = ', res.fun) # print(res.success) # print(res.message) x_star = res.x return x_star, objhist
plt.plot(tt, temp, label='temp') plt.plot(tt, ff_temp, label='ff_temp') plt.legend() plt.grid() tsp = 200 sim_wfs = np.zeros(tt.size) sim_temp = np.zeros(tt.size) fplant = weld1.filter1.Filter1((0,0.002985),(1., -0.99004983)) ctrl2 = 0.2 * control.tf((1,43), (1,0)) * control.tf((1,7), (1/10,1)) # ctrl2 = 2 * control.tf((1/1,1), (1,0)) * control.tf((0/1,1), (0/2,1)) ctrl2 = 0.25 * control.tf((1,20), (1,0)) * control.tf((1,10), (1/15,1)) ctrl2 = control.tf(3, 1) + control.tf(2, (1,0)) + control.tf((0,0), 1) ctrl2 = control.tf(7.5, 1) + control.tf(50, (1,0)) + control.tf((0.25,0), (0,1)) ctrl2_d = mt.c2d(ctrl2, .001) fctrl = weld1.filter1.Filter1(ctrl2_d.num[0][0],ctrl2_d.den[0][0]) b, a = signal.butter(3, 100/500, 'low') ftemp = weld1.filter1.Filter1(b, a) for i in range(1,tt.size): tsp = max(temp[i] - 1100, 0) # err = tsp - ftemp.step(sim_temp[i-1]) err = tsp - sim_temp[i - 1] # ftemp.step(sim_temp[i-1]) sim_wfs[i] = fctrl.step(err) sim_temp[i] = fplant.step(sim_wfs[i]) plt.figure('Simulation 10') plt.plot(tt, temp-1100, label='temp') plt.plot(tt, sim_wfs, label='sim_wfs') plt.plot(tt, sim_temp, label='sim_temp') plt.legend()