Skip to content

Commit a029e89

Browse files
MartinOtterMartinOtter
committed
New function toStateSpace added (together with several tests)
1 parent f3f407f commit a029e89

File tree

5 files changed

+374
-3
lines changed

5 files changed

+374
-3
lines changed

‎Project.toml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
name = "ModiaBase"
22
uuid = "ec7bf1ca-419d-4510-bbab-199861c55244"
33
authors = ["Hilding Elmqvist <Hilding.Elmqvist@Mogram.net>", "Martin Otter <Martin.Otter@dlr.de>"]
4-
version = "0.7.1"
4+
version = "0.7.2-dev"
55

66
[deps]
77
DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0"

‎src/ModiaBase.jl

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,8 @@ Main module of ModiaBase.
99
module ModiaBase
1010

1111
const path = dirname(dirname(@__FILE__)) # Absolute path of package directory
12-
const Version = "0.7.0"
13-
const Date = "2021-02-01"
12+
const Version = "0.7.2-dev"
13+
const Date = "2021-03-08"
1414

1515
#println("\nImporting ModiaBase Version $Version ($Date)")
1616

@@ -35,5 +35,6 @@ using .Symbolic
3535

3636
include("EquationAndStateInfo.jl")
3737
include("StateSelection.jl")
38+
include("toStateSpace.jl")
3839

3940
end

‎src/toStateSpace.jl

