Skip to content

Commit 64a8bf3

Browse files
baggepinnenbaggepinnen
committed
refactor internals of ExtendedStateSpace to allow for named signals
1 parent 67f3081 commit 64a8bf3

File tree

1 file changed

+137
-69
lines changed

1 file changed

+137
-69
lines changed

‎src/ExtendedStateSpace.jl‎

Lines changed: 137 additions & 69 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
## Data Type Declarations ##
33
#####################################################################
44
"""
5-
ExtendedStateSpace{TE, T} <: AbstractStateSpace{TE}
5+
ExtendedStateSpace{TE, T, S, I} <: AbstractStateSpace{TE}
66
77
A type that represents the two-input, two-output system
88
```
@@ -18,7 +18,7 @@ where
1818
- `w` denotes external inputs, such as disturbances or references
1919
- `u` denotes control inputs
2020
21-
The call `lft(P, K)` forms the (lower) linear fractional transform
21+
The call `lft(P, K)` forms the (lower) linear fractional transform
2222
```
2323
z ┌─────┐ w
2424
◄──┤ │◄──
@@ -49,52 +49,88 @@ and the following design functions expect `ExtendedStateSpace` as inputs
4949
- [`LQGProblem`](@ref) (also accepts other types)
5050
5151
A video tutorial on how to use this type is available [here](https://youtu.be/huYRrn--AKc).
52+
53+
## Internal Representation
54+
Internally, this type stores an `AbstractStateSpace` along with index vectors that partition the inputs and outputs:
55+
- `sys`: The underlying state-space system
56+
- `w`: Indices for disturbance inputs (corresponds to B1)
57+
- `u`: Indices for control inputs (corresponds to B2)
58+
- `z`: Indices for performance outputs (corresponds to C1)
59+
- `y`: Indices for measured outputs (corresponds to C2)
60+
61+
The type parameter `I` allows index vectors to be `Vector{Int}`, `UnitRange{Int}`, or `Vector{Symbol}` (for NamedStateSpace).
5262
"""
53-
struct ExtendedStateSpace{TE,T} <: AbstractStateSpace{TE}
54-
A::Matrix{T}
55-
B1::Matrix{T}
56-
B2::Matrix{T}
57-
C1::Matrix{T}
58-
C2::Matrix{T}
59-
D11::Matrix{T}
60-
D12::Matrix{T}
61-
D21::Matrix{T}
62-
D22::Matrix{T}
63-
timeevol::TE
64-
function ExtendedStateSpace{TE,T}(
65-
A,
66-
B1,
67-
B2,
68-
C1,
69-
C2,
70-
D11,
71-
D12,
72-
D21,
73-
D22,
74-
timeevol::TE,
75-
) where {TE,T}
76-
nx = size(A, 1)
77-
nw = size(B1, 2)
78-
nu = size(B2, 2)
79-
nz = size(C1, 1)
80-
ny = size(C2, 1)
81-
82-
size(A, 2) != nx && nx != 0 && error("A must be square")
83-
size(B1, 1) == nx || error("B1 must have the same row size as A")
84-
size(B2, 1) == nx || error("B2 must have the same row size as A")
85-
size(C1, 2) == nx || error("C1 must have the same column size as A")
86-
size(C2, 2) == nx || error("C2 must have the same column size as A")
87-
size(D11, 2) == nw || error("D11 must have the same column size as B1")
88-
size(D21, 2) == nw || error("D21 must have the same column size as B1")
89-
size(D12, 2) == nu || error("D12 must have the same column size as B2")
90-
size(D22, 2) == nu || error("D22 must have the same column size as B2")
91-
size(D11, 1) == nz || error("D11 must have the same row size as C1")
92-
size(D12, 1) == nz || error("D12 must have the same row size as C1")
93-
size(D21, 1) == ny || error("D21 must have the same row size as C2")
94-
size(D22, 1) == ny || error("D22 must have the same row size as C2")
95-
96-
new{TE,T}(A, B1, B2, C1, C2, D11, D12, D21, D22, timeevol)
97-
end
63+
struct ExtendedStateSpace{TE, T, S<:AbstractStateSpace{TE}, I} <: AbstractStateSpace{TE}
64+
sys::S # The underlying StateSpace
65+
w::I # Disturbance input indices (corresponds to B1)
66+
u::I # Control input indices (corresponds to B2)
67+
z::I # Performance output indices (corresponds to C1)
68+
y::I # Measured output indices (corresponds to C2)
69+
end
70+
71+
# Inner constructor taking individual matrices (for backward compatibility)
72+
function ExtendedStateSpace{TE,T}(
73+
A,
74+
B1,
75+
B2,
76+
C1,
77+
C2,
78+
D11,
79+
D12,
80+
D21,
81+
D22,
82+
timeevol::TE,
83+
) where {TE,T}
84+
nx = size(A, 1)
85+
nw = size(B1, 2)
86+
nu = size(B2, 2)
87+
nz = size(C1, 1)
88+
ny = size(C2, 1)
89+
90+
size(A, 2) != nx && nx != 0 && error("A must be square")
91+
size(B1, 1) == nx || error("B1 must have the same row size as A")
92+
size(B2, 1) == nx || error("B2 must have the same row size as A")
93+
size(C1, 2) == nx || error("C1 must have the same column size as A")
94+
size(C2, 2) == nx || error("C2 must have the same column size as A")
95+
size(D11, 2) == nw || error("D11 must have the same column size as B1")
96+
size(D21, 2) == nw || error("D21 must have the same column size as B1")
97+
size(D12, 2) == nu || error("D12 must have the same column size as B2")
98+
size(D22, 2) == nu || error("D22 must have the same column size as B2")
99+
size(D11, 1) == nz || error("D11 must have the same row size as C1")
100+
size(D12, 1) == nz || error("D12 must have the same row size as C1")
101+
size(D21, 1) == ny || error("D21 must have the same row size as C2")
102+
size(D22, 1) == ny || error("D22 must have the same row size as C2")
103+
104+
# Build combined matrices
105+
B = [B1 B2]
106+
C = [C1; C2]
107+
D = [D11 D12; D21 D22]
108+
sys = StateSpace{TE, T}(A, B, C, D, timeevol)
109+
110+
# Compute index vectors
111+
w_inds = 1:nw
112+
u_inds = (nw+1):(nw+nu)
113+
z_inds = 1:nz
114+
y_inds = (nz+1):(nz+ny)
115+
116+
ExtendedStateSpace{TE, T, typeof(sys), typeof(w_inds)}(sys, w_inds, u_inds, z_inds, y_inds)
117+
end
118+
119+
# Constructor with all 4 type parameters (for type-preserving operations like negation)
120+
function ExtendedStateSpace{TE, T, S, I}(
121+
A,
122+
B1,
123+
B2,
124+
C1,
125+
C2,
126+
D11,
127+
D12,
128+
D21,
129+
D22,
130+
timeevol::TE,
131+
) where {TE, T, S, I}
132+
# Delegate to the simpler constructor
133+
ExtendedStateSpace{TE, T}(A, B1, B2, C1, C2, D11, D12, D21, D22, timeevol)
98134
end
99135

