plt.ylabel('position, degrees')
## define systems
x, v, u = dynamicsymbols('x v u')
l, m = sp.symbols('l m')
parameters = {l: 1, m: 1}
inertia = DynamicalSystem(state_equation=r_[v, u / (m * l**2)],
state=r_[x, v],
input_=u,
constants_values=parameters)
g = sp.symbols('g')
parameters[g] = 9.8
gravity = DynamicalSystem(output_equation=-g * m * l * sp.sin(x),
input_=x,
constants_values=parameters)
## put them together
BD = BlockDiagram(inertia, gravity)
BD.connect(gravity, inertia)
BD.connect(inertia, gravity, outputs=[0])
plt.figure()
plot_x(BD.simulate(8), 'first simulation!')
shape_figure()
plt.savefig("first_sim.pdf")
c_l = 2/(3*ft_per_m)
a_l = b_l
aero_model = simupy_flight.get_constant_aero(Cp_b=-1.0, Cq_b=-1.0, Cr_b=-1.0)
vehicle = simupy_flight.Vehicle(base_aero_coeffs=aero_model, m=m, I_xx=Ixx, I_yy=Iyy, I_zz=Izz, I_xy=Ixy, I_yz=Iyz, I_xz=Izx, x_com=x, y_com=y, z_com=z, x_mrc=x, y_mrc=y, z_mrc=z, S_A=S_A, a_l=a_l, b_l=b_l, c_l=c_l, d_l=0.,)
BD = BlockDiagram(planet, vehicle)
BD.connect(planet, vehicle, inputs=np.arange(planet.dim_output))
BD.connect(vehicle, planet, inputs=np.arange(vehicle.dim_output))
lat_ic = 0.*np.pi/180
long_ic = 0.*np.pi/180
h_ic = 30_000/ft_per_m
V_N_ic = 0.
V_E_ic = 0.
V_D_ic = 0.
psi_ic = 0.*np.pi/180
theta_ic = 0.*np.pi/180
phi_ic = 0.*np.pi/180
omega_X_ic = 10.*np.pi/180
omega_Y_ic = 20.*np.pi/180
omega_Z_ic = 30.*np.pi/180
planet.initial_condition = planet.ic_from_planetodetic(long_ic, lat_ic, h_ic, V_N_ic, V_E_ic, V_D_ic, psi_ic, theta_ic, phi_ic)
planet.initial_condition[-3:] = omega_X_ic, omega_Y_ic, omega_Z_ic
with benchmark() as b:
res = BD.simulate(30, integrator_options=int_opts)
b.tfinal = res.t[-1]
plot_nesc_comparisons(res, '03')
state=r_[x,v]
input_u,
constants_values=parameters
)
g=sp.symbols('g')
parameters[g]=9.8
gravity=DynamicalSystem(
output_equation=-g*m*l*sp.sin(x),
input=x,
constants_values=parameters))
##put them together
BD=BlockDiagram(inertia, gravity)
BD.connect(gravity,inertia)
BD.connect(inertia,gravity,outputs=[0])
plt.figure()
plot_x(BD.simulate(8), 'first simulation!')
shape_figure()
plt.savefig("first_sim.pdf")
#lift the pedulum, plot!
inertia.initial_condition=np.r_[60*np.pi/180,0]
plot_x(BD.simulate(8),'$x(0)$=60')
shape_figure()
plt.savefig("really.pdf")
B_aug,
)
# construct PID system
Kc = 1
tau_I = 1
tau_D = 1
K = -np.r_[Kc / tau_I, Kc, Kc * tau_D].reshape((1, 3))
pid = LTISystem(K)
# construct reference
ref = SystemFromCallable(lambda *args: np.ones(1), 0, 1)
# create block diagram
BD = BlockDiagram(aug_sys, pid, ref)
BD.connect(aug_sys, pid) # PID requires feedback
BD.connect(pid, aug_sys, inputs=[0]) # PID output to system control input
BD.connect(ref, aug_sys,
inputs=[1]) # reference output to system command input
res = BD.simulate(10) # simulate
# plot
plt.figure()
plt.plot(res.t, res.y[:, 0], label=r'$\int x$')
plt.plot(res.t, res.y[:, 1], label='$x$')
plt.plot(res.t, res.y[:, 2], label=r'$\dot{x}$')
plt.plot(res.t, res.y[:, 3], label='$u$')
plt.plot(res.t, res.y[:, 4], label='$x_c$')
plt.legend()
plt.show()
Sd = linalg.solve_discrete_are(
Ad,
Bd,
Q,
R,
)
Kd = linalg.solve(Bd.T @ Sd @ Bd + R, Bd.T @ Sd @ Ad)
dt_ctr = LTISystem(-Kd, dt=dT)
# Equality of discrete-time equivalent and original continuous-time
# system
dtct_bd = BlockDiagram(ct_sys, dt_ctr)
dtct_bd.connect(ct_sys, dt_ctr)
dtct_bd.connect(dt_ctr, ct_sys)
dtct_res = dtct_bd.simulate(Tsim)
dtdt_bd = BlockDiagram(dt_sys, dt_ctr)
dtdt_bd.connect(dt_sys, dt_ctr)
dtdt_bd.connect(dt_ctr, dt_sys)
dtdt_res = dtdt_bd.simulate(Tsim)
plt.figure()
for st in range(n):
plt.subplot(n + m, 1, st + 1)
plt.plot(dtct_res.t, dtct_res.y[:, st], '+-')
plt.stem(dtdt_res.t,
dtdt_res.y[:, st],
linefmt='-C1',
markerfmt='xC1',
basefmt='C1-')
# construct systems from matrix differential equations and reference
SG_sys = system_from_matrix_DE(SGdot, SG, rxs, SG_subs)
def ref_input_ctr(t, *args):
return np.r_[2 * (tF - t), 0]
ref_input_ctr_sys = SystemFromCallable(ref_input_ctr, 0, 2)
# simulate matrix differential equation with reference input
tF = 20
RiccatiBD = BlockDiagram(SG_sys, ref_input_ctr_sys)
RiccatiBD.connect(ref_input_ctr_sys, SG_sys)
sg_sim_res = RiccatiBD.simulate(tF)
# create callable to interpolate simulation results
sim_data_unique_t, sim_data_unique_t_idx = np.unique(sg_sim_res.t,
return_index=True)
mat_sg_result = array_callable_from_vector_trajectory(
np.flipud(tF - sg_sim_res.t[sim_data_unique_t_idx]),
np.flipud(sg_sim_res.x[sim_data_unique_t_idx]), SG_sys.state, SG)
vec_sg_result = array_callable_from_vector_trajectory(
np.flipud(tF - sg_sim_res.t[sim_data_unique_t_idx]),
np.flipud(sg_sim_res.x[sim_data_unique_t_idx]), SG_sys.state, SG_sys.state)
# Plot S components
plt.figure()
plt.plot()
A,
B,
Q,
R,
)
K = linalg.solve(R, B.T @ S).reshape(1, -1)
ctr_sys = LTISystem(-K)
# Construct block diagram
BD = BlockDiagram(sys, ctr_sys)
BD.connect(sys, ctr_sys)
BD.connect(ctr_sys, sys)
# case 1 - un-recoverable
sys.initial_condition = np.r_[1, 1, 2.25]
result1 = BD.simulate(tF)
plt.figure()
plt.plot(result1.t, result1.y)
plt.legend(legends)
plt.title('controlled system with unstable initial conditions')
plt.xlabel('$t$, s')
plt.tight_layout()
plt.show()
# case 2 - recoverable
sys.initial_condition = np.r_[5, -3, 1]
result2 = BD.simulate(tF)
plt.figure()
plt.plot(result2.t, result2.y)
plt.legend(legends)
plt.title('controlled system with stable initial conditions')
x = x1, x2 = Array(dynamicsymbols('x1:3'))
mu = sp.symbols('mu')
state_equation = r_[x2, -x1 + mu * (1 - x1**2) * x2]
output_equation = r_[x1**2 + x2**2, sp.atan2(x2, x1)]
sys = DynamicalSystem(state_equation,
x,
output_equation=output_equation,
constants_values={mu: 5})
sys.initial_condition = np.array([1, 1]).T
BD = BlockDiagram(sys)
res = BD.simulate(30)
plt.figure()
plt.plot(res.t, res.x)
plt.legend([sp.latex(s, mode='inline') for s in sys.state])
plt.ylabel('$x_i(t)$')
plt.xlabel('$t$, s')
plt.title('system state vs time')
plt.tight_layout()
plt.show()
plt.figure()
plt.plot(*res.x.T)
plt.xlabel('$x_1(t)$')
plt.ylabel('$x_2(t)$')
plt.title('phase portrait of system')
from simupy.array import Array, r_
from simupy.discontinuities import SwitchedOutput
plt.ion()
# This example shows how to implement a simple saturation block
llim = -0.75
ulim = 0.75
x = Array([dynamicsymbols('x')])
tvar = dynamicsymbols._t
sin = DynamicalSystem(Array([sp.cos(tvar)]), x)
sin_bd = BlockDiagram(sin)
sin_res = sin_bd.simulate(2 * np.pi)
plt.figure()
plt.plot(sin_res.t, sin_res.x)
limit = r_[llim, ulim]
saturation_output = r_['0,2', llim, x[0], ulim]
sat = SwitchedOutput(x[0], limit, output_equations=saturation_output, input_=x)
sat_bd = BlockDiagram(sin, sat)
sat_bd.connect(sin, sat)
sat_res = sat_bd.simulate(2 * np.pi)
plt.plot(sat_res.t, sat_res.y[:, -1])
plt.xlabel('$t$, s')
plt.ylabel('$x(t)$')