RobustAndOptimalControl.jl

Installation

pkg> add RobustAndOptimalControl

Named systems

See complicated-feedback example

Connecting systems together

See complicated-feedback example

The following complicated feedback interconnection

                 yF
              ┌────────────────────────────────┐
              │                                │
    ┌───────┐ │  ┌───────┐ yR   ┌─────────┐    │    ┌───────┐
uF  │       │ │  │       ├──────►         │ yC │  uP│       │    yP
────►   F   ├─┴──►   R   │      │    C    ├────+────►   P   ├────┬────►
    │       │    │       │   ┌──►         │         │       │    │
    └───────┘    └───────┘   │  └─────────┘         └───────┘    │
                             │                                   │
                             └───────────────────────────────────┘

can be created by

F = named_ss(ssrand(1, 1, 2, proper=true), x=:xF, u=:uF, y=:yF)
R = named_ss(ssrand(1, 1, 2, proper=true), x=:xR, u=:uR, y=:yR)
C = named_ss(ssrand(1, 1, 2, proper=true), x=:xC, u=:uC, y=:yC)
P = named_ss(ssrand(1, 1, 3, proper=true), x=:xP, u=:uP, y=:yP)

addP = sumblock("uP = yF + yC") # Sum node before P
addC = sumblock("uC = yR - yP") # Sum node before C (drawn as two arrows into C in the diagram)

connections = [
    :yP => :yP # Output to input
    :uP => :uP # addP's output is called the same as P's input
    :yC => :yC
    :yF => :yF
    :yF => :uR
    :uC => :uC
    :yR => :yR
]
w1 = [:uF] # External inputs

G = connect([F, R, C, P, addP, addC], connections; w1)

If an external input is to be connected to multiple points, use a splitter to split up the signal into a set of unique names which are then used in the connections.

Model augmentation

TODO.

Add disturbance and performance models to your system model.

$H_\infty$ and $H_2$ design

TODO Examples are available in the example folder.

LQG design

TODO

Structured singular value and diskmargin

Closed-loop analysis

Exported functions and types

Index

RobustAndOptimalControl.DiskType
Disk

Represents a perturbation disc in the complex plane. Disk(0.5, 2) represents all perturbations in the circle centered at 1.25 with radius 0.75, or in other words, a gain margin of 2 and a pahse margin of 36.9 degrees.

A disk can be converted to a Nyquist exclusion disk by nyquist(disk) and plotted using plot(disk).

Arguments:

  • γmin: Lower intercept
  • γmax: Upper intercept
  • c: Center
  • r: Radius
  • ϕm: Angle of tangent line through origin.

If γmax < γmin the disk is inverted. See diskmargin for disk margin computations.

RobustAndOptimalControl.DiskmarginType
Diskmargin

The notation follows "An Introduction to Disk Margins", Peter Seiler, Andrew Packard, and Pascal Gahinet

Fields:

α: The disk margin ω0: The worst-case frequency f0: The destabilizing perturbation f0 is a complex number with simultaneous gain and phase variation. This critical perturbation causes an instability with closed-loop pole on the imaginary axis at the critical frequency ω0 δ0: The uncertain element generating f0. γmin: The lower real-axis intercept of the disk (analogous to classical lower gain margin). γmax: The upper real-axis intercept of the disk (analogous to classical upper gain margin). ϕm: is analogous to the classical phase margin. σ: The skew parameter that was used to calculate the margin

Note, γmax and ϕm are in smaller than the classical gain and phase margins sicne the classical margins do not consider simultaneous perturbations in gain and phase.

The "disk" margin becomes a half plane for α = 2 and an inverted circle for α > 2. In this case, the upper gain margin is infinite. See the paper for more details, in particular figure 6.

RobustAndOptimalControl.LQGProblemType
G = LQG(sys::AbstractStateSpace, Q1, Q2, R1, R2; qQ=0, qR=0, M = I, N = I)

Return an LQG object that describes the closed control loop around the process sys=ss(A,B,C,D) where the controller is of LQG-type. The controller is specified by weight matrices Q1,Q2 that penalizes state deviations and control signal variance respectively, and covariance matrices R1,R2 which specify state drift and measurement covariance respectively.

qQ and qR can be set to incorporate loop transfer recovery, i.e.,

