Merge pull request #176 from rabraker/update_place

murrayrm · web-flow · commit 7c5f28da492a · 2018-01-04T21:04:22.000-08:00
Update place to use scipy.signal.place_poles
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -45,12 +45,13 @@
 from . import statesp
 from .exception import ControlSlycot, ControlArgument, ControlDimension
 
-__all__ = ['ctrb', 'obsv', 'gram', 'place', 'lqr', 'acker']
+__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'acker']
+
 
 # Pole placement
 def place(A, B, p):
     """Place closed loop eigenvalues
-
+    K = place(A, B, p)
     Parameters
     ----------
     A : 2-d array
@@ -63,13 +64,92 @@ def place(A, B, p):
     Returns
     -------
     K : 2-d array
-        Gains such that A - B K has given eigenvalues
+        Gain 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])
+
+    See Also
+    --------
+    place_varga, acker
+    """
+    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]):
+        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
+
+
+def place_varga(A, B, p):
+    """Place closed loop eigenvalues
+    K = place_varga(A, B, p)
+
+    Parameters
+    ----------
+    A : 2-d array
+        Dynamics matrix
+    B : 2-d array
+        Input matrix
+    p : 1-d list
+        Desired eigenvalue locations
+    Returns
+    -------
+    K : 2-d array
+        Gain such that A - B K has eigenvalues given in p.
+
+
+    Algorithm
+    ---------
+        This function is a wrapper for the slycot function sb01bd, which
+        implements the pole placement algorithm of Varga [1]. In contrast to
+        the algorithm used by place(), the Varga algorithm can place
+        multiple poles at the same location. The placement, however, may not
+        be as robust.
+
+        [1] Varga A. "A Schur method for pole assignment."
+            IEEE Trans. Automatic Control, Vol. AC-26, pp. 517-519, 1981.
+
+    Examples
+    --------
+    >>> A = [[-1, -1], [0, 1]]
+    >>> B = [[0], [1]]
+    >>> K = place(A, B, [-2, -5])
+
+    See Also:
+    --------
+    place, acker
     """
 
     # Make sure that SLICOT is installed
diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py
@@ -401,8 +401,15 @@ def testModred(self):
         modred(self.siso_ss3, [1], 'truncate')
 
     @unittest.skipIf(not slycot_check(), "slycot not installed")
+    def testPlace_varga(self):
+        place_varga(self.siso_ss1.A, self.siso_ss1.B, [-2, -2])
+
     def testPlace(self):
-        place(self.siso_ss1.A, self.siso_ss1.B, [-2, -2])
+        place(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5])
+
+    def testAcker(self):
+        acker(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5])
+
 
     @unittest.skipIf(not slycot_check(), "slycot not installed")
     def testLQR(self):
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -8,7 +8,7 @@
 import numpy as np
 from control.statefbk import ctrb, obsv, place, lqr, gram, acker
 from control.matlab import *
-from control.exception import slycot_check
+from control.exception import slycot_check, ControlDimension
 
 class TestStatefbk(unittest.TestCase):
     """Test state feedback functions"""
@@ -157,6 +157,51 @@ 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.5+1j, -0.5-1j, -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)
+
+        # Test that the dimension checks work.
+        np.testing.assert_raises(ControlDimension, place, A[1:, :], B, P)
+        np.testing.assert_raises(ControlDimension, place, A, B[1:, :], P)
+
+        # Check that we get an error if we ask for too many poles in the same
+        # location. Here, rank(B) = 2, so lets place three at the same spot.
+        P_repeated = np.array([-0.5, -0.5, -0.5, -8.6659])
+        np.testing.assert_raises(ValueError, place, A, B, P_repeated)
+
+    @unittest.skipIf(not slycot_check(), "slycot not installed")
+    def testPlace_varga(self):
+        A = np.array([[1., -2.], [3., -4.]])
+        B = np.array([[5.], [7.]])
+
+        P = np.array([-2., -2.])
+        K = place_varga(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)
+
+        # Test that the dimension checks work.
+        np.testing.assert_raises(ControlDimension, place, A[1:, :], B, P)
+        np.testing.assert_raises(ControlDimension, place, A, B[1:, :], P)
+
     def check_LQR(self, K, S, poles, Q, R):
         S_expected = np.array(np.sqrt(Q * R))
         K_expected = S_expected / R
