def testSimulation(self):
T = range(100)
U = np.sin(T)
# For now, just check calling syntax
# TODO: add checks on output of simulations
tout, yout = step_response(self.siso_ss1d)
tout, yout = step_response(self.siso_ss1d, T)
tout, yout = impulse_response(self.siso_ss1d, T)
tout, yout = impulse_response(self.siso_ss1d)
tout, yout, xout = forced_response(self.siso_ss1d, T, U, 0)
tout, yout, xout = forced_response(self.siso_ss2d, T, U, 0)
tout, yout, xout = forced_response(self.siso_ss3d, T, U, 0)
def test_Ny2_Nu2(i, j, zero_Pzw=True, Ts=0.008):
""" Correct impulse response for y in R and u in R.
"""
P = random_dtime_sys(5, 5, Ts)
Pzw, Pzu, Pyw, Pyu = Ss.get_partitioned_transfer_matrices(P, 2, 2)
if zero_Pzw:
Pzw = Ss.tf_blocks(np.zeros((3, 3)), dt=Ts)
weight = np.random.randn(12)
# weight = np.array([0, 0, 0., 1., 0.0, 0.0]) # for debug
Tsim = np.arange(30) * Ts
z = co.tf([1, 0], [1], Ts)
Q00 = Ts * (weight[0] + weight[1] * z ** (-1) + weight[2] * z ** (-2))
Q01 = Ts * (weight[3] + weight[4] * z ** (-1) + weight[5] * z ** (-2))
Q10 = Ts * (weight[6] + weight[7] * z ** (-1) + weight[8] * z ** (-2))
Q11 = Ts * (weight[9] + weight[10] * z ** (-1) + weight[11] * z ** (-2))
# expected result
H_expected = Pzw[i, j] + (Pzu[i, 0] * Q00 * Pyw[0, j]
+ Pzu[i, 0] * Q01 * Pyw[1, j]
+ Pzu[i, 1] * Q10 * Pyw[0, j]
+ Pzu[i, 1] * Q11 * Pyw[1, j])
_, imp_expected = co.impulse_response(H_expected, Tsim)
imp_expected = imp_expected[0, :]
# actual result
imp_actual = Ss.Qsyn.obtain_time_response_var(weight, (i, j), Tsim, Pzw, Pzu, Pyw)
# # debug plot
# plt.plot(imp_actual)
# plt.plot(imp_expected, '--')
# plt.show()
np.testing.assert_allclose(imp_expected, imp_actual, atol=1e-8)
def graficado(valor_ajustado_de_la_deslizadera_del_tiempo, tamaño_impulso,
tamaño_escalon, tamaño_rampa, boton_pulsado, G0):
fig.clf()
retardo = eval(input_del_retardo_en_la_ventana.get())
if boton_pulsado == 'Impulso':
t, y = control.impulse_response(
G0 * tamaño_impulso,
T=(valor_ajustado_de_la_deslizadera_del_tiempo))
if boton_pulsado == 'Escalon':
t, y = control.step_response(
G0 * tamaño_escalon,
T=(valor_ajustado_de_la_deslizadera_del_tiempo))
if boton_pulsado == 'Rampa':
t, y = control.step_response(
G0 * tamaño_rampa / s,
T=(valor_ajustado_de_la_deslizadera_del_tiempo))
if boton_pulsado == 'Arbitraria':
t, y, xout = control.forced_response(G0,
T=(t_experimental),
U=(u_experimental))
t_a_dibujar, y_a_dibujar = ajusta_el_retardo_segun_el_input_de_la_ventana(
t, y, retardo)
fig.add_subplot(111).plot(t_a_dibujar, y_a_dibujar)
canvas.draw()
def impulse(tf_list=[], N=100, T=None, name=None):
data = []
T_max = get_T_max(tf_list, T=T, N=N)
for index, tf in enumerate(tf_list):
if ctl.isctime(tf):
T = np.linspace(0, T_max, N)
line_shape = "linear"
else:
T = np.arange(0, T_max, tf.dt)
line_shape = "hv"
t, y = ctl.impulse_response(tf, T=T)
tf_name = "tf {}".format(index + 1)
data.append({
"x": np.ravel(t),
"y": np.ravel(y),
"name": tf_name,
"mode": "lines",
"showlegend": False,
"line_shape": line_shape
})
layout = default_layout("time (s)", "response", name)
fig = go.Figure(data, layout=layout)
return fig
def FunBotao1(FuncaoControlador):
print(FuncaoControlador)
print(type(FuncaoControlador))
x,y = FuncaoControlador.split(';')
print(x)
print(type(x))
print(y)
print(type(y))
x = np.fromstring(x, dtype=int, sep = ',')
y = np.fromstring(y, dtype=int, sep = ',')
print(x)
print(type(x))
print(y)
print(type(y))
G1 = co.tf(x, y)
print(G1)
G1
t = np.linspace(0, 10, 100)
t1, y1 = co.impulse_response(G1,t)
plt.plot(t1, y1)
plt.xlabel('time(s)')
plt.ylabel('amplitude')
plt.grid()
plt.show()
def plot_response(sys, T=None, U=0.0, impulse=True, step=True) -> None:
if impulse:
t, yout = control.impulse_response(sys, T=T, X0=U)
plot_params(t, yout)
if step:
t, yout = control.step_response(sys, T=T, X0=U)
plot_params(t, yout)
return
def plotImpulseResponse(self, CE, axx = None, Plot=True): t, q = control.impulse_response(CE) if axx is not None: axx.plot(t,q, marker = np.random.choice(self.marker), label = 'P:%s, I:%s, D:%s' %(self.P, self.I, self.D)) axx.legend() if Plot: plt.plot(t,q, marker = np.random.choice(self.marker), label = 'P:%s, I:%s, D:%s' %(self.P, self.I, self.D)) plt.show() return t,q
def plot_response(sys, T=None, U=0.0, X0=np.zeros(4), impulse=False, step=False, lsim=False, sym=True, input=0) -> None:
if impulse:
t, yout = control.impulse_response(sys, T=T, X0=X0, input=0)
plot_params(t, yout, sym=sym)
if step:
t, yout = control.step_response(sys, T=T, X0=X0, input=0)
plot_params(t, yout, sym=sym)
if lsim:
t, yout, xout = control.forced_response(sys, U=U, T=T, X0=X0)
plot_params(t, yout, sym=sym)
return
def impulse(tf_list=[], N=200, T=None, name=None):
data = []
T = get_T(tf_list, N=N, T=T)
for index, tf in enumerate(tf_list):
t, y = ctl.impulse_response(tf, T=T)
tf_name = "tf {}".format(index + 1)
data.append({"x": t, "y": y, "name": tf_name, "showlegend": False})
layout = default_layout("time (s)", "response", name)
fig = go.Figure(data, layout=layout)
fig.show()
def main():
# Create a state space pendulum
# Set variables
m = 1 # Mass
l = 1 # Length
sys = create_pendulum(m, l)
# Impulse response
t, y = control.impulse_response(sys)
plt.plot(t, y)
plt.xlabel('time [s]')
plt.ylabel('theta [rad]')
plt.grid(True)
plt.show()
def time (tf, method = 'step', plot = False):
print "==================================================================";
print "Poles: ", ctrl.pole (tf);
print "Zeros: ", ctrl.zero (tf);
dc = ctrl.dcgain (tf);
print "DC gain: ", dc;
if (method == 'step'):
t, y = ctrl.step_response (tf);
if (method == 'impulse'):
t, y = ctrl.impulse_response (tf);
ys = filter (lambda l: l >= 0.98 * dc, y);
i = np.ndarray.tolist(y).index (min (ys));
print "Ts: ", t[i];
print "Overshoot: ", (max (y) / dc) - 1;
i = np.ndarray.tolist(y).index (max (y));
print "Tr: ", t[i];
if (plot == True):
p = Plotter ({'grid' : True});
p.plot ([(t, y)]);
return t, y
def test_Ny1_Nu1(i, j, Ts=0.008):
""" Correct impulse response for y in R and u in R.
"""
P = random_dtime_sys(5, 5, Ts)
Pzw, Pzu, Pyw, Pyu = Ss.get_partitioned_transfer_matrices(P, 1, 1)
weight = np.array([0.1, 0.4, 0, 0.2])
Tsim = np.arange(100) * Ts
z = co.tf([1, 0], [1], Ts)
Q = Ts * (0.1 + 0.4 * z ** (-1) + 0 + 0.2 * z ** (-3))
# expected result
H_expected = Pzw[i, j] + Pzu[i, 0] * Q * Pyw[0, j]
_, imp_expected = co.impulse_response(H_expected, Tsim)
imp_expected = imp_expected[0, :]
# actual result
imp_actual = Ss.Qsyn.obtain_time_response_var(weight, (i, j), Tsim, Pzw, Pzu, Pyw)
# # debug plot
# plt.plot(imp_actual)
# plt.plot(imp_expected, '--')
# plt.show()
np.testing.assert_allclose(imp_expected, imp_actual, atol=1e-5)
G2d_num = G2d_num / den_coeff[z**2] # Monic numerator
G2d_den = G2d_den / den_coeff[z**2] # Monic denominator
G2d_monic = G2d_num / G2d_den # Monic trasnfer function
# In[Plot saturation]
I = np.arange(0., 20., 0.1)
fig, ax = plt.subplots(1, 1, sharex=True, figsize=(4, 3))
ax.plot(I, L_val * 1e6 * saturation_formula(I), 'k')
ax.grid(True)
ax.set_xlabel('Inductor current $i_L$ (A)', fontsize=14)
ax.set_ylabel('Inductance $L$ ($\mu$H)', fontsize=14)
if not os.path.exists("fig"):
os.makedirs("fig")
fig.savefig(os.path.join("fig", "RLC_characteristics.pdf"),
bbox_inches='tight')
# In[Linear model analysis]
import control
ss_model = control.ss(A_nominal, B_nominal, np.array([1, 0]), np.array(0))
ss_model_dt = control.c2d(ss_model, Td_val)
t = np.arange(200) * Td_val
t, y = control.impulse_response(ss_model_dt, t)
# t, y = control.step_response(ss_model, t)
plt.figure()
plt.plot(y)
K_i = 1
K_d = 0.3
tf_controller = tf([K_d, K_p, K_i], [1, 0])
# Assume reference input is Sun's angle which is a step function.
tf_reference = tf([target_position], [1, 0])
# Closed loop transfer function and output can be calculated.
tf_closed_loop = (tf_plant*tf_controller)/(1+tf_plant*tf_controller)
tf_output = tf_reference*tf_closed_loop
# Now by finding output transfer function's impulse reponse, we can see it's behaviour in time domain.
duration = 5
# Output will be computed for this many seconds.
t = np.linspace(0, duration, num = int(duration/(TIME_STEP*1e-3)))
t, output = impulse_response(tf_output, t)
# Let's plot the panel's expected angle vs time.
plt.plot(t, output)
plt.plot(t, target_position*np.ones(len(t)))
plt.ylabel(f'Expected Angle')
plt.xlabel('Time (s)')
plt.show()
# Compare the performance of pid controller to the other controllers.
# Controller function is defined.
def get_controller_output(K_p, K_i, K_d, errors):
return (K_p*errors[-1] + K_d*np.gradient(errors)[-1]/(TIME_STEP*1e-3) + K_i*np.sum(errors)*(TIME_STEP*1e-3))
# Gradient function is used in order to compute the numerical derivative of the error.
F - b * v) * np.cos(theta)) / (l * (M + m * (1 - np.cos(theta) ** 2)))
return der
# Brutal forward euler discretization
Ad = np.eye(nx) + Ac * Ts_MPC
Bd = Bc * Ts_MPC
# Force disturbance
wu = 10 # bandwidth of the force disturbance
std_du = 0.3
Ts = 1e-3
Hu = control.TransferFunction([1], [1 / wu, 1])
Hu = Hu * Hu
Hud = control.matlab.c2d(Hu, Ts)
t_imp = np.arange(5000) * Ts
t, y = control.impulse_response(Hud, t_imp)
y = y[0]
std_tmp = np.sqrt(np.sum(y ** 2)) # np.sqrt(trapz(y**2,t))
Hu = Hu / (std_tmp) * std_du
N_sim = 100000
e = np.random.randn(N_sim)
te = np.arange(N_sim) * Ts
_, d_fast, _ = control.forced_response(Hu, te, e)
d_fast = d_fast[1000:]
# Reference input and states
t_ref_vec = np.array([0.0, 10.0, 20.0, 30.0, 40.0])
p_ref_vec = np.array([0.0, 0.3, 0.3, 0.0, 0.0])
rp_fun = interp1d(t_ref_vec, p_ref_vec, kind='zero')
r_fun = lambda t: np.array([rp_fun(t), 0.0, 0.0, 0.0])
def num_model_asym_data(output=1,
t_lookup=3717,
t_limit=14,
eigenmotion="dutch roll",
block_fuel=2700,
passenger_weight=771,
CY_b=-0.7500,
Cn_r=-0.2061,
Cn_p=-0.0602,
Cl_r=0.2376,
Cl_p=-0.7108,
input=1):
# Outputs: 1 - phi / 2 - pb/2V / 3 - rb/2V
t_interval = t_lookup + t_limit
# Flight data imported
mat = scipy.io.loadmat('flight_actual.mat')
flight_data = mat['flightdata']
flight_data = flight_data[0, 0]
# Get data location
index = int((t_lookup - flight_data['time'][0][0][0][0][0]) / 0.1)
n_points = int(t_limit / 0.1) + 1
# Obtain correct weight (manoeuvre start) and velocity, get system
used_fuel = flight_data['lh_engine_FU'][0][0][0][index] + flight_data[
'rh_engine_FU'][0][0][0][index]
mass_event = (block_fuel - used_fuel + 9165) * 0.453592 + passenger_weight
tas_event = flight_data['Dadc1_tas'][0][0][0][index] * 0.514444
# obtain correct rho
h_p = flight_data['Dadc1_alt'][0][0][0][index] * 0.3048
p = 101325 * (1 + (-0.0065 * h_p / 288.15))**(-9.81 / (-0.0065 * 287.05))
T = flight_data['Dadc1_sat'][0][0][0][index] + 273.15
rho = p / (287.05 * T)
# Obtain correspondent flight data
data_event = np.zeros((n_points, 2))
if output == 0:
for i in range(n_points):
data_event[i, 0] = flight_data['time'][0][0][0][0][
index + i] - flight_data['time'][0][0][0][0][index]
if output == 1:
for i in range(n_points):
data_event[i, 0] = flight_data['time'][0][0][0][0][
index + i] - flight_data['time'][0][0][0][0][index]
data_event[i,
1] = flight_data['Ahrs1_Roll'][0][0][0][index +
i] # Output phi
elif output == 2:
for i in range(n_points):
data_event[i, 0] = flight_data['time'][0][0][0][0][
index + i] - flight_data['time'][0][0][0][0][index]
data_event[i, 1] = flight_data['Ahrs1_bRollRate'][0][0][0][
index + i] # Output pb/2V
elif output == 3:
for i in range(n_points):
data_event[i, 0] = flight_data['time'][0][0][0][0][
index + i] - flight_data['time'][0][0][0][0][index]
data_event[i, 1] = flight_data['Ahrs1_bYawRate'][0][0][0][
index + i] # Output rb/2V
t1 = data_event[:, 0]
y1 = data_event[:, 1] * m.pi / 180
if eigenmotion == "dutch roll":
input_delta_a = (
flight_data['delta_a'][0][0][0][index:index + n_points] * m.pi /
180 -
flight_data['delta_a'][0][0][0][index + n_points] * m.pi / 180)
input_delta_r = -flight_data['delta_r'][0][0][0][index:index +
n_points] * m.pi / 180
if eigenmotion == "aperiodic":
input_delta_a = -(
flight_data['delta_a'][0][0][0][index:index + n_points] * m.pi /
180 -
flight_data['delta_a'][0][0][0][index + n_points] * m.pi / 180)
input_delta_r = -flight_data['delta_r'][0][0][0][index:index +
n_points] * m.pi / 180
if eigenmotion == "spiral":
input_delta_a = -(
flight_data['delta_a'][0][0][0][index:index + n_points] * m.pi /
180 -
flight_data['delta_a'][0][0][0][index + n_points] * m.pi / 180)
input_delta_r = -(
flight_data['delta_r'][0][0][0][index:index + n_points] * m.pi /
180 -
flight_data['delta_r'][0][0][0][index + n_points] * m.pi / 180)
input_tot = np.array([input_delta_a[:, 0], input_delta_r[:, 0]])
sys = ss_asym(rho=rho,
m=mass_event,
theta_0=flight_data['Ahrs1_Pitch'][0][0][0][index] * m.pi /
180,
v=tas_event,
CY_b=CY_b,
Cn_r=Cn_r,
Cn_p=Cn_p,
Cl_r=Cl_r,
Cl_p=Cl_p)
# t2, out, p2 = control.forced_response(sys, T=t1, U=input_tot,X0=[0., -flight_data['Ahrs1_Roll'][0][0][0][index][0]* m.pi / 180, -flight_data['Ahrs1_bRollRate'][0][0][0][index][0]* m.pi / 180, -flight_data['Ahrs1_bYawRate'][0][0][0][index][0]* m.pi / 180])
t2, out = control.impulse_response(sys, t1, input=input)
# t2, out, p2 = control.forced_response(sys, T=t1, U=input_tot,
# X0=[0., flight_data['Ahrs1_Roll'][0][0][0][index][0] * m.pi / 180,
# flight_data['Ahrs1_bRollRate'][0][0][0][index][0] * m.pi / 180,
# flight_data['Ahrs1_bYawRate'][0][0][0][index][0] * m.pi / 180])
y2 = out[output, :] # Outputs: 1 - phi / 2 - pb/2V / 3 - rb/2V
# flight data, model response, time vectors and input vectors
return y1, y2, t1, t2, input_delta_a, input_delta_r, t_lookup, t_interval
ghat_reg = np.linalg.inv(P_prior @ PHI.T @ PHI + sigma_e ** 2 * np.eye(m)) @ P_prior @ PHI.T @ Y
ghat_reg = ghat_reg.ravel()
# Covariance of the Bayesian estimate
P_post = P_prior - P_prior @ PHI.T @ np.linalg.inv(PHI @ P_prior @ PHI.T + sigma_e ** 2 * np.eye(N)) @ PHI @ P_prior # slow, try a faster implementation...
sigma_reg = np.sqrt(np.diag(P_post))
ghat_reg_max = ghat_reg + 3*sigma_reg
ghat_reg_min = ghat_reg - 3*sigma_reg
# In[Evaluate the true inpulse response of the RLC]
ss_model = control.ss(examples.RLC.symbolic_RLC.A_nominal, examples.RLC.symbolic_RLC.B_nominal, np.array([1, 0]), np.array(0))
ss_model_dt = control.c2d(ss_model, Td)
T_imp = np.arange(m+1) * Td
T_imp, g_true = control.impulse_response(ss_model_dt, T_imp)
T_imp = np.arange(T_imp.size)
# In[Plot estimated models]
fig, ax = plt.subplots(1, 2)
ax[0].plot(T, ghat_ML, 'r')
ax[0].fill_between(T, ghat_ML_min, ghat_ML_max, where=ghat_ML_max >= ghat_ML_min, facecolor='red', interpolate=True, alpha=0.2)
ax[0].plot(T_imp, g_true, 'k')
ax[1].plot(T, ghat_reg, 'g')
ax[1].fill_between(T, ghat_reg_min, ghat_reg_max, where=ghat_reg_max >= ghat_reg_min, facecolor='green', alpha=0.2)
ax[1].plot(T_imp, g_true, 'k')
# In[Prior model plot]
sigma_prio = np.sqrt(np.diag(P_prior))
def simulate_pendulum_MPC(sim_options):
use_NN_model = get_parameter(sim_options, 'use_NN_model')
if use_NN_model:
Ts_fit = 10e-3 # model was fitted with this sampling time
ss_model = CartPoleStateSpaceModel(Ts=Ts_fit)
nn_solution = NeuralStateSpaceSimulator(ss_model, Ts=Ts_fit)
model_name = "model_SS_1step_nonoise.pkl"
nn_solution.ss_model.load_state_dict(
torch.load(os.path.join("models", model_name)))
f_ODE = nn_solution.f_ODE
else:
f_ODE = f_ODE_wrapped
seed_val = get_parameter(sim_options, 'seed_val')
if seed_val is not None:
np.random.seed(seed_val)
Ac = get_parameter(sim_options, 'Ac')
Bc = get_parameter(sim_options, 'Bc')
Cc = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.]])
Dc = np.zeros((2, 1))
[nx, nu] = Bc.shape # number of states and number or inputs
ny = np.shape(Cc)[0]
Ts_MPC = get_parameter(sim_options, 'Ts_MPC')
ratio_Ts = int(Ts_MPC // Ts_fast)
Ts_MPC = (
(Ts_MPC //
Ts_fast)) * Ts_fast # make Ts_MPC an integer multiple of Ts_fast
# Brutal forward euler discretization
Ad = np.eye(nx) + Ac * Ts_MPC
Bd = Bc * Ts_MPC
Cd = Cc
Dd = Dc
# Standard deviation of the measurement noise on position and angle
std_npos = get_parameter(sim_options, 'std_npos')
std_nphi = get_parameter(sim_options, 'std_nphi')
# Force disturbance
std_dF = get_parameter(sim_options, 'std_dF')
# disturbance power spectrum
w_F = get_parameter(sim_options,
'w_F') # bandwidth of the force disturbance
tau_F = 1 / w_F
Hu = control.TransferFunction([1], [1 / w_F, 1])
Hu = Hu * Hu
Hud = control.matlab.c2d(Hu, Ts_MPC)
N_sim_imp = tau_F / Ts_MPC * 20
t_imp = np.arange(N_sim_imp) * Ts_MPC
t, y = control.impulse_response(Hud, t_imp)
y = y[0]
std_tmp = np.sqrt(np.sum(y**2)) # np.sqrt(trapz(y**2,t))
Hu = Hu / (std_tmp) * std_dF
N_skip = 1 #int(20 * tau_F // Ts_MPC) # skip initial samples to get a regime sample of d
t_sim_d = get_parameter(sim_options, 'len_sim') # simulation length (s)
N_sim_d = int(t_sim_d // Ts_MPC)
N_sim_d = N_sim_d + N_skip + 1
e = np.random.randn(N_sim_d)
te = np.arange(N_sim_d) * Ts_MPC
_, d, _ = control.forced_response(Hu, te, e)
d = d.ravel()
p_ref = d[N_skip:]
#td = np.arange(len(d)) * Ts_fast
Np = get_parameter(sim_options, 'Np')
Nc = get_parameter(sim_options, 'Nc')
Qx = get_parameter(sim_options, 'Qx')
QxN = get_parameter(sim_options, 'QxN')
Qu = get_parameter(sim_options, 'Qu')
QDu = get_parameter(sim_options, 'QDu')
# Reference input and states
xref_fun = get_parameter(sim_options, 'xref_fun') # reference state
xref_fun_v = np.vectorize(xref_fun, signature='()->(n)')
t0 = 0
xref_MPC = xref_fun(t0)
uref = get_parameter(sim_options, 'uref')
uminus1 = np.array(
[0.0]
) # input at time step negative one - used to penalize the first delta u at time instant 0. Could be the same as uref.
# Constraints
xmin = np.array([-1000, -1000, -1000, -1000])
xmax = np.array([1000, 1000.0, 1000, 1000])
umin = np.array([-10])
umax = np.array([10])
Dumin = np.array([-100 * Ts_MPC])
Dumax = np.array([100 * Ts_MPC])
# Initialize simulation system
phi0 = 0.0 #10*2*np.pi/360
x0 = np.array([0, 0, phi0, 0]) # initial state
# system_dyn = ode(f_ODE_wrapped).set_integrator('vode', method='bdf') # dopri5
system_dyn = ode(f_ODE).set_integrator('dopri5')
system_dyn.set_initial_value(x0, t0)
system_dyn.set_f_params(0.0)
QP_eps_rel = get_parameter(sim_options, 'QP_eps_rel')
QP_eps_abs = get_parameter(sim_options, 'QP_eps_abs')
# Emergency exit conditions
EMERGENCY_STOP = False
EMERGENCY_POS = 30.0
EMERGENCY_ANGLE = 90 * DEG_TO_RAD
K = MPCController(Ad,
Bd,
Np=Np,
Nc=Nc,
x0=x0,
xref=xref_MPC,
uminus1=uminus1,
Qx=Qx,
QxN=QxN,
Qu=Qu,
QDu=QDu,
xmin=xmin,
xmax=xmax,
umin=umin,
umax=umax,
Dumin=Dumin,
Dumax=Dumax,
eps_feas=1e3,
eps_rel=QP_eps_rel,
eps_abs=QP_eps_abs)
try:
K.setup(solve=True) # setup initial problem and also solve it
except:
EMERGENCY_STOP = True
if not EMERGENCY_STOP:
if K.res.info.status != 'solved':
EMERGENCY_STOP = True
# Basic Kalman filter design
Q_kal = get_parameter(sim_options, 'Q_kal')
R_kal = get_parameter(sim_options, 'R_kal')
L, P, W = kalman_design_simple(Ad,
Bd,
Cd,
Dd,
Q_kal,
R_kal,
type='predictor')
x0_est = x0
KF = LinearStateEstimator(x0_est, Ad, Bd, Cd, Dd, L)
# Simulate in closed loop
len_sim = get_parameter(sim_options, 'len_sim') # simulation length (s)
nsim = int(
np.ceil(len_sim / Ts_MPC)
) # simulation length(timesteps) # watch out! +1 added, is it correct?
t_vec = np.zeros((nsim, 1))
t_calc_vec = np.zeros(
(nsim, 1)) # computational time to get MPC solution (+ estimator)
status_vec = np.zeros((nsim, 1))
x_vec = np.zeros((nsim, nx))
x_ref_vec = np.zeros((nsim, nx))
y_vec = np.zeros((nsim, ny))
y_meas_vec = np.zeros((nsim, ny))
y_est_vec = np.zeros((nsim, ny))
x_est_vec = np.zeros((nsim, nx))
u_vec = np.zeros((nsim, nu))
x_MPC_pred = np.zeros(
(nsim, Np + 1, nx)) # on-line predictions from the Kalman Filter
nsim_fast = int(len_sim // Ts_fast)
t_vec_fast = np.zeros((nsim_fast, 1))
x_vec_fast = np.zeros(
(nsim_fast, nx)) # finer integration grid for performance evaluation
x_ref_vec_fast = np.zeros(
(nsim_fast, nx)) # finer integration grid for performance evaluatio
u_vec_fast = np.zeros(
(nsim_fast, nu)) # finer integration grid for performance evaluatio
Fd_vec_fast = np.zeros((nsim_fast, nu)) #
t_int_vec_fast = np.zeros((nsim_fast, 1))
emergency_vec_fast = np.zeros((nsim_fast, 1)) #
t_pred_all = t0 + np.arange(nsim + Np + 1) * Ts_MPC
Xref_MPC_all = xref_fun_v(t_pred_all)
t_step = t0
x_step = x0
u_MPC = None
for idx_fast in range(nsim_fast):
## Determine step type: fast simulation only or MPC step
idx_MPC = idx_fast // ratio_Ts
run_MPC = (idx_fast % ratio_Ts) == 0
# Output for step i
# Ts_MPC outputs
if run_MPC: # it is also a step of the simulation at rate Ts_MPC
t_vec[idx_MPC, :] = t_step
x_vec[idx_MPC, :] = x_step #system_dyn.y
xref_MPC = np.array([p_ref[idx_MPC], 0.0, 0.0, 0.0])
#xref_MPC = xref_fun(t_step) # reference state
t_pred = t_step + np.arange(Np + 1) * Ts_MPC
x_ref_vec[idx_MPC, :] = xref_MPC
if not EMERGENCY_STOP:
# u_MPC, info_MPC = K.output(return_x_seq=True, return_status=True) # u[i] = k(\hat x[i]) possibly computed at time instant -1
u_MPC, info_MPC = K.output(
return_status=True, return_x_seq=True
) # u[i] = k(\hat x[i]) possibly computed at time instant -1
else:
u_MPC = np.zeros(nu)
if not EMERGENCY_STOP:
if info_MPC['status'] != 'solved':
EMERGENCY_STOP = True
if not EMERGENCY_STOP:
#pass
x_MPC_pred[idx_MPC, :, :] = info_MPC[
'x_seq'] # x_MPC_pred[i,i+1,...| possibly computed at time instant -1]
u_vec[idx_MPC, :] = u_MPC
y_step = Cd.dot(x_step) # y[i] from the system
ymeas_step = np.copy(y_step)
ymeas_step[0] += std_npos * np.random.randn()
ymeas_step[1] += std_nphi * np.random.randn()
y_vec[idx_MPC, :] = y_step
y_meas_vec[idx_MPC, :] = ymeas_step
if not EMERGENCY_STOP:
status_vec[idx_MPC, :] = (info_MPC['status'] != 'solved')
if np.abs(ymeas_step[0]) > EMERGENCY_POS or np.abs(
ymeas_step[1]) > EMERGENCY_ANGLE:
EMERGENCY_STOP = True
# Ts_fast outputs
t_vec_fast[idx_fast, :] = t_step
x_vec_fast[idx_fast, :] = x_step #system_dyn.y
x_ref_vec_fast[idx_fast, :] = xref_MPC
u_fast = u_MPC
u_vec_fast[idx_fast, :] = u_fast
Fd_vec_fast[idx_fast, :] = 0.0
emergency_vec_fast[idx_fast, :] = EMERGENCY_STOP
## Update to step i+1
# Controller simulation step at rate Ts_MPC
if run_MPC:
time_calc_start = time.perf_counter()
# Kalman filter: update and predict
#KF.update(ymeas_step) # \hat x[i|i]
#KF.predict(u_MPC) # \hat x[i+1|i]
KF.predict_update(u_MPC, ymeas_step)
# MPC update
if not EMERGENCY_STOP:
#Xref_MPC = Xref_MPC_all[idx_MPC:idx_MPC + Np + 1]
K.update(
KF.x, u_MPC,
xref=xref_MPC) # update with measurement and reference
#K.update(KF.x, u_MPC, xref=xref_MPC) # update with measurement and reference
t_calc_vec[idx_MPC, :] = time.perf_counter() - time_calc_start
if t_calc_vec[idx_MPC, :] > 2 * Ts_MPC:
EMERGENCY_STOP = True
# System simulation step at rate Ts_fast
time_integrate_start = time.perf_counter()
system_dyn.set_f_params(u_fast)
system_dyn.integrate(t_step + Ts_fast)
x_step = system_dyn.y
#x_step = x_step + f_ODE(t_step, x_step, u_fast)*Ts_fast
t_int_vec_fast[
idx_fast, :] = time.perf_counter() - time_integrate_start
# Time update
t_step += Ts_fast
simout = {
't': t_vec,
'x': x_vec,
'u': u_vec,
'y': y_vec,
'y_meas': y_meas_vec,
'x_ref': x_ref_vec,
'x_MPC_pred': x_MPC_pred,
'status': status_vec,
'Fd_fast': Fd_vec_fast,
't_fast': t_vec_fast,
'x_fast': x_vec_fast,
'x_ref_fast': x_ref_vec_fast,
'u_fast': u_vec_fast,
'emergency_fast': emergency_vec_fast,
'KF': KF,
'K': K,
'nsim': nsim,
'Ts_MPC': Ts_MPC,
't_calc': t_calc_vec,
't_int_fast': t_int_vec_fast,
'use_NN_model': use_NN_model
}
return simout
plt.plot(t, y_hat, 'b', label="$\hat y$")
plt.plot(t, y_meas - y_hat, 'r', label="$e$")
plt.grid(True)
plt.xlabel('Time (s)')
plt.ylabel('Voltage (V)')
plt.legend(loc='upper right')
plt.savefig('WH_fit.pdf')
# In[Inspect linear model]
n_imp = 128
G1_num, G1_den = G1.get_tfdata()
G1_sys = control.TransferFunction(G1_num, G1_den, ts)
plt.figure()
plt.title("$G_1$ impulse response")
_, y_imp = control.impulse_response(G1_sys, np.arange(n_imp) * ts)
# plt.plot(G1_num)
plt.plot(y_imp)
plt.savefig(os.path.join("models", model_name, "G1_imp.pdf"))
plt.figure()
mag_G1, phase_G1, omega_G1 = control.bode(G1_sys, omega_limits=[1e2, 1e5])
plt.suptitle("$G_1$ bode plot")
plt.savefig(os.path.join("models", model_name, "G1_bode.pdf"))
# G2_b = G2.G.weight.detach().numpy()[0, 0, ::-1]
G2_num, G2_den = G2.get_tfdata()
G2_sys = control.TransferFunction(G2_num, G2_den, ts)
plt.figure()
plt.title("$G_2$ impulse response")
_, y_imp = control.impulse_response(G2_sys, np.arange(n_imp) * ts)
plt.plot(y_imp)
for elem in co.pole(h2):
print(f'{elem: .2f}')
print("poles (k=5)")
for elem in co.pole(h3):
print(f'{elem: .2f}')
plt.figure(1)
h1_map = co.pzmap(h1, plot=True, title='pzmap (k=2)')
plt.figure(2)
h2_map = co.pzmap(h2, plot=True, title='pzmap (k=3)')
plt.figure(3)
h3_map = co.pzmap(h3, plot=True, title='pzmap (k=5)')
t = np.linspace(0, 10, 1000)
t1, imp1 = co.impulse_response(h1, t)
t2, imp2 = co.impulse_response(h2, t)
t3, imp3 = co.impulse_response(h3, t)
t4, stp1 = co.step_response(h1, t)
t5, stp2 = co.step_response(h2, t)
t6, stp3 = co.step_response(h3, t)
fig1, axs = plt.subplots(3, 1)
fig1.suptitle("Impulse Response")
axs[0].plot(t1, imp1)
axs[1].plot(t2, imp2)
axs[2].plot(t3, imp3)
axs[0].set_ylabel("Amplitude")
axs[1].set_ylabel("Amplitude")
control_zero, conf, 0)
plt.semilogx(f,
20.0 * np.log10(np.abs(pid_results[2])),
linewidth="3",
label='%s *k_i' % ff)
unity = np.ones(len(s))
plt.semilogx(f, 20.0 * np.log10(np.abs(unity)), '--')
plt.legend()
plt.show()
elif args.f == 'ir':
print("Impulse response")
T = np.arange(0, 0.5, 0.0001) # Frequency from 1 to 10^6 Hz
sys = control.tf2ss([1], [1 / wc, 1])
T, yout = control.impulse_response(sys, T)
#plt.plot(T, yout)
print("Step response")
cavity = control.tf([1], [1 / wc, 1])
kp = 10
ki = 5
controller = control.tf([kp, ki], [1, 0])
ol = cavity * controller
sys = control.tf2ss(ol)
T, yout = control.step_response(sys, T)
plt.plot(T, yout)
plt.show()
else:
lpf(f, wc, cavity)
def test_squeeze(self, fcn, nstate, nout, ninp, squeeze, shape1, shape2):
# Figure out if we have SciPy 1+
scipy0 = StrictVersion(sp.__version__) < '1.0'
# Define the system
if fcn == ct.tf and (nout > 1 or ninp > 1) and not slycot_check():
pytest.skip("Conversion of MIMO systems to transfer functions "
"requires slycot.")
else:
sys = fcn(ct.rss(nstate, nout, ninp, strictly_proper=True))
# Generate the time and input vectors
tvec = np.linspace(0, 1, 8)
uvec = np.ones((sys.ninputs, 1)) @ np.reshape(np.sin(tvec), (1, 8))
#
# Pass squeeze argument and make sure the shape is correct
#
# For responses that are indexed by the input, check against shape1
# For responses that have no/fixed input, check against shape2
#
# Impulse response
if isinstance(sys, StateSpace):
# Check the states as well
_, yvec, xvec = ct.impulse_response(
sys, tvec, squeeze=squeeze, return_x=True)
if sys.issiso():
assert xvec.shape == (sys.nstates, 8)
else:
assert xvec.shape == (sys.nstates, sys.ninputs, 8)
else:
_, yvec = ct.impulse_response(sys, tvec, squeeze=squeeze)
assert yvec.shape == shape1
# Step response
if isinstance(sys, StateSpace):
# Check the states as well
_, yvec, xvec = ct.step_response(
sys, tvec, squeeze=squeeze, return_x=True)
if sys.issiso():
assert xvec.shape == (sys.nstates, 8)
else:
assert xvec.shape == (sys.nstates, sys.ninputs, 8)
else:
_, yvec = ct.step_response(sys, tvec, squeeze=squeeze)
assert yvec.shape == shape1
# Initial response (only indexed by output)
if isinstance(sys, StateSpace):
# Check the states as well
_, yvec, xvec = ct.initial_response(
sys, tvec, 1, squeeze=squeeze, return_x=True)
assert xvec.shape == (sys.nstates, 8)
else:
_, yvec = ct.initial_response(sys, tvec, 1, squeeze=squeeze)
assert yvec.shape == shape2
# Forced response (only indexed by output)
if isinstance(sys, StateSpace):
# Check the states as well
_, yvec, xvec = ct.forced_response(
sys, tvec, uvec, 0, return_x=True, squeeze=squeeze)
assert xvec.shape == (sys.nstates, 8)
else:
# Just check the input/output response
_, yvec = ct.forced_response(sys, tvec, uvec, 0, squeeze=squeeze)
assert yvec.shape == shape2
# Test cases where we choose a subset of inputs and outputs
_, yvec = ct.step_response(
sys, tvec, input=ninp-1, output=nout-1, squeeze=squeeze)
if squeeze is False:
# Shape should be unsqueezed
assert yvec.shape == (1, 1, 8)
else:
# Shape should be squeezed
assert yvec.shape == (8, )
# For InputOutputSystems, also test input/output response
if isinstance(sys, ct.InputOutputSystem) and not scipy0:
_, yvec = ct.input_output_response(sys, tvec, uvec, squeeze=squeeze)
assert yvec.shape == shape2
#
# Changing config.default to False should return 3D frequency response
#
ct.config.set_defaults('control', squeeze_time_response=False)
_, yvec = ct.impulse_response(sys, tvec)
if squeeze is not True or sys.ninputs > 1 or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, sys.ninputs, 8)
_, yvec = ct.step_response(sys, tvec)
if squeeze is not True or sys.ninputs > 1 or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, sys.ninputs, 8)
_, yvec = ct.initial_response(sys, tvec, 1)
if squeeze is not True or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, 8)
if isinstance(sys, ct.StateSpace):
_, yvec, xvec = ct.forced_response(
sys, tvec, uvec, 0, return_x=True)
assert xvec.shape == (sys.nstates, 8)
else:
_, yvec = ct.forced_response(sys, tvec, uvec, 0)
if squeeze is not True or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, 8)
# For InputOutputSystems, also test input_output_response
if isinstance(sys, ct.InputOutputSystem) and not scipy0:
_, yvec = ct.input_output_response(sys, tvec, uvec)
if squeeze is not True or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, 8)
def plotImpulseResponse(self):
t, q = control.impulse_response(self.CE)
plt.plot(t, q)
plt.show()
def init_via_simulation(Pssd_design, A1dss, T=5):
self = AtomicFIRController()
nu = A1dss.outputs
ny = A1dss.inputs
nxc = A1dss.states
nx = Pssd_design.states
dT = Pssd_design.dt
self.T = T
self.dT = dT
self.NT = int(T / dT)
self.nx = nx
self.ny = ny
self.nxc = nxc
self.nu = nu
self.ny = ny
self.nw = Pssd_design.inputs - A1dss.outputs
self.nz = Pssd_design.outputs - A1dss.inputs
# closed-loop response, check stability
Pssd_cl = Pssd_design.lft(A1dss)
w, q = np.linalg.eig(Pssd_cl.A)
wmax = w[np.argmax(np.abs(w))]
if np.abs(wmax) > 1:
print(" -- Closed-loop system UNSTABLE. w_max={:}"
" ----> Return None".format(wmax))
return None
else:
print(" -- Closed-loop system STABLE. w_max={:}".format(wmax))
# control systems
self.plant_ol = Pssd_design
self.plant_cl = Pssd_cl
self.exec_ctrl = A1dss
# matrix impulse response
A, B1, B2, C1, C2, D11, D12, D21, D22 = map(
np.array, Ss.get_partitioned_mats(Pssd_design, nu, ny))
Ak, Bk, Ck, Dk = A1dss.A, A1dss.B, A1dss.C, A1dss.D
assert np.all(D22 == 0)
# Concatenate system matrices to find the matrices of the ss
# rep of the impulse response mapping
# [R N] = A_cb | B_cb
# [M L] ------|-------
# C_cb | D_cb
A_cb = np.block([[A + B2.dot(Dk).dot(C2), B2.dot(Ck)],
[Bk.dot(C2), Ak]])
B_cb = np.block([[np.eye(nx), B2.dot(Dk)],
[np.zeros((nxc, nx)), Bk]])
C_cb = np.block([[np.eye(nx), np.zeros((nx, nxc))],
[Dk.dot(C2), Ck]])
D_cb = np.block([[np.zeros((nx, nx)), np.zeros((nx, ny))],
[np.zeros((nu, nx)), Dk]])
P_resp_dss = co.ss(A_cb, B_cb, C_cb, D_cb, dT)
# Compute impulse responses of the matrices R, N, M, L
Tarr = np.arange(0, T, dT) # 5 sec horizon
NT = Tarr.shape[0]
mtrx_impulses = []
for i in range(self.nx + self.ny):
_, yarr = co.impulse_response(
P_resp_dss, Tarr, input=i, transpose=True)
if np.linalg.norm(yarr[-1]) > 1e-10:
raise(ValueError("Matrix Impulse response has non-zero norm at the last time-step! ({:}) "
"Consider increasing horizont T. (Current val={:})".format(
np.linalg.norm(yarr[-1]), T)))
mtrx_impulses.append(yarr)
mtrx_impulses = np.stack(mtrx_impulses, axis=2)
# Individual responses
self.R = mtrx_impulses[:, :self.nx, :self.nx]
self.N = mtrx_impulses[:, :self.nx, self.nx:self.nx + self.ny]
self.M = mtrx_impulses[:, self.nx:self.nx + self.nu, :self.nx]
self.L = mtrx_impulses[:, self.nx:self.nx + self.nu, self.nx:self.nx + self.ny]
self.MB2 = self.M@B2
# Output Impulse responses H (from control input w to output z)
output_impulses = []
for i in range(self.nw):
_, yarr = co.impulse_response(
Pssd_cl, Tarr, input=i, transpose=True
)
# no need to check for finite horizon since H is a linear
# combination of the matrix responses
output_impulses.append(yarr)
self.H = np.stack(output_impulses, axis=2)
# frequency response of the mapping
W = Ss.dft_matrix(self.NT)
self.H_fft = np.zeros_like(self.H) * 1j
for i in range(self.nw):
for j in range(self.nz):
self.H_fft[:, i, j] = [email protected][:, i, j]
return self
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
t = np.linspace(0, 10, 1000)
t1, y1 = co.impulse_response(G1, t)
plt.plot(t1, y1)
plt.xlabel('time (s)')
plt.ylabel('amplitude')
plt.grid()
# Simulation time and sampling time
simulation_time = np.arange(0., 4 * time_constants[-1], 0.1)
# Creating the axes
fig, axs = plt.subplots(3, figsize=(12, 8))
#------------------------ IMPULSE --------------------------#
# Plotting the input
axs[0].axvline(0, ymin=0.04, ymax=0.96, color='k', ls='--', label='Input')
axs[0].plot(0, 1, 'ko')
# Creating the transfer function and then plotting the impulse response
for T in time_constants:
G = 1. / ((T * s) + 1)
t, y = control.impulse_response(G, T=simulation_time)
axs[0].plot(t, y, label='T = {}s'.format(T))
# Setting the graph's labels and title
axs[0].set_title('Impulse Response for a First Order System')
axs[0].set_xlabel('t(s)')
axs[0].set_ylabel('Amplitude')
axs[0].legend()
#------------------------ STEP --------------------------#
# Plotting the input
u = 0 * simulation_time
u[:] = 1
axs[1].plot(simulation_time, u, 'k--', label='Input')
axs[1].axvline(0, ymin=0.04, ymax=0.96, color='k', ls='--')
#plt.yscale('log')
#plt.yticks([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10])
plt.xlim(0, 4)
plt.plot(omega, mag)
plt.plot(omega, phase)
mag, phase, omega = H.freqresp(omega)
mag = np.reshape(mag, -1)
phase = np.reshape(phase, -1)
plt.figure()
plt.grid(True)
plt.xlim(0, 4)
plt.plot(omega, mag)
plt.plot(omega, phase)
T, yout = control.impulse_response(H, range(0, 200))
plt.figure()
plt.grid(True)
plt.plot(T, yout)
#impulse response
N = 201
u = [1]
h = ifft([H(np.exp(complex(0, 2 * math.pi * k / N))) for k in range(0, N - 1)])
H_re = np.convolve(u, h)
T = np.linspace(0, 200, 200)
plt.figure()
plt.grid(True)
plt.plot(T, H_re.real)
#dft
def problem_b3():
m_val = 0.425 # mass
g_val = 9.81 # acceleration due to gravity
d_val = 0.42 # natural length of spring
delta_val = 0.65 # x(max) - distance to electromagnet
r_val = 0.125 # ball radius
R_val = 53 # resistance of electromagnet
L0_val = 0.12 # inductance
L1_val = 0.025 # positive constant
alpha_val = 1.2 # positive constant
c_val = 6.815
k_val = 1880 # spring
b_damper_val = 10.4 # damper
phi_val = np.deg2rad(42) # 90 - angle of slope
slope_val = np.deg2rad(48) # angle of slope
m, g, d, delta, r, R, L0, L1, a, c, k, b_damper, b, phi, x1, x2, x3, V = \
sym.symbols('m, g, d, delta, r, R, L0, L1, a, c, k, b_damper, b, phi, x1, x2, x3, V', real=True,
positive=True)
# Equations Used
x_min = d + (m * g * (math.sin(np.deg2rad(48)))) / k
x_max = delta
x_min_val = x_min.subs([(d, d_val), (m, m_val), (g, g_val),
(phi, phi_val), (k, k_val)])
x_max_val = x_max.subs([(delta, delta_val)])
x = (0.75 * x_min_val) + (0.25 * x_max_val)
print("X: " + str(x))
I = sym.sqrt((((m_val * g_val * math.sin(slope_val)) -
(k_val * (x - d_val))) / (-c_val)) * (delta_val - x)**2)
print("I: " + str(I))
a_equation = ((10 * c_val * I) / (3 * m_val * ((delta_val - x)**2)))
b_equation = ((10 * c_val * I**2) /
(3 * m_val * ((delta_val - x)**3))) - ((5 * k_val) /
(3 * m_val))
c_equation = (5 * b_damper_val) / (3 * m_val)
a_equation_value = float(a_equation.subs([(c, c_val), (m, m_val)]))
b_equation_value = float(b_equation.subs([(k, k_val), (m, m_val)]))
f_equation = float(L0_val + (L1_val * math.exp(-alpha_val *
(d_val - x))))
transfer_function = ctrl.TransferFunction([a_equation_value], [
f_equation, ((c_equation * f_equation) + R_val),
((-b_equation_value * f_equation) + (c_equation * R_val)),
(-b_equation_value * R_val)
])
t_final = 1
sampling_rate = 2000
sampling_time = 1 / sampling_rate
n_points = t_final * sampling_rate
t_span = np.linspace(0, t_final, n_points)
t_imp, x1_imp = ctrl.impulse_response(transfer_function, t_span)
t_step, x1_step = ctrl.step_response(transfer_function, t_span)
plt.plot(t_imp, x1_imp, 'r', label='Impulse Response')
plt.plot(t_step, x1_step, 'b', label='Step Response')
plt.xlabel("Time (s)")
plt.ylabel("Response (m)")
plt.grid()
plt.legend()
plt.show()
def simulate_pendulum_MPC(sim_options):
seed_val = get_parameter(sim_options, 'seed_val')
if seed_val is not None:
np.random.seed(seed_val)
Ac = get_parameter(sim_options, 'Ac')
Bc = get_parameter(sim_options, 'Bc')
Cc = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.]])
Dc = np.zeros((2, 1))
[nx, nu] = Bc.shape # number of states and number or inputs
ny = np.shape(Cc)[0]
Ts_slower_loop = get_parameter(sim_options, 'Ts_slower_loop')
ratio_Ts = int(Ts_slower_loop // Ts_PID)
# Brutal forward euler discretization
Ad = np.eye(nx) + Ac * Ts_slower_loop
Bd = Bc * Ts_slower_loop
Cd = Cc
Dd = Dc
# Standard deviation of the measurement noise on position and angle
std_npos = get_parameter(sim_options, 'std_npos')
std_nphi = get_parameter(sim_options, 'std_nphi')
# Force disturbance
std_dF = get_parameter(sim_options, 'std_dF')
# Disturbance power spectrum
w_F = get_parameter(sim_options,
'w_F') # bandwidth of the force disturbance
tau_F = 1 / w_F
Hu = control.TransferFunction([1], [1 / w_F, 1])
Hu = Hu * Hu * Hu
Hud = control.matlab.c2d(Hu, Ts_PID)
N_sim_imp = tau_F / Ts_PID * 20
t_imp = np.arange(N_sim_imp) * Ts_PID
t, y = control.impulse_response(Hud, t_imp)
y = y[0]
std_tmp = np.sqrt(np.sum(y**2)) # np.sqrt(trapz(y**2,t))
Hu = Hu / (std_tmp) * std_dF
N_skip = 1 #int(20 * tau_F // Ts_PID) # skip initial samples to get a regime sample of d
t_sim_d = get_parameter(sim_options, 'len_sim') # simulation length (s)
N_sim_d = int(t_sim_d // Ts_PID)
N_sim_d = N_sim_d + N_skip + 1
e = np.random.randn(N_sim_d)
te = np.arange(N_sim_d) * Ts_PID
_, d, _ = control.forced_response(Hu, te, e)
d = d.ravel()
pos_ref = d[N_skip:]
# Initialize simulation system
t0 = 0
phi0 = np.pi #0.0*2*np.pi/360
x0 = np.array([0, 0, phi0, 0]) # initial state
# system_dyn = ode(f_ODE_wrapped).set_integrator('vode', method='bdf') # dopri5
system_dyn = ode(f_ODE_wrapped).set_integrator('dopri5')
system_dyn.set_initial_value(x0, t0)
system_dyn.set_f_params(0.0)
# Default controller parameters -
P = 30.0 #-100.0
#I = -1
#D = -10
#N = 100.0
kP = control.tf(P, 1, Ts_PID)
#kI = I * Ts_PID * control.tf([0, 1], [1, -1], Ts_PID)
#kD = D*control.tf([N, -N], [1.0, Ts_PID * N - 1], Ts_PID)
PID_tf = kP #+ kD + kI
PID_ss = control.ss(PID_tf)
k_PID = LinearStateSpaceSystem(A=PID_ss.A,
B=PID_ss.B,
C=PID_ss.C,
D=PID_ss.D)
# Simulate in closed loop
len_sim = get_parameter(sim_options, 'len_sim') # simulation length (s)
nsim = int(
len_sim // Ts_slower_loop
) #int(np.ceil(len_sim / Ts_slower_loop)) # simulation length(timesteps) # watch out! +1 added, is it correct?
t_vec = np.zeros((nsim, 1))
t_calc_vec = np.zeros(
(nsim, 1)) # computational time to get MPC solution (+ estimator)
status_vec = np.zeros((nsim, 1))
x_vec = np.zeros((nsim, nx))
x_ref_vec = np.zeros((nsim, nx))
y_vec = np.zeros((nsim, ny))
y_meas_vec = np.zeros((nsim, ny))
u_vec = np.zeros((nsim, nu))
nsim_fast = int(len_sim // Ts_PID)
t_vec_fast = np.zeros((nsim_fast, 1))
x_vec_fast = np.zeros(
(nsim_fast, nx)) # finer integration grid for performance evaluation
ref_angle_vec_fast = np.zeros((nsim_fast, 1))
y_meas_vec_fast = np.zeros((nsim_fast, ny))
x_ref_vec_fast = np.zeros(
(nsim_fast, nx)) # finer integration grid for performance evaluatio
u_vec_fast = np.zeros(
(nsim_fast, nu)) # finer integration grid for performance evaluatio
Fd_vec_fast = np.zeros((nsim_fast, nu)) #
t_int_vec_fast = np.zeros((nsim_fast, 1))
emergency_vec_fast = np.zeros((nsim_fast, 1)) #
t_step = t0
x_step = x0
u_PID = None
for idx_fast in range(nsim_fast):
## Determine step type: fast simulation only or MPC step
idx_inner_controller = idx_fast // ratio_Ts
run_inner_controller = (idx_fast % ratio_Ts) == 0
y_step = Cd.dot(x_step) # y[i] from the system
ymeas_step = np.copy(y_step)
ymeas_step[0] += std_npos * np.random.randn()
ymeas_step[1] += std_nphi * np.random.randn()
y_meas_vec_fast[idx_fast, :] = ymeas_step
# Output for step i
# Ts_slower_loop outputs
if run_inner_controller: # it is also a step of the simulation at rate Ts_slower_loop
if idx_inner_controller < nsim:
t_vec[idx_inner_controller, :] = t_step
y_vec[idx_inner_controller, :] = y_step
y_meas_vec[idx_inner_controller, :] = ymeas_step
u_vec[idx_inner_controller, :] = u_PID
# PID angle CONTROLLER
ref_pos = pos_ref[idx_fast]
error_angle = ref_pos - ymeas_step[0]
u_PID = k_PID.output(error_angle)
u_PID[u_PID > 10.0] = 10.0
u_PID[u_PID < -10.0] = -10.0
u_TOT = u_PID
# Ts_fast outputs
t_vec_fast[idx_fast, :] = t_step
x_vec_fast[idx_fast, :] = x_step #system_dyn.y
u_vec_fast[idx_fast, :] = u_TOT
Fd_vec_fast[idx_fast, :] = 0.0
ref_angle_vec_fast[idx_fast, :] = ref_pos
## Update to step i+1
k_PID.update(error_angle)
# Controller simulation step at rate Ts_slower_loop
if run_inner_controller:
pass
# System simulation step at rate Ts_fast
time_integrate_start = time.perf_counter()
system_dyn.set_f_params(u_TOT)
system_dyn.integrate(t_step + Ts_PID)
x_step = system_dyn.y
#x_step = x_step + f_ODE_jit(t_step, x_step, u_TOT)*Ts_fast
#x_step = x_step + f_ODE(0.0, x_step, u_TOT) * Ts_fast
t_int_vec_fast[
idx_fast, :] = time.perf_counter() - time_integrate_start
# Time update
t_step += Ts_PID
simout = {
't': t_vec,
'x': x_vec,
'u': u_vec,
'y': y_vec,
'y_meas': y_meas_vec,
'x_ref': x_ref_vec,
'status': status_vec,
'Fd_fast': Fd_vec_fast,
't_fast': t_vec_fast,
'x_fast': x_vec_fast,
'x_ref_fast': x_ref_vec_fast,
'u_fast': u_vec_fast,
'y_meas_fast': y_meas_vec_fast,
'emergency_fast': emergency_vec_fast,
'K': k_PID,
'nsim': nsim,
'Ts_slower_loop': Ts_slower_loop,
't_calc': t_calc_vec,
'ref_angle_fast': ref_angle_vec_fast,
't_int_fast': t_int_vec_fast
}
return simout
g_poles = co.pole(acbaef)
print("G poles")
for elem in g_poles:
print(f'{elem: .2f}')
g_zeros = co.zero(acbaef)
print("G zeros")
for elem in g_zeros:
print(f'{elem: .2f}')
g1_map = co.pzmap(acbaef, plot=True)
t = np.linspace(0, 25, 100)
t1, stp = co.step_response(acbaef, t)
t2, imp = co.impulse_response(acbaef, t) # Warning for infinite impulse at t=0
plt.figure(2)
plt.plot(t1, stp)
plt.title("Step Response")
plt.ylabel("Amplitude")
plt.xlabel("Time")
plt.grid()
plt.figure(3)
plt.plot(t2, imp)
plt.title("Impulse Response")
plt.ylabel("Amplitude")
plt.xlabel("Time")
plt.grid()
plt.show()
def SLS_synthesis2(atoms, m, b, k, freqs_bnd_yn=[1e-2, 255], mag_bnd_yn=[-10, -10],
freqs_bnd_T=[1e-2, 357], mag_bnd_T=[6, 6]):
""" The new (version 2) SLS synthesis procedure.
"""
keys = list(atoms.keys())
key0 = keys[0]
NT = atoms[key0].NT
dT = atoms[key0].dT
# optimization parameters
theta = cvx.Variable(len(atoms))
regularization = cvx.norm1(theta)
constraints = [cvx.sum(theta) == 1]
# desired impulse response
Tarr = np.arange(NT) * dT
desired_model = co.c2d(co.tf([1], [m, b, k]), dT)
_, H_desired = co.impulse_response(desired_model, Tarr, transpose=True)
H_desired = np.array(H_desired).flatten()
# affine controller's properties
H_affcomb = 0
H_fft_affcomb = 0
for i, key in enumerate(keys):
H_affcomb += theta[i] * atoms[key].H[:, 0]
H_fft_affcomb += theta[i] * atoms[key].H_fft
# list of frequencies for frequency-domain constraints
omegas = np.arange(int(NT / 2)) * 2 * np.pi / NT / dT
omegas[0] = 1e-2
# frequency-based upper bound for noise attenuation
wN_inv = np.ones(NT) * 100 # infinity
wN_inv[:int(NT / 2)] = np.power(
10, np.interp(np.log10(omegas), np.log10(freqs_bnd_yn), mag_bnd_yn) / 20)
# frequency-based upper bound on the complementary sensitivity
# function for robust stability
wT_inv = np.ones(NT) * 100 # infinity
wT_inv[:int(NT / 2)] = np.power(
10, np.interp(np.log10(omegas), np.log10(freqs_bnd_T), mag_bnd_T) / 20)
# add frequency-based constraints
constraints.append(cvx.abs(H_fft_affcomb[:, 0]) <= wN_inv)
constraints.append(cvx.abs(H_fft_affcomb[:, 1]) <= wT_inv)
# mimic desired system response
mse = cvx.norm(H_affcomb - H_desired)
obj = cvx.Minimize(mse + regularization)
constraints.append(
H_affcomb >= -1e-4
)
# opt problem
prob = cvx.Problem(obj, constraints)
prob.solve(verbose=True)
if prob.status == "optimal":
report_string = " -- Optimization successful!\n controller key | weight\n"
for i, key in enumerate(keys):
report_string += " {:10} | {:.3f}\n".format(key, theta[i].value)
print(report_string)
# form the affinely combined controller
atoms_list = [atoms[key] for key in keys]
res_ctrl = AtomicFIRController.init_as_convex_comb(theta.value, atoms_list)
return res_ctrl, None
else:
print(" -- Optimization fails! Returning None!")
return None, None
