Optimal_Control_Report/oc_solution.py at main · SENATOROVAI/Optimal_Control_Report · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
import numpy as np
from scipy.integrate import solve_ivp
from scipy.linalg import solve_continuous_are
import matplotlib.pyplot as plt

A = np.array([[0.0, 1.0], [0.0, 0.0]])
B = np.array([[0.0], [1.0]])
Q = np.eye(2)
R = np.array([[1.0]])
F = np.eye(2)
tf = 5.0
x0 = np.array([1.0, 0.0])
Ri = np.linalg.inv(R)
S = B @ Ri @ B.T

def vec(P):
    return np.array([P[0, 0], P[0, 1], P[1, 1]])

def mat(p):
    return np.array([[p[0], p[1]], [p[1], p[2]]])

def rde(t, p):
    P = mat(p)
    dP = -(A.T @ P + P @ A - P @ S @ P + Q)
    return vec(dP)

t_grid = np.linspace(tf, 0.0, 501)
sol = solve_ivp(rde, [tf, 0.0], vec(F), t_eval=t_grid, dense_output=True, rtol=1e-9, atol=1e-12)
t_fwd = t_grid[::-1]
P11 = sol.y[0][::-1]
P12 = sol.y[1][::-1]
P22 = sol.y[2][::-1]

P0 = mat(sol.sol(0.0))
Jopt = float(x0 @ P0 @ x0)

def K_of_t(t):
    return Ri @ B.T @ mat(sol.sol(t))

def closed(t, x):
    u = -(K_of_t(t) @ x)
    return (A @ x + (B @ u)).ravel()

st = solve_ivp(closed, [0.0, tf], x0, t_eval=np.linspace(0.0, tf, 501), rtol=1e-9, atol=1e-12)
xt = st.y
ut = np.array([-(K_of_t(tt) @ st.y[:, i]).item() for i, tt in enumerate(st.t)])

Pss = solve_continuous_are(A, B, Q, R)
Kss = Ri @ B.T @ Pss
Acl = A - B @ Kss
w = np.logspace(-2, 2, 800)
sig = []
for wi in w:
    G = np.linalg.solve(1j * wi * np.eye(2) - Acl, B)
    s = np.linalg.svd(G, compute_uv=False)
    sig.append(s[0])
sig = np.array(sig)

x2_fixed = 1.0
x1_free = -P0[0, 1] * x2_fixed / P0[0, 0]
x_free = np.array([x1_free, x2_fixed])
J_free = float(x_free @ P0 @ x_free)
J_fixed0 = float(np.array([0.0, x2_fixed]) @ P0 @ np.array([0.0, x2_fixed]))

print("P(0)=", np.array_str(P0, precision=4))
print("Kss=", np.array_str(Kss, precision=4))
print("eig(Acl)=", np.array_str(np.linalg.eigvals(Acl), precision=4))
print("Jopt=", round(Jopt, 6))
print("x1_free*=", round(x1_free, 4), "J_free=", round(J_free, 4), "J_fixed0=", round(J_fixed0, 4))
print("sigma_max=", round(float(sig.max()), 4), "at w=", round(float(w[np.argmax(sig)]), 4))

plt.figure(figsize=(6.2, 4.0))
plt.plot(t_fwd, P11, label=r"$P_{11}(t)$")
plt.plot(t_fwd, P12, label=r"$P_{12}(t)$")
plt.plot(t_fwd, P22, label=r"$P_{22}(t)$")
plt.xlabel("t, s")
plt.ylabel("Riccati matrix entries")
plt.title("Solution of the differential Riccati equation $P(t)$")
plt.legend()
plt.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig("fig_P.png", dpi=180)
plt.close()

plt.figure(figsize=(6.2, 4.0))
plt.plot(st.t, xt[0], label=r"$x_1(t)$")
plt.plot(st.t, xt[1], label=r"$x_2(t)$")
plt.plot(st.t, ut, "--", label=r"$u^*(t)$")
plt.xlabel("t, s")
plt.ylabel("state / control")
plt.title("Optimal closed-loop response and control")
plt.legend()
plt.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig("fig_xu.png", dpi=180)
plt.close()

plt.figure(figsize=(6.2, 4.0))
plt.semilogx(w, 20 * np.log10(sig))
plt.xlabel(r"frequency $\omega$, rad/s")
plt.ylabel(r"$\sigma(\omega)$, dB")
plt.title(r"Singular value plot of $G(j\omega)=(j\omega I-(A+BK))^{-1}B$")
plt.grid(True, which="both", alpha=0.3)
plt.tight_layout()
plt.savefig("fig_sigma.png", dpi=180)
plt.close()