Skip to content

Commit 1364080

Browse files
baggepinnenbaggepinnen
committed
add root-locus methods for matrix gains
1 parent 948aa79 commit 1364080

File tree

3 files changed

+269
-5
lines changed

3 files changed

+269
-5
lines changed

‎src/root_locus.jl‎

Lines changed: 166 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -69,17 +69,161 @@ function getpoles(G, K::AbstractVector{T}) where {T<:Number}
6969
copy(poleout'), K
7070
end
7171

72+
# function getpoles(sys::StateSpace, K_matrix::AbstractMatrix; tol = 1e-6)
73+
# # Ensure the system is StateSpace (already enforced by method signature)
74+
# A, B = sys.A, sys.B
75+
# nx = size(A, 1) # State dimension
76+
77+
# # Check for compatibility of K_matrix dimensions with B
78+
# if size(K_matrix, 2) != nx
79+
# error("The number of columns in K_matrix ($(size(K_matrix, 2))) must match the number of states ($(nx)).")
80+
# end
81+
# # The number of rows in K_matrix must match the number of inputs (columns of B)
82+
# if size(K_matrix, 1) != size(B, 2)
83+
# 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))).")
84+
# end
85+
86+
# poleout = Matrix{ComplexF64}(undef, nx, length(k_values))
87+
# D = zeros(nx, nx) # distance matrix for Hungarian algorithm
88+
# temppoles = zeros(ComplexF64, nx) # temporary storage for sorted poles
89+
90+
# prevpoles = ComplexF64[] # Initialize prevpoles for the first iteration
91+
92+
# stepsize = 1e-6
93+
# k_scalar = 0.0
94+
95+
# while k_scalar < 1
96+
# A_cl = A - k_scalar * B * K_matrix
97+
# current_poles = eigvals(A_cl)
98+
99+
# if i > 1 && length(current_poles) == length(prevpoles)
100+
# for r in 1:nx
101+
# for c in 1:nx
102+
# D[r, c] = abs(current_poles[r] - prevpoles[c])
103+
# end
104+
# end
105+
# assignment, cost = Hungarian.hungarian(D)
106+
# for j = 1:nx
107+
# temppoles[assignment[j]] = current_poles[j]
108+
# end
109+
# poleout[:, i] .= temppoles
110+
# else
111+
# poleout[:, i] .= current_poles # For the first iteration or mismatch, just assign
112+
# end
113+
# prevpoles = poleout[:, i] # Update prevpoles for the next iteration
114+
# end
115+
# return copy(poleout'), k_values .* Ref(K_matrix) # Return transposed pole matrix and k_values as a vector
116+
# end
117+
118+
"""
119+
getpoles(sys::StateSpace, K::AbstractMatrix; tol = 1e-2, initial_stepsize = 1e-3, output=false)
120+
121+
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.
122+
123+
If `output = true`, `K` is assumed to be an output feedback matrix of dim `(nu, ny)`
124+
"""
125+
function getpoles(sys::StateSpace, K_matrix::AbstractMatrix; tol = 1e-2, initial_stepsize = 1e-3, output=false)
126+
(; A, B, C) = sys
127+
nx = size(A, 1) # State dimension
128+
ny = size(C, 1) # Output dimension
129+
tol = tol*nx # Scale tolerance with state dimension
130+
# Check for compatibility of K_matrix dimensions with B
131+
if size(K_matrix, 2) != (output ? ny : nx)
132+
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.")
133+
end
134+
if size(K_matrix, 1) != size(B, 2)
135+
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))).")
136+
end
137+
138+
if output
139+
# We bake C into K here to avoid repeated multiplications below
140+
K_matrix = K_matrix * C
141+
end
142+
143+
poleout_list = Vector{Vector{ComplexF64}}() # To store pole sets at each accepted step
144+
k_scalars_collected = Float64[] # To store accepted k_scalar values
145+
146+
prevpoles = ComplexF64[] # Initialize prevpoles for the first iteration
147+
148+
stepsize = initial_stepsize
149+
k_scalar = 0.0
150+
151+
# Initial poles at k_scalar = 0.0
152+
A_cl_initial = A - 0.0 * B * K_matrix
153+
initial_poles = eigvals(A_cl_initial)
154+
push!(poleout_list, initial_poles)
155+
push!(k_scalars_collected, 0.0)
156+
prevpoles = initial_poles # Set prevpoles for the first actual step
157+
158+
while k_scalar < 1.0
159+
# Propose a new k_scalar value
160+
next_k_scalar = min(1.0, k_scalar + stepsize)
161+
162+
# Calculate poles for the proposed next_k_scalar
163+
A_cl_proposed = A - next_k_scalar * B * K_matrix
164+
current_poles_proposed = eigvals(A_cl_proposed)
165+
166+
# Calculate cost using Hungarian algorithm
167+
D = zeros(nx, nx)
168+
for r in 1:nx
169+
for c in 1:nx
170+
D[r, c] = abs(current_poles_proposed[r] - prevpoles[c])
171+
end
172+
end
173+
assignment, cost = Hungarian.hungarian(D)
174+
175+
# Adaptive step size logic
176+
if cost > 2 * tol # Cost is too high, reject step and reduce stepsize
177+
stepsize /= 2.0
178+
# Ensure stepsize doesn't become too small
179+
if stepsize < 100eps()
180+
@warn "Step size became extremely small, potentially stuck. Breaking loop."
181+
break
182+
end
183+
# Do not update k_scalar, try again with smaller stepsize
184+
else # Step is acceptable or too small
185+
# Sort poles using the assignment from Hungarian algorithm
186+
temppoles = zeros(ComplexF64, nx)
187+
for j = 1:nx
188+
temppoles[assignment[j]] = current_poles_proposed[j]
189+
end
190+
current_poles_sorted = temppoles
191+
192+
# Accept the step
193+
push!(poleout_list, current_poles_sorted)
194+
push!(k_scalars_collected, next_k_scalar)
195+
prevpoles = current_poles_sorted # Update prevpoles for the next iteration
196+
k_scalar = next_k_scalar # Advance k_scalar
197+
198+
if cost < tol # Cost is too low, increase stepsize
199+
stepsize *= 1.1
200+
end
201+
# Cap stepsize to prevent overshooting 1.0 significantly in a single step
202+
stepsize = min(stepsize, 1e-1)
203+
end
204+
205+
# Break if k_scalar has reached or exceeded 1.0
206+
if k_scalar >= 1.0
207+
break
208+
end
209+
end
210+
211+
return hcat(poleout_list...)' |> copy, k_scalars_collected .* Ref(K_matrix) # Return transposed pole matrix and k_values
212+
end
213+
72214

73215
"""
74216
roots, Z, K = rlocus(P::LTISystem, K = 500)
75217
76218
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.
77219
78220
`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.
221+
222+
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)`.
79223
"""
80-
function rlocus(P, K)
224+
function rlocus(P, K; kwargs...)
81225
Z = tzeros(P)
82-
roots, K = getpoles(P,K)
226+
roots, K = getpoles(P, K; kwargs...)
83227
ControlSystemsBase.RootLocusResult(roots, Z, K, P)
84228
end
85229

@@ -89,6 +233,7 @@ rlocus(P; K=500) = rlocus(P, K)
89233
# This will be called on plot(rlocus(sys, args...))
90234
@recipe function rootlocusresultplot(r::RootLocusResult)
91235
roots, Z, K = r
236+
array_K = eltype(K) <: AbstractArray
92237
redata = real.(roots)
93238
imdata = imag.(roots)
94239
all_redata = [vec(redata); real.(Z)]
@@ -103,7 +248,9 @@ rlocus(P; K=500) = rlocus(P, K)
103248
form(k, p) = Printf.@sprintf("%.4f", k) * " pole=" * Printf.@sprintf("%.3f%+.3fim", real(p), imag(p))
104249
@series begin
105250
legend --> false
106-
hover := "K=" .* form.(K,roots)
251+
if !array_K
252+
hover := "K=" .* form.(K,roots)
253+
end
107254
label := ""
108255
redata, imdata
109256
end
@@ -121,6 +268,15 @@ rlocus(P; K=500) = rlocus(P, K)
121268
label --> "Open-loop poles"
122269
redata[1,:], imdata[1,:]
123270
end
271+
if array_K
272+
@series begin
273+
seriestype := :scatter
274+
markershape --> :diamond
275+
markersize --> 10
276+
label --> "Closed-loop poles"
277+
redata[end,:], imdata[end,:]
278+
end
279+
end
124280
end
125281

126282

@@ -129,5 +285,10 @@ end
129285
130286
Plot the root locus of the SISO LTISystem `P` as computed by `rlocus`.
131287
"""
132-
@recipe rlocusplot(::Type{Rlocusplot}, p::Rlocusplot; K=500) =
133-
rlocus(p.args[1]; K=K)
288+
@recipe function rlocusplot(::Type{Rlocusplot}, p::Rlocusplot; K=500, output=false)
289+
if length(p.args) >= 2
290+
rlocus(p.args[1], p.args[2]; output)
291+
else
292+
rlocus(p.args[1]; K=K)
293+
end
294+
end

‎test/runtests_controlsystems.jl‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,5 +20,6 @@ using ControlSystems
2020
@testset "rootlocus" begin
2121
@info "Testing rootlocus"
2222
include("test_rootlocus.jl")
23+
include("test_root_locus_matrix.jl")
2324
end
2425
end

‎test/test_root_locus_matrix.jl‎

Lines changed: 102 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
using ControlSystems
2+
using ControlSystems: getpoles
3+
using Test
4+
using Plots
5+
6+
@testset "Root Locus with Matrix Feedback" begin
7+
8+
@testset "State Feedback Matrix" begin
9+
# Test with double mass model
10+
P = DemoSystems.double_mass_model()
11+
12+
# Design a simple state feedback matrix
13+
K_state = [1.0 0.5 0.2 0.1] # nu x nx matrix (1x4)
14+
15+
# Test that getpoles works with matrix feedback
16+
roots, K_values = getpoles(P, K_state)
17+
18+
# Basic sanity checks
19+
@test size(roots, 2) == size(P.A, 1) # Should have nx poles
20+
@test length(K_values) == size(roots, 1) # K_values should match number of steps
21+
@test eltype(K_values) <: AbstractMatrix # K_values should be matrices
22+
23+
# Test that initial K_value is zero matrix
24+
@test all(K_values[1] .≈ 0.0)
25+
26+
# Test that final K_value is the input matrix
27+
@test K_values[end] ≈ K_state
28+
29+
# Test poles are continuous (no sudden jumps)
30+
# The poles should change smoothly
31+
pole_diffs = diff(roots, dims=1)
32+
max_diff = maximum(abs, pole_diffs)
33+
@test max_diff < 1.0 # Poles shouldn't jump too much between steps
34+
35+
# Test that rlocus works with matrix K
36+
result = rlocus(P, K_state)
37+
@test result isa ControlSystemsBase.RootLocusResult
38+
@test size(result.roots) == size(roots)
39+
40+
# Test plotting (should not error)
41+
plt = plot(result)
42+
@test plt isa Plots.Plot
43+
end
44+
45+
@testset "Output Feedback Matrix" begin
46+
P = DemoSystems.double_mass_model(outputs=1:2)
47+
48+
# Design output feedback matrix
49+
# P has 2 outputs, 1 input, so K should be 1x2
50+
K_output = [1.0 0.5] # nu x ny matrix (1x2)
51+
52+
# Test output feedback
53+
roots, K_values = getpoles(P, K_output; output=true)
54+
55+
# Basic checks
56+
@test size(roots, 2) == size(P.A, 1) # Should have nx poles
57+
@test length(K_values) == size(roots, 1)
58+
@test eltype(K_values) <: AbstractMatrix
59+
60+
# Test rlocus with output feedback
61+
result = rlocus(P, K_output; output=true)
62+
@test result isa ControlSystemsBase.RootLocusResult
63+
64+
# Test plotting
65+
plt = plot(result)
66+
@test plt isa Plots.Plot
67+
end
68+
69+
@testset "Error Handling" begin
70+
P = DemoSystems.double_mass_model()
71+
72+
# Test wrong dimensions for state feedback
73+
K_wrong = [1.0 0.5 0.2] # Wrong size (1x3 instead of 1x4)
74+
@test_throws ErrorException getpoles(P, K_wrong)
75+
76+
# Test wrong dimensions for output feedback
77+
K_wrong_output = [1.0 0.5 0.2] # Wrong size (1x3 instead of 1x2)
78+
@test_throws ErrorException getpoles(P, K_wrong_output; output=true)
79+
80+
# Test wrong number of rows
81+
K_wrong_rows = [1.0 0.5 0.2 0.1; 2.0 1.0 0.4 0.2] # 2x4 instead of 1x4
82+
@test_throws ErrorException getpoles(P, K_wrong_rows)
83+
end
84+
85+
86+
@testset "Adaptive Step Size" begin
87+
P = DemoSystems.double_mass_model()
88+
K_state = [1.0 0.5 0.2 0.1]
89+
90+
# Test with different tolerances
91+
roots_loose, K_loose = getpoles(P, K_state; tol=1e-1)
92+
roots_tight, K_tight = getpoles(P, K_state; tol=1e-4)
93+
94+
# Tighter tolerance should result in more steps
95+
@test length(K_tight) >= length(K_loose)
96+
97+
# Both should start and end at the same points
98+
@test roots_loose[1, :] ≈ roots_tight[1, :] # Same initial poles
99+
@test roots_loose[end, :] ≈ roots_tight[end, :] atol=1e-3 # Similar final poles
100+
end
101+
102+
end

0 commit comments

Comments
 (0)