Lines changed: 261 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,261 @@
1+
# License for this file: MIT (expat)
2+
# Copyright 2021, DLR Institute of System Dynamics and Control
3+
# Author: Martin Otter, DLR-SR
4+
5+
6+
using LinearAlgebra
7+
export toStateSpace
8+
9+
"""
10+
(Ar,Br,Cr,Dr,pr) = toStateSpace(E,A,B,C,D)
11+
12+
Transform the descriptor system
13+
14+
```julia
15+
E*dx/dt = A*x + B*u
16+
y = C*x + D*u
17+
```
18+
19+
where `E` may be singular, into state space form
20+
21+
```julia
22+
dxr/dt = Ar*xr + Br*u
23+
y = Cr*xr + Dr*u
24+
xr = x[pr]
25+
```
26+
27+
The elements of `xr` are a subset of the elements of `x`
28+
characterized by the index vector `pr`.
29+
30+
`E, A, B, C, D` must be of type `Matrix{T<:AbstractFloat}` of
31+
appropriate dimensions.
32+
33+
34+
The function triggers an error in the following cases:
35+
36+
- The system has only algebraic equations (for example, if `E=0`).
37+
38+
- The system has no unique solution
39+
(that is, it has none or an infinite number of solutions;
40+
for example, if it has redundant equations).
41+
42+
- A transformation to state space form with `xr` a subset of `x` would introduce a term
43+
`B2*der(u)`, that is the derivative of `u` would be required.
44+
45+
46+
# Main developer
47+
48+
[Martin Otter](https://rmc.dlr.de/sr/en/staff/martin.otter/),
49+
[DLR - Institute of System Dynamics and Control](https://www.dlr.de/sr/en)
50+
51+
52+
# Algorithm
53+
54+
The algorithm is an extended version published in:
55+
56+
Martin Otter (2001): Kapitel 20.8 - Singuläre Deskriptorsysteme. Section in book:
57+
Dierk Schröder: Elektrische Antriebe - Regelung von Antriebssystemen, 2. Auflage.
58+
"""
59+
function toStateSpace(E::Matrix{T}, A::Matrix{T}, B::Matrix{T}, C::Matrix{T}, D::Matrix{T}) where {T<:AbstractFloat}
60+
#=
61+
1. QR decomposition of E
62+
E*der(x) = Q1*R*der(x)[p1] = A[:,p1]*x[p1] + B*u
63+
R*der(x)[p1] = transpose(Q1)*A[:,p1]*x[p1] + transpose(Q1)*B*u
64+
[R1
65+
0]*der(x)[p1] = A1*x[p1] + B1*u
66+
A1 = transpose(Q1)*A[:,p1]
67+
B1 = transpose(Q1)*B
68+
n1 = size(R1,1)
69+
x1 = x[p1]
70+
[R1
71+
0]*der(x1) = [A11
72+
A21]*x1 + [B11
73+
B21]*u
74+
A11 = A1[1:n1,:]
75+
A21 = A1[n1+1:end,:]
76+
B11 = B1[1:n1,:]
77+
B21 = B1[n1+1:end,:]
78+
79+
R1*der(x1) = A11*x1 + B11*u
80+
0 = A21*x1 + B21*u
81+
82+
2. QR decomposition of A21
83+
0 = QQ*RR*x1[p2] + B21*u
84+
x2 = x1[p2]
85+
= x[p1][p2]
86+
= x[p1[p2]]
87+
0 = RR*x2 + transpose(QQ)*B21*u
88+
0 = [RR1 RR2]*[x12;x22] + B3*u
89+
n2 = size(RR1,2)
90+
B3 = transpose(QQ)*B21
91+
p3 = p1[p2]
92+
x12 = x2[1:n2]
93+
= x[p3[1:n2]]
94+
x22 = x2[n2+1:end]
95+
= x[p3[n2+1:end]]
96+
RR1 = RR[:,1:n2]
97+
RR2 = RR[:,n2+1:end]
98+
99+
0 = RR1*x12 + RR2*x22 + B3*u # RR1 must be non-singular
100+
x12 = RR1\(-RR2*x22 - B3*u)
101+
= A4*x22 + B4*u
102+
A4 = RR1\(-RR2)
103+
B4 = RR1\(-B3)
104+
105+
3. Elimination of x12
106+
R1*der(x1) = A11*x1 + B11*u
107+
R1[:,p2]*der(x1)[p2] = A11[:,p2]*x1[p2] + B11*u
108+
R1[:,p2]*der(x2) = A11[:,p2]*x2 + B11*u
109+
R21*der(x12) + R22*der(x22) = A31*x12 + A32*x22 + B11*u
110+
R21 = R1[:,p2[1:n2]]
111+
R22 = R1[:,p2[n2+1:end]]
112+
A31 = A11[:,p2[1:n2]]
113+
A32 = A11[:,p2[n2+1:end]]
114+
115+
R21*(A4*der(x22) + B4*der(u)) + R22*der(x22) = A31*(A4*x22 + B4*u) + A32*x22 + B11*u
116+
(R21*A4 + R22)*der(x22) = (A31*A4 + A32)*x22 + (A31*B4 + B11)*u
117+
Requirement: R21*B4 = 0
118+
Enew*der(xnew) = Anew*xnew + Bnew*u
119+
Enew = R21*A4 + R22
120+
Anew = A31*A4 + A32
121+
Bnew = A31*B4 + B11
122+
xnew = x22 = x[p3[n2+1:end]]
123+
= x[pnew]
124+
pnew = p3[n2+1:end]
125+
126+
y = C*x + D*u
127+
= C[:,p3]*x[p3] + D*u
128+
= [C11 C12]*[x12;x22] + D*u
129+
C11 = C[:,p3][:,1:n2]
130+
C12 = C[:,p3][:,n2+1:end]
131+
y = C11*x12 + C12*x22 + D*u
132+
= C11*(A4*x22 + B4*u) + C12*x22 + D*u
133+
= (C11*A4 + C12)*x22 + (C11*B4 + D)*u
134+
135+
y = Cnew*xnew + Dnew*u
136+
Cnew = (C11*A4 + C12)
137+
Dnew = (C11*B4 + D)
138+
=#
139+
@assert(size(E,1) == size(E,2))
140+
@assert(size(A,1) == size(E,1))
141+
@assert(size(A,2) == size(A,1))
142+
@assert(size(B,1) == size(A,1))
143+
@assert(size(C,2) == size(A,1))
144+
@assert(size(D,1) == size(C,1))
145+
@assert(size(D,2) == size(B,2))
146+
147+
E = deepcopy(E)
148+
A = deepcopy(A)
149+
B = deepcopy(B)
150+
C = deepcopy(C)
151+
D = deepcopy(D)
152+
p = 1:size(A,1)
153+
154+
while true
155+
# 1. QR decomposition of E
156+
nx = size(E,1)
157+
F = qr(E, Val(true))
158+
Q1 = F.Q
159+
R = F.R
160+
p1 = F.p
161+
162+
# Determine the lower part of R that is zero
163+
n1 = 0
164+
tol = eps(T)*nx*max(norm(A,Inf), norm(E,Inf))
165+
for i = nx:-1:1
166+
if abs(R[i,i]) > tol
167+
n1 = i
168+
break
169+
end
170+
end
171+
172+
if n1 < 1
173+
# Temporarily trigger an error. However, would be possible
174+
# to completely eliminate x and then compute Cr, Dr.
175+
error("toStateSpace(E,A,B,C,D): E*der(x) = A*x + B*u; y = C*x + D*u cannot be transformed\n",
176+
"to state space form, because the system has only algebraic equations!")
177+
end
178+
179+
# Transform A and B
180+
A1 = transpose(Q1)*A[:,p1]
181+
B1 = transpose(Q1)*B
182+
183+
# If R is regular, transform directly to state space form
184+
if n1 == nx
185+
# R*der(x)[p1] = A1*x[p1] + B1*u
186+
# y = C*x + D*u
187+
# = C[:,p1]*x[p1] + D*u
188+
Ar = R\A1
189+
Br = R\B1
190+
Cr = C[:,p1]
191+
pr = p[p1]
192+
return (Ar,Br,Cr,D,pr)
193+
end
194+
195+
# Current status
196+
# R1*der(x1) = A11*x1 + B11*u
197+
# 0 = A21*x1 + B21*u
198+
# A11 = A1[1:n1,:]
199+
# A21 = A1[n1+1:end,:]
200+
# B11 = B1[1:n1,:]
201+
# B21 = B1[n1+1:end,:]
202+
#
203+
# QR decomposition of A21
204+
R1 = R[1:n1,:]
205+
A21 = A1[n1+1:nx,:]
206+
B11 = B1[1:n1,:]
207+
B21 = B1[n1+1:nx,:]
208+
n2 = nx - n1
209+
F = qr(A21, Val(true))
210+
QQ = F.Q
211+
RR = F.R
212+
p2 = F.p
213+
if abs(RR[n2,n2]) <= tol
214+
error("toStateSpace(E,A,B): E*der(x) = A*x + B*u; y = C*x + D*u cannot be transformed\n",
215+
"to state space form, because the system has no unique solution!")
216+
end
217+
218+
# Current status
219+
# R21*der(x12) + R22*der(x22) = A31*x12 + A32*x22 + B11*u
220+
# 0 = RR1*x12 + RR2*x22 + B3*u
221+
# R21 = R1[:,p2[1:n2]]
222+
# R22 = R1[:,p2[n2+1:end]]
223+
# A31 = A11[:,p2[1:n2]]
224+
# A32 = A11[:,p2[n2+1:end]]
225+
# RR1 = RR[:,1:n2]
226+
# RR2 = RR[:,n2+1:end]
227+
p12 = p2[1:n2]
228+
p22 = p2[n2+1:nx]
229+
R21 = R1[:,p12]
230+
R22 = R1[:,p22]
231+
A31 = A1[1:n1,p12]
232+
A32 = A1[1:n1,p22]
233+
B3 = transpose(QQ)*B21
234+
RR1 = RR[:,1:n2]
235+
RR2 = RR[:,n2+1:nx]
236+
237+
# Solve for x12
238+
# x12 = RR1\(-RR2*x22 - B3*u)
239+
# = A4*x22 + B4*u
240+
A4 = RR1\(-RR2)
241+
B4 = RR1\(-B3)
242+
243+
# Check that transformation to state space form is possible
244+
if norm(R21*B4, Inf) > tol
245+
error("toStateSpace(E,A,B): E*der(x) = A*x + B*u; y = C*x + D*u cannot be transformed\n",
246+
"to state space form der(xr) = Ar*xr + Br*u; y = Cr*xr + Dr*u with xr a subset of x,\n",
247+
"because the derivative of u is needed for the transformed system.")
248+
end
249+
250+
# Elimination of x12
251+
p3 = p1[p2]
252+
C11 = C[:,p3[1:n2]]
253+
C12 = C[:,p3[n2+1:nx]]
254+
E = R21*A4 + R22
255+
A = A31*A4 + A32
256+
B = A31*B4 + B11
257+
C = C11*A4 + C12
258+
D = C11*B4 + D
259+
p = p[ p3[n2+1:nx] ]
260+
end
261+
end