L = lqr(A, B, Q1+qQ*C'C, Q2)
K = kalman(A, C, R1+qR*B*B', R2)

Increasing qQ will add more cost in output direction, e.g., encouraging the use of cheap control, while increasing qR adds fictious dynamics noise, makes the observer faster in the direction we control.

M is a matrix that defines the controlled variables z, i.e., the variables for which you provide reference signals. If no M is provided, the default is to consider all state variables of the system as controlled. The definitions of z and y are given below

y = C*x
z = M*x

size(M, 1) determines the size of the Q1 matrix you need to supply.

N is a matrix that defines how the dynamics noise v enters the system, i.e. If no N is provided, the default is to consider all state variables being affected by independent noise components. The definition of v is given below

x′ = A*x + B*u + N*v

size(N, 2) determines the size of the R1 matrix you need to supply.

Example

s = tf("s")
P = [1/(s+1) 2/(s+2); 1/(s+3) 1/(s-1)]
sys = ss(P)
eye(n) = Matrix{Float64}(I,n,n) # For convinience

qQ = 1
qR = 1
Q1 = 10eye(4)
Q2 = 1eye(2)
R1 = 1eye(6)
R2 = 1eye(2)

G = LQG(sys, Q1, Q2, R1, R2, qQ=qQ, qR=qR)

Gcl = G.cl
T = G.T
S = G.S
sigmaplot([S,T],exp10.(range(-3, stop=3, length=1000)))
stepplot(Gcl)
RobustAndOptimalControl.LQGProblemMethod
LQGProblem(P::ExtendedStateSpace)

If only an ExtendedStateSpace system is provided, the system P is assumed to correspond to the H₂ optimal control problem with

C1'C1    = Q1
D12'D12  = Q2

B1*B1'   = R1
D21*D21' = R2

and an LQGProblem with the above covariance matrices is returned. The system description in the returned LQGProblem will have B1 = C1 = I. See Ch. 13 in Robust and optimal control for reference.

RobustAndOptimalControl.add_input_differentiatorFunction
add_input_differentiator(sys::StateSpace, ui = 1:sys.nu; goodwin=false)

Augment the output of sys with the difference u(k+1)-u(k)

Arguments:

  • ui: An index or vector of indices indicating which inputs to differentiate.
  • goodwin: If true, the difference operator will use the Goodwin δ operator, i.e., (u(k+1)-u(k)) / sys.Ts.

The augmented system will have the matrices

[A 0; 0 0]  [B; I]  [C 0; 0 -I]  [D; I]

with length(ui) added states and outputs.

RobustAndOptimalControl.add_low_frequency_disturbanceMethod
add_low_frequency_disturbance(sys::StateSpace; ϵ = 0, measurement = false)

Augment sys with a low-frequency (integrating if ϵ=0) disturbance model. If an integrating input disturbance is used together with an observer, the controller will have integral action.

Arguments:

  • ϵ: Move the integrator pole ϵ into the stable region.
  • measurement: If true, the disturbance is a measurement disturbance, otherwise it's an input diturbance.
RobustAndOptimalControl.add_output_differentiatorFunction
add_differentiator(sys::StateSpace{<:Discrete})

Augment the output of sys with the numerical difference (discrete-time derivative) of output, i.e., y_aug = [y; (y-y_prev)/sys.Ts] To add both an integrator and a differentiator to a SISO system, use

Gd = add_output_integrator(add_output_differentiator(G), 1)
RobustAndOptimalControl.add_output_integratorFunction

addoutputintegrator(sys::StateSpace{<:Discrete}, ind = 1; ϵ = 0)

Augment the output of sys with the integral of output at index ind, i.e., y_aug = [y; ∫y[ind]] To add both an integrator and a differentiator to a SISO system, use

Gd = add_output_integrator(add_output_differentiator(G), 1)

Note: numerical integration is subject to numerical drift. If the output of the system corresponds to, e.g., a velocity reference and the integral to position reference, consider methods for mitigating this drift.

RobustAndOptimalControl.add_resonant_disturbanceMethod
add_resonant_disturbance(sys::StateSpace{Continuous}, ω, ζ, Ai::Int; measurement = false)

Augment sys with a resonant disturbance model.

Arguments:

  • ω: Frequency
  • ζ: Relative damping.
  • Ai: The affected state
  • measurement: If true, the disturbace is acting on the output, this will cause the controller to have zeros at ω (roots of poly s² + 2ζωs + ω²). If false, the disturbance is acting on the input, this will cause the controller to have poles at ω (roots of poly s² + 2ζωs + ω²).
RobustAndOptimalControl.baltrunc2Method
baltrunc2(sys::LTISystem; residual=false, n=missing, kwargs...)

Compute the a balanced truncation of order n and the hankel singular values

For keyword arguments, see the docstring of DescriptorSystems.gbalmr, reproduced below

gbalmr(sys, balance = false, matchdc = false, ord = missing, atolhsv = 0, rtolhsv = nϵ, 
       atolmin = atolhsv, rtolmin = rtolhsv, 
       atol = 0, atol1 = atol, atol2 = atol, rtol, fast = true) -> (sysr, hs)

Compute for a proper and stable descriptor system sys = (A-λE,B,C,D) with the transfer function matrix G(λ), a reduced order realization sysr = (Ar-λEr,Br,Cr,Dr) and the vector hs of decreasingly ordered Hankel singular values of the system sys. If balance = true, a balancing-based approach is used to determine a reduced order minimal realization of the form sysr = (Ar-λI,Br,Cr,Dr). For a continuous-time system sys, the resulting realization sysr is balanced, i.e., the controllability and observability grammians are equal and diagonal. If additonally matchdc = true, the resulting sysr is computed using state rezidualization formulas (also known as singular perturbation approximation) which additionally preserves the DC-gain of sys. In this case, the resulting realization sysr is balanced (for both continuous- and discrete-time systems). If balance = false, an enhanced accuracy balancing-free approach is used to determine the reduced order system sysr.

If ord = nr, the resulting order of sysr is min(nr,nrmin), where nrmin is the order of a minimal realization of sys determined as the number of Hankel singular values exceeding max(atolmin,rtolmin*HN), with HN, the Hankel norm of G(λ). If ord = missing, the resulting order is chosen as the number of Hankel singular values exceeding max(atolhsv,rtolhsv*HN).

The keyword arguments atol1, atol2, and rtol, specify, respectively, the absolute tolerance for the nonzero elements of A, B, C, D, the absolute tolerance for the nonzero elements of E, and the relative tolerance for the nonzero elements of A, B, C, D and E. The default relative tolerance is , where ϵ is the working machine epsilon and n is the order of the system sys. The keyword argument atol can be used to simultaneously set atol1 = atol and atol2 = atol.

If E is singular, the uncontrollable infinite eigenvalues of the pair (A,E) and the non-dynamic modes are elliminated using minimal realization techniques. The rank determinations in the performed reductions are based on rank revealing QR-decompositions with column pivoting if fast = true or the more reliable SVD-decompositions if fast = false.

Method: For the order reduction of a standard system, the balancing-free method of [1] or the balancing-based method of [2] are used. For a descriptor system the balancing related order reduction methods of [3] are used. To preserve the DC-gain of the original system, the singular perturbation approximation method of [4] is used in conjunction with the balancing-based or balancing-free approach of [5].

References

[1] A. Varga. Efficient minimal realization procedure based on balancing. In A. El Moudni, P. Borne, and S.G. Tzafestas (Eds.), Prepr. of the IMACS Symp. on Modelling and Control of Technological Systems, Lille, France, vol. 2, pp.42-47, 1991.

[2] M. S. Tombs and I. Postlethwaite. Truncated balanced realization of a stable non-minimal state-space system. Int. J. Control, vol. 46, pp. 1319–1330, 1987.

[3] T. Stykel. Gramian based model reduction for descriptor systems. Mathematics of Control, Signals, and Systems, 16:297–319, 2004.

[4] Y. Liu Y. and B.D.O. Anderson Singular Perturbation Approximation of Balanced Systems, Int. J. Control, Vol. 50, pp. 1379-1405, 1989.

[5] Varga A. Balancing-free square-root algorithm for computing singular perturbation approximations. Proc. 30-th IEEE CDC, Brighton, Dec. 11-13, 1991, Vol. 2, pp. 1062-1065.

RobustAndOptimalControl.bilinearc2dMethod
bilinearc2d(Ac::AbstractArray, Bc::AbstractArray, Cc::AbstractArray, Dc::AbstractArray, Ts::Number; tolerance=1e-12)

Balanced Bilinear transformation in State-Space. This method computes a discrete time equivalent of a continuous-time system, such that

G_d(z) = s2z[G_c(s)]

in a manner which accomplishes the following (i) Preserves the infinity L-infinity norm over the transformation (ii) Finds a system which balances B and C, in the sense that ||B||2=||C||2 (iii) Satisfies Gc(s) = z2s[s2z[Gc(s)]] for some map z2s[]

RobustAndOptimalControl.bilinearc2dMethod
bilinearc2d(sys::ExtendedStateSpace, Ts::Number)

Applies a Balanced Bilinear transformation to a discrete-time extended statespace object

RobustAndOptimalControl.bilineard2cMethod
bilineard2c(Ad::AbstractArray, Bd::AbstractArray, Cd::AbstractArray, Dd::AbstractArray, Ts::Number; tolerance=1e-12)

Balanced Bilinear transformation in State-Space. This method computes a continuous time equivalent of a discrete time system, such that

G_c(z) = z2s[G_d(z)]

in a manner which accomplishes the following (i) Preserves the infinity L-infinity norm over the transformation (ii) Finds a system which balances B and C, in the sense that ||B||2=||C||2 (iii) Satisfies Gd(z) = s2z[z2s[Gd(z)]] for some map s2z[]

RobustAndOptimalControl.blocksortMethod
blocks, M = blocksort(P::UncertainSS)

Returns the block structure of P.Δ as well as P.M permuted according to the sorted block structure. blocks is a vector of vectors with the block structure of perturbation blocks as described by μ-tools, i.e.

  • [-N, 0] denotes a repeated real block of size N
  • [N, 0] denotes a repeated complex block of size N
  • [ny, nu] denotes a full complex block of size ny × nu
RobustAndOptimalControl.broken_feedbackMethod
broken_feedback(L, i)

Closes all loops in square MIMO system L except for loops i. Forms L1 in fig 14. of "An Introduction to Disk Margins" https://arxiv.org/abs/2003.04771

RobustAndOptimalControl.closedloopFunction
closedloop(l::LQGProblem, L = lqr(l), K = kalman(l))

Closed-loop system as defined in Glad and Ljung eq. 8.28

The return value will be the closed loop from reference only, other disturbance signals (B1) are ignored. See feedback for a more advanced option.

RobustAndOptimalControl.comp_sensitivityMethod

See output_comp_sensitivity

         ▲
         │e₁
         │  ┌─────┐
d₁────+──┴──►  P  ├─────┬──►e₄
      │-    └─────┘     │
      │                 │
      │     ┌─────┐     │
 e₂◄──┴─────┤  C  ◄──┬──+───d₂
            └─────┘  │
                     │e₃
                     ▼
  • Input sensitivity is the transfer function from d₁ to e₁, (I + CP)⁻¹
  • Output sensitivity is the transfer function from d₂ to e₂, (I + PC)⁻¹
  • Input complementary sensitivity is the transfer function from d₁ to e₂, CP(I + CP)⁻¹
  • Output complementary sensitivity is the transfer function from d₂ to e₄, PC(I + PC)⁻¹
RobustAndOptimalControl.connectMethod
connect(systems, connections; w1, z1 = (:), verbose = true, kwargs...)

Create complicated feedback interconnection.

Addition and subtraction nodes are achieved by creating a linear combination node, i.e., a system with a D matrix only.

Arguments:

  • systems: A vector of named systems to be connected
  • connections: a vector of pairs indicating output => input mappings.
    • u1: input mappings (alternative input argument)
    • y1: output mappings (alternative input argument)
  • w1: external signals
  • z1: outputs (can overlap with y1)
  • verbose: Issue warnings for signals that have no connection

Example: The following complicated feedback interconnection

                 yF
              ┌────────────────────────────────┐
              │                                │
    ┌───────┐ │  ┌───────┐ yR   ┌─────────┐    │    ┌───────┐
uF  │       │ │  │       ├──────►         │ yC │  uP│       │    yP
────►   F   ├─┴──►   R   │      │    C    ├────+────►   P   ├────┬────►
    │       │    │       │   ┌──►         │         │       │    │
    └───────┘    └───────┘   │  └─────────┘         └───────┘    │
                             │                                   │
                             └───────────────────────────────────┘

can be created by

F = named_ss(ssrand(1, 1, 2, proper=true), x=:xF, u=:uF, y=:yF)
R = named_ss(ssrand(1, 1, 2, proper=true), x=:xR, u=:uR, y=:yR)
C = named_ss(ssrand(1, 1, 2, proper=true), x=:xC, u=:uC, y=:yC)
P = named_ss(ssrand(1, 1, 3, proper=true), x=:xP, u=:uP, y=:yP)

addP = sumblock("uP = yF + yC") # Sum node before P
addC = sumblock("uC = yR - yP") # Sum node before C

connections = [
    :yP => :yP # Output to input
    :uP => :uP
    :yC => :yC
    :yF => :yF
    :yF => :uR
    :uC => :uC
    :yR => :yR
]
w1 = [:uF] # External inputs

G = connect([F, R, C, P, addP, addC], connections; w1)

If an external input is to be connected to multiple points, use a splitter to split up the signal into a set of unique names which are then used in the connections.

RobustAndOptimalControl.diskmarginFunction
diskmargin(L, σ = 0)
diskmargin(L, σ::Real, ω)

Calculate the disk margin of LTI system L. L is supposed to be a loop-transfer function, i.e., it should be square. If L = PC the disk margin for output perturbations is computed, whereas if L = CP, input perturbations are considered. If the method diskmargin(P, C, args...) is used, both are computed.

The implementation and notation follows "An Introduction to Disk Margins", Peter Seiler, Andrew Packard, and Pascal Gahinet https://arxiv.org/abs/2003.04771

The margins are aviable as fields of the returned objects, see Diskmargin.

Arguments:

  • L: A loop-transfer function.
  • σ: If little is known about the distribution of gain variations then σ = 0 is a reasonable choice as it allows for a gain increase or decrease by the same relative amount. The choice σ < 0 is justified if the gain can decrease by a larger factor than it can increase. Similarly, the choice σ > 0 is justified when the gain can increase by a larger factor than it can decrease. If σ = −1 then the disk margin condition is αmax = inv(MT). This margin is related to the robust stability condition for models with multiplicative uncertainty of the form P (1 + δ). If σ = +1 then the disk margin condition is αmax = inv(MS)
  • kwargs: Are sent to the hinfnorm calculation
  • ω: If a vector of frequencies is supplied, the frequency-dependent disk margin will be computed, see example below.

Example:

L = tf(25, [1,10,10,10])
dm = diskmargin(L, 0)
plot(dm) # Plot the disk margin to illustrate maximum allowed simultaneous gain and phase variations.

nyquistplot(L)
plot!(dm, nyquist=true) # plot a nyquist exclusion disk. The Nyquist curve will be tangent to this disk at `dm.ω0`
nyquistplot!(dm.f0*L) # If we perturb the system with the worst-case perturbation `f0`, the curve will pass through the critical point -1.

## Frequency-dependent margin
w = exp10.(LinRange(-2, 2, 500))
dms = diskmargin(L, 0, w)
plot(w, dms)

See also ncfmargin.

RobustAndOptimalControl.diskmarginMethod
diskmargin(P::LTISystem, C::LTISystem, σ, w::AbstractVector, args...; kwargs...)

Simultaneuous diskmargin at outputs, inputs and input/output simultaneously of P. Returns a named tuple with the fields input, output, simultaneous_input, simultaneous_output, simultaneous where input and output represent loop-at-a-time margins, simultaneous_input is the margin for simultaneous perturbations on all inputs and simultaneous is the margin for perturbations on all inputs and outputs simultaneously. See also ncfmargin.

RobustAndOptimalControl.diskmarginMethod
diskmargin(L::LTISystem, σ::Real, ω)

Calculate the diskmargin at a particular frequency or vector of frequencies. If ω is a vector, you get a frequency-dependent diskmargin plot if you plot the returned value. See also ncfmargin.

RobustAndOptimalControl.expand_symbolMethod
expand_symbol(s::Symbol, n::Int)

Takes a symbol and an integer and returns a vector of symbols with increasing numbers appended to the end. E.g., (:x, 3) -> [:x1, :x2, :x3]

The short-hand syntax s^n is also available, e.g., :x^3 == expand_symbol(:x, 3).

Useful to create signal names for named systems.

RobustAndOptimalControl.extended_controllerFunction
extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l))

