@@ -203,3 +203,219 @@ function blockdiag(mats::AbstractMatrix{T}...) where T
203203 end
204204 return res
205205end
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