def testMimoW123(self): """MIMO plant with all weights""" from control import augw, ss, append, minreal g = ss([[-1., -2], [-3, -4]], [[1., 0.], [0., 1.]], [[1., 0.], [0., 1.]], [[1., 0.], [0., 1.]]) # this should be expaned to w1*I w1 = ss([-2.], [2.], [1.], [2.]) # diagonal weighting w2 = append(ss([-3.], [3.], [1.], [3.]), ss([-4.], [4.], [1.], [4.])) # full weighting w3 = ss([[-4., -5], [-6, -7]], [[2., 3.], [5., 7.]], [[11., 13.], [17., 19.]], [[23., 29.], [31., 37.]]) p = augw(g, w1, w2, w3) self.assertEqual(8, p.outputs) self.assertEqual(4, p.inputs) # w->z1 should be w1 self.siso_almost_equal(w1, p[0, 0]) self.siso_almost_equal(0, p[0, 1]) self.siso_almost_equal(0, p[1, 0]) self.siso_almost_equal(w1, p[1, 1]) # w->z2 should be 0 self.siso_almost_equal(0, p[2, 0]) self.siso_almost_equal(0, p[2, 1]) self.siso_almost_equal(0, p[3, 0]) self.siso_almost_equal(0, p[3, 1]) # w->z3 should be 0 self.siso_almost_equal(0, p[4, 0]) self.siso_almost_equal(0, p[4, 1]) self.siso_almost_equal(0, p[5, 0]) self.siso_almost_equal(0, p[5, 1]) # w->v should be I self.siso_almost_equal(1, p[6, 0]) self.siso_almost_equal(0, p[6, 1]) self.siso_almost_equal(0, p[7, 0]) self.siso_almost_equal(1, p[7, 1]) # u->z1 should be -w1*g self.siso_almost_equal(-w1 * g[0, 0], p[0, 2]) self.siso_almost_equal(-w1 * g[0, 1], p[0, 3]) self.siso_almost_equal(-w1 * g[1, 0], p[1, 2]) self.siso_almost_equal(-w1 * g[1, 1], p[1, 3]) # u->z2 should be w2 self.siso_almost_equal(w2[0, 0], p[2, 2]) self.siso_almost_equal(w2[0, 1], p[2, 3]) self.siso_almost_equal(w2[1, 0], p[3, 2]) self.siso_almost_equal(w2[1, 1], p[3, 3]) # u->z3 should be w3*g w3g = w3 * g self.siso_almost_equal(w3g[0, 0], minreal(p[4, 2])) self.siso_almost_equal(w3g[0, 1], minreal(p[4, 3])) self.siso_almost_equal(w3g[1, 0], minreal(p[5, 2])) self.siso_almost_equal(w3g[1, 1], minreal(p[5, 3])) # u->v should be -g self.siso_almost_equal(-g[0, 0], p[6, 2]) self.siso_almost_equal(-g[0, 1], p[6, 3]) self.siso_almost_equal(-g[1, 0], p[7, 2]) self.siso_almost_equal(-g[1, 1], p[7, 3])
def ss_error(g, err_step=None, err_ramp=None, err_para=None): s = ct.TransferFunction([1, 0], [1]) kx = None ess = None if err_step is not None: ess = ct.evalfr(ct.minreal(g), 0j) kx = 1 / err_step elif err_ramp is not None: ess = ct.evalfr(ct.minreal(s*g), 0j) kx = 1 / err_ramp elif err_para is not None: ess = ct.evalfr(ct.minreal(s**2*g), 0j) kx = 1 / err_para return kx, ess
def step_system(xopt, label, ax): kp, tz1, tz2, tz3, tp = xopt Co = kp * (tz1 * s + 1) * (tz2 * s + 1) * (tz3 * s + 1) / (s * s * (tp * s + 1)) To = control.minreal(Co * Go / (1 + Co * Go), verbose=False) _, yout = control.forced_response(To, T, U=ramp, squeeze=True) ax.plot(T, yout, label=label)
def get_plant_op(self, u_h, reduce_states): ''' Interpolate system matrices using wind speed operating point uh_op = mean(u_h) inputs: u_h timeseries of wind speeds ''' uh_op = np.mean(u_h) if len(self.u_h) > 1: f_A = sp.interpolate.interp1d(self.u_h, self.A_ops) f_B = sp.interpolate.interp1d(self.u_h, self.B_ops) f_C = sp.interpolate.interp1d(self.u_h, self.C_ops) f_D = sp.interpolate.interp1d(self.u_h, self.D_ops) f_u = sp.interpolate.interp1d(self.u_h, self.u_ops) f_y = sp.interpolate.interp1d(self.u_h, self.y_ops) f_x = sp.interpolate.interp1d(self.u_h, self.x_ops) A = f_A(uh_op) B = f_B(uh_op) C = f_C(uh_op) D = f_D(uh_op) u_op = f_u(uh_op) x_op = f_x(uh_op) y_op = f_y(uh_op) else: print('WARNING: Only one linearization, at ' + str(self.u_h[0]) + ' m/s') print('Simulation operating point is ' + str(uh_op) + 'm/s') print('Results may not be as expected') # Set linear system to only linearization A = self.A_ops[:, :, 0] B = self.B_ops[:, :, 0] C = self.C_ops[:, :, 0] D = self.D_ops[:, :, 0] u_op = self.u_ops[:, 0] y_op = self.y_ops[:, 0] x_op = self.x_ops[:, 0] # form state space model TODO: generalize using linear description strings P_op = co.StateSpace(A, B, C, D) if reduce_states: P_op = co.minreal(P_op, tol=1e-7, verbose=False) P_op.InputName = ['RtVAvgxh', 'BldPitch'] P_op.OutputName = ['GenSpeed', 'TwrBsMyt', 'PtfmPitch', 'NcIMUTAzs'] # operating points dict ops = {} ops['u'] = u_op[self.ind_fast_inps] ops['uh'] = uh_op ops['y'] = y_op[self.ind_fast_outs] ops['x'] = x_op return ops, P_op
def siso_almost_equal(self,g,h): """siso_almost_equal(g,h) -> None Raises AssertionError if g and h, two SISO LTI objects, are not almost equal""" from control import tf, minreal gmh = tf(minreal(g-h,verbose=False)) if not (gmh.num[0][0]<self.TOL).all(): maxnum = max(abs(gmh.num[0][0])) raise AssertionError('systems not approx equal; max num. coeff is {}\nsys 1:\n{}\nsys 2:\n{}'.format(maxnum,g,h))
def objective(x, metric=ise): kp, tz1, tz2, tz3, tp = x Co = kp * (tz1 * s + 1) * (tz2 * s + 1) * (tz3 * s + 1) / (s * s * (tp * s + 1)) So = control.minreal(1 / (1 + Co * Go), verbose=False) T = np.arange(0.0, sim_time, 1e-5) ramp = T * gradient _, yout = control.forced_response(So, T, U=ramp, squeeze=True) return (metric(T, yout) / sim_time, )
def siso_almost_equal(self, g, h): """siso_almost_equal(g,h) -> None Raises AssertionError if g and h, two SISO LTI objects, are not almost equal """ # TODO: use pytest's assertion rewriting feature gmh = tf(minreal(g - h, verbose=False)) if not (gmh.num[0][0] < self.TOL).all(): maxnum = max(abs(gmh.num[0][0])) raise AssertionError("systems not approx equal; " "max num. coeff is {}\n" "sys 1:\n" "{}\n" "sys 2:\n" "{}".format(maxnum, g, h))
def print_TF(self, window, entry1, entry2, fontsize="9", relx=0, rely=0, relwidth=1, min=False, relheight=1): transfer_function = self.get_TF(entry1, entry2) if min: transfer_function = co.minreal(transfer_function) label2 = tk.Label(window, text=str(transfer_function), bg="#EBE7E7", font="comicsans " + fontsize) label2.place(relx=relx, rely=rely, relwidth=relwidth)
D = 0.0 G = con.StateSpace(A, B, C, D) # Weighting functions w0 = 8.0 # Desired closed-loop bandwidth A = 1 / 100 # Desired disturbance attenuation inside bandwidth M = 1.5 # Desired bound on hinfnorm(S) & hinfnorm(T) w1 = con.TransferFunction([1 / M, w0], [1.0, w0 * A]) #sensitivity weight w2 = con.TransferFunction([1.0, 0.1], [1, 100]) # K*S weight # w3 = [] #empty control weight on T k, cl, info = con.mixsyn(G, w1, w2, w3=None) print(info[0]) #gamma L = G * k Ltf = con.ss2tf(L) Ltf = con.minreal(Ltf) S = 1.0 / (1.0 + Ltf) T = 1 - S plt.figure(1) sw1 = triv_sigma(1.0 / w1, w) sigS = triv_sigma(S, w) plt.semilogx(w, 20 * np.log10(sw1[:, 0]), label=r'$\sigma_1(1/W1)$') plt.semilogx(w, 20 * np.log10(sigS[:, 0]), label=r'$\sigma_1(S)$') plt.ylim([-60, 10]) plt.ylabel('magnitude [dB]') plt.xlim([1e-3, 1e3]) plt.xlabel('freq [rad/s]') plt.legend() plt.title('Singular values of S') plt.grid(1)
import control as co import numpy as np import matplotlib.pyplot as plt G1 = co.tf([2,5],[1,2,3]) # 2s + 5 / s^2 + 2s + 3 --- Provided only the coeficients G2 = 5*co.tf(np.poly([-2,-5]), np.poly([-4,-5,-9])) # Provided the 'zeros' and the 'poles' of the function # Algebrism is supported G3 = G1+G2 G4 = G1*G2 G5 = G1/G2 G6 = G1-G2 G7 = co.feedback(G1,G2) # Minimum Realization (Pole-Zero Cancellation) co.minreal(G2) # TF Properties print(G1) print('DC Gain: '+ str(co.dcgain(G1))) # DC Gain print('Pole: ' + str(co.pole(G1))) # Pole for G6 print('Zero: ' + str(co.zero(G1))) # Zero for G6
def code_generation(): x = ca.SX.sym('x', 14) x_gz = ca.SX.sym('x_gz', 14) p = ca.SX.sym('p', 16) u = ca.SX.sym('u', 4) t = ca.SX.sym('t') dt = ca.SX.sym('dt') gz_eqs = gazebo_equations() f_state = gz_eqs['state_from_gz'] eqs = rocket_equations() C_FLT_FRB = gz_eqs['C_FLT_FRB'] F_FRB, M_FRB = eqs['force_moment'](x, u, p) F_FLT = ca.mtimes(C_FLT_FRB, F_FRB) M_FLT = ca.mtimes(C_FLT_FRB, M_FRB) f_u_to_fin = ca.Function('rocket_u_to_fin', [u], [u_to_fin(u)], ['u'], ['fin']) f_force_moment = ca.Function('rocket_force_moment', [x, u, p], [F_FLT, M_FLT], ['x', 'u', 'p'], ['F_FLT', 'M_FLT']) u_control = eqs['control'](x, p, t, dt) f_control = ca.Function('rocket_control', [x, p, t, dt], [u_control], ['x', 'p', 't', 'dt'], ['u']) gen = ca.CodeGenerator('casadi_gen.c', { 'main': False, 'mex': False, 'with_header': True, 'with_mem': True }) x0, u0, p0 = do_trim(vt=100, gamma_deg=60, m_fuel=0.8, z=60) x1, u1, p1 = do_trim(vt=100, gamma_deg=75, m_fuel=0.6, z=90) x2, u2, p2 = do_trim(vt=100, gamma_deg=90, m_fuel=0.3, z=100) # x3, u3, p3 = do_trim(vt=100, gamma_deg=85, m_fuel=0, z=100) lin = linearize() import control sys1 = control.ss(*lin(x0, u0, p0)) sys2 = control.ss(*lin(x1, u1, p1)) sys3 = control.ss(*lin(x2, u2, p2)) # sys4 = control.ss(*lin(x3, u3, p3)) G1 = control.ss2tf(sys1[1, 2]) G2 = control.ss2tf(sys2[1, 2]) G3 = control.ss2tf(sys3[1, 2]) # G4 = control.ss2tf(sys4[1, 2]) G1 = control.c2d(G1, .01) G2 = control.c2d(G2, .01) G3 = control.c2d(G3, .01) # G4 = control.c2d(G4, .01) s = control.tf([1, 0], [0, 1]) Hd1 = (20 + 3 / s) Hd2 = (20 + 3 / s) Hd3 = (20 + 3 / s) G1 = control.minreal(G1 * Hd1) G2 = control.minreal(G2 * Hd2) G3 = control.minreal(G3 * Hd3) G1 = control.tf2ss(G1) G2 = control.tf2ss(G2) G3 = control.tf2ss(G3) # G4 = control.minreal(G4) #print (G3) x0 = ca.SX.sym('x0', 8) u = ca.SX.sym('u', 1) x = ca.mtimes(G1.A, x0) + ca.mtimes(G1.B, u) y = ca.mtimes(G1.C, x0) + ca.mtimes(G1.D, u) controller1 = ca.Function('controller1', [x0, u], [x, y]) x = ca.mtimes(G2.A, x0) + ca.mtimes(G2.B, u) y = ca.mtimes(G2.C, x0) + ca.mtimes(G2.D, u) controller2 = ca.Function('controller2', [x0, u], [x, y]) x0 = ca.SX.sym('x', 4) x = ca.mtimes(G3.A, x0) + ca.mtimes(G3.B, u) y = ca.mtimes(G3.C, x0) + ca.mtimes(G3.D, u) controller3 = ca.Function('controller3', [x0, u], [x, y]) gen.add(controller1) gen.add(controller2) gen.add(controller3) gen.add(f_state) gen.add(f_force_moment) gen.add(f_control) gen.add(f_u_to_fin) #gen.generate() return { 'controller1': controller1, 'controller2': controller2, 'controller3': controller3 }
[time2, yout2] = con.step_response(t_function, time_simulation, start_condition) plt.figure(1) plt.plot(time2, yout2, 'r') plt.xlabel('time') plt.ylabel('volts') plt.figure(2) plt.plot(t, y_out, 'b*', t, input_u, 'g-') #ploting time x output and time x input plt.xlabel('time') plt.ylabel('volts') plt.show() print("natural frequency = ", wn) print("damping constant = ", zetas) print("system poles = ", poles) #take the transfer function and now getting an space state [a, b, c, d] = con.ssdata(t_function) state_space_2 = con.ss(a, b, c, d) my_sys = con.minreal(state_space_2) print("G(s) = ", t_function) #function transfer print("###################") print(my_sys) # state space
Z2 = R2 / (1 + s * R2 * C2) # Z2 = 1 / (s * C3) * (R2 + 1 / (s * C2)) / (1 / (s * C3) + (R2 + 1 / (s * C2))) # sys_und_tst = (s) / (1000 + s) # sys_und_tst = 1 / (s ** 3 + 2 * s ** 2 + 2 * s) sys_und_tst = Z2 / (Z1 + Z2) # discrete # sys_und_tst = z / (z - 1) # sys_und_tst = 1 / (z - 1) # sys_und_tst = 1 + z ** -1 + z ** -2 print(sys_und_tst) sim_sys = con.minreal(sys_und_tst) print(sim_sys) sys_und_tst = sim_sys gg = s_plot_val_func(sys_und_tst.num, sys_und_tst.den, x) # https://www.sympy.org/scipy-2017-codegen-tutorial/notebooks/22-lambdify.html # https://jakevdp.github.io/PythonDataScienceHandbook/04.12-three-dimensional-plotting.html g = sp.lambdify([x], gg) fstop_c = 10e3 nsamp_pos_neg = 1000 nsamp_single = int(nsamp_pos_neg / 2)
pi_regulator = control.tf([Kp, Kp * Ki], [1, 0]) display(dc_motor) display(pi_regulator) # figure() # mag, phase, omega = bode_plot(dc_motor, 2*np.pi*np.logspace(-2,4,1000), dB=1, Hz=1, deg=1) # figure() # mag, phase, omega = bode_plot(pi_regulator, 2*np.pi*np.logspace(-2,4,1000), dB=1, Hz=1, deg=1) open_sys = control.series(pi_regulator, dc_motor) closed_sys = control.feedback(dc_motor, pi_regulator, sign=-1) # open_sys = pi_regulator * dc_motor closed_sys = open_sys / (1 + open_sys) display(open_sys) display(control.minreal(closed_sys)) closed_sys = control.minreal(closed_sys) # figure() # mag, phase, omega = bode_plot(open_sys, 2*np.pi*np.logspace(-2,4,1000), dB=1, Hz=1, deg=1) plt.figure() mag, phase, omega = bode_plot(closed_sys, 2 * np.pi * np.logspace(-2, 4, 1000), dB=1, Hz=1, deg=1) T, yout = control.step_response(closed_sys, np.arange(0, 2, 1e-4)) plt.figure() plt.plot(T, yout)
def __init__(self, lin_file, reduceStates=False, fromMat=False): ''' inputs: lin_file - directory of linear file outputs from OpenFAST - if fromMat = True, lin_file is .mat from matlab version of mbc3 ''' if not fromMat: # figure out number of linear cases out_prefix = 'lin' out_suffix = '.outb' out_files = glob.glob( os.path.join(lin_file, out_prefix + '*' + out_suffix)) n_lin_cases = len(out_files) if not n_lin_cases: print('No linear outputs found in ' + lin_file) quit() if n_lin_cases <= 10: num_string = '%01d' else: num_string = '%02d' u_ops = np.array([], []) for iCase in range(0, n_lin_cases): lin_files_i = glob.glob( os.path.join( lin_file, out_prefix + '_' + num_string % (iCase) + '.*.lin')) MBC, matData, FAST_linData = mbc.fx_mbc3(lin_files_i) if not iCase: # first time through # Initialize operating points, matrices u_ops = np.zeros((matData['NumInputs'], n_lin_cases)) y_ops = np.zeros((matData['NumOutputs'], n_lin_cases)) x_ops = np.zeros((matData['NumStates'], n_lin_cases)) # operating points # u_ops \in real(n_inps,n_ops) u_ops[:, iCase] = np.mean(matData['u_op'], 1) # y_ops \in real(n_outs,n_ops) y_ops[:, iCase] = np.mean(matData['y_op'], 1) # x_ops \in real(n_states,n_ops), note this is un-reduced state space model states, with all hydro states x_ops[:, iCase] = np.mean(matData['xop'], 1) # Matrices, TODO: automate index selection here PitchDesc = 'ED Extended input: collective blade-pitch command, rad' WindDesc = 'IfW Extended input: horizontal wind speed (steady/uniform wind), m/s' GenDesc = 'ED GenSpeed, (rpm)' TwrDesc = 'ED TwrBsMyt, (kN-m)' AzDesc = 'ED Variable speed generator DOF (internal DOF index = DOF_GeAz), rad' PltPitchDesc = 'ED PtfmPitch, (deg)' NacIMUFADesc = 'ED NcIMURAxs, (deg/s^2)' indPitch = matData['DescCntrlInpt'].index(PitchDesc) indWind = matData['DescCntrlInpt'].index(WindDesc) indGen = matData['DescOutput'].index(GenDesc) indTwr = matData['DescOutput'].index(TwrDesc) indAz = matData['DescStates'].index(AzDesc) indPltPitch = matData['DescOutput'].index(PltPitchDesc) indNacIMU = matData['DescOutput'].index(NacIMUFADesc) indOuts = np.array([indGen, indTwr, indPltPitch, indNacIMU]).reshape(-1, 1) indInps = np.array([indWind, indPitch]).reshape(-1, 1) # TODO: add torque # remove azimuth state indStates = np.arange(0, matData['NumStates']) indStates = np.delete(indStates, indAz) indStates = indStates.reshape(-1, 1) if not iCase: A_ops = np.zeros( (len(indStates), len(indStates), n_lin_cases)) B_ops = np.zeros( (len(indStates), len(indInps), n_lin_cases)) C_ops = np.zeros( (len(indOuts), len(indStates), n_lin_cases)) D_ops = np.zeros((len(indOuts), len(indInps), n_lin_cases)) # A \in real(n_states,n_states,n_ops) A_ops[:, :, iCase] = MBC['AvgA'][indStates, indStates.T] # B \in real(n_states,n_inputs,n_ops) B_ops[:, :, iCase] = MBC['AvgB'][indStates, indInps.T] # C \in real(n_outs,n_states,n_ops) C_ops[:, :, iCase] = MBC['AvgC'][indOuts, indStates.T] # D \in real(n_outs,n_inputs,n_ops) D_ops[:, :, iCase] = MBC['AvgD'][indOuts, indInps.T] if reduceStates: if not iCase: n_states = np.zeros((n_lin_cases, 1), dtype=int) P = co.StateSpace(A_ops[:, :, iCase], B_ops[:, :, iCase], C_ops[:, :, iCase], D_ops[:, :, iCase], remove_useless=False) # figure out minimum number of states for the systems at each operating point P_min = co.minreal(P, tol=1e-7, verbose=False) n_states[iCase] = P_min.states # Now loop through and reduce number of states to maximum of n_states # This is broken!! Works fine if reduceStates = False and isn't problematic to have all the extra states...yet if reduceStates: for iCase in range(0, n_lin_cases): if not iCase: self.A_ops = np.zeros( (max(n_states)[0], max(n_states)[0], n_lin_cases)) self.B_ops = np.zeros( (max(n_states)[0], len(indInps), n_lin_cases)) self.C_ops = np.zeros( (len(indOuts), max(n_states)[0], n_lin_cases)) self.D_ops = np.zeros( (len(indOuts), len(indInps), n_lin_cases)) P = co.StateSpace(A_ops[:, :, iCase], B_ops[:, :, iCase], C_ops[:, :, iCase], D_ops[:, :, iCase], remove_useless=False) P_red = co.balred( P, max(n_states)[0], method='matchdc' ) # I don't know why it's not reducing to max(n_states), must add 2 # P_red = co.minreal(P,tol=1e-7,verbose=False) # I don't know why it's not reducing to max(n_states), must add 2 self.A_ops[:, :, iCase] = P_red.A self.B_ops[:, :, iCase] = P_red.B self.C_ops[:, :, iCase] = P_red.C self.D_ops[:, :, iCase] = P_red.D else: self.A_ops = A_ops self.B_ops = B_ops self.C_ops = C_ops self.D_ops = D_ops # Save wind speed as own array since that's what we'll schedule over to start self.u_ops = u_ops self.u_h = self.u_ops[0] self.y_ops = y_ops self.x_ops = x_ops # Input/Output Indices self.ind_fast_inps = indInps.squeeze() self.ind_fast_outs = indOuts.squeeze() else: # from matlab .mat file m matDict = loadmat(lin_file) # operating points # u_ops \in real(n_inps,n_ops) u_ops = np.zeros(matDict['u_ops'][0][0].shape) for u_op in matDict['u_ops'][0]: u_ops = np.concatenate((u_ops, u_op), axis=1) self.u_ops = u_ops[:, 1:] # y_ops \in real(n_outs,n_ops) y_ops = np.zeros(matDict['y_ops'][0][0].shape) for y_op in matDict['y_ops'][0]: y_ops = np.concatenate((y_ops, y_op), axis=1) self.y_ops = y_ops[:, 1:] # x_ops \in real(n_states,n_ops), note this is un-reduced state space model states, with all hydro states x_ops = np.zeros(matDict['x_ops'][0][0].shape) for x_op in matDict['x_ops'][0]: x_ops = np.concatenate((x_ops, x_op), axis=1) self.x_ops = x_ops[:, 1:] # Matrices # A \in real(n_states,n_states,n_ops) n_states = np.shape(matDict['A'][0][0])[0] A_ops = np.zeros((n_states, n_states, 1)) for A_op in matDict['A'][0]: A_ops = np.concatenate((A_ops, np.expand_dims(A_op, 2)), axis=2) self.A_ops = A_ops[:, :, 1:] # B \in real(n_states,n_inputs,n_ops) n_states = np.shape(matDict['B'][0][0])[0] n_inputs = np.shape(matDict['B'][0][0])[1] B_ops = np.zeros((n_states, n_inputs, 1)) for B_op in matDict['B'][0]: B_ops = np.concatenate((B_ops, np.expand_dims(B_op, 2)), axis=2) self.B_ops = B_ops[:, :, 1:] # C \in real(n_outs,n_states,n_ops) n_states = np.shape(matDict['C'][0][0])[1] n_outs = np.shape(matDict['C'][0][0])[0] C_ops = np.zeros((n_outs, n_states, 1)) for C_op in matDict['C'][0]: C_ops = np.concatenate((C_ops, np.expand_dims(C_op, 2)), axis=2) self.C_ops = C_ops[:, :, 1:] # D \in real(n_outs,n_inputs,n_ops) n_states = np.shape(matDict['D'][0][0])[1] n_outs = np.shape(matDict['D'][0][0])[0] D_ops = np.zeros((n_outs, n_inputs, 1)) for D_op in matDict['D'][0]: D_ops = np.concatenate((D_ops, np.expand_dims(D_op, 2)), axis=2) self.D_ops = D_ops[:, :, 1:] # Save wind speed as own array since that's what we'll schedule over to start self.u_h = self.u_ops[0] # Input/Output Indices self.ind_fast_inps = matDict['indInps'][0] - 1 self.ind_fast_outs = matDict['indOuts'][0] - 1
wn_h = wn_th / 20.0 kph = 2 * damp_h * wn_h / (k_thDC * Va) kih = wn_h**2 / (k_thDC * Va) # successive loop closure to form transfer function from h_c to h # transfer function from del_e to q q_del_e = ctrl.tf([a_theta3, 0], [1, a_theta1, a_theta2]) # close pitch rate loop to get del_e_c to q q_del_e_c = ctrl.feedback(q_del_e, kdth) # integrator 1/s to get theta from q integrator = ctrl.tf(1, [1, 0]) # transfer function from theta_c to theta th_th_c = ctrl.minreal(ctrl.feedback(kpth * q_del_e_c * integrator, 1)) # implementation of altitude PI control pi_control = ctrl.tf([kph, kih], [1, 0]) # close PI feedback loop on altitude to get h_c to h h_h_c = ctrl.feedback(pi_control * th_th_c * Va * integrator, 1) # (note this h_h_c transfer function does not approximate th_th_c as KthDC # instead it contains the full linearlized altitude dynamics) # step response t = 0.1 * np.arange(600) t, h = ctrl.step_response(h_h_c, t) # response to unit step command altitude_step_size = 100.0
A = [[Y_b/u_0, -(1.0-Y_r/u_0)],[N_b, N_r]] # x1 = beta, x2 = r B = [[0.0,Y_dr/u_0],[N_da,N_dr]] #u1 = delta_a, u2 = delta_r C = [[1.0,0.0],[0.0,1.0]] # y1 = beta, y2 = r D = [[0.0,0.0],[0.0,0.0]] G = ss(A,B,C,D) #weighting function w1 = weighting(2, 2, 0.005) w1 = ss(w1) #needs to be converted for append wp = w1.append(w1) wu = ss([], [], [], np.eye(2)) #unwieghted actuator authority size of 2 #Augmented plant need to make by hand for hinf syn I = ss([], [], [], np.eye(2)) P = augw(G,wp,wu) #generalized plant P P = minreal(P) wps = tf(wp) Gs= tf(G) Is= tf(I) # correct generalized plant p11 = wps p12 = wps*Gs p21 = -Is p22 = -Gs K2 = h2syn(P, 2, 2) # H INF CONTROLLER K, CL, gam, rcond = hinfsyn(P,2,2) #generalized plant incorrect so doing mixsyn print(gam)
def __call__(self, variant: int, codeddata: str, _globals: dict): """ Check whether a given answer is within tolerance. This checks the variable defined in the constructor against a reference value in the codeddata. Parameters ---------- variant : int Variant of the answer. codeddata : str Encoded answer array. Returns ------- testname : str Short name for the test score : float Normalized score, 0 or 1 result : str Text describing the result studentanswer : str Student's answer, as interpreted modelanswer : str Reference answer """ dec = json.JSONDecoder() Aref, Bref, Cref, Dref = map( np.array, dec.decode(cmpr.decompress(b64decode(codeddata.encode('ascii'))) .decode('utf-8'))[variant]) sys_ref = StateSpace(Aref, Bref, Cref, Dref) # checking function def within_tolerance(xr, x): return np.allclose(xr, x, self.d_rel, self.d_abs) fails = 0 value = self._extractValue(_globals) try: A = np.array(value["A"]) B = np.array(value["B"]) C = np.array(value["C"]) D = np.array(value["D"]) dt = value["dt"] value = StateSpace(A, B, C, D, dt) except Exception: return self._return(0.0, "1 is not a state-space system", _globals[self.var], sys_ref) report = [] try: if A.shape != Aref.shape: value = minreal(value) A = value.A B = value.B C = value.C if A.shape == Aref.shape: fails += round(self.threshold / 2) report.append('system is not minimal realization') if A.shape != Aref.shape: fails += self.threshold report.append('incorrect system order') if B.shape[1] != Bref.shape[1]: fails += self.threshold report.append('incorrect number of inputs') if C.shape[0] != Cref.shape[0]: fails += self.threshold report.append('incorrect number of outputs') if fails > self.threshold: return self._return(0.0, "\n".join(report), value, sys_ref) # d matrix ndfail = Dref.size - \ np.sum(np.isclose(Dref, D, self.d_rel, self.d_abs)) if ndfail: report.append('{ndfail} incorrect elements in D matrix' ''.format(ndfail=ndfail)) fails += ndfail # orders/sizes correct, can now check transfers zpk_ref = ss_to_zpk(Aref, Bref, Cref) zpk_val = ss_to_zpk(A, B, C) for im in range(len(zpk_ref)): for ip in range(len(zpk_ref[0])): ok, r = zpk_ref[im][ip].check(zpk_val[im][ip], im, ip, within_tolerance) if not ok: fails += 1 report.extend(r) except Exception: return self._return(0.0, "not a valid state-space system", value, sys_ref) score = max(round( (self.threshold + 1 - fails) / (self.threshold + 1), 3), 0.0) return self._return( score, ((not report) and "answer is correct") or "\n".join(report), value, sys_ref)
C = [0.0, 1.0] D = 0.0 G = con.StateSpace(A, B, C, D) # Controller synthesis wb = 8 # Desired closed-loop bandwidth A = 1 / 100 # Desired disturbance attenuation inside bandwidth M = 1.5 # Desired bound on hinfnorm(S) & hinfnorm(T) w1 = con.TransferFunction([1 / M, wb], [1.0, wb * A]) #tracking/disturbance weight w2 = con.TransferFunction([1.0, 0.1], [1, 100]) #K*S (actuator authority) weight # Augmented plant P = con.augw(G, w1, w2) #generalized plant P P = con.minreal(P) # H 2 CONTROLLER K2 = con.h2syn(P, 1, 1) L = G * K2 Ltf = con.ss2tf(L) So2 = 1.0 / (1.0 + Ltf) So2 = con.minreal(So2) To2 = G * K2 * So2 To2 = con.minreal(To2) # H INF CONTROLLER K, CL, gam, rcond = con.hinfsyn(P, 1, 1) print(gam)
print(GF) print('GFB aka Feedback Gain (GFB)') print(GFB) K = 0.5 unreduced_Gcl = (K * GF) / (1 + K * GF * GFB) print( 'Using equation GCL_SetLev_Pout = GF/(1+GOL) aka GCL_SetLev_Pout = GF/(1+GF*GFB) with K={}' .format(K)) # looking at McNeill's notes, you can see GOL should be better termed as Loop Gain # Forward Gain GF (from input to output) # Feedback Gain GFB (from output to - term of sum node) # Loop Gain aka Open Loop Gain GOL (break loop at - term, GF*GFB) print(unreduced_Gcl) reduced_sys = con.minreal((K * GF) / (1 + K * GF * GFB)) print( 'Reduced GCL_SetLev_Pout. It should be the same as GCL_SetLev_Pout returned from con.feedback(K * GF, GFB) with K={}' .format(K)) print(reduced_sys) n = np.arange(25) # step is done on the closed loop system plt.figure() GCL_SetLev_Pout = con.feedback(K * GF, GFB) # step respoonse for when Set Level is increased from 0 to 1500, GCL_SetLev_Pout => Set Level to Pout step_size = 1500 # bits n, yout = con.step_response(step_size * GCL_SetLev_Pout, T=n) print('GCL_SetLev_Pout from con.feedback with K={}'.format(K)) print(GCL_SetLev_Pout)
def _nq_serial_decomposition(system: "IdealSystem", q: StaticQuantizer, verbose: bool) -> Tuple["DynamicQuantizer", float]: """ Finds the stable and optimal dynamic quantizer for `system` using serial decomposition[1]_. Parameters ---------- system : IdealSystem q : StaticQuantizer T : int gain_wv : float verbose : bool dim : int Returns ------- (Q, E) : Tuple[DynamicQuantizer, float] """ if verbose: print("Trying to calculate quantizer using serial system decomposition...") tf = system.P.tf1 zeros_ = _ctrl.zero(tf) poles = _ctrl.pole(tf) unstable_zeros = [zero for zero in zeros_ if abs(zero) > 1] z = _ctrl.TransferFunction.z z.dt = tf.dt if len(unstable_zeros) == 0: n_time_delay = len([p for p in poles if p == 0]) # count pole-zero G = 1 / z**n_time_delay F = tf / G F_ss = _ctrl.tf2ss(F) B_F = matrix(F_ss.B) C_F = matrix(F_ss.C) E = abs(C_F @ B_F)[0, 0] * q.delta elif len(unstable_zeros) == 1: i = len(poles) - len(zeros_) # relative order if i < 1: return None, inf a = unstable_zeros[0] G = (z - a) / z**i F = _ctrl.minreal(tf / G, verbose=False) F_ss = _ctrl.tf2ss(F) B_F = matrix(F_ss.B) C_F = matrix(F_ss.C) E = (1 + abs(a)) * abs(C_F @ B_F)[0, 0] * q.delta else: return None, inf A_F = matrix(F_ss.A) D_F = matrix(F_ss.D) # check if (C_F @ B_F)[0, 0] == 0: if verbose: print("CF @ BF == 0 became true. Couldn't calculate by using serial system decomposition.") return None, inf if D_F[0, 0] != 0: if verbose: print("DF == 0 became true. Couldn't calculate by using serial system decomposition.") return None, inf Q = DynamicQuantizer( A=A_F, B=B_F, C=- 1 / (C_F @ B_F)[0, 0] * C_F @ A_F, q=q ) if verbose: print("Success!") return Q, E