Returns an expression for the controller that is obtained when state-feedback u = -L(xᵣ-x̂) is combined with a Kalman filter with gain K that produces state estimates x̂. The controller is an instance of ExtendedStateSpace where C2 = -L, D21 = L and B2 = K.

The returned system has inputs [xᵣ; y] and outputs the control signal u. If a reference model R is used to generate state references xᵣ, the controller from e = ry - y -> u is given by

Ce = extended_controller(l)
Ce = named_ss(Ce; x = :xC, y = :u, u = [R.y; :y^l.ny]) # Name the inputs of Ce the same as the outputs of `R`.
connect([R, Ce]; u1 = R.y, y1 = R.y, w1 = [:ry^l.ny, :y^l.ny], z1=[:u])

Since the negative part of the feedback is built into the returned system, we have

C = observer_controller(l)
Ce = extended_controller(l)
system_mapping(Ce) == -C
RobustAndOptimalControl.extended_controllerMethod
extended_controller(K::AbstractStateSpace)

Takes a controller and returns an ExtendedStateSpace version which has augmented input [r; y] and output y (z output is 0-dim).

RobustAndOptimalControl.extended_gangoffourMethod
extended_gangoffour(P, C)

Returns a single statespace system that maps

  • w1 reference or measurement noise
  • w2 load disturbance

