44from .exception import ControlNotImplemented
55from .lti import issiso
66from .statesp import StateSpace
7- from .statefbk import ctrb
7+ from .statefbk import ctrb , obsv
88
99from numpy import zeros , shape , poly
10- from numpy .linalg import inv
10+ from numpy .linalg import solve , matrix_rank
1111
12- __all__ = ['canonical_form' , 'reachable_form' ]
12+ __all__ = ['canonical_form' , 'reachable_form' , 'observable_form' ]
1313
1414def canonical_form (xsys , form = 'reachable' ):
1515 """Convert a system into canonical form
@@ -21,7 +21,7 @@ def canonical_form(xsys, form='reachable'):
2121 form : String
2222 Canonical form for transformation. Chosen from:
2323 * 'reachable' - reachable canonical form
24- * 'observable' - observable canonical form [not implemented]
24+ * 'observable' - observable canonical form
2525 * 'modal' - modal canonical form [not implemented]
2626
2727 Returns
@@ -35,6 +35,8 @@ def canonical_form(xsys, form='reachable'):
3535 # Call the appropriate tranformation function
3636 if form == 'reachable' :
3737 return reachable_form (xsys )
38+ elif form == 'observable' :
39+ return observable_form (xsys )
3840 else :
3941 raise ControlNotImplemented (
4042 "Canonical form '%s' not yet implemented" % form )
@@ -66,22 +68,74 @@ def reachable_form(xsys):
6668
6769 # Generate the system matrices for the desired canonical form
6870 zsys .B = zeros (shape (xsys .B ))
69- zsys .B [0 , 0 ] = 1
71+ zsys .B [0 , 0 ] = 1.0
7072 zsys .A = zeros (shape (xsys .A ))
7173 Apoly = poly (xsys .A ) # characteristic polynomial
7274 for i in range (0 , xsys .states ):
7375 zsys .A [0 , i ] = - Apoly [i + 1 ] / Apoly [0 ]
7476 if (i + 1 < xsys .states ):
75- zsys .A [i + 1 , i ] = 1
77+ zsys .A [i + 1 , i ] = 1.0
7678
7779 # Compute the reachability matrices for each set of states
7880 Wrx = ctrb (xsys .A , xsys .B )
7981 Wrz = ctrb (zsys .A , zsys .B )
8082
83+ if matrix_rank (Wrx ) != xsys .states :
84+ raise ValueError ("System not controllable to working precision." )
85+
86+ # Transformation from one form to another
87+ Tzx = solve (Wrx .T , Wrz .T ).T # matrix right division, Tzx = Wrz * inv(Wrx)
88+
89+ if matrix_rank (Tzx ) != xsys .states :
90+ raise ValueError ("Transformation matrix singular to working precision." )
91+
92+ # Finally, compute the output matrix
93+ zsys .C = solve (Tzx .T , xsys .C .T ).T # matrix right division, zsys.C = xsys.C * inv(Tzx)
94+
95+ return zsys , Tzx
96+
97+
98+ def observable_form (xsys ):
99+ """Convert a system into observable canonical form
100+
101+ Parameters
102+ ----------
103+ xsys : StateSpace object
104+ System to be transformed, with state `x`
105+
106+ Returns
107+ -------
108+ zsys : StateSpace object
109+ System in observable canonical form, with state `z`
110+ T : matrix
111+ Coordinate transformation: z = T * x
112+ """
113+ # Check to make sure we have a SISO system
114+ if not issiso (xsys ):
115+ raise ControlNotImplemented (
116+ "Canonical forms for MIMO systems not yet supported" )
117+
118+ # Create a new system, starting with a copy of the old one
119+ zsys = StateSpace (xsys )
120+
121+ # Generate the system matrices for the desired canonical form
122+ zsys .C = zeros (shape (xsys .C ))
123+ zsys .C [0 , 0 ] = 1
124+ zsys .A = zeros (shape (xsys .A ))
125+ Apoly = poly (xsys .A ) # characteristic polynomial
126+ for i in range (0 , xsys .states ):
127+ zsys .A [i , 0 ] = - Apoly [i + 1 ] / Apoly [0 ]
128+ if (i + 1 < xsys .states ):
129+ zsys .A [i , i + 1 ] = 1
130+
131+ # Compute the observability matrices for each set of states
132+ Wrx = obsv (xsys .A , xsys .C )
133+ Wrz = obsv (zsys .A , zsys .C )
134+
81135 # Transformation from one form to another
82- Tzx = Wrz * inv ( Wrx )
136+ Tzx = inv ( Wrz ) * Wrx
83137
84138 # Finally, compute the output matrix
85- zsys .C = xsys . C * inv ( Tzx )
139+ zsys .B = Tzx * xsys . B
86140
87141 return zsys , Tzx
0 commit comments