test ESS with named ss

Author: baggepinnen

src/ExtendedStateSpace.jl

@@ -253,10 +253,17 @@ function Base.getproperty(esys::ExtendedStateSpace, s::Symbol)
 
     # Get the underlying system for matrix extraction
     sys = getfield(esys, :sys)
-    w = getfield(esys, :w)
-    u = getfield(esys, :u)
-    z = getfield(esys, :z)
-    y = getfield(esys, :y)
+    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
@@ -459,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
 
 #####################################################################

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...)

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, w, u, z, y)
 end
 
 """

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