to

  • z1 control error
  • z2 control input
      z1          z2
      ▲  ┌─────┐  ▲      ┌─────┐
      │  │     │  │      │     │
w1──+─┴─►│  C  ├──┴───+─►│  P  ├─┐
    │    │     │      │  │     │ │
    │    └─────┘      │  └─────┘ │
    │                 w2         │
    └────────────────────────────┘

The returned system has the transfer-function matrix

\[\begin{bmatrix} I \\ C \end{bmatrix} (I + PC)^{-1} \begin{bmatrix} I & P \end{bmatrix}\]

The gang of four can be plotted like so

Gcl = extended_gangoffour(G, C) # Form closed-loop system
bodeplot(Gcl, lab=["S" "CS" "PS" "T"], plotphase=false) |> display # Plot gang of four

Note, the last output of Gcl is the negative of the CS and PS transfer functions from gangoffour2. See glover_mcfarlane for an extended example. See also ncfmargin.

RobustAndOptimalControl.find_lftMethod
l = find_lft(sys::StateSpace{<:Any, <:StaticParticles{<:Any, N}}, δ) where N

Given an systems sys with uncertain coefficients in the form of StaticParticles, find a lower linear fractional transformation M such that lft(M, δ) ≈ sys.

δ can be either the source of uncertainty in sys, i.e., a vector of the unique uncertain parameters that were used to create sys. These should be constructed as uniform randomly distributed particles for most robust-control theory to be applicable. δ can also be an integer, in which case a numer of δ sources of uncertainty are automatically created. This could be used for order reduction if the number of uncertainty sources in sys is large.

Note, uncertainty in sys is only supported in A and B, C and D must be deterministic.

Returns l::LFT that internaly contains all four blocks of M as well as δ. Call ss(l,sys) do obtain lft(M, δ) ≈ sys.

Call Matrix(l) to obtain M = [M11 M12; M21 M22]

RobustAndOptimalControl.fit_complex_perturbationsMethod
centers, radii = fit_complex_perturbations(P, w; relative=true, nominal=:mean)

For each frequency in w, fit a circle in the complex plane that contains all models in the model set P, represented as an LTISystem with Particles coefficients. Note, the resulting radii can be quite unstable if the number of particles is small, in particular if the particles are normally distributed instead of uniformly.

If realtive = true, circles encompassing |(P - Pn)/Pn| will be returned (multiplicative/relative uncertainty). If realtive = false, circles encompassing |P - Pn| will be returned (additive uncertainty).

If nominal = :mean, the mean of P will be used as nominal model. If nominal = :first, the first particle will be used. See MonteCarloMeasurements.with_nominal to set the nominal value in the first particle.

See also nyquistcircles to plot circles (only if relative=false).

RobustAndOptimalControl.frequency_weighted_reductionFunction
frequency_weighted_reduction(G, Wo, Wi; residual=true)

Find Gr such that ||Wₒ(G-Gr)Wᵢ||∞ is minimized. For a realtive reduction, set Wo = inv(G) and Wi = I.

If residual = true, matched static gain is achieved through "residualization", i.e., setting

\[0 = A_{21}x_{1} + A_{22}x_{2} + B_{2}u\]

where indices 1/2 correspond to the remaining/truncated states respectively. This choice typically results in a better match in the low-frequency region and a smaller overall error.

Ref: Andras Varga and Brian D.O. Anderson, "Accuracy enhancing methods for the frequency-weighted balancing related model reduction" https://elib.dlr.de/11746/1/varga_cdc01p2.pdf

RobustAndOptimalControl.gain_and_delay_uncertaintyMethod
gain_and_delay_uncertainty(kmin, kmax, Lmax)

Return a multiplicative weight to represent the uncertainty coming from neglecting the dynamics k*exp(-s*L) where k ∈ [kmin, kmax] and L ≤ Lmax. This weight is slightly optimistic, an expression for a more exact weight appears in eq (7.27), "Multivariable Feedback Control: Analysis and Design"

RobustAndOptimalControl.glover_mcfarlaneFunction
K, γ, info = glover_mcfarlane(G::AbstractStateSpace{Continuous}, γ = 1.1)

Design a controller for G that maximizes the stability margin ϵ = 1/γ with normalized coprime factor uncertainty using the method of Glover and McFarlane

γ = 1/ϵ = ||[K;I] inv(I-G*K)*inv(M)||∞
G = inv(M + ΔM)*(N + ΔN)

γ is given as a relative factor above γmin and must be greater than 1, i.e., if γ = 1.1, the controller will be designed for γ = 1.1*γmin.

We want γmin (which is always ≥ 1) as small as possible, and we usually require that γmin is less than 4, corresponding to 25% allowed coprime uncertainty.

Performance modeling is incorporated in the design by calling glover_mcfarlane on the shaped system W2*G*W1 and then forming the controller as W1*K*W2. Using this formulation, traditional loop shaping can be done on W2*G*W1.

Example:

Example 9.3 from the reference below.

using RobustAndOptimalControl, ControlSystems, Plots, Test
G = tf(200, [10, 1])*tf(1, [0.05, 1])^2     |> ss
Gd = tf(100, [10, 1])                       |> ss
W1 = tf([1, 2], [1, 1e-6])                  |> ss
K, γ, info = glover_mcfarlane(G, 1.1; W1)
@test info.γmin ≈ 2.34 atol=0.005
Gcl = extended_gangoffour(G, K) # Form closed-loop system

bodeplot([G, info.Gs, G*K], lab=["G" "" "G scaled" "" "Loop transfer"]) |> display
bodeplot(Gcl, lab=["S" "KS" "PS" "T"], plotphase=false) |> display # Plot gang of four

plot( step(Gd*feedback(1, info.Gs), 3), lab="Initial controller")
plot!(step(Gd*feedback(1, G*K), 3), lab="Robustified") |> display

nyquistplot([info.Gs, G*K], ylims=(-2,1), xlims=(-2, 1),
    Ms_circles = 1.5,
    lab = ["Initial controller" "Robustified"],
    title = "Loop transfers with and without robustified controller"
    ) |> display

Ref: Sec 9.4.1 of Skogestad, "Multivariable Feedback Control: Analysis and Design"

Extended help

