diff --git a/control/statefbk.py b/control/statefbk.py index 634922131..e2f02c1a4 100644 --- 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 index 8c8a1f199..1793dee16 100644 --- 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 index 34070aca7..042bda701 100644 --- 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