Repository navigation
Expand file tree
/
Copy pathcanonical_test.py
More file actions
293 lines (238 loc) · 13 KB
/
Copy pathcanonical_test.py
File metadata and controls
293 lines (238 loc) · 13 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
#!/usr/bin/env python
import unittest
import numpy as np
from control import ss, tf, tf2ss, ss2tf
from control.canonical import canonical_form, reachable_form, \
observable_form, modal_form, similarity_transform
from control.exception import ControlNotImplemented
class TestCanonical(unittest.TestCase):
"""Tests for the canonical forms class"""
def test_reachable_form(self):
"""Test the reachable canonical form"""
# Create a system in the reachable canonical form
coeffs = [1.0, 2.0, 3.0, 4.0, 1.0]
A_true = np.polynomial.polynomial.polycompanion(coeffs)
A_true = np.fliplr(np.rot90(A_true))
B_true = np.matrix("1.0 0.0 0.0 0.0").T
C_true = np.matrix("1.0 1.0 1.0 1.0")
D_true = 42.0
# Perform a coordinate transform with a random invertible matrix
T_true = np.array([[-0.27144004, -0.39933167, 0.75634684, 0.44135471],
[-0.74855725, -0.39136285, -0.18142339, -0.50356997],
[-0.40688007, 0.81416369, 0.38002113, -0.16483334],
[-0.44769516, 0.15654653, -0.50060858, 0.72419146]])
A = np.linalg.solve(T_true, A_true).dot(T_true)
B = np.linalg.solve(T_true, B_true)
C = C_true.dot(T_true)
D = D_true
# Create a state space system and convert it to the reachable canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), "reachable")
# Check against the true values
np.testing.assert_array_almost_equal(sys_check.A, A_true)
np.testing.assert_array_almost_equal(sys_check.B, B_true)
np.testing.assert_array_almost_equal(sys_check.C, C_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
np.testing.assert_array_almost_equal(T_check, T_true)
# Reachable form only supports SISO
sys = tf([[ [1], [1] ]], [[ [1, 2, 1], [1, 2, 1] ]])
np.testing.assert_raises(ControlNotImplemented, reachable_form, sys)
def test_unreachable_system(self):
"""Test reachable canonical form with an unreachable system"""
# Create an unreachable system
A = np.matrix("1.0 2.0 2.0; 4.0 5.0 5.0; 7.0 8.0 8.0")
B = np.matrix("1.0 1.0 1.0").T
C = np.matrix("1.0 1.0 1.0")
D = 42.0
sys = ss(A, B, C, D)
# Check if an exception is raised
np.testing.assert_raises(ValueError, canonical_form, sys, "reachable")
def test_modal_form(self):
"""Test the modal canonical form"""
# Create a system in the modal canonical form
A_true = np.diag([4.0, 3.0, 2.0, 1.0]) # order from the largest to the smallest
B_true = np.matrix("1.1 2.2 3.3 4.4").T
C_true = np.matrix("1.3 1.4 1.5 1.6")
D_true = 42.0
# Perform a coordinate transform with a random invertible matrix
T_true = np.array([[-0.27144004, -0.39933167, 0.75634684, 0.44135471],
[-0.74855725, -0.39136285, -0.18142339, -0.50356997],
[-0.40688007, 0.81416369, 0.38002113, -0.16483334],
[-0.44769516, 0.15654653, -0.50060858, 0.72419146]])
A = np.linalg.solve(T_true, A_true).dot(T_true)
B = np.linalg.solve(T_true, B_true)
C = C_true*T_true
D = D_true
# Create a state space system and convert it to the modal canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), "modal")
# Check against the true values
# TODO: Test in respect to ambiguous transformation (system characteristics?)
np.testing.assert_array_almost_equal(sys_check.A, A_true)
#np.testing.assert_array_almost_equal(sys_check.B, B_true)
#np.testing.assert_array_almost_equal(sys_check.C, C_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
#np.testing.assert_array_almost_equal(T_check, T_true)
# Check conversion when there are complex eigenvalues
A_true = np.array([[-1, 1, 0, 0],
[-1, -1, 0, 0],
[ 0, 0, -2, 0],
[ 0, 0, 0, -3]])
B_true = np.array([[0], [1], [0], [1]])
C_true = np.array([[1, 0, 0, 1]])
D_true = np.array([[0]])
A = np.linalg.solve(T_true, A_true).dot(T_true)
B = np.linalg.solve(T_true, B_true)
C = C_true.dot(T_true)
D = D_true
# Create state space system and convert to modal canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), 'modal')
# Check A and D matrix, which are uniquely defined
np.testing.assert_array_almost_equal(sys_check.A, A_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
# B matrix should be all ones (or zero if not controllable)
# TODO: need to update modal_form() to implement this
if np.allclose(T_check, T_true):
np.testing.assert_array_almost_equal(sys_check.B, B_true)
np.testing.assert_array_almost_equal(sys_check.C, C_true)
# Make sure Hankel coefficients are OK
from numpy.linalg import matrix_power
for i in range(A.shape[0]):
np.testing.assert_almost_equal(
np.dot(np.dot(C_true, matrix_power(A_true, i)), B_true),
np.dot(np.dot(C, matrix_power(A, i)), B))
# Reorder rows to get complete coverage (real eigenvalue cxrtvfirst)
A_true = np.array([[-1, 0, 0, 0],
[ 0, -2, 1, 0],
[ 0, -1, -2, 0],
[ 0, 0, 0, -3]])
B_true = np.array([[0], [0], [1], [1]])
C_true = np.array([[0, 1, 0, 1]])
D_true = np.array([[0]])
A = np.linalg.solve(T_true, A_true).dot(T_true)
B = np.linalg.solve(T_true, B_true)
C = C_true.dot(T_true)
D = D_true
# Create state space system and convert to modal canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), 'modal')
# Check A and D matrix, which are uniquely defined
np.testing.assert_array_almost_equal(sys_check.A, A_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
# B matrix should be all ones (or zero if not controllable)
# TODO: need to update modal_form() to implement this
if np.allclose(T_check, T_true):
np.testing.assert_array_almost_equal(sys_check.B, B_true)
np.testing.assert_array_almost_equal(sys_check.C, C_true)
# Make sure Hankel coefficients are OK
from numpy.linalg import matrix_power
for i in range(A.shape[0]):
np.testing.assert_almost_equal(
np.dot(np.dot(C_true, matrix_power(A_true, i)), B_true),
np.dot(np.dot(C, matrix_power(A, i)), B))
# Modal form only supports SISO
sys = tf([[ [1], [1] ]], [[ [1, 2, 1], [1, 2, 1] ]])
np.testing.assert_raises(ControlNotImplemented, modal_form, sys)
def test_observable_form(self):
"""Test the observable canonical form"""
# Create a system in the observable canonical form
coeffs = [1.0, 2.0, 3.0, 4.0, 1.0]
A_true = np.polynomial.polynomial.polycompanion(coeffs)
A_true = np.fliplr(np.flipud(A_true))
B_true = np.matrix("1.0 1.0 1.0 1.0").T
C_true = np.matrix("1.0 0.0 0.0 0.0")
D_true = 42.0
# Perform a coordinate transform with a random invertible matrix
T_true = np.array([[-0.27144004, -0.39933167, 0.75634684, 0.44135471],
[-0.74855725, -0.39136285, -0.18142339, -0.50356997],
[-0.40688007, 0.81416369, 0.38002113, -0.16483334],
[-0.44769516, 0.15654653, -0.50060858, 0.72419146]])
A = np.linalg.solve(T_true, A_true).dot(T_true)
B = np.linalg.solve(T_true, B_true)
C = C_true.dot(T_true)
D = D_true
# Create a state space system and convert it to the observable canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), "observable")
# Check against the true values
np.testing.assert_array_almost_equal(sys_check.A, A_true)
np.testing.assert_array_almost_equal(sys_check.B, B_true)
np.testing.assert_array_almost_equal(sys_check.C, C_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
np.testing.assert_array_almost_equal(T_check, T_true)
# Observable form only supports SISO
sys = tf([[ [1], [1] ]], [[ [1, 2, 1], [1, 2, 1] ]])
np.testing.assert_raises(ControlNotImplemented, observable_form, sys)
def test_unobservable_system(self):
"""Test observable canonical form with an unobservable system"""
# Create an unobservable system
A = np.matrix("1.0 2.0 2.0; 4.0 5.0 5.0; 7.0 8.0 8.0")
B = np.matrix("1.0 1.0 1.0").T
C = np.matrix("1.0 1.0 1.0")
D = 42.0
sys = ss(A, B, C, D)
# Check if an exception is raised
np.testing.assert_raises(ValueError, canonical_form, sys, "observable")
def test_arguments(self):
# Additional unit tests added on 25 May 2019 to increase coverage
# Unknown canonical forms should generate exception
sys = tf([1], [1, 2, 1])
np.testing.assert_raises(
ControlNotImplemented, canonical_form, sys, 'unknown')
def test_similarity(self):
"""Test similarty transform"""
# Single input, single output systems
siso_ini = tf2ss(tf([1, 1], [1, 1, 1]))
for form in 'reachable', 'observable':
# Convert the system to one of the canonical forms
siso_can, T_can = canonical_form(siso_ini, form)
# Use a similarity transformation to transform it back
siso_sim = similarity_transform(siso_can, np.linalg.inv(T_can))
# Make sure everything goes back to the original form
np.testing.assert_array_almost_equal(siso_sim.A, siso_ini.A)
np.testing.assert_array_almost_equal(siso_sim.B, siso_ini.B)
np.testing.assert_array_almost_equal(siso_sim.C, siso_ini.C)
np.testing.assert_array_almost_equal(siso_sim.D, siso_ini.D)
# Multi-input, multi-output systems
mimo_ini = ss(
[[-1, 1, 0, 0], [0, -2, 1, 0], [0, 0, -3, 1], [0, 0, 0, -4]],
[[1, 0], [0, 0], [0, 1], [1, 1]],
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]],
np.zeros((3, 2)))
# Simple transformation: row/col flips + scaling
mimo_txf = np.array(
[[0, 1, 0, 0], [2, 0, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
# Transform the system and transform it back
mimo_sim = similarity_transform(mimo_ini, mimo_txf)
mimo_new = similarity_transform(mimo_sim, np.linalg.inv(mimo_txf))
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Make sure rescaling by identify does nothing
mimo_new = similarity_transform(mimo_ini, np.eye(4))
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Time rescaling
mimo_tim = similarity_transform(mimo_ini, np.eye(4), timescale=0.3)
mimo_new = similarity_transform(mimo_tim, np.eye(4), timescale=1/0.3)
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Time + transformation, in one step
mimo_sim = similarity_transform(mimo_ini, mimo_txf, timescale=0.3)
mimo_new = similarity_transform(mimo_sim, np.linalg.inv(mimo_txf),
timescale=1/0.3)
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Time + transformation, in two steps
mimo_sim = similarity_transform(mimo_ini, mimo_txf, timescale=0.3)
mimo_tim = similarity_transform(mimo_sim, np.eye(4), timescale=1/0.3)
mimo_new = similarity_transform(mimo_tim, np.linalg.inv(mimo_txf))
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
if __name__ == "__main__":
unittest.main()