# design for state feedback gain to stabilize system P
regulator_poles = [-5 + 5j, -5 - 5j]
F = -acker(P.A, P.B, regulator_poles)
# true behavior of the state
Gsf = ss(P.A + P.B * F, P.B, np.eye(2), [[0], [0]])
x, t = initial(Gsf, Td, X0)
ax[0].plot(t, x[:, 0], ls='-.', label='${x}_1$')
ax[1].plot(t, x[:, 1], ls='-.', label='${x}_2$')
# behaviour that is estimated by observer
# input : u = F * x
u = [[F[0, 0] * x[i, 0] + F[0, 1] * x[i, 1]] for i in range(len(x))]
# output :y = C * x + d
y = x[:, 0] + d
# state estimation by observer
Obs = ss(P.A + L * P.C, np.c_[P.B, -L], np.eye(2), [[0, 0], [0, 0]])
xhat, t, x0 = lsim(Obs, np.c_[u, y], Td, [0, 0])
ax[0].plot(t, xhat[:, 0], label='$\hat{x}_1$')
ax[1].plot(t, xhat[:, 1], label='$\hat{x}_2$')
for i in [0, 1]:
func.plot_set(ax[i], 't', '', 'best')
ax[0].set_ylabel('$x_1, \hat{x}_1$')
ax[1].set_ylabel('$x_2, \hat{x}_2$')
fig.tight_layout()
plt.show()
# design for observer gains [correspondents to state feedbacks]
L = -acker(P.A.T, P.C.T, observer_poles).T
# design for state feedback gain to stabilize system P
regulator_poles = [-5 + 5j, -5 - 5j]
F = -acker(P.A, P.B, regulator_poles)
# output feedback (observer + state feedback)
K = ss(P.A + P.B * F + L * P.C, -L, F, 0)
print('K : \n', K)
print('----------------')
print('K(s) = ', tf(K))
# output feedbacked system
Gfb = feedback(P, K, sign=1)
fig, ax = plt.subplots()
Td = np.arange(0, 3, 0.01)
# without output feedback
y, t = initial(P, Td, [-1, 0.5])
ax.plot(t, y, ls='-.', label='w/o controller', color='k')
# with output feedback
y, t = initial(Gfb, Td, [-1, 0.5, 0, 0])
ax.plot(t, y, label='w/ controller', color='k')
func.plot_set(ax, 't', 'y', 'best')
plt.show()
gain, _, w = bode(Tsys, logspace(-3, 3), Plot=False)
ax[1].semilogx(w, 20 * np.log10(gain), ls='-', lw=2, label='$T$')
gain, _, w = bode(1 / WT, logspace(-3, 3), Plot=False)
ax[1].semilogx(w, 20 * np.log10(gain), ls='--', label='$1/W_T$')
for i in range(2):
ax[i].set_ylim(-40, 40)
ax[i].legend()
ax[i].grid(which="both", ls=':')
ax[i].set_ylabel('Gain [dB]')
ax[i].set_xlabel('$\omega$ [rad/s]')
# step response of the controller
fig, ax2 = plt.subplots()
# performance to model with uncertainty
for i in range(len(delta)):
P = (1 + WT * delta[i]) * Pn
Gyr = feedback(P * K, 1)
y, t = step(Gyr, np.arange(0, 5, 0.01))
ax2.plot(t, y * ref, color='k', lw=0.3)
# performance to nominal model
Gyr = feedback(Pn * K, 1)
y, t = step(Gyr, np.arange(0, 5, 0.01))
ax2.plot(t, y * ref, lw=2, color='k')
func.plot_set(ax2, 't', 'y')
fig.tight_layout()
plt.show()
P = tf([0, 1], [0.5, 1])
print(P)
ts = 0.2 # sampling time
fig, ax = plt.subplots(1, 2)
# digitalizing by 0th order hold
Pd1 = c2d(P, ts, method='zoh')
print('digitalized time system (zoh)', Pd1)
T = np.arange(0, 3, ts)
y, t = step(Pd1, T)
ax[0].plot(t, y, ls='', marker='o', label='zoh')
# digitalizing by bilinear transform
Pd2 = c2d(P, ts, method='tustin')
print('digitalized time system (tustin)', Pd2)
T = np.arange(0, 3, ts)
y, t = step(Pd2, T)
ax[1].plot(t, y, ls='', marker='o', label='tustin')
# continuous time system
Tc = np.arange(0, 3, 0.01)
y, t = step(P, Tc)
ax[0].plot(t, y, ls='-.')
ax[1].plot(t, y, ls='-.')
func.plot_set(ax[0], 't', 'y', 'best')
func.plot_set(ax[1], 't', 'y', 'best')
plt.show()