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'))
