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
