DirectSD-Python 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- directsd/__init__.py +141 -0
- directsd/analysis/__init__.py +4 -0
- directsd/analysis/charpol.py +89 -0
- directsd/analysis/errors.py +185 -0
- directsd/analysis/norms.py +630 -0
- directsd/design/__init__.py +9 -0
- directsd/design/convex.py +620 -0
- directsd/design/lifting.py +403 -0
- directsd/design/polynomial.py +5925 -0
- directsd/examples/__init__.py +0 -0
- directsd/examples/_common.py +36 -0
- directsd/examples/demos.py +1161 -0
- directsd/examples/examples.py +296 -0
- directsd/examples/help_examples.py +1043 -0
- directsd/glopt/__init__.py +4 -0
- directsd/glopt/advanced.py +1658 -0
- directsd/glopt/optimize.py +420 -0
- directsd/linalg/__init__.py +3 -0
- directsd/linalg/linsys.py +66 -0
- directsd/linalg/matrices.py +156 -0
- directsd/linalg/minreal.py +425 -0
- directsd/linalg/riccati.py +197 -0
- directsd/polynomial/__init__.py +11 -0
- directsd/polynomial/diophantine.py +426 -0
- directsd/polynomial/operations.py +247 -0
- directsd/polynomial/poln.py +742 -0
- directsd/polynomial/spectral.py +368 -0
- directsd/polynomial/transforms.py +124 -0
- directsd/polynomial/utils.py +449 -0
- directsd/sspace/__init__.py +4 -0
- directsd/sspace/design.py +1083 -0
- directsd/sspace/plant.py +198 -0
- directsd/tf/__init__.py +9 -0
- directsd/tf/interconnect.py +105 -0
- directsd/zpk/__init__.py +1 -0
- directsd/zpk/zpk.py +400 -0
- directsd_python-0.1.0.dist-info/METADATA +450 -0
- directsd_python-0.1.0.dist-info/RECORD +41 -0
- directsd_python-0.1.0.dist-info/WHEEL +5 -0
- directsd_python-0.1.0.dist-info/licenses/LICENSE +36 -0
- directsd_python-0.1.0.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,620 @@
|
|
|
1
|
+
"""
|
|
2
|
+
directsd.design.convex
|
|
3
|
+
======================
|
|
4
|
+
Convex optimisation-based controller synthesis for sampled-data systems.
|
|
5
|
+
|
|
6
|
+
Uses CVXPY to formulate and solve problems that have no closed-form
|
|
7
|
+
polynomial/Riccati solution:
|
|
8
|
+
|
|
9
|
+
• L1 (peak-to-peak) optimal design
|
|
10
|
+
• Mixed H2 / L1 design (energy + amplitude)
|
|
11
|
+
• Template / envelope matching (hard time-domain constraints)
|
|
12
|
+
• General Q-parameterisation (Youla) basis for custom objectives
|
|
13
|
+
|
|
14
|
+
All methods follow the same lifting → Youla → convex program workflow:
|
|
15
|
+
|
|
16
|
+
1. Lift the continuous plant to an exact discrete equivalent via lift_h2.
|
|
17
|
+
2. Compute a stabilising base controller (H2-optimal via h2reg).
|
|
18
|
+
3. Parameterise all stabilising controllers via a stable FIR parameter Q.
|
|
19
|
+
4. Write the closed-loop map as a *linear* function of Q.
|
|
20
|
+
5. Minimise the desired norm / cost subject to constraints.
|
|
21
|
+
|
|
22
|
+
Install the optional dependency before use::
|
|
23
|
+
|
|
24
|
+
pip install cvxpy
|
|
25
|
+
|
|
26
|
+
References
|
|
27
|
+
----------
|
|
28
|
+
[1] Dahleh & Diaz-Bobillo, "Control of Uncertain Systems: A Linear
|
|
29
|
+
Programming Approach," Prentice-Hall, 1995.
|
|
30
|
+
[2] Chen & Francis, "Optimal Sampled-Data Control Systems," Springer, 1995.
|
|
31
|
+
[3] Khammash, "A new approach to the solution of the l1 control problem:
|
|
32
|
+
the scaled-Q method," IEEE TAC, 2000.
|
|
33
|
+
"""
|
|
34
|
+
|
|
35
|
+
import warnings
|
|
36
|
+
|
|
37
|
+
import numpy as np
|
|
38
|
+
import scipy.linalg as la
|
|
39
|
+
import scipy.signal as sig
|
|
40
|
+
|
|
41
|
+
# ── optional CVXPY import ────────────────────────────────────────────────────
|
|
42
|
+
try:
|
|
43
|
+
import cvxpy as cp
|
|
44
|
+
_CVXPY_AVAILABLE = True
|
|
45
|
+
except ImportError:
|
|
46
|
+
_CVXPY_AVAILABLE = False
|
|
47
|
+
|
|
48
|
+
|
|
49
|
+
def _require_cvxpy():
|
|
50
|
+
"""Raise ImportError if cvxpy is not installed."""
|
|
51
|
+
if not _CVXPY_AVAILABLE:
|
|
52
|
+
raise ImportError(
|
|
53
|
+
"cvxpy is required for convex synthesis.\n"
|
|
54
|
+
"Install it with: pip install cvxpy"
|
|
55
|
+
)
|
|
56
|
+
|
|
57
|
+
|
|
58
|
+
# ── Toeplitz helpers ─────────────────────────────────────────────────────────
|
|
59
|
+
|
|
60
|
+
def _build_toeplitz_cvxpy(q_vec, N_fir, nc, nm):
|
|
61
|
+
"""
|
|
62
|
+
Build a CVXPY lower-triangular block-Toeplitz matrix from FIR coefficients.
|
|
63
|
+
|
|
64
|
+
Parameters
|
|
65
|
+
----------
|
|
66
|
+
q_vec : cp.Variable shape (N_fir * nc * nm,)
|
|
67
|
+
N_fir : int
|
|
68
|
+
nc : int n_ctrl
|
|
69
|
+
nm : int n_meas
|
|
70
|
+
|
|
71
|
+
Returns
|
|
72
|
+
-------
|
|
73
|
+
Q_mat : cp.Expression shape (N_fir*nc, N_fir*nm)
|
|
74
|
+
"""
|
|
75
|
+
blocks = []
|
|
76
|
+
for i in range(N_fir):
|
|
77
|
+
row_blocks = []
|
|
78
|
+
for j in range(N_fir):
|
|
79
|
+
lag = i - j
|
|
80
|
+
if lag < 0:
|
|
81
|
+
row_blocks.append(np.zeros((nc, nm)))
|
|
82
|
+
else:
|
|
83
|
+
start = lag * nc * nm
|
|
84
|
+
q_block = cp.reshape(
|
|
85
|
+
q_vec[start: start + nc * nm], (nc, nm), order='C'
|
|
86
|
+
)
|
|
87
|
+
row_blocks.append(q_block)
|
|
88
|
+
blocks.append(row_blocks)
|
|
89
|
+
return cp.bmat(blocks)
|
|
90
|
+
|
|
91
|
+
|
|
92
|
+
def _build_toeplitz_np(Q_fir, nc, nm):
|
|
93
|
+
"""
|
|
94
|
+
Build a numpy lower-triangular block-Toeplitz matrix from solved Q.
|
|
95
|
+
|
|
96
|
+
Parameters
|
|
97
|
+
----------
|
|
98
|
+
Q_fir : np.ndarray shape (N_fir, nc, nm)
|
|
99
|
+
nc, nm : int
|
|
100
|
+
|
|
101
|
+
Returns
|
|
102
|
+
-------
|
|
103
|
+
T : np.ndarray shape (N_fir*nc, N_fir*nm)
|
|
104
|
+
"""
|
|
105
|
+
N_fir = Q_fir.shape[0]
|
|
106
|
+
T = np.zeros((N_fir * nc, N_fir * nm))
|
|
107
|
+
for i in range(N_fir):
|
|
108
|
+
for j in range(N_fir):
|
|
109
|
+
lag = i - j
|
|
110
|
+
if lag >= 0:
|
|
111
|
+
T[i*nc:(i+1)*nc, j*nm:(j+1)*nm] = Q_fir[lag]
|
|
112
|
+
return T
|
|
113
|
+
|
|
114
|
+
|
|
115
|
+
# ── Impulse-response helpers ─────────────────────────────────────────────────
|
|
116
|
+
|
|
117
|
+
def _impulse_response(A, B, C, D, N):
|
|
118
|
+
"""
|
|
119
|
+
Compute the first N samples of the discrete impulse response.
|
|
120
|
+
|
|
121
|
+
Returns h : (N, n_out, n_in) array.
|
|
122
|
+
"""
|
|
123
|
+
n_out, n_in = D.shape
|
|
124
|
+
h = np.zeros((N, n_out, n_in))
|
|
125
|
+
h[0] = D
|
|
126
|
+
x = B.copy()
|
|
127
|
+
for k in range(1, N):
|
|
128
|
+
h[k] = C @ x
|
|
129
|
+
x = A @ x
|
|
130
|
+
return h
|
|
131
|
+
|
|
132
|
+
|
|
133
|
+
def _ss_impulse_matrix(A, B, C, D, N):
|
|
134
|
+
"""
|
|
135
|
+
Build the lower-triangular convolution (Toeplitz) matrix of shape
|
|
136
|
+
(N*n_out, N*n_in) such that vec(y) = T_cl @ vec(u).
|
|
137
|
+
"""
|
|
138
|
+
h = _impulse_response(A, B, C, D, N)
|
|
139
|
+
N, p, m = h.shape
|
|
140
|
+
T = np.zeros((N * p, N * m))
|
|
141
|
+
for i in range(N):
|
|
142
|
+
for j in range(i + 1):
|
|
143
|
+
T[i*p:(i+1)*p, j*m:(j+1)*m] = h[i - j]
|
|
144
|
+
return T
|
|
145
|
+
|
|
146
|
+
|
|
147
|
+
# ── Youla internals ──────────────────────────────────────────────────────────
|
|
148
|
+
|
|
149
|
+
def _stabilising_h2(dsys, n_meas, n_ctrl):
|
|
150
|
+
"""Return H2-optimal base controller K0."""
|
|
151
|
+
from directsd.sspace.design import h2reg
|
|
152
|
+
K0, _ = h2reg(dsys, n_meas=n_meas, n_ctrl=n_ctrl)
|
|
153
|
+
return K0
|
|
154
|
+
|
|
155
|
+
|
|
156
|
+
def _youla_maps(dsys, K0, n_meas, n_ctrl, N_fir):
|
|
157
|
+
"""
|
|
158
|
+
Compute the three closed-loop maps for Q-parameterisation.
|
|
159
|
+
|
|
160
|
+
All stabilising controllers: K = K0 + Δ(Q)
|
|
161
|
+
|
|
162
|
+
For a discrete plant P = [[P11, P12], [P21, P22]] and base
|
|
163
|
+
controller K0, the closed-loop w→z map is:
|
|
164
|
+
|
|
165
|
+
T(Q) = T_cl + Phi12 @ Q_mat @ Phi21
|
|
166
|
+
|
|
167
|
+
Returns
|
|
168
|
+
-------
|
|
169
|
+
T_cl : (N*p, N*m) closed-loop map with K0
|
|
170
|
+
Phi12 : (N*p, N_fir*n_ctrl) input map for Q
|
|
171
|
+
Phi21 : (N_fir*n_meas, N*m) output map for Q
|
|
172
|
+
N : int total impulse-response horizon used
|
|
173
|
+
"""
|
|
174
|
+
A = dsys.A; B = dsys.B; C = dsys.C; D = dsys.D
|
|
175
|
+
nout, nin = C.shape[0], B.shape[1]
|
|
176
|
+
i1 = nin - n_ctrl
|
|
177
|
+
o1 = nout - n_meas
|
|
178
|
+
|
|
179
|
+
B1 = B[:, :i1]; B2 = B[:, i1:]
|
|
180
|
+
C1 = C[:o1, :]; C2 = C[o1:, :]
|
|
181
|
+
D11 = D[:o1, :i1]; D12 = D[:o1, i1:]
|
|
182
|
+
D21 = D[o1:, :i1]; D22 = D[o1:, i1:]
|
|
183
|
+
|
|
184
|
+
Ak, Bk, Ck, Dk = K0.A, K0.B, K0.C, K0.D
|
|
185
|
+
|
|
186
|
+
n = A.shape[0]
|
|
187
|
+
nk = Ak.shape[0]
|
|
188
|
+
|
|
189
|
+
Icl = np.eye(n_ctrl) - Dk @ D22
|
|
190
|
+
Jcl = np.eye(n_meas) - D22 @ Dk
|
|
191
|
+
|
|
192
|
+
A_cl = np.block([
|
|
193
|
+
[A + B2 @ np.linalg.solve(Icl, Dk @ C2),
|
|
194
|
+
B2 @ np.linalg.solve(Icl, Ck)],
|
|
195
|
+
[Bk @ np.linalg.solve(Jcl, C2),
|
|
196
|
+
Ak + Bk @ np.linalg.solve(Jcl, D22 @ Ck)]
|
|
197
|
+
])
|
|
198
|
+
B_cl = np.vstack([
|
|
199
|
+
B1 + B2 @ np.linalg.solve(Icl, Dk @ D21),
|
|
200
|
+
Bk @ np.linalg.solve(Jcl, D21)
|
|
201
|
+
])
|
|
202
|
+
C_cl = np.hstack([
|
|
203
|
+
C1 + D12 @ np.linalg.solve(Icl, Dk @ C2),
|
|
204
|
+
D12 @ np.linalg.solve(Icl, Ck)
|
|
205
|
+
])
|
|
206
|
+
D_cl = D11 + D12 @ np.linalg.solve(Icl, Dk @ D21)
|
|
207
|
+
|
|
208
|
+
N = N_fir + n + nk
|
|
209
|
+
T_cl = _ss_impulse_matrix(A_cl, B_cl, C_cl, D_cl, N)
|
|
210
|
+
|
|
211
|
+
# P12 closed-loop: from Q-input (ctrl-dim) to z-output
|
|
212
|
+
B_12 = np.vstack([
|
|
213
|
+
B2 @ np.linalg.solve(Icl, np.eye(n_ctrl)),
|
|
214
|
+
Bk @ np.linalg.solve(Jcl, D22 @ np.eye(n_ctrl))
|
|
215
|
+
])
|
|
216
|
+
C_12 = np.hstack([
|
|
217
|
+
C1 + D12 @ np.linalg.solve(Icl, Dk @ C2),
|
|
218
|
+
D12 @ np.linalg.solve(Icl, Ck)
|
|
219
|
+
])
|
|
220
|
+
D_12 = D12 @ np.linalg.solve(Icl, np.eye(n_ctrl))
|
|
221
|
+
Phi12 = _ss_impulse_matrix(A_cl, B_12, C_12, D_12, N)[:, :N_fir * n_ctrl]
|
|
222
|
+
|
|
223
|
+
# P21 closed-loop: from w-input to Q-output (meas-dim)
|
|
224
|
+
B_21 = np.vstack([
|
|
225
|
+
B1 + B2 @ np.linalg.solve(Icl, Dk @ D21),
|
|
226
|
+
Bk @ np.linalg.solve(Jcl, D21)
|
|
227
|
+
])
|
|
228
|
+
C_21 = np.hstack([
|
|
229
|
+
np.linalg.solve(Jcl, C2),
|
|
230
|
+
np.linalg.solve(Jcl, D22 @ Ck)
|
|
231
|
+
])
|
|
232
|
+
D_21 = np.linalg.solve(Jcl, D21)
|
|
233
|
+
Phi21 = _ss_impulse_matrix(A_cl, B_21, C_21, D_21, N)[:N_fir * n_meas, :]
|
|
234
|
+
|
|
235
|
+
return T_cl, Phi12, Phi21, N
|
|
236
|
+
|
|
237
|
+
|
|
238
|
+
# ── Internal: reconstruct controller from FIR Q ──────────────────────────────
|
|
239
|
+
|
|
240
|
+
def _fir_controller_from_q(K0, Q_fir, dsys, n_meas, n_ctrl):
|
|
241
|
+
"""
|
|
242
|
+
Build the final controller as K0 plus a parallel FIR correction Q.
|
|
243
|
+
|
|
244
|
+
The full Youla reconstruction K = K0 + Δ(Q) is approximated here as a
|
|
245
|
+
parallel connection K_total = K0 + Q (valid when ‖Q‖ is small relative
|
|
246
|
+
to K0, i.e. close to the H2-optimal base).
|
|
247
|
+
"""
|
|
248
|
+
if np.all(np.abs(Q_fir) < 1e-10):
|
|
249
|
+
return K0
|
|
250
|
+
|
|
251
|
+
N_fir, nc, nm = Q_fir.shape
|
|
252
|
+
dt = K0.dt if K0.dt is not None else 1.0
|
|
253
|
+
|
|
254
|
+
# FIR state-space: shift register of depth (N_fir-1)*nm
|
|
255
|
+
n_fir_state = (N_fir - 1) * nm
|
|
256
|
+
A_fir = np.zeros((n_fir_state, n_fir_state))
|
|
257
|
+
for i in range(N_fir - 2):
|
|
258
|
+
A_fir[nm*(i+1):nm*(i+2), nm*i:nm*(i+1)] = np.eye(nm)
|
|
259
|
+
B_fir = np.zeros((n_fir_state, nm))
|
|
260
|
+
B_fir[:nm, :] = np.eye(nm)
|
|
261
|
+
C_fir = np.hstack([Q_fir[k] for k in range(1, N_fir)]) if N_fir > 1 \
|
|
262
|
+
else np.zeros((nc, n_fir_state))
|
|
263
|
+
D_fir = Q_fir[0]
|
|
264
|
+
|
|
265
|
+
A_tot = la.block_diag(K0.A, A_fir)
|
|
266
|
+
B_tot = np.vstack([K0.B, B_fir])
|
|
267
|
+
C_tot = np.hstack([K0.C, C_fir])
|
|
268
|
+
D_tot = K0.D + D_fir
|
|
269
|
+
|
|
270
|
+
return sig.StateSpace(A_tot, B_tot, C_tot, D_tot, dt=dt)
|
|
271
|
+
|
|
272
|
+
|
|
273
|
+
# ── Public API ────────────────────────────────────────────────────────────────
|
|
274
|
+
|
|
275
|
+
def youla_basis(dsys, K0, N_fir, n_meas=1, n_ctrl=1):
|
|
276
|
+
"""
|
|
277
|
+
Compute the Youla (Q) parameterisation basis matrices.
|
|
278
|
+
|
|
279
|
+
Every stabilising controller for the discrete plant ``dsys`` can be
|
|
280
|
+
written as K = K0 + Δ(Q) where Q is a stable FIR sequence of length
|
|
281
|
+
N_fir. The closed-loop w→z map is *affine* in the coefficients of Q:
|
|
282
|
+
|
|
283
|
+
T(Q) = T_cl + Phi12 @ diag_block(Q) @ Phi21
|
|
284
|
+
|
|
285
|
+
Parameters
|
|
286
|
+
----------
|
|
287
|
+
dsys : scipy.signal.StateSpace (discrete)
|
|
288
|
+
Lifted discrete-time generalised plant.
|
|
289
|
+
K0 : scipy.signal.StateSpace (discrete)
|
|
290
|
+
Base stabilising controller (typically H2-optimal).
|
|
291
|
+
N_fir : int
|
|
292
|
+
FIR horizon — number of Q taps.
|
|
293
|
+
n_meas, n_ctrl : int
|
|
294
|
+
|
|
295
|
+
Returns
|
|
296
|
+
-------
|
|
297
|
+
T_cl : np.ndarray (N*p, N*m) closed-loop impulse-response matrix
|
|
298
|
+
Phi12 : np.ndarray (N*p, N_fir*n_ctrl)
|
|
299
|
+
Phi21 : np.ndarray (N_fir*n_meas, N*m)
|
|
300
|
+
N : int total impulse-response horizon used
|
|
301
|
+
"""
|
|
302
|
+
return _youla_maps(dsys, K0, n_meas, n_ctrl, N_fir)
|
|
303
|
+
|
|
304
|
+
|
|
305
|
+
def sdl1_reg(dsys, N_fir=30, n_meas=1, n_ctrl=1, solver=None, verbose=False):
|
|
306
|
+
"""
|
|
307
|
+
L1-optimal (peak-to-peak) controller synthesis via Linear Programming.
|
|
308
|
+
|
|
309
|
+
Minimises the induced ℓ∞→ℓ1 norm of the closed-loop impulse response:
|
|
310
|
+
|
|
311
|
+
min_Q max_j Σ_i |T(Q)_ij| (max column-sum of |T|)
|
|
312
|
+
|
|
313
|
+
which equals the peak output amplitude for any unit-amplitude bounded
|
|
314
|
+
disturbance signal. For SISO systems this reduces to Σ_k |h[k]|.
|
|
315
|
+
|
|
316
|
+
Parameters
|
|
317
|
+
----------
|
|
318
|
+
dsys : scipy.signal.StateSpace (discrete) lifted plant from lift_h2
|
|
319
|
+
N_fir : int FIR horizon for Q (longer = tighter approximation)
|
|
320
|
+
n_meas : int
|
|
321
|
+
n_ctrl : int
|
|
322
|
+
solver : str or None CVXPY solver ('HIGHS', 'CLARABEL', 'SCS', …)
|
|
323
|
+
verbose : bool
|
|
324
|
+
|
|
325
|
+
Returns
|
|
326
|
+
-------
|
|
327
|
+
K_opt : scipy.signal.StateSpace (discrete)
|
|
328
|
+
l1_norm : float achieved L1-norm (peak-to-peak gain)
|
|
329
|
+
Q_fir : np.ndarray, shape (N_fir, n_ctrl, n_meas)
|
|
330
|
+
result : dict {'status', 'objective', 'solver'}
|
|
331
|
+
"""
|
|
332
|
+
_require_cvxpy()
|
|
333
|
+
|
|
334
|
+
K0 = _stabilising_h2(dsys, n_meas, n_ctrl)
|
|
335
|
+
T_cl, Phi12, Phi21, N = _youla_maps(dsys, K0, n_meas, n_ctrl, N_fir)
|
|
336
|
+
|
|
337
|
+
p = T_cl.shape[0] // N
|
|
338
|
+
m = T_cl.shape[1] // N
|
|
339
|
+
nc, nm = n_ctrl, n_meas
|
|
340
|
+
|
|
341
|
+
Q_var = cp.Variable(N_fir * nc * nm, name="Q_fir")
|
|
342
|
+
Q_mat = _build_toeplitz_cvxpy(Q_var, N_fir, nc, nm)
|
|
343
|
+
T_Q = T_cl + Phi12 @ Q_mat @ Phi21
|
|
344
|
+
|
|
345
|
+
# Induced ℓ∞→ℓ1 norm = max column-sum of |T|
|
|
346
|
+
# LP reformulation: introduce t ≥ |T|, then min max_j Σ_i t_ij
|
|
347
|
+
t = cp.Variable((N * p, N * m), name="t_abs")
|
|
348
|
+
col_sums = cp.sum(t, axis=0) # shape (N*m,)
|
|
349
|
+
objective = cp.Minimize(cp.max(col_sums))
|
|
350
|
+
constraints = [t >= T_Q, t >= -T_Q]
|
|
351
|
+
|
|
352
|
+
prob = cp.Problem(objective, constraints)
|
|
353
|
+
prob.solve(solver=solver, verbose=verbose)
|
|
354
|
+
|
|
355
|
+
if prob.status not in ("optimal", "optimal_inaccurate"):
|
|
356
|
+
warnings.warn(f"L1 synthesis: solver status '{prob.status}'")
|
|
357
|
+
|
|
358
|
+
Q_opt = np.array(Q_var.value).reshape(N_fir, nc, nm) \
|
|
359
|
+
if Q_var.value is not None else np.zeros((N_fir, nc, nm))
|
|
360
|
+
|
|
361
|
+
K_opt = _fir_controller_from_q(K0, Q_opt, dsys, n_meas, n_ctrl)
|
|
362
|
+
l1_val = float(prob.value) if prob.value is not None else float('nan')
|
|
363
|
+
|
|
364
|
+
return K_opt, l1_val, Q_opt, {
|
|
365
|
+
"status": prob.status,
|
|
366
|
+
"objective": l1_val,
|
|
367
|
+
"solver": prob.solver_stats.solver_name if prob.solver_stats else None,
|
|
368
|
+
}
|
|
369
|
+
|
|
370
|
+
|
|
371
|
+
def sd_mixed_h2_l1(dsys, N_fir=30, n_meas=1, n_ctrl=1,
|
|
372
|
+
l1_bound=None, h2_bound=None,
|
|
373
|
+
solver=None, verbose=False):
|
|
374
|
+
"""
|
|
375
|
+
Mixed H2 / L1 optimal controller synthesis.
|
|
376
|
+
|
|
377
|
+
Solves one of three problems depending on which bound is given:
|
|
378
|
+
|
|
379
|
+
A) ``l1_bound`` given → **minimise H2** subject to ‖T‖_L1 ≤ l1_bound
|
|
380
|
+
B) ``h2_bound`` given → **minimise L1** subject to ‖T‖_H2 ≤ h2_bound
|
|
381
|
+
C) Both or neither → minimise H2 + L1 (scalarised, equal weight)
|
|
382
|
+
|
|
383
|
+
Parameters
|
|
384
|
+
----------
|
|
385
|
+
dsys : scipy.signal.StateSpace (discrete) lifted plant
|
|
386
|
+
N_fir : int FIR horizon
|
|
387
|
+
n_meas : int
|
|
388
|
+
n_ctrl : int
|
|
389
|
+
l1_bound : float or None upper bound on L1-norm
|
|
390
|
+
h2_bound : float or None upper bound on H2-norm
|
|
391
|
+
solver : str or None
|
|
392
|
+
verbose : bool
|
|
393
|
+
|
|
394
|
+
Returns
|
|
395
|
+
-------
|
|
396
|
+
K_opt : scipy.signal.StateSpace
|
|
397
|
+
cost : dict {'h2': float, 'l1': float}
|
|
398
|
+
Q_fir : np.ndarray
|
|
399
|
+
result : dict
|
|
400
|
+
"""
|
|
401
|
+
_require_cvxpy()
|
|
402
|
+
|
|
403
|
+
K0 = _stabilising_h2(dsys, n_meas, n_ctrl)
|
|
404
|
+
T_cl, Phi12, Phi21, N = _youla_maps(dsys, K0, n_meas, n_ctrl, N_fir)
|
|
405
|
+
|
|
406
|
+
p = T_cl.shape[0] // N
|
|
407
|
+
m = T_cl.shape[1] // N
|
|
408
|
+
nc, nm = n_ctrl, n_meas
|
|
409
|
+
|
|
410
|
+
Q_var = cp.Variable(N_fir * nc * nm, name="Q")
|
|
411
|
+
Q_mat = _build_toeplitz_cvxpy(Q_var, N_fir, nc, nm)
|
|
412
|
+
T_Q = T_cl + Phi12 @ Q_mat @ Phi21
|
|
413
|
+
|
|
414
|
+
# H2 proxy: squared Frobenius norm of impulse-response matrix (= H2² for ZOH)
|
|
415
|
+
h2_sq = cp.sum_squares(T_Q)
|
|
416
|
+
|
|
417
|
+
# L1 proxy: induced ℓ∞→ℓ1 norm = max column-sum of |T|
|
|
418
|
+
t_abs = cp.Variable((N * p, N * m), name="t_abs")
|
|
419
|
+
col_sums = cp.sum(t_abs, axis=0)
|
|
420
|
+
l1_obj = cp.max(col_sums)
|
|
421
|
+
|
|
422
|
+
constraints = [t_abs >= T_Q, t_abs >= -T_Q]
|
|
423
|
+
|
|
424
|
+
if l1_bound is not None and h2_bound is None:
|
|
425
|
+
objective = cp.Minimize(h2_sq)
|
|
426
|
+
constraints.append(l1_obj <= l1_bound)
|
|
427
|
+
elif h2_bound is not None and l1_bound is None:
|
|
428
|
+
objective = cp.Minimize(l1_obj)
|
|
429
|
+
constraints.append(h2_sq <= h2_bound ** 2)
|
|
430
|
+
else:
|
|
431
|
+
scale = float(np.linalg.norm(T_cl, 'fro') + 1e-8)
|
|
432
|
+
objective = cp.Minimize(h2_sq / scale ** 2 + l1_obj / scale)
|
|
433
|
+
if l1_bound is not None:
|
|
434
|
+
constraints.append(l1_obj <= l1_bound)
|
|
435
|
+
if h2_bound is not None:
|
|
436
|
+
constraints.append(h2_sq <= h2_bound ** 2)
|
|
437
|
+
|
|
438
|
+
prob = cp.Problem(objective, constraints)
|
|
439
|
+
prob.solve(solver=solver, verbose=verbose)
|
|
440
|
+
|
|
441
|
+
if prob.status not in ("optimal", "optimal_inaccurate"):
|
|
442
|
+
warnings.warn(f"Mixed H2/L1: solver status '{prob.status}'")
|
|
443
|
+
|
|
444
|
+
Q_opt = np.array(Q_var.value).reshape(N_fir, nc, nm) \
|
|
445
|
+
if Q_var.value is not None else np.zeros((N_fir, nc, nm))
|
|
446
|
+
|
|
447
|
+
T_opt = T_cl + Phi12 @ _build_toeplitz_np(Q_opt, nc, nm) @ Phi21
|
|
448
|
+
K_opt = _fir_controller_from_q(K0, Q_opt, dsys, n_meas, n_ctrl)
|
|
449
|
+
cost = {
|
|
450
|
+
"h2": float(np.sqrt(max(np.sum(T_opt ** 2), 0))),
|
|
451
|
+
"l1": float(np.max(np.sum(np.abs(T_opt), axis=0))),
|
|
452
|
+
}
|
|
453
|
+
|
|
454
|
+
return K_opt, cost, Q_opt, {"status": prob.status}
|
|
455
|
+
|
|
456
|
+
|
|
457
|
+
def sd_constrained(dsys, N_fir=30, n_meas=1, n_ctrl=1,
|
|
458
|
+
objective='h2',
|
|
459
|
+
envelope=None,
|
|
460
|
+
output_bound=None,
|
|
461
|
+
input_bound=None,
|
|
462
|
+
solver=None, verbose=False):
|
|
463
|
+
"""
|
|
464
|
+
Synthesis with hard time-domain constraints.
|
|
465
|
+
|
|
466
|
+
Parameters
|
|
467
|
+
----------
|
|
468
|
+
dsys : scipy.signal.StateSpace lifted plant
|
|
469
|
+
N_fir : int FIR horizon
|
|
470
|
+
n_meas, n_ctrl : int
|
|
471
|
+
objective : str 'h2' | 'l1' | 'linf'
|
|
472
|
+
'h2' — minimise squared Frobenius norm of impulse response (= H2²)
|
|
473
|
+
'l1' — minimise induced ℓ∞→ℓ1 norm (peak-to-peak gain)
|
|
474
|
+
'linf'— minimise induced ℓ2→ℓ2 norm proxy (largest singular value
|
|
475
|
+
of the finite-horizon Toeplitz matrix)
|
|
476
|
+
envelope : (lo, hi) arrays of shape (N_horizon, p) or (N_horizon,)
|
|
477
|
+
Hard upper and lower bounds on the closed-loop impulse response at
|
|
478
|
+
each time step. Pass ``None`` to skip.
|
|
479
|
+
output_bound : float or None
|
|
480
|
+
Peak output constraint: max_k ‖y[k]‖_∞ ≤ output_bound.
|
|
481
|
+
input_bound : float or None
|
|
482
|
+
Peak control constraint: max_k ‖u[k]‖_∞ ≤ input_bound.
|
|
483
|
+
solver : str or None
|
|
484
|
+
verbose : bool
|
|
485
|
+
|
|
486
|
+
Returns
|
|
487
|
+
-------
|
|
488
|
+
K_opt : scipy.signal.StateSpace
|
|
489
|
+
achieved : dict {'h2', 'l1', 'linf'} — norms of the achieved closed-loop
|
|
490
|
+
Q_fir : np.ndarray
|
|
491
|
+
result : dict
|
|
492
|
+
"""
|
|
493
|
+
_require_cvxpy()
|
|
494
|
+
|
|
495
|
+
K0 = _stabilising_h2(dsys, n_meas, n_ctrl)
|
|
496
|
+
T_cl, Phi12, Phi21, N = _youla_maps(dsys, K0, n_meas, n_ctrl, N_fir)
|
|
497
|
+
|
|
498
|
+
p = T_cl.shape[0] // N
|
|
499
|
+
m = T_cl.shape[1] // N
|
|
500
|
+
nc, nm = n_ctrl, n_meas
|
|
501
|
+
|
|
502
|
+
Q_var = cp.Variable(N_fir * nc * nm, name="Q")
|
|
503
|
+
Q_mat = _build_toeplitz_cvxpy(Q_var, N_fir, nc, nm)
|
|
504
|
+
T_Q = T_cl + Phi12 @ Q_mat @ Phi21
|
|
505
|
+
|
|
506
|
+
constraints = []
|
|
507
|
+
|
|
508
|
+
# ── Envelope / template constraint ───────────────────────────────────────
|
|
509
|
+
if envelope is not None:
|
|
510
|
+
lo_arr, hi_arr = envelope
|
|
511
|
+
lo_arr = np.atleast_2d(lo_arr)
|
|
512
|
+
hi_arr = np.atleast_2d(hi_arr)
|
|
513
|
+
N_env = min(lo_arr.shape[0], N)
|
|
514
|
+
for k in range(N_env):
|
|
515
|
+
rs, re = k * p, (k + 1) * p
|
|
516
|
+
for col in range(T_Q.shape[1]):
|
|
517
|
+
constraints.append(T_Q[rs:re, col] >= lo_arr[k])
|
|
518
|
+
constraints.append(T_Q[rs:re, col] <= hi_arr[k])
|
|
519
|
+
|
|
520
|
+
# ── Peak output bound ────────────────────────────────────────────────────
|
|
521
|
+
if output_bound is not None:
|
|
522
|
+
constraints.append(cp.norm_inf(T_Q) <= output_bound)
|
|
523
|
+
|
|
524
|
+
# ── Peak input (control) bound ───────────────────────────────────────────
|
|
525
|
+
if input_bound is not None:
|
|
526
|
+
constraints.append(cp.norm_inf(T_Q[-nc * N:, :]) <= input_bound)
|
|
527
|
+
|
|
528
|
+
# ── Objective ────────────────────────────────────────────────────────────
|
|
529
|
+
t_abs = cp.Variable((N * p, N * m), name="t_abs")
|
|
530
|
+
col_sums = cp.sum(t_abs, axis=0)
|
|
531
|
+
constraints += [t_abs >= T_Q, t_abs >= -T_Q]
|
|
532
|
+
|
|
533
|
+
if objective == 'h2':
|
|
534
|
+
obj = cp.Minimize(cp.sum_squares(T_Q))
|
|
535
|
+
elif objective == 'l1':
|
|
536
|
+
obj = cp.Minimize(cp.max(col_sums))
|
|
537
|
+
elif objective == 'linf':
|
|
538
|
+
# Minimise the largest singular value of the finite-horizon Toeplitz
|
|
539
|
+
# matrix — a convex proxy for the induced ℓ2→ℓ2 norm.
|
|
540
|
+
obj = cp.Minimize(cp.norm(T_Q, 2))
|
|
541
|
+
else:
|
|
542
|
+
raise ValueError(
|
|
543
|
+
f"Unknown objective '{objective}'. Choose 'h2', 'l1', or 'linf'."
|
|
544
|
+
)
|
|
545
|
+
|
|
546
|
+
prob = cp.Problem(obj, constraints)
|
|
547
|
+
prob.solve(solver=solver, verbose=verbose)
|
|
548
|
+
|
|
549
|
+
if prob.status not in ("optimal", "optimal_inaccurate"):
|
|
550
|
+
warnings.warn(f"sd_constrained: solver status '{prob.status}'")
|
|
551
|
+
|
|
552
|
+
Q_opt = np.array(Q_var.value).reshape(N_fir, nc, nm) \
|
|
553
|
+
if Q_var.value is not None else np.zeros((N_fir, nc, nm))
|
|
554
|
+
|
|
555
|
+
T_opt = T_cl + Phi12 @ _build_toeplitz_np(Q_opt, nc, nm) @ Phi21
|
|
556
|
+
K_opt = _fir_controller_from_q(K0, Q_opt, dsys, n_meas, n_ctrl)
|
|
557
|
+
|
|
558
|
+
sv = np.linalg.svd(T_opt, compute_uv=False)
|
|
559
|
+
achieved = {
|
|
560
|
+
"h2": float(np.sqrt(max(np.sum(T_opt ** 2), 0))),
|
|
561
|
+
"l1": float(np.max(np.sum(np.abs(T_opt), axis=0))),
|
|
562
|
+
"linf": float(sv[0]) if len(sv) > 0 else float('nan'),
|
|
563
|
+
}
|
|
564
|
+
|
|
565
|
+
return K_opt, achieved, Q_opt, {"status": prob.status}
|
|
566
|
+
|
|
567
|
+
|
|
568
|
+
# ── L1-norm analysis (no synthesis) ──────────────────────────────────────────
|
|
569
|
+
|
|
570
|
+
def sdl1norm(plant, K, T=None, N=200):
|
|
571
|
+
"""
|
|
572
|
+
Compute the L1-norm (peak-to-peak gain) of a sampled-data closed loop.
|
|
573
|
+
|
|
574
|
+
The L1-norm equals the induced ℓ∞→ℓ1 gain: the maximum, over all
|
|
575
|
+
unit-amplitude bounded disturbances, of the total output amplitude.
|
|
576
|
+
For SISO systems: ‖G‖_L1 = Σ_k |h[k]|.
|
|
577
|
+
|
|
578
|
+
Parameters
|
|
579
|
+
----------
|
|
580
|
+
plant : (num, den) tuple or scipy.signal.lti
|
|
581
|
+
K : (num, den) tuple or scipy.signal.dlti
|
|
582
|
+
T : float sampling period
|
|
583
|
+
N : int impulse response horizon
|
|
584
|
+
|
|
585
|
+
Returns
|
|
586
|
+
-------
|
|
587
|
+
l1 : float L1-norm (induced ℓ∞→ℓ1 gain)
|
|
588
|
+
h : np.ndarray impulse response samples (SISO: shape (N,);
|
|
589
|
+
MIMO: shape (N, n_out, n_in))
|
|
590
|
+
"""
|
|
591
|
+
from directsd.analysis.norms import _unpack_lti
|
|
592
|
+
from directsd.polynomial.transforms import dtfm
|
|
593
|
+
|
|
594
|
+
plant_num, plant_den, _ = _unpack_lti(plant)
|
|
595
|
+
K_num, K_den, dt_k = _unpack_lti(K)
|
|
596
|
+
if T is None:
|
|
597
|
+
T = dt_k
|
|
598
|
+
if T is None:
|
|
599
|
+
raise ValueError("T must be provided")
|
|
600
|
+
|
|
601
|
+
with warnings.catch_warnings():
|
|
602
|
+
warnings.simplefilter("ignore")
|
|
603
|
+
D22num, D22den = dtfm((plant_num, plant_den), T)
|
|
604
|
+
|
|
605
|
+
KD_num = np.polymul(K_num, D22num)
|
|
606
|
+
KD_den = np.polymul(K_den, D22den)
|
|
607
|
+
S_den = np.polyadd(KD_den, KD_num)
|
|
608
|
+
|
|
609
|
+
with warnings.catch_warnings():
|
|
610
|
+
warnings.simplefilter("ignore")
|
|
611
|
+
ss_obj = sig.dlti(KD_den, S_den, dt=T).to_ss()
|
|
612
|
+
|
|
613
|
+
A, B, C, D = ss_obj.A, ss_obj.B, ss_obj.C, ss_obj.D
|
|
614
|
+
h_mat = _impulse_response(A, B, C, D, N) # (N, p, m)
|
|
615
|
+
|
|
616
|
+
h2d = h_mat.reshape(N, -1)
|
|
617
|
+
l1 = float(np.max(np.sum(np.abs(h2d), axis=0))) # max column-sum
|
|
618
|
+
|
|
619
|
+
h_out = h_mat[:, 0, 0] if h_mat.shape[1:] == (1, 1) else h_mat
|
|
620
|
+
return l1, h_out
|