add generic transfer function (#233)

* add generic transfer function

* formatting

* add test

Author: baggepinnen

src/Blocks/Blocks.jl

@@ -25,7 +25,7 @@ include("sources.jl")
 export Limiter, DeadZone, SlewRateLimiter
 include("nonlinear.jl")
 
-export Integrator, Derivative, FirstOrder, SecondOrder, StateSpace
+export Integrator, Derivative, FirstOrder, SecondOrder, StateSpace, TransferFunction
 export PI, LimPI, PID, LimPID
 include("continuous.jl")
 

src/Blocks/continuous.jl

@@ -553,3 +553,85 @@ linearized around the operating point `x₀, u₀`, we have `y0, u0 = h(x₀, u
 end
 
 StateSpace(A, B, C, D = nothing; kwargs...) = StateSpace(; A, B, C, D, kwargs...)
+
+symbolic_eps(t) = eps(t)
+@register_symbolic symbolic_eps(t)
+
+"""
+    TransferFunction(; b, a, name)
+
+A single input, single output, linear time-invariant system provided as a transfer-function.
+```
+Y(s) = b(s) / a(s)  U(s)
+```
+where `b` and `a` are vectors of coefficients of the numerator and denominator polynomials, respectively, ordered such that the coefficient of the highest power of `s` is first.
+
+The internal state realization is on controller canonical form, with state variable `x`, output variable `y` and input variable `u`. For numerical robustness, the realization used by the integrator is scaled by the last entry of the `a` parameter. The internally scaled state variable is available as `x_scaled`.
+
+To set the initial state, it's recommended to set the initial condition for `x`, and let that of `x_scaled` be computed automatically.
+
+# Parameters:
+- `b`: Numerator polynomial coefficients, e.g., `2s + 3` is specified as `[2, 3]`
+- `a`: Denomenator polynomial coefficients, e.g., `s² + 2ωs + ω^2` is specified as `[1, 2ω, ω^2]`
+
+# Connectors:
+  - `input`
+  - `output`
+
+See also [`StateSpace`](@ref) which handles MIMO systems, as well as [ControlSystemsMTK.jl](https://juliacontrol.github.io/ControlSystemsMTK.jl/stable/) for an interface between [ControlSystems.jl](https://juliacontrol.github.io/ControlSystems.jl/stable/) and ModelingToolkit.jl for advanced manipulation of transfer functions and linear statespace systems. For linearization, see [`linearize`](@ref) and [Linear Analysis](https://docs.sciml.ai/ModelingToolkitStandardLibrary/stable/API/linear_analysis/).
+"""
+@component function TransferFunction(; b = [1], a = [1, 1], name)
+    nb = length(b)
+    na = length(a)
+    nb <= na ||
+        error("Transfer function is not proper, the numerator must not be longer than the denominator")
+    nx = na - 1
+    nbb = max(0, na - nb)
+
+    @named begin
+        input = RealInput()
+        output = RealOutput()
+    end
+
+    @parameters begin
+        b[1:nb] = b,
+        [
+            description = "Numerator coefficients of transfer function (e.g., 2s + 3 is specified as [2,3])",
+        ]
+        a[1:na] = a,
+        [
+            description = "Denominator coefficients of transfer function (e.g., `s² + 2ωs + ω^2` is specified as [1, 2ω, ω^2])",
+        ]
+        bb[1:(nbb + nb)] = [zeros(nbb); b]
+        d = bb[1] / a[1], [description = "Direct feedthrough gain"]
+    end
+
+    a = collect(a)
+    @parameters a_end = ifelse(a[end] > 100 * symbolic_eps(sqrt(a' * a)), a[end], 1.0)
+
+    pars = [collect(b); a; collect(bb); d; a_end]
+    @variables begin
+        x(t)[1:nx] = zeros(nx),
+        [description = "State of transfer function on controller canonical form"]
+        x_scaled(t)[1:nx] = collect(x) * a_end, [description = "Scaled vector x"]
+        u(t), [description = "Input of transfer function"]
+        y(t), [description = "Output of transfer function"]
+    end
+
+    x = collect(x)
+    x_scaled = collect(x_scaled)
+
+    sts = [x; x_scaled; y; u]
+
+    if nx == 0
+        eqs = [y ~ d * u]
+    else
+        eqs = [D(x_scaled[1]) ~ (-a[2:na]'x_scaled + a_end * u) / a[1]
+            D.(x_scaled[2:nx]) .~ x_scaled[1:(nx - 1)]
+            y ~ ((bb[2:na] - d * a[2:na])'x_scaled) / a_end + d * u
+            x .~ x_scaled ./ a_end]
+    end
+    push!(eqs, input.u ~ u)
+    push!(eqs, output.u ~ y)
+    compose(ODESystem(eqs, t, sts, pars; name = name), input, output)
+end

test/Blocks/continuous.jl

@@ -1,6 +1,7 @@
 using ModelingToolkit, ModelingToolkitStandardLibrary, OrdinaryDiffEq
 using ModelingToolkitStandardLibrary.Blocks
 using OrdinaryDiffEq: ReturnCode.Success
+using Test
 
 @parameters t
 
@@ -408,4 +409,71 @@ end
         @test sol[plant.output.u][end]≈re_val atol=1e-3 # zero control error after 100s
         @test all(-1.5 .<= sol[pid_controller.ctr_output.u] .<= 1.5) # test limit
     end
+
+    @testset "TransferFunction" begin
+        pt1_func(t, k, T) = k * (1 - exp(-t / T)) # Known solution to first-order system
+
+        @named c = Constant(; k = 1)
+        @named pt1 = TransferFunction(b = [1.2], a = [3.14, 1])
+        @named iosys = ODESystem(connect(c.output, pt1.input), t, systems = [pt1, c])
+        sys = structural_simplify(iosys)
+        prob = ODEProblem(sys, Pair[pt1.a_end => 1], (0.0, 100.0))
+        sol = solve(prob, Rodas4())
+        @test sol.retcode == Success
+        @test sol[pt1.output.u]≈pt1_func.(sol.t, 1.2, 3.14) atol=1e-3
+
+        # Test logic for a_end by constructing an integrator
+        @named c = Constant(; k = 1)
+        @named pt1 = TransferFunction(b = [1.2], a = [3.14, 0])
+        @named iosys = ODESystem(connect(c.output, pt1.input), t, systems = [pt1, c])
+        sys = structural_simplify(iosys)
+        prob = ODEProblem(sys, Pair[pt1.a_end => 1], (0.0, 100.0))
+        sol = solve(prob, Rodas4())
+        @test sol.retcode == Success
+        @test sol[pt1.output.u] ≈ sol.t .* (1.2 / 3.14)
+        @test sol[pt1.x[1]] ≈ sol.t .* (1 / 3.14) # Test that scaling of state works properly
+
+        # Test higher order
+
+        function pt2_func(t, k, w, d)
+            y = if d == 0
+                -k * (-1 + cos(t * w))
+            else
+                d = complex(d)
+                real(k * (1 +
+                      (-cosh(sqrt(-1 + d^2) * t * w) -
+                       (d * sinh(sqrt(-1 + d^2) * t * w)) / sqrt(-1 + d^2)) /
+                      exp(d * t * w)))
+            end
+        end
+
+        k, w, d = 1.0, 1.0, 0.5
+        @named pt1 = TransferFunction(b = [w^2], a = [1, 2d * w, w^2])
+        @named iosys = ODESystem(connect(c.output, pt1.input), t, systems = [pt1, c])
+        sys = structural_simplify(iosys)
+        prob = ODEProblem(sys, Pair[pt1.a_end => 1], (0.0, 100.0))
+        sol = solve(prob, Rodas4())
+        @test sol.retcode == Success
+        @test sol[pt1.output.u]≈pt2_func.(sol.t, k, w, d) atol=1e-3
+
+        # test zeros (high-pass version of first test)
+        @named c = Constant(; k = 1)
+        @named pt1 = TransferFunction(b = [1, 0], a = [1, 1])
+        @named iosys = ODESystem(connect(c.output, pt1.input), t, systems = [pt1, c])
+        sys = structural_simplify(iosys)
+        prob = ODEProblem(sys, Pair[pt1.a_end => 1], (0.0, 100.0))
+        sol = solve(prob, Rodas4())
+        @test sol.retcode == Success
+        @test sol[pt1.output.u]≈1 .- pt1_func.(sol.t, 1, 1) atol=1e-3
+        @test sol[pt1.x[1]]≈pt1_func.(sol.t, 1, 1) atol=1e-3 # Test that scaling of state works properly
+
+        # Test with no state
+        @named pt1 = TransferFunction(b = [2.7], a = [pi])
+        @named iosys = ODESystem(connect(c.output, pt1.input), t, systems = [pt1, c])
+        sys = structural_simplify(iosys)
+        prob = ODEProblem(sys, Pair[pt1.a_end => 1], (0.0, 100.0))
+        sol = solve(prob, Rodas4())
+        @test sol.retcode == Success
+        @test all(==(2.7 / pi), sol[pt1.output.u])
+    end
 end