‎test/TestToStateSpace.jl

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
module TestToStateSpace
2+
3+
using ModiaBase
4+
using Test
5+
6+
@testset "\nTest toStateSpace.jl" begin
7+
8+
#=
9+
Two inertias connected by an ideal gear
10+
11+
der(phi1) = w1
12+
der(phi2) = w2
13+
J1*der(w1) = -tau1 + u
14+
J2*der(w2) = tau2
15+
0 = -phi1 + r*phi2
16+
0 = -r*tau1 + tau2
17+
18+
Correct solution:
19+
w1 = der(phi1)
20+
tau2 = r*tau1
21+
J2*der(w2) = tau2 = r*tau1
22+
tau1 = J2/r*der(w2)
23+
= J2/r*der(w1)/r
24+
= J2/r^2*der(w1)
25+
J1*der(w1) = -tau1 + u
26+
= -J2/r^2*der(w1) + u
27+
(J1 + J2/r^2)*der(w1) = u
28+
der(w1) = 1/(J1 + J2/r^2) * u
29+
= 0.3121019108280255*u
30+
y1 = tau1
31+
= J2/r^2*der(w1)
32+
= J2/r^2*0.3121019108280255*u
33+
= 0.06369426751592358*u
34+
y2 = phi1
35+
36+
x_name = [:phi1, :phi2, :w1, :w2, :tau1, :tau2]
37+
=#
38+
39+
J1 = 3.0;
40+
J2 = 10.0;
41+
r = 7.0; # gear ratio
42+
43+
E1 = [1 0 0 0 0 0
44+
0 1 0 0 0 0
45+
0 0 J1 0 0 0
46+
0 0 0 J2 0 0
47+
0 0 0 0 0 0
48+
0 0 0 0 0 0]
49+
50+
A1 = [0 0 1 0 0 0
51+
0 0 0 1 0 0
52+
0 0 0 0 -1 0
53+
0 0 0 0 0 1
54+
-1 r 0 0 0 0
55+
0 0 0 0 -r 1]
56+
57+
B1 = reshape([0, 0, 1.0, 0, 0, 0],6,1)
58+
59+
C1 = [0 0 0 0 1.0 0; # y1 = tau1
60+
1.0 0 0 0 0 0] # y2 = phi1
61+
62+
D1 = zeros(2,1)
63+
64+
65+
# First test
66+
(A2,B2,C2,D2,p2) = toStateSpace(E1, A1, B1, C1, D1)
67+
68+
abstol = 1e-10
69+
k = 1/(J1 + J2/r^2)
70+
@test isapprox(A2, [0.0 0.0;1.0 0.0] , atol=abstol)
71+
@test isapprox(B2, reshape([k,0],2,1) , atol=abstol)
72+
@test isapprox(C2, [0.0 0.0; 0.0 1.0] , atol=abstol)
73+
@test isapprox(D2, reshape([J2/r^2*k; 0.0],2,1), atol=abstol)
74+
@test p2 == [3,1]
75+
76+
# Second test
77+
(A3,B3,C3,D3,p3) = toStateSpace([1.0 0.0;0.0 1.0], A2, B2, C2, D2)
78+
@test isapprox(A3, A2, atol=abstol)
79+
@test isapprox(B3, B2, atol=abstol)
80+
@test isapprox(C3, C2, atol=abstol)
81+
@test isapprox(D3, D2, atol=abstol)
82+
@test p3 == [1,2]
83+
84+
# Third test
85+
#=
86+
# Find arbitrary regular matrix
87+
S = rand(6,6)
88+
while cond(S) > 1e6
89+
S = rand(6,6)
90+
end
91+
@show S
92+
=#
93+
S = [0.04657712210063569 0.5977856114171689 0.4786954916224406 0.04284939567378343 0.0695733063167907 0.43956659685818256;
94+
0.5787577182369117 0.8137678461414808 0.4806941411619978 0.6472825897209098 0.5896925199894048 0.7869391012985769;
95+
0.689055872370582 0.3725597060839918 0.03468538463077531 0.9068630223342913 0.6083998830118467 0.8483843026618747;
96+
0.22390058951722946 0.6851371355207307 0.5880543138208856 0.11322673490297497 0.33179865225658967 0.5333918928054828;
97+
0.42423565831828003 0.6321345004194057 0.8419777975896683 0.3094611960849114 0.5643223914924458 0.4373555938996141;
98+
0.4172735713046267 0.794126703767219 0.7967916197035996 0.17448224138354762 0.3026262810569218 0.9738148273956648]
99+
100+
(A4,B4,C4,D4,p4) = toStateSpace(S*E1, S*A1, S*B1, C1, D1)
101+
@test isapprox(A4, [0.0 0.0;1.0 0.0] , atol=abstol)
102+
@test isapprox(B4, reshape([k,0],2,1) , atol=abstol)
103+
@test isapprox(C4, [0.0 0.0; 0.0 1.0] , atol=abstol)
104+
@test isapprox(D4, reshape([J2/r^2*k; 0.0],2,1), atol=abstol)
105+
@test p2 == [3,1]
106+
end
107+
108+
end

0 commit comments

Comments
 (0)