Skogestad gives the following general advice:

  1. Scale the plant outputs and inputs. This is very important for most design procedures and is sometimes forgotten. In general, scaling improves the conditioning of the design problem, it enables meaningful analysis to be made of the robustness properties of the feedback system in the frequency domain, and for loop-shaping it can simplify the selection of weights. There are a variety of methods available including normalization with respect to the magnitude of the maximum or average value of the signal in question. If one is to go straight to a design the following variation has proved useful in practice: (a) The outputs are scaled such that equal magnitudes of cross-coupling into each of the outputs is equally undesirable. (b) Each input is scaled by a given percentage (say 10%) of its expected range of operation. That is, the inputs are scaled to reflect the relative actuator capabilities. An example of this type of scaling is given in the aero-engine case study of Chapter 12.

  2. Order the inputs and outputs so that the plant is as diagonal as possible. The relative gain array rga can be useful here. The purpose of this pseudo-diagonalization is to ease the design of the pre- and post-compensators which, for simplicity, will be chosen to be diagonal.

    Next, we discuss the selection of weights to obtain the shaped plant $G_s = W_2 G W_1$ where $W_1 = W_p W_a W_g$

  3. Select the elements of diagonal pre- and post-compensators $W_p$ and $W_2$ so that the singular values of $W_2 G W_p$ are desirable. This would normally mean high gain at low frequencies, roll-off rates of approximately 20 dB/decade (a slope of about 1) at the desired bandwidth(s), with higher rates at high frequencies. Some trial and error is involved here. $W_2$ is usually chosen as a constant, reflecting the relative importance of the outputs to be controlled and the other measurements being fed back to the controller. For example, if there are feedback measurements of two outputs to be controlled and a velocity signal, then $W_2$ might be chosen to be diag([1, 1, 0.1]), where 0.1 is in the velocity signal channel. $W_p$ contains the dynamic shaping. Integral action, for low frequency performance; phase-advance for reducing the roll-off rates at crossover, and phase-lag to increase the roll-off rates at high frequencies should all be placed in $W_p$ if desired. The weights should be chosen so that no unstable hidden modes are created in $G_s$.

  4. Optional: Introduce an additional gain matrix $W_g$ cascaded with $W_a$ to provide control over actuator usage. $W_g$ is diagonal and is adjusted so that actuator rate limits are not exceeded for reference demands and typical disturbances on the scaled plant outputs. This requires some trial and error.

  5. Robustly stabilize the shaped plant $G_s = W_2 G W_1$ , where $W_1 = W_p W_a W_g$, using glover_mcfarlane. First, the maximum stability margin $ϵ_{max} = 1/γ_{min}$ is calculated. If the margin is too small, $ϵmax < 0.25$, then go back and modify the weights. Otherwise, a γ-suboptimal controller is synthesized. There is usually no advantage to be gained by using the optimal controller. When $ϵ_{max}$ > 0.25 (respectively $γ_{min}$ < 4) the design is usually successful. In this case, at least 25% coprime factor uncertainty is allowed, and we also find that the shape of the open-loop singular values will not have changed much after robust stabilization. A small value of ϵmax indicates that the chosen singular value loop-shapes are incompatible with robust stability requirements. That the loop-shapes do not change much following robust stabilization if γ is small (ϵ large), is justified theoretically in McFarlane and Glover (1990).

  6. Analyze the design and if all the specifications are not met make further modifications to the weights.

  7. Implement the controller. The configuration shown in below has been found useful when compared with the conventional set up. This is because the references do not directly excite the dynamics of $K$, which can result in large amounts of overshoot (classical derivative kick). The constant prefilter ensures a steady-state gain of 1 between r and y, assuming integral action in $W_1$ or $G$ (note, the K returned by this function has opposite sign compared to that of Skogestad, so we use negative feedback here).

Anti-windup can be added to $W_1$ but putting $W_1$ on Hanus form after the synthesis, see hanus.

       ┌─────────┐      ┌────────┐      ┌────────┐
    r  │         │    us│        │  u   │        │  y
   ───►│(K*W2)(0)├──+──►│   W1   ├─────►│   G    ├────┬──►
       │         │  │-  │        │      │        │    │
       └─────────┘  │   └────────┘      └────────┘    │
                    │                                 │
                    │                                 │
                    │   ┌────────┐      ┌────────┐    │
                    │   │        │  ys  │        │    │
                    └───┤   K    │◄─────┤   W2   │◄───┘
                        │        │      │        │
                        └────────┘      └────────┘
RobustAndOptimalControl.h2normMethod
n = h2norm(sys::LTISystem; kwargs...)

A numerically robust version of norm using DescriptorSystems.jl

For keyword arguments, see the docstring of DescriptorSystems.gh2norm, reproduced below

gh2norm(sys, fast = true, offset = sqrt(ϵ), atol = 0, atol1 = atol, atol2 = atol, atolinf = atol, rtol = n*ϵ)

Compute for a descriptor system sys = (A-λE,B,C,D) the H2 norm of its transfer function matrix G(λ). The H2 norm is infinite, if sys has unstable poles, or, for a continuous-time, the system has nonzero gain at infinity. To check the stability, the eigenvalues of the pole pencil A-λE must have real parts less than for a continuous-time system or have moduli less than 1-β for a discrete-time system, where β is the stability domain boundary offset. The offset β to be used can be specified via the keyword parameter offset = β. The default value used for β is sqrt(ϵ), where ϵ is the working machine precision.

For a continuous-time system sys with E singular, a reduced order realization is determined first, without uncontrollable and unobservable nonzero finite and infinite eigenvalues of the corresponding pole pencil. The rank determinations in the performed reductions are based on rank revealing QR-decompositions with column pivoting if fast = true or the more reliable SVD-decompositions if fast = false.

The keyword arguments atol1, atol2, and rtol, specify, respectively, the absolute tolerance for the nonzero elements of A, B, C, D, the absolute tolerance for the nonzero elements of E, and the relative tolerance for the nonzero elements of A, B, C, D and E. The keyword argument atolinf is the absolute tolerance for the gain of G(λ) at λ = ∞. The used default value is atolinf = 0. The default relative tolerance is n*ϵ, where ϵ is the working machine epsilon and n is the order of the system sys. The keyword argument atol can be used to simultaneously set atol1 = atol and atol2 = atol.

RobustAndOptimalControl.h2synthesizeFunction
K, Cl = h2synthesize(P::ExtendedStateSpace, γ = nothing)

Synthesize H₂-optimal controller K and calculate the closed-loop transfer function from w to z. Ref: Cha. 14.5 in Robust and Optimal Control.

If γ = nothing, use the formulas for H₂ in Ch 14.5. If γ is a large value, the H∞ formulas are used. As γ → ∞, these two are equivalent. The h∞ formulas do a coordinate transfromation that handles slightly more general systems so if you run into an error, it might be worth trying setting γ to something large, e.g., 1000.

RobustAndOptimalControl.hankelnormMethod
n, hsv = hankelnorm(sys::LTISystem; kwargs...)

Compute the hankelnorm and the hankel singular values

For keyword arguments, see the docstring of DescriptorSystems.ghanorm, reproduced below

ghanorm(sys, fast = true, atol = 0, atol1 = atol, atol2 = atol, rtol = n*ϵ) -> (hanorm, hs)

Compute for a proper and stable descriptor system sys = (A-λE,B,C,D) with the transfer function matrix G(λ), the Hankel norm hanorm = $\small ||G(\lambda)||_H$ and the vector of Hankel singular values hs of the system.

For a proper system with E singular, the uncontrollable infinite eigenvalues of the pair (A,E) and the non-dynamic modes are elliminated using minimal realization techniques. The rank determinations in the performed reductions are based on rank revealing QR-decompositions with column pivoting if fast = true or the more reliable SVD-decompositions if fast = false.

The keyword arguments atol1, atol2, and rtol, specify, respectively, the absolute tolerance for the nonzero elements of A, B, C, D, the absolute tolerance for the nonzero elements of E, and the relative tolerance for the nonzero elements of A, B, C, D and E. The default relative tolerance is n*ϵ, where ϵ is the working machine epsilon and n is the order of the system sys. The keyword argument atol can be used to simultaneously set atol1 = atol and atol2 = atol.

RobustAndOptimalControl.hanusMethod
Wh = hanus(W)

Return Wh on Hanus form. Wh has twice the number of inputs, where the second half of the inputs are "actual inputs", e.g., potentially saturated. This is used to endow W with anti-windup protection. W must have an invertable D matrix and be minimum phase.

Ref: Sec 9.4.5 of Skogestad, "Multivariable Feedback Control: Analysis and Design"

RobustAndOptimalControl.hinfgradMethod
∇A, ∇B, ∇C, ∇D, hn, ω = hinfgrad(sys; rtolinf=1e-8, kwargs...)
∇A, ∇B, ∇C, ∇D        = hinfgrad(sys, hn, ω)

