From 1364080f505bfef90cf0fc4568f0940e18a7579c Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 23 Jul 2025 15:58:59 +0200 Subject: [PATCH 1/3] add root-locus methods for matrix gains --- src/root_locus.jl | 171 +++++++++++++++++++++++++++++++- test/runtests_controlsystems.jl | 1 + test/test_root_locus_matrix.jl | 102 +++++++++++++++++++ 3 files changed, 269 insertions(+), 5 deletions(-) create mode 100644 test/test_root_locus_matrix.jl diff --git a/src/root_locus.jl b/src/root_locus.jl index 98ab43e1f..6a6346fea 100644 --- a/src/root_locus.jl +++ b/src/root_locus.jl @@ -69,6 +69,148 @@ function getpoles(G, K::AbstractVector{T}) where {T<:Number} copy(poleout'), K end +# function getpoles(sys::StateSpace, K_matrix::AbstractMatrix; tol = 1e-6) +# # Ensure the system is StateSpace (already enforced by method signature) +# A, B = sys.A, sys.B +# nx = size(A, 1) # State dimension + +# # Check for compatibility of K_matrix dimensions with B +# if size(K_matrix, 2) != nx +# error("The number of columns in K_matrix ($(size(K_matrix, 2))) must match the number of states ($(nx)).") +# end +# # The number of rows in K_matrix must match the number of inputs (columns of B) +# if size(K_matrix, 1) != size(B, 2) +# error("The number of rows in K_matrix ($(size(K_matrix, 1))) must match the number of inputs (columns of B, which is $(size(B, 2))).") +# end + +# poleout = Matrix{ComplexF64}(undef, nx, length(k_values)) +# D = zeros(nx, nx) # distance matrix for Hungarian algorithm +# temppoles = zeros(ComplexF64, nx) # temporary storage for sorted poles + +# prevpoles = ComplexF64[] # Initialize prevpoles for the first iteration + +# stepsize = 1e-6 +# k_scalar = 0.0 + +# while k_scalar < 1 +# A_cl = A - k_scalar * B * K_matrix +# current_poles = eigvals(A_cl) + +# if i > 1 && length(current_poles) == length(prevpoles) +# for r in 1:nx +# for c in 1:nx +# D[r, c] = abs(current_poles[r] - prevpoles[c]) +# end +# end +# assignment, cost = Hungarian.hungarian(D) +# for j = 1:nx +# temppoles[assignment[j]] = current_poles[j] +# end +# poleout[:, i] .= temppoles +# else +# poleout[:, i] .= current_poles # For the first iteration or mismatch, just assign +# end +# prevpoles = poleout[:, i] # Update prevpoles for the next iteration +# end +# return copy(poleout'), k_values .* Ref(K_matrix) # Return transposed pole matrix and k_values as a vector +# end + +""" + getpoles(sys::StateSpace, K::AbstractMatrix; tol = 1e-2, initial_stepsize = 1e-3, output=false) + +Compute the poles of the closed-loop system defined by `sys` with feedback gains `γ*K` where `γ` is a scalar that ranges from 0 to 1. + +If `output = true`, `K` is assumed to be an output feedback matrix of dim `(nu, ny)` +""" +function getpoles(sys::StateSpace, K_matrix::AbstractMatrix; tol = 1e-2, initial_stepsize = 1e-3, output=false) + (; A, B, C) = sys + nx = size(A, 1) # State dimension + ny = size(C, 1) # Output dimension + tol = tol*nx # Scale tolerance with state dimension + # Check for compatibility of K_matrix dimensions with B + if size(K_matrix, 2) != (output ? ny : nx) + error("The number of columns in K_matrix ($(size(K_matrix, 2))) must match the state dimension ($(nx)) or output dimension ($(ny)) depending on whether output feedback is used.") + end + if size(K_matrix, 1) != size(B, 2) + error("The number of rows in K_matrix ($(size(K_matrix, 1))) must match the number of inputs (columns of B, which is $(size(B, 2))).") + end + + if output + # We bake C into K here to avoid repeated multiplications below + K_matrix = K_matrix * C + end + + 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 + + stepsize = initial_stepsize + k_scalar = 0.0 + + # Initial poles at k_scalar = 0.0 + A_cl_initial = A - 0.0 * B * K_matrix + initial_poles = eigvals(A_cl_initial) + push!(poleout_list, initial_poles) + push!(k_scalars_collected, 0.0) + prevpoles = initial_poles # Set prevpoles for the first actual step + + while k_scalar < 1.0 + # Propose a new k_scalar value + next_k_scalar = min(1.0, k_scalar + stepsize) + + # Calculate poles for the proposed next_k_scalar + A_cl_proposed = A - next_k_scalar * B * K_matrix + current_poles_proposed = eigvals(A_cl_proposed) + + # Calculate cost using Hungarian algorithm + D = zeros(nx, nx) + for r in 1:nx + for c in 1:nx + D[r, c] = abs(current_poles_proposed[r] - prevpoles[c]) + end + end + assignment, cost = Hungarian.hungarian(D) + + # 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 < 100eps() + @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 or too small + # Sort poles using the assignment from Hungarian algorithm + temppoles = zeros(ComplexF64, nx) + for j = 1:nx + temppoles[assignment[j]] = current_poles_proposed[j] + end + current_poles_sorted = temppoles + + # 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 + # Cap stepsize to prevent overshooting 1.0 significantly in a single step + stepsize = min(stepsize, 1e-1) + end + + # Break if k_scalar has reached or exceeded 1.0 + if k_scalar >= 1.0 + break + end + end + + return hcat(poleout_list...)' |> copy, k_scalars_collected .* Ref(K_matrix) # Return transposed pole matrix and k_values +end + """ roots, Z, K = rlocus(P::LTISystem, K = 500) @@ -76,10 +218,12 @@ end 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. `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. + +If `K` is a matrix and `P` a `StateSpace` system, 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, use, pass `output = true` and `K` of dimension `(nu, ny)`. """ -function rlocus(P, K) +function rlocus(P, K; kwargs...) Z = tzeros(P) - roots, K = getpoles(P,K) + roots, K = getpoles(P, K; kwargs...) ControlSystemsBase.RootLocusResult(roots, Z, K, P) end @@ -89,6 +233,7 @@ rlocus(P; K=500) = rlocus(P, K) # This will be called on plot(rlocus(sys, args...)) @recipe function rootlocusresultplot(r::RootLocusResult) roots, Z, K = r + array_K = eltype(K) <: AbstractArray redata = real.(roots) imdata = imag.(roots) all_redata = [vec(redata); real.(Z)] @@ -103,7 +248,9 @@ rlocus(P; K=500) = rlocus(P, K) form(k, p) = Printf.@sprintf("%.4f", k) * " pole=" * Printf.@sprintf("%.3f%+.3fim", real(p), imag(p)) @series begin legend --> false - hover := "K=" .* form.(K,roots) + if !array_K + hover := "K=" .* form.(K,roots) + end label := "" redata, imdata end @@ -121,6 +268,15 @@ rlocus(P; K=500) = rlocus(P, K) label --> "Open-loop poles" redata[1,:], imdata[1,:] end + if array_K + @series begin + seriestype := :scatter + markershape --> :diamond + markersize --> 10 + label --> "Closed-loop poles" + redata[end,:], imdata[end,:] + end + end end @@ -129,5 +285,10 @@ end Plot the root locus of the SISO LTISystem `P` as computed by `rlocus`. """ -@recipe rlocusplot(::Type{Rlocusplot}, p::Rlocusplot; K=500) = - rlocus(p.args[1]; K=K) +@recipe function rlocusplot(::Type{Rlocusplot}, p::Rlocusplot; K=500, output=false) + if length(p.args) >= 2 + rlocus(p.args[1], p.args[2]; output) + else + rlocus(p.args[1]; K=K) + end +end \ No newline at end of file diff --git a/test/runtests_controlsystems.jl b/test/runtests_controlsystems.jl index 2e6ab7586..72a67f4b9 100644 --- a/test/runtests_controlsystems.jl +++ b/test/runtests_controlsystems.jl @@ -20,5 +20,6 @@ using ControlSystems @testset "rootlocus" begin @info "Testing rootlocus" include("test_rootlocus.jl") + include("test_root_locus_matrix.jl") end end \ No newline at end of file diff --git a/test/test_root_locus_matrix.jl b/test/test_root_locus_matrix.jl new file mode 100644 index 000000000..d366ffa7b --- /dev/null +++ b/test/test_root_locus_matrix.jl @@ -0,0 +1,102 @@ +using ControlSystems +using ControlSystems: getpoles +using Test +using Plots + +@testset "Root Locus with Matrix Feedback" begin + + @testset "State Feedback Matrix" begin + # Test with double mass model + P = DemoSystems.double_mass_model() + + # Design a simple state feedback matrix + K_state = [1.0 0.5 0.2 0.1] # nu x nx matrix (1x4) + + # Test that getpoles works with matrix feedback + roots, K_values = getpoles(P, K_state) + + # Basic sanity checks + @test size(roots, 2) == size(P.A, 1) # Should have nx poles + @test length(K_values) == size(roots, 1) # K_values should match number of steps + @test eltype(K_values) <: AbstractMatrix # K_values should be matrices + + # Test that initial K_value is zero matrix + @test all(K_values[1] .≈ 0.0) + + # Test that final K_value is the input matrix + @test K_values[end] ≈ K_state + + # Test poles are continuous (no sudden jumps) + # The poles should change smoothly + pole_diffs = diff(roots, dims=1) + max_diff = maximum(abs, pole_diffs) + @test max_diff < 1.0 # Poles shouldn't jump too much between steps + + # Test that rlocus works with matrix K + result = rlocus(P, K_state) + @test result isa ControlSystemsBase.RootLocusResult + @test size(result.roots) == size(roots) + + # Test plotting (should not error) + plt = plot(result) + @test plt isa Plots.Plot + end + + @testset "Output Feedback Matrix" begin + P = DemoSystems.double_mass_model(outputs=1:2) + + # Design output feedback matrix + # P has 2 outputs, 1 input, so K should be 1x2 + K_output = [1.0 0.5] # nu x ny matrix (1x2) + + # Test output feedback + roots, K_values = getpoles(P, K_output; output=true) + + # Basic checks + @test size(roots, 2) == size(P.A, 1) # Should have nx poles + @test length(K_values) == size(roots, 1) + @test eltype(K_values) <: AbstractMatrix + + # Test rlocus with output feedback + result = rlocus(P, K_output; output=true) + @test result isa ControlSystemsBase.RootLocusResult + + # Test plotting + plt = plot(result) + @test plt isa Plots.Plot + end + + @testset "Error Handling" begin + P = DemoSystems.double_mass_model() + + # Test wrong dimensions for state feedback + K_wrong = [1.0 0.5 0.2] # Wrong size (1x3 instead of 1x4) + @test_throws ErrorException getpoles(P, K_wrong) + + # Test wrong dimensions for output feedback + K_wrong_output = [1.0 0.5 0.2] # Wrong size (1x3 instead of 1x2) + @test_throws ErrorException getpoles(P, K_wrong_output; output=true) + + # Test wrong number of rows + K_wrong_rows = [1.0 0.5 0.2 0.1; 2.0 1.0 0.4 0.2] # 2x4 instead of 1x4 + @test_throws ErrorException getpoles(P, K_wrong_rows) + end + + + @testset "Adaptive Step Size" begin + P = DemoSystems.double_mass_model() + K_state = [1.0 0.5 0.2 0.1] + + # Test with different tolerances + roots_loose, K_loose = getpoles(P, K_state; tol=1e-1) + roots_tight, K_tight = getpoles(P, K_state; tol=1e-4) + + # Tighter tolerance should result in more steps + @test length(K_tight) >= length(K_loose) + + # Both should start and end at the same points + @test roots_loose[1, :] ≈ roots_tight[1, :] # Same initial poles + @test roots_loose[end, :] ≈ roots_tight[end, :] atol=1e-3 # Similar final poles + end + +end \ No newline at end of file From 626178e87b4acae27c20c8534800de4cfa4b497d Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 23 Jul 2025 15:59:39 +0200 Subject: [PATCH 2/3] rm comment --- src/root_locus.jl | 46 ---------------------------------------------- 1 file changed, 46 deletions(-) diff --git a/src/root_locus.jl b/src/root_locus.jl index 6a6346fea..0de70528e 100644 --- a/src/root_locus.jl +++ b/src/root_locus.jl @@ -69,52 +69,6 @@ function getpoles(G, K::AbstractVector{T}) where {T<:Number} copy(poleout'), K end -# function getpoles(sys::StateSpace, K_matrix::AbstractMatrix; tol = 1e-6) -# # Ensure the system is StateSpace (already enforced by method signature) -# A, B = sys.A, sys.B -# nx = size(A, 1) # State dimension - -# # Check for compatibility of K_matrix dimensions with B -# if size(K_matrix, 2) != nx -# error("The number of columns in K_matrix ($(size(K_matrix, 2))) must match the number of states ($(nx)).") -# end -# # The number of rows in K_matrix must match the number of inputs (columns of B) -# if size(K_matrix, 1) != size(B, 2) -# error("The number of rows in K_matrix ($(size(K_matrix, 1))) must match the number of inputs (columns of B, which is $(size(B, 2))).") -# end - -# poleout = Matrix{ComplexF64}(undef, nx, length(k_values)) -# D = zeros(nx, nx) # distance matrix for Hungarian algorithm -# temppoles = zeros(ComplexF64, nx) # temporary storage for sorted poles - -# prevpoles = ComplexF64[] # Initialize prevpoles for the first iteration - -# stepsize = 1e-6 -# k_scalar = 0.0 - -# while k_scalar < 1 -# A_cl = A - k_scalar * B * K_matrix -# current_poles = eigvals(A_cl) - -# if i > 1 && length(current_poles) == length(prevpoles) -# for r in 1:nx -# for c in 1:nx -# D[r, c] = abs(current_poles[r] - prevpoles[c]) -# end -# end -# assignment, cost = Hungarian.hungarian(D) -# for j = 1:nx -# temppoles[assignment[j]] = current_poles[j] -# end -# poleout[:, i] .= temppoles -# else -# poleout[:, i] .= current_poles # For the first iteration or mismatch, just assign -# end -# prevpoles = poleout[:, i] # Update prevpoles for the next iteration -# end -# return copy(poleout'), k_values .* Ref(K_matrix) # Return transposed pole matrix and k_values as a vector -# end - """ getpoles(sys::StateSpace, K::AbstractMatrix; tol = 1e-2, initial_stepsize = 1e-3, output=false) From fa55616850126dc7abf797a7e176e5eca4f604c4 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 24 Jul 2025 07:54:03 +0200 Subject: [PATCH 3/3] add kwargs... --- src/root_locus.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/root_locus.jl b/src/root_locus.jl index 0de70528e..f695432d4 100644 --- a/src/root_locus.jl +++ b/src/root_locus.jl @@ -3,7 +3,7 @@ import ControlSystemsBase.RootLocusResult @userplot Rlocusplot -function getpoles(G, K::Number) +function getpoles(G, K::Number; kwargs...) issiso(G) || error("root locus only supports SISO systems") G isa TransferFunction || (G = tf(G)) P = numpoly(G)[]