From 88ac73b7fd8a7ed26b12e2a6a29ad360b3c95b97 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 15 Aug 2025 11:17:37 +0200 Subject: [PATCH 1/2] generalize `ff_controller` to accept any system --- Project.toml | 2 +- src/lqg.jl | 21 +++++++++++++++------ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/Project.toml b/Project.toml index 36c7bb1..06bff93 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "RobustAndOptimalControl" uuid = "21fd56a4-db03-40ee-82ee-a87907bee541" authors = ["Fredrik Bagge Carlson", "Marcus Greiff"] -version = "0.4.44" +version = "0.4.45" [deps] ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" diff --git a/src/lqg.jl b/src/lqg.jl index cb4d56b..0545e3f 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -349,13 +349,15 @@ function ControlSystemsBase.observer_controller(l::LQGProblem, L::AbstractMatrix ss(Ac, Bc, Cc, Dc, l.timeevol) end + """ + ff_controller(sys, L, K; comp_dc = true) ff_controller(l::LQGProblem, L = lqr(l), K = kalman(l)) Return the feedforward controller ``C_{ff}`` that maps references to plant inputs: ``u = C_{fb}y + C_{ff}r`` -The following should hold +The following should hold for an LQGProblem `l`: ``` Cff = RobustAndOptimalControl.ff_controller(l) Cfb = observer_controller(l) @@ -367,21 +369,28 @@ Note, if [`extended_controller`](@ref) is used, the DC-gain compensation above c See also [`observer_controller`](@ref). """ -function ff_controller(l::LQGProblem, L = lqr(l), K = kalman(l); comp_dc = true) - Ae,Be,Ce,De = ssdata(system_mapping(l, identity)) +function ff_controller(sys::AbstractStateSpace, L, K, Lr = nothing; comp_dc = true) + Ae,Be,Ce,De = ssdata(sys) Ac = Ae - Be*L - K*Ce + K*De*L # 8.26c Cc = L Dc = 0 if comp_dc - Lr = static_gain_compensation(l, L) + if Lr === nothing + Cfb = observer_controller(sys, L, K) + Lr = pinv(dcgain(feedback(sys*Cfb))) + end Bc = Be*Lr - return Lr - ss(Ac, Bc, Cc, Dc, l.timeevol) + return Lr - ss(Ac, Bc, Cc, Dc, sys.timeevol) else Bc = Be - return I(size(Cc, 1)) - ss(Ac, Bc, Cc, Dc, l.timeevol) + return I(size(Cc, 1)) - ss(Ac, Bc, Cc, Dc, sys.timeevol) end end +function ff_controller(l::LQGProblem, L = lqr(l), K = kalman(l); kwargs...) + ff_controller(system_mapping(l, identity), L, K, static_gain_compensation(l, L); kwargs...) +end + """ closedloop(l::LQGProblem, L = lqr(l), K = kalman(l)) From a2b5d535a669c491e519baae472f1bdedaca5b54 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 15 Aug 2025 11:43:56 +0200 Subject: [PATCH 2/2] generalize also extended controller --- src/lqg.jl | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/lqg.jl b/src/lqg.jl index 0545e3f..d7e8b50 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -271,6 +271,7 @@ function extended_controller(K::AbstractStateSpace) end """ + extended_controller(P::StateSpace, L, K; z = nothing) extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l); z = nothing) Returns a statespace system representing 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`. @@ -291,8 +292,7 @@ system_mapping(Ce) == -C Please note, without the reference pre-filter, the DC gain from references to controlled outputs may not be identity. If a vector of output indices is provided through the keyword argument `z`, the closed-loop system from state reference `xᵣ` to outputs `z` is returned as a second return argument. The inverse of the DC-gain of this closed-loop system may be useful to compensate for the DC-gain of the controller. """ -function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); z::Union{Nothing, AbstractVector} = nothing) - P = system_mapping(l, identity) +function extended_controller(P::AbstractStateSpace, L::AbstractMatrix, K::AbstractMatrix; z::Union{Nothing, AbstractVector} = nothing) A,B,C,D = ssdata(P) Ac = A - B*L - K*C + K*D*L # 8.26b (; nx, nu, ny) = P @@ -302,7 +302,7 @@ function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::Abstr D21 = L # L*xᵣ # should be D21? C2 = -L # - L*x̂ C1 = zeros(0, nx) - Ce0 = ss(Ac, B1, B2, C1, C2; D21, Ts = l.timeevol) + Ce0 = ss(Ac, B1, B2, C1, C2; D21, Ts = P.timeevol) if z === nothing return Ce0 end @@ -313,6 +313,11 @@ function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::Abstr end +function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); kwargs...) + P = system_mapping(l, identity) + extended_controller(P, L, K; kwargs...) +end + """ observer_controller(l::LQGProblem, L = lqr(l), K = kalman(l)) @@ -377,7 +382,8 @@ function ff_controller(sys::AbstractStateSpace, L, K, Lr = nothing; comp_dc = tr if comp_dc if Lr === nothing Cfb = observer_controller(sys, L, K) - Lr = pinv(dcgain(feedback(sys*Cfb))) + Cff0 = ss(I(sys.nu), sys.timeevol) - ss(Ac, Be, Cc, Dc, sys.timeevol) + Lr = pinv(dcgain(feedback(sys, Cfb)*Cff0)) end Bc = Be*Lr return Lr - ss(Ac, Bc, Cc, Dc, sys.timeevol)