Compute the gradient of the H∞ norm w.r.t. the statespace matrices A,B,C,D. If only a system is provided, the norm hn and the peak frequency ω are automatically calculated. kwargs are sent to hinfnorm2. Note, the default tolerance to which the norm is calculated is set smaller than default for hinfnorm2, gradients will be discontinuous with any non-finite tolerance, and sensitive optimization algorithms may require even tighter tolerance.

In cases where the maximum singular value is reached at more than one frequency, a random frequency is used.

If the system is unstable, the gradients are NaN. Strategies to find an initial stabilizing controllers are outlined in Apkarian and D. Noll, "Nonsmooth H∞ Synthesis" in IEEE Transactions on Automatic Control.

An rrule for ChainRules is defined using this function, so hn is differentiable with any AD package that derives its rules from ChainRules (only applies to the hn return value, not ω).

RobustAndOptimalControl.hinfnorm2Method
n, ω = hinfnorm2(sys::LTISystem; kwargs...)

A numerically robust version of hinfnorm using DescriptorSystems.jl

For keyword arguments, see the docstring of DescriptorSystems.ghinfnorm, reproduced below

ghinfnorm(sys, rtolinf = 0.001, fast = true, offset = sqrt(ϵ), atol = 0, atol1 = atol, atol2 = atol, rtol = n*ϵ) -> (hinfnorm, fpeak)

Compute for a descriptor system sys = (A-λE,B,C,D) with the transfer function matrix G(λ) the H∞ norm hinfnorm (i.e., the peak gain of G(λ)) and the corresponding peak frequency fpeak, where the peak gain is achieved. The H∞ norm is infinite if the pole pencil A-λE has unstable zeros (i.e., sys has unstable poles). To check the stability, the eigenvalues of the pencil A-λE must have real parts less than for a continuous-time system or have moduli less than 1-β for a discrete-time system, where β is the stability domain boundary offset. The offset β to be used can be specified via the keyword parameter offset = β. The default value used for β is sqrt(ϵ), where ϵ is the working machine precision.

The keyword argument rtolinf specifies the relative accuracy for the computed infinity norm. The default value used for rtolinf is 0.001.

For a continuous-time system sys with E singular, a reduced order realization is determined first, without uncontrollable and unobservable nonzero finite and infinite eigenvalues of the corresponding pole pencil. The rank determinations in the performed reductions are based on rank revealing QR-decompositions with column pivoting if fast = true or the more reliable SVD-decompositions if fast = false.

The keyword arguments atol1, atol2, and rtol, specify, respectively, the absolute tolerance for the nonzero elements of matrices A, B, C, D, the absolute tolerance for the nonzero elements of E, and the relative tolerance for the nonzero elements of A, B, C, D and E. The default relative tolerance is n*ϵ, where ϵ is the working machine epsilon and n is the order of the system sys. The keyword argument atol can be used to simultaneously set atol1 = atol and atol2 = atol.

RobustAndOptimalControl.hinfpartitionMethod
P = hinfpartition(G, WS, WU, WT)

Transform a SISO or MIMO system G, with weighting functions WS, WU, WT into an LFT with an isolated controller, and write the resulting system, P(s), on a state-space form. Valid inputs for G are transfer functions (with dynamics, can be both MIMO and SISO, both in tf and ss forms). Valid inputs for the weighting functions are empty arrays, numbers (static gains), and LTISystems.

RobustAndOptimalControl.hinfsignalsMethod
hinfsignals(P::ExtendedStateSpace, G::LTISystem, C::LTISystem)

Use the extended state-space model, a plant and the found controller to extract the closed loop transfer functions.

  • Pcl : w → z : From input to the weighted functions
  • S : w → e : From input to error
  • CS : w → u : From input to control
  • T : w → y : From input to output
RobustAndOptimalControl.hinfsynthesizeMethod
flag, K, γ, mats = hinfsynthesize(P::ExtendedStateSpace; maxIter = 20, interval = (2 / 3, 20), verbose = false, tolerance = 1.0e-10, γrel = 1.01)

Computes an H-infinity optimal controller K for an extended plant P such that ||F_l(P, K)||∞ < γ for the smallest possible γ given P. The routine is known as the γ-iteration, and is based on the paper "State-space formulae for all stabilizing controllers that satisfy an H∞-norm bound and relations to risk sensitivity" by Glover and Doyle.

Arguments:

  • maxIter: Maximum number of γ iterations
  • interval: The starting interval for the bisection.
  • verbose: Print progress?
  • tolerance: Stop when the interval is this small.
  • γrel: If γrel > 1, the optimal γ will be found by γ iteration after which a controller will be designed for γ = γopt * γrel. It is often a good idea to design a slightly suboptimal controller, both for numerical reasons, but also since the optimal controller may contain very fast dynamics. If γrel → ∞, the computed controller will approach the 𝑯₂ optimal controller. Getting a mix between 𝑯∞ and 𝑯₂ properties is another reason to choose γrel > 1.
RobustAndOptimalControl.hsvdMethod
hsvd(sys::AbstractStateSpace{Continuous})

Return the Hankel singular values of sys, computed as the eigenvalues of QP Where Q and P are the Gramians of sys.

RobustAndOptimalControl.input_comp_sensitivityMethod
input_comp_sensitivity(P,C)
input_comp_sensitivity(l::LQGProblem)

Transfer function from load disturbance to control signal.

  • "Input" signifies that the transfer function is from the input of the plant.
  • "Complimentary" signifies that the transfer function is to an output (in this case controller output)
         ▲
         │e₁
         │  ┌─────┐
d₁────+──┴──►  P  ├─────┬──►e₄
      │-    └─────┘     │
      │                 │
      │     ┌─────┐     │
 e₂◄──┴─────┤  C  ◄──┬──+───d₂
            └─────┘  │
                     │e₃
                     ▼
  • Input sensitivity is the transfer function from d₁ to e₁, (I + CP)⁻¹
  • Output sensitivity is the transfer function from d₂ to e₂, (I + PC)⁻¹
  • Input complementary sensitivity is the transfer function from d₁ to e₂, CP(I + CP)⁻¹
  • Output complementary sensitivity is the transfer function from d₂ to e₄, PC(I + PC)⁻¹
RobustAndOptimalControl.input_sensitivityMethod
input_sensitivity(P, C)
input_sensitivity(l::LQGProblem)

Transfer function from load disturbance to total plant input.

  • "Input" signifies that the transfer function is from the input of the plant.
         ▲
         │e₁
         │  ┌─────┐
d₁────+──┴──►  P  ├─────┬──►e₄
      │-    └─────┘     │
      │                 │
      │     ┌─────┐     │
 e₂◄──┴─────┤  C  ◄──┬──+───d₂
            └─────┘  │
                     │e₃
                     ▼
  • Input sensitivity is the transfer function from d₁ to e₁, (I + CP)⁻¹
  • Output sensitivity is the transfer function from d₂ to e₂, (I + PC)⁻¹
  • Input complementary sensitivity is the transfer function from d₁ to e₂, CP(I + CP)⁻¹
  • Output complementary sensitivity is the transfer function from d₂ to e₄, PC(I + PC)⁻¹
RobustAndOptimalControl.loop_diskmarginMethod
loop_diskmargin(P, C, args...; kwargs...)

Calculate the loop-at-a-time diskmargin for each output and input of P. See also diskmargin, sim_diskmargin. Ref: "An Introduction to Disk Margins", Peter Seiler, Andrew Packard, and Pascal Gahinet https://arxiv.org/abs/2003.04771

RobustAndOptimalControl.loop_scaleFunction
loop_scale(L::LTISystem, w = 0)

