def test_interconnect_docstring(): """Test the examples from the interconnect() docstring""" # MIMO interconnection (note: use [C, P] instead of [P, C] for state order) P = ct.LinearIOSystem( ct.rss(2, 2, 2, strictly_proper=True), name='P') C = ct.LinearIOSystem(ct.rss(2, 2, 2), name='C') T = ct.interconnect( [C, P], connections = [ ['P.u[0]', 'C.y[0]'], ['P.u[1]', 'C.y[1]'], ['C.u[0]', '-P.y[0]'], ['C.u[1]', '-P.y[1]']], inplist = ['C.u[0]', 'C.u[1]'], outlist = ['P.y[0]', 'P.y[1]'], ) T_ss = ct.feedback(P * C, ct.ss([], [], [], np.eye(2))) np.testing.assert_almost_equal(T.A, T_ss.A) np.testing.assert_almost_equal(T.B, T_ss.B) np.testing.assert_almost_equal(T.C, T_ss.C) np.testing.assert_almost_equal(T.D, T_ss.D) # Implicit interconnection (note: use [C, P, sumblk] for proper state order) P = ct.tf2io(ct.tf(1, [1, 0]), inputs='u', outputs='y') C = ct.tf2io(ct.tf(10, [1, 1]), inputs='e', outputs='u') sumblk = ct.summing_junction(inputs=['r', '-y'], output='e') T = ct.interconnect([C, P, sumblk], inplist='r', outlist='y') T_ss = ct.feedback(P * C, 1) np.testing.assert_almost_equal(T.A, T_ss.A) np.testing.assert_almost_equal(T.B, T_ss.B) np.testing.assert_almost_equal(T.C, T_ss.C) np.testing.assert_almost_equal(T.D, T_ss.D)
def test_interconnect_exceptions(): # First make sure the docstring example works P = ct.tf2io(ct.tf(1, [1, 0]), input='u', output='y') C = ct.tf2io(ct.tf(10, [1, 1]), input='e', output='u') sumblk = ct.summing_junction(inputs=['r', '-y'], output='e') T = ct.interconnect((P, C, sumblk), input='r', output='y') assert (T.ninputs, T.noutputs, T.nstates) == (1, 1, 2) # Unrecognized arguments # LinearIOSystem with pytest.raises(TypeError, match="unknown parameter"): P = ct.LinearIOSystem(ct.rss(2, 1, 1), output_name='y') # Interconnect with pytest.raises(TypeError, match="unknown parameter"): T = ct.interconnect((P, C, sumblk), input_name='r', output='y') # Interconnected system with pytest.raises(TypeError, match="unknown parameter"): T = ct.InterconnectedSystem((P, C, sumblk), input_name='r', output='y') # NonlinearIOSytem with pytest.raises(TypeError, match="unknown parameter"): nlios = ct.NonlinearIOSystem( None, lambda t, x, u, params: u*u, input_count=1, output_count=1) # Summing junction with pytest.raises(TypeError, match="input specification is required"): sumblk = ct.summing_junction() with pytest.raises(TypeError, match="unknown parameter"): sumblk = ct.summing_junction(input_count=2, output_count=2)
def closeLoop(tf,k_p,h_multiplier=1): H=control.TransferFunction([h_multiplier],[1]) H=control.tf2io(H) H.name='H' H.set_inputs(['in']) H.set_outputs(['out']) D=control.TransferFunction([k_p],[1]) D=control.tf2io(D) D.name='D' D.set_inputs(['in']) D.set_outputs(['out']) system=control.tf2io(tf) system.name='system' system.set_inputs(['in']) system.set_outputs(['out']) # + # in ---->o---->--D-->system----> out # |- | # -------H------------ system_MF = control.InterconnectedSystem([H,D,system] ,name='system_MF', connections=[ ['H.in','system.out'], ['D.in','-H.out'], ['system.in','D.out'], ], inplist=['D.in'], inputs=['in'], outlist=['system.out','D.out','-H.out'], outputs=['out','control','error']) return system_MF
def test_tf2io(self): # Create a transfer function from the state space system linsys = self.siso_linsys tfsys = ct.ss2tf(linsys) iosys = ct.tf2io(tfsys) # Verify correctness via simulation T, U, X0 = self.T, self.U, self.X0 lti_t, lti_y, lti_x = ct.forced_response(linsys, T, U, X0) ios_t, ios_y = ios.input_output_response(iosys, T, U, X0) np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
from matplotlib import pyplot as plt print("Reading Data") y = np.loadtxt("KHU_KongBot2/SRIVC/RefinedY.txt", delimiter=",") # linux u = np.loadtxt("KHU_KongBot2/SRIVC/RefinedU.txt", delimiter=",") # linux t = u[0:15000, 0] y = y[0:15000, 1] u = u[0:15000, 1] theta=np.loadtxt("KHU_KongBot2/SRIVC/PD_control_theta.txt") """ PD_Control 452.2 s + 5781 -------------------------------- s^3 + 38.85 s^2 + 851.1 s + 5774 """ n=3 m=1 A = theta[0:n] A = np.insert(A, 0, 1) B = theta[n:] sys=control.tf(B,A) print(sys) sys=control.tf2io(sys) y_m=control.input_output_response(sys,t,u) fig = plt.figure() plt.plot(t, u, color="b") plt.plot(t, y, color='r') plt.plot(y_m[0], y_m[1], color="g") plt.show()
def test_interconnect_implicit(): """Test the use of implicit connections in interconnect()""" import random # System definition P = ct.ss2io( ct.rss(2, 1, 1, strictly_proper=True), inputs='u', outputs='y', name='P') kp = ct.tf(random.uniform(1, 10), [1]) ki = ct.tf(random.uniform(1, 10), [1, 0]) C = ct.tf2io(kp + ki, inputs='e', outputs='u', name='C') # Block diagram computation Tss = ct.feedback(P * C, 1) # Construct the interconnection explicitly Tio_exp = ct.interconnect( (C, P), connections = [['P.u', 'C.u'], ['C.e', '-P.y']], inplist='C.e', outlist='P.y') # Compare to bdalg computation np.testing.assert_almost_equal(Tio_exp.A, Tss.A) np.testing.assert_almost_equal(Tio_exp.B, Tss.B) np.testing.assert_almost_equal(Tio_exp.C, Tss.C) np.testing.assert_almost_equal(Tio_exp.D, Tss.D) # Construct the interconnection via a summing junction sumblk = ct.summing_junction(inputs=['r', '-y'], output='e', name="sum") Tio_sum = ct.interconnect( (C, P, sumblk), inplist=['r'], outlist=['y']) np.testing.assert_almost_equal(Tio_sum.A, Tss.A) np.testing.assert_almost_equal(Tio_sum.B, Tss.B) np.testing.assert_almost_equal(Tio_sum.C, Tss.C) np.testing.assert_almost_equal(Tio_sum.D, Tss.D) # Setting connections to False should lead to an empty connection map empty = ct.interconnect( (C, P, sumblk), connections=False, inplist=['r'], outlist=['y']) np.testing.assert_array_equal(empty.connect_map, np.zeros((4, 3))) # Implicit summation across repeated signals kp_io = ct.tf2io(kp, inputs='e', outputs='u', name='kp') ki_io = ct.tf2io(ki, inputs='e', outputs='u', name='ki') Tio_sum = ct.interconnect( (kp_io, ki_io, P, sumblk), inplist=['r'], outlist=['y']) np.testing.assert_almost_equal(Tio_sum.A, Tss.A) np.testing.assert_almost_equal(Tio_sum.B, Tss.B) np.testing.assert_almost_equal(Tio_sum.C, Tss.C) np.testing.assert_almost_equal(Tio_sum.D, Tss.D) # TODO: interconnect a MIMO system using implicit connections # P = control.ss2io( # control.rss(2, 2, 2, strictly_proper=True), # input_prefix='u', output_prefix='y', name='P') # C = control.ss2io( # control.rss(2, 2, 2), # input_prefix='e', output_prefix='u', name='C') # sumblk = control.summing_junction( # inputs=['r', '-y'], output='e', dimension=2) # S = control.interconnect([P, C, sumblk], inplist='r', outlist='y') # Make sure that repeated inplist/outlist names generate an error # Input not unique Cbad = ct.tf2io(ct.tf(10, [1, 1]), inputs='r', outputs='x', name='C') with pytest.raises(ValueError, match="not unique"): Tio_sum = ct.interconnect( (Cbad, P, sumblk), inplist=['r'], outlist=['y']) # Output not unique Cbad = ct.tf2io(ct.tf(10, [1, 1]), inputs='e', outputs='y', name='C') with pytest.raises(ValueError, match="not unique"): Tio_sum = ct.interconnect( (Cbad, P, sumblk), inplist=['r'], outlist=['y']) # Signal not found with pytest.raises(ValueError, match="could not find"): Tio_sum = ct.interconnect( (C, P, sumblk), inplist=['x'], outlist=['y']) with pytest.raises(ValueError, match="could not find"): Tio_sum = ct.interconnect( (C, P, sumblk), inplist=['r'], outlist=['x'])
# PD CONTROLLER #n = 3 #m = 1 # Pi CONTROLLER n = 4 m = 1 result = Queue() phi = np.zeros((n + m + 1, len(u))) phi_hat = np.zeros((n + m + 1, len(u))) A = np.array([1, 10, 100, 100, 10000]) # INITIAL ESTIMATE for i in range(n): p_i = np.zeros(n) p_i[i] = 1 F = control.tf(p_i, A) # P^n/A(p) F = control.tf2io(F) Yf = control.input_output_response(F, t, y) phi[i] = -Yf[1].T for i in range(m + 1): p_i = np.zeros(m + 1) p_i[i] = 1 Uf = control.tf(p_i, A) # P^m/A(p) Uf = control.tf2io(Uf) Uf = control.input_output_response(Uf, t, u) phi[n + i] = Uf[1].T Pn = np.zeros(n + 1) Pn[0] = 1 yfn = control.tf(Pn, A) yfn = control.tf2io(yfn) yfn = control.input_output_response(yfn, t, y)
# Define the input/output system for the vehicle vehicle = ct.NonlinearIOSystem( vehicle_update, None, name='vehicle', inputs=('u', 'gear', 'theta'), outputs=('v'), states=('v')) # Figure 1.11: A feedback system for controlling the speed of a vehicle. In # this example, the speed of the vehicle is measured and compared to the # desired speed. The controller is a PI controller represented as a transfer # function. In the textbook, the simulations are done for LTI systems, but # here we simulate the full nonlinear system. # Construct a PI controller with rolloff, as a transfer function Kp = 0.5 # proportional gain Ki = 0.1 # integral gain control_tf = ct.tf2io( ct.TransferFunction([Kp, Ki], [1, 0.01*Ki/Kp]), name='control', inputs='u', outputs='y') # Construct the closed loop control system # Inputs: vref, gear, theta # Outputs: v (vehicle velocity) cruise_tf = ct.InterconnectedSystem( (control_tf, vehicle), name='cruise', connections=( ['control.u', '-vehicle.v'], ['vehicle.u', 'control.y']), inplist=('control.u', 'vehicle.gear', 'vehicle.theta'), inputs=('vref', 'gear', 'theta'), outlist=('vehicle.v', 'vehicle.u'), outputs=('v', 'u'))