move root locus implementation to ControlSystemsBase (#1013)

* move root locus implementation to ControlSystemsBase

* better error messages

* rm import

* include file

* add docs

* update paths

* update deps

* rm import

* import Hungarian

Author: baggepinnen

Project.toml

@@ -9,7 +9,6 @@ ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 DelayDiffEq = "bcd4f6db-9728-5f36-b5f7-82caef46ccdb"
 DiffEqCallbacks = "459566f4-90b8-5000-8ac3-15dfb0a30def"
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
-Hungarian = "e91730f6-4275-51fb-a7a0-7064cfbd3b39"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"
@@ -25,7 +24,6 @@ ControlSystemsBase = "1.3"
 DelayDiffEq = "5.31"
 DiffEqCallbacks = "2.16, 3, 4"
 ForwardDiff = "0.10, 1"
-Hungarian = "0.6, 0.7"
 OrdinaryDiffEq = "6.60"
 RecipesBase = "1"
 Reexport = "1"

docs/src/lib/plotting.md

@@ -16,6 +16,10 @@ Pages   = [libpath*"/plotting.jl"]
 Order   = [:function]
 Private = false
 ```
+```@docs
+ControlSystemsBase.rlocusplot
+```
+- To plot simulation results such as step and impulse responses, use `plot(::SimResult)`, see also [`lsim`](@ref).
 
 ## Examples
 

docs/src/man/introduction.md

@@ -10,14 +10,14 @@ For workflows that do not require continuous-time simulation, you may instead op
 ```julia
 using Pkg; Pkg.add("ControlSystemsBase")
 ```
-ControlSystemsBase contains all functionality of ControlSystems except continuous-time simulation and root locus, and is *considerably* faster to load and precompile. To enjoy the faster pre-compilation, do not even install ControlSystems since this will cause pre-compilation of OrdinaryDiffEq, which can take several minutes.
+ControlSystemsBase contains all functionality of ControlSystems except continuous-time simulation, and is *considerably* faster to load and precompile. To enjoy the faster pre-compilation, do not even install ControlSystems since this will cause pre-compilation of OrdinaryDiffEq, which can take several minutes.
 
 ## Basic functions
 ```@meta
 DocTestSetup = quote
     using ControlSystems
     P = tf([1],[1,1])
-    T = P/(1+P)
+    T = feedback(P)
     plotsDir = joinpath(dirname(pathof(ControlSystems)), "..", "docs", "build", "plots")
     mkpath(plotsDir)
     save_docs_plot(name) = Plots.savefig(joinpath(plotsDir,name))
@@ -29,7 +29,7 @@ State-space systems can be created using the function [`ss`](@ref) and transfer
 Example:
 ```jldoctest INTRO
 P = tf([1.0],[1,1])
-T = P/(1+P)
+T = P/(1+P)         # Not recommended, see below
 
 # output
 
@@ -74,3 +74,8 @@ using Plots
 bodeplot(tf(1,[1,2,1]))
 ```
 
+
+Step, impulse and other simulation responses do not have their own plot functions, instead, call `plot` on the result of the simulation function:
+```@example INTRO
+plot(step(tf(1,[1,0.5,1]), 20))
+```

lib/ControlSystemsBase/Project.toml

@@ -6,6 +6,7 @@ version = "1.17.5"
 
 [deps]
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
+Hungarian = "e91730f6-4275-51fb-a7a0-7064cfbd3b39"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 MacroTools = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09"
 MatrixEquations = "99c1a7ee-ab34-5fd5-8076-27c950a045f4"
@@ -32,6 +33,7 @@ Aqua = "0.5"
 ComponentArrays = "0.15"
 DSP = "0.6.1, 0.7, 0.8"
 ForwardDiff = "0.10, 1.0"
+Hungarian = "0.7.0"
 ImplicitDifferentiation = "0.7.2"
 LinearAlgebra = "<0.0.1, 1"
 MacroTools = "0.5"

lib/ControlSystemsBase/src/ControlSystemsBase.jl

@@ -112,6 +112,8 @@ export  LTISystem,
         pade,
         thiran,
         nonlinearity,
+        rlocus,
+        rlocusplot,
         # demo systems
         ssrand,
         DemoSystems, # A module containing some example systems
@@ -136,6 +138,7 @@ import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op
 import Base: getproperty, getindex
 import Base: exp # for exp(-s)
 import LinearAlgebra: BlasFloat
+import Hungarian
 
 export lyap # Make sure LinearAlgebra.lyap is available
 export plyap
@@ -211,6 +214,8 @@ include("types/staticsystems.jl")
 
 include("plotting.jl")
 
+include("root_locus.jl")
+
 @deprecate pole poles
 @deprecate tzero tzeros
 @deprecate num numvec

src/root_locus.jl renamed to lib/ControlSystemsBase/src/root_locus.jl

@@ -2,54 +2,117 @@ const Polynomials = ControlSystemsBase.Polynomials
 import ControlSystemsBase.RootLocusResult
 @userplot Rlocusplot
 
+"""
+    rlocusplot(siso_sys)
+    rlocusplot(sys::StateSpace, K::Matrix; output=false)
+
+Plot the root locus of a system under feedback.
+
+If a SISO system is passed, the feedback gain `K` is a scalar that ranges from 0 to `K` (if provided). If a `StateSpace` system is passed, `K` is a matrix that defines the feedback gain, and the poles are computed as `K` ranges from `0*K` to `1*K`. In this case, `K` is assumed to be a state-feedback matrix of dimension `(nu, nx)`. To compute the poles for output feedback, pass `output = true` and `K` of dimension `(nu, ny)`.
+"""
+rlocusplot
+
 
-function getpoles(G, K::Number; kwargs...)
-    issiso(G) || error("root locus only supports SISO systems")
+function getpoles(G, K::Number; tol = 1e-2, initial_stepsize = 1e-3, kwargs...)
+    issiso(G) || error("root locus with scalar gain only supports SISO systems, did you intend to pass a feedback gain matrix `K`?")
     G isa TransferFunction || (G = tf(G))
     P = numpoly(G)[]
     Q = denpoly(G)[]
     T = float(typeof(K))
     ϵ = eps(T)
     nx = length(Q)
-    D = zeros(nx-1, nx-1) # distance matrix
-    prevpoles = ComplexF64[]
+    
+    # Scale tolerance with system order
+    tol = tol * (nx - 1)
+    
+    poleout_list = Vector{Vector{ComplexF64}}() # To store pole sets at each accepted step
+    k_scalars_collected = Float64[] # To store accepted k_scalar values
+    
+    prevpoles = ComplexF64[] # Initialize prevpoles for the first iteration
     temppoles = zeros(ComplexF64, nx-1)
-    f = function (_,_,k)
+    D = zeros(nx-1, nx-1) # distance matrix
+    
+    stepsize = initial_stepsize
+    k_scalar = 0.0
+    
+    # Function to compute poles for a given k value
+    compute_poles = function(k)
         if k == 0 && length(P) > length(Q)
             # More zeros than poles, make sure the vector of roots is of correct length when k = 0
             # When this happens, there are fewer poles for k = 0, these poles can be seen as being located somewhere at Inf
             # We get around the problem by not allowing k = 0 for non-proper systems.
             k = ϵ
         end
-        newpoles = ComplexF64.(Polynomials.roots(k[1]*P+Q))
+        ComplexF64.(Polynomials.roots(k*P+Q))
+    end
+    
+    # Initial poles at k_scalar = 0.0
+    initial_poles = compute_poles(0.0)
+    push!(poleout_list, initial_poles)
+    push!(k_scalars_collected, 0.0)
+    prevpoles = initial_poles # Set prevpoles for the first actual step
+    
+    while k_scalar < K
+        # Propose a new k_scalar value
+        next_k_scalar = min(K, k_scalar + stepsize)
+        
+        # Calculate poles for the proposed next_k_scalar
+        current_poles_proposed = compute_poles(next_k_scalar)
+        
+        # Calculate cost using Hungarian algorithm
         if !isempty(prevpoles)
-            D .= abs.(newpoles .- transpose(prevpoles))
+            D .= abs.(current_poles_proposed .- transpose(prevpoles))
             assignment, cost = Hungarian.hungarian(D)
-            for i = 1:nx-1
-                temppoles[assignment[i]] = newpoles[i]
+        else
+            cost = 0.0
+            assignment = collect(1:nx-1)
+        end
+        
+        # Adaptive step size logic
+        if cost > 2 * tol # Cost is too high, reject step and reduce stepsize
+            stepsize /= 2.0
+            # Ensure stepsize doesn't become too small
+            if stepsize < 100*eps(T)
+                @warn "Step size became extremely small, potentially stuck. Breaking loop."
+                break
+            end
+            # Do not update k_scalar, try again with smaller stepsize
+        else # Step is acceptable
+            # Sort poles using the assignment from Hungarian algorithm
+            if !isempty(prevpoles)
+                for i = 1:nx-1
+                    temppoles[assignment[i]] = current_poles_proposed[i]
+                end
+                current_poles_sorted = copy(temppoles)
+            else
+                current_poles_sorted = current_poles_proposed
+            end
+            
+            # Accept the step
+            push!(poleout_list, current_poles_sorted)
+            push!(k_scalars_collected, next_k_scalar)
+            prevpoles = current_poles_sorted # Update prevpoles for the next iteration
+            k_scalar = next_k_scalar # Advance k_scalar
+            
+            if cost < tol # Cost is too low, increase stepsize
+                stepsize *= 1.1
             end
-            newpoles .= temppoles
+            # Cap stepsize to prevent overshooting K significantly in a single step
+            stepsize = min(stepsize, K/10)
+        end
+        
+        # Break if k_scalar has reached or exceeded K
+        if k_scalar >= K
+            break
         end
-        prevpoles = newpoles
-        newpoles
-    end
-    prob       = OrdinaryDiffEq.ODEProblem(f,f(0.,0.,0),(0,K[end]))
-    integrator = OrdinaryDiffEq.init(prob,OrdinaryDiffEq.Tsit5(),reltol=1e-8,abstol=1e-8)
-    ts         = Vector{Float64}()
-    poleout    = Vector{Vector{ComplexF64}}()
-    push!(poleout,integrator.k[1])
-    push!(ts,0)
-    for i in integrator
-        push!(poleout,integrator.k[end])
-        push!(ts,integrator.t[1])
     end
-    poleout = copy(hcat(poleout...)')
-    poleout, ts
+    
+    return hcat(poleout_list...)' |> copy, k_scalars_collected
 end
 
 
 function getpoles(G, K::AbstractVector{T}) where {T<:Number}
-    issiso(G) || error("root locus only supports SISO systems")
+    issiso(G) || error("root locus with scalar gain only supports SISO systems, did you intend to pass a feedback gain matrix `K`?")
     G isa TransferFunction || (G = tf(G))
     P, Q = numpoly(G)[], denpoly(G)[]
     poleout = Matrix{ComplexF64}(undef, Polynomials.degree(Q), length(K))
@@ -169,7 +232,7 @@ end
 """
     roots, Z, K = rlocus(P::LTISystem, K = 500)
 
-Compute the root locus of the SISO LTISystem `P` with a negative feedback loop and feedback gains between 0 and `K`. `rlocus` will use an adaptive step-size algorithm to determine the values of the feedback gains used to generate the plot.
+Compute the root locus of the LTISystem `P` with a negative feedback loop and feedback gains between 0 and `K`. `rlocus` will use an adaptive step-size algorithm to determine the values of the feedback gains used to generate the plot.
 
 `roots` is a complex matrix containing the poles trajectories of the closed-loop `1+k⋅G(s)` as a function of `k`, `Z` contains the zeros of the open-loop system `G(s)` and `K` the values of the feedback gain.
 
@@ -236,8 +299,9 @@ end
 
 """
     rlocusplot(P::LTISystem; K)
+    rlocusplot(P::StateSpace, K::Matrix; output = false)
 
-Plot the root locus of the SISO LTISystem `P` as computed by `rlocus`.
+Plot the root locus of the LTISystem `P` as computed by `rlocus`.
 """
 @recipe function rlocusplot(::Type{Rlocusplot}, p::Rlocusplot; K=500, output=false)
     if length(p.args) >= 2

lib/ControlSystemsBase/test/runtests.jl

@@ -43,6 +43,8 @@ my_tests = [
             "test_plots",
             "test_dsp",
             "test_implicit_diff",
+            "test_rootlocus",
+            "test_root_locus_matrix",
             ]
 
 @testset "All Tests" begin

test/test_root_locus_matrix.jl renamed to lib/ControlSystemsBase/test/test_root_locus_matrix.jl

@@ -8,19 +8,17 @@ using ControlSystemsBase: issiso, ninputs, noutputs, nstates, numeric_type
 using LinearAlgebra
 import OrdinaryDiffEq
 import LinearAlgebra: BlasFloat
-import Hungarian # For pole assignment in rlocusplot
 import DiffEqCallbacks: SavingCallback, SavedValues
 import DelayDiffEq
 using SparseArrays
 using StaticArrays
 using RecipesBase
 using Printf
 
-export Simulator, rlocus, rlocusplot
+export Simulator
 
 include("timeresp.jl")
 include("simulators.jl")
-include("root_locus.jl")
 
 # The path has to be evaluated upon initial import
 const __CONTROLSYSTEMS_SOURCE_DIR__ = dirname(Base.source_path())