Find the optimal diagonal scaling matrix D such that D\L(iw)*D has a minimized condition number at frequency w. Applicable to square L only. Use loop_scaling to obtain D.

RobustAndOptimalControl.makeweightMethod
makeweight(low, f_mid, high)
makeweight(low, (f_mid, gain_mid), high)

Create a weighting function that goes from gain low at zero frequency, through gain gain_mid to gain high at ∞

Arguments:

  • low: A number specifying the DC gain
  • mid: A number specifying the frequency at which the gain is 1, or a tuple (freq, gain). If gain_mid is not specified, the geometric mean of high and low is used.
  • high: A number specifying the gain at ∞
using ControlSystems, Plots
W = makeweight(10, (5,2), 1/10)
bodeplot(W)
hline!([10, 2, 1/10], l=(:black, :dash), primary=false)
vline!([5], l=(:black, :dash), primary=false)
RobustAndOptimalControl.muplotFunction
muplot(sys, args...; hz=false)
muplot(LTISystem[sys1, sys2...], args...; hz=false)

Plot the structured singular values of the frequency response of the LTISystem(s). This plot is similar to sigmaplot, but scales the loop-transfer function to minimize the maximum singular value. Only applicable to square systems. A frequency vector w can be optionally provided.

If hz=true, the plot x-axis will be displayed in Hertz, the input frequency vector is still treated as rad/s.

kwargs is sent as argument to Plots.plot.

RobustAndOptimalControl.mvnyquistplotFunction
fig = mvnyquistplot(sys, w;  unit_circle=false, hz = false, kwargs...)

Create a Nyquist plot of the LTISystem. A frequency vector w must be provided.

  • unit_circle: if the unit circle should be displayed

If hz=true, the hover information will be displayed in Hertz, the input frequency vector is still treated as rad/s.

kwargs is sent as argument to plot.

RobustAndOptimalControl.named_ssMethod
named_ss(sys::AbstractStateSpace{T}; x, u, y)

Create a NamedStateSpace system. This kind of system uses names rather than integer indices to refer to states, inputs and outputs

Arguments:

  • sys: A system to add names to.
  • x: A list of symbols with names of the states.
  • u: A list of symbols with names of the inputs.
  • y: A list of symbols with names of the outputs.

Default names of signals if none are provided are x,u,y.

Example

G1 = ss(1,1,1,0)
G2 = ss(1,1,1,0)
s1 = named_ss(G1, x = :x, u = :u1, y=:y1)
s2 = named_ss(G2, x = :z, u = :u2, y=:y2)

s1[:y1, :u1] # Index using symbols

fb = feedback(s1, s2, r = :r) # 
RobustAndOptimalControl.ncfmarginMethod
m, ω = ncfmargin(P, K)

Normalized coprime factor margin, defined has the inverse H∞ norm of

\[\begin{bmatrix} I \\ K \end{bmatrix} (I + PK)^{-1} \begin{bmatrix} I & P \end{bmatrix}\]

A margin ≥ 0.25-0.3 is a reasonable for robustness.

If controller K stabilizes P with margin m, then K will also stabilize if nugap(P, P̃) < m.

See also extended_gangoffour, diskmargin.

RobustAndOptimalControl.neglected_delayMethod
neglected_delay(Lmax)

Return a multiplicative weight to represent the uncertainty coming from neglecting the dynamics exp(-s*L) where L ≤ Lmax. "Multivariable Feedback Control: Analysis and Design" Ch 7.4.5

RobustAndOptimalControl.neglected_lagMethod
neglected_lag(τmax)

Return a multiplicative weight to represent the uncertainty coming from neglecting the dynamics 1/(s*τ + 1) where τ ≤ τmax. "Multivariable Feedback Control: Analysis and Design" Ch 7.4.5

RobustAndOptimalControl.nugapMethod
nugap(sys0::LTISystem, sys1::LTISystem; kwargs...)

Compute the ν-gap metric between two systems. See also ncfmargin.

For keyword arguments, see the docstring of DescriptorSystems.gnugap, reproduced below

gnugap(sys1, sys2; freq = ω, rtolinf = 0.00001, fast = true, offset = sqrt(ϵ), 
       atol = 0, atol1 = atol, atol2 = atol, rtol = n*ϵ) -> (nugapdist, fpeak)

Compute the ν-gap distance nugapdist between two descriptor systems sys1 = (A1-λE1,B1,C1,D1) and sys2 = (A2-λE2,B2,C2,D2) and the corresponding frequency fpeak (in rad/TimeUnit), where the ν-gap distance achieves its peak value.

If freq = missing, the resulting nugapdist satisfies 0 <= nugapdist <= 1. The value nugapdist = 1 results, if the winding number is different of zero in which case fpeak = [].

If freq = ω, where ω is a given vector of real frequency values, the resulting nugapdist is a vector of pointwise ν-gap distances of the dimension of ω, whose components satisfies 0 <= maximum(nugapdist) <= 1. In this case, fpeak is the frequency for which the pointwise distance achieves its peak value. All components of nugapdist are set to 1 if the winding number is different of zero in which case fpeak = [].

The stability boundary offset, β, to be used to assess the finite zeros which belong to the boundary of the stability domain can be specified via the keyword parameter offset = β. Accordingly, for a continuous-time system, these are the finite zeros having real parts within the interval [-β,β], while for a discrete-time system, these are the finite zeros having moduli within the interval [1-β,1+β]. The default value used for β is sqrt(ϵ), where ϵ is the working machine precision.

Pencil reduction algorithms are employed to compute range and coimage spaces which perform rank decisions based on rank revealing QR-decompositions with column pivoting if fast = true or the more reliable SVD-decompositions if fast = false.

The keyword arguments atol1, atol2 and rtol, specify, respectively, the absolute tolerance for the nonzero elements of A1, A2, B1, B2, C1, C2, D1 and D2, the absolute tolerance for the nonzero elements of E1 and E2, and the relative tolerance for the nonzero elements of all above matrices. The default relative tolerance is n*ϵ, where ϵ is the working machine epsilon and n is the maximum of the orders of the systems sys1 and sys2. The keyword argument atol can be used to simultaneously set atol1 = atol, atol2 = atol.

The keyword argument rtolinf specifies the relative accuracy to be used to compute the ν-gap as the infinity norm of the relevant system according to [1]. The default value used for rtolinf is 0.00001.

Method: The evaluation of ν-gap uses the definition proposed in [1], extended to generalized LTI (descriptor) systems. The computation of winding number is based on enhancements covering zeros on the boundary of the stability domain and infinite zeros.

References:

[1] G. Vinnicombe. Uncertainty and feedback: H∞ loop-shaping and the ν-gap metric. Imperial College Press, London, 2001.

RobustAndOptimalControl.output_comp_sensitivityMethod
output_comp_sensitivity(P,C)
output_comp_sensitivity(l::LQGProblem)

Transfer function from measurement noise / reference to plant output.

  • "output" signifies that the transfer function is from the output of the plant.
  • "Complimentary" signifies that the transfer function is to an output (in this case plant output)
         ▲
         │e₁
         │  ┌─────┐
d₁────+──┴──►  P  ├─────┬──►e₄
      │-    └─────┘     │
      │                 │
      │     ┌─────┐     │
 e₂◄──┴─────┤  C  ◄──┬──+───d₂
            └─────┘  │
                     │e₃
                     ▼
  • Input sensitivity is the transfer function from d₁ to e₁, (I + CP)⁻¹
  • Output sensitivity is the transfer function from d₂ to e₂, (I + PC)⁻¹
  • Input complementary sensitivity is the transfer function from d₁ to e₂, CP(I + CP)⁻¹
  • Output complementary sensitivity is the transfer function from d₂ to e₄, PC(I + PC)⁻¹
