Modify statefbk.place to use the YT algorithm implemented in scipy.signals.place_poles

rabraker · rabraker · commit 99302cdb4455 · 2018-01-02T17:17:18.000-07:00
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -47,6 +47,7 @@
 
 __all__ = ['ctrb', 'obsv', 'gram', 'place', 'lqr', 'acker']
 
+
 # Pole placement
 def place(A, B, p):
     """Place closed loop eigenvalues
@@ -63,43 +64,48 @@ def place(A, B, p):
     Returns
     -------
     K : 2-d array
-        Gains such that A - B K has given eigenvalues
+        Gains such that A - B K has eigenvalues given in p
+
+    Algorithm
+    ---------
+    This is a wrapper function for scipy.signal.place_poles, which
+    implements the Tits and Yang algorithm [1]. It will handle SISO,
+    MISO, and MIMO systems. If you want more control over the algorithm,
+    use scipy.signal.place_poles directly.
+
+    [1] A.L. Tits and Y. Yang, "Globally convergent algorithms for robust
+    pole assignment by state feedback, IEEE Transactions on Automatic
+    Control, Vol. 41, pp. 1432-1452, 1996.
+
+    Limitations
+    -----------
+    The algorithm will not place poles at the same location more
+    than rank(B) times.
 
     Examples
     --------
     >>> A = [[-1, -1], [0, 1]]
     >>> B = [[0], [1]]
     >>> K = place(A, B, [-2, -5])
     """
-
-    # Make sure that SLICOT is installed
-    try:
-        from slycot import sb01bd
-    except ImportError:
-        raise ControlSlycot("can't find slycot module 'sb01bd'")
+    from scipy.signal import place_poles
 
     # Convert the system inputs to NumPy arrays
-    A_mat = np.array(A);
-    B_mat = np.array(B);
-    if (A_mat.shape[0] != A_mat.shape[1] or
-        A_mat.shape[0] != B_mat.shape[0]):
-        raise ControlDimension("matrix dimensions are incorrect")
-
-    # Compute the system eigenvalues and convert poles to numpy array
-    system_eigs = np.linalg.eig(A_mat)[0]
-    placed_eigs = np.array(p);
-
-    # SB01BD sets eigenvalues with real part less than alpha
-    # We want to place all poles of the system => set alpha to minimum
-    alpha = min(system_eigs.real);
-
-    # Call SLICOT routine to place the eigenvalues
-    A_z,w,nfp,nap,nup,F,Z = \
-        sb01bd(B_mat.shape[0], B_mat.shape[1], len(placed_eigs), alpha,
-               A_mat, B_mat, placed_eigs, 'C');
-
-    # Return the gain matrix, with MATLAB gain convention
-    return -F
+    A_mat = np.array(A)
+    B_mat = np.array(B)
+    if (A_mat.shape[0] != A_mat.shape[1]):
+        raise ControlDimension("A must be a square matrix")
+
+    if (A_mat.shape[0] != B_mat.shape[0]):
+        err_str = "The number of rows of A must equal the number of rows in B"
+        raise ControlDimension(err_str)
+
+    # Convert desired poles to numpy array
+    placed_eigs = np.array(p)
+
+    result = place_poles(A_mat, B_mat, placed_eigs, method='YT')
+    K = result.gain_matrix
+    return K
 
 # Contributed by Roberto Bucher <roberto.bucher@supsi.ch>
 def acker(A, B, poles):
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -157,6 +157,25 @@ def testAcker(self):
                 np.testing.assert_array_almost_equal(np.sort(poles),
                                                      np.sort(placed), decimal=4)
 
+    def testPlace(self):
+        # Matrices shamelessly stolen from scipy example code.
+        A = np.array([[1.380,  -0.2077,  6.715, -5.676],
+                      [-0.5814, -4.290,   0,      0.6750],
+                      [1.067,   4.273,  -6.654,  5.893],
+                      [0.0480,  4.273,   1.343, -2.104]])
+
+        B = np.array([[0,      5.679],
+                      [1.136,  1.136],
+                      [0,      0,],
+                      [-3.146,  0]])
+        P = np.array([-0.2, -0.5, -5.0566, -8.6659])
+        K = place(A, B, P)
+        P_placed = np.linalg.eigvals(A - B.dot(K))
+        # No guarantee of the ordering, so sort them
+        P.sort()
+        P_placed.sort()
+        np.testing.assert_array_almost_equal(P, P_placed)
+
     def check_LQR(self, K, S, poles, Q, R):
         S_expected = np.array(np.sqrt(Q * R))
         K_expected = S_expected / R
