def testSisoW123(self):
"""SISO plant with all weights"""
g = ss([-1.], [1.], [1.], [1.])
w1 = ss([-2.], [2.], [1.], [2.])
w2 = ss([-3.], [3.], [1.], [3.])
w3 = ss([-4.], [4.], [1.], [4.])
p = augw(g, w1, w2, w3)
assert p.noutputs == 4
assert p.ninputs == 2
# w->z1 should be w1
self.siso_almost_equal(w1, p[0, 0])
# w->z2 should be 0
self.siso_almost_equal(0, p[1, 0])
# w->z3 should be 0
self.siso_almost_equal(0, p[2, 0])
# w->v should be 1
self.siso_almost_equal(ss([], [], [], [1]), p[3, 0])
# u->z1 should be -w1*g
self.siso_almost_equal(-w1 * g, p[0, 1])
# u->z2 should be w2
self.siso_almost_equal(w2, p[1, 1])
# u->z3 should be w3*g
self.siso_almost_equal(w3 * g, p[2, 1])
# u->v should be -g
self.siso_almost_equal(-g, p[3, 1])
def test_matplotlib_kwargs():
# Create a SISO system for use in parameterized tests
sys = control.ss([[-1, 1], [0, -1]], [[0], [1]], [[1, 0]], 0, dt=None)
ctl = control.ss([[-1, 1], [0, -1]], [[0], [1]], [[1, 0]], 0, dt=None)
table = [
[control.bode, (sys, ), {}],
[control.bode_plot, (sys, ), {}],
[
control.describing_function_plot,
(sys, control.descfcn.saturation_nonlinearity(1), [1, 2, 3, 4]),
{}
],
[control.gangof4, (sys, ctl), {}],
[control.gangof4_plot, (sys, ctl), {}],
[control.nyquist, (sys, ), {}],
[control.nyquist_plot, (sys, ), {}],
[control.singular_values_plot, (sys, ), {}],
]
for function, args, kwargs in table:
# Call the function normally and make sure it works
function(*args, **kwargs)
# Now add an unrecognized keyword and make sure there is an error
with pytest.raises(AttributeError, match="has no property"):
function(*args, **kwargs, unknown=None)
# If we opened any figures, close them to avoid matplotlib warnings
if plt.gca():
plt.close('all')
def updateModel(self, l):
g = 9.81
self.j_p = self.M * l**2 / 3
a1 = np.zeros((4, 4))
b1 = np.zeros((4, 1))
a2 = np.array([[0, 1], [0, 0]])
b2 = np.zeros((2, 1))
x = 2 * self.j_p * self.j_w + 2 * self.M * l**2 * self.j_w + self.M * self.r**2 * self.j_p + \
2 * self.m * self.r**2 * self.j_p + 2 * self.m * self.M * (l * self.r)**2
a1[0, 1] = 1
a1[2, 3] = 1
a1[1, 2] = -g * (l * self.M * self.r)**2 / x
a1[3, 2] = l * self.M * g * (2 * self.j_w + self.M * self.r**2 +
2 * self.m * self.r**2) / x
b1[1,
0] = self.r * (self.M * l**2 + self.M * l * self.r + self.j_p) / x
b1[3, 0] = -(2 * self.j_w + self.M * self.r**2 +
2 * self.m * self.r**2 + l * self.M * self.r) / x
b2[1, 0] = self.d * self.r / (
(self.m * self.r**2 + self.j_w) * self.d**2 +
2 * self.j_d * self.r**2)
self.sys1 = ss(a1, b1, np.eye(4), np.zeros((4, 1)))
self.sys2 = ss(a2, b2, np.eye(2), np.zeros((2, 1)))
rospy.loginfo("State Space Model Updated.")
# print(self.sys1)
# print("----------------")
# print(self.sys2)
return True
def testMimoW1(self):
"""MIMO plant with S weighting"""
from control import augw, ss
g = ss([[-1., -2], [-3, -4]], [[1., 0.], [0., 1.]],
[[1., 0.], [0., 1.]], [[1., 0.], [0., 1.]])
w1 = ss([-2], [2.], [1.], [2.])
p = augw(g, w1)
self.assertEqual(4, p.outputs)
self.assertEqual(4, p.inputs)
# w->z1 should be diag(w1,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->v should be I
self.siso_almost_equal(1, p[2, 0])
self.siso_almost_equal(0, p[2, 1])
self.siso_almost_equal(0, p[3, 0])
self.siso_almost_equal(1, p[3, 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->v should be -g
self.siso_almost_equal(-g[0, 0], p[2, 2])
self.siso_almost_equal(-g[0, 1], p[2, 3])
self.siso_almost_equal(-g[1, 0], p[3, 2])
self.siso_almost_equal(-g[1, 1], p[3, 3])
def test_legacy_defaults(self):
with pytest.deprecated_call():
ct.use_legacy_defaults('0.8.3')
assert (isinstance(ct.ss(0, 0, 0, 1).D, np.matrix))
ct.reset_defaults()
assert isinstance(ct.ss(0, 0, 0, 1).D, np.ndarray)
assert not isinstance(ct.ss(0, 0, 0, 1).D, np.matrix)
ct.use_legacy_defaults('0.8.4')
assert ct.config.defaults['forced_response.return_x'] is True
ct.use_legacy_defaults('0.9.0')
assert isinstance(ct.ss(0, 0, 0, 1).D, np.ndarray)
assert not isinstance(ct.ss(0, 0, 0, 1).D, np.matrix)
# test that old versions don't raise a problem
ct.use_legacy_defaults('REL-0.1')
ct.use_legacy_defaults('control-0.3a')
ct.use_legacy_defaults('0.6c')
ct.use_legacy_defaults('0.8.2')
ct.use_legacy_defaults('0.1')
# Make sure that nonsense versions generate an error
with pytest.raises(ValueError):
ct.use_legacy_defaults("a.b.c")
with pytest.raises(ValueError):
ct.use_legacy_defaults("1.x.3")
def testSisoW123(self):
"SISO plant with all weights"
from control import augw, ss
g = ss([-1.],[1.],[1.],[1.])
w1 = ss([-2.],[2.],[1.],[2.])
w2 = ss([-3.],[3.],[1.],[3.])
w3 = ss([-4.],[4.],[1.],[4.])
p = augw(g,w1,w2,w3)
self.assertEqual(4,p.outputs)
self.assertEqual(2,p.inputs)
# w->z1 should be w1
self.siso_almost_equal(w1,p[0,0])
# w->z2 should be 0
self.siso_almost_equal(0,p[1,0])
# w->z3 should be 0
self.siso_almost_equal(0,p[2,0])
# w->v should be 1
self.siso_almost_equal(ss([],[],[],[1]),p[3,0])
# u->z1 should be -w1*g
self.siso_almost_equal(-w1*g,p[0,1])
# u->z2 should be w2
self.siso_almost_equal(w2,p[1,1])
# u->z3 should be w3*g
self.siso_almost_equal(w3*g,p[2,1])
# u->v should be -g
self.siso_almost_equal(-g,p[3,1])
def set_aw(sys,poles):
"""
Divide in controller in input and feedback part
for anti-windup
Usage
=====
[sys_in,sys_fbk] = set_aw(sys,poles)
Inputs
------
sys: controller
poles : poles for the anti-windup filter
Outputs
-------
sys_in, sys_fbk: controller in input and feedback part
"""
sys = ct.ss(sys)
Ts = sys.dt
den_old = sp.poly(la.eigvals(sys.A))
sys = ct.tf(sys)
den = sp.poly(poles)
tmp = ct.tf(den_old,den,sys.dt)
sys_in = tmp*sys
sys_in = sys_in.minreal()
sys_in = ct.ss(sys_in)
sys_fbk = 1-tmp
sys_fbk = sys_fbk.minreal()
sys_fbk = ct.ss(sys_fbk)
return sys_in, sys_fbk
def testMimoW3(self):
"MIMO plant with T weighting"
from control import augw, ss
g = ss([[-1.,-2],[-3,-4]],
[[1.,0.],[0.,1.]],
[[1.,0.],[0.,1.]],
[[1.,0.],[0.,1.]])
w3 = ss([-2],[2.],[1.],[2.])
p = augw(g,w3=w3)
self.assertEqual(4,p.outputs)
self.assertEqual(4,p.inputs)
# w->z3 should be 0
self.siso_almost_equal(0, p[0,0])
self.siso_almost_equal(0, p[0,1])
self.siso_almost_equal(0, p[1,0])
self.siso_almost_equal(0, p[1,1])
# w->v should be I
self.siso_almost_equal(1, p[2,0])
self.siso_almost_equal(0, p[2,1])
self.siso_almost_equal(0, p[3,0])
self.siso_almost_equal(1, p[3,1])
# u->z3 should be w3*g
self.siso_almost_equal(w3*g[0,0], p[0,2])
self.siso_almost_equal(w3*g[0,1], p[0,3])
self.siso_almost_equal(w3*g[1,0], p[1,2])
self.siso_almost_equal(w3*g[1,1], p[1,3])
# # u->v should be -g
self.siso_almost_equal(-g[0,0], p[2,2])
self.siso_almost_equal(-g[0,1], p[2,3])
self.siso_almost_equal(-g[1,0], p[3,2])
self.siso_almost_equal(-g[1,1], p[3,3])
def testMimoW2(self):
"""MIMO plant with KS weighting"""
g = ss([[-1., -2], [-3, -4]], [[1., 0.], [0., 1.]],
[[1., 0.], [0., 1.]], [[1., 0.], [0., 1.]])
w2 = ss([-2], [2.], [1.], [2.])
p = augw(g, w2=w2)
assert p.outputs == 4
assert p.inputs == 4
# w->z2 should be 0
self.siso_almost_equal(0, p[0, 0])
self.siso_almost_equal(0, p[0, 1])
self.siso_almost_equal(0, p[1, 0])
self.siso_almost_equal(0, p[1, 1])
# w->v should be I
self.siso_almost_equal(1, p[2, 0])
self.siso_almost_equal(0, p[2, 1])
self.siso_almost_equal(0, p[3, 0])
self.siso_almost_equal(1, p[3, 1])
# u->z2 should be w2
self.siso_almost_equal(w2, p[0, 2])
self.siso_almost_equal(0, p[0, 3])
self.siso_almost_equal(0, p[1, 2])
self.siso_almost_equal(w2, p[1, 3])
# # u->v should be -g
self.siso_almost_equal(-g[0, 0], p[2, 2])
self.siso_almost_equal(-g[0, 1], p[2, 3])
self.siso_almost_equal(-g[1, 0], p[3, 2])
self.siso_almost_equal(-g[1, 1], p[3, 3])
def testMimoW3(self):
"""MIMO plant with T weighting"""
g = ss([[-1., -2], [-3, -4]], [[1., 0.], [0., 1.]],
[[1., 0.], [0., 1.]], [[1., 0.], [0., 1.]])
w3 = ss([-2], [2.], [1.], [2.])
p = augw(g, w3=w3)
assert p.noutputs == 4
assert p.ninputs == 4
# w->z3 should be 0
self.siso_almost_equal(0, p[0, 0])
self.siso_almost_equal(0, p[0, 1])
self.siso_almost_equal(0, p[1, 0])
self.siso_almost_equal(0, p[1, 1])
# w->v should be I
self.siso_almost_equal(1, p[2, 0])
self.siso_almost_equal(0, p[2, 1])
self.siso_almost_equal(0, p[3, 0])
self.siso_almost_equal(1, p[3, 1])
# u->z3 should be w3*g
self.siso_almost_equal(w3 * g[0, 0], p[0, 2])
self.siso_almost_equal(w3 * g[0, 1], p[0, 3])
self.siso_almost_equal(w3 * g[1, 0], p[1, 2])
self.siso_almost_equal(w3 * g[1, 1], p[1, 3])
# # u->v should be -g
self.siso_almost_equal(-g[0, 0], p[2, 2])
self.siso_almost_equal(-g[0, 1], p[2, 3])
self.siso_almost_equal(-g[1, 0], p[3, 2])
self.siso_almost_equal(-g[1, 1], p[3, 3])
def test_discrete_lqr():
# oscillator model defined in 2D
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.5403, -0.8415], [0.8415, 0.5403]]
B = [[-0.4597], [0.8415]]
C = [[1, 0]]
D = [[0]]
# Linear discrete-time model with sample time 1
sys = ct.ss2io(ct.ss(A, B, C, D, 1))
# Include weights on states/inputs
Q = np.eye(2)
R = 1
K, S, E = ct.dlqr(A, B, Q, R)
# Compute the integral and terminal cost
integral_cost = opt.quadratic_cost(sys, Q, R)
terminal_cost = opt.quadratic_cost(sys, S, None)
# Solve the LQR problem
lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1))
# Generate a simulation of the LQR controller
time = np.arange(0, 5, 1)
x0 = np.array([1, 1])
_, _, lqr_x = ct.input_output_response(lqr_sys, time, 0, x0, return_x=True)
# Use LQR input as initial guess to avoid convergence/precision issues
lqr_u = np.array(-K @ lqr_x[0:time.size]) # convert from matrix
# Formulate the optimal control problem and compute optimal trajectory
optctrl = opt.OptimalControlProblem(sys,
time,
integral_cost,
terminal_cost=terminal_cost,
initial_guess=lqr_u)
res1 = optctrl.compute_trajectory(x0, return_states=True)
# Compare to make sure results are the same
np.testing.assert_almost_equal(res1.inputs, lqr_u[0])
np.testing.assert_almost_equal(res1.states, lqr_x)
# Add state and input constraints
trajectory_constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -.5], [5, 5, 0.5]),
]
# Re-solve
res2 = opt.solve_ocp(sys,
time,
x0,
integral_cost,
trajectory_constraints,
terminal_cost=terminal_cost,
initial_guess=lqr_u)
# Make sure we got a different solution
assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def test_change_default_dt(self):
ct.set_defaults('statesp', default_dt=0)
self.assertEqual(ct.ss(0, 0, 0, 1).dt, 0)
ct.set_defaults('statesp', default_dt=None)
self.assertEqual(ct.ss(0, 0, 0, 1).dt, None)
ct.set_defaults('xferfcn', default_dt=0)
self.assertEqual(ct.tf(1, 1).dt, 0)
ct.set_defaults('xferfcn', default_dt=None)
self.assertEqual(ct.tf(1, 1).dt, None)
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 main():
m = 1500 # Mass. Gives it a bit delay in the beginning.
k = 450 # Static gain. Tune so end values are similar to experimental data.
c = 240 # Time constant. Higher is slower. Damping.
# Transfer function. Static gain is numerator. Denominator is c * s + 1. dt = 1.
sys = control.tf([k], [m, c, 1])
sys = control.ss(sys)
res = scipy.linalg.expm(np.array([[sys.A[0, 0], sys.A[0, 1], sys.B[0]],
[sys.A[1, 0], sys.A[1, 1], sys.B[1]],
[0, 0, 0]]))
A = [[res[0, 0], res[0, 1]],
[res[1, 0], res[1, 1]]]
B = [[res[0, 2]], [res[1, 2]]]
sys_d = control.ss(A, B, sys.C, sys.D, True)
Kp = 0.1
Ki = 0
Kd = 4
pid = PID(Kp, Ki, Kd, u_max=1, u_min=0)
timestep_control = 2
ts = 1
r = np.concatenate((np.arange(50, 150, 50 / (120 // ts)),
np.arange(150, 180, 30 / (80 // ts)),
np.arange(180, 240, 60 / (60 // ts)),
np.arange(240, 0, -240 / (60 // ts)),
))
n = len(r)
T = np.arange(0, n, ts)
# r = np.full(n, fill_value=150) # input vector. Step response so single value.
y = np.zeros(n)
u = np.zeros(n)
x = np.matrix([[0], [0]])
y_prev = 0
time_since_control_update = 9000
for i, t in enumerate(T):
time_since_control_update += t
if time_since_control_update >= timestep_control:
pid.setpoint = r[i]
u[i] = pid.get_control_input(y_prev)
time_since_control_update = 0
else:
u[i] = u[i - 1]
x = np.add(np.matmul(sys_d.A, x), sys_d.B * u[i])
y[i] = np.add(np.matmul(sys_d.C, x), sys_d.D * u[i])
y_prev = y[i]
fig, axs = plt.subplots(2, 1, sharex=True, tight_layout=True)
axs[0].plot(T, y, label='y')
axs[0].plot(T, r, label='r')
axs[0].legend()
axs[1].plot(T, u)
def test_legacy_defaults(self):
ct.use_legacy_defaults('0.8.3')
assert(isinstance(ct.ss(0,0,0,1).D, np.matrix))
ct.reset_defaults()
assert(isinstance(ct.ss(0,0,0,1).D, np.ndarray))
# test that old versions don't raise a problem
ct.use_legacy_defaults('0.6c')
ct.use_legacy_defaults('0.8.2')
ct.use_legacy_defaults('0.1')
ct.config.reset_defaults()
def buildStateSpace(C1,C2,C3,subscript='S'):
A,B = ssMatrix(C1,C2,C3)
C = np.matrix([[1.,0.,0.,0.],[0.,1.,0.,0.],\
[0.,0.,1.,0.],[0.,0.,0.,1.]])
if subscript == 'S':
D = np.matrix([[0.],[0.],[0.],[0.]])
return c.ss(A,B,C,D)
elif subscript == 'A':
D = np.matrix([[0.,0.],[0.,0.],[0.,0.],[0.,0.]])
return c.ss(A,B,C,D)
def test_change_default_dt_static(self):
"""Test that static gain systems always have dt=None"""
ct.set_defaults('control', default_dt=0)
assert ct.tf(1, 1).dt is None
assert ct.ss([], [], [], 1).dt is None
# Make sure static gain is preserved for the I/O system
sys = ct.ss([], [], [], 1)
sys_io = ct.ss2io(sys)
assert sys_io.dt is None
def test_discrete_lqr():
# oscillator model defined in 2D
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.5403, -0.8415], [0.8415, 0.5403]]
B = [[-0.4597], [0.8415]]
C = [[1, 0]]
D = [[0]]
# Linear discrete-time model with sample time 1
sys = ct.ss2io(ct.ss(A, B, C, D, 1))
# Include weights on states/inputs
Q = np.eye(2)
R = 1
K, S, E = ct.lqr(A, B, Q, R) # note: *continuous* time LQR
# Compute the integral and terminal cost
integral_cost = opt.quadratic_cost(sys, Q, R)
terminal_cost = opt.quadratic_cost(sys, S, None)
# Formulate finite horizon MPC problem
time = np.arange(0, 5, 1)
x0 = np.array([1, 1])
optctrl = opt.OptimalControlProblem(sys,
time,
integral_cost,
terminal_cost=terminal_cost)
res1 = optctrl.compute_trajectory(x0, return_states=True)
with pytest.xfail("discrete LQR not implemented"):
# Result should match LQR
K, S, E = ct.dlqr(A, B, Q, R)
lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1))
_, _, lqr_x = ct.input_output_response(lqr_sys,
time,
0,
x0,
return_x=True)
np.testing.assert_almost_equal(res1.states, lqr_x)
# Add state and input constraints
trajectory_constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-10, -10, -1], [10, 10, 1]),
]
# Re-solve
res2 = opt.solve_ocp(sys,
time,
x0,
integral_cost,
constraints,
terminal_cost=terminal_cost)
# Make sure we got a different solution
assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def testErrors(self):
"Error cases handled"
from control import augw,ss
# no weights
g1by1 = ss(-1,1,1,0)
g2by2 = ss(-np.eye(2),np.eye(2),np.eye(2),np.zeros((2,2)))
self.assertRaises(ValueError,augw,g1by1)
# mismatched size of weight and plant
self.assertRaises(ValueError,augw,g1by1,w1=g2by2)
self.assertRaises(ValueError,augw,g1by1,w2=g2by2)
self.assertRaises(ValueError,augw,g1by1,w3=g2by2)
def testMimoSeries(self, tsys):
"""regression: bdalg.series reverses order of arguments"""
g1 = ctrl.ss([], [], [], [[1, 2], [0, 3]])
g2 = ctrl.ss([], [], [], [[1, 0], [2, 3]])
ref = g2 * g1
tst = ctrl.series(g1, g2)
np.testing.assert_array_equal(ref.A, tst.A)
np.testing.assert_array_equal(ref.B, tst.B)
np.testing.assert_array_equal(ref.C, tst.C)
np.testing.assert_array_equal(ref.D, tst.D)
def sys_dict():
sdict = {}
sdict['ss'] = ct.ss([[-1]], [[1]], [[1]], [[0]])
sdict['tf'] = ct.tf([1], [0.5, 1])
sdict['tfx'] = ct.tf([1, 1], [1]) # non-proper transfer function
sdict['frd'] = ct.frd([10 + 0j, 9 + 1j, 8 + 2j], [1, 2, 3])
sdict['lio'] = ct.LinearIOSystem(ct.ss([[-1]], [[5]], [[5]], [[0]]))
sdict['ios'] = ct.NonlinearIOSystem(sdict['lio']._rhs, sdict['lio']._out,
1, 1, 1)
sdict['arr'] = np.array([[2.0]])
sdict['flt'] = 3.
return sdict
def loadBlocks(self, s):
for i in Workspace.blocks:
if i.type == 'input':
i.input = np.zeros(len(s['t']))
s['inputs'][i.id] = i
elif i.type == "function":
i.y = np.zeros(len(s['t']))
i.y.fill(np.nan)
s['functions'][i.id] = i
elif i.type == "sum":
i.y = np.zeros(len(s['t']))
i.y.fill(np.nan)
s['sums'][i.id] = i
elif i.type == 'system':
if i.code['type'] == "TF":
if i.code['sub_type'] == "continuous":
i.ss = c.ssdata(
c.c2d(
c.ss(
c.tf(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]))),
s['T']))
else:
i.ss = c.ssdata(
c.ss(
c.tf(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]), s['T'])))
elif i.code['type'] == "SS":
if i.code['sub_type'] == "continuous":
i.ss = c.ssdata(
c.c2d(
c.ss(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]),
json.loads(i.code['self'][2]),
json.loads(i.code['self'][3])), s['T']))
else:
i.ss = c.ssdata(
c.ss(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]),
json.loads(i.code['self'][2]),
json.loads(i.code['self'][3]), s['T']))
else:
Dialog.alert(
"Alerta",
["Um dos blocos não está configurado como TF ou SS"])
lt = len(s['t'])
lA = len(i.ss[0])
i.x = np.zeros((lt, lA, 1))
i.y = np.zeros(lt)
i.y.fill(np.nan)
s['systems'][i.id] = i
return s
def test_mpc_iosystem():
# model of an aircraft discretized with 0.2s sampling time
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.99, 0.01, 0.18, -0.09, 0],
[ 0, 0.94, 0, 0.29, 0],
[ 0, 0.14, 0.81, -0.9, 0],
[ 0, -0.2, 0, 0.95, 0],
[ 0, 0.09, 0, 0, 0.9]]
B = [[ 0.01, -0.02],
[-0.14, 0],
[ 0.05, -0.2],
[ 0.02, 0],
[-0.01, 0]]
C = [[0, 1, 0, 0, -1],
[0, 0, 1, 0, 0],
[0, 0, 0, 1, 0],
[1, 0, 0, 0, 0]]
model = ct.ss2io(ct.ss(A, B, C, 0, 0.2))
# For the simulation we need the full state output
sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2))
# compute the steady state values for a particular value of the input
ud = np.array([0.8, -0.3])
xd = np.linalg.inv(np.eye(5) - A) @ B @ ud
yd = C @ xd
# provide constraints on the system signals
constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])]
# provide penalties on the system signals
Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C
R = np.diag([3, 2])
cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud)
# online MPC controller object is constructed with a horizon 6
ctrl = opt.create_mpc_iosystem(
model, np.arange(0, 6) * 0.2, cost, constraints)
# Define an I/O system implementing model predictive control
loop = ct.feedback(sys, ctrl, 1)
# Choose a nearby initial condition to speed up computation
X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99
Nsim = 12
tout, xout = ct.input_output_response(
loop, np.arange(0, Nsim) * 0.2, 0, X0)
# Make sure the system converged to the desired state
np.testing.assert_allclose(
xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01)
def init_ss(self):
A = -np.linalg.solve(self.C, self.K)
B = np.array([np.linalg.solve(self.C, self.f / q)]).T
C1 = np.zeros(self.X.size)
C2 = np.zeros(self.X.size)
C1[np.where(self.X == xo1)[0][0]] = 1
C2[np.where(self.X == xo2)[0][0]] = 1
D = 0
self.ss_pot1, self.ss_pot2 = control.ss(A, B, C1,
D), control.ss(A, B, C2, D)
Bvel = np.array([np.linalg.solve(self.C, np.gradient(self.T0))
]).T * cp * rho
self.ss_vel1, self.ss_vel2 = control.ss(A, Bvel, C1,
D), control.ss(A, Bvel, C2, D)
def init_ss(self):
A = -np.linalg.solve(self.C, self.K)
B = np.array([np.linalg.solve(self.C, self.f / q)]).T
C1 = np.zeros(self.X.size)
C2 = np.zeros(self.X.size)
no1, no2 = self.no1, self.no2
C1[int(no1[0, 0])], C1[int(no1[1, 0])] = no1[0, 1], no1[1, 1]
C2[int(no2[0, 0])], C2[int(no2[1, 0])] = no2[0, 1], no2[1, 1]
D = 0
self.ss_pot1, self.ss_pot2 = control.ss(A, B, C1,
D), control.ss(A, B, C2, D)
Bvel = np.array([np.linalg.solve(self.C, -np.gradient(self.T0))
]).T * cp * rho
self.ss_vel1, self.ss_vel2 = control.ss(A, Bvel, C1,
D), control.ss(A, Bvel, C2, D)
def synth(wb1, wb2):
"""synth(wb1,wb2) -> k,gamma
wb1: S weighting frequency
wb2: KS weighting frequency
k: controller
gamma: H-infinity norm of 'design', that is, of evaluation system
with loop closed through design
"""
g = plant()
wu = ss([], [], [], np.eye(2))
wp1 = ss(weighting(wb=wb1, m=1.5, a=1e-4))
wp2 = ss(weighting(wb=wb2, m=1.5, a=1e-4))
wp = wp1.append(wp2)
k, _, info = mixsyn(g, wp, wu)
return k, info[0]
def testSisoW3(self):
"""SISO plant with T weighting"""
g = ss([-1.], [1.], [1.], [1.])
w3 = ss([-2], [1.], [1.], [2.])
p = augw(g, w3=w3)
assert p.noutputs == 2
assert p.ninputs == 2
# w->z3 should be 0
self.siso_almost_equal(ss([], [], [], 0), p[0, 0])
# w->v should be 1
self.siso_almost_equal(ss([], [], [], [1]), p[1, 0])
# u->z3 should be w3*g
self.siso_almost_equal(w3 * g, p[0, 1])
# u->v should be -g
self.siso_almost_equal(-g, p[1, 1])
def synth(wb1, wb2):
"""synth(wb1,wb2) -> k,gamma
wb1: S weighting frequency
wb2: KS weighting frequency
k: controller
gamma: H-infinity norm of 'design', that is, of evaluation system
with loop closed through design
"""
g = plant()
wu = ss([], [], [], np.eye(2))
wp1 = ss(weighting(wb=wb1, m=1.5, a=1e-4))
wp2 = ss(weighting(wb=wb2, m=1.5, a=1e-4))
wp = wp1.append(wp2)
k, _, info = mixsyn(g, wp, wu)
return k, info[0]
def testSisoW2(self):
"""SISO plant with KS weighting"""
g = ss([-1.], [1.], [1.], [1.])
w2 = ss([-2], [1.], [1.], [2.])
p = augw(g, w2=w2)
assert p.noutputs == 2
assert p.ninputs == 2
# w->z2 should be 0
self.siso_almost_equal(ss([], [], [], 0), p[0, 0])
# w->v should be 1
self.siso_almost_equal(ss([], [], [], [1]), p[1, 0])
# u->z2 should be w2
self.siso_almost_equal(w2, p[0, 1])
# u->v should be -g
self.siso_almost_equal(-g, p[1, 1])
def testSisoW1(self):
"""SISO plant with S weighting"""
g = ss([-1.], [1.], [1.], [1.])
w1 = ss([-2], [2.], [1.], [2.])
p = augw(g, w1)
assert p.noutputs == 2
assert p.ninputs == 2
# w->z1 should be w1
self.siso_almost_equal(w1, p[0, 0])
# w->v should be 1
self.siso_almost_equal(ss([], [], [], [1]), p[1, 0])
# u->z1 should be -w1*g
self.siso_almost_equal(-w1 * g, p[0, 1])
# u->v should be -g
self.siso_almost_equal(-g, p[1, 1])
def testErrors(self):
"""Error cases handled"""
from control import augw, ss
# no weights
g1by1 = ss(-1, 1, 1, 0)
g2by2 = ss(-np.eye(2), np.eye(2), np.eye(2), np.zeros((2, 2)))
with pytest.raises(ValueError):
augw(g1by1)
# mismatched size of weight and plant
with pytest.raises(ValueError):
augw(g1by1, w1=g2by2)
with pytest.raises(ValueError):
augw(g1by1, w2=g2by2)
with pytest.raises(ValueError):
augw(g1by1, w3=g2by2)
def testMimoSeries(self):
"""regression: bdalg.series reverses order of arguments"""
g1 = ctrl.ss([],[],[],[[1,2],[0,3]])
g2 = ctrl.ss([],[],[],[[1,0],[2,3]])
ref = g2*g1
tst = ctrl.series(g1,g2)
# assert_array_equal on mismatched matrices gives
# "repr failed for <matrix>: ..."
def assert_equal(x,y):
np.testing.assert_array_equal(np.asarray(x),
np.asarray(y))
assert_equal(ref.A, tst.A)
assert_equal(ref.B, tst.B)
assert_equal(ref.C, tst.C)
assert_equal(ref.D, tst.D)
def test_modal_form(self):
"""Test the modal canonical form"""
# Create a system in the modal canonical form
A_true = np.diag([4.0, 3.0, 2.0, 1.0]) # order from the largest to the smallest
B_true = np.matrix("1.1 2.2 3.3 4.4").T
C_true = np.matrix("1.3 1.4 1.5 1.6")
D_true = 42.0
# Perform a coordinate transform with a random invertible matrix
T_true = np.matrix([[-0.27144004, -0.39933167, 0.75634684, 0.44135471],
[-0.74855725, -0.39136285, -0.18142339, -0.50356997],
[-0.40688007, 0.81416369, 0.38002113, -0.16483334],
[-0.44769516, 0.15654653, -0.50060858, 0.72419146]])
A = np.linalg.solve(T_true, A_true)*T_true
B = np.linalg.solve(T_true, B_true)
C = C_true*T_true
D = D_true
# Create a state space system and convert it to the modal canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), "modal")
# Check against the true values
#TODO: Test in respect to ambiguous transformation (system characteristics?)
np.testing.assert_array_almost_equal(sys_check.A, A_true)
#np.testing.assert_array_almost_equal(sys_check.B, B_true)
#np.testing.assert_array_almost_equal(sys_check.C, C_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
def design():
"""Show results of designs"""
# equal weighting on each output
k1, gam1 = synth(0.25, 0.25)
# increase "bandwidth" of output 2 by moving crossover weighting frequency 100 times higher
k2, gam2 = synth(0.25, 25)
# now weight output 1 more heavily
# won't plot this one, just want gamma
_, gam3 = synth(25, 0.25)
print('design 1 gamma {:.3g} (Skogestad: 2.80)'.format(gam1))
print('design 2 gamma {:.3g} (Skogestad: 2.92)'.format(gam2))
print('design 3 gamma {:.3g} (Skogestad: 6.73)'.format(gam3))
# do the designs
g = plant()
w = np.logspace(-2, 2, 101)
I = ss([], [], [], np.eye(2))
s1 = I.feedback(g * k1)
s2 = I.feedback(g * k2)
# frequency response
sv1 = triv_sigma(s1, w)
sv2 = triv_sigma(s2, w)
plt.figure(2)
plt.subplot(1, 2, 1)
plt.semilogx(w, 20 * np.log10(sv1[:, 0]), label=r'$\sigma_1(S_1)$')
plt.semilogx(w, 20 * np.log10(sv1[:, 1]), label=r'$\sigma_2(S_1)$')
plt.semilogx(w, 20 * np.log10(sv2[:, 0]), label=r'$\sigma_1(S_2)$')
plt.semilogx(w, 20 * np.log10(sv2[:, 1]), label=r'$\sigma_2(S_2)$')
plt.ylim([-60, 10])
plt.ylabel('magnitude [dB]')
plt.xlim([1e-2, 1e2])
plt.xlabel('freq [rad/s]')
plt.legend()
plt.title('Singular values of S')
# time response
# in design 1, both outputs have an inverse initial response; in
# design 2, output 2 does not, and is very fast, while output 1
# has a larger initial inverse response than in design 1
time = np.linspace(0, 10, 301)
t1 = (g * k1).feedback(I)
t2 = (g * k2).feedback(I)
y1 = step_opposite(t1, time)
y2 = step_opposite(t2, time)
plt.subplot(1, 2, 2)
plt.plot(time, y1[0], label='des. 1 $y_1(t))$')
plt.plot(time, y1[1], label='des. 1 $y_2(t))$')
plt.plot(time, y2[0], label='des. 2 $y_1(t))$')
plt.plot(time, y2[1], label='des. 2 $y_2(t))$')
plt.xlabel('time [s]')
plt.ylabel('response [1]')
plt.legend()
plt.title('c/l response to reference [1,-1]')
def test_observable_form(self):
"""Test the observable canonical form"""
# Create a system in the observable canonical form
coeffs = [1.0, 2.0, 3.0, 4.0, 1.0]
A_true = np.polynomial.polynomial.polycompanion(coeffs)
A_true = np.fliplr(np.flipud(A_true))
B_true = np.matrix("1.0 1.0 1.0 1.0").T
C_true = np.matrix("1.0 0.0 0.0 0.0")
D_true = 42.0
# Perform a coordinate transform with a random invertible matrix
T_true = np.matrix([[-0.27144004, -0.39933167, 0.75634684, 0.44135471],
[-0.74855725, -0.39136285, -0.18142339, -0.50356997],
[-0.40688007, 0.81416369, 0.38002113, -0.16483334],
[-0.44769516, 0.15654653, -0.50060858, 0.72419146]])
A = np.linalg.solve(T_true, A_true)*T_true
B = np.linalg.solve(T_true, B_true)
C = C_true*T_true
D = D_true
# Create a state space system and convert it to the observable canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), "observable")
# Check against the true values
np.testing.assert_array_almost_equal(sys_check.A, A_true)
np.testing.assert_array_almost_equal(sys_check.B, B_true)
np.testing.assert_array_almost_equal(sys_check.C, C_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
np.testing.assert_array_almost_equal(T_check, T_true)
# Observable form only supports SISO
sys = tf([[ [1], [1] ]], [[ [1, 2, 1], [1, 2, 1] ]])
np.testing.assert_raises(ControlNotImplemented, observable_form, sys)
def testSisoW2(self):
"SISO plant with KS weighting"
from control import augw, ss
g = ss([-1.],[1.],[1.],[1.])
w2 = ss([-2],[1.],[1.],[2.])
p = augw(g,w2=w2)
self.assertEqual(2,p.outputs)
self.assertEqual(2,p.inputs)
# w->z2 should be 0
self.siso_almost_equal(ss([],[],[],0),p[0,0])
# w->v should be 1
self.siso_almost_equal(ss([],[],[],[1]),p[1,0])
# u->z2 should be w2
self.siso_almost_equal(w2,p[0,1])
# u->v should be -g
self.siso_almost_equal(-g,p[1,1])
def testSisoW1(self):
"SISO plant with S weighting"
from control import augw, ss
g = ss([-1.],[1.],[1.],[1.])
w1 = ss([-2],[2.],[1.],[2.])
p = augw(g,w1)
self.assertEqual(2,p.outputs)
self.assertEqual(2,p.inputs)
# w->z1 should be w1
self.siso_almost_equal(w1,p[0,0])
# w->v should be 1
self.siso_almost_equal(ss([],[],[],[1]),p[1,0])
# u->z1 should be -w1*g
self.siso_almost_equal(-w1*g,p[0,1])
# u->v should be -g
self.siso_almost_equal(-g,p[1,1])
def discretizeSystem(self,n,nu,ny,Ts,method):
self.Ts = Ts
self.method = method
sysc = ctrl.ss(self.hcw(n),self.inputMats(nu),self.observer(ny),self.feedForward(ny,nu))
sysd = ctrl.matlab.c2d(sysc,Ts,method)
return sysc,sysd
def simulate_block(self,u,init,T0,len_block,dt):
T = np.arange(T0,T0+len_block+dt,dt)
MA = coeff.matrixA()
MB = coeff.matrixB()
MC = np.eye(len(MA))
MD = np.zeros(MB.shape)
ssS=control.ss(MA,MB,MC,MD)
yout,T,xout = control.lsim(ssS,u,T,init)
def stateSpace(self,symmetric=True):
MA = self.MatrixA(symmetric)
MB = self.MatrixB(symmetric)
MC = np.eye(4)
MD = np.zeros(MB.shape)
return control.ss(MA,MB,MC,MD)
def plant():
"""plant() -> g
g - LTI object; 2x2 plant with a RHP zero, at s=0.5.
"""
den = [0.2, 1.2, 1]
gtf = tf([[[1], [1]],
[[2, 1], [2]]],
[[den, den],
[den, den]])
return ss(gtf)
def test_unreachable_system(self):
"""Test reachable canonical form with an unreachable system"""
# Create an unreachable system
A = np.matrix("1.0 2.0 2.0; 4.0 5.0 5.0; 7.0 8.0 8.0")
B = np.matrix("1.0 1.0 1.0").T
C = np.matrix("1.0 1.0 1.0")
D = 42.0
sys = ss(A, B, C, D)
# Check if an exception is raised
np.testing.assert_raises(ValueError, canonical_form, sys, "reachable")
def test_ss_input_with_int_element(self):
ident = np.matrix(np.identity(2), dtype=int)
a = np.matrix([[0, 1],
[-1, -2]], dtype=int) * ident
b = np.matrix([[0],
[1]], dtype=int)
c = np.matrix([[0, 1]], dtype=int)
d = 0
sys = ctl.ss(a, b, c, d)
sys2 = ctl.ss2tf(sys)
self.assertAlmostEqual(ctl.dcgain(sys), ctl.dcgain(sys2))
def testH2syn(self):
"Test h2syn"
p = control.ss(-1, [1, 1], [[1], [1]], [[0, 1], [1, 0]])
k = control.robust.h2syn(p, 1, 1)
# from Octave, which also uses SB10HD for H-2 synthesis:
# a= -1; b1= 1; b2= 1; c1= 1; c2= 1; d11= 0; d12= 1; d21= 1; d22= 0;
# g = ss(a,[b1,b2],[c1;c2],[d11,d12;d21,d22]);
# k = h2syn(g,1,1);
# the solution is the same as for the hinfsyn test
np.testing.assert_array_almost_equal(k.A, [[-3]])
np.testing.assert_array_almost_equal(k.B, [[1]])
np.testing.assert_array_almost_equal(k.C, [[-1]])
np.testing.assert_array_almost_equal(k.D, [[0]])
def form_PI_cl(data):
A = np.array([[1.0]])
B = np.array([[1.0]])
for i in range(N):
C = np.array([[dt*data['Ki_fit'][i]]])
D = np.array([[data['Kp_fit'][i]]])
pi_block = ss(A, B, C, D)
bike_block = ss(data['A_cl'][i], data['B_cl'][i], data['C_cl'][i], 0)
pc = series(pi_block, bike_block)
cl = feedback(pc, 1, sign=-1)
data['yr_cl_evals'][i] = la.eigvals(cl.A)
assert(np.all(abs(data['yr_cl_evals'][i]) < 1.0))
data['A_yr_cl'][i] = cl.A
data['B_yr_cl'][i] = cl.B
data['C_yr_cl'][i] = cl.C
assert(cl.D == 0)
num, den = ss2tf(cl.A, cl.B, cl.C, cl.D)
data['w_psi_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_psi_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_psi_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_psi_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
def testSs2tfStaticMimo(self):
"""Regression: ss2tf for MIMO static gain"""
import control
# 2x3 TFM
a = []
b = []
c = []
d = np.matrix([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]])
gtf = control.ss2tf(control.ss(a,b,c,d))
# we need a 3x2x1 array to compare with gtf.num
# np.testing.assert_array_equal doesn't seem to like a matrices
# with an extra dimension, so convert to ndarray
numref = np.asarray(d)[...,np.newaxis]
np.testing.assert_array_equal(numref, np.array(gtf.num) / np.array(gtf.den))
def testTf2SsDuplicatePoles(self):
"""Tests for "too few poles for MIMO tf #111" """
import control
try:
import slycot
num = [ [ [1], [0] ],
[ [0], [1] ] ]
den = [ [ [1,0], [1] ],
[ [1], [1,0] ] ]
g = control.tf(num, den)
s = control.ss(g)
np.testing.assert_array_equal(g.pole(), s.pole())
except ImportError:
print("Slycot not present, skipping")
def testHinfsyn(self):
"Test hinfsyn"
p = control.ss(-1, [1, 1], [[1], [1]], [[0, 1], [1, 0]])
k, cl, gam, rcond = control.robust.hinfsyn(p, 1, 1)
# from Octave, which also uses SB10AD:
# a= -1; b1= 1; b2= 1; c1= 1; c2= 1; d11= 0; d12= 1; d21= 1; d22= 0;
# g = ss(a,[b1,b2],[c1;c2],[d11,d12;d21,d22]);
# [k,cl] = hinfsyn(g,1,1);
np.testing.assert_array_almost_equal(k.A, [[-3]])
np.testing.assert_array_almost_equal(k.B, [[1]])
np.testing.assert_array_almost_equal(k.C, [[-1]])
np.testing.assert_array_almost_equal(k.D, [[0]])
np.testing.assert_array_almost_equal(cl.A, [[-1, -1], [1, -3]])
np.testing.assert_array_almost_equal(cl.B, [[1], [1]])
np.testing.assert_array_almost_equal(cl.C, [[1, -1]])
np.testing.assert_array_almost_equal(cl.D, [[0]])
def simulate_block(self,init,T0,len_block,dt):
T = np.arange(T0-dt,T0+len_block,dt)
u = np.zeros((len(T)))
if self.diag:
print "Time segment: "+str(T[0])+" to "+str(T[-1])
self.V0,self.y0,self.R0,self.q0,self.a0 = init
self.update()
MA = self.MA(self.a0,self.M0)
MB = self.MB()
MC = np.eye(len(MA))
MD = np.zeros(MB.shape)
ssS=control.ss(MA,MB,MC,MD)
if self.diag:
print MA
print ssS
yout,T,xout = control.lsim(ssS,u,T,init)
fix1=yout[1:]
self.T_sim=T[-1]
return fix1[:int(len_block/dt)]
def calcK(self, Plant): #Calculates the K matrix using python control systems and lqr pole placement #Define A matrix A = [ [0, 1], [-Plant.g / Plant.l, -Plant.b / (Plant.m * (Plant.l ** 2))] ] #Define B matrix B = [ [0], [1 / (Plant.m * (Plant.l ** 2))] ] #define C matrix. It's just the identity C = np.identity(2) #The D matrix is just 0 D = [ [0], [0] ] #Create the statespace representation sys = control.ss(A, B, C, D) #Create the state weight matrix, weight position 20000, and speed 100 Q = [ [20000, 0], [0, 100] ] #Create input weight matrix R = [ [1] ] #Compute LQR LQR_vals = control.lqr(sys, Q, R) return LQR_vals[0] #return K values of LQR return
k = (1.0*2*np.pi)**2 # spring constant (N/m) wn = np.sqrt(k/m) # natural frequency (rad/s) # Select damping ratio and use it to choose an appropriate c zeta = 0.1 # damping ratio c = 2*zeta*wn*m # damping coeff. U_max = 50 # Maximum actuator effort (N) A = np.array([[0, 1], [-wn**2, -2*zeta*wn]]) B = np.array([[0], [1/m]]) C = np.eye(2) D = np.zeros((2, 1)) sys = control.ss(A, B, C, D) # Convert the system to digital. We need to use the discrete version of the # system for the MPC solution digital_sys = control.sample_system(sys, dt) # Get the number of states and inputs - for use in setting up the optimization # problem num_states = np.shape(A)[0] # Number of states num_inputs = np.shape(B)[1] # Number of inputs # Define the desired trajectory to track. Here, it's just a desired final position XD = 1.0 XD_dot = 0.0 # Define the weights on the system states and input
def testSs2tfStaticSiso(self):
"""Regression: ss2tf for SISO static gain"""
import control
gsiso = control.ss2tf(control.ss([], [], [], 0.5))
np.testing.assert_array_equal([[[0.5]]], gsiso.num)
np.testing.assert_array_equal([[[1.]]], gsiso.den)
def compute_gains(Q, R, W, V, dt):
"""Given LQR Q and R matrices, and Kalman W and V matrices, and sample
time, compute optimal feedback gain and optimal filter gains."""
data = np.empty((N,), dtype=controller_t)
# Loop over all speeds for which we have system dynamics
for i in range(N):
data['theta_R_dot'][i] = theta_R_dot[i]
data['dt'][i] = dt
# Convert the bike dynamics to discrete time using a zero order hold
data['A'][i], data['B'][i], _, _, _ = cont2discrete(
(A_w[i], B_w[i, :], eye(4), zeros((4, 1))), dt)
data['plant_evals_d'][i] = la.eigvals(data['A'][i])
data['plant_evals_c'][i] = np.log(data['plant_evals_d'][i]) / dt
# Bicycle measurement matrices
# - steer angle
# - roll rate
data['C_m'][i] = C_w[i, :2, :]
# - yaw rate
data['C_z'][i] = C_w[i, 2, :]
A = data['A'][i]
B = data['B'][i, :, 2].reshape((4, 1))
C_m = data['C_m'][i]
C_z = data['C_z'][i]
# Controllability from steer torque
data['ctrb_plant'][i] = ctrb(A, B)
u, s, v = la.svd(data['ctrb_plant'][i])
assert(np.all(s > 1e-13))
# Solve discrete algebraic Ricatti equation associated with LQI problem
P_c = dare(A, B, R, Q)
# Optimal feedback gain using solution of Ricatti equation
K_c = -la.solve(R + dot(B.T, dot(P_c, B)),
dot(B.T, dot(P_c, A)))
data['K_c'][i] = K_c
data['A_c'][i] = A + dot(B, K_c)
data['B_c'][i] = B
data['controller_evals'][i] = la.eigvals(data['A_c'][i])
data['controller_evals_c'][i] = np.log(data['controller_evals'][i]) / dt
assert(np.all(abs(data['controller_evals'][i]) < 1.0))
# Observability from steer angle and roll rate measurement
# Note that (A, C_m * A) must be observable in the "current estimator"
# formulation
data['obsv_plant'][i] = obsv(A, dot(C_m, A))
u, s, v = la.svd(data['obsv_plant'][i])
assert(np.all(s > 1e-13))
# Solve Riccati equation
P_e = dare(A.T, C_m.T, V, W)
# Compute Kalman gain
K_e = dot(P_e, dot(C_m.T, la.inv(dot(C_m, dot(P_e, C_m.T)) + V)))
data['K_e'][i] = K_e
data['A_e'][i] = dot(eye(4) - dot(K_e, C_m), A)
data['B_e'][i] = np.hstack((dot(eye(4) - dot(K_e, C_m), B), K_e))
data['estimator_evals'][i] = la.eigvals(data['A_e'][i])
data['estimator_evals_c'][i] = np.log(data['estimator_evals'][i]) / dt
# Verify that Kalman estimator eigenvalues are stable
assert(np.all(abs(data['estimator_evals'][i]) < 1.0))
# Closed loop state space equations
A_cl = np.zeros((8, 8))
A_cl[:4, :4] = A
A_cl[:4, 4:] = dot(B, K_c)
A_cl[4:, :4] = dot(K_e, dot(C_m, A))
A_cl[4:, 4:] = A - A_cl[4:, :4] + A_cl[:4, 4:]
data['A_cl'][i] = A_cl
data['closed_loop_evals'][i] = la.eigvals(A_cl)
assert(np.all(abs(data['closed_loop_evals'][i]) < 1.0))
B_cl = np.zeros((8, 1))
B_cl[:4, 0] = B.reshape((4,))
B_cl[4:, 0] = dot(eye(4) - dot(K_e, C_m), B).reshape((4,))
data['B_cl'][i] = B_cl
C_cl = np.hstack((C_z, np.zeros((1, 4))))
data['C_cl'][i] = C_cl
# Transfer functions from r to yaw rate
num, den = ss2tf(A_cl, B_cl, C_cl, 0)
data['w_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
# Open loop transfer function from e to yaw rate (PI loop not closed,
# but LQR/LQG loop closed.
inner_cl = ss(A_cl, B_cl, C_cl, 0)
pi_block = ss([[1]], [[1]], [[data['Ki_fit'][i]*dt]], [[data['Kp_fit'][i]]])
e_to_psi_dot = series(pi_block, inner_cl)
num, den = ss2tf(e_to_psi_dot.A, e_to_psi_dot.B, e_to_psi_dot.C, e_to_psi_dot.D)
data['w_e_to_psi_dot'][i], y = freqz(num[0], den)
data['w_e_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_e_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_e_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
return data
# Select damping ratio and use it to choose an appropriate c
zeta = 0.00 # damping ratio
c = 2*zeta*wn*m # damping coeff.
# Form the continuous time version of the system
A_cont = np.array([[0, 1],
[-wn**2, -2*zeta*wn]])
B_cont = np.array([[0],
[1/m]])
C_cont = np.array([[1, 0]]) #np.eye(2)
D_cont = np.zeros((np.shape(C_cont)[0],np.shape(B_cont)[1]))
sys = control.ss(A_cont, B_cont, C_cont, D_cont)
# Now, digitize it
digital_sys = control.sample_system(sys, dt)
# And extract the state space components
A = digital_sys.A
B = digital_sys.B
C = digital_sys.C
D = digital_sys.D
# Example arrays from the link in the preamble. Were used to check this code.
# A = np.array([[1, 1],[0, 1]])
# B = np.array([[0.5],[1]])
# C = np.array([[1, 0]])
# D = np.zeros((np.shape(C)[0],np.shape(B)[1]))
K = 1
tau = 5
numer = [K]
denom = [tau,1]
sys1 = tf(numer,denom)
Kc = 2
tau_i = 5
#PI controller
controllerA = [Kc*tau_i, Kc]
controllerB = [tau_i, 0]
cont_sys1 = tf(controllerA, controllerB)
cont_sys = ss(cont_sys1)
#Transformation to State-Space
sys = ss(sys1)
A, B, C, D = np.asarray(sys.A), np.asarray(sys.B), np.asarray(sys.C), \
np.asarray(sys.D)
contA, contB, contC, contD = np.asarray(cont_sys.A), np.asarray(cont_sys.B), np.asarray(cont_sys.C), \
np.asarray(cont_sys.D)
Nstates = A.shape[0]
contNstates = contA.shape[0]
z_cont = np.zeros((contNstates, 1))
z = np.zeros((Nstates, 1))
def testMimoW123(self):
"MIMO plant with all weights"
from control import augw, ss, append
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], p[4,2])
self.siso_almost_equal(w3g[0,1], p[4,3])
self.siso_almost_equal(w3g[1,0], p[5,2])
self.siso_almost_equal(w3g[1,1], 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])
co._aircraft_properties(b,c,A,S,e,m,Ixx,Iyy,Izz,Ixz,xcg,xac,propInc,propArm)
co._tail(tail)
co._mainWing(main)
co._canard(canard)
co._vert(vert)
co.deriv()
damp=co.damping()
print "Damping 1: ",damp[0]
print "Damping 2: ",damp[1]
MA=co.MatrixA()
MB=co.MatrixB()
MC = np.eye(4)
MD = np.zeros(MB.shape)
SS=control.ss(MA,MB,MC,MD)
"""
CX0 = W * np.sin(th0) / (0.5 * rho * V0 ** 2 * S)
CXu = -0.02792
CXa = -0.47966
CXadot = +0.08330
CXq = -0.28170
CXde = -0.03728
CZ0 = -W * np.cos(th0) / (0.5 * rho * V0 ** 2 * S)
CZu = -0.37616
CZa = -5.74340
CZadot = -0.00350
CZq = -5.66290
CZde = -0.69612