RobustAndOptimalControl.output_sensitivityMethod
output_sensitivity(P, C)
output_sensitivity(l::LQGProblem)

Transfer function from measurement noise / reference to control signal.

  • "output" signifies that the transfer function is from the output of the plant.
         ▲
         │e₁
         │  ┌─────┐
d₁────+──┴──►  P  ├─────┬──►e₄
      │-    └─────┘     │
      │                 │
      │     ┌─────┐     │
 e₂◄──┴─────┤  C  ◄──┬──+───d₂
            └─────┘  │
                     │e₃
                     ▼
  • Input sensitivity is the transfer function from d₁ to e₁, (I + CP)⁻¹
  • Output sensitivity is the transfer function from d₂ to e₂, (I + PC)⁻¹
  • Input complementary sensitivity is the transfer function from d₁ to e₂, CP(I + CP)⁻¹
  • Output complementary sensitivity is the transfer function from d₂ to e₄, PC(I + PC)⁻¹
RobustAndOptimalControl.robstabMethod
robstab(M0::UncertainSS, w=exp10.(LinRange(-3, 3, 1500)); kwargs...)

Return the robust stability margin of an uncertain model, defined as the inverse of the structured singular value. Currently, only diagonal complex perturbations supported.

RobustAndOptimalControl.sensitivityMethod

See output_sensitivity

         ▲
         │e₁
         │  ┌─────┐
d₁────+──┴──►  P  ├─────┬──►e₄
      │-    └─────┘     │
      │                 │
      │     ┌─────┐     │
 e₂◄──┴─────┤  C  ◄──┬──+───d₂
            └─────┘  │
                     │e₃
                     ▼
  • Input sensitivity is the transfer function from d₁ to e₁, (I + CP)⁻¹
  • Output sensitivity is the transfer function from d₂ to e₂, (I + PC)⁻¹
  • Input complementary sensitivity is the transfer function from d₁ to e₂, CP(I + CP)⁻¹
  • Output complementary sensitivity is the transfer function from d₂ to e₄, PC(I + PC)⁻¹
RobustAndOptimalControl.show_constructionMethod
show_construction([io::IO,] sys::LTISystem; name = "temp", letb = true)

Print code to io that reconstructs sys.

  • letb: If true, the code is surrounded by a let block.
julia> sys = ss(tf(1, [1, 1]))
StateSpace{Continuous, Float64}
A = 
 -1.0
B = 
 1.0
C = 
 1.0
D = 
 0.0

Continuous-time state-space model

julia> show_construction(sys, name="Jörgen")
sys = let
    JörgenA = [-1.0;;]
    JörgenB = [1.0;;]
    JörgenC = [1.0;;]
    JörgenD = [0.0;;]
    ss(JörgenA, JörgenB, JörgenC, JörgenD)
end
RobustAndOptimalControl.sim_diskmarginFunction
sim_diskmargin(P::LTISystem, C::LTISystem, σ::Real = 0)

Simultaneuous diskmargin at both outputs and inputs of P. Ref: "An Introduction to Disk Margins", Peter Seiler, Andrew Packard, and Pascal Gahinet https://arxiv.org/abs/2003.04771 See also ncfmargin.

RobustAndOptimalControl.specificationplotFunction
specificationplot([S,CS,T], [WS,WU,WT])

This function visualizes the control synthesis using the hInf_synthesize with the three weighting functions {WS(jω), WU(jω), WT(jω)} inverted and scaled by γ, against the corresponding transfer fucntions {S(jω), C(jω)S(jω), T(jω)}, to verify visually that the specifications are met. This may be run using both MIMO and SISO systems.

RobustAndOptimalControl.structured_singular_valueMethod
μ = structured_singular_value(M; tol=1e-4, scalings=false, dynamic=false)

Compute (an upper bound of) the structured singular value μ for diagonal Δ of complex perturbations (other structures of Δ are not yet supported). M is assumed to be an (n × n × N_freq) array or a matrix.

We currently don't have any methods to compute a lower bound, but if all perturbations are complex the spectral radius ρ(M) is always a lower bound (usually not a good one).

If scalings = true, return also a n × nf matrix Dm with the diagonal scalings D such that

D = Diagonal(Dm[:, i])
σ̄(D\M[:,:,i]*D)

is minimized.

If dynamic = true, the perturbations are assumed to be time-varying Δ(t). In this case, the same scaling is used for all frequencies and the returned D if scalings=true is a vector d such that D = Diagonal(d).

RobustAndOptimalControl.sumblockMethod
sumblock(ex::String; Ts = 0, n = 1)

Create a summation node that sums (or subtracts) vectors of length n.

Arguments:

  • Ts: Sample time
  • n: The length of the input and output vectors. Set n=1 for scalars.

Examples:

julia> sumblock("uP = vf + yL")
NamedStateSpace{Continuous, Int64}
D = 
 1  1

With state  names: 
     input  names: vf yL
     output names: uP


julia> sumblock("x_diff = xr - xh"; n=3)
NamedStateSpace{Continuous, Int64}
D = 
 1  0  0  -1   0   0
 0  1  0   0  -1   0
 0  0  1   0   0  -1

With state  names: 
     input  names: xr1 xr2 xr3 xh1 xh2 xh3
     output names: x_diff1 x_diff2 x_diff3
     

julia> sumblock("a = b + c - d")
NamedStateSpace{Continuous, Int64}
D = 
 1  1  -1

With state  names: 
     input  names: b c d
     output names: a
RobustAndOptimalControl.ussFunction
uss(d::AbstractVector{<:δ}, Ts = nothing)

Create a diagonal uncertain statespace object with the uncertain elements d on the diagonal.

RobustAndOptimalControl.ussFunction
uss(D11, D12, D21, D22, Δ, Ts = nothing)

Create an uncertain statespace object with only gin matrices.

RobustAndOptimalControl.ussFunction
uss(D::AbstractArray, Δ, Ts = nothing)

If only a single D matrix is provided, it's treated as D11 if Δ is given, and as D22 if no Δ is provided.

RobustAndOptimalControl.vec2sysFunction
vec2sys(v::AbstractArray, ny::Int, nu::Int, ts = nothing)

Create a statespace system from the parameters

v = vec(sys) = [vec(sys.A); vec(sys.B); vec(sys.C); vec(sys.D)]

Use vec(sys) to create v.

This can be useful in order to convert to and from vectors for, e.g., optimization.

julia> sys  = ss(tf(1, [1, 1]))
StateSpace{Continuous, Float64}
A = 
 -1.0
B = 
 1.0
C = 
 1.0
D = 
 0.0

Continuous-time state-space model

julia> v    = vec(sys)
4-element Vector{Float64}:
 -1.0
  1.0
  1.0
  0.0

julia> sys2 = vec2sys(v, sys.ny, sys.nu)
StateSpace{Continuous, Float64}
A = 
 -1.0
B = 
 1.0
C = 
 1.0
D = 
 0.0

Continuous-time state-space model
RobustAndOptimalControl.δcFunction
δc(val::Complex = complex(0.0), radius::Real = 1.0, name)

Create a complex, uncertain parameter. If no name is given, a boring name will be generated automatically.

RobustAndOptimalControl.δrFunction
δr(val::Real = 0.0, radius::Real = 1.0, name)

Create a real, uncertain parameter. If no name is given, a boring name will be generated automatically.