forked from python-control/python-control
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathcanonical.py
More file actions
144 lines (114 loc) · 4.34 KB
/
Copy pathcanonical.py
File metadata and controls
144 lines (114 loc) · 4.34 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
# canonical.py - functions for converting systems to canonical forms
# RMM, 10 Nov 2012
from .exception import ControlNotImplemented
from .lti import issiso
from .statesp import StateSpace
from .statefbk import ctrb, obsv
from numpy import zeros, shape, poly
from numpy.linalg import solve, matrix_rank
__all__ = ['canonical_form', 'reachable_form', 'observable_form']
def canonical_form(xsys, form='reachable'):
"""Convert a system into canonical form
Parameters
----------
xsys : StateSpace object
System to be transformed, with state 'x'
form : String
Canonical form for transformation. Chosen from:
* 'reachable' - reachable canonical form
* 'observable' - observable canonical form
* 'modal' - modal canonical form [not implemented]
Returns
-------
zsys : StateSpace object
System in desired canonical form, with state 'z'
T : matrix
Coordinate transformation matrix, z = T * x
"""
# Call the appropriate tranformation function
if form == 'reachable':
return reachable_form(xsys)
elif form == 'observable':
return observable_form(xsys)
else:
raise ControlNotImplemented(
"Canonical form '%s' not yet implemented" % form)
# Reachable canonical form
def reachable_form(xsys):
"""Convert a system into reachable canonical form
Parameters
----------
xsys : StateSpace object
System to be transformed, with state `x`
Returns
-------
zsys : StateSpace object
System in reachable canonical form, with state `z`
T : matrix
Coordinate transformation: z = T * x
"""
# Check to make sure we have a SISO system
if not issiso(xsys):
raise ControlNotImplemented(
"Canonical forms for MIMO systems not yet supported")
# Create a new system, starting with a copy of the old one
zsys = StateSpace(xsys)
# Generate the system matrices for the desired canonical form
zsys.B = zeros(shape(xsys.B))
zsys.B[0, 0] = 1.0
zsys.A = zeros(shape(xsys.A))
Apoly = poly(xsys.A) # characteristic polynomial
for i in range(0, xsys.states):
zsys.A[0, i] = -Apoly[i+1] / Apoly[0]
if (i+1 < xsys.states):
zsys.A[i+1, i] = 1.0
# Compute the reachability matrices for each set of states
Wrx = ctrb(xsys.A, xsys.B)
Wrz = ctrb(zsys.A, zsys.B)
if matrix_rank(Wrx) != xsys.states:
raise ValueError("System not controllable to working precision.")
# Transformation from one form to another
Tzx = solve(Wrx.T, Wrz.T).T # matrix right division, Tzx = Wrz * inv(Wrx)
if matrix_rank(Tzx) != xsys.states:
raise ValueError("Transformation matrix singular to working precision.")
# Finally, compute the output matrix
zsys.C = solve(Tzx.T, xsys.C.T).T # matrix right division, zsys.C = xsys.C * inv(Tzx)
return zsys, Tzx
def observable_form(xsys):
"""Convert a system into observable canonical form
Parameters
----------
xsys : StateSpace object
System to be transformed, with state `x`
Returns
-------
zsys : StateSpace object
System in observable canonical form, with state `z`
T : matrix
Coordinate transformation: z = T * x
"""
# Check to make sure we have a SISO system
if not issiso(xsys):
raise ControlNotImplemented(
"Canonical forms for MIMO systems not yet supported")
# Create a new system, starting with a copy of the old one
zsys = StateSpace(xsys)
# Generate the system matrices for the desired canonical form
zsys.C = zeros(shape(xsys.C))
zsys.C[0, 0] = 1
zsys.A = zeros(shape(xsys.A))
Apoly = poly(xsys.A) # characteristic polynomial
for i in range(0, xsys.states):
zsys.A[i, 0] = -Apoly[i+1] / Apoly[0]
if (i+1 < xsys.states):
zsys.A[i, i+1] = 1
# Compute the observability matrices for each set of states
Wrx = obsv(xsys.A, xsys.C)
Wrz = obsv(zsys.A, zsys.C)
# Transformation from one form to another
Tzx = solve(Wrz, Wrx) # matrix left division, Tzx = inv(Wrz) * Wrx
if matrix_rank(Tzx) != xsys.states:
raise ValueError("Transformation matrix singular to working precision.")
# Finally, compute the output matrix
zsys.B = Tzx * xsys.B
return zsys, Tzx