diff --git a/src/ExtendedStateSpace.jl b/src/ExtendedStateSpace.jl index 084953e6..0c31a653 100644 --- a/src/ExtendedStateSpace.jl +++ b/src/ExtendedStateSpace.jl @@ -2,7 +2,7 @@ ## Data Type Declarations ## ##################################################################### """ - ExtendedStateSpace{TE, T} <: AbstractStateSpace{TE} + ExtendedStateSpace{TE, T, S, I} <: AbstractStateSpace{TE} A type that represents the two-input, two-output system ``` @@ -18,7 +18,7 @@ where - `w` denotes external inputs, such as disturbances or references - `u` denotes control inputs -The call `lft(P, K)` forms the (lower) linear fractional transform +The call `lft(P, K)` forms the (lower) linear fractional transform ``` z ┌─────┐ w ◄──┤ │◄── @@ -49,52 +49,88 @@ and the following design functions expect `ExtendedStateSpace` as inputs - [`LQGProblem`](@ref) (also accepts other types) A video tutorial on how to use this type is available [here](https://youtu.be/huYRrn--AKc). + +## Internal Representation +Internally, this type stores an `AbstractStateSpace` along with index vectors that partition the inputs and outputs: +- `sys`: The underlying state-space system +- `w`: Indices for disturbance inputs (corresponds to B1) +- `u`: Indices for control inputs (corresponds to B2) +- `z`: Indices for performance outputs (corresponds to C1) +- `y`: Indices for measured outputs (corresponds to C2) + +The type parameter `I` allows index vectors to be `Vector{Int}`, `UnitRange{Int}`, or `Vector{Symbol}` (for NamedStateSpace). """ -struct ExtendedStateSpace{TE,T} <: AbstractStateSpace{TE} - A::Matrix{T} - B1::Matrix{T} - B2::Matrix{T} - C1::Matrix{T} - C2::Matrix{T} - D11::Matrix{T} - D12::Matrix{T} - D21::Matrix{T} - D22::Matrix{T} - timeevol::TE - function ExtendedStateSpace{TE,T}( - A, - B1, - B2, - C1, - C2, - D11, - D12, - D21, - D22, - timeevol::TE, - ) where {TE,T} - nx = size(A, 1) - nw = size(B1, 2) - nu = size(B2, 2) - nz = size(C1, 1) - ny = size(C2, 1) - - size(A, 2) != nx && nx != 0 && error("A must be square") - size(B1, 1) == nx || error("B1 must have the same row size as A") - size(B2, 1) == nx || error("B2 must have the same row size as A") - size(C1, 2) == nx || error("C1 must have the same column size as A") - size(C2, 2) == nx || error("C2 must have the same column size as A") - size(D11, 2) == nw || error("D11 must have the same column size as B1") - size(D21, 2) == nw || error("D21 must have the same column size as B1") - size(D12, 2) == nu || error("D12 must have the same column size as B2") - size(D22, 2) == nu || error("D22 must have the same column size as B2") - size(D11, 1) == nz || error("D11 must have the same row size as C1") - size(D12, 1) == nz || error("D12 must have the same row size as C1") - size(D21, 1) == ny || error("D21 must have the same row size as C2") - size(D22, 1) == ny || error("D22 must have the same row size as C2") - - new{TE,T}(A, B1, B2, C1, C2, D11, D12, D21, D22, timeevol) - end +struct ExtendedStateSpace{TE, T, S<:AbstractStateSpace{TE}, I} <: AbstractStateSpace{TE} + sys::S # The underlying StateSpace + w::I # Disturbance input indices (corresponds to B1) + u::I # Control input indices (corresponds to B2) + z::I # Performance output indices (corresponds to C1) + y::I # Measured output indices (corresponds to C2) +end + +# Inner constructor taking individual matrices (for backward compatibility) +function ExtendedStateSpace{TE,T}( + A, + B1, + B2, + C1, + C2, + D11, + D12, + D21, + D22, + timeevol::TE, +) where {TE,T} + nx = size(A, 1) + nw = size(B1, 2) + nu = size(B2, 2) + nz = size(C1, 1) + ny = size(C2, 1) + + size(A, 2) != nx && nx != 0 && error("A must be square") + size(B1, 1) == nx || error("B1 must have the same row size as A") + size(B2, 1) == nx || error("B2 must have the same row size as A") + size(C1, 2) == nx || error("C1 must have the same column size as A") + size(C2, 2) == nx || error("C2 must have the same column size as A") + size(D11, 2) == nw || error("D11 must have the same column size as B1") + size(D21, 2) == nw || error("D21 must have the same column size as B1") + size(D12, 2) == nu || error("D12 must have the same column size as B2") + size(D22, 2) == nu || error("D22 must have the same column size as B2") + size(D11, 1) == nz || error("D11 must have the same row size as C1") + size(D12, 1) == nz || error("D12 must have the same row size as C1") + size(D21, 1) == ny || error("D21 must have the same row size as C2") + size(D22, 1) == ny || error("D22 must have the same row size as C2") + + # Build combined matrices + B = [B1 B2] + C = [C1; C2] + D = [D11 D12; D21 D22] + sys = StateSpace{TE, T}(A, B, C, D, timeevol) + + # Compute index vectors + w_inds = 1:nw + u_inds = (nw+1):(nw+nu) + z_inds = 1:nz + y_inds = (nz+1):(nz+ny) + + ExtendedStateSpace{TE, T, typeof(sys), typeof(w_inds)}(sys, w_inds, u_inds, z_inds, y_inds) +end + +# Constructor with all 4 type parameters (for type-preserving operations like negation) +function ExtendedStateSpace{TE, T, S, I}( + A, + B1, + B2, + C1, + C2, + D11, + D12, + D21, + D22, + timeevol::TE, +) where {TE, T, S, I} + # Delegate to the simpler constructor + ExtendedStateSpace{TE, T}(A, B1, B2, C1, C2, D11, D12, D21, D22, timeevol) end function ExtendedStateSpace( @@ -133,6 +169,26 @@ function ExtendedStateSpace( ) end +""" + ExtendedStateSpace(sys::AbstractStateSpace, w, u, z, y) + +Create an [`ExtendedStateSpace`](@ref) from an existing state-space system with specified index vectors. + +This constructor preserves the type of `sys` (e.g., `NamedStateSpace`). + +# Arguments +- `sys`: The underlying state-space system +- `w`: Disturbance input indices (corresponds to B1) +- `u`: Control input indices (corresponds to B2) +- `z`: Performance output indices (corresponds to C1) +- `y`: Measured output indices (corresponds to C2) +""" +function ExtendedStateSpace(sys::S, w::I, u::I, z::I, y::I) where {S<:AbstractStateSpace, I} + TE = typeof(sys.timeevol) + T = ControlSystemsBase.numeric_type(sys) + ExtendedStateSpace{TE, T, S, I}(sys, w, u, z, y) +end + """ ss(A, B1, B2, C1, C2, D11, D12, D21, D22 [, Ts]) @@ -181,53 +237,92 @@ function ss( return ExtendedStateSpace(A, B1, B2, C1, C2, D11, D12, D21, D22, Ts) end -function Base.promote_rule(::Type{StateSpace{TE, F1}}, ::Type{ExtendedStateSpace{TE, F2}}) where {TE, F1, F2} +function Base.promote_rule(::Type{StateSpace{TE, F1}}, ::Type{<:ExtendedStateSpace{TE, F2}}) where {TE, F1, F2} ExtendedStateSpace{TE, promote_type(F1, F2)} end -function Base.convert(::Type{ExtendedStateSpace{TE, F2}}, s::StateSpace{TE, F1})where {TE, F1, F2} +function Base.convert(::Type{<:ExtendedStateSpace{TE}}, s::StateSpace{TE, F1}) where {TE, F1} partition(s, 0, 0) end -function Base.getproperty(sys::ExtendedStateSpace, s::Symbol) - if s === :Ts - # if !isdiscrete(sys) # NOTE this line seems to be breaking inference of isdiscrete (is there a test for this?) - if isdiscrete(sys) - return timeevol(sys).Ts +function Base.getproperty(esys::ExtendedStateSpace, s::Symbol) + # Access to underlying system and index vectors + if s === :sys || s === :w || s === :u || s === :z || s === :y + return getfield(esys, s) + end + + # Get the underlying system for matrix extraction + sys = getfield(esys, :sys) + if sys isa NamedStateSpace + w = names2indices(getfield(esys, :w), sys.u) + u = names2indices(getfield(esys, :u), sys.u) + z = names2indices(getfield(esys, :z), sys.y) + y = names2indices(getfield(esys, :y), sys.y) + else + w = getfield(esys, :w) + u = getfield(esys, :u) + z = getfield(esys, :z) + y = getfield(esys, :y) + end + + # Extract matrices via indexing + if s === :A + return sys.A + elseif s === :B1 + return sys.B[:, w] + elseif s === :B2 + return sys.B[:, u] + elseif s === :C1 + return sys.C[z, :] + elseif s === :C2 + return sys.C[y, :] + elseif s === :D11 + return sys.D[z, w] + elseif s === :D12 + return sys.D[z, u] + elseif s === :D21 + return sys.D[y, w] + elseif s === :D22 + return sys.D[y, u] + elseif s === :timeevol + return sys.timeevol + elseif s === :Ts + if isdiscrete(esys) + return timeevol(esys).Ts else @warn "Getting time 0.0 for non-discrete systems is deprecated. Check `isdiscrete` before trying to access time." return 0.0 end elseif s === :nx - return nstates(sys) + return nstates(esys) elseif s === :nu - return size(sys.B2, 2) - elseif s === :ny # TODO: now size(sys.C, 1) is not always the same as sys.ny - return size(sys.C2, 1) + return length(u) + elseif s === :ny + return length(y) elseif s === :nw - return size(sys.B1, 2) + return length(w) elseif s === :nz - return size(sys.C1, 1) + return length(z) elseif s === :B - [sys.B1 sys.B2] + return sys.B elseif s === :C - [sys.C1; sys.C2] + return sys.C elseif s === :D - [sys.D11 sys.D12; sys.D21 sys.D22] + return sys.D elseif s === :zinds - return 1:size(sys.C1, 1) + return z elseif s === :yinds - return size(sys.C1, 1) .+ (1:size(sys.C2, 1)) + return y elseif s === :winds - return 1:size(sys.B1, 2) + return w elseif s === :uinds - return size(sys.B1, 2) .+ (1:size(sys.B2, 2)) + return u else - return getfield(sys, s) + error("type ExtendedStateSpace has no field $s") end end -Base.propertynames(sys::ExtendedStateSpace) = (:A, :B, :C, :D, :B1, :B2, :C1, :C2, :D11, :D12, :D21, :D22, :Ts, :timeevol, :nx, :ny, :nu, :nw, :nz, :zinds, :yinds, :winds, :uinds) +Base.propertynames(::ExtendedStateSpace) = (:A, :B, :C, :D, :B1, :B2, :C1, :C2, :D11, :D12, :D21, :D22, :Ts, :timeevol, :nx, :ny, :nu, :nw, :nz, :zinds, :yinds, :winds, :uinds, :sys, :w, :u, :z, :y) ControlSystemsBase.StateSpace(s::ExtendedStateSpace) = ss(ssdata(s)..., s.timeevol) @@ -315,14 +410,29 @@ end function Base.:*(s1::ExtendedStateSpace, s2::Number) A, B1, B2, C1, C2, D11, D12, D21, D22 = ssdata_e(s1) + # The reason for only scaling one channel is the use in UncertainSS ss(A, s2*B1, B2, C1, C2, s2*D11, D12, s2*D21, D22, s1.timeevol) + # ExtendedStateSpace(s1.sys*s2, s1.w, s1.u, s1.z, s1.y) end function Base.:*(s2::Number, s1::ExtendedStateSpace) A, B1, B2, C1, C2, D11, D12, D21, D22 = ssdata_e(s1) ss(A, B1, B2, s2*C1, C2, s2*D11, s2*D12, D21, D22, s1.timeevol) + # ExtendedStateSpace(s2*s1.sys, s1.w, s1.u, s1.z, s1.y) end +# function invert_mappings(s::ExtendedStateSpace) +# # Reorder system: inputs [u; w] and outputs [y; z] +# # This swaps the w↔u and z↔y mappings +# (; w,u,z,y) = s +# new_i = [u; w] +# new_o = [y; z] +# ExtendedStateSpace(s.sys, s.u, s.w, s.y, s.z) +# # ExtendedStateSpace(s.sys[new_o, new_i], s.u, s.w, s.y, s.z) +# # ExtendedStateSpace(s.sys[new_o, new_i], s.w, s.u, s.z, s.y) +# end + + function invert_mappings(s::ExtendedStateSpace) A, B1, B2, C1, C2, D11, D12, D21, D22 = ssdata_e(s) ss(A, B2, B1, C2, C1, D22, D21, D12, D11, s.timeevol) @@ -341,9 +451,9 @@ end # end ## NEGATION ## -function Base.:-(sys::ST) where ST <: ExtendedStateSpace - A, B1, B2, C1, C2, D11, D12, D21, D22 = ssdata_e(sys) - ST(A, B1, B2, -C1, -C2, -D11, -D12, -D21, -D22, sys.timeevol) +function Base.:-(sys::ExtendedStateSpace) + # Negating a StateSpace negates C and D matrices, preserving the internal sys type + ExtendedStateSpace(-sys.sys, sys.w, sys.u, sys.z, sys.y) end ##################################################################### @@ -356,17 +466,7 @@ Base.eltype(::Type{S}) where {S<:ExtendedStateSpace} = S ControlSystemsBase.numeric_type(sys::ExtendedStateSpace) = eltype(sys.A) function Base.getindex(sys::ExtendedStateSpace, inds...) - if size(inds, 1) != 2 - error("Must specify 2 indices to index statespace model") - end - rows, cols = ControlSystemsBase.index2range(inds...) # FIXME: ControlSystemsBase.index2range(inds...) - return ss( - copy(sys.A), - sys.B[:, cols], - sys.C[rows, :], - sys.D[rows, cols], - sys.timeevol, - ) + getindex(sys.sys, inds...) end ##################################################################### @@ -725,7 +825,7 @@ Return the system from u -> y See also [`performance_mapping`](@ref), [`system_mapping`](@ref), [`noise_mapping`](@ref) """ function system_mapping(P::ExtendedStateSpace, sminreal=sminreal) - sminreal(ss(P.A, P.B2, P.C2, P.D22, P.timeevol)) + sminreal(P.sys[P.y, P.u]) end """ @@ -735,7 +835,7 @@ Return the system from w -> z See also [`performance_mapping`](@ref), [`system_mapping`](@ref), [`noise_mapping`](@ref) """ function performance_mapping(P::ExtendedStateSpace, sminreal=sminreal) - sminreal(ss(P.A, P.B1, P.C1, P.D11, P.timeevol)) + sminreal(P.sys[P.z, P.w]) end """ @@ -745,5 +845,5 @@ Return the system from w -> y See also [`performance_mapping`](@ref), [`system_mapping`](@ref), [`noise_mapping`](@ref) """ function noise_mapping(P::ExtendedStateSpace, sminreal=sminreal) - sminreal(ss(P.A, P.B1, P.C2, P.D21, P.timeevol)) + sminreal(P.sys[P.y, P.w]) end diff --git a/src/lqg.jl b/src/lqg.jl index f049fcfd..00f637c4 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -330,7 +330,8 @@ Note: the transfer function returned is only a representation of the controller See also [`ff_controller`](@ref) that generates ``C_{ff}``. """ function ControlSystemsBase.observer_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::Union{AbstractMatrix, Nothing} = nothing; direct = false) - A,B,C,D = ssdata(system_mapping(l, identity)) + sys = system_mapping(l, identity) + A,B,C,D = ssdata(sys) if K === nothing K = kalman(l; direct) end @@ -351,7 +352,12 @@ function ControlSystemsBase.observer_controller(l::LQGProblem, L::AbstractMatrix iszero(l.D22) || error("Nonzero D22 not supported. The _transformP2Pbar is not used for LQG, but perhaps shpuld be?") end # do we need some way to specify which non-controllable inputs are measurable? No, because they will automatically appear in the measured outputs :) - ss(Ac, Bc, Cc, Dc, l.timeevol) + Gc = ss(Ac, Bc, Cc, Dc, l.timeevol) + if sys isa NamedStateSpace + named_ss(Gc; u=sys.y, y=sys.u) + else + Gc + end end function ControlSystemsBase.observer_predictor(l::LQGProblem, K::Union{AbstractMatrix, Nothing} = nothing; direct = false, kwargs...) diff --git a/src/named_systems2.jl b/src/named_systems2.jl index 41c76179..0252a5a7 100644 --- a/src/named_systems2.jl +++ b/src/named_systems2.jl @@ -899,12 +899,7 @@ function names2indices(name::Symbol, allnames) end function ExtendedStateSpace(P::NamedStateSpace; z=P.y, y=P.y, w=P.u, u=P.u) - zi = names2indices(z, P.y) - yi = names2indices(y, P.y) - wi = names2indices(w, P.u) - ui = names2indices(u, P.u) - ss(P.A, P.B[:, wi], P.B[:, ui], P.C[zi, :], P.C[yi, :], - P.D[zi, wi], P.D[zi, ui], P.D[yi, wi], P.D[yi, ui], P.timeevol) + ExtendedStateSpace(P, isempty(w) ? Symbol[] : w, isempty(u) ? Symbol[] : u, isempty(z) ? Symbol[] : z, isempty(y) ? Symbol[] : y) end """ diff --git a/src/uncertainty_interface.jl b/src/uncertainty_interface.jl index 1e2e5dc8..8209bb3c 100644 --- a/src/uncertainty_interface.jl +++ b/src/uncertainty_interface.jl @@ -114,21 +114,21 @@ end function Base.getproperty(sys::UncertainSS, s::Symbol) s ∈ fieldnames(typeof(sys)) && return getfield(sys, s) - if s === :nz - # return foldl((l, d)->l + size(d, 1), sys.Δ, init=0) - return size(sys.C1, 1) - elseif s === :nw - # return foldl((l, d)->l + size(d, 2), sys.Δ, init=0) - return size(sys.B1, 2) - elseif s === :zinds - return 1:sys.nz - elseif s === :yinds - return sys.nz .+ (1:size(sys.C2, 1)) - elseif s === :winds - return 1:sys.nw - elseif s === :uinds - return sys.nw .+ (1:size(sys.B2, 2)) - elseif s ===:M + # if s === :nz + # # return foldl((l, d)->l + size(d, 1), sys.Δ, init=0) + # return size(sys.C1, 1) + # elseif s === :nw + # # return foldl((l, d)->l + size(d, 2), sys.Δ, init=0) + # return size(sys.B1, 2) + # elseif s === :zinds + # return 1:sys.nz + # elseif s === :yinds + # return sys.nz .+ (1:size(sys.C2, 1)) + # elseif s === :winds + # return 1:sys.nw + # elseif s === :uinds + # return sys.nw .+ (1:size(sys.B2, 2)) + if s ===:M return sminreal(performance_mapping(sys.sys)) # return sys.sys elseif s ===:delta diff --git a/test/test_lqg.jl b/test/test_lqg.jl index 9d5903cf..209d2396 100644 --- a/test/test_lqg.jl +++ b/test/test_lqg.jl @@ -535,3 +535,56 @@ Ce, cl = extended_controller(lqg, z=[1, 2]) Ce, cl = extended_controller(lqg, z=[1]) @test pinv(dcgain(cl)[1,2]) ≈ dc_gain_compensation atol=1e-8 +## Test LQGProblem with NamedStateSpace inside ExtendedStateSpace +# This tests that the index-based ExtendedStateSpace preserves the type of the internal system + +# Aircraft control + +A = [-0.292 8.13 -201 9.77 0 -12.5 17.1 + -0.152 -2.54 0.561 -0.0004 0 107 7.68 + 0.0364 -0.0678 -0.481 0.0012 0 4.67 -7.98 + 0 1 0.0401 0 0 0 0 + 0 0 1 0 0 0 0 + 0 0 0 0 0 -20 0 + 0 0 0 0 0 0 -20 +] + +B = [ + 0 -2.15 + -31.7 0.0274 + 0 1.48 + 0 0 + 0 0 + 20 0 + 0 20 +] +# Example 9.1 +M = [0 0 0 1 0 0 0 + 0 0 0 0 1 0 0] +C = M + +N = I(7) +nx,ny,nu = size(A,1), size(C, 1), size(B,2) + + +Q1 = I(ny) +Q2 = I(nu) +R1 = I(nx) +R2 = I(ny) + + +sys = named_ss(ss(A, [N B], M, 0), x=:x, u=[:w^7; :u^2], y=:y^2) +sys = ExtendedStateSpace(sys, w=:w^7, u=:u^2, z=:y^2, y=:y^2) +G = LQGProblem(sys, Q1, Q2, R1, R2) +@test lqr(G) ≈ [-0.0022 0.17 0.12 0.98 0.31 0.76 0.018 + -0.0038 0.028 -0.25 0.16 -0.95 0.072 0.10] rtol=0.01 + + +gangoffourplot(G) + +@test system_mapping(G).y == :y^2 +@test system_mapping(G).u == :u^2 +@test performance_mapping(G).u == :w^7 + +@test observer_controller(G).y == :u^2 # Names of contorller are swapped compared to plant +@test observer_controller(G).u == :y^2 \ No newline at end of file diff --git a/test/test_named_systems2.jl b/test/test_named_systems2.jl index 5b2ef8d7..9d940186 100644 --- a/test/test_named_systems2.jl +++ b/test/test_named_systems2.jl @@ -160,7 +160,7 @@ s2 = named_ss(G2, x = :z, u = :u2, y=:y2) @test_nowarn check_all_unique(s1, s2) s1e = ExtendedStateSpace(s1, y=s1.y, u=s1.u, z=[], w=[]) -@test s1.sys == system_mapping(s1e) +@test s1.sys == system_mapping(s1e).sys @test ss(s1e) == ss(s1) s1d = c2d(s1, 1.0)