From 98e71cbcdcfa028dd5c140d8b9c7d873bb68f1dd Mon Sep 17 00:00:00 2001
From: Yuichi NAGAYAMA <yuina822@red-queen.moe>
Date: Sun, 7 Aug 2016 21:17:45 +0900
Subject: [PATCH] Add function to convert a system into observable canonical
 form

---
 control/canonical.py | 54 +++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 51 insertions(+), 3 deletions(-)

diff --git a/control/canonical.py b/control/canonical.py
index 468013e8d..d148ca411 100644
--- a/control/canonical.py
+++ b/control/canonical.py
@@ -4,12 +4,12 @@
 from .exception import ControlNotImplemented
 from .lti import issiso
 from .statesp import StateSpace
-from .statefbk import ctrb
+from .statefbk import ctrb, obsv
 
 from numpy import zeros, shape, poly
 from numpy.linalg import inv
 
-__all__ = ['canonical_form', 'reachable_form']
+__all__ = ['canonical_form', 'reachable_form', 'observable_form']
 
 def canonical_form(xsys, form='reachable'):
     """Convert a system into canonical form
@@ -21,7 +21,7 @@ def canonical_form(xsys, form='reachable'):
     form : String
         Canonical form for transformation.  Chosen from:
           * 'reachable' - reachable canonical form
-          * 'observable' - observable canonical form [not implemented]
+          * 'observable' - observable canonical form
           * 'modal' - modal canonical form [not implemented]
 
     Returns
@@ -35,6 +35,8 @@ def canonical_form(xsys, form='reachable'):
     # 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)
@@ -85,3 +87,49 @@ def reachable_form(xsys):
     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 = inv(Wrz) * Wrx
+
+    # Finally, compute the output matrix
+    zsys.B = Tzx * xsys.B
+
+    return zsys, Tzx