Merge pull request #197 from rabraker/fix_place_varga

Fix place varga

control/statefbk.py

@@ -113,18 +113,32 @@ def place(A, B, p):
     return K
 
 
-def place_varga(A, B, p):
+def place_varga(A, B, p, dtime=False, alpha=None):
     """Place closed loop eigenvalues
-    K = place_varga(A, B, p)
+    K = place_varga(A, B, p, dtime=False, alpha=None)
 
-    Parameters
+    Required Parameters
     ----------
     A : 2-d array
         Dynamics matrix
     B : 2-d array
         Input matrix
     p : 1-d list
         Desired eigenvalue locations
+
+    Optional Parameters
+    ---------------
+    dtime: False for continuous time pole placement or True for discrete time.
+            The default is dtime=False.
+    alpha: double scalar
+           If DICO='C', then place_varga will leave the eigenvalues with real
+           real part less than alpha untouched.
+           If DICO='D', the place_varga will leave eigenvalues with modulus
+           less than alpha untouched.
+
+           By default (alpha=None), place_varga computes alpha such that all
+           poles will be placed.
+
     Returns
     -------
     K : 2-d array
@@ -146,7 +160,7 @@ def place_varga(A, B, p):
     --------
     >>> A = [[-1, -1], [0, 1]]
     >>> B = [[0], [1]]
-    >>> K = place(A, B, [-2, -5])
+    >>> K = place_varga(A, B, [-2, -5])
 
     See Also:
     --------
@@ -160,24 +174,46 @@ def place_varga(A, B, p):
         raise ControlSlycot("can't find slycot module 'sb01bd'")
 
     # Convert the system inputs to NumPy arrays
-    A_mat = np.array(A);
-    B_mat = np.array(B);
+    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);
+    placed_eigs = np.array(p)
+
+    # Need a character parameter for SB01BD
+    if dtime:
+        DICO = 'D'
+    else:
+        DICO = 'C'
+
+    if alpha is None:
+        # SB01BD ignores eigenvalues with real part less than alpha
+        # (if DICO='C') or with modulus less than alpha
+        # (if DICO = 'D').
+        if dtime:
+            # For discrete time, slycot only cares about modulus, so just make
+            # alpha the smallest it can be.
+            alpha = 0.0
+        else:
+            # Choosing alpha=min_eig is insufficient and can lead to an
+            # error or not having all the eigenvalues placed that we wanted.
+            # Evidently, what python thinks are the eigs is not precisely
+            # the same as what slicot thinks are the eigs. So we need some
+            # numerical breathing room. The following is pretty heuristic,
+            # but does the trick
+            alpha = -2*abs(min(system_eigs.real))
+    elif dtime and alpha < 0.0:
+        raise ValueError("Need alpha > 0 when DICO='D'")
 
-    # 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');
+               A_mat, B_mat, placed_eigs, DICO)
 
     # Return the gain matrix, with MATLAB gain convention
     return -F

control/tests/statefbk_test.py

@@ -6,7 +6,7 @@
 from __future__ import print_function
 import unittest
 import numpy as np
-from control.statefbk import ctrb, obsv, place, lqr, gram, acker
+from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker
 from control.matlab import *
 from control.exception import slycot_check, ControlDimension
 
@@ -186,7 +186,10 @@ def testPlace(self):
         np.testing.assert_raises(ValueError, place, A, B, P_repeated)
 
     @unittest.skipIf(not slycot_check(), "slycot not installed")
-    def testPlace_varga(self):
+    def testPlace_varga_continuous(self):
+        """
+        Check that we can place eigenvalues for dtime=False
+        """
         A = np.array([[1., -2.], [3., -4.]])
         B = np.array([[5.], [7.]])
 
@@ -202,6 +205,77 @@ def testPlace_varga(self):
         np.testing.assert_raises(ControlDimension, place, A[1:, :], B, P)
         np.testing.assert_raises(ControlDimension, place, A, B[1:, :], P)
 
+        # Regression test against bug #177
+        # https://github.com/python-control/python-control/issues/177
+        A = np.array([[0, 1], [100, 0]])
+        B = np.array([[0], [1]])
+        P = np.array([-20 + 10*1j, -20 - 10*1j])
+        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)
+
+    @unittest.skipIf(not slycot_check(), "slycot not installed")
+    def testPlace_varga_continuous_partial_eigs(self):
+        """
+        Check that we are able to use the alpha parameter to only place
+        a subset of the eigenvalues, for the continous time case.
+        """
+        # A matrix has eigenvalues at s=-1, and s=-2. Choose alpha = -1.5
+        # and check that eigenvalue at s=-2 stays put.
+        A = np.array([[1., -2.], [3., -4.]])
+        B = np.array([[5.], [7.]])
+
+        P = np.array([-3.])
+        P_expected = np.array([-2.0, -3.0])
+        alpha = -1.5
+        K = place_varga(A, B, P, alpha=alpha)
+
+        P_placed = np.linalg.eigvals(A - B.dot(K))
+        # No guarantee of the ordering, so sort them
+        P_expected.sort()
+        P_placed.sort()
+        np.testing.assert_array_almost_equal(P_expected, P_placed)
+
+    @unittest.skipIf(not slycot_check(), "slycot not installed")
+    def testPlace_varga_discrete(self):
+        """
+        Check that we can place poles using dtime=True (discrete time)
+        """
+        A = np.array([[1., 0], [0, 0.5]])
+        B = np.array([[5.], [7.]])
+
+        P = np.array([0.5, 0.5])
+        K = place_varga(A, B, P, dtime=True)
+        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)
+
+    @unittest.skipIf(not slycot_check(), "slycot not installed")
+    def testPlace_varga_discrete_partial_eigs(self):
+        """"
+        Check that we can only assign a single eigenvalue in the discrete
+        time case.
+        """
+        # A matrix has eigenvalues at 1.0 and 0.5. Set alpha = 0.51, and
+        # check that the eigenvalue at 0.5 is not moved.
+        A = np.array([[1., 0], [0, 0.5]])
+        B = np.array([[5.], [7.]])
+        P = np.array([0.2, 0.6])
+        P_expected = np.array([0.5, 0.6])
+        alpha = 0.51
+        K = place_varga(A, B, P, dtime=True, alpha=alpha)
+        P_placed = np.linalg.eigvals(A - B.dot(K))
+        P_expected.sort()
+        P_placed.sort()
+        np.testing.assert_array_almost_equal(P_expected, P_placed)
+
+
     def check_LQR(self, K, S, poles, Q, R):
         S_expected = np.array(np.sqrt(Q * R))
         K_expected = S_expected / R