Merge pull request #201 from ipa-lth/modal_form

murrayrm · web-flow · commit 70db51392753 · 2018-12-21T14:44:52.000-08:00
add canonical form 'modal'
diff --git a/control/canonical.py b/control/canonical.py
@@ -6,8 +6,8 @@
 from .statesp import StateSpace
 from .statefbk import ctrb, obsv
 
-from numpy import zeros, shape, poly
-from numpy.linalg import solve, matrix_rank
+from numpy import zeros, shape, poly, iscomplex, hstack
+from numpy.linalg import solve, matrix_rank, eig
 
 __all__ = ['canonical_form', 'reachable_form', 'observable_form']
 
@@ -22,7 +22,7 @@ def canonical_form(xsys, form='reachable'):
         Canonical form for transformation.  Chosen from:
           * 'reachable' - reachable canonical form
           * 'observable' - observable canonical form
-          * 'modal' - modal canonical form [not implemented]
+          * 'modal' - modal canonical form
 
     Returns
     -------
@@ -37,6 +37,8 @@ def canonical_form(xsys, form='reachable'):
         return reachable_form(xsys)
     elif form == 'observable':
         return observable_form(xsys)
+    elif form == 'modal':
+        return modal_form(xsys)
     else:
         raise ControlNotImplemented(
             "Canonical form '%s' not yet implemented" % form)
@@ -142,3 +144,69 @@ def observable_form(xsys):
     zsys.B = Tzx * xsys.B
 
     return zsys, Tzx
+
+def modal_form(xsys):
+    """Convert a system into modal canonical form
+
+    Parameters
+    ----------
+    xsys : StateSpace object
+        System to be transformed, with state `x`
+
+    Returns
+    -------
+    zsys : StateSpace object
+        System in modal 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)
+
+    # Calculate eigenvalues and matrix of eigenvectors Tzx,
+    eigval, eigvec = eig(xsys.A)
+
+    # Eigenvalues and according eigenvectors are not sorted,
+    # thus modal transformation is ambiguous
+    # Sorting eigenvalues and respective vectors by largest to smallest eigenvalue
+    idx = eigval.argsort()[::-1]
+    eigval = eigval[idx]
+    eigvec = eigvec[:,idx]
+
+    # If all eigenvalues are real, the matrix of eigenvectors is Tzx directly
+    if not iscomplex(eigval).any():
+        Tzx = eigvec
+    else:
+        # A is an arbitrary semisimple matrix
+
+        # Keep track of complex conjugates (need only one)
+        lst_conjugates = []
+        Tzx = None
+        for val, vec in zip(eigval, eigvec.T):
+            if iscomplex(val):
+                if val not in lst_conjugates:
+                    lst_conjugates.append(val.conjugate())
+                    if Tzx is not None:
+                        Tzx = hstack((Tzx, hstack((vec.real.T, vec.imag.T))))
+                    else:
+                        Tzx = hstack((vec.real.T, vec.imag.T))
+                else:
+                    # if conjugate has already been seen, skip this eigenvalue
+                    lst_conjugates.remove(val)
+            else:
+                if Tzx is not None:
+                    Tzx = hstack((Tzx, vec.real.T))
+                else:
+                    Tzx = vec.real.T
+
+    # Generate the system matrices for the desired canonical form
+    zsys.A = solve(Tzx, xsys.A).dot(Tzx)
+    zsys.B = solve(Tzx, xsys.B)
+    zsys.C = xsys.C.dot(Tzx)
+
+    return zsys, Tzx
diff --git a/control/tests/canonical_test.py b/control/tests/canonical_test.py
@@ -53,6 +53,36 @@ def test_unreachable_system(self):
         # 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.matrix([[-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)*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)
+        
     def test_observable_form(self):
         """Test the observable canonical form"""
 
