@@ -203,3 +203,219 @@ function blockdiag(mats::AbstractMatrix{T}...) where T
203
203
end
204
204
return res
205
205
end
206
+
207
+
208
+
209
+ """
210
+ `feedback(L)` Returns L/(1+L)
211
+ `feedback(P1,P2)` Returns P1/(1+P1*P2)
212
+ """
213
+ feedback (L:: TransferFunction ) = L/ (1 + L)
214
+ feedback (P1:: TransferFunction , P2:: TransferFunction ) = P1/ (1 + P1* P2)
215
+
216
+ # Efficient implementations
217
+ function feedback (L:: TransferFunction{T} ) where T<: SisoRational
218
+ if size (L) != (1 ,1 )
219
+ error (" MIMO TransferFunction feedback isn't implemented, use L/(1+L)" )
220
+ end
221
+ P = numpoly (L)
222
+ Q = denpoly (L)
223
+ tf (P, P+ Q, L. Ts)
224
+ end
225
+
226
+ function feedback (L:: TransferFunction{T} ) where {T<: SisoZpk }
227
+ if size (L) != (1 ,1 )
228
+ error (" MIMO TransferFunction feedback isn't implemented, use L/(1+L)" )
229
+ end
230
+ # Extract polynomials and create P/(P+Q)
231
+ k = L. matrix[1 ]. k
232
+ denpol = numpoly (L)[1 ]+ denpoly (L)[1 ]
233
+ kden = denpol[end ] # Get coeff of s^n
234
+ # Create siso system
235
+ sisozpk = T (L. matrix[1 ]. z, roots (denpol), k/ kden)
236
+ return TransferFunction {T} (fill (sisozpk,1 ,1 ), L. Ts)
237
+ end
238
+
239
+ """
240
+ `feedback(sys)`
241
+
242
+ `feedback(sys1,sys2)`
243
+
244
+ Forms the negative feedback interconnection
245
+ ```julia
246
+ >-+ sys1 +-->
247
+ | |
248
+ (-)sys2 +
249
+ ```
250
+ If no second system is given, negative identity feedback is assumed
251
+ """
252
+ function feedback (sys:: Union{StateSpace, DelayLtiSystem} )
253
+ ninputs (sys) != noutputs (sys) && error (" Use feedback(sys1, sys2) if number of inputs != outputs" )
254
+ feedback (sys,ss (Matrix {numeric_type(sys)} (I,size (sys)... )))
255
+ end
256
+
257
+ function feedback (sys1:: StateSpace ,sys2:: StateSpace )
258
+ if sys1. Ts != sys2. Ts # FIXME : replace with common_sample_time
259
+ error (" Sampling time mismatch" )
260
+ end
261
+
262
+ ! (iszero (sys1. D) || iszero (sys2. D)) && error (" There cannot be a direct term (D) in both sys1 and sys2" )
263
+ A = [sys1. A+ sys1. B* (- sys2. D)* sys1. C sys1. B* (- sys2. C);
264
+ sys2. B* sys1. C sys2. A+ sys2. B* sys1. D* (- sys2. C)]
265
+ B = [sys1. B; sys2. B* sys1. D]
266
+ C = [sys1. C sys1. D* (- sys2. C)]
267
+
268
+ ss (A, B, C, sys1. D, sys1. Ts)
269
+ end
270
+
271
+
272
+ """
273
+ feedback(s1::AbstractStateSpace, s2::AbstractStateSpace;
274
+ U1=:, Y1=:, U2=:, Y2=:, W1=:, Z1=:, W2=Int[], Z2=Int[],
275
+ Wperm=:, Zperm=:, pos_feedback::Bool=false)
276
+
277
+
278
+ `U1`, `Y1`, `U2`, `Y2` contain the indices of the signals that should be connected.
279
+ `W1`, `Z1`, `W2`, `Z2` contain the signal indices of `s1` and `s2` that should be kept.
280
+
281
+ Specify `Wperm` and `Zperm` to reorder [w1; w2] and [z1; z2] in the resulting statespace model.
282
+
283
+ Negative feedback is the default. Specify `pos_feedback=true` for positive feedback.
284
+
285
+ See Zhou etc. for similar (somewhat less symmetric) formulas.
286
+ """
287
+ @views function feedback (sys1:: AbstractStateSpace , sys2:: AbstractStateSpace ;
288
+ U1= :, Y1= :, U2= :, Y2= :, W1= :, Z1= :, W2= Int[], Z2= Int[],
289
+ Wperm= :, Zperm= :, pos_feedback:: Bool = false )
290
+
291
+ if sys1. Ts != sys2. Ts # FIXME : replace with common_sample_time
292
+ error (" Sampling time mismatch" )
293
+ end
294
+
295
+ if ! (isa (Y1, Colon) || allunique (Y1)); @warn " Connecting single output to multiple inputs Y1=$Y1 " ; end
296
+ if ! (isa (Y2, Colon) || allunique (Y2)); @warn " Connecting single output to multiple inputs Y2=$Y2 " ; end
297
+ if ! (isa (U1, Colon) || allunique (U1)); @warn " Connecting multiple outputs to a single input U1=$U1 " ; end
298
+ if ! (isa (U2, Colon) || allunique (U2)); @warn " Connecting a single output to multiple inputs U2=$U2 " ; end
299
+
300
+ if (U1 isa Colon ? size (sys1, 2 ) : length (U1)) != (Y2 isa Colon ? size (sys2, 1 ) : length (Y2))
301
+ error (" Lengths of U1 ($U1 ) and Y2 ($Y2 ) must be equal" )
302
+ end
303
+ if (U2 isa Colon ? size (sys2, 2 ) : length (U2)) != (Y1 isa Colon ? size (sys1, 1 ) : length (Y1))
304
+ error (" Lengths of U1 ($U2 ) and Y2 ($Y1 ) must be equal" )
305
+ end
306
+
307
+ α = pos_feedback ? 1 : - 1 # The sign of feedback
308
+
309
+ s1_B1 = sys1. B[:,W1]
310
+ s1_B2 = sys1. B[:,U1]
311
+ s1_C1 = sys1. C[Z1,:]
312
+ s1_C2 = sys1. C[Y1,:]
313
+ s1_D11 = sys1. D[Z1,W1]
314
+ s1_D12 = sys1. D[Z1,U1]
315
+ s1_D21 = sys1. D[Y1,W1]
316
+ s1_D22 = sys1. D[Y1,U1]
317
+
318
+ s2_B1 = sys2. B[:,W2]
319
+ s2_B2 = sys2. B[:,U2]
320
+ s2_C1 = sys2. C[Z2,:]
321
+ s2_C2 = sys2. C[Y2,:]
322
+ s2_D11 = sys2. D[Z2,W2]
323
+ s2_D12 = sys2. D[Z2,U2]
324
+ s2_D21 = sys2. D[Y2,W2]
325
+ s2_D22 = sys2. D[Y2,U2]
326
+
327
+ if iszero (s1_D22) || iszero (s2_D22)
328
+ A = [sys1. A + α* s1_B2* s2_D22* s1_C2 α* s1_B2* s2_C2;
329
+ s2_B2* s1_C2 sys2. A + α* s2_B2* s1_D22* s2_C2]
330
+
331
+ B = [s1_B1 + α* s1_B2* s2_D22* s1_D21 α* s1_B2* s2_D21;
332
+ s2_B2* s1_D21 s2_B1 + α* s2_B2* s1_D22* s2_D21]
333
+ C = [s1_C1 + α* s1_D12* s2_D22* s1_C2 α* s1_D12* s2_C2;
334
+ s2_D12* s1_C2 s2_C1 + α* s2_D12* s1_D22* s2_C2]
335
+ D = [s1_D11 + α* s1_D12* s2_D22* s1_D21 α* s1_D12* s2_D21;
336
+ s2_D12* s1_D21 s2_D11 + α* s2_D12* s1_D22* s2_D21]
337
+ else
338
+ # inv seems to be better than lu
339
+ R1 = try
340
+ inv (α* I - s2_D22* s1_D22) # slightly faster than α*inv(I - α*s2_D22*s1_D22)
341
+ catch
342
+ error (" Ill-posed feedback interconnection, I - α*s2_D22*s1_D22 or I - α*s2_D22*s1_D22 not invertible" )
343
+ end
344
+
345
+ R2 = try
346
+ inv (I - α* s1_D22* s2_D22)
347
+ catch
348
+ error (" Ill-posed feedback interconnection, I - α*s2_D22*s1_D22 or I - α*s2_D22*s1_D22 not invertible" )
349
+ end
350
+
351
+ A = [sys1. A + s1_B2* R1* s2_D22* s1_C2 s1_B2* R1* s2_C2;
352
+ s2_B2* R2* s1_C2 sys2. A + α* s2_B2* R2* s1_D22* s2_C2]
353
+
354
+ B = [s1_B1 + s1_B2* R1* s2_D22* s1_D21 s1_B2* R1* s2_D21;
355
+ s2_B2* R2* s1_D21 s2_B1 + α* s2_B2* R2* s1_D22* s2_D21]
356
+ C = [s1_C1 + s1_D12* R1* s2_D22* s1_C2 s1_D12* R1* s2_C2;
357
+ s2_D12* R2* s1_C2 s2_C1 + α* s2_D12* R2* s1_D22* s2_C2]
358
+ D = [s1_D11 + s1_D12* R1* s2_D22* s1_D21 s1_D12* R1* s2_D21;
359
+ s2_D12* R2* s1_D21 s2_D11 + α* s2_D12* R2* s1_D22* s2_D21]
360
+ end
361
+
362
+ return StateSpace (A, B[:, Wperm], C[Zperm,:], D[Zperm, Wperm], sys1. Ts)
363
+ end
364
+
365
+
366
+ """
367
+ `feedback2dof(P,R,S,T)` Return `BT/(AR+ST)` where B and A are the numerator and denomenator polynomials of `P` respectively
368
+ `feedback2dof(B,A,R,S,T)` Return `BT/(AR+ST)`
369
+ """
370
+ function feedback2dof (P:: TransferFunction ,R,S,T)
371
+ ! issiso (P) && error (" Feedback not implemented for MIMO systems" )
372
+ tf (conv (poly2vec (numpoly (P)[1 ]),T),zpconv (poly2vec (denpoly (P)[1 ]),R,poly2vec (numpoly (P)[1 ]),S))
373
+ end
374
+
375
+ feedback2dof (B,A,R,S,T) = tf (conv (B,T),zpconv (A,R,B,S))
376
+
377
+
378
+ """
379
+ lft(G, Δ, type=:l)
380
+
381
+ Lower and upper linear fractional transformation between systems `G` and `Δ`.
382
+
383
+ Specify `:l` lor lower LFT, and `:u` for upper LFT.
384
+
385
+ `G` must have more inputs and outputs than `Δ` has outputs and inputs.
386
+
387
+ For details, see Chapter 9.1 in
388
+ **Zhou, K. and JC Doyle**. Essentials of robust control, Prentice hall (NJ), 1998
389
+ """
390
+ function lft (G, Δ, type= :l )
391
+
392
+ if ! (G. nu > Δ. ny && G. ny > Δ. nu)
393
+ error (" Must have G.nu > Δ.ny and G.ny > Δ.nu for lower/upper lft" )
394
+ end
395
+
396
+ if type == :l
397
+ feedback (G, Δ, U1= G. nu- Δ. ny+ 1 : G. nu, Y1= G. ny- Δ. nu+ 1 : G. ny, W1= 1 : G. ny- Δ. nu, Z1= 1 : G. nu- Δ. ny, pos_feedback= true )
398
+ elseif type == :u
399
+ feedback (G, Δ, U1= 1 : Δ. ny, Y1= 1 : Δ. nu, W1= Δ. nu+ 1 : G. ny, Z1= Δ. nu+ 1 : G. ny, pos_feedback= true )
400
+ else
401
+ error (" Invalid type of lft ($type ), specify type=:l (:u) for lower (upper) lft" )
402
+ end
403
+ end
404
+
405
+
406
+
407
+ """
408
+ starprod(sys1, sys2, dimu, dimy)
409
+
410
+ Compute the Redheffer star product.
411
+
412
+ `length(U1) = length(Y2) = dimu` and `length(Y1) = length(U2) = dimy`
413
+
414
+ For details, see Chapter 9.3 in
415
+ **Zhou, K. and JC Doyle**. Essentials of robust control, Prentice hall (NJ), 1998
416
+ """
417
+ starprod (G1, G2, dimy:: Int , dimu:: Int ) = feedback (G1, G2,
418
+ U1= G1. nu- dimu+ 1 : G1. nu, Y1= G1. ny- dimy+ 1 : G1. ny, W1= 1 : G1. nu- dimu, Z1= 1 : G1. ny- dimy,
419
+ U2= 1 : dimy, Y2= 1 : dimu, W2= dimy+ 1 : G2. nu, Z2= dimu+ 1 : G2. ny,
420
+ pos_feedback= true )
421
+ starprod (sys1, sys2) = lft (sys1, sys2, :l )
0 commit comments