100136
function ExtendedStateSpace(
@@ -181,53 +217,85 @@ function ss(
181217
return ExtendedStateSpace(A, B1, B2, C1, C2, D11, D12, D21, D22, Ts)
182218
end
183219

184-
function Base.promote_rule(::Type{StateSpace{TE, F1}}, ::Type{ExtendedStateSpace{TE, F2}}) where {TE, F1, F2}
220+
function Base.promote_rule(::Type{StateSpace{TE, F1}}, ::Type{<:ExtendedStateSpace{TE, F2}}) where {TE, F1, F2}
185221
ExtendedStateSpace{TE, promote_type(F1, F2)}
186222
end
187223

188-
function Base.convert(::Type{ExtendedStateSpace{TE, F2}}, s::StateSpace{TE, F1})where {TE, F1, F2}
224+
function Base.convert(::Type{<:ExtendedStateSpace{TE}}, s::StateSpace{TE, F1}) where {TE, F1}
189225
partition(s, 0, 0)
190226
end
191227

192-
function Base.getproperty(sys::ExtendedStateSpace, s::Symbol)
193-
if s === :Ts
194-
# if !isdiscrete(sys) # NOTE this line seems to be breaking inference of isdiscrete (is there a test for this?)
195-
if isdiscrete(sys)
196-
return timeevol(sys).Ts
228+
function Base.getproperty(esys::ExtendedStateSpace, s::Symbol)
229+
# Access to underlying system and index vectors
230+
if s === :sys || s === :w || s === :u || s === :z || s === :y
231+
return getfield(esys, s)
232+
end
233+
234+
# Get the underlying system for matrix extraction
235+
sys = getfield(esys, :sys)
236+
w = getfield(esys, :w)
237+
u = getfield(esys, :u)
238+
z = getfield(esys, :z)
239+
y = getfield(esys, :y)
240+
241+
# Extract matrices via indexing
242+
if s === :A
243+
return sys.A
244+
elseif s === :B1
245+
return sys.B[:, w]
246+
elseif s === :B2
247+
return sys.B[:, u]
248+
elseif s === :C1
249+
return sys.C[z, :]
250+
elseif s === :C2
251+
return sys.C[y, :]
252+
elseif s === :D11
253+
return sys.D[z, w]
254+
elseif s === :D12
255+
return sys.D[z, u]
256+
elseif s === :D21
257+
return sys.D[y, w]
258+
elseif s === :D22
259+
return sys.D[y, u]
260+
elseif s === :timeevol
261+
return sys.timeevol
262+
elseif s === :Ts
263+
if isdiscrete(esys)
264+
return timeevol(esys).Ts
197265
else
198266
@warn "Getting time 0.0 for non-discrete systems is deprecated. Check `isdiscrete` before trying to access time."
199267
return 0.0
200268
end
201269
elseif s === :nx
202-
return nstates(sys)
270+
return nstates(esys)
203271
elseif s === :nu
204-
return size(sys.B2, 2)
205-
elseif s === :ny # TODO: now size(sys.C, 1) is not always the same as sys.ny
206-
return size(sys.C2, 1)
272+
return length(u)
273+
elseif s === :ny
274+
return length(y)
207275
elseif s === :nw
208-
return size(sys.B1, 2)
276+
return length(w)
209277
elseif s === :nz
210-
return size(sys.C1, 1)
278+
return length(z)
211279
elseif s === :B
212-
[sys.B1 sys.B2]
280+
return sys.B
213281
elseif s === :C
214-
[sys.C1; sys.C2]
282+
return sys.C
215283
elseif s === :D
216-
[sys.D11 sys.D12; sys.D21 sys.D22]
284+
return sys.D
217285
elseif s === :zinds
218-
return 1:size(sys.C1, 1)
286+
return z
219287
elseif s === :yinds
220-
return size(sys.C1, 1) .+ (1:size(sys.C2, 1))
288+
return y
221289
elseif s === :winds
222-
return 1:size(sys.B1, 2)
290+
return w
223291
elseif s === :uinds
224-
return size(sys.B1, 2) .+ (1:size(sys.B2, 2))
292+
return u
225293
else
226-
return getfield(sys, s)
294+
error("type ExtendedStateSpace has no field $s")
227295
end
228296
end
229297

230-
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)
298+
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)
231299

232300
ControlSystemsBase.StateSpace(s::ExtendedStateSpace) = ss(ssdata(s)..., s.timeevol)
233301

0 commit comments

Comments
 (0)