Added a check for unreachable systems to canonical

mp4096 · mp4096 · commit 1fad4c68344c · 2016-11-20T21:08:11.000+01:00
And a corresponding unit test
diff --git a/control/canonical.py b/control/canonical.py
@@ -7,7 +7,7 @@
 from .statefbk import ctrb, obsv
 
 from numpy import zeros, shape, poly
-from numpy.linalg import solve
+from numpy.linalg import solve, matrix_rank
 
 __all__ = ['canonical_form', 'reachable_form', 'observable_form']
 
@@ -80,9 +80,15 @@ def reachable_form(xsys):
     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)
 
diff --git a/control/tests/canonical_test.py b/control/tests/canonical_test.py
@@ -39,3 +39,16 @@ def test_reachable_form(self